nav2_core interface, with lifecycle changes and cleanup

nav2_core/CMakeLists.txt

@@ -2,16 +2,48 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_core)
 
 find_package(ament_cmake REQUIRED)
-find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(nav_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav_msgs REQUIRED)
 
+nav2_package()
+
+include_directories(
+  include
+)
+
+set(dependencies
+  rclcpp
+  rclcpp_lifecycle
+  std_msgs
+  geometry_msgs
+  nav2_costmap_2d
+  pluginlib
+  visualization_msgs
+  nav_msgs
+  tf2_ros
+)
+
 install(DIRECTORY include/
   DESTINATION include/
 )
 
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  set(ament_cmake_copyright_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
 ament_export_include_directories(include)
+ament_export_dependencies(${dependencies})
+
 ament_package()

nav2_core/include/nav2_core/exceptions.hpp

+/*
+ * Software License Agreement (BSD License)
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  Copyright (c) 2019, Intel Corporation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV2_CORE__EXCEPTIONS_HPP_
+#define NAV2_CORE__EXCEPTIONS_HPP_
+
+#include <stdexcept>
+#include <string>
+#include <memory>
+
+namespace nav2_core
+{
+
+class PlannerException : public std::runtime_error
+{
+public:
+  explicit PlannerException(const std::string description)
+  : std::runtime_error(description) {}
+  using Ptr = std::shared_ptr<PlannerException>;
+};
+
+}  // namespace nav2_core
+
+#endif  // NAV2_CORE__EXCEPTIONS_HPP_

nav2_core/include/nav2_core/goal_checker.hpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV2_CORE__GOAL_CHECKER_HPP_
+#define NAV2_CORE__GOAL_CHECKER_HPP_
+
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+
+namespace nav2_core
+{
+
+/**
+ * @class GoalChecker
+ * @brief Function-object for checking whether a goal has been reached
+ *
+ * This class defines the plugin interface for determining whether you have reached
+ * the goal state. This primarily consists of checking the relative positions of two poses
+ * (which are presumed to be in the same frame). It can also check the velocity, as some
+ * applications require that robot be stopped to be considered as having reached the goal.
+ */
+class GoalChecker
+{
+public:
+  typedef std::shared_ptr<nav2_core::GoalChecker> Ptr;
+
+  virtual ~GoalChecker() {}
+
+  /**
+   * @brief Initialize any parameters from the NodeHandle
+   * @param nh NodeHandle for grabbing parameters
+   */
+  virtual void initialize(const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh) = 0;
+
+  /**
+   * @brief Check whether the goal should be considered reached
+   * @param query_pose The pose to check
+   * @param goal_pose The pose to check against
+   * @param velocity The robot's current velocity
+   * @return True if goal is reached
+   */
+  virtual bool isGoalReached(
+    const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+    const geometry_msgs::msg::Twist & velocity) = 0;
+};
+
+}  // namespace nav2_core
+
+#endif  // NAV2_CORE__GOAL_CHECKER_HPP_

