Skip to content
Snippets Groups Projects
Select Git revision
  • a860b0c0635500e628f91d26082a5df477f454e7
  • master default protected
  • ci
  • relations
4 results

source-view.component.html

Blame
  • MqttToRosNode.cpp 5.17 KiB
    #include "mqtt/client.h"
    
    #include "ros/ros.h"
    #include "geometry_msgs/PoseArray.h"
    
    #include "util/MqttUtil.h"
    
    #include "dataconfig.pb.h"
    #include "robotconfig.pb.h"
    #include "trajectory.pb.h"
    
    const std::string CLIENT_ID{"mqtt_to_ros"};
    
    const std::string ROBOT_CONFIG{"robotconfig"};
    const std::string DATA_CONFIG{"dataconfig"};
    const std::string TRAJECTORY_CONFIG{"trajectoryconfig"};
    
    MqttUtil *mqttUtil = nullptr;
    
    void handleRobotConfig(const config::RobotConfig &robotConfig, const ros::NodeHandle &n) {
      ROS_INFO_STREAM("Retrieved new target-speed: " << robotConfig.speed());
      n.setParam("robot_speed_factor", robotConfig.speed());
    
      ROS_INFO_STREAM("Retrieved new loop-mode: " << robotConfig.looptrajectory());
      n.setParam("loop_trajectory", robotConfig.looptrajectory());
    
      ROS_INFO_STREAM("Retrieved new planning-mode.");
      switch (robotConfig.planningmode()) {
        case config::RobotConfig_PlanningMode_FLUID:
          n.setParam("robot_planning_mode", "fluid_path");
          ROS_INFO_STREAM("Planning-mode: fluid");
          break;
        case config::RobotConfig_PlanningMode_CARTESIAN:
          n.setParam("robot_planning_mode", "cartesian_path");
          ROS_INFO_STREAM("Planning-mode: cartesian");
          break;
        case config::RobotConfig_PlanningMode_RobotConfig_PlanningMode_INT_MIN_SENTINEL_DO_NOT_USE_:
          ROS_ERROR_STREAM("Planning-mode: INT_MIN_SENTINEL_DO_NOT_USE_");
          break;
        case config::RobotConfig_PlanningMode_RobotConfig_PlanningMode_INT_MAX_SENTINEL_DO_NOT_USE_:
          ROS_ERROR_STREAM("Planning-mode: INT_MAX_SENTINEL_DO_NOT_USE_");
          break;
      }
    }
    
    void
    handleNewTrajectory(const plan::Trajectory &trajectory, const ros::NodeHandle &n, const ros::Publisher &posePublisher) {
    
      geometry_msgs::PoseArray poseArray;
    
      for (const auto &protoPose : trajectory.pos()) {
        geometry_msgs::Pose pose;
        pose.orientation.w = 1;
        pose.position.x = protoPose.x();
        pose.position.y = protoPose.y();
        pose.position.z = protoPose.z();
        poseArray.poses.push_back(pose);
      }
    
      posePublisher.publish(poseArray);
    
    }
    
    void handleDataConfig(const config::DataConfig &dataConfig, const ros::NodeHandle &n) {
      ROS_INFO_STREAM("Retrieved new data-config:" << std::endl
                      << " -- publish-rate: " << dataConfig.publishrate() << std::endl
                      << " -- enablePosition: " << dataConfig.enableposition() << std::endl
                      << " -- enableOrientation: " << dataConfig.enableorientation() << std::endl
                      << " -- enableTwistLinear: " << dataConfig.enabletwistlinear() << std::endl
                      << " -- enableTwistAngular: " << dataConfig.enabletwistangular() << std::endl);
    
      n.setParam("data_publish_rate", dataConfig.publishrate());
      n.setParam("data_enable_position", dataConfig.enableposition());
      n.setParam("data_enable_orientation", dataConfig.enableorientation());
      n.setParam("data_enable_twist_linear", dataConfig.enabletwistlinear());
      n.setParam("data_enable_twist_angular", dataConfig.enabletwistangular());
    }
    
    void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &posePublisher) {
      if (mqttUtil->ensureConnection()) {
        mqtt::const_message_ptr msg;
        if (mqttUtil->getClient().try_consume_message_for(&msg, std::chrono::milliseconds(500))) {
          ROS_INFO_STREAM("NEW MESSAGE ON TOPIC " << msg.get()->get_topic());
          if (msg->get_topic() == ROBOT_CONFIG) {
            const std::string rc_payload = msg->get_payload_str();
            config::RobotConfig robotConfig;
            robotConfig.ParseFromString(rc_payload);
            handleRobotConfig(robotConfig, n);
          } else if (msg->get_topic() == DATA_CONFIG) {
            const std::string dc_payload = msg->get_payload_str();
            config::DataConfig dataConfig;
            dataConfig.ParseFromString(dc_payload);
            handleDataConfig(dataConfig, n);
          } else if (msg->get_topic() == TRAJECTORY_CONFIG) {
            const std::string tc_payload = msg->get_payload_str();
            plan::Trajectory trajectoryConfig;
            trajectoryConfig.ParseFromString(tc_payload);
            handleNewTrajectory(trajectoryConfig, n, posePublisher);
          }
        }
      } else {
        ROS_ERROR_STREAM_NAMED("MqttToRosNode", "Not connected! Unable to listen to messages.");
      }
    }
    
    int main(int argc, char *argv[]) {
      ros::init(argc, argv, "mqtt_listener");
      ros::NodeHandle n("panda_mqtt_connector");
    
      ros::Publisher posePublisher = n.advertise<geometry_msgs::PoseArray>("poses", 1000);
    
      std::string server;
      if (!n.getParam("server", server)) {
        ROS_ERROR_STREAM("Could not get string value for " << n.getNamespace() << "/server from param server");
        return -1;
      }
    
      mqttUtil = new MqttUtil(CLIENT_ID, server);
    
      std::string robotConfig = "robotconfig";
    
      std::map<std::string, std::string> topics;
      if (!n.getParam("topics", topics)) {
        ROS_ERROR_STREAM("Could not get string value for " << n.getNamespace() << "/topics from param server");
      }
    
      for (const auto &topic : topics) {
        ROS_INFO_STREAM("Listening to MQTT topic " << topic.second);
        mqttUtil->addTopic(topic.second);
      }
    
      if (!mqttUtil->connect()) {
        ROS_ERROR_STREAM("Unable to connect to MQTT server. Exiting...");
        return -1;
      }
    
      while (ros::ok()) {
        receiveMqttMessages(n, posePublisher);
        ros::spinOnce();
      }
    
      return 0;
    }