diff --git a/src/MqttToRosNode.cpp b/src/MqttToRosNode.cpp index c40e1d45b8041c462dc007bc5a5e26ec8cd87656..11de299835d279f1dd56105cb46713941c5a3900 100644 --- a/src/MqttToRosNode.cpp +++ b/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(); } diff --git a/src/TimedPlanner.cpp b/src/TimedPlanner.cpp index 6cc4cfbb88d70644d67ca62340ab33920555b7ca..4edaaaab86ed1f6a4547e99924e221d5bfe05c4e 100644 --- a/src/TimedPlanner.cpp +++ b/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); diff --git a/src/TimedPlanner.h b/src/TimedPlanner.h index 7c5ad38ea04c88f840ad0e75a9de795668e3529d..2c1b39cabad3eebab1d3eaf5066e5714ac50c9ca 100644 --- a/src/TimedPlanner.h +++ b/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;