Add Multi-agent demo

CMakeLists.txt

@@ -12,7 +12,10 @@ find_package(ament_cmake REQUIRED)
 # find_package(<dependency> REQUIRED)
 
 
+install(DIRECTORY definitions DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY maps DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY models DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
 

README.md

@@ -1,48 +1,112 @@
 # mvsim_nav2_demos
 
+## Launch with Nav2
+
 Launch and configuration files for running [Nav2](https://navigation.ros.org/) on [MVsim](https://github.com/MRPT/mvsim) worlds using ROS 2.
 
 Usage:
+
 - Clone into your `workspace/src` directory.
 - Install dependencies (TESTED ON ROS humble !!):
 
-```bash
-sudo apt install \
-  ros-$ROS_DISTRO-mvsim \
-  ros-$ROS_DISTRO-navigation2 \
-  ros-$ROS_DISTRO-nav2-bringup \
-  ros-$ROS_DISTRO-turtlebot3*
-```
+  ```bash
+  sudo apt install \
+    ros-$ROS_DISTRO-mvsim \
+    ros-$ROS_DISTRO-navigation2 \
+    ros-$ROS_DISTRO-nav2-bringup \
+    ros-$ROS_DISTRO-turtlebot3*
+  ```
 
 - Build as usual (colcon build).
 
-```bash
-colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
-. install/setup.bash
-```
+  ```bash
+  colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
+  . install/setup.bash
+  ```
 
 - Invoke with:
 
-      ros2 launch mvsim_nav2_demos turtlebot_simulation_launch.py
-      ros2 launch mvsim_nav2_demos greenhouse_simulation_launch.py
+  ```bash
+  ros2 launch mvsim_nav2_demos turtlebot_simulation_launch.py
+  ros2 launch mvsim_nav2_demos greenhouse_simulation_launch.py
+
+  ros2 launch mvsim_nav2_demos greenhouse_simulation_launch.py world_file:=$(pwd)/src/mvsim_nav2_demos/launch/demo_greenhouse_jjaa.world.xml
+  ```
 
-      ros2 launch mvsim_nav2_demos greenhouse_simulation_launch.py world_file:=$(pwd)/src/mvsim_nav2_demos/launch/demo_greenhouse_jjaa.world.xml
-
-
 This package is based on [nav2_bringup](https://github.com/ros-planning/navigation2/tree/main/nav2_bringup), so we inherit their Apache-2 License here.
 
+### Multi-agent demo
+
+> ROS2 Humble only ([Issue](https://github.com/ros2/rviz/issues/463) is occurred in Iron and Jazzy.)
+
+Multi-agent demo supports multiple agents with fake-amcl (ground truth on mvsim).
+Therefore, this demo is useful for verification and implementation of the overall system, including path planning.
+
+- Invoke mvsim with:
+
+  ```bash
+  ros2 launch mvsim_nav2_demos demo_warehouse_6robots.launch.py
+  ```
+
+- Invoke nav2 with:
+
+  ```bash
+  ros2 launch mvsim_nav2_demos mvsim_vehs_launch.py namespace:=veh1
+  ros2 launch mvsim_nav2_demos mvsim_vehs_launch.py namespace:=veh2
+  ros2 launch mvsim_nav2_demos mvsim_vehs_launch.py namespace:=veh3
+  ros2 launch mvsim_nav2_demos mvsim_vehs_launch.py namespace:=veh4
+  ros2 launch mvsim_nav2_demos mvsim_vehs_launch.py namespace:=veh5
+  ros2 launch mvsim_nav2_demos mvsim_vehs_launch.py namespace:=veh6
+  ```
+
+#### How to send a goal
+
+- Send goal via rviz:
+
+  - Click the ***Nav2 Goal*** button on the top bar.
+  - Click and hold on the goal position and drag to the goal heading.
+  - Then you can see a green arrow and that is the goal (2d position and heading).
+
+- Send goal action via terminal:
+
+  - The namespace is ***veh1*** and ***-f*** option is for the feedback information
+
+  ```bash
+  ros2 action send_goal /veh1/navigate_to_pose nav2_msgs/action/NavigateToPose \
+  "{pose: {header: {stamp: {sec: 0, nanosec: 0}, frame_id: \"map\"}, \
+  pose: {position: {x: 1.0, y: 0.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}, \
+  behavior_tree: \"\"}" -f
+  ```
+
+- Send goal poses action via terminal:
+
+  - The namespace is ***veh1*** and ***-f*** option is for the feedback information
+
+  ```bash
+  ros2 action send_goal /veh1/navigate_through_poses nav2_msgs/action/NavigateThroughPoses \
+    "{poses: [
+    {header: {stamp: {sec: 0, nanosec: 0}, frame_id: \"map\"}, \
+    pose: {position: {x: 1.0, y: 0.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}, \
+    {header: {stamp: {sec: 0, nanosec: 0}, frame_id: \"map\"}, \
+    pose: {position: {x: 3.0, y: 0.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}, \
+    {header: {stamp: {sec: 0, nanosec: 0}, frame_id: \"map\"}, \
+    pose: {position: {x: 5.0, y: 1.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}, \
+    ], \
+    behavior_tree: \"\"}" -f
+  ```
 
 ## Launch alternative with mrpt_navigation
 
 - Clone into "src" https://github.com/mrpt-ros-pkg/mrpt_navigation.git
 - Build:
-```bash
-colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
-. install/setup.bash
-```
+
+  ```bash
+  colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
+  . install/setup.bash
+  ```
 
 - Launch:
 
-```bash
-ros2 launch mrpt_tutorials demo_reactive_nav_mvsim.launch.py  world_file:=$(pwd)/src/mvsim_nav2_demos/launch/demo_greenhouse_jjaa.world.xml
-```
+  ```bash
+  ros2 launch mrpt_tutorials demo_reactive_nav_mvsim.launch.py  world_file:=$(pwd)/src/mvsim_nav2_demos/launch/demo_greenhouse_jjaa.world.xml
+  ```

launch/camera.sensor.xml → definitions/camera.sensor.xml

@@ -17,7 +17,7 @@
     <clip_max>${clip_max|1e+4}</clip_max>
 
     <visual>
-      <model_uri>simple_camera.dae</model_uri>
+      <model_uri>../models/simple_camera.dae</model_uri>
       <model_scale>${sensor_visual_scale|1.0}</model_scale>
     </visual>
 

definitions/helios-32-FOV-31.sensor.xml

@@ -0,0 +1,40 @@
+<sensor class="lidar3d" name="${sensor_name|lidar1}">
+    <pose_3d> ${sensor_x|0.5}  ${sensor_y|0.0}  ${sensor_z|0.7}  ${sensor_yaw|0.0} ${sensor_pitch|0.0} ${sensor_roll|0.0}</pose_3d>
+
+    <!-- vert_fov_degrees: If defined, a symmetric vertical FOV is used.
+         The alternative is using a custom list of angles in "vertical_ray_angles"
+    -->
+    <!-- <vert_fov_degrees>${vert_fov_degrees|70}</vert_fov_degrees> -->
+
+    <vert_nrays>32</vert_nrays>
+    <vertical_ray_angles>15 14 13 12 11 10 9 8 7 6 5 4 3 2 1 0 -1 -2 -3 -4 -5 -6 -7 -8 -9 -10 -11 -12 -13 -14 -15 -16</vertical_ray_angles>
+
+    <!-- Horizontal / azimuth angular resolution:
+        The rotation of the Helios sensor configurable: 5 / 10 / 20 Hz
+        Firing timing of the sensor is fixed at 55.296 μs (=18.084 kHz).
+
+        Set the "sensor_rate" (Hz) variable from the parent XML to automatically
+        adjust the number of points per horizontal line.
+    -->
+    <sensor_period>$f{1.0/${sensor_rate|10}}</sensor_period>
+    <horz_nrays>$f{(1.0/${sensor_rate|10})/55.296e-6}</horz_nrays>
+
+    <!-- 1.0=minimum (faster), larger values=potentially finer details captured -->
+    <horz_resolution_factor>${horz_resolution_factor|1.0}</horz_resolution_factor>
+    <vert_resolution_factor>${vert_resolution_factor|1.0}</vert_resolution_factor>
+
+    <!-- Depth ratio (0 to 1) between adjacent depth image to allow linear interpolation of ranges -->
+    <max_vert_relative_depth_to_interpolatate>${max_vert_relative_depth_to_interpolatate|0.3}</max_vert_relative_depth_to_interpolatate>
+    <max_horz_relative_depth_to_interpolatate>${max_horz_relative_depth_to_interpolatate|0.1}</max_horz_relative_depth_to_interpolatate>
+
+    <range_std_noise>${sensor_std_noise|0.005}</range_std_noise>
+    <min_range>${min_range|0.20}</min_range>
+    <max_range>${max_range|110.0}</max_range>
+
+    <visual> <model_uri>../models/velodyne-vlp16.dae</model_uri> <model_roll>90</model_roll> </visual>
+
+    <!-- Publish sensor on MVSIM ZMQ topic? (Note, this is **not** related to ROS at all) -->
+    <publish enabled="${sensor_publish|false}">
+        <publish_topic>/${PARENT_NAME}/${NAME}</publish_topic>
+    </publish>
+</sensor>

definitions/jackal.vehicle.xml

@@ -0,0 +1,62 @@
+<!--
+  Vehicle class definition file.
+  Intended to be included into world XML files.
+  Common sensors could be included here, but they are left to be included in
+  class instantiations.
+  Refer to mvsim_tutorial example files.
+-->
+<vehicle:class name="jackal">
+
+    <!--  Dynamical model -->
+    <dynamics class="differential_4_wheels">
+        <!-- Params -->
+        <lf_wheel pos="0.13  0.16" mass="1.0" width="0.03" diameter="0.20">
+          <visual> <model_uri>../models/jackal_description/jackal-wheel.dae</model_uri> </visual>
+        </lf_wheel>
+        <rf_wheel pos="0.13 -0.16" mass="1.0" width="0.03" diameter="0.20">
+          <visual> <model_uri>../models/jackal_description/jackal-wheel.dae</model_uri> </visual>
+        </rf_wheel>
+        <lr_wheel pos="-0.13  0.16" mass="1.0" width="0.03" diameter="0.20">
+          <visual> <model_uri>../models/jackal_description/jackal-wheel.dae</model_uri> </visual>
+        </lr_wheel>
+        <rr_wheel pos="-0.13 -0.16" mass="1.0" width="0.03" diameter="0.20">
+          <visual> <model_uri>../models/jackal_description/jackal-wheel.dae</model_uri> </visual>
+        </rr_wheel>
+
+        <chassis mass="10.0" zmin="0.05" zmax="0.25">
+            <shape_from_visual/> <!-- automatic shape,zmin,zmax from 3D mesh-->
+        </chassis>
+
+        <!--   Motor controller -->
+        <controller class="twist_ideal" />
+
+    </dynamics>
+
+    <!-- Friction force simulation -->
+    <friction class="default">
+        <mu>0.8</mu>
+        <C_damping>0.05</C_damping>
+    </friction>
+
+    <!--  Custom visualization model -->
+    <!-- 3D model filename to load (local or remote http://uri ) -->
+    <visual>
+        <model_uri>../models/jackal_description/jackal.dae</model_uri>
+        <model_yaw>0.0</model_yaw>
+        <model_pitch>0.0</model_pitch>
+        <model_roll>90.0</model_roll>
+    </visual>
+
+
+    <if condition="${default_sensors|true}">
+      <!-- Sensors -->
+      <include file="lidar2d.sensor.xml"
+        sensor_x="-0.15" sensor_z="0.25" sensor_yaw="180"
+        sensor_period_sec="0.10"
+        raytrace_3d="false"
+        sensor_name="scanner1"
+        fov_degrees="360"
+      />
+    </if>
+
+</vehicle:class>

definitions/lidar2d.sensor.xml

@@ -0,0 +1,28 @@
+<sensor class="laser" name="${sensor_name|laser1}">
+    <pose_3d> ${sensor_x|1.80}  ${sensor_y|0.0}  ${sensor_z|0.7}  ${sensor_yaw|0.0} 0.0 0.0</pose_3d>
+    <fov_degrees>${fov_degrees|180}</fov_degrees>
+    <sensor_period>${sensor_period_sec|0.05}</sensor_period>
+    <nrays>${sensor_nrays|181}</nrays>
+    <range_std_noise>${sensor_std_noise|0.01}</range_std_noise>
+    <angle_std_noise_deg>${sensor_std_noise_deg|0.01}</angle_std_noise_deg>
+    <raytrace_3d>${raytrace_3d|false}</raytrace_3d>
+    <max_range>${max_range|30.0}</max_range>
+
+    <visual enabled="${sensor_custom_visual|true}">
+        <model_uri>../models/hokuyo_utm30.dae</model_uri>
+    </visual>
+
+    <!-- Publish sensor on MVSIM ZMQ topic? (Note, this is **not** related to ROS at all) -->
+    <publish enabled="${sensor_publish|false}">
+        <publish_topic>/${PARENT_NAME}/${NAME}</publish_topic>
+    </publish>
+
+    <!-- Visualization of sensed points in MVSIM GUI -->
+    <viz_visiblePoints>${viz_visiblePoints|true}</viz_visiblePoints>
+    <viz_pointSize>${viz_pointSize|3.0}</viz_pointSize>
+    <viz_visiblePlane>${viz_visiblePlane|false}</viz_visiblePlane>
+    <viz_visibleLines>${viz_visibleLines|true}</viz_visibleLines>
+    <viz_pointsColor>${viz_pointsColor|#ff000080}</viz_pointsColor>
+    <viz_planeColor>${viz_planeColor|#0000ff10}</viz_planeColor>
+
+</sensor>

launch/velodyne-vlp16.sensor.xml → definitions/velodyne-vlp16.sensor.xml

@@ -26,8 +26,8 @@
 
     <range_std_noise>${sensor_std_noise|0.005}</range_std_noise>
     <max_range>${max_range|80.0}</max_range>
-    
-    <visual> <model_uri>velodyne-vlp16.dae</model_uri> <model_roll>90</model_roll> </visual>
+
+    <visual> <model_uri>../models/velodyne-vlp16.dae</model_uri> <model_roll>90</model_roll> </visual>
 
     <!-- Publish sensor on MVSIM ZMQ topic? (Note, this is **not** related to ROS at all) -->
     <publish enabled="${sensor_publish|false}">

launch/demo_greenhouse_jjaa.world.xml

@@ -27,17 +27,17 @@
 
 	<variable name="NUM_TILTED_BEAMS_X" value="21"></variable>
 	<variable name="NUM_TILTED_BEAMS_Y" value="10"></variable>
-	
+
 	<variable name="NUM_CORRIDORS_X" value="10"></variable>
-	
+
 	<variable name="NUM_PLANTS_CORRIDOR_SOUTH" value="4"></variable>
 	<variable name="NUM_PLANTS_CORRIDOR_NORTH" value="7"></variable>
 
 	<variable name="NUM_POLES_X" value="10"></variable>
 	<variable name="NUM_POLES_Y" value="10"></variable>
 
 	<variable name="PLANTS_RAND_XY" value="0.35"></variable>
-	
+
 	<variable name="CEILING_HEIGHT" value="3.0"></variable>
 
 	<!-- ========================
@@ -107,7 +107,7 @@
 	<!-- =============================
 		   Vehicle classes definition
 	     ============================= -->
-	<include file="agricobiot2.vehicle.xml" />
+	<include file="../definitions/agricobiot2.vehicle.xml" />
 
 	<!-- ========================
 		   Vehicle(s) definition
@@ -142,16 +142,16 @@
 
 		<include file="rplidar-a2.sensor.xml"
 			raytrace_3d="true"
-			sensor_x="0.0" sensor_z="1.15" sensor_yaw="0" sensor_name="laser1" 
+			sensor_x="0.0" sensor_z="1.15" sensor_yaw="0" sensor_name="laser1"
 			sensor_hz="10.0"
 		/>
 
 <!--
-		<include file="camera.sensor.xml" 
+		<include file="camera.sensor.xml"
 		  sensor_x="0.9"  sensor_y="0" sensor_z="0.75"
 		  sensor_period_sec="0.20"
 		  sensor_name="cam1"
-		  />		
+		  />
 -->
 	</vehicle>
 
@@ -206,9 +206,9 @@
 	     ====================================== -->
 	<!-- You can assign an optional custom name to each object, as an attribute to block, like name="shelf_001", etc. -->
 
-	<!-- All coordinates are global coords: 
+	<!-- All coordinates are global coords:
 	     SE(2) <init_pose>x y yaw(deg)</init_pose> or
-	     SE(3) <init_pose3d>x y z yaw(deg) pitch(deg) roll(deg)</init_pose3d> 
+	     SE(3) <init_pose3d>x y z yaw(deg) pitch(deg) roll(deg)</init_pose3d>
 	-->
 
 	<!-- H-beams along the X axis -->