ROS2: Path Planning Algorithms Python Implementation
If you want to thank the author, please use icons on the right next time you shop on Amazon

Compiling and Running

Compilation

We use different mode values (simple - slam - map) and accordingly, different RViz config files. Alltogether, it creates kind of a mess, but the logic is simple.

                        # Terminal 1
                        # echo $ROS_DISTRO
                        # galactic
                        #
                        # source /opt/ros/galactic/setup.bash
                        #
                        $ cd ~/SnowCron/ros_projects/harsh
                        $ colcon build --packages-select multi_bot_nav_03
                        $ source install/setup.bash
                    

Single bot mode


                    # To run a single robot without SLAM or Map (robot name in code is set to ""):
                    $ ros2 launch multi_bot_nav_03 multi_simulation_launch.py world:=src/worlds/world_01.sdf map:=src/maps/world_01.yaml keepout_mask:=src/maps/keepout_world_01.yaml rviz_config_file:=src/multi_bot_nav_03/rviz/simple_single.rviz mode:=simple
                    
                

                    # To run a single robot simulation (SLAM) (robot name set to ""):
                    $ ros2 launch multi_bot_nav_03 multi_simulation_launch.py world:=src/worlds/world_01.sdf map:=src/maps/world_01.yaml keepout_mask:=src/maps/keepout_world_01.yaml rviz_config_file:=src/multi_bot_nav_03/rviz/slam_single.rviz mode:=slam
                    
                


                    # To run a single robot simulation (Map) (robot name set to ""):
                    $ ros2 launch multi_bot_nav_03 multi_simulation_launch.py world:=src/worlds/world_01.sdf map:=src/maps/world_01.yaml keepout_mask:=src/maps/keepout_world_01.yaml rviz_config_file:=src/multi_bot_nav_03/rviz/map_single.rviz mode:=map
                    
                


                    # To run a multi robot without SLAM or Map (robot name set to robot1, robot2). Do not
                    # forget to uncomment code for multiple robots in the launch file, and to comment out
                    # the code for a single robot with empty namespace:
                    $ ros2 launch multi_bot_nav_03 multi_simulation_launch.py world:=src/worlds/world_01.sdf map:=src/maps/world_01.yaml keepout_mask:=src/maps/keepout_world_01.yaml rviz_config_file:=src/multi_bot_nav_03/rviz/simple_multi.rviz mode:=simple
                    
                


                    # To run a multi robot simulation (SLAM):
                    $ ros2 launch multi_bot_nav_03 multi_simulation_launch.py world:=src/worlds/world_01.sdf map:='src/maps/world_01.yaml' keepout_mask:=src/maps/keepout_world_01.yaml rviz_config_file:=src/multi_bot_nav_03/rviz/slam_multi.rviz mode:=slam
                    
                

                    # To run a multi robot simulation (Map):
                    $ ros2 launch multi_bot_nav_03 multi_simulation_launch.py world:=src/worlds/world_01.sdf map:='src/maps/world_01.yaml' keepout_mask:=src/maps/keepout_world_01.yaml rviz_config_file:=src/multi_bot_nav_03/rviz/map_multi.rviz mode:=map
                    
                

Issue commands

To control robots manually:



                    # Terminal 2
                    $ cd ~/SnowCron/ros_projects/harsh
                    $ source install/setup.bash

                    # To move a single robot
                    # Single command:
                    $ ros2 topic pub cmd_vel geometry_msgs/msg/Twist '{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x:0.0, y: 0.0, z: 0.0}}'
                    # Teleop tool:
                    $ ros2 run teleop_twist_keyboard teleop_twist_keyboard

                    # to move the robot in multi-robot launch, use one of the following
                    # (you can use them simultaneously):
                    $ ros2 topic pub /robot1/cmd_vel geometry_msgs/msg/Twist '{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}'
                    $ ros2 topic pub /robot2/cmd_vel geometry_msgs/msg/Twist '{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}'
                    $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --remap cmd_vel:=/robot1/cmd_vel
                    $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --remap cmd_vel:=/robot2/cmd_vel

                    
                

Nav2 Scripts

Waypoints following and Docking to charger

These are for backward compatibility: we have our custom path planning and following, but Nav2 still works.

To run these scripts, Terminal 1 should have our launch file started with "mode" equal "map", either for single robot configuration (robot name set to "") or in multi robot configuration. Just to remind you: it can be changed by commenting / uncommenting the following lines in multi_simulation_launch.py:


                    robots = [
                    {'name': '', 'x_pos': 0.0, 'y_pos': 0.5, 'z_pos': 0.01,
                    "color_name" : "Green", "color_rgb" : "0 1 0 1"},
                    # {'name': 'robot1', 'x_pos': 0.0, 'y_pos': 0.5, 'z_pos': 0.01,
                    # "color_name" : 'Yellow', "color_rgb" : "1 1 0 1"},
                    # {'name': 'robot2', 'x_pos': 0.0, 'y_pos': -0.5, 'z_pos': 0.01,
                    # "color_name" : 'Blue', "color_rgb" : "0 0 1 1"},
                    ]
                    
                

To run simple waypoint follower script for a single robot (robot name is ""):

                        # Terminal 3:
                        $ cd ~/SnowCron/ros_projects/harsh/src/multi_bot_nav_03/multi_bot_nav_03
                        $ python3 01_waypoint_follower.py
                    

To run docker charging script for a single robot (robot name is ""):

                        # Terminal 3:
                        $ cd ~/SnowCron/ros_projects/harsh/src/multi_bot_nav_03/multi_bot_nav_03
                        $ python3 03_charger_docking.py
                    

Finally, to run dockcing charging script on multiple (2) robots:

                        # Terminal 3:
                        $ cd ~/SnowCron/ros_projects/harsh/src/multi_bot_nav_03/multi_bot_nav_03
                        $ python3 04_charger_docking_obstacles_multi.py
                    

Custom path planning and following

Same as earlier: to run these scripts, Terminal 1 should have our launch file started with "mode" equal "map", either for single robot configuration (robot name set to "") or in multi robot configuration. Just to remind you: it can be changed by commenting / uncommenting the following lines in multi_simulation_launch.py:


                    robots = [
                    {'name': '', 'x_pos': 0.0, 'y_pos': 0.5, 'z_pos': 0.01,
                    "color_name" : "Green", "color_rgb" : "0 1 0 1"},
                    # {'name': 'robot1', 'x_pos': 0.0, 'y_pos': 0.5, 'z_pos': 0.01,
                    # "color_name" : 'Yellow', "color_rgb" : "1 1 0 1"},
                    # {'name': 'robot2', 'x_pos': 0.0, 'y_pos': -0.5, 'z_pos': 0.01,
                    # "color_name" : 'Blue', "color_rgb" : "0 0 1 1"},
                    ]
                    
                

To run custom path planning and following with a single robot (robot name is ""):

                        # Terminal 3:
                        $ cd ~/SnowCron/ros_projects/harsh/src/multi_bot_nav_03/multi_bot_nav_03
                        $ source ../../../install/setup.bash
                        $ python3 Coordinator.py
                    

To run path custom path planning and following with a multiple robots:

                        # Terminal 3:
                        $ cd ~/SnowCron/ros_projects/harsh/src/multi_bot_nav_03/multi_bot_nav_03
                        $ source ../../../install/setup.bash
                        $ python3 Coordinator.py
                    

(C) snowcron.com, all rights reserved

Please read the disclaimer