diff --git a/nav2_core/CMakeLists.txt b/nav2_core/CMakeLists.txt
index c028762046431dca0d53ac8da6674e6484dba7fc..36e0b80e5d9955a00b19ab955a1d06138e9549b3 100644
--- a/nav2_core/CMakeLists.txt
+++ b/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()
diff --git a/nav2_core/include/nav2_core/exceptions.hpp b/nav2_core/include/nav2_core/exceptions.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..67380e623f1b47b7515e4fe9b3807b9ae0d69bbe
--- /dev/null
+++ b/nav2_core/include/nav2_core/exceptions.hpp
@@ -0,0 +1,57 @@
+/*
+ * 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_
diff --git a/nav2_core/include/nav2_core/goal_checker.hpp b/nav2_core/include/nav2_core/goal_checker.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f1c8500fe87636c80f07f231f8b16dc44c31194f
--- /dev/null
+++ b/nav2_core/include/nav2_core/goal_checker.hpp
@@ -0,0 +1,84 @@
+/*
+ * 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_
diff --git a/nav2_core/include/nav2_core/local_planner.hpp b/nav2_core/include/nav2_core/local_planner.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..80bd1daf1af65f1b517f79e4c73afb770fcf9a11
--- /dev/null
+++ b/nav2_core/include/nav2_core/local_planner.hpp
@@ -0,0 +1,137 @@
+/*
+ * 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_
diff --git a/nav2_core/package.xml b/nav2_core/package.xml
index 4ebe23e081722f6f8a7e449dd3684c72ac399c8c..c7f7b8fd3928ed7b98454060488c2186c4daa3a0 100644
--- a/nav2_core/package.xml
+++ b/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>
diff --git a/nav2_dwb_controller/dwb_core/CMakeLists.txt b/nav2_dwb_controller/dwb_core/CMakeLists.txt
index fc503b4610fff262baf16ef29821cc03649d6d3a..5bd5dd8a49ee257dbd9d3f7a7e5c498d70e02502 100644
--- a/nav2_dwb_controller/dwb_core/CMakeLists.txt
+++ b/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
diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp
index 65b220c880bbc7d1302d9511f547bbcd0c58cfdb..16251f51a09898dd14059f6cc60bd2cab12535e8 100644
--- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp
+++ b/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_;
diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/exceptions.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/exceptions.hpp
index fae2788fee6c8bc25fdf1ad694c91e48216cb444..157383a4197ef5ce7c6e521981893ad7d3ff1a83 100644
--- a/nav2_dwb_controller/dwb_core/include/dwb_core/exceptions.hpp
+++ b/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_
diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.hpp
index d32008d3396466c0e1dec9bc0285e12d0742acac..31c306f6659e3b8b0f4df659ec2215df1c96bb8f 100644
--- a/nav2_dwb_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.hpp
+++ b/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_;
};
diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp
index f25b96978352b9c705f4bdb9b2604e17cf6a1519..2dca4ed1b03f56920146beaf1fe1685834f4d496 100644
--- a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp
+++ b/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);
diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp
index 5b6c03d3e13faf3151b92d47f6b1796a6b3f5a5f..c7d48ab9be2eb47b200c7bebe77d7697d8964abc 100644
--- a/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp
+++ b/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_;
};
diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml
index 5d394f51c060c802bfdf74300d9527f0ebea8909..d1897b3de4c41f6d0e7195fa4a6ea886a62a58a6 100644
--- a/nav2_dwb_controller/dwb_core/package.xml
+++ b/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>
diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
index a9214244c588632a406349da88dbe2512df74f3b..a124f0eabd8acb3765693406d8693e74ca9f5f74 100644
--- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
+++ b/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;
}
diff --git a/nav2_dwb_controller/dwb_core/src/illegal_trajectory_tracker.cpp b/nav2_dwb_controller/dwb_core/src/illegal_trajectory_tracker.cpp
index 4fcdbee51b063ce735ccd57e6d58d11b32a7b53d..ce0578e1fee45e231491ae1afb4dcaff460f79d1 100644
--- a/nav2_dwb_controller/dwb_core/src/illegal_trajectory_tracker.cpp
+++ b/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_++;
diff --git a/nav2_dwb_controller/dwb_core/src/publisher.cpp b/nav2_dwb_controller/dwb_core/src/publisher.cpp
index 245958708af12741f755bc66ce9bfd69a150ae52..a88ae0a50617b40f28995248d1d6e11df7ae68f3 100644
--- a/nav2_dwb_controller/dwb_core/src/publisher.cpp
+++ b/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;}
diff --git a/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp b/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp
index 03cde45f17d59054a6534bdee73093d88471ed5b..301a86d4a4b4276b1b7f758aa1b30e63283a7480 100644
--- a/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp
+++ b/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;
}
diff --git a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp b/nav2_dwb_controller/dwb_critics/src/map_grid.cpp
index e7218fd8f8dfebfa081fefdc98ea8426dce8faa1..1adce22e24b00605423dbc460ecce405b0266ec3 100644
--- a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp
+++ b/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);
}
diff --git a/nav2_dwb_controller/dwb_critics/src/obstacle_footprint.cpp b/nav2_dwb_controller/dwb_critics/src/obstacle_footprint.cpp
index d885afdb23d3b038bfce7025ddb7522791c3a6e6..36d19a446e40337fef8db2646155d96c3c719228 100644
--- a/nav2_dwb_controller/dwb_critics/src/obstacle_footprint.cpp
+++ b/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;
diff --git a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp
index 9d6f8edc091eaf615d7d45b35ff697b8b2feae99..5ef71d668dc6f20fc924e9ed86e25b59b004802a 100644
--- a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp
+++ b/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;
}
diff --git a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp
index b70f6eed002c5d48cd24def4076a4fa560bd21fa..f3767dc9c87de19a67186c8abafce0598bcdfd96 100644
--- a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp
+++ b/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;
diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp
index 3fce8b2f373342275f2af5ecc13ab0329631d478..e96e510018c2beed24b30130a0bed5c892762718 100644
--- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp
+++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp
@@ -89,7 +89,7 @@ public:
*/
bool isValidSpeed(double x, double y, double theta);
- typedef std::shared_ptr<KinematicParameters> Ptr;
+ using Ptr = std::shared_ptr<KinematicParameters>;
protected:
// For parameter descriptions, see cfg/KinematicParams.cfg
diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp
index 32a98a6ef5e74ecf838ff24cf99c587a461de534..c7a6a028d830f71172a5109737c089fa16e79d1f 100644
--- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp
+++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp
@@ -38,8 +38,8 @@
#include <memory>
#include "rclcpp/rclcpp.hpp"
-#include "dwb_core/goal_checker.hpp"
-#include "nav2_util/lifecycle_node.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "nav2_core/goal_checker.hpp"
namespace dwb_plugins
{
@@ -48,12 +48,12 @@ namespace dwb_plugins
* @class SimpleGoalChecker
* @brief Goal Checker plugin that only checks the position difference
*/
-class SimpleGoalChecker : public dwb_core::GoalChecker
+class SimpleGoalChecker : public nav2_core::GoalChecker
{
public:
SimpleGoalChecker();
// Standard GoalChecker Interface
- void initialize(const nav2_util::LifecycleNode::SharedPtr & nh) override;
+ void initialize(const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh) override;
bool isGoalReached(
const geometry_msgs::msg::Pose2D & query_pose, const geometry_msgs::msg::Pose2D & goal_pose,
const nav_2d_msgs::msg::Twist2D & velocity) override;
diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp
index a2e10ad45cc2618bd1f31e933b4635a6e7ab5739..d83c1a7b161aa4cb64721c1b40b5644f1e3d56e2 100644
--- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp
+++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp
@@ -38,8 +38,8 @@
#include <memory>
#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "dwb_plugins/simple_goal_checker.hpp"
-#include "nav2_util/lifecycle_node.hpp"
namespace dwb_plugins
{
@@ -53,7 +53,7 @@ class StoppedGoalChecker : public SimpleGoalChecker
public:
StoppedGoalChecker();
// Standard GoalChecker Interface
- void initialize(const nav2_util::LifecycleNode::SharedPtr & nh) override;
+ void initialize(const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh) override;
bool isGoalReached(
const geometry_msgs::msg::Pose2D & query_pose, const geometry_msgs::msg::Pose2D & goal_pose,
const nav_2d_msgs::msg::Twist2D & velocity) override;
diff --git a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp
index c9efd36f0aa50fc55b084f5a3f050800fccfb451..90b935b9b15bb5088b9277910c875607f432ce27 100644
--- a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp
@@ -69,7 +69,8 @@ void LimitedAccelGenerator::checkUseDwaParam(const nav2_util::LifecycleNode::Sha
bool use_dwa = false;
nh->get_parameter("use_dwa", use_dwa);
if (!use_dwa) {
- throw nav_core2::PlannerException("Deprecated parameter use_dwa set to false. "
+ throw nav2_core::
+ PlannerException("Deprecated parameter use_dwa set to false. "
"Please use StandardTrajectoryGenerator for that functionality.");
}
}
diff --git a/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp b/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp
index 8e6387eed1f880f609a965ae1611e744b84bc499..486a310eedc458af594499e88780f0f8a150bbad 100644
--- a/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp
@@ -34,6 +34,7 @@
#include "dwb_plugins/simple_goal_checker.hpp"
#include <memory>
+#include "dwb_plugins/simple_goal_checker.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "angles/angles.h"
#include "nav2_util/node_utils.hpp"
@@ -46,7 +47,7 @@ SimpleGoalChecker::SimpleGoalChecker()
{
}
-void SimpleGoalChecker::initialize(const nav2_util::LifecycleNode::SharedPtr & nh)
+void SimpleGoalChecker::initialize(const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh)
{
nav2_util::declare_parameter_if_not_declared(nh,
"xy_goal_tolerance", rclcpp::ParameterValue(0.25));
diff --git a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
index 46fae624067e8625f971b53766207b4c60248616..0260d1bd63b1d72e81066d831a63649176f52760 100644
--- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
@@ -94,7 +94,8 @@ void StandardTrajectoryGenerator::checkUseDwaParam(
bool use_dwa = true;
nh->get_parameter("use_dwa", use_dwa);
if (use_dwa) {
- throw nav_core2::PlannerException("Deprecated parameter use_dwa set to true. "
+ throw nav2_core::
+ PlannerException("Deprecated parameter use_dwa set to true. "
"Please use LimitedAccelGenerator for that functionality.");
}
}
diff --git a/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp b/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp
index cf5d29346c95d26b23148d202f182447ab407533..b33cf3a5d62ae3423e5d5ddd8774253d5989c7fb 100644
--- a/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp
@@ -35,6 +35,7 @@
#include "dwb_plugins/stopped_goal_checker.hpp"
#include <cmath>
#include <memory>
+#include "dwb_plugins/stopped_goal_checker.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "nav2_util/node_utils.hpp"
@@ -48,7 +49,7 @@ StoppedGoalChecker::StoppedGoalChecker()
{
}
-void StoppedGoalChecker::initialize(const nav2_util::LifecycleNode::SharedPtr & nh)
+void StoppedGoalChecker::initialize(const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh)
{
SimpleGoalChecker::initialize(nh);
diff --git a/nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp b/nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp
index 90a8d1ce42c029e16bde9c9762a1bfddb760484e..6e627e3282508c0e547965777a22d8a894977914 100644
--- a/nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp
+++ b/nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp
@@ -43,7 +43,7 @@ using dwb_plugins::SimpleGoalChecker;
using dwb_plugins::StoppedGoalChecker;
void checkMacro(
- dwb_core::GoalChecker & gc,
+ nav2_core::GoalChecker & gc,
double x0, double y0, double theta0,
double x1, double y1, double theta1,
double xv, double yv, double thetav,
@@ -68,7 +68,7 @@ void checkMacro(
}
void sameResult(
- dwb_core::GoalChecker & gc0, dwb_core::GoalChecker & gc1,
+ nav2_core::GoalChecker & gc0, nav2_core::GoalChecker & gc1,
double x0, double y0, double theta0,
double x1, double y1, double theta1,
double xv, double yv, double thetav,
@@ -79,7 +79,7 @@ void sameResult(
}
void trueFalse(
- dwb_core::GoalChecker & gc0, dwb_core::GoalChecker & gc1,
+ nav2_core::GoalChecker & gc0, nav2_core::GoalChecker & gc1,
double x0, double y0, double theta0,
double x1, double y1, double theta1,
double xv, double yv, double thetav)
diff --git a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp
index a78d9d52f0a9f0db77e82dda5a6f38626803dfc6..b1c90aedafc5e1878f70f4b521b42681964e7c02 100644
--- a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp
+++ b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp
@@ -213,7 +213,7 @@ TEST(VelocityIterator, dwa_gen_exception)
auto nh = makeTestNode("dwa_gen_exception");
nh->set_parameters({rclcpp::Parameter("use_dwa", true)});
StandardTrajectoryGenerator gen;
- EXPECT_THROW(gen.initialize(nh), nav_core2::PlannerException);
+ EXPECT_THROW(gen.initialize(nh), nav2_core::PlannerException);
}
TEST(VelocityIterator, no_dwa_gen_exception)
@@ -221,7 +221,7 @@ TEST(VelocityIterator, no_dwa_gen_exception)
auto nh = makeTestNode("no_dwa_gen_exception");
nh->set_parameters({rclcpp::Parameter("use_dwa", false)});
dwb_plugins::LimitedAccelGenerator gen;
- EXPECT_THROW(gen.initialize(nh), nav_core2::PlannerException);
+ EXPECT_THROW(gen.initialize(nh), nav2_core::PlannerException);
}
TEST(VelocityIterator, dwa_gen)
diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
index 74035a8bfd3dae106c90b44fae1d8dfa14151ba9..2165117e1a3bcb70c4deb81cd7b9ad600a677cec 100644
--- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
+++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
@@ -76,7 +76,7 @@ protected:
std::map<std::string, std::shared_ptr<nav2_util::LifecycleServiceClient>> node_map_;
std::map<std::uint8_t, std::string> transition_label_map_;
-
+
// A map of the expected transitions to primary states
std::unordered_map<std::uint8_t, std::uint8_t> transition_state_map_;