diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 852833d60ae..e2bc8d2fbae 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -90,7 +90,7 @@ class BtNavigator : public nav2_util::LifecycleNode nav2_bt_navigator::NavigatorMuxer plugin_muxer_; // Odometry smoother object - std::unique_ptr odom_smoother_; + std::shared_ptr odom_smoother_; // Metrics for feedback std::string robot_frame_; diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp index 82c31deaeb6..3f71d704944 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp @@ -135,13 +135,15 @@ class Navigator * @param feedback_utils Some utilities useful for navigators to have * @param plugin_muxer The muxing object to ensure only one navigator * can be active at a time + * @param odom_smoother Object to get current smoothed robot's speed * @return bool If successful */ bool on_configure( rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node, const std::vector & plugin_lib_names, const FeedbackUtils & feedback_utils, - nav2_bt_navigator::NavigatorMuxer * plugin_muxer) + nav2_bt_navigator::NavigatorMuxer * plugin_muxer, + std::shared_ptr odom_smoother) { auto node = parent_node.lock(); logger_ = node->get_logger(); @@ -173,7 +175,7 @@ class Navigator blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT - return configure(parent_node) && ok; + return configure(parent_node, odom_smoother) && ok; } /** @@ -293,7 +295,12 @@ class Navigator /** * @param Method to configure resources. */ - virtual bool configure(rclcpp_lifecycle::LifecycleNode::WeakPtr /*node*/) {return true;} + virtual bool configure( + rclcpp_lifecycle::LifecycleNode::WeakPtr /*node*/ + std::shared_ptr /*odom_smoother*/) + { + return true; + } /** * @brief Method to cleanup resources. diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index d86736bad1e..eb0b204515f 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -51,9 +51,11 @@ class NavigateThroughPosesNavigator /** * @brief A configure state transition to configure navigator's state * @param node Weakptr to the lifecycle node + * @param odom_smoother Object to get current smoothed robot's speed */ bool configure( - rclcpp_lifecycle::LifecycleNode::WeakPtr node) override; + rclcpp_lifecycle::LifecycleNode::WeakPtr node, + std::shared_ptr odom_smoother) override; /** * @brief Get action name for this navigator @@ -106,7 +108,7 @@ class NavigateThroughPosesNavigator std::string path_blackboard_id_; // Odometry smoother object - std::unique_ptr odom_smoother_; + std::shared_ptr odom_smoother_; }; } // namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp index 9773ef65c3d..00dfeaffdbe 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp @@ -50,9 +50,11 @@ class NavigateToPoseNavigator /** * @brief A configure state transition to configure navigator's state * @param node Weakptr to the lifecycle node + * @param odom_smoother Object to get current smoothed robot's speed */ bool configure( - rclcpp_lifecycle::LifecycleNode::WeakPtr node) override; + rclcpp_lifecycle::LifecycleNode::WeakPtr node, + std::shared_ptr odom_smoother) override; /** * @brief A cleanup state transition to remove memory allocated @@ -122,7 +124,7 @@ class NavigateToPoseNavigator std::string path_blackboard_id_; // Odometry smoother object - std::unique_ptr odom_smoother_; + std::shared_ptr odom_smoother_; }; } // namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index a41260214cd..cfc23d85387 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -111,21 +111,21 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) feedback_utils.robot_frame = robot_frame_; feedback_utils.transform_tolerance = transform_tolerance_; + // Odometry smoother object for getting current speed + odom_smoother_ = std::make_shared(shared_from_this(), 0.3, odom_topic_) + if (!pose_navigator_->on_configure( - shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_)) + shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_)) { return nav2_util::CallbackReturn::FAILURE; } if (!poses_navigator_->on_configure( - shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_)) + shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_)) { return nav2_util::CallbackReturn::FAILURE; } - // Odometry smoother object for getting current speed - odom_smoother_ = std::make_unique(shared_from_this(), 0.3, odom_topic_); - return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 20e7d1b139d..f07d851e2d5 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -24,7 +24,8 @@ namespace nav2_bt_navigator bool NavigateThroughPosesNavigator::configure( - rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node) + rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node, + std::shared_ptr odom_smoother) { start_time_ = rclcpp::Time(0); auto node = parent_node.lock(); @@ -36,7 +37,7 @@ NavigateThroughPosesNavigator::configure( path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string(); // Odometry smoother object for getting current speed - odom_smoother_ = std::make_unique(node, 0.3); + odom_smoother_ = odom_smoother; return true; } diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index de0422e1034..4bc13de1e16 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -24,7 +24,8 @@ namespace nav2_bt_navigator bool NavigateToPoseNavigator::configure( - rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node) + rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node, + std::shared_ptr odom_smoother) { start_time_ = rclcpp::Time(0); auto node = parent_node.lock(); @@ -36,7 +37,7 @@ NavigateToPoseNavigator::configure( path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string(); // Odometry smoother object for getting current speed - odom_smoother_ = std::make_unique(node, 0.3); + odom_smoother_ = odom_smoother; self_client_ = rclcpp_action::create_client(node, getName());