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 46a7a2c63e..eb177ab7ae 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 @@ -148,12 +148,6 @@ class DiffDriveController : public controller_interface::ChainableControllerInte rclcpp::Time previous_update_timestamp_{0}; - // publish rate limiter - // TODO(bhavin-umatiya): Remove these two member variables - double publish_rate_ = 50.0; - rclcpp::Duration publish_period_ = rclcpp::Duration::from_seconds(0.0); - rclcpp::Time previous_publish_timestamp_{0}; - rclcpp::Service::SharedPtr set_odom_service_; std::atomic set_odom_requested_{false}; realtime_tools::RealtimeThreadSafeBox diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 45e9db2409..24dadce198 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -271,54 +271,31 @@ controller_interface::return_type DiffDriveController::update_and_write_commands tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - // TODO(bhavin-umatiya): Remove publish rate functionality - bool should_publish = false; - if (previous_publish_timestamp_.get_clock_type() != time.get_clock_type()) + if (realtime_odometry_publisher_) { - should_publish = true; - } - else if (previous_publish_timestamp_ + publish_period_ <= time) - { - should_publish = true; + odometry_message_.header.stamp = time; + odometry_message_.pose.pose.position.x = odometry_.getX(); + odometry_message_.pose.pose.position.y = odometry_.getY(); + odometry_message_.pose.pose.orientation.x = orientation.x(); + odometry_message_.pose.pose.orientation.y = orientation.y(); + odometry_message_.pose.pose.orientation.z = orientation.z(); + odometry_message_.pose.pose.orientation.w = orientation.w(); + odometry_message_.twist.twist.linear.x = odometry_.getLinear(); + odometry_message_.twist.twist.angular.z = odometry_.getAngular(); + realtime_odometry_publisher_->try_publish(odometry_message_); } - if (should_publish) + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_) { - if (previous_publish_timestamp_.get_clock_type() != time.get_clock_type()) - { - previous_publish_timestamp_ = time; - } - else - { - previous_publish_timestamp_ += publish_period_; - } - - if (realtime_odometry_publisher_) - { - odometry_message_.header.stamp = time; - odometry_message_.pose.pose.position.x = odometry_.getX(); - odometry_message_.pose.pose.position.y = odometry_.getY(); - odometry_message_.pose.pose.orientation.x = orientation.x(); - odometry_message_.pose.pose.orientation.y = orientation.y(); - odometry_message_.pose.pose.orientation.z = orientation.z(); - odometry_message_.pose.pose.orientation.w = orientation.w(); - odometry_message_.twist.twist.linear.x = odometry_.getLinear(); - odometry_message_.twist.twist.angular.z = odometry_.getAngular(); - realtime_odometry_publisher_->try_publish(odometry_message_); - } - - if (params_.enable_odom_tf && realtime_odometry_transform_publisher_) - { - auto & transform = odometry_transform_message_.transforms.front(); - transform.header.stamp = time; - transform.transform.translation.x = odometry_.getX(); - transform.transform.translation.y = odometry_.getY(); - transform.transform.rotation.x = orientation.x(); - transform.transform.rotation.y = orientation.y(); - transform.transform.rotation.z = orientation.z(); - transform.transform.rotation.w = orientation.w(); - realtime_odometry_transform_publisher_->try_publish(odometry_transform_message_); - } + auto & transform = odometry_transform_message_.transforms.front(); + transform.header.stamp = time; + transform.transform.translation.x = odometry_.getX(); + transform.transform.translation.y = odometry_.getY(); + transform.transform.rotation.x = orientation.x(); + transform.transform.rotation.y = orientation.y(); + transform.transform.rotation.z = orientation.z(); + transform.transform.rotation.w = orientation.w(); + realtime_odometry_transform_publisher_->try_publish(odometry_transform_message_); } } @@ -523,19 +500,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( odometry_message_.header.frame_id = odom_frame_id; odometry_message_.child_frame_id = base_frame_id; - // limit the publication on the topics /odom and /tf - publish_rate_ = params_.publish_rate; - publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); - - // TODO(bhavin-umatiya): Remove this warning - if (publish_rate_ > 0.0 && !std::isnan(publish_rate_)) - { - RCLCPP_WARN( - get_node()->get_logger(), - "[deprecated] publish_rate parameter is deprecated and will be removed in a future release. " - "The publish rate of odometry and TF messages should not be limited."); - } - // initialize odom values zeros odometry_message_.twist = geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); @@ -567,7 +531,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( std::placeholders::_2, std::placeholders::_3)); previous_update_timestamp_ = get_node()->get_clock()->now(); - previous_publish_timestamp_ = get_node()->get_clock()->now(); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 97acd56721..b0155c58f5 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -122,13 +122,7 @@ diff_drive_controller: description: "Size of the rolling window for calculation of mean velocity use in odometry.", read_only: true, } - # TODO(bhavin-umatiya): Remove this parameter as it is deprecated - publish_rate: { - type: double, - default_value: 50.0, # Hz - description: "Publishing rate (Hz) of the odometry and TF messages. This parameter is deprecated and will be removed in a future release.", - read_only: true, - } + linear: x: max_velocity: { diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 6cbbbe06c6..5c9e7645a4 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -18,6 +18,7 @@ diff_drive_controller * Parameter ``tf_frame_prefix_enable`` got deprecated and will be removed in a future release (`#1997 `_). * Now any tilde ("~") character in ``tf_frame_prefix`` is substituted with node namespace. (`#1997 `_). * Set odometry service added to be used at runtime. (`#2096 `_). +* Removed deprecated parameter ``publish_rate`` and associated rate-limiting logic. (`#2259 `_). mecanum_drive_controller *****************************