Fix pose/twist publisher rate drop when downsampling publish rate (#18)

* Updated how time between pose/twist publish is computed

* cleaned up cmake. clamped publish rate accumulation. reset time accumulator in on_activate

---------

Co-authored-by: Vidullan Surendran <vsurendran@theaiinstitute.com>

Author: vidullan

CMakeLists.txt

@@ -18,11 +18,37 @@ endif()
 # Enable precompiled headers
 option(USE_PRECOMPILED_HEADERS "Use precompiled headers" ON)
 
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-    add_compile_options(-Wall -Wextra -Wpedantic)
+option(CHECK_TIDY "Adds clang-tidy tests" OFF)
+
+option(ENABLE_NATIVE_OPTIMIZATION
+  "Enable CPU-specific optimizations (-march=native)"
+  OFF
+)
+
+# Default to release build
+if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE)
 endif()
 
-option(CHECK_TIDY "Adds clang-tidy tests" OFF)
+# Compiler flags by build type
+add_library(project_compile_options INTERFACE)
+target_compile_options(project_compile_options INTERFACE
+  # Warnings (build only)
+  $<$<CXX_COMPILER_ID:GNU,Clang>:-Wall -Wextra -Wpedantic>
+
+  # Debug
+  $<$<AND:$<CONFIG:Debug>,$<CXX_COMPILER_ID:GNU,Clang>>:-O0 -g>
+
+  # Release
+  $<$<AND:$<CONFIG:Release>,$<CXX_COMPILER_ID:GNU,Clang>>:-O3 -DNDEBUG>
+)
+
+# Optional CPU-specific optimizations (non-portable)
+if(ENABLE_NATIVE_OPTIMIZATION)
+  target_compile_options(project_compile_options INTERFACE
+    $<$<AND:$<CONFIG:Release>,$<CXX_COMPILER_ID:GNU,Clang>>:-march=native>
+)
+endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
@@ -65,6 +91,11 @@ add_library(
         src/twist_broadcaster.cpp
 )
 
+target_link_libraries(${PROJECT_NAME}
+  PRIVATE
+    project_compile_options
+)
+
 target_include_directories(${PROJECT_NAME}
     PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

include/crisp_controllers/pose_broadcaster.hpp

@@ -64,7 +64,9 @@ class PoseBroadcaster
     {"JointModelRUBX", "JointModelRUBY", "JointModelRUBZ"};
 
   Eigen::VectorXd q;
-  rclcpp::Time last_publish_time_;
+  
+  rclcpp::Duration publish_elapsed_{0,0};
+  rclcpp::Duration publish_interval_{0,0};
 };
 
 } // namespace crisp_controllers

include/crisp_controllers/twist_broadcaster.hpp

@@ -65,7 +65,9 @@ class TwistBroadcaster
 
   Eigen::VectorXd q;
   Eigen::VectorXd q_dot;
-  rclcpp::Time last_publish_time_;
+  
+  rclcpp::Duration publish_elapsed_{0,0};
+  rclcpp::Duration publish_interval_{0,0};
 };
 
 } // namespace crisp_controllers

src/pose_broadcaster.cpp

