Skip to content

Commit

Permalink
Support for multi robot simulation (#51)
Browse files Browse the repository at this point in the history
* Multi robot configuration

Signed-off-by: Franco Cipollone <[email protected]>

* Add some todos

Signed-off-by: Franco Cipollone <[email protected]>

* Fixes scope of arguments.

Signed-off-by: Franco Cipollone <[email protected]>

* Fixes format.

Signed-off-by: Franco Cipollone <[email protected]>

* Updates documentation.

Signed-off-by: Franco Cipollone <[email protected]>

* Updates docs

Signed-off-by: Franco Cipollone <[email protected]>

* Some fixes to readme file/.

Signed-off-by: Franco Cipollone <[email protected]>

---------

Signed-off-by: Franco Cipollone <[email protected]>
  • Loading branch information
francocipollone authored May 6, 2024
1 parent 0dc6248 commit bf6e19c
Show file tree
Hide file tree
Showing 12 changed files with 246 additions and 141 deletions.
60 changes: 38 additions & 22 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -55,29 +55,61 @@ Once the package is built and sourced, you can start a simulation.

_Note: You can use `--world_name` flag to indicate other [world](andino_gz/worlds/) to use. (For example: `depot.sdf`(default), `empty.sdf`)_

If you'd like to work from ROS you can launch the ros bridge by adding the corresponding flag
By default the ros bridge and rviz is initialized. In case you prefer to disable any of those you can do it via its flags:

```sh
ros2 launch andino_gz andino_gz.launch.py ros_bridge:=true
ros2 launch andino_gz andino_gz.launch.py ros_bridge:=false rviz:=false
```

(Optional) Or launching it separately via:

To see a complete list of available arguments for the launch file do:
```sh
ros2 launch andino_gz gz_ros_bridge.launch.py
ros2 launch andino_gz andino_gz.launch.py --show-args
```

Make sure to review the required topics using `ign topics` and `ros2 topic` CLI tools.
Also, consider using looking at the translation entries under `andino_gz/config/bridge_config.yaml`.

### Multi robot simulation

This simulation also support multi robot simulation.

```sh
ros2 launch andino_gz andino_gz.launch.py robots:="
andino1={x: 0.0, y: 0.0, z: 0.1, yaw: 0.};
andino2={x: -0.4, y: 0.1, z: 0.1, yaw: 0.};
andino3={x: -0.4, y: -0.1, z: 0.1, yaw: 0.};
andino4={x: -0.8, y: 0.2, z: 0.1, yaw: 0.};
andino5={x: -0.8, y: -0.2, z: 0.1, yaw: 0.};
andino6={x: -0.8, y: 0.0, z: 0.1, yaw: 0.};"
```

_Note: You can add as many as you want_

<img src="./docs/media/andino_gz_multi_robot.png" width="800"/>

The launch file is in charge of:
- Start Gazebo simulator with a defined world (See '--world_name' flag)
- Spawn as many robots as commanded.
- Launch ros bridge for each robot.
- Launch rviz visualization for each robot.

The simulation allows spawn as many robots as you want via the `--robots` flags.
For that you can pass the information of the robots in some sort of YAML format via ROS2 cli:
```yaml
<robot_name>={x: 0.0, y: 0.0, yaw: 0.0, roll: 0.0, pitch: 0.0, yaw: 0.0};
```
Note a ROS Namespace is pushed for each robot so all the topics and nodes are called the same with a difference of a `<robot_name>` prefix.


### SLAM

<img src="./docs/media/andino_slam.png" width="800"/>

1. Run simulation with ros bridge and RViz.

```sh
ros2 launch andino_gz andino_gz.launch.py ros_bridge:=true rviz:=true
ros2 launch andino_gz andino_gz.launch.py
```

2. Run slam toolbox
Expand All @@ -93,22 +125,6 @@ Also, consider using looking at the translation entries under `andino_gz/config/

3. Visualize in RViz: Add `map` panel to RViz and see how the map is being generated.

### Spawn multiple Andinos
Launch simulation as before:
```sh
ros2 launch andino_gz andino_gz.launch.py
```
This will spawn only one Andino in the simulation
For spawning more Andinos you can use the `spawn_robot` launch file. Make sure a different `entity` name is passed as argument as well as initial positions.
```sh
ros2 launch andino_gz spawn_robot.launch.py entity:=andino_n initial_pose_x:=1 initial_pose_y:=1
```
_Note: Andino is spawned but no bridge to ROS2 bridge are spawned for those robots._
## :raised_hands: Contributing

Issues or PRs are always welcome! Please refer to [CONTRIBUTING](CONTRIBUTING.md) doc.
Expand Down
40 changes: 19 additions & 21 deletions andino_gz/config/bridge_config.yaml
Original file line number Diff line number Diff line change
@@ -1,46 +1,44 @@
- ros_topic_name: "/clock"
gz_topic_name: "/clock"
ros_type_name: "rosgraph_msgs/msg/Clock"
gz_type_name: "gz.msgs.Clock"
direction: GZ_TO_ROS
- ros_topic_name: "/odom"
gz_topic_name: "/model/andino/odometry"
# Placeholders to be rewritten by launch file:
# - <entity> : Should be replaced by entity name: For example "andino", "andino_1", "andino_2".

- ros_topic_name: "odom"
gz_topic_name: "/model/<entity>/odometry"
ros_type_name: "nav_msgs/msg/Odometry"
gz_type_name: "gz.msgs.Odometry"
direction: GZ_TO_ROS
# odom <-> base_link transform
- ros_topic_name: "/tf"
gz_topic_name: "/model/andino/pose"
- ros_topic_name: "tf"
gz_topic_name: "/model/<entity>/pose"
ros_type_name: "tf2_msgs/msg/TFMessage"
gz_type_name: "gz.msgs.Pose_V"
direction: GZ_TO_ROS
- ros_topic_name: "/joint_states"
gz_topic_name: "/world/gazebo_world/model/andino/joint_state"
- ros_topic_name: "joint_states"
gz_topic_name: "/world/gazebo_world/model/<entity>/joint_state"
ros_type_name: "sensor_msgs/msg/JointState"
gz_type_name: "gz.msgs.Model"
direction: GZ_TO_ROS
- ros_topic_name: "/camera_info"
gz_topic_name: "/world/gazebo_world/model/andino/link/base_link/sensor/camera/camera_info"
- ros_topic_name: "camera_info"
gz_topic_name: "/world/gazebo_world/model/<entity>/link/base_link/sensor/camera/camera_info"
ros_type_name: "sensor_msgs/msg/CameraInfo"
gz_type_name: "gz.msgs.CameraInfo"
direction: GZ_TO_ROS
- ros_topic_name: "/image_raw"
gz_topic_name: "/world/gazebo_world/model/andino/link/base_link/sensor/camera/image"
- ros_topic_name: "image_raw"
gz_topic_name: "/world/gazebo_world/model/<entity>/link/base_link/sensor/camera/image"
ros_type_name: "sensor_msgs/msg/Image"
gz_type_name: "gz.msgs.Image"
direction: GZ_TO_ROS
- ros_topic_name: "/scan"
gz_topic_name: "/world/gazebo_world/model/andino/link/base_link/sensor/sensor_ray_front/scan"
- ros_topic_name: "scan"
gz_topic_name: "/world/gazebo_world/model/<entity>/link/base_link/sensor/sensor_ray_front/scan"
ros_type_name: "sensor_msgs/msg/LaserScan"
gz_type_name: "gz.msgs.LaserScan"
direction: GZ_TO_ROS
- ros_topic_name: "/scan/points"
gz_topic_name: "/world/gazebo_world/model/andino/link/base_link/sensor/sensor_ray_front/scan/points"
- ros_topic_name: "scan/points"
gz_topic_name: "/world/gazebo_world/model/<entity>/link/base_link/sensor/sensor_ray_front/scan/points"
ros_type_name: "sensor_msgs/msg/PointCloud2"
gz_type_name: "gz.msgs.PointCloudPacked"
direction: GZ_TO_ROS
- ros_topic_name: "/cmd_vel"
gz_topic_name: "/model/andino/cmd_vel"
- ros_topic_name: "cmd_vel"
gz_topic_name: "/model/<entity>/cmd_vel"
ros_type_name: "geometry_msgs/msg/Twist"
gz_type_name: "gz.msgs.Twist"
direction: ROS_TO_GZ
170 changes: 113 additions & 57 deletions andino_gz/launch/andino_gz.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,75 +3,131 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, LogInfo
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
import xacro
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression, TextSubstitution
from launch_ros.actions import Node, PushRosNamespace

from nav2_common.launch import ParseMultiRobotPose


def generate_launch_description():
pkg_andino_gz = get_package_share_directory('andino_gz')

ros_bridge_arg = DeclareLaunchArgument(
'ros_bridge', default_value='false', description = 'Run ROS bridge node.')
rviz_arg = DeclareLaunchArgument('rviz', default_value='false', description='Start RViz.')
world_name_arg = DeclareLaunchArgument('world_name', default_value='depot.sdf', description='Name of the world to load.')
'ros_bridge', default_value='true', description='Run ROS bridge node.')
rviz_arg = DeclareLaunchArgument('rviz', default_value='true', description='Start RViz.')
world_name_arg = DeclareLaunchArgument(
'world_name', default_value='depot.sdf', description='Name of the world to load.')
robots_arg = DeclareLaunchArgument(
'robots', default_value="andino={x: 0., y: 0., z: 0.1, yaw: 0.};",
description='Robots to spawn, multiple robots can be stated separated by a ; ')

# Variables of launch file.
rviz = LaunchConfiguration('rviz')
ros_bridge = LaunchConfiguration('ros_bridge')
world_name = LaunchConfiguration('world_name')
world_path = PathJoinSubstitution([pkg_andino_gz, 'worlds', world_name])

# Gazebo Sim
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')
),
launch_arguments={'gz_args': world_path}.items(),
)

# Spawn the robot and the Robot State Publisher node.
spawn_robot_and_rsp = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_andino_gz, 'launch', 'spawn_robot.launch.py')
),
launch_arguments={
'entity': 'andino',
'initial_pose_x': '0',
'initial_pose_y': '0',
'initial_pose_z': '0.1',
'initial_pose_yaw': '0',
'robot_description_topic': '/robot_description',
'use_sim_time': 'true',
}.items(),
)
# Obtains world path.
world_path = PathJoinSubstitution([pkg_andino_gz, 'worlds', world_name])

# RViz
rviz = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(pkg_andino_gz, 'rviz', 'andino_gz.rviz')],
parameters=[{'use_sim_time': True}],
condition=IfCondition(LaunchConfiguration('rviz'))
base_group = GroupAction(
scoped=True, forwarding=False,
launch_configurations={
'ros_bridge': ros_bridge,
'world_name': world_name
},
actions=[
# Gazebo Sim
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')
),
launch_arguments={'gz_args': world_path}.items(),
),
# ROS Bridge for generic Gazebo stuff
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'],
output='screen',
namespace='andino_gz_sim',
condition=IfCondition(ros_bridge),
),
]
)

# Run ros_gz bridge
ros_bridge = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_andino_gz, 'launch', 'gz_ros_bridge.launch.py')
),
condition=IfCondition(LaunchConfiguration('ros_bridge'))
)
robots_list = ParseMultiRobotPose('robots').value()
# When no robots are specified, spawn a single robot at the origin.
# The default value isn't getting parsed correctly, so we need to check for an empty dictionary.
if (robots_list == {}):
robots_list = {"andino": {"x": 0., "y": 0., "z": 0.1, "yaw": 0.}}
log_number_robots = LogInfo(msg="Robots to spawn: " + str(robots_list))
spawn_robots_group = []
for robot_name in robots_list:
init_pose = robots_list[robot_name]
# As it is scoped and not forwarding, the launch configuration in this context gets cleared.
group = GroupAction(
scoped=True, forwarding=False,
launch_configurations={
'rviz': rviz,
'ros_bridge': ros_bridge
},
actions=[
LogInfo(msg="Group for robot: " + robot_name),
PushRosNamespace(
condition=IfCondition(
PythonExpression([TextSubstitution(text=str(len(robots_list.keys()))), ' > 1'])),
namespace=robot_name),
# Spawn the robot and the Robot State Publisher node.
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_andino_gz, 'launch', 'include', 'spawn_robot.launch.py')
),
launch_arguments={
'entity': robot_name,
'initial_pose_x': str(init_pose['x']),
'initial_pose_y': str(init_pose['y']),
'initial_pose_z': str(init_pose['z']),
'initial_pose_yaw': str(init_pose['yaw']),
'robot_description_topic': 'robot_description',
'use_sim_time': 'true',
}.items(),
),
# RViz
Node(
condition=IfCondition(rviz),
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(pkg_andino_gz, 'rviz', 'andino_gz.rviz')],
parameters=[{'use_sim_time': True}],
remappings=[
('/tf', 'tf'),
('/tf_static', 'tf_static'),
],
),
# Run ros_gz bridge
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_andino_gz, 'launch', 'include', 'gz_ros_bridge.launch.py')
),
launch_arguments={
'entity': robot_name,
}.items(),
condition=IfCondition(LaunchConfiguration('ros_bridge')),
)
]
)
spawn_robots_group.append(group)

return LaunchDescription(
[
# Arguments and Nodes
ros_bridge_arg,
rviz_arg,
world_name_arg,
gazebo,
ros_bridge,
spawn_robot_and_rsp,
rviz,
]
)
ld = LaunchDescription()
ld.add_action(log_number_robots)
ld.add_action(ros_bridge_arg)
ld.add_action(rviz_arg)
ld.add_action(world_name_arg)
ld.add_action(robots_arg)
ld.add_action(base_group)
for group in spawn_robots_group:
ld.add_action(group)
return ld
26 changes: 0 additions & 26 deletions andino_gz/launch/gz_ros_bridge.launch.py

This file was deleted.

Loading

0 comments on commit bf6e19c

Please sign in to comment.