├── CMakeLists.txt ├── LICENSE ├── README.md ├── config └── pure_pursuit_controller.yaml ├── images └── rviz.png ├── include └── curio_navigation │ └── PurePursuitController.h ├── launch ├── pure_pursuit.launch └── test_send_waypoints.launch ├── package.xml ├── rviz └── default.rviz ├── scripts ├── show_trajectory.py └── test_send_ref_path.py ├── src ├── PurePursuitController.cpp └── PurePursuitControllerNode.cpp └── wp └── path.txt /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pure_pursuit_controller) 3 | 4 | # Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | roslaunch 9 | roscpp 10 | rospy 11 | geometry_msgs 12 | nav_msgs 13 | visualization_msgs 14 | tf 15 | ) 16 | 17 | roslaunch_add_file_check(launch) 18 | 19 | ################################################################################ 20 | # Declare catkin configuration 21 | catkin_package( 22 | INCLUDE_DIRS include 23 | LIBRARIES PurePursuitController 24 | ) 25 | ################################################################################ 26 | # Build 27 | include_directories( 28 | include 29 | ${catkin_INCLUDE_DIRS} 30 | ) 31 | 32 | add_library(PurePursuitController 33 | src/PurePursuitController.cpp 34 | ) 35 | target_link_libraries(PurePursuitController ${catkin_LIBRARIES}) 36 | 37 | add_executable(PurePursuitControllerNode 38 | src/PurePursuitControllerNode.cpp 39 | ) 40 | target_link_libraries(PurePursuitControllerNode 41 | PurePursuitController ${catkin_LIBRARIES} 42 | ) 43 | add_dependencies(PurePursuitControllerNode PurePursuitController) 44 | 45 | ################################################################################ 46 | # Install 47 | 48 | catkin_install_python(PROGRAMS 49 | scripts/test_send_ref_path.py 50 | scripts/show_trajectory.py 51 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 52 | ) 53 | 54 | install(TARGETS PurePursuitController 55 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 56 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 57 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 58 | 59 | install(DIRECTORY include/${PROJECT_NAME}/ 60 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 61 | 62 | install( 63 | DIRECTORY config launch 64 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 65 | ) 66 | 67 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Todd Tang 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Pure Pursuit Controller for ROS 2 | Pure Pursuit Path Tracking Controller for ROS (Modified from [here](https://github.com/jmaye/pure-pursuit-controller-ros)), tested on an Ackermann-steering vehicle. 3 | ![](images/rviz.png) 4 | 5 | ## Environment: 6 | - Ubuntu 18.04 7 | - ROS Melodic 8 | 9 | 10 | ## Requirements: 11 | 1. **tf from map to base_link**, which can be obtained using *[AMCL](http://wiki.ros.org/amcl)* node or *[robot_localization](http://docs.ros.org/melodic/api/robot_localization/html/index.html)* node. 12 | 13 | 2. The *reference_path* is published on *'map'* frame. 14 | 15 | 16 | ## Usage: 17 | 18 | ### Configuration: 19 | 20 | **See pure_pursuit_controller.yaml** 21 | 22 | ### Launch: 23 | 0. Make .py Executable 24 | ```bash 25 | $chmod +x [path_to_your_workspace]/scripts/show_trajectory.py 26 | $chmod +x [path_to_your_workspace]/scripts/test_send_ref_path.py 27 | ``` 28 | 29 | 1. Run the Pure Pursuit Controller: 30 | 31 | ```bash 32 | $roslaunch pure_pursuit_controller pure_pursuit.launch 33 | ``` 34 | 35 | 2. Send waypoints reference path 36 | 37 | ```bash 38 | $roslaunch pure_pursuit_controller test_send_waypoints.launch 39 | ``` 40 | 41 | ### Tuning: 42 | In General, the ***look_ahead_ratio*** and ***look_ahead_constant*** in *pure_pursuit_controller.yaml* determines the Look-ahead-Distance in Pure-Pursuit, which mainly affects the Path Tracking Performance. See [Matlab: PurePursuit](https://ww2.mathworks.cn/help/robotics/ug/pure-pursuit-controller.html) for more information. 43 | 44 | 45 | ## Reference: 46 | 47 | - [Pure-Pursuit ROS](https://github.com/jmaye/pure-pursuit-controller-ros) 48 | 49 | - [PythonRobotics: PurePursuit](https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathTracking/pure_pursuit) 50 | 51 | - [Matlab: PurePursuit](https://ww2.mathworks.cn/help/robotics/ug/pure-pursuit-controller.html) -------------------------------------------------------------------------------- /config/pure_pursuit_controller.yaml: -------------------------------------------------------------------------------- 1 | pure_pursuit_controller: 2 | # Reference Path Topic Name 3 | ref_path_topic_name: "/reference_path" 4 | # Vehicle Odometry Topic Name 5 | odom_topic_name: "/odometry/filtered_map" 6 | # Vehicel Command-Velocity Topic Name 7 | cmd_vel_topic_name: "/ackermann_drive_controller/cmd_vel" 8 | # Next Waypoint Topic Name for Rviz 9 | interp_waypoint_topic_name: "/interpolated_waypoint_pose" 10 | # Vehicle Pose Frame ID 11 | pose_frame_id: "base_link" 12 | # Max Queue Size for publishers and subscribers 13 | queue_depth: 100 14 | # The Pure Pursuit Controller Frequency 15 | control_frequency: 20.0 16 | # Vehicle Target Linear Velocity 17 | target_velocity: 0.2 # [m/s] 18 | # Look-ahead-Distance = look_ahead_ratio*v + look_ahead_constant 19 | look_ahead_ratio: 0.1 20 | look_ahead_constant: 1.0 21 | # Delta Pose Error Threshold 22 | epsilon: 1e-6 23 | 24 | trajectory_drawer: 25 | # Trajectory Topic Name for Rviz 26 | traj_topic_name: "/planned_trajectory" 27 | # Vehicle Odometry Topic Name 28 | odom_topic_name: "/odometry/filtered_map" 29 | # Trajectoy Update Frequency 30 | frequency: 1.0 -------------------------------------------------------------------------------- /images/rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/reso1/PurePursuitControllerROS/97366eed250af6feb5e04078f6f2d042f289ed49/images/rviz.png -------------------------------------------------------------------------------- /include/curio_navigation/PurePursuitController.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2014 by Todd Tang * 3 | * todd.j.tang@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file PurePursuitController.h 20 | \brief This file defines the PurePursuitController class which 21 | implements a pure pursuit controller. 22 | */ 23 | 24 | #ifndef PURE_PURSUIT_CONTROLLER_H 25 | #define PURE_PURSUIT_CONTROLLER_H 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | #include 41 | 42 | 43 | /** The class PurePursuitController implements a pure pursuit controller. 44 | \brief Pure pursuit controller 45 | */ 46 | class PurePursuitController { 47 | public: 48 | /** \name Constructors/destructor @{ */ 49 | /// Constructor 50 | PurePursuitController(const ros::NodeHandle& nh); 51 | /// Copy constructor 52 | PurePursuitController(const PurePursuitController& other) = delete; 53 | /// Copy assignment operator 54 | PurePursuitController& operator = 55 | (const PurePursuitController& other) = delete; 56 | /// Move constructor 57 | PurePursuitController(PurePursuitController&& other) = delete; 58 | /// Move assignment operator 59 | PurePursuitController& operator = 60 | (PurePursuitController&& other) = delete; 61 | /// Destructor 62 | virtual ~PurePursuitController(); 63 | /** @} */ 64 | 65 | /** \name Methods @{ */ 66 | /// Spin once 67 | void spin(); 68 | /// Step once 69 | bool step(geometry_msgs::Twist& twist); 70 | /// Returns the current pose of the robot 71 | geometry_msgs::PoseStamped getCurrentPose() const; 72 | /// Returns the lookahead distance for the given pose 73 | double getDistanceToPose(const geometry_msgs::PoseStamped& pose) const; 74 | /// Returns the lookahead angle for the given pose in [rad] 75 | double getAngleToPose(const geometry_msgs::PoseStamped& pose) const; 76 | /// Returns the current lookahead distance threshold 77 | double getLookAheadThreshold() const; 78 | /// Returns the next way point by linear search from the current waypoint 79 | int getNextWayPoint(); 80 | /// Returns the current closest waypoint 81 | int getClosestWayPoint() const; 82 | /** @} */ 83 | 84 | protected: 85 | /** \name Protected methods @{ */ 86 | /// Retrieves parameters 87 | void getParameters(); 88 | /// Initialize messages 89 | void initMessages(); 90 | /// Path message callback 91 | void pathCallback(const nav_msgs::Path& msg); 92 | /// Odometry message callback 93 | void odometryCallback(const nav_msgs::Odometry& msg); 94 | /// Timer callback 95 | void timerCallback(const ros::TimerEvent& event); 96 | /// Trajectory timer callback 97 | void trajectoryCallback(const ros::TimerEvent& event); 98 | /** @} */ 99 | 100 | /** \name Protected members @{ */ 101 | /// ROS node handle 102 | ros::NodeHandle _nodeHandle; 103 | /// Path message subscriber 104 | ros::Subscriber _pathSubscriber; 105 | /// Path message topic name 106 | std::string _pathTopicName; 107 | /// Odometry message subscriber 108 | ros::Subscriber _odometrySubscriber; 109 | /// Odometry message topic name 110 | std::string _odometryTopicName; 111 | /// Frame id of reference path 112 | std::string _refPathFrameId; 113 | /// Frame id of pose estimates 114 | std::string _poseFrameId; 115 | /// Queue size for receiving messages 116 | int _queueDepth; 117 | /// Current reference path 118 | nav_msgs::Path _currentReferencePath; 119 | /// Reference Path Pose length 120 | int _refPathLength; 121 | /// Current pose 122 | geometry_msgs::PoseStamped _currentPose; 123 | /// Current velocity 124 | geometry_msgs::Twist _currentVelocity; 125 | /// Controller frequency 126 | double _control_frequency; 127 | /// Trajectory frequency 128 | double _trajectory_frequency; 129 | /// Next way point 130 | int _nextWayPoint; 131 | /// Commanded velocity publisher 132 | ros::Publisher _cmdVelocityPublisher; 133 | /// Commanded velocity topic name 134 | std::string _cmdVelocityTopicName; 135 | /// Interpolated waypoint pose publisher 136 | ros::Publisher _targetWayPointPublisher; 137 | /// Interpolated waypoint topic name 138 | std::string _targetWayPointPubTopicName; 139 | /// target waypoint marker message 140 | visualization_msgs::Marker _target_marker_msg; 141 | /// Path end pose 142 | geometry_msgs::Pose _pathEndPose; 143 | /// Velocity 144 | double _velocity; 145 | /// Lookahead ratio 146 | double _lookAheadRatio; 147 | /// Lookahead constant 148 | double _lookAheadConstant; 149 | /// Epsilon 150 | double _epsilon; 151 | /// Transform listener for robot's pose w.r.t. map 152 | tf::TransformListener _tfListener; 153 | /// Main Timer 154 | ros::Timer _timer; 155 | /** @} */ 156 | 157 | }; 158 | 159 | 160 | #endif // PURE_PURSUIT_CONTROLLER_H 161 | -------------------------------------------------------------------------------- /launch/pure_pursuit.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 11 | 12 | 14 | -------------------------------------------------------------------------------- /launch/test_send_waypoints.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pure_pursuit_controller 4 | 0.2.2 5 | Pure Pursuit Controller 6 | Todd Tang 7 | BSD-3-Clause 8 | https://github.com/srmainwaring/curio 9 | Todd Tang 10 | catkin 11 | roslaunch 12 | rospy 13 | rospy 14 | rospy 15 | roscpp 16 | geometry_msgs 17 | visualization_msgs 18 | nav_msgs 19 | tf 20 | 21 | 22 | -------------------------------------------------------------------------------- /rviz/default.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Pure Pursuit1 10 | Splitter Ratio: 0.5 11 | Tree Height: 664 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Angle Tolerance: 0.10000000149011612 57 | Class: rviz/Odometry 58 | Covariance: 59 | Orientation: 60 | Alpha: 0.5 61 | Color: 255; 255; 127 62 | Color Style: Unique 63 | Frame: Local 64 | Offset: 1 65 | Scale: 1 66 | Value: true 67 | Position: 68 | Alpha: 0.30000001192092896 69 | Color: 204; 51; 204 70 | Scale: 1 71 | Value: true 72 | Value: true 73 | Enabled: true 74 | Keep: 100 75 | Name: Odometry 76 | Position Tolerance: 0.10000000149011612 77 | Shape: 78 | Alpha: 1 79 | Axes Length: 1 80 | Axes Radius: 0.10000000149011612 81 | Color: 255; 25; 0 82 | Head Length: 0.30000001192092896 83 | Head Radius: 0.10000000149011612 84 | Shaft Length: 1 85 | Shaft Radius: 0.05000000074505806 86 | Value: Arrow 87 | Topic: /odometry/filtered_map 88 | Unreliable: false 89 | Value: true 90 | - Class: rviz/TF 91 | Enabled: true 92 | Frame Timeout: 15 93 | Frames: 94 | All Enabled: true 95 | back_left_corner_link: 96 | Value: true 97 | back_left_wheel_link: 98 | Value: true 99 | back_right_corner_link: 100 | Value: true 101 | back_right_wheel_link: 102 | Value: true 103 | base_link: 104 | Value: true 105 | body_box_link: 106 | Value: true 107 | diff_brace_link: 108 | Value: true 109 | front_left_corner_link: 110 | Value: true 111 | front_left_wheel_link: 112 | Value: true 113 | front_right_corner_link: 114 | Value: true 115 | front_right_wheel_link: 116 | Value: true 117 | imu_link: 118 | Value: true 119 | left_bogie_link: 120 | Value: true 121 | left_rocker_link: 122 | Value: true 123 | map: 124 | Value: true 125 | mid_left_wheel_link: 126 | Value: true 127 | mid_right_wheel_link: 128 | Value: true 129 | odom: 130 | Value: true 131 | right_bogie_link: 132 | Value: true 133 | right_rocker_link: 134 | Value: true 135 | utm: 136 | Value: true 137 | Marker Scale: 1 138 | Name: TF 139 | Show Arrows: true 140 | Show Axes: true 141 | Show Names: true 142 | Tree: 143 | map: 144 | odom: 145 | base_link: 146 | body_box_link: 147 | {} 148 | diff_brace_link: 149 | {} 150 | imu_link: 151 | {} 152 | left_rocker_link: 153 | front_left_corner_link: 154 | front_left_wheel_link: 155 | {} 156 | left_bogie_link: 157 | back_left_corner_link: 158 | back_left_wheel_link: 159 | {} 160 | mid_left_wheel_link: 161 | {} 162 | right_rocker_link: 163 | front_right_corner_link: 164 | front_right_wheel_link: 165 | {} 166 | right_bogie_link: 167 | back_right_corner_link: 168 | back_right_wheel_link: 169 | {} 170 | mid_right_wheel_link: 171 | {} 172 | utm: 173 | {} 174 | Update Interval: 0 175 | Value: true 176 | - Class: rviz/Group 177 | Displays: 178 | - Class: rviz/Marker 179 | Enabled: true 180 | Marker Topic: /interpolated_waypoint_pose 181 | Name: Target 182 | Namespaces: 183 | "": true 184 | Queue Size: 100 185 | Value: true 186 | - Alpha: 1 187 | Buffer Length: 1 188 | Class: rviz/Path 189 | Color: 25; 255; 0 190 | Enabled: true 191 | Head Diameter: 0.30000001192092896 192 | Head Length: 0.20000000298023224 193 | Length: 0.30000001192092896 194 | Line Style: Lines 195 | Line Width: 0.029999999329447746 196 | Name: Reference Path 197 | Offset: 198 | X: 0 199 | Y: 0 200 | Z: 0 201 | Pose Color: 255; 85; 255 202 | Pose Style: None 203 | Radius: 0.029999999329447746 204 | Shaft Diameter: 0.10000000149011612 205 | Shaft Length: 0.10000000149011612 206 | Topic: /reference_path 207 | Unreliable: false 208 | Value: true 209 | - Alpha: 1 210 | Buffer Length: 1 211 | Class: rviz/Path 212 | Color: 25; 255; 0 213 | Enabled: true 214 | Head Diameter: 0.30000001192092896 215 | Head Length: 0.20000000298023224 216 | Length: 0.30000001192092896 217 | Line Style: Lines 218 | Line Width: 0.029999999329447746 219 | Name: Trajectory 220 | Offset: 221 | X: 0 222 | Y: 0 223 | Z: 0 224 | Pose Color: 255; 85; 255 225 | Pose Style: None 226 | Radius: 0.029999999329447746 227 | Shaft Diameter: 0.10000000149011612 228 | Shaft Length: 0.10000000149011612 229 | Topic: /planned_trajectory 230 | Unreliable: false 231 | Value: true 232 | Enabled: true 233 | Name: Pure Pursuit 234 | Enabled: true 235 | Global Options: 236 | Background Color: 48; 48; 48 237 | Default Light: true 238 | Fixed Frame: map 239 | Frame Rate: 30 240 | Name: root 241 | Tools: 242 | - Class: rviz/Interact 243 | Hide Inactive Objects: true 244 | - Class: rviz/MoveCamera 245 | - Class: rviz/Select 246 | - Class: rviz/FocusCamera 247 | - Class: rviz/Measure 248 | - Class: rviz/SetInitialPose 249 | Theta std deviation: 0.2617993950843811 250 | Topic: /initialpose 251 | X std deviation: 0.5 252 | Y std deviation: 0.5 253 | - Class: rviz/SetGoal 254 | Topic: /move_base_simple/goal 255 | - Class: rviz/PublishPoint 256 | Single click: true 257 | Topic: /clicked_point 258 | Value: true 259 | Views: 260 | Current: 261 | Class: rviz/Orbit 262 | Distance: 34.178462982177734 263 | Enable Stereo Rendering: 264 | Stereo Eye Separation: 0.05999999865889549 265 | Stereo Focal Distance: 1 266 | Swap Stereo Eyes: false 267 | Value: false 268 | Focal Point: 269 | X: 4.327235221862793 270 | Y: 6.2528605461120605 271 | Z: 2.8131871223449707 272 | Focal Shape Fixed Size: true 273 | Focal Shape Size: 0.05000000074505806 274 | Invert Z Axis: false 275 | Name: Current View 276 | Near Clip Distance: 0.009999999776482582 277 | Pitch: 0.95539790391922 278 | Target Frame: 279 | Value: Orbit (rviz) 280 | Yaw: 3.990431308746338 281 | Saved: ~ 282 | Window Geometry: 283 | Displays: 284 | collapsed: false 285 | Height: 961 286 | Hide Left Dock: false 287 | Hide Right Dock: false 288 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000323fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000323000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000323fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000323000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000059d0000003efc0100000002fb0000000800540069006d006501000000000000059d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000032c0000032300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 289 | Selection: 290 | collapsed: false 291 | Time: 292 | collapsed: false 293 | Tool Properties: 294 | collapsed: false 295 | Views: 296 | collapsed: false 297 | Width: 1437 298 | X: 67 299 | Y: 27 300 | -------------------------------------------------------------------------------- /scripts/show_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | 5 | from nav_msgs.msg import Path, Odometry 6 | from geometry_msgs.msg import Pose, PoseStamped 7 | from visualization_msgs.msg import Marker 8 | 9 | class TrajectoryDrawer(): 10 | def __init__(self): 11 | rospy.init_node('trajectory_drawer') 12 | 13 | frequency = rospy.get_param( 14 | '/trajectory_drawer/frequency', 1.0) 15 | odom_topic_name = rospy.get_param( 16 | '/trajectory_drawer/odom_topic_name', '/odometry/filtered_map') 17 | traj_topic_name = rospy.get_param( 18 | '/trajectory_drawer/traj_topic_name', '/planned_trajectory') 19 | 20 | self.odom_sub = rospy.Subscriber(odom_topic_name, Odometry, self.odom_callback) 21 | self.traj_pub = rospy.Publisher(traj_topic_name, Path, queue_size=1) 22 | self.timer = rospy.Timer(rospy.Duration(1.0/frequency), self.timer_callback) 23 | 24 | self.cur_pose = Pose() 25 | self.trajectory = Path() 26 | self.trajectory.header.frame_id = 'map' 27 | 28 | 29 | def spin(self): 30 | rospy.spin() 31 | 32 | def odom_callback(self, msg): 33 | self.cur_pose = msg.pose.pose 34 | 35 | def timer_callback(self, event): 36 | ps = PoseStamped() 37 | ps.pose = self.cur_pose 38 | self.trajectory.poses.append(ps) 39 | self.traj_pub.publish(self.trajectory) 40 | 41 | 42 | if __name__ == '__main__': 43 | try: 44 | traj_drawer = TrajectoryDrawer() 45 | traj_drawer.spin() 46 | 47 | except rospy.ROSInterruptException: 48 | rospy.logerr('something went wrong.') 49 | -------------------------------------------------------------------------------- /scripts/test_send_ref_path.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import os 4 | import sys 5 | import math 6 | 7 | import rospy 8 | import rospkg 9 | 10 | from std_msgs.msg import ColorRGBA 11 | from nav_msgs.msg import Path 12 | from geometry_msgs.msg import PoseStamped, Point, Quaternion, Vector3 13 | from visualization_msgs.msg import Marker, MarkerArray 14 | 15 | from tf.transformations import quaternion_from_euler 16 | 17 | 18 | def build_path(wp_path): 19 | # read waypoints 20 | wps = [] 21 | wp_file = open(wp_path) 22 | for line in wp_file.readlines(): 23 | x, y = [float(i) for i in line.split(',')] 24 | wps.append([x, y]) 25 | wp_file.close() 26 | 27 | # build path 28 | path = Path() 29 | path.header.frame_id = 'map' 30 | path.header.stamp = rospy.Time.now() 31 | 32 | for idx in range(len(wps)-1): 33 | # calculate quaternion 34 | dx = math.sqrt((wps[idx+1][0]-wps[idx][0])**2) 35 | dy = math.sqrt((wps[idx+1][1]-wps[idx][1])**2) 36 | quaternion = quaternion_from_euler(0, 0, math.atan2(dy, dx)) 37 | 38 | pose = PoseStamped() 39 | pose.header.frame_id = 'map' 40 | pose.pose.position = Point(wps[idx][0], wps[idx][1], 0.0) 41 | pose.pose.orientation = Quaternion(0, 0, quaternion[2], quaternion[3]) 42 | 43 | path.poses.append(pose) 44 | 45 | rospy.loginfo('fetched ' + str(len(wps)) + ' waypoints') 46 | 47 | if wps == []: 48 | rospy.signal_shutdown('No waypoint to draw...shutdown') 49 | 50 | return path 51 | 52 | 53 | def main(): 54 | rospy.init_node('test_send_ref_path') 55 | 56 | ref_path_topic_name = rospy.get_param( 57 | '/pure_pursuit_controller/ref_path_topic_name', '/reference_path') 58 | ref_path_pub = rospy.Publisher(ref_path_topic_name, Path, queue_size=1) 59 | 60 | r = rospy.Rate(1) 61 | 62 | wp_path = rospy.get_param('waypoint_file_path', 63 | default=os.path.join(rospkg.RosPack().get_path('pure_pursuit_controller'),'wp','path.txt')) 64 | 65 | path = build_path(wp_path) 66 | 67 | while not rospy.is_shutdown(): 68 | ref_path_pub.publish(path) 69 | r.sleep() 70 | 71 | 72 | if __name__ == "__main__": 73 | try: 74 | main() 75 | 76 | except rospy.ROSInterruptException: 77 | rospy.logerr('Get KeyBoardInterrupt...Shutdown') -------------------------------------------------------------------------------- /src/PurePursuitController.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2014 by Todd Tang * 3 | * todd.j.tang@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include "curio_navigation/PurePursuitController.h" 20 | 21 | #include 22 | 23 | #include 24 | 25 | /******************************************************************************/ 26 | /* Constructors and Destructor */ 27 | /******************************************************************************/ 28 | 29 | PurePursuitController::PurePursuitController(const ros::NodeHandle& nh) 30 | : _nodeHandle(nh) 31 | , _nextWayPoint(-1) 32 | { 33 | getParameters(); 34 | 35 | initMessages(); 36 | 37 | _pathSubscriber = _nodeHandle.subscribe( 38 | _pathTopicName, _queueDepth, 39 | &PurePursuitController::pathCallback, this); 40 | 41 | _odometrySubscriber = _nodeHandle.subscribe( 42 | _odometryTopicName, _queueDepth, 43 | &PurePursuitController::odometryCallback, this); 44 | 45 | _cmdVelocityPublisher = _nodeHandle.advertise( 46 | _cmdVelocityTopicName, _queueDepth); 47 | 48 | _targetWayPointPublisher = _nodeHandle.advertise( 49 | _targetWayPointPubTopicName, _queueDepth); 50 | 51 | _timer = _nodeHandle.createTimer(ros::Duration(1.0/_control_frequency), 52 | &PurePursuitController::timerCallback, this); 53 | 54 | } 55 | 56 | PurePursuitController::~PurePursuitController() 57 | { 58 | 59 | } 60 | 61 | /******************************************************************************/ 62 | /* Methods */ 63 | /******************************************************************************/ 64 | 65 | void PurePursuitController::spin() 66 | { 67 | ros::spin(); 68 | } 69 | 70 | 71 | void PurePursuitController::pathCallback(const nav_msgs::Path& msg) 72 | { 73 | if (_nextWayPoint == -1) 74 | { 75 | _refPathFrameId = msg.header.frame_id; 76 | _currentReferencePath = msg; 77 | _refPathLength = _currentReferencePath.poses.size(); 78 | ROS_INFO_STREAM("Received reference path of size " << _refPathLength); 79 | _nextWayPoint = getClosestWayPoint(); 80 | ROS_INFO_STREAM("Start Pure-Pursuit from WayPoint #"<< _nextWayPoint); 81 | _pathEndPose = _currentReferencePath.poses.back().pose; 82 | } 83 | } 84 | 85 | 86 | void PurePursuitController::odometryCallback(const nav_msgs::Odometry& msg) 87 | { 88 | _currentPose.header = msg.header; 89 | _currentPose.pose = msg.pose.pose; 90 | _currentVelocity = msg.twist.twist; 91 | } 92 | 93 | 94 | void PurePursuitController::timerCallback(const ros::TimerEvent& event) 95 | { 96 | if (_nextWayPoint == -1) { 97 | ROS_WARN_ONCE("No reference path received, will keep waiting..."); 98 | return; 99 | } 100 | 101 | geometry_msgs::Point currentPosition = getCurrentPose().pose.position; 102 | tf::Vector3 v1(currentPosition.x, currentPosition.y, currentPosition.z); 103 | tf::Vector3 v2(_pathEndPose.position.x, _pathEndPose.position.y, _pathEndPose.position.z); 104 | 105 | if (tf::tfDistance(v1, v2) < 1.0f) { 106 | ROS_INFO_STREAM_ONCE("Path Following Finished...shutdown"); 107 | ros::shutdown(); 108 | } 109 | 110 | geometry_msgs::Twist cmdVelocity; 111 | 112 | if (step(cmdVelocity)) { 113 | _cmdVelocityPublisher.publish(cmdVelocity); 114 | } 115 | } 116 | 117 | 118 | bool PurePursuitController::step(geometry_msgs::Twist& twist) 119 | { 120 | twist.linear.x = 0.0; 121 | twist.linear.y = 0.0; 122 | twist.linear.z = 0.0; 123 | 124 | twist.angular.x = 0.0; 125 | twist.angular.y = 0.0; 126 | twist.angular.z = 0.0; 127 | 128 | _nextWayPoint = getNextWayPoint(); 129 | 130 | if (_nextWayPoint >= 0) 131 | { 132 | geometry_msgs::PoseStamped target = _currentReferencePath.poses[_nextWayPoint]; 133 | 134 | _target_marker_msg.pose = target.pose; 135 | _targetWayPointPublisher.publish(_target_marker_msg); 136 | 137 | // ld: delta-distance from current pose to goal pose 138 | double l_t = getLookAheadThreshold(); 139 | 140 | geometry_msgs::PoseStamped origin = getCurrentPose(); 141 | double dy = target.pose.position.y - origin.pose.position.y; 142 | double dx = target.pose.position.x - origin.pose.position.x; 143 | // alpha: delta-angle from current pose to goal pose 144 | double alpha = atan2(dy, dx) - tf::getYaw(origin.pose.orientation); 145 | 146 | double angularVelocity = 0.0; 147 | 148 | if (std::abs(std::sin(alpha)) >= _epsilon) 149 | { 150 | // r = ld / (2 * sin(alpha)) 151 | double radius = 0.5 * (l_t / std::sin(alpha)); 152 | 153 | // try to reach the desired linear velocity 154 | double linearVelocity = _velocity; 155 | 156 | // only steering when delta_angle is larger than error_threshold _epsilon 157 | if (std::abs(radius) >= _epsilon) 158 | // omega = v / r 159 | angularVelocity = linearVelocity / radius; 160 | 161 | twist.linear.x = linearVelocity; 162 | twist.angular.z = angularVelocity; 163 | 164 | } 165 | 166 | return true; 167 | } 168 | 169 | return false; 170 | } 171 | 172 | 173 | geometry_msgs::PoseStamped PurePursuitController::getCurrentPose() const 174 | { 175 | geometry_msgs::PoseStamped transformedPose; 176 | 177 | try { 178 | _tfListener.transformPose(_refPathFrameId, _currentPose, transformedPose); 179 | } 180 | catch (tf::TransformException& exception) { 181 | ROS_ERROR_STREAM("PurePursuitController::getCurrentPose: " << 182 | exception.what()); 183 | } 184 | 185 | // get current vehicle pose in ReferencePath frame 186 | return transformedPose; 187 | } 188 | 189 | double PurePursuitController::getDistanceToPose( 190 | const geometry_msgs::PoseStamped& pose) const 191 | { 192 | geometry_msgs::PoseStamped origin = getCurrentPose(); 193 | geometry_msgs::PoseStamped transformedPose; 194 | 195 | try { 196 | _tfListener.transformPose(_refPathFrameId, pose, transformedPose); 197 | } 198 | catch (tf::TransformException& exception) { 199 | ROS_ERROR_STREAM("PurePursuitController::getDistanceToPose: " << 200 | exception.what()); 201 | 202 | return -1.0; 203 | } 204 | 205 | tf::Vector3 v1(origin.pose.position.x, 206 | origin.pose.position.y, 207 | origin.pose.position.z); 208 | tf::Vector3 v2(transformedPose.pose.position.x, 209 | transformedPose.pose.position.y, 210 | transformedPose.pose.position.z); 211 | 212 | // Distance from current vehicle pose to current waypoint in ReferencePath frame 213 | return tf::tfDistance(v1, v2); 214 | } 215 | 216 | double PurePursuitController::getAngleToPose( 217 | const geometry_msgs::PoseStamped& pose) const 218 | { 219 | geometry_msgs::PoseStamped origin = getCurrentPose(); 220 | geometry_msgs::PoseStamped transformedPose; 221 | 222 | try { 223 | _tfListener.transformPose(_refPathFrameId, pose, transformedPose); 224 | } 225 | catch (tf::TransformException& exception) { 226 | ROS_ERROR_STREAM("PurePursuitController::getAngleToPose: " << 227 | exception.what()); 228 | 229 | return -1.0; 230 | } 231 | 232 | tf::Vector3 v1(origin.pose.position.x, 233 | origin.pose.position.y, 234 | origin.pose.position.z); 235 | tf::Vector3 v2(transformedPose.pose.position.x, 236 | transformedPose.pose.position.y, 237 | transformedPose.pose.position.z); 238 | 239 | // Delta Angle from current vehilce pose to current waypoint in ReferencePath frame 240 | return tf::tfAngle(v1, v2); 241 | } 242 | 243 | 244 | double PurePursuitController::getLookAheadThreshold() const 245 | { 246 | return _lookAheadRatio * _currentVelocity.linear.x + _lookAheadConstant; 247 | } 248 | 249 | 250 | int PurePursuitController::getNextWayPoint() 251 | { 252 | int wayPoint = _nextWayPoint; 253 | if (!_currentReferencePath.poses.empty()) { 254 | if (wayPoint >= 0) { 255 | double thisDistance = getDistanceToPose(_currentReferencePath.poses[wayPoint]); 256 | 257 | while (wayPoint < _refPathLength-1) 258 | { 259 | double nextDistance = getDistanceToPose(_currentReferencePath.poses[wayPoint+1]); 260 | if (thisDistance < nextDistance) 261 | break; 262 | wayPoint = wayPoint + 1; 263 | thisDistance = nextDistance; 264 | } 265 | 266 | while (getLookAheadThreshold() > getDistanceToPose(_currentReferencePath.poses[wayPoint])) 267 | { 268 | if (wayPoint + 1 >= _refPathLength) 269 | break; 270 | 271 | wayPoint = wayPoint + 1; 272 | } 273 | 274 | return wayPoint; 275 | } 276 | else 277 | return 0; 278 | 279 | } 280 | 281 | return -1; 282 | } 283 | 284 | 285 | int PurePursuitController::getClosestWayPoint() const { 286 | if (!_currentReferencePath.poses.empty()) { 287 | int closestWaypoint = 0; 288 | double minDistance = getDistanceToPose(_currentReferencePath.poses.front()); 289 | 290 | for (int i = closestWaypoint+1; i < _refPathLength; ++i) { 291 | double distance = getDistanceToPose(_currentReferencePath.poses[i]); 292 | 293 | if (distance < minDistance) { 294 | closestWaypoint = i; 295 | minDistance = distance; 296 | } 297 | } 298 | 299 | return closestWaypoint; 300 | } 301 | 302 | return -1; 303 | } 304 | 305 | 306 | void PurePursuitController::getParameters() 307 | { 308 | std::string param_ns = "pure_pursuit_controller"; 309 | 310 | _nodeHandle.param(param_ns + "ref_path_topic_name", 311 | _pathTopicName, "/reference_path"); 312 | 313 | _nodeHandle.param(param_ns + "odom_topic_name", 314 | _odometryTopicName, "/odometry/filtered_map"); 315 | 316 | _nodeHandle.param(param_ns + "cmd_vel_topic_name", 317 | _cmdVelocityTopicName, "/ackermann_drive_controller/cmd_vel"); 318 | 319 | _nodeHandle.param("interp_waypoint_topic_name", 320 | _targetWayPointPubTopicName, "/interpolated_waypoint_pose"); 321 | 322 | _nodeHandle.param("pose_frame_id", _poseFrameId, "base_link"); 323 | 324 | _nodeHandle.param(param_ns + "queue_depth", _queueDepth, 100); 325 | 326 | _nodeHandle.param(param_ns + "control_frequency", _control_frequency, 20.0); 327 | 328 | _nodeHandle.param(param_ns + "target_velocity", _velocity, 0.2); 329 | 330 | _nodeHandle.param(param_ns + "look_ahead_ratio", _lookAheadRatio, 0.1); 331 | 332 | _nodeHandle.param(param_ns + "look_ahead_constant", _lookAheadConstant, 1.0); 333 | 334 | _nodeHandle.param(param_ns + "epsilon", _epsilon, 1e-6); 335 | } 336 | 337 | 338 | void PurePursuitController::initMessages() 339 | { 340 | // targer marker message 341 | _target_marker_msg.header.frame_id = "map"; 342 | _target_marker_msg.type = visualization_msgs::Marker::SPHERE; 343 | _target_marker_msg.action = visualization_msgs::Marker::ADD; 344 | _target_marker_msg.scale.x = 0.2; 345 | _target_marker_msg.scale.y = 0.2; 346 | _target_marker_msg.scale.z = 0.2; 347 | _target_marker_msg.color.r = 0.5; 348 | _target_marker_msg.color.g = 0; 349 | _target_marker_msg.color.b = 1; 350 | _target_marker_msg.color.a = 1; 351 | } -------------------------------------------------------------------------------- /src/PurePursuitControllerNode.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2014 by Todd Tang * 3 | * todd.j.tang@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file pure_pursuit_controller_node.cpp 20 | \brief This file is the ROS node for a pure pursuit controller. 21 | */ 22 | 23 | #include 24 | 25 | #include "curio_navigation/PurePursuitController.h" 26 | 27 | int main(int argc, char** argv) { 28 | ros::init(argc, argv, "pure_pursuit_controller"); 29 | ros::NodeHandle nh; 30 | try 31 | { 32 | PurePursuitController node(nh); 33 | node.spin(); 34 | } 35 | catch (const std::exception& e) 36 | { 37 | ROS_ERROR_STREAM("Exception: " << e.what()); 38 | return 1; 39 | } 40 | catch (...) 41 | { 42 | ROS_ERROR_STREAM("Unknown Exception"); 43 | return 1; 44 | } 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /wp/path.txt: -------------------------------------------------------------------------------- 1 | 1.0,0.0 2 | 6.0,9.0 3 | 9.0,12.0 4 | 15.0,15.0 5 | 17.0,21.0 6 | 16.0,28.0 --------------------------------------------------------------------------------