Select Git revision
moveit_mediator.cpp
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;
}