Revert get_default_velocity_cmd to have no parameters as it's used ex…

…ternally, set the stamp outside that function (since we may create the command, do some math, and then publish it later). Add an unstamped velocity command input

irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control_node.hpp

@@ -13,6 +13,7 @@
 #include "tf2_ros/transform_listener.h"
 
 #include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
 #include "irobot_create_msgs/msg/kidnap_status.hpp"
 #include "irobot_create_msgs/msg/hazard_detection.hpp"
 #include "irobot_create_msgs/msg/wheel_status.hpp"
@@ -83,6 +84,7 @@ class MotionControlNode : public rclcpp::Node
 
   /// \brief Callback for new velocity commands
   void commanded_velocity_callback(geometry_msgs::msg::TwistStamped::ConstSharedPtr msg);
+  void commanded_velocity_unstamped_callback(geometry_msgs::msg::Twist::ConstSharedPtr msg);
 
   /// \brief Callback for robot odometry
   void robot_pose_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg);
@@ -107,6 +109,7 @@ class MotionControlNode : public rclcpp::Node
   rclcpp::Subscription<irobot_create_msgs::msg::HazardDetectionVector>::SharedPtr
     hazard_detection_sub_;
   rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr teleop_subscription_;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr teleop_unstamped_subscription_;
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_pose_sub_;
   rclcpp::Subscription<irobot_create_msgs::msg::KidnapStatus>::SharedPtr kidnap_sub_;
   rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_vel_out_pub_;

irobot_create_common/irobot_create_nodes/src/motion_control/drive_goal_behaviors.cpp

@@ -14,9 +14,7 @@
 namespace irobot_create_nodes
 {
 //// Helper functions ////
-geometry_msgs::msg::TwistStamped get_default_velocity_cmd(
-  const rclcpp::Time & current_time
-)
+geometry_msgs::msg::TwistStamped get_default_velocity_cmd()
 {
   geometry_msgs::msg::TwistStamped default_cmd;
   default_cmd.twist.linear.x = 0;
@@ -25,7 +23,6 @@ geometry_msgs::msg::TwistStamped get_default_velocity_cmd(
   default_cmd.twist.angular.x = 0;
   default_cmd.twist.angular.y = 0;
   default_cmd.twist.angular.z = 0;
-  default_cmd.header.stamp = current_time;
   return default_cmd;
 }
 geometry_msgs::msg::PoseStamped get_current_pose_stamped(
@@ -76,8 +73,8 @@ void DriveArcBehavior::initialize_goal(const irobot_create_msgs::action::DriveAr
     goal.radius, goal.angle, max_speed);
 
   const std::lock_guard<std::mutex> lock(drive_arc_params_mutex_);
-  rclcpp::Time current_time = clock_->now();
-  arc_velocity_cmd_ = get_default_velocity_cmd(current_time);
+  arc_velocity_cmd_ = get_default_velocity_cmd();
+  arc_velocity_cmd_.header.stamp = clock_->now();
   arc_velocity_cmd_.twist.linear.x = max_speed;
   arc_velocity_cmd_.twist.angular.z = std::copysign(max_speed / std::abs(goal.radius), goal.angle);
   if (goal.translate_direction == irobot_create_msgs::action::DriveArc::Goal::TRANSLATE_BACKWARD) {
@@ -170,8 +167,8 @@ void DriveDistanceBehavior::initialize_goal(
 {
   first_iter_ = true;
   const std::lock_guard<std::mutex> lock(drive_distance_params_mutex_);
-  rclcpp::Time current_time = clock_->now();
-  drive_velocity_cmd_ = get_default_velocity_cmd(current_time);
+  drive_velocity_cmd_ = get_default_velocity_cmd();
+  drive_velocity_cmd_.header.stamp = clock_->now();
   goal_travel_ = goal.distance;
   travel_distance_sq_ = goal_travel_ * goal_travel_;
   remaining_travel_ = goal_travel_;
@@ -256,8 +253,8 @@ void RotateAngleBehavior::initialize_goal(
 {
   first_iter_ = true;
   const std::lock_guard<std::mutex> lock(rotate_angle_params_mutex_);
-  rclcpp::Time current_time = clock_->now();
-  rotate_velocity_cmd_ = get_default_velocity_cmd(current_time);
+  rotate_velocity_cmd_ = get_default_velocity_cmd();
+  rotate_velocity_cmd_.header.stamp = clock_->now();
   remain_angle_travel_ = goal.angle;
   start_sign_ = std::copysign(1, remain_angle_travel_);
   float max_speed = std::min(max_rot_speed_radps_, goal.max_rotation_speed);

irobot_create_common/irobot_create_nodes/src/motion_control_node.cpp

@@ -114,6 +114,9 @@ MotionControlNode::MotionControlNode(const rclcpp::NodeOptions & options)
   teleop_subscription_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
     "cmd_vel", rclcpp::SensorDataQoS(),
     std::bind(&MotionControlNode::commanded_velocity_callback, this, _1));
+  teleop_unstamped_subscription_ = this->create_subscription<geometry_msgs::msg::Twist>(
+    "cmd_vel_unstamped", rclcpp::SensorDataQoS(),
+    std::bind(&MotionControlNode::commanded_velocity_unstamped_callback, this, _1));
 
   odom_pose_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
     "odom", rclcpp::SensorDataQoS(),
@@ -349,6 +352,30 @@ void MotionControlNode::commanded_velocity_callback(geometry_msgs::msg::TwistSta
   last_teleop_ts_ = this->now();
 }
 
+
+void MotionControlNode::commanded_velocity_unstamped_callback(geometry_msgs::msg::Twist::ConstSharedPtr msg)
+{
+  geometry_msgs::msg::TwistStamped stamped_msg;
+  stamped_msg.twist = *msg;
+  stamped_msg.header.stamp = this->get_clock()->now();
+
+  if (scheduler_->has_behavior()) {
+    const auto time_now = this->now();
+    if (time_now - auto_override_print_ts_ > repeat_print_) {
+      auto_override_print_ts_ = time_now;
+      RCLCPP_WARN(
+        this->get_logger(),
+        "Ignoring velocities commanded while an autonomous behavior is running!");
+    }
+    return;
+  }
+
+  const std::lock_guard<std::mutex> lock(mutex_);
+
+  last_teleop_cmd_ = stamped_msg;
+  last_teleop_ts_ = this->now();
+}
+
 void MotionControlNode::robot_pose_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg)
 {
   const std::lock_guard<std::mutex> lock(current_state_mutex_);
@@ -368,6 +395,7 @@ void MotionControlNode::reset_last_teleop_cmd()
   const std::lock_guard<std::mutex> lock(mutex_);
 
   last_teleop_cmd_ = get_default_velocity_cmd();
+  last_teleop_cmd_.header.stamp = this->get_clock()->now();
   last_teleop_ts_ = this->now();
 }