Add publish_rate option for the diff_drive_controller (#278)

Author: BenjaminHug8

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

@@ -158,6 +158,11 @@ class DiffDriveController : public controller_interface::ControllerInterface
 
   rclcpp::Time previous_update_timestamp_{0};
 
+  // publish rate limiter
+  double publish_rate_ = 50.0;
+  rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0);
+  rclcpp::Time previous_publish_timestamp_{0};
+
   bool is_halted = false;
   bool use_stamped_vel_ = true;
 

diff_drive_controller/src/diff_drive_controller.cpp

@@ -100,6 +100,7 @@ CallbackReturn DiffDriveController::on_init()
     auto_declare<double>("angular.z.min_acceleration", NAN);
     auto_declare<double>("angular.z.max_jerk", NAN);
     auto_declare<double>("angular.z.min_jerk", NAN);
+    auto_declare<double>("publish_rate", publish_rate_);
   }
   catch (const std::exception & e)
   {
@@ -224,32 +225,37 @@ controller_interface::return_type DiffDriveController::update(
   tf2::Quaternion orientation;
   orientation.setRPY(0.0, 0.0, odometry_.getHeading());
 
-  if (realtime_odometry_publisher_->trylock())
+  if (previous_publish_timestamp_ + publish_period_ < current_time)
   {
-    auto & odometry_message = realtime_odometry_publisher_->msg_;
-    odometry_message.header.stamp = current_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_->unlockAndPublish();
-  }
+    previous_publish_timestamp_ += publish_period_;
 
-  if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock())
-  {
-    auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front();
-    transform.header.stamp = current_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_->unlockAndPublish();
+    if (realtime_odometry_publisher_->trylock())
+    {
+      auto & odometry_message = realtime_odometry_publisher_->msg_;
+      odometry_message.header.stamp = current_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_->unlockAndPublish();
+    }
+
+    if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock())
+    {
+      auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front();
+      transform.header.stamp = current_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_->unlockAndPublish();
+    }
   }
 
   const auto update_dt = current_time - previous_update_timestamp_;
@@ -464,6 +470,11 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State &
   odometry_message.header.frame_id = odom_params_.odom_frame_id;
   odometry_message.child_frame_id = odom_params_.base_frame_id;
 
+  // limit the publication on the topics /odom and /tf
+  publish_rate_ = node_->get_parameter("publish_rate").as_double();
+  publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_);
+  previous_publish_timestamp_ = node_->get_clock()->now();
+
   // initialize odom values zeros
   odometry_message.twist =
     geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL);