diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index 30fce79946..ddbb322b8a 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -50,9 +50,6 @@ Subscribers ~/cmd_vel [geometry_msgs/msg/TwistStamped] Velocity command for the controller, if ``use_stamped_vel=true``. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. -~/cmd_vel_unstamped [geometry_msgs::msg::Twist] - Velocity command for the controller, if ``use_stamped_vel=false``. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. - Publishers ,,,,,,,,,,, diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 3584a1d1f7..9523fe4050 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -126,8 +126,6 @@ class DiffDriveController : public controller_interface::ControllerInterface bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - rclcpp::Subscription::SharedPtr - velocity_command_unstamped_subscriber_ = nullptr; realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; @@ -150,7 +148,6 @@ class DiffDriveController : public controller_interface::ControllerInterface rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED}; bool is_halted = false; - bool use_stamped_vel_ = true; bool reset(); void halt(); diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index be4ba1fc5d..78709d75c0 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -31,7 +31,6 @@ namespace { constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; -constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped"; constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out"; constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; @@ -302,7 +301,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( cmd_vel_timeout_ = std::chrono::milliseconds{static_cast(params_.cmd_vel_timeout * 1000.0)}; publish_limited_velocity_ = params_.publish_limited_velocity; - use_stamped_vel_ = params_.use_stamped_vel; limiter_linear_ = SpeedLimiter( params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits, @@ -340,50 +338,25 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( previous_commands_.emplace(empty_twist); // initialize command subscriber - if (use_stamped_vel_) - { - velocity_command_subscriber_ = get_node()->create_subscription( - DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + velocity_command_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) { - if (!subscriber_is_active_) - { - RCLCPP_WARN( - get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); - return; - } - if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) - { - RCLCPP_WARN_ONCE( - get_node()->get_logger(), - "Received TwistStamped with zero timestamp, setting it to current " - "time, this message will only be shown once"); - msg->header.stamp = get_node()->get_clock()->now(); - } - received_velocity_msg_ptr_.set(std::move(msg)); - }); - } - else - { - velocity_command_unstamped_subscriber_ = - get_node()->create_subscription( - DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void - { - if (!subscriber_is_active_) - { - RCLCPP_WARN( - get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); - return; - } - - // Write fake header in the stored stamped command - std::shared_ptr twist_stamped; - received_velocity_msg_ptr_.get(twist_stamped); - twist_stamped->twist = *msg; - twist_stamped->header.stamp = get_node()->get_clock()->now(); - }); - } + RCLCPP_WARN(get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) + { + RCLCPP_WARN_ONCE( + get_node()->get_logger(), + "Received TwistStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->get_clock()->now(); + } + received_velocity_msg_ptr_.set(std::move(msg)); + }); // initialize odometry publisher and message odometry_publisher_ = get_node()->create_publisher( @@ -534,7 +507,6 @@ bool DiffDriveController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - velocity_command_unstamped_subscriber_.reset(); received_velocity_msg_ptr_.set(nullptr); is_halted = false; diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 3f370bcc65..6ff0083233 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -111,11 +111,6 @@ diff_drive_controller: default_value: 10, description: "Size of the rolling window for calculation of mean velocity use in odometry.", } - use_stamped_vel: { - type: bool, - default_value: true, - description: "Use stamp from input velocity message to calculate how old the command actually is.", - } publish_rate: { type: double, default_value: 50.0, # Hz