nav2_core/include/nav2_core/local_planner.hpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  Copyright (c) 2019, Intel Corporation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV2_CORE__LOCAL_PLANNER_HPP_
+#define NAV2_CORE__LOCAL_PLANNER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "tf2_ros/transform_listener.h"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "pluginlib/class_loader.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav2_msgs/msg/path.hpp"
+
+
+namespace nav2_core
+{
+
+/**
+ * @class LocalPlanner
+ * @brief planner interface that will be inherited by all local planners
+ */
+class LocalPlanner
+{
+public:
+  using Ptr = std::shared_ptr<nav2_core::LocalPlanner>;
+
+
+  /**
+   * @brief Virtual destructor
+   */
+  virtual ~LocalPlanner() {}
+
+  /**
+   * @param  parent pointer to user's node
+   * @param  costmap_ros A pointer to the costmap
+   */
+  virtual void configure(
+    const rclcpp_lifecycle::LifecycleNode::SharedPtr &,
+    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> &) = 0;
+
+  /**
+   * @brief Method to cleanup resources used on shutdown.
+   */
+  virtual void cleanup() = 0;
+
+  /**
+   * @brief Method to active planner and any threads involved in execution.
+   */
+  virtual void activate() = 0;
+
+  /**
+   * @brief Method to deactive planner and any threads involved in execution.
+   */
+  virtual void deactivate() = 0;
+
+  /**
+   * @brief Method to shutdown planner and any threads involved in execution.
+   */
+  virtual void shutdown() = 0;
+
+  /**
+   * @brief local setPlan - Sets the global plan
+   * @param path The global plan
+   */
+  virtual void setPlan(const nav2_msgs::msg::Path & path) = 0;
+
+  /**
+   * @brief local_planner computeVelocityCommands - calculates the best command given the current pose and velocity
+   *
+   * It is presumed that the global plan is already set.
+   *
+   * This is mostly a wrapper for the protected computeVelocityCommands
+   * function which has additional debugging info.
+   *
+   * @param pose Current robot pose
+   * @param velocity Current robot velocity
+   * @return The best command for the robot to drive
+   */
+  virtual geometry_msgs::msg::TwistStamped computeVelocityCommands(
+    const geometry_msgs::msg::PoseStamped & pose,
+    const geometry_msgs::msg::Twist & velocity) = 0;
+
+  /**
+   * @brief local_planner isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
+   *
+   * The pose that it checks against is the last pose in the current global plan.
+   * The calculation is delegated to the goal_checker plugin.
+   *
+   * @param pose Current pose
+   * @param velocity Current velocity
+   * @return True if the robot should be considered as having reached the goal.
+   */
+  virtual bool isGoalReached(
+    const geometry_msgs::msg::PoseStamped & pose,
+    const geometry_msgs::msg::Twist & velocity) = 0;
+};
+
+}  // namespace nav2_core
+
+#endif  // NAV2_CORE__LOCAL_PLANNER_HPP_

nav2_core/package.xml

@@ -5,17 +5,30 @@
   <version>0.2.4</version>
   <description>A set of headers for plugins core to the navigation2 stack</description>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
+  <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>nav2_common</build_depend>
 
-  <depend>nav2_costmap_2d</depend>
-  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
   <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>pluginlib</depend>
+  <depend>nav_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>nav2_util</depend>
   <depend>nav_msgs</depend>
 
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>launch_testing</test_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

nav2_dwb_controller/dwb_core/CMakeLists.txt

