smoother fix (#2822)

padhupradheep · web-flow · commit a6e2949c81e5 · 2022-02-15T10:58:38.000-08:00
diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
@@ -96,6 +96,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
@@ -97,6 +97,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();
@@ -123,7 +124,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
`@@ -97,6 +97,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /state/)`
`97`	`97`	`global_frame_ = get_parameter("global_frame").as_string();`
`98`	`98`	`robot_frame_ = get_parameter("robot_base_frame").as_string();`
`99`	`99`	`transform_tolerance_ = get_parameter("transform_tolerance").as_double();`
	`100`	`+ odom_topic_ = get_parameter("odom_topic").as_string();`
`100`	`101`
`101`	`102`	`// Libraries to pull plugins (BT Nodes) from`
`102`	`103`	`auto plugin_lib_names = get_parameter("plugin_lib_names").as_string_array();`
`@@ -123,7 +124,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /state/)`
`123`	`124`	`}`
`124`	`125`
`125`	`126`	`// Odometry smoother object for getting current speed`
`126`		`- odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(shared_from_this(), 0.3);`
	`127`	`+ odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(shared_from_this(), 0.3, odom_topic_);`
`127`	`128`
`128`	`129`	`return nav2_util::CallbackReturn::SUCCESS;`
`129`	`130`	`}`