smoother fix (#2822) (#2823)

mergify[bot] · padhupradheep · web-flow · commit 28dbc9472e49 · 2022-02-15T11:02:25.000-08:00
(cherry picked from commit a6e2949) Co-authored-by: Pradheep Krishna <padhupradheep@gmail.com>
diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
@@ -95,6 +95,7 @@ class BtNavigator : public nav2_util::LifecycleNode
   std::string robot_frame_;
   std::string global_frame_;
   double transform_tolerance_;
+  std::string odom_topic_;
 
   // Spinning transform that can be used by the BT nodes
   std::shared_ptr<tf2_ros::Buffer> tf_;
diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -93,6 +93,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   global_frame_ = get_parameter("global_frame").as_string();
   robot_frame_ = get_parameter("robot_base_frame").as_string();
   transform_tolerance_ = get_parameter("transform_tolerance").as_double();
+  odom_topic_ = get_parameter("odom_topic").as_string();
 
   // Libraries to pull plugins (BT Nodes) from
   auto plugin_lib_names = get_parameter("plugin_lib_names").as_string_array();
@@ -119,7 +120,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   }
 
   // Odometry smoother object for getting current speed
-  odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(shared_from_this(), 0.3);
+  odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(shared_from_this(), 0.3, odom_topic_);
 
   return nav2_util::CallbackReturn::SUCCESS;
 }

Original file line number	Diff line number	Diff line change
`@@ -93,6 +93,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /state/)`
`93`	`93`	`global_frame_ = get_parameter("global_frame").as_string();`
`94`	`94`	`robot_frame_ = get_parameter("robot_base_frame").as_string();`
`95`	`95`	`transform_tolerance_ = get_parameter("transform_tolerance").as_double();`
	`96`	`+ odom_topic_ = get_parameter("odom_topic").as_string();`
`96`	`97`
`97`	`98`	`// Libraries to pull plugins (BT Nodes) from`
`98`	`99`	`auto plugin_lib_names = get_parameter("plugin_lib_names").as_string_array();`
`@@ -119,7 +120,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /state/)`
`119`	`120`	`}`
`120`	`121`
`121`	`122`	`// Odometry smoother object for getting current speed`
`122`		`- odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(shared_from_this(), 0.3);`
	`123`	`+ odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(shared_from_this(), 0.3, odom_topic_);`
`123`	`124`
`124`	`125`	`return nav2_util::CallbackReturn::SUCCESS;`
`125`	`126`	`}`