ROS2: Path Planning Algorithms Python Implementation

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