From 46865fe7bbc1c5bb477ceeda9e1cade85f8a7d38 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Tue, 13 Aug 2024 10:30:49 -0400 Subject: [PATCH] Revert get_default_velocity_cmd to have no parameters as it's used externally, 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 --- .../motion_control_node.hpp | 3 ++ .../motion_control/drive_goal_behaviors.cpp | 17 +++++------ .../src/motion_control_node.cpp | 28 +++++++++++++++++++ 3 files changed, 38 insertions(+), 10 deletions(-) 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(); }