ROS2 Humble Port of nav2_pure_pursuit_controller tutorial

nav2_pure_pursuit_controller/CMakeLists.txt

@@ -0,0 +1,66 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_pure_pursuit_controller)
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(tf2 REQUIRED)
+
+nav2_package()
+
+include_directories(
+  include
+)
+
+set(dependencies
+  rclcpp
+  geometry_msgs
+  nav2_costmap_2d
+  pluginlib
+  nav_msgs
+  nav2_util
+  nav2_core
+  tf2
+)
+
+add_library(nav2_pure_pursuit_controller SHARED
+        src/pure_pursuit_controller.cpp)
+
+# prevent pluginlib from using boost
+target_compile_definitions(nav2_pure_pursuit_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+ament_target_dependencies(nav2_pure_pursuit_controller
+  ${dependencies}
+)
+
+install(TARGETS nav2_pure_pursuit_controller
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+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_libraries(nav2_pure_pursuit_controller)
+ament_export_dependencies(${dependencies})
+
+pluginlib_export_plugin_description_file(nav2_core nav2_pure_pursuit_controller.xml)
+
+ament_package()
+

nav2_pure_pursuit_controller/README.md

@@ -0,0 +1,11 @@
+# Nav2 Pure pursuit controller
+Tutorial code referenced in https://docs.nav2.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html
+
+This controller implements a the pure pursuit algorithm to track a path.
+
+## How the algorithm works
+The global path is continuously pruned to the closest point to the robot (see the figure below).
+Then the path is transformed to the robot frame and a lookahead point is determined.
+This lookahead point will be given to the pure pursuite algorithm to calculate a command velocity.
+
+![Lookahead algorithm](./doc/lookahead_algorithm.png)

nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp

@@ -0,0 +1,76 @@
+/*
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ *  Author(s): Shrijit Singh <shrijitsingh99@gmail.com>
+ *
+ */
+
+#ifndef NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_
+#define NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include "nav2_core/controller.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "pluginlib/class_loader.hpp"
+#include "pluginlib/class_list_macros.hpp"
+
+namespace nav2_pure_pursuit_controller
+{
+
+class PurePursuitController : public nav2_core::Controller
+{
+public:
+  PurePursuitController() = default;
+  ~PurePursuitController() override = default;
+
+  void configure(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+    std::string name, const std::shared_ptr<tf2_ros::Buffer> & tf,
+    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros) override;
+
+
+  void cleanup() override;
+  void activate() override;
+  void deactivate() override;
+  void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
+
+  geometry_msgs::msg::TwistStamped computeVelocityCommands(
+    const geometry_msgs::msg::PoseStamped & pose,
+    const geometry_msgs::msg::Twist & velocity
+    nav2_core::GoalChecker * goal_checker) override;
+
+  void setPlan(const nav_msgs::msg::Path & path) override;
+
+protected:
+  nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose);
+
+  bool transformPose(
+    const std::shared_ptr<tf2_ros::Buffer> tf,
+    const std::string frame,
+    const geometry_msgs::msg::PoseStamped & in_pose,
+    geometry_msgs::msg::PoseStamped & out_pose,
+    const rclcpp::Duration & transform_tolerance
+  ) const;
+
+  rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::string plugin_name_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  rclcpp::Logger logger_ {rclcpp::get_logger("PurePursuitController")};
+  rclcpp::Clock::SharedPtr clock_;
+
+  double desired_linear_vel_;
+  double lookahead_dist_;
+  double max_angular_vel_;
+  rclcpp::Duration transform_tolerance_ {0, 0};
+
+  nav_msgs::msg::Path global_plan_;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_pub_;
+};
+
+}  // namespace nav2_pure_pursuit_controller
+
+#endif  // NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_

nav2_pure_pursuit_controller/nav2_pure_pursuit_controller.xml

@@ -0,0 +1,10 @@
+<class_libraries>
+    <library path="nav2_pure_pursuit_controller">
+        <class type="nav2_pure_pursuit_controller::PurePursuitController" base_class_type="nav2_core::Controller">
+            <description>
+                nav2_pure_pursuit_controller
+            </description>
+        </class>
+    </library>
+</class_libraries>
+

nav2_pure_pursuit_controller/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_pure_pursuit_controller</name>
+  <version>1.0.0</version>
+  <description>Pure pursuit controller</description>
+  <maintainer email="shrijitsingh99@gmail.com">Shrijit Singh</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>nav2_common</depend>
+  <depend>nav2_core</depend>
+  <depend>nav2_util</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav2_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>tf2</depend>
+
+  <exec_depend>nav2_bringup</exec_depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>