diff --git a/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control_node.hpp b/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control_node.hpp index f7aa124b..59ab536c 100644 --- a/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control_node.hpp +++ b/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::SharedPtr hazard_detection_sub_; rclcpp::Subscription::SharedPtr teleop_subscription_; + rclcpp::Subscription::SharedPtr teleop_unstamped_subscription_; rclcpp::Subscription::SharedPtr odom_pose_sub_; rclcpp::Subscription::SharedPtr kidnap_sub_; rclcpp::Publisher::SharedPtr cmd_vel_out_pub_; diff --git a/irobot_create_common/irobot_create_nodes/src/motion_control/drive_goal_behaviors.cpp b/irobot_create_common/irobot_create_nodes/src/motion_control/drive_goal_behaviors.cpp index 9f6a2c37..0d38cab2 100644 --- a/irobot_create_common/irobot_create_nodes/src/motion_control/drive_goal_behaviors.cpp +++ b/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 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 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 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); diff --git a/irobot_create_common/irobot_create_nodes/src/motion_control_node.cpp b/irobot_create_common/irobot_create_nodes/src/motion_control_node.cpp index a1dda9e5..77ba54bf 100644 --- a/irobot_create_common/irobot_create_nodes/src/motion_control_node.cpp +++ b/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( "cmd_vel", rclcpp::SensorDataQoS(), std::bind(&MotionControlNode::commanded_velocity_callback, this, _1)); + teleop_unstamped_subscription_ = this->create_subscription( + "cmd_vel_unstamped", rclcpp::SensorDataQoS(), + std::bind(&MotionControlNode::commanded_velocity_unstamped_callback, this, _1)); odom_pose_sub_ = this->create_subscription( "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 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 lock(current_state_mutex_); @@ -368,6 +395,7 @@ void MotionControlNode::reset_last_teleop_cmd() const std::lock_guard lock(mutex_); last_teleop_cmd_ = get_default_velocity_cmd(); + last_teleop_cmd_.header.stamp = this->get_clock()->now(); last_teleop_ts_ = this->now(); }