@@ -16,6 +16,7 @@ find_package(nav_2d_utils REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(nav2_util REQUIRED)
+find_package(nav2_core REQUIRED)
 
 nav2_package()
 
@@ -37,6 +38,7 @@ set(dependencies
   nav_msgs
   tf2_ros
   nav2_util
+  nav2_core
 )
 
 add_library(dwb_core SHARED

nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp

@@ -39,13 +39,15 @@
 #include <string>
 #include <vector>
 
-#include "dwb_core/goal_checker.hpp"
+#include "nav2_core/local_planner.hpp"
+#include "nav2_core/goal_checker.hpp"
 #include "dwb_core/publisher.hpp"
 #include "dwb_core/trajectory_critic.hpp"
 #include "dwb_core/trajectory_generator.hpp"
 #include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
 #include "nav_2d_msgs/msg/twist2_d_stamped.hpp"
-#include "nav2_util/lifecycle_node.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "pluginlib/class_loader.hpp"
 
 namespace dwb_core
@@ -55,30 +57,34 @@ namespace dwb_core
  * @class DWBLocalPlanner
  * @brief Plugin-based flexible local_planner
  */
-class DWBLocalPlanner : public nav2_util::LifecycleHelperInterface
+class DWBLocalPlanner : public nav2_core::LocalPlanner
 {
 public:
   /**
    * @brief Constructor that brings up pluginlib loaders
    */
-  DWBLocalPlanner(
-    nav2_util::LifecycleNode::SharedPtr node, TFBufferPtr tf, CostmapROSPtr costmap_ros);
+  DWBLocalPlanner();
+
+  void configure(
+    const rclcpp_lifecycle::LifecycleNode::SharedPtr & node,
+    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros) override;
 
   virtual ~DWBLocalPlanner() {}
 
-  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
-  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
-  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
-  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  void activate() override;
+  void deactivate() override;
+  void cleanup() override;
+  void shutdown() override {}
+
 
   /**
-   * @brief nav_core2 setPlan - Sets the global plan
+   * @brief nav2_core setPlan - Sets the global plan
    * @param path The global plan
    */
-  void setPlan(const nav_2d_msgs::msg::Path2D & path);
+  void setPlan(const nav2_msgs::msg::Path & path) override;
 
   /**
-   * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
+   * @brief nav2_core computeVelocityCommands - calculates the best command given the current pose and velocity
    *
    * It is presumed that the global plan is already set.
    *
@@ -94,7 +100,7 @@ public:
     const nav_2d_msgs::msg::Twist2D & velocity);
 
   /**
-   * @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
+   * @brief nav2_core isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
    *
    * The pose that it checks against is the last pose in the current global plan.
    * The calculation is delegated to the goal_checker plugin.
@@ -192,9 +198,9 @@ protected:
 
   void loadBackwardsCompatibleParameters();
 
-  nav2_util::LifecycleNode::SharedPtr node_;
-  TFBufferPtr tf_;
-  CostmapROSPtr costmap_ros_;
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
 
   std::unique_ptr<DWBPublisher> pub_;
   std::vector<std::string> default_critic_namespaces_;
@@ -203,8 +209,7 @@ protected:
   pluginlib::ClassLoader<TrajectoryGenerator> traj_gen_loader_;
   TrajectoryGenerator::Ptr traj_generator_;
 
-  pluginlib::ClassLoader<GoalChecker> goal_checker_loader_;
-  GoalChecker::Ptr goal_checker_;
+  nav2_core::GoalChecker::Ptr goal_checker_;
 
   pluginlib::ClassLoader<TrajectoryCritic> critic_loader_;
   std::vector<TrajectoryCritic::Ptr> critics_;

nav2_dwb_controller/dwb_core/include/dwb_core/exceptions.hpp

@@ -38,54 +38,37 @@
 #include <string>
 #include <memory>
 
-namespace nav_core2
-{
+#include "nav2_core/exceptions.hpp"
 
-class PlannerException : public std::runtime_error
+namespace dwb_core
 {
-public:
-  explicit PlannerException(const std::string description)
-  : std::runtime_error(description) {}
-  typedef std::shared_ptr<PlannerException> Ptr;
-};
 
 /**
  * @class PlannerTFException
  * @brief Thrown when the planner cannot complete its operation due to TF errors
  */
-class PlannerTFException : public PlannerException
+class PlannerTFException : public nav2_core::PlannerException
 {
 public:
   explicit PlannerTFException(const std::string description)
-  : PlannerException(description) {}
+  : nav2_core::PlannerException(description) {}
 };
 
 /**
  * @class IllegalTrajectoryException
  * @brief Thrown when one of the critics encountered a fatal error
  */
-class IllegalTrajectoryException : public PlannerException
+class IllegalTrajectoryException : public nav2_core::PlannerException
 {
 public:
   IllegalTrajectoryException(const std::string critic_name, const std::string description)
-  : PlannerException(description), critic_name_(critic_name) {}
+  : nav2_core::PlannerException(description), critic_name_(critic_name) {}
   std::string getCriticName() const {return critic_name_;}
 
 protected:
   std::string critic_name_;
 };
 
-/**
- * @class NoLegalTrajectoriesException
- * @brief Thrown when all the trajectories explored are illegal
- */
-class NoLegalTrajectoriesException : public PlannerException
-{
-public:
-  explicit NoLegalTrajectoriesException(const std::string description)
-  : PlannerException(description) {}
-};
-
-}  // namespace nav_core2
+}  // namespace dwb_core
 
 #endif  // DWB_CORE__EXCEPTIONS_HPP_

nav2_dwb_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.hpp

@@ -39,6 +39,7 @@
 #include <utility>
 #include <string>
 #include "dwb_core/exceptions.hpp"
+#include "nav2_core/exceptions.hpp"
 
 namespace dwb_core
 {
@@ -48,7 +49,7 @@ public:
   IllegalTrajectoryTracker()
   : legal_count_(0), illegal_count_(0) {}
 
-  void addIllegalTrajectory(const nav_core2::IllegalTrajectoryException & e);
+  void addIllegalTrajectory(const IllegalTrajectoryException & e);
   void addLegalTrajectory();
 
   std::map<std::pair<std::string, std::string>, double> getPercentages() const;
@@ -64,11 +65,13 @@ protected:
  * @class NoLegalTrajectoriesException
  * @brief Thrown when all the trajectories explored are illegal
  */
-class NoLegalTrajectoriesException : public nav_core2::NoLegalTrajectoriesException
+class NoLegalTrajectoriesException
+  : public nav2_core::PlannerException
 {
 public:
   explicit NoLegalTrajectoriesException(const IllegalTrajectoryTracker & tracker)
-  : nav_core2::NoLegalTrajectoriesException(tracker.getMessage()), tracker_(tracker) {}
+  : PlannerException(tracker.getMessage()),
+    tracker_(tracker) {}
   IllegalTrajectoryTracker tracker_;
 };
 

nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp

@@ -39,7 +39,7 @@
 #include <string>
 #include <vector>
 
-#include "dwb_core/common_types.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "dwb_core/trajectory_critic.hpp"
 #include "dwb_msgs/msg/local_plan_evaluation.hpp"
 #include "nav2_util/lifecycle_helper_interface.hpp"
@@ -66,15 +66,15 @@ namespace dwb_core
  *   5) Markers representing the different trajectories evaluated
  *   6) The CostGrid (in the form of a complex PointCloud2)
  */
