Select Git revision
abstract__base__implementation_8h.html
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);