Very basic Dynamic Window Approach implementation. This is an reimplementation of DWA based on the repo https://github.com/amslabtech/dwa_planner/tree/master for ROS 2 Humble. This repo was made as an attemp to fully understand the implementation of the Dynamic Window Approach. The repository is for learning purpose.
- Ubuntu 22.04
- ROS Humble
- geometry_msgs
- nav_msgs
- sensor_msgs
- visualization_msgs
- rosgraph_msgs
- builtin_interfaces
- rclcpp
- tf2
- tf2_ros
- tf2_geometry_msgs: For Quaternion related calculation
- Eigen3: For vector and matrix operation
mkdir ~/ros_ws/src
cd /ros_ws/src
git clone https://github.com/TanLeVan/ros2_dwa_planner.git
colcon build --packages-select ros2_dwa_planner
- Publish appropriate velocity command for the robot to avoid obstacle while navigating to goal
- "/scan" topic:
- Description: 2D scan message for obstacle detection
- Type: sensor_msgs/msg/LaserScan
- "/odom" topic:
- Decsription: current velocity and position of the robot
- Type: sensor_msgs/msg/Odom
- "/goal" topic:
- Description: 2D pose of goal
- Type: geometry_msgs/msg/PoseStamped
- "/cmd_vel" topic:
- Description: Topic which the robot is expected to subscribe. The velocity of the robot is controlled via this topic
- Type: geometry_msgs/msg/Twist
Adjust the behaviour of DWA through these parameter:
- linear_vel_sample_size_: how many discrete linear velocity sample is divided from the dinamic window.
- yaw_rate_sample_size_: how many discrete angular velocity sample is divided from the dinamic window excluding 0(rad/s) (no turn motion). NOTE that 0 angular velocity case is always considered seperately.
- sim_time_: how far in the future the motion of robot is predicted for 1 pair of linear angular velocity
- sim_timestep_: time intervel in the process of predicting robot trajectory
- scan_range_: scan data point within this range will be acknowledged as obstacle. Point outside this range will not be considered as obstacle.
- skip_point_: while converting scan data point to obstacle point, how many points in scan data will be skipped consercutively. This is to reduce the number of obstacle point to be processed.
- use_footprint_: If this is set to true, footprint of the robot should be provided via the "/footprint" topic with type geometry_msgs/msg/PolygonStamped.
- circle_footprint_radius_: Default footprint is circle. This is the radius of the default footprint.
- distance_to_goal_threshold_: How close to the goal to be considered as goal reached.
Using turtlebot3_gazebo for demostration
- Start turtlebot3 simulation:
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
- Start goal publisher:
ros2 run ros2_dwa_planner goal_publisher --ros-arg --params-file <path/to/config.yaml/file.installation>
- Start dwa node:
ros2 run ros2_dwa_planner dwa_planner_node --ros-arg --params-file <path/to/config.yaml/file.installation>
- The process of calculating the distance between polygon footprint to obstacle take to long
- Create trajectory visualization
- Adding rotation at goal to rotate to the desired posed.
- Adding more custom criteria for selecting velocity beside goal cost, obstacle cost and speed cost
- Update README
The original proposal of DWA algorithm collision avoidance is in:
- D. Fox, W. Burgard, and S.Thrun, "The dynamic window approach to collision avoidance", IEEE Robotics Automation Magazine, 1997.