-class DWBPublisher : public nav2_util::LifecycleHelperInterface
+class DWBPublisher
 {
 public:
   explicit DWBPublisher(nav2_util::LifecycleNode::SharedPtr node);
 
-  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
-  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
-  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
-  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  nav2_util::CallbackReturn on_configure();
+  nav2_util::CallbackReturn on_activate();
+  nav2_util::CallbackReturn on_deactivate();
+  nav2_util::CallbackReturn on_cleanup();
 
   /**
    * @brief Does the publisher require that the LocalPlanEvaluation be saved
@@ -90,7 +90,7 @@ public:
     const std_msgs::msg::Header & header,
     const dwb_msgs::msg::Trajectory2D & traj);
   void publishCostGrid(
-    const CostmapROSPtr costmap_ros,
+    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
     const std::vector<TrajectoryCritic::Ptr> critics);
   void publishGlobalPlan(const nav_2d_msgs::msg::Path2D plan);
   void publishTransformedPlan(const nav_2d_msgs::msg::Path2D plan);

nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp

@@ -40,7 +40,7 @@
 #include <memory>
 
 #include "rclcpp/rclcpp.hpp"
-#include "dwb_core/common_types.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "geometry_msgs/msg/pose2_d.hpp"
 #include "nav_2d_msgs/msg/twist2_d.hpp"
 #include "nav_2d_msgs/msg/path2_d.hpp"
@@ -77,7 +77,7 @@ namespace dwb_core
 class TrajectoryCritic
 {
 public:
-  typedef std::shared_ptr<dwb_core::TrajectoryCritic> Ptr;
+  using Ptr = std::shared_ptr<dwb_core::TrajectoryCritic>;
 
   virtual ~TrajectoryCritic() {}
 
@@ -94,7 +94,7 @@ public:
   void initialize(
     const nav2_util::LifecycleNode::SharedPtr & nh,
     std::string & name,
-    CostmapROSPtr costmap_ros)
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
   {
     name_ = name;
     costmap_ros_ = costmap_ros;
@@ -174,7 +174,7 @@ public:
 
 protected:
   std::string name_;
-  CostmapROSPtr costmap_ros_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   double scale_;
   nav2_util::LifecycleNode::SharedPtr nh_;
 };

nav2_dwb_controller/dwb_core/package.xml

@@ -23,6 +23,7 @@
   <build_depend>nav_msgs</build_depend>
   <build_depend>tf2_ros</build_depend>
   <build_depend>nav2_util</build_depend>
+  <build_depend>nav2_core</build_depend>
 
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>std_msgs</exec_depend>
@@ -36,6 +37,7 @@
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
   <exec_depend>nav2_util</exec_depend>
+  <exec_depend>nav2_core</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp

@@ -55,16 +55,20 @@ using nav2_util::declare_parameter_if_not_declared;
 namespace dwb_core
 {
 
-DWBLocalPlanner::DWBLocalPlanner(
-  nav2_util::LifecycleNode::SharedPtr node, TFBufferPtr tf,
-  CostmapROSPtr costmap_ros)
-: node_(node),
-  tf_(tf),
-  costmap_ros_(costmap_ros),
-  traj_gen_loader_("dwb_core", "dwb_core::TrajectoryGenerator"),
-  goal_checker_loader_("dwb_core", "dwb_core::GoalChecker"),
+DWBLocalPlanner::DWBLocalPlanner()
+: traj_gen_loader_("dwb_core", "dwb_core::TrajectoryGenerator"),
+  goal_checker_loader_("dwb_core", "nav2_core::GoalChecker"),
   critic_loader_("dwb_core", "dwb_core::TrajectoryCritic")
 {
+}
+
+void DWBLocalPlanner::configure(
+  const rclcpp_lifecycle::LifecycleNode::SharedPtr & node,
+  const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros)
+{
+  node_ = node;
+  costmap_ros_ = costmap_ros;
+  tf_ = costmap_ros_->getTfBuffer();
   declare_parameter_if_not_declared(node_, "critics");
   declare_parameter_if_not_declared(node_, "prune_plan", rclcpp::ParameterValue(true));
   declare_parameter_if_not_declared(node_, "prune_distance", rclcpp::ParameterValue(1.0));
@@ -76,11 +80,7 @@ DWBLocalPlanner::DWBLocalPlanner(
     rclcpp::ParameterValue(std::string("dwb_plugins::SimpleGoalChecker")));
   declare_parameter_if_not_declared(node_, "use_dwa", rclcpp::ParameterValue(false));
   declare_parameter_if_not_declared(node_, "transform_tolerance", rclcpp::ParameterValue(0.1));
-}
 
-nav2_util::CallbackReturn
-DWBLocalPlanner::on_configure(const rclcpp_lifecycle::State & state)
-{
   std::string traj_generator_name;
   std::string goal_checker_name;
 
@@ -96,7 +96,7 @@ DWBLocalPlanner::on_configure(const rclcpp_lifecycle::State & state)
   node_->get_parameter("goal_checker_name", goal_checker_name);
 
   pub_ = std::make_unique<DWBPublisher>(node_);
-  pub_->on_configure(state);
+  pub_->on_configure();
 
   traj_generator_ = traj_gen_loader_.createUniqueInstance(traj_generator_name);
   goal_checker_ = goal_checker_loader_.createUniqueInstance(goal_checker_name);
@@ -108,37 +108,29 @@ DWBLocalPlanner::on_configure(const rclcpp_lifecycle::State & state)
     loadCritics();
   } catch (const std::exception & e) {
     RCLCPP_ERROR(node_->get_logger(), "Couldn't load critics! Caught exception: %s", e.what());
-    return nav2_util::CallbackReturn::FAILURE;
+    throw;
   }
-
-  return nav2_util::CallbackReturn::SUCCESS;
 }
 
-nav2_util::CallbackReturn
-DWBLocalPlanner::on_activate(const rclcpp_lifecycle::State & state)
+void
+DWBLocalPlanner::activate()
 {
-  pub_->on_activate(state);
-
-  return nav2_util::CallbackReturn::SUCCESS;
+  pub_->on_activate();
 }
 
-nav2_util::CallbackReturn
-DWBLocalPlanner::on_deactivate(const rclcpp_lifecycle::State & state)
+void
+DWBLocalPlanner::deactivate()
 {
-  pub_->on_deactivate(state);
-
-  return nav2_util::CallbackReturn::SUCCESS;
+  pub_->on_deactivate();
 }
 
-nav2_util::CallbackReturn
-DWBLocalPlanner::on_cleanup(const rclcpp_lifecycle::State & state)
+void
+DWBLocalPlanner::cleanup()
 {
-  pub_->on_cleanup(state);
+  pub_->on_cleanup();
 
   traj_generator_.reset();
   goal_checker_.reset();
-
-  return nav2_util::CallbackReturn::SUCCESS;
 }
 
 std::string
@@ -287,7 +279,7 @@ DWBLocalPlanner::computeVelocityCommands(
     nav_2d_msgs::msg::Twist2DStamped cmd_vel = computeVelocityCommands(pose, velocity, results);
     pub_->publishEvaluation(results);
     return cmd_vel;
-  } catch (const nav_core2::PlannerException & e) {
+  } catch (const nav2_core::PlannerException & e) {
     pub_->publishEvaluation(results);
     throw;
   }
@@ -348,7 +340,7 @@ DWBLocalPlanner::computeVelocityCommands(
     pub_->publishCostGrid(costmap_ros_, critics_);
 
     return cmd_vel;
-  } catch (const nav_core2::NoLegalTrajectoriesException & e) {
+  } catch (const dwb_core::NoLegalTrajectoriesException & e) {
     nav_2d_msgs::msg::Twist2D empty_cmd;
     dwb_msgs::msg::Trajectory2D empty_traj;
     // debrief stateful scoring functions
@@ -398,7 +390,7 @@ DWBLocalPlanner::coreScoringAlgorithm(
           results->worst_index = results->twists.size() - 1;
         }
       }
-    } catch (const nav_core2::IllegalTrajectoryException & e) {
+    } catch (const dwb_core::IllegalTrajectoryException & e) {
       if (results) {
         dwb_msgs::msg::TrajectoryScore failed_score;
         failed_score.traj = traj;
@@ -476,7 +468,7 @@ DWBLocalPlanner::transformGlobalPlan(
   const nav_2d_msgs::msg::Pose2DStamped & pose)
 {
   if (global_plan_.poses.size() == 0) {
-    throw nav_core2::PlannerException("Received plan with zero length");
+    throw nav2_core::PlannerException("Received plan with zero length");
   }
 
   // let's get the pose of the robot in the frame of the plan
@@ -484,7 +476,8 @@ DWBLocalPlanner::transformGlobalPlan(
   if (!nav_2d_utils::transformPose(tf_, global_plan_.header.frame_id, pose,
     robot_pose, transform_tolerance_))
   {
-    throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
+    throw dwb_core::
+          PlannerTFException("Unable to transform robot pose into global plan's frame");
   }
 
   // we'll discard points on the plan that are outside the local costmap
@@ -569,7 +562,7 @@ DWBLocalPlanner::transformGlobalPlan(
   }
 
   if (transformed_plan.poses.size() == 0) {
-    throw nav_core2::PlannerException("Resulting plan has 0 poses in it.");
+    throw nav2_core::PlannerException("Resulting plan has 0 poses in it.");
   }
   return transformed_plan;
 }

nav2_dwb_controller/dwb_core/src/illegal_trajectory_tracker.cpp

@@ -40,7 +40,8 @@
 
 namespace dwb_core
 {
-void IllegalTrajectoryTracker::addIllegalTrajectory(const nav_core2::IllegalTrajectoryException & e)
+void IllegalTrajectoryTracker::addIllegalTrajectory(
+  const dwb_core::IllegalTrajectoryException & e)
 {
   counts_[std::make_pair(e.getCriticName(), e.what())]++;
   illegal_count_++;

nav2_dwb_controller/dwb_core/src/publisher.cpp

@@ -65,7 +65,7 @@ DWBPublisher::DWBPublisher(nav2_util::LifecycleNode::SharedPtr node)
 }
 
 nav2_util::CallbackReturn
-DWBPublisher::on_configure(const rclcpp_lifecycle::State & /*state*/)
+DWBPublisher::on_configure()
 {
   node_->get_parameter("publish_evaluation", publish_evaluation_);
   node_->get_parameter("publish_global_plan", publish_global_plan_);
@@ -87,7 +87,7 @@ DWBPublisher::on_configure(const rclcpp_lifecycle::State & /*state*/)
 }
 
 nav2_util::CallbackReturn
