Skip to content
Snippets Groups Projects
Select Git revision
  • 31199a0ac61b59ef6bc90f5676f2fb3a689361f1
  • main default protected
  • mg2bt
  • Part1
4 results

abstract__base__implementation_8h.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);