Skip to content

Commit 8d80918

Browse files
mitchellsayerSayer
mitchellsayer
and
Sayer
authored
ROS2 Humble Port of nav2_pure_pursuit_controller tutorial (#92)
* Added Humble port of Pure Pursuit Controller Signed-off-by: Sayer <[email protected]> Signed-off-by: Mitchell Sayer <[email protected]> * Fixed broken README link Signed-off-by: Sayer <[email protected]> Signed-off-by: Mitchell Sayer <[email protected]> --------- Signed-off-by: Sayer <[email protected]> Signed-off-by: Mitchell Sayer <[email protected]> Signed-off-by: Sayer <[email protected]> Co-authored-by: Sayer <[email protected]>
1 parent cb800b7 commit 8d80918

File tree

7 files changed

+504
-0
lines changed

7 files changed

+504
-0
lines changed

‎nav2_pure_pursuit_controller/CMakeLists.txt‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
project(nav2_pure_pursuit_controller)
3+
4+
find_package(ament_cmake REQUIRED)
5+
find_package(nav2_common REQUIRED)
6+
find_package(nav2_core REQUIRED)
7+
find_package(nav2_costmap_2d REQUIRED)
8+
find_package(nav2_util REQUIRED)
9+
find_package(rclcpp REQUIRED)
10+
find_package(geometry_msgs REQUIRED)
11+
find_package(nav_msgs REQUIRED)
12+
find_package(pluginlib REQUIRED)
13+
find_package(tf2 REQUIRED)
14+
15+
nav2_package()
16+
17+
include_directories(
18+
include
19+
)
20+
21+
set(dependencies
22+
rclcpp
23+
geometry_msgs
24+
nav2_costmap_2d
25+
pluginlib
26+
nav_msgs
27+
nav2_util
28+
nav2_core
29+
tf2
30+
)
31+
32+
add_library(nav2_pure_pursuit_controller SHARED
33+
src/pure_pursuit_controller.cpp)
34+
35+
# prevent pluginlib from using boost
36+
target_compile_definitions(nav2_pure_pursuit_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
37+
38+
ament_target_dependencies(nav2_pure_pursuit_controller
39+
${dependencies}
40+
)
41+
42+
install(TARGETS nav2_pure_pursuit_controller
43+
ARCHIVE DESTINATION lib
44+
LIBRARY DESTINATION lib
45+
RUNTIME DESTINATION bin
46+
)
47+
48+
install(DIRECTORY include/
49+
DESTINATION include/
50+
)
51+
52+
if(BUILD_TESTING)
53+
find_package(ament_lint_auto REQUIRED)
54+
# the following line skips the linter which checks for copyrights
55+
set(ament_cmake_copyright_FOUND TRUE)
56+
ament_lint_auto_find_test_dependencies()
57+
endif()
58+
59+
ament_export_include_directories(include)
60+
ament_export_libraries(nav2_pure_pursuit_controller)
61+
ament_export_dependencies(${dependencies})
62+
63+
pluginlib_export_plugin_description_file(nav2_core nav2_pure_pursuit_controller.xml)
64+
65+
ament_package()
66+

‎nav2_pure_pursuit_controller/README.md‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
# Nav2 Pure pursuit controller
2+
Tutorial code referenced in https://docs.nav2.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html
3+
4+
This controller implements a the pure pursuit algorithm to track a path.
5+
6+
## How the algorithm works
7+
The global path is continuously pruned to the closest point to the robot (see the figure below).
8+
Then the path is transformed to the robot frame and a lookahead point is determined.
9+
This lookahead point will be given to the pure pursuite algorithm to calculate a command velocity.
10+
11+
![Lookahead algorithm](./doc/lookahead_algorithm.png)

‎nav2_pure_pursuit_controller/doc/lookahead_algorithm.png‎

106 KB
Loading

‎nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp‎

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
/*
2+
* SPDX-License-Identifier: BSD-3-Clause
3+
*
4+
* Author(s): Shrijit Singh <[email protected]>
5+
*
6+
*/
7+
8+
#ifndef NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_
9+
#define NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_
10+
11+
#include <string>
12+
#include <vector>
13+
#include <memory>
14+
15+
#include "nav2_core/controller.hpp"
16+
#include "rclcpp/rclcpp.hpp"
17+
#include "pluginlib/class_loader.hpp"
18+
#include "pluginlib/class_list_macros.hpp"
19+
20+
namespace nav2_pure_pursuit_controller
21+
{
22+
23+
class PurePursuitController : public nav2_core::Controller
24+
{
25+
public:
26+
PurePursuitController() = default;
27+
~PurePursuitController() override = default;
28+
29+
void configure(
30+
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
31+
std::string name, const std::shared_ptr<tf2_ros::Buffer> & tf,
32+
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros) override;
33+
34+
35+
void cleanup() override;
36+
void activate() override;
37+
void deactivate() override;
38+
void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
39+
40+
geometry_msgs::msg::TwistStamped computeVelocityCommands(
41+
const geometry_msgs::msg::PoseStamped & pose,
42+
const geometry_msgs::msg::Twist & velocity
43+
nav2_core::GoalChecker * goal_checker) override;
44+
45+
void setPlan(const nav_msgs::msg::Path & path) override;
46+
47+
protected:
48+
nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose);
49+
50+
bool transformPose(
51+
const std::shared_ptr<tf2_ros::Buffer> tf,
52+
const std::string frame,
53+
const geometry_msgs::msg::PoseStamped & in_pose,
54+
geometry_msgs::msg::PoseStamped & out_pose,
55+
const rclcpp::Duration & transform_tolerance
56+
) const;
57+
58+
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
59+
std::shared_ptr<tf2_ros::Buffer> tf_;
60+
std::string plugin_name_;
61+
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
62+
rclcpp::Logger logger_ {rclcpp::get_logger("PurePursuitController")};
63+
rclcpp::Clock::SharedPtr clock_;
64+
65+
double desired_linear_vel_;
66+
double lookahead_dist_;
67+
double max_angular_vel_;
68+
rclcpp::Duration transform_tolerance_ {0, 0};
69+
70+
nav_msgs::msg::Path global_plan_;
71+
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_pub_;
72+
};
73+
74+
} // namespace nav2_pure_pursuit_controller
75+
76+
#endif // NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_

‎nav2_pure_pursuit_controller/nav2_pure_pursuit_controller.xml‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
<class_libraries>
2+
<library path="nav2_pure_pursuit_controller">
3+
<class type="nav2_pure_pursuit_controller::PurePursuitController" base_class_type="nav2_core::Controller">
4+
<description>
5+
nav2_pure_pursuit_controller
6+
</description>
7+
</class>
8+
</library>
9+
</class_libraries>
10+

‎nav2_pure_pursuit_controller/package.xml‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>nav2_pure_pursuit_controller</name>
5+
<version>1.0.0</version>
6+
<description>Pure pursuit controller</description>
7+
<maintainer email="[email protected]">Shrijit Singh</maintainer>
8+
<license>BSD-3-Clause</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
12+
<depend>nav2_common</depend>
13+
<depend>nav2_core</depend>
14+
<depend>nav2_util</depend>
15+
<depend>nav2_costmap_2d</depend>
16+
<depend>rclcpp</depend>
17+
<depend>geometry_msgs</depend>
18+
<depend>nav2_msgs</depend>
19+
<depend>pluginlib</depend>
20+
<depend>tf2</depend>
21+
22+
<exec_depend>nav2_bringup</exec_depend>
23+
24+
<test_depend>ament_cmake_gtest</test_depend>
25+
26+
<export>
27+
<build_type>ament_cmake</build_type>
28+
</export>
29+
30+
</package>

0 commit comments

Comments
 (0)