-DWBPublisher::on_activate(const rclcpp_lifecycle::State & /*state*/)
+DWBPublisher::on_activate()
 {
   eval_pub_->on_activate();
   global_pub_->on_activate();
@@ -100,7 +100,7 @@ DWBPublisher::on_activate(const rclcpp_lifecycle::State & /*state*/)
 }
 
 nav2_util::CallbackReturn
-DWBPublisher::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
+DWBPublisher::on_deactivate()
 {
   eval_pub_->on_deactivate();
   global_pub_->on_deactivate();
@@ -113,7 +113,7 @@ DWBPublisher::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
 }
 
 nav2_util::CallbackReturn
-DWBPublisher::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
+DWBPublisher::on_cleanup()
 {
   eval_pub_.reset();
   global_pub_.reset();
@@ -210,7 +210,7 @@ DWBPublisher::publishLocalPlan(
 
 void
 DWBPublisher::publishCostGrid(
-  const CostmapROSPtr costmap_ros,
+  const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
   const std::vector<TrajectoryCritic::Ptr> critics)
 {
   if (!publish_cost_grid_pc_) {return;}

nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp

@@ -68,11 +68,13 @@ double BaseObstacleCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
 {
   unsigned int cell_x, cell_y;
   if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
-    throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
+    throw dwb_core::
+          IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
   }
   unsigned char cost = costmap_->getCost(cell_x, cell_y);
   if (!isValidCost(cost)) {
-    throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
+    throw dwb_core::
+          IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
   }
   return cost;
 }

nav2_dwb_controller/dwb_critics/src/map_grid.cpp

@@ -119,9 +119,11 @@ double MapGridCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
     grid_dist = scorePose(traj.poses[i]);
     if (stop_on_failure_) {
       if (grid_dist == obstacle_score_) {
-        throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
+        throw dwb_core::
+              IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
       } else if (grid_dist == unreachable_score_) {
-        throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Unreachable Area.");
+        throw dwb_core::
+              IllegalTrajectoryException(name_, "Trajectory Hits Unreachable Area.");
       }
     }
 
@@ -148,7 +150,8 @@ double MapGridCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
   unsigned int cell_x, cell_y;
   // we won't allow trajectories that go off the map... shouldn't happen that often anyways
   if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
-    throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
+    throw dwb_core::
+          IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
   }
   return getScore(cell_x, cell_y);
 }