@@ -35,7 +35,7 @@ PoseBroadcaster::state_interface_configuration() const {
 
 controller_interface::return_type
 PoseBroadcaster::update(const rclcpp::Time &time,
-                                   const rclcpp::Duration & /*period*/) {
+                                   const rclcpp::Duration &period) {
 
   size_t num_joints = params_.joints.size();
   Eigen::VectorXd q_pin = Eigen::VectorXd::Zero(model_.nq);
@@ -63,28 +63,28 @@ PoseBroadcaster::update(const rclcpp::Time &time,
       Eigen::Quaterniond(current_pose.rotation());
 
   // Decide whether to publish the pose or not
-  bool should_publish = true;
-  if (params_.publish_frequency > 0.0) {
-    auto time_since_last = time - last_publish_time_;
-    auto min_interval = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency);
-    should_publish = time_since_last >= min_interval;
-  }
-
-  if (should_publish && realtime_pose_publisher_ && realtime_pose_publisher_->trylock())
+  publish_elapsed_ = publish_elapsed_ + period;
+  bool should_publish = (publish_elapsed_ >= publish_interval_) || 
+                          (publish_interval_.nanoseconds() == 0); 
+  if (should_publish && realtime_pose_publisher_)
   {
-    auto & pose_msg = realtime_pose_publisher_->msg_;
-
-    pose_msg.header.stamp = time;
-    pose_msg.header.frame_id = params_.base_frame;
-    pose_msg.pose.position.x = current_pose.translation()[0];
-    pose_msg.pose.position.y = current_pose.translation()[1];
-    pose_msg.pose.position.z = current_pose.translation()[2];
-    pose_msg.pose.orientation.x = current_quaternion.x();
-    pose_msg.pose.orientation.y = current_quaternion.y();
-    pose_msg.pose.orientation.z = current_quaternion.z();
-    pose_msg.pose.orientation.w = current_quaternion.w();
-    realtime_pose_publisher_->unlockAndPublish();
-    last_publish_time_ = time;
+    if (realtime_pose_publisher_->trylock()) {
+      auto & pose_msg = realtime_pose_publisher_->msg_;
+      pose_msg.header.stamp = time;
+      pose_msg.header.frame_id = params_.base_frame;
+      pose_msg.pose.position.x = current_pose.translation()[0];
+      pose_msg.pose.position.y = current_pose.translation()[1];
+      pose_msg.pose.position.z = current_pose.translation()[2];
+      pose_msg.pose.orientation.x = current_quaternion.x();
+      pose_msg.pose.orientation.y = current_quaternion.y();
+      pose_msg.pose.orientation.z = current_quaternion.z();
+      pose_msg.pose.orientation.w = current_quaternion.w();
+      realtime_pose_publisher_->unlockAndPublish();
+      
+      publish_elapsed_ = publish_elapsed_ - publish_interval_;
+      // clamp to publish only 1 time even if missed multiple intervals
+      publish_elapsed_ = std::min(publish_elapsed_, publish_interval_);      
+    }
   }
 
   return controller_interface::return_type::OK;
@@ -168,12 +168,19 @@ CallbackReturn PoseBroadcaster::on_configure(
       std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::PoseStamped>>(
         pose_publisher_);
 
-  last_publish_time_ = this->get_node()->now();
+  if (params_.publish_frequency > 0.0) {
+    publish_interval_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency);
+  } else {
+    publish_interval_ = rclcpp::Duration(0, 0);  // publish every cycle
+  }
+  
   return CallbackReturn::SUCCESS;
 }
 
 CallbackReturn PoseBroadcaster::on_activate(
     const rclcpp_lifecycle::State & /*previous_state*/) {
+  // reset publish time accumulation
+  publish_elapsed_ = rclcpp::Duration(0, 0); 
   return CallbackReturn::SUCCESS;
 }
 

src/twist_broadcaster.cpp

@@ -36,7 +36,7 @@ TwistBroadcaster::state_interface_configuration() const {
 
 controller_interface::return_type
 TwistBroadcaster::update(const rclcpp::Time &time,
-                         const rclcpp::Duration & /*period*/) {
+                         const rclcpp::Duration &period) {
 
     size_t num_joints = params_.joints.size();
     Eigen::VectorXd q_pin = Eigen::VectorXd::Zero(model_.nq);
@@ -67,27 +67,28 @@ TwistBroadcaster::update(const rclcpp::Time &time,
     auto current_velocity = pinocchio::getFrameVelocity(model_, data_, end_effector_frame_id);
 
     // Decide whether to publish the twist or not
-    bool should_publish = true;
-    if (params_.publish_frequency > 0.0) {
-        auto time_since_last = time - last_publish_time_;
-        auto min_interval = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency);
-        should_publish = time_since_last >= min_interval;
-    }
+    publish_elapsed_ = publish_elapsed_ + period;
+    bool should_publish = (publish_elapsed_ >= publish_interval_) || 
+                            (publish_interval_.nanoseconds() == 0); 
 
-    if (should_publish && realtime_twist_publisher_ && realtime_twist_publisher_->trylock())
+    if (should_publish && realtime_twist_publisher_)
     {
-        auto & twist_msg = realtime_twist_publisher_->msg_;
-
-        twist_msg.header.stamp = time;
-        twist_msg.header.frame_id = params_.end_effector_frame;
-        twist_msg.twist.linear.x = current_velocity.linear()[0];
-        twist_msg.twist.linear.y = current_velocity.linear()[1];
-        twist_msg.twist.linear.z = current_velocity.linear()[2];
-        twist_msg.twist.angular.x = current_velocity.angular()[0];
-        twist_msg.twist.angular.y = current_velocity.angular()[1];
-        twist_msg.twist.angular.z = current_velocity.angular()[2];
-        realtime_twist_publisher_->unlockAndPublish();
-        last_publish_time_ = time;
+        if (realtime_twist_publisher_->trylock()) {
+            auto & twist_msg = realtime_twist_publisher_->msg_;
+            twist_msg.header.stamp = time;
+            twist_msg.header.frame_id = params_.end_effector_frame;
+            twist_msg.twist.linear.x = current_velocity.linear()[0];
+            twist_msg.twist.linear.y = current_velocity.linear()[1];
+            twist_msg.twist.linear.z = current_velocity.linear()[2];
+            twist_msg.twist.angular.x = current_velocity.angular()[0];
+            twist_msg.twist.angular.y = current_velocity.angular()[1];
+            twist_msg.twist.angular.z = current_velocity.angular()[2];
+            realtime_twist_publisher_->unlockAndPublish();
+            
+            publish_elapsed_ = publish_elapsed_ - publish_interval_;
+            // clamp to publish only 1 time even if missed multiple intervals
+            publish_elapsed_ = std::min(publish_elapsed_, publish_interval_);  
+        }
     }
 
     return controller_interface::return_type::OK;
@@ -172,12 +173,19 @@ CallbackReturn TwistBroadcaster::on_configure(
         std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::TwistStamped>>(
             twist_publisher_);
 
-    last_publish_time_ = this->get_node()->now();
+    if (params_.publish_frequency > 0.0) {
+        publish_interval_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency);
+    } else {
+        publish_interval_ = rclcpp::Duration(0, 0);  // publish every cycle
+    }
+
     return CallbackReturn::SUCCESS;
 }
 
 CallbackReturn TwistBroadcaster::on_activate(
     const rclcpp_lifecycle::State & /*previous_state*/) {
+    // reset publish time accumulation
+    publish_elapsed_ = rclcpp::Duration(0, 0); 
     return CallbackReturn::SUCCESS;
 }