update motion_speed_factor using ros messages rather than the parameter

server, so the value can be observed

src/MqttToRosNode.cpp

@@ -6,6 +6,7 @@
 
 #include "panda_mqtt_connector/Trajectory.h"
 #include "panda_mqtt_connector/Waypoint.h"
+#include "std_msgs/Float64.h"
 
 #include "config.pb.h"
 #include "trajectory.pb.h"
@@ -18,9 +19,11 @@ const std::string TRAJECTORY_CONFIG{"trajectory"};
 
 MqttUtil *mqttUtil = nullptr;
 
-void handleRobotConfig(const config::RobotConfig &robotConfig, const ros::NodeHandle &n) {
+void handleRobotConfig(const config::RobotConfig &robotConfig, const ros::Publisher &velocityPublisher) {
   ROS_INFO_STREAM("Retrieved new target-speed: " << robotConfig.speed());
-  n.setParam("robot_speed_factor", robotConfig.speed());
+  std_msgs::Float64 velocity;
+  velocity.data = robotConfig.speed();
+  velocityPublisher.publish(velocity);
 }
 
 void handleNewTrajectory(const plan::Trajectory &protoTrajectory, const ros::Publisher &trajectoryPublisher) {
@@ -61,7 +64,7 @@ void handleNewTrajectory(const plan::Trajectory &protoTrajectory, const ros::Pub
 
 }
 
-void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &trajectoryPublisher) {
+void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &trajectoryPublisher, const ros::Publisher &velocityPublisher) {
   if (mqttUtil->ensureConnection()) {
     mqtt::const_message_ptr msg;
     if (mqttUtil->getClient().try_consume_message_for(&msg, std::chrono::milliseconds(500))) {
@@ -70,7 +73,7 @@ void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &traject
         const std::string rc_payload = msg->get_payload_str();
         config::RobotConfig robotConfig;
         robotConfig.ParseFromString(rc_payload);
-        handleRobotConfig(robotConfig, n);
+        handleRobotConfig(robotConfig, velocityPublisher);
       } else if (msg->get_topic() == TRAJECTORY_CONFIG) {
         const std::string tc_payload = msg->get_payload_str();
         plan::Trajectory trajectoryConfig;
@@ -88,6 +91,7 @@ int main(int argc, char *argv[]) {
   ros::NodeHandle n("panda_mqtt_connector");
 
   ros::Publisher trajectoryPublisher = n.advertise<panda_mqtt_connector::Trajectory>("trajectory_update", 1000);
+  ros::Publisher maxVelocityPublisher = n.advertise<std_msgs::Float64>("max_velocity", 1000);
 
   std::string server;
   if (!n.getParam("server", server)) {
@@ -115,7 +119,7 @@ int main(int argc, char *argv[]) {
   }
 
   while (ros::ok()) {
-    receiveMqttMessages(n, trajectoryPublisher);
+    receiveMqttMessages(n, trajectoryPublisher, maxVelocityPublisher);
     ros::spinOnce();
   }
 

src/TimedPlanner.cpp

@@ -126,14 +126,6 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning
         }
 
         for (auto trajectory : split(group, plan)) {
-          double motionSpeedFactor;
-          if (!node_handle.getParam("robot_speed_factor", motionSpeedFactor)) {
-            motionSpeedFactor = default_velocity;
-          }
-          if (motionSpeedFactor <= 0 || motionSpeedFactor > 1.0) {
-            ROS_ERROR_STREAM("Invalid motion speed factor " << motionSpeedFactor << ". Must be in (0,1].");
-            return;
-          }
           ROS_INFO_STREAM(
               "Moving to the next waypoint with speed factor " << motionSpeedFactor << " in mode " << waypoint.mode);
           TrajectoryUtil::applyMotionSpeedFactor(trajectory, motionSpeedFactor);
@@ -157,6 +149,19 @@ void TimedPlanner::newTrajectoryCallback(const panda_mqtt_connector::Trajectory:
   nextTrajectory = *msg;
 }
 
+void TimedPlanner::newMotionSpeedFactorCallback(const std_msgs::Float64ConstPtr &msg) {
+  ROS_INFO_STREAM("Received new motionSpeedFactor " << msg->data);
+
+  if (msg->data <= 0 || msg->data > 1.0) {
+    ROS_ERROR_STREAM(
+        "Invalid motion speed factor " << msg->data << ". Must be in (0,1]. Setting to default " << default_velocity);
+    motionSpeedFactor = default_velocity;
+  } else {
+    motionSpeedFactor = msg->data;
+  }
+
+}
+
 TimedPlanner::TimedPlanner(moveit::planning_interface::MoveGroupInterface &group, const double defaultVelocity,
                            std::string defaultPlanningMode) : default_velocity(
     defaultVelocity), default_planning_mode(std::move(defaultPlanningMode)) {
@@ -229,6 +234,21 @@ int main(int argc, char **argv) {
   ros::Subscriber sub = node_handle.subscribe("trajectory_update", 1000, &TimedPlanner::newTrajectoryCallback,
                                               &planner);
 
+  ros::Subscriber velocitySubscriber = node_handle.subscribe("max_velocity", 1000,
+                                                             &TimedPlanner::newMotionSpeedFactorCallback,
+                                                             &planner);
+
+  // init motionSpeedFactor
+  double factor = -1;
+  if (node_handle.getParam("robot_speed_factor", factor)) {
+    ros::Publisher pub = node_handle.advertise<std_msgs::Float64>("max_velocity", 1000);
+    std_msgs::Float64 velocity;
+    velocity.data = factor;
+    pub.publish(velocity);
+  } else {
+    ROS_WARN_STREAM("No default robot_speed_factor provided!");
+  }
+
   // add the ground plate
   std::vector<moveit_msgs::CollisionObject> collision_objects;
   constructPlate(collision_objects, 5.0, 5.0);

src/TimedPlanner.h

@@ -9,6 +9,7 @@
 #include <boost/optional.hpp>
 #include "util/TrajectoryUtil.h"
 
+#include "std_msgs/Float64.h"
 #include "panda_mqtt_connector/Trajectory.h"
 
 using moveit::planning_interface::MoveGroupInterface;
@@ -27,8 +28,11 @@ public:
   TimedPlanner(MoveGroupInterface &group, double defaultVelocity, std::string defaultPlanningMode);
 
   void newTrajectoryCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg);
+  void newMotionSpeedFactorCallback(const std_msgs::Float64ConstPtr &msg);
 
 private:
+  double motionSpeedFactor;
+
   panda_mqtt_connector::Trajectory currentTrajectory;
   boost::optional<panda_mqtt_connector::Trajectory> nextTrajectory;