nav2_dwb_controller/dwb_critics/src/obstacle_footprint.cpp

@@ -77,7 +77,8 @@ double ObstacleFootprintCritic::scorePose(const geometry_msgs::msg::Pose2D & pos
 {
   unsigned int cell_x, cell_y;
   if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
-    throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
+    throw dwb_core::
+          IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
   }
   return scorePose(pose, getOrientedFootprint(pose, footprint_spec_));
 }
@@ -95,12 +96,14 @@ double ObstacleFootprintCritic::scorePose(
   for (unsigned int i = 0; i < footprint.size() - 1; ++i) {
     // get the cell coord of the first point
     if (!costmap_->worldToMap(footprint[i].x, footprint[i].y, x0, y0)) {
-      throw nav_core2::IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
+      throw dwb_core::
+            IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
     }
 
     // get the cell coord of the second point
     if (!costmap_->worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) {
-      throw nav_core2::IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
+      throw dwb_core::
+            IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
     }
 
     line_cost = lineCost(x0, x1, y0, y1);
@@ -110,12 +113,14 @@ double ObstacleFootprintCritic::scorePose(
   // we also need to connect the first point in the footprint to the last point
   // get the cell coord of the last point
   if (!costmap_->worldToMap(footprint.back().x, footprint.back().y, x0, y0)) {
-    throw nav_core2::IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
+    throw dwb_core::
+          IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
   }
 
   // get the cell coord of the first point
   if (!costmap_->worldToMap(footprint.front().x, footprint.front().y, x1, y1)) {
-    throw nav_core2::IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
+    throw dwb_core::
+          IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
   }
 
   line_cost = lineCost(x0, x1, y0, y1);
@@ -146,9 +151,11 @@ double ObstacleFootprintCritic::pointCost(int x, int y)
   unsigned char cost = costmap_->getCost(x, y);
   // if the cell is in an obstacle the path is invalid or unknown
   if (cost == nav2_costmap_2d::LETHAL_OBSTACLE) {
-    throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
+    throw dwb_core::
+          IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
   } else if (cost == nav2_costmap_2d::NO_INFORMATION) {
-    throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Unknown Region.");
+    throw dwb_core::
+          IllegalTrajectoryException(name_, "Trajectory Hits Unknown Region.");
   }
 
   return cost;

nav2_dwb_controller/dwb_critics/src/oscillation.cpp

@@ -211,7 +211,8 @@ double OscillationCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & tr
     y_trend_.isOscillating(traj.velocity.y) ||
     theta_trend_.isOscillating(traj.velocity.theta))
   {
-    throw nav_core2::IllegalTrajectoryException(name_, "Trajectory is oscillating.");
+    throw dwb_core::
+          IllegalTrajectoryException(name_, "Trajectory is oscillating.");
   }
   return 0.0;
 }

nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp

@@ -77,11 +77,12 @@ double RotateToGoalCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & t
 
   // If we're sufficiently close to the goal, any transforming velocity is invalid
   if (fabs(traj.velocity.x) > 0 || fabs(traj.velocity.y) > 0) {
-    throw nav_core2::IllegalTrajectoryException(name_, "Nonrotation command near goal.");
+    throw dwb_core::
+          IllegalTrajectoryException(name_, "Nonrotation command near goal.");
   }
 
   if (traj.poses.empty()) {
-    throw nav_core2::IllegalTrajectoryException(name_, "Empty trajectory.");
+    throw dwb_core::IllegalTrajectoryException(name_, "Empty trajectory.");
   }
 
   double end_yaw = traj.poses.back().theta;