// // Created by sebastian on 31.03.20. // #include "config.pb.h" #include "trajectory.pb.h" #include "ros/ros.h" #include "util/MqttUtil.h" const std::string CLIENT_ID{"ros_mqtt_tester"}; MqttUtil *mqttUtil = nullptr; void testTrajectoryUpdate(const ros::NodeHandle& n) { ROS_INFO_STREAM("TRAJECTORY UPDATE TEST: Sending simple two-point trajectory to planner."); plan::Trajectory trajectory{}; trajectory.set_loop(false); plan::Trajectory_Pose* pose = trajectory.add_pose(); pose->set_mode(plan::Trajectory_PlanningMode_FLUID); pose->mutable_orientation()->set_w(0); pose->mutable_orientation()->set_x(0.7071067811865476); pose->mutable_orientation()->set_y(0.7071067811865476); pose->mutable_orientation()->set_z(0); pose->mutable_position()->set_x(0.4); pose->mutable_position()->set_y(0.4); pose->mutable_position()->set_z(0.4); plan::Trajectory_Pose* secondPose = trajectory.add_pose(); secondPose->CopyFrom(*pose); secondPose->mutable_position()->set_x(-0.4); std::string mqtt_msg{}; trajectory.SerializeToString(&mqtt_msg); auto pubmsg = mqtt::make_message("trajectory", mqtt_msg); mqttUtil->getClient().publish(pubmsg); } void testSpeedFactorChange(const ros::NodeHandle &n) { std::string planning_mode; if (!n.getParam("robot_planning_mode", planning_mode)) { ROS_ERROR_STREAM("No " << n.getNamespace() << "/robot_planning_mode set. Aborting planning mode change test."); return; } config::RobotConfig rc; double motionSpeedFactor; if (!n.getParam("robot_speed_factor", motionSpeedFactor)) { ROS_ERROR_STREAM("No " << n.getNamespace() << "/robot_speed_factor set. Aborting planning mode change test."); return; } if (motionSpeedFactor < 0.5) { ROS_INFO_STREAM("SPEED FACTOR CHANGE TEST: changed from " << motionSpeedFactor << " to 0.9"); rc.set_speed(0.9); } else { ROS_INFO_STREAM("SPEED FACTOR CHANGE TEST: changed from " << motionSpeedFactor << " to 0.2"); rc.set_speed(0.2); } std::string mqtt_msg; rc.SerializeToString(&mqtt_msg); auto pubmsg = mqtt::make_message("robotconfig", mqtt_msg); mqttUtil->getClient().publish(pubmsg); } void receiveMqttMessages(const ros::NodeHandle &n) { if (mqttUtil->ensureConnection()) { auto *msg = new mqtt::const_message_ptr; if (mqttUtil->getClient().try_consume_message_for(msg, std::chrono::milliseconds(500))) { ROS_INFO_STREAM("Received message on topic " << msg->get()->get_topic()); if (msg->get()->get_topic() == "test_speed_change") { testSpeedFactorChange(n); } else if (msg->get()->get_topic() == "test_trajectory_update") { testTrajectoryUpdate(n); } } } else { ROS_ERROR_STREAM("Not connected! Unable to listen to messages."); } } int main(int argc, char **argv) { ros::init(argc, argv, "MqttRosConnectionTestNode"); ros::NodeHandle n("panda_mqtt_connector"); ROS_INFO_NAMED("MqttRosConnectionTestNode", "Setting up MQTT."); std::string server; if (!n.getParam("server", server)) { ROS_ERROR("Could not get string value for panda_mqtt_connector/server from param server"); return -1; } mqttUtil = new MqttUtil(CLIENT_ID, server); mqttUtil->addTopic("test_speed_change"); mqttUtil->addTopic("test_trajectory_update"); mqttUtil->connect(); while (ros::ok()) { receiveMqttMessages(n); ros::spinOnce(); } return 0; }