Replace OdomSubscriber with OdomSmoother

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

nav2_controller/include/nav2_controller/controller_server.hpp

@@ -32,7 +32,7 @@
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_ros_common/simple_action_server.hpp"
 #include "nav2_util/robot_utils.hpp"
-#include "nav2_util/odom_subscriber.hpp"
+#include "nav2_util/odometry_utils.hpp"
 #include "nav2_util/twist_publisher.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
@@ -233,7 +233,7 @@ class ControllerServer : public nav2::LifecycleNode
   std::unique_ptr<nav2::NodeThread> costmap_thread_;
 
   // Publishers and subscribers
-  std::unique_ptr<nav2_util::OdomSubscriber> odom_sub_;
+  std::unique_ptr<nav2_util::OdomSmoother> odom_sub_;
   std::unique_ptr<nav2_util::TwistPublisher> vel_publisher_;
   nav2::Subscription<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_sub_;
 

nav2_controller/src/controller_server.cpp

@@ -63,6 +63,9 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
   declare_parameter("publish_zero_velocity", rclcpp::ParameterValue(true));
   declare_parameter("costmap_update_timeout", 0.30);  // 300ms
 
+  declare_parameter("odom_topic", rclcpp::ParameterValue("odom"));
+  declare_parameter("odom_duration", rclcpp::ParameterValue(0.3));
+
   // The costmap node is used in the implementation of the controller
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "local_costmap", std::string{get_namespace()},
@@ -123,8 +126,11 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
   get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_);
   RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
 
-  std::string speed_limit_topic;
+  std::string speed_limit_topic, odom_topic;
   get_parameter("speed_limit_topic", speed_limit_topic);
+  get_parameter("odom_topic", odom_topic);
+  double odom_duration;
+  get_parameter("odom_duration", odom_duration);
   get_parameter("failure_tolerance", failure_tolerance_);
   get_parameter("use_realtime_priority", use_realtime_priority_);
   get_parameter("publish_zero_velocity", publish_zero_velocity_);
@@ -217,7 +223,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
     get_logger(),
     "Controller Server has %s controllers available.", controller_ids_concat_.c_str());
 
-  odom_sub_ = std::make_unique<nav2_util::OdomSubscriber>(node);
+  odom_sub_ = std::make_unique<nav2_util::OdomSmoother>(node, odom_duration, odom_topic);
   vel_publisher_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel");
 
   double costmap_update_timeout_dbl;
@@ -622,7 +628,7 @@ void ControllerServer::computeAndPublishVelocity()
     throw nav2_core::FailedToMakeProgress("Failed to make progress");
   }
 
-  geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getTwist());
+  geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist());
 
   geometry_msgs::msg::TwistStamped cmd_vel_2d;
 
@@ -790,7 +796,7 @@ bool ControllerServer::isGoalReached()
     return false;
   }
 
-  geometry_msgs::msg::Twist velocity = getThresholdedTwist(odom_sub_->getTwist());
+  geometry_msgs::msg::Twist velocity = getThresholdedTwist(odom_sub_->getRawTwist());
 
   geometry_msgs::msg::PoseStamped transformed_end_pose;
   nav2_util::transformPoseInTargetFrame(

nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp

@@ -27,7 +27,7 @@
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_ros_common/simple_action_server.hpp"
 #include "nav2_util/twist_publisher.hpp"
-#include "nav2_util/odom_subscriber.hpp"
+#include "nav2_util/odometry_utils.hpp"
 #include "opennav_docking/controller.hpp"
 #include "opennav_docking/utils.hpp"
 #include "opennav_docking/types.hpp"
@@ -264,7 +264,7 @@ class DockingServer : public nav2::LifecycleNode
   rclcpp::Time action_start_time_;
 
   std::unique_ptr<nav2_util::TwistPublisher> vel_publisher_;
-  std::unique_ptr<nav2_util::OdomSubscriber> odom_sub_;
+  std::unique_ptr<nav2_util::OdomSmoother> odom_sub_;
   typename DockingActionServer::SharedPtr docking_action_server_;
   typename UndockingActionServer::SharedPtr undocking_action_server_;
 

nav2_docking/opennav_docking/src/docking_server.cpp

@@ -42,6 +42,7 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options)
   declare_parameter("dock_backwards", rclcpp::PARAMETER_BOOL);
   declare_parameter("dock_prestaging_tolerance", 0.5);
   declare_parameter("odom_topic", "odom");
+  declare_parameter("odom_duration", 0.3);
   declare_parameter("rotation_angular_tolerance", 0.05);
 }
 
@@ -83,7 +84,9 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state)
   // Create odom subscriber for backward blind docking
   std::string odom_topic;
   get_parameter("odom_topic", odom_topic);
-  odom_sub_ = std::make_unique<nav2_util::OdomSubscriber>(node, odom_topic);
+  double odom_duration;
+  get_parameter("odom_duration", odom_duration);
+  odom_sub_ = std::make_unique<nav2_util::OdomSmoother>(node, odom_duration, odom_topic);
 
   // Create the action servers for dock / undock
   docking_action_server_ = node->create_action_server<DockRobot>(
@@ -464,7 +467,7 @@ void DockingServer::rotateToDock(const geometry_msgs::msg::PoseStamped & dock_po
     }
 
     auto current_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
-    current_vel->twist.angular.z = odom_sub_->getTwist().angular.z;
+    current_vel->twist.angular.z = odom_sub_->getRawTwist().angular.z;
 
     auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
     command->header = robot_pose.header;