diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 2178008725..cef90d026a 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -167,11 +167,6 @@ class TricycleController : public controller_interface::ControllerInterface TractionLimiter limiter_traction_; SteeringLimiter limiter_steering_; - // 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 --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 114731242c..4f4e22ec9d 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -71,7 +71,6 @@ CallbackReturn TricycleController::on_init() auto_declare("publish_ackermann_command", publish_ackermann_command_); auto_declare("velocity_rolling_window_size", 10); auto_declare("use_stamped_vel", use_stamped_vel_); - auto_declare("publish_rate", publish_rate_); auto_declare("traction.max_velocity", NAN); auto_declare("traction.min_velocity", NAN); @@ -169,40 +168,35 @@ controller_interface::return_type TricycleController::update( tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - if (previous_publish_timestamp_ + publish_period_ < time) + if (realtime_odometry_publisher_->trylock()) { - previous_publish_timestamp_ += publish_period_; - - if (realtime_odometry_publisher_->trylock()) + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.stamp = time; + if (!odom_params_.odom_only_twist) { - auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.stamp = time; - if (!odom_params_.odom_only_twist) - { - 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(); + 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 = 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 (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + { + auto & transform = realtime_odometry_transform_publisher_->msg_.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_->unlockAndPublish(); } // Compute wheel velocity and angle @@ -305,9 +299,6 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); - publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); - publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); - try { limiter_traction_ = TractionLimiter( @@ -421,8 +412,6 @@ CallbackReturn TricycleController::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; - previous_publish_timestamp_ = get_node()->get_clock()->now(); - // initialize odom values zeros odometry_message.twist = geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL);