New features: you can add several rigid bodies, several markers and create new rigid_bodies

README.md

@@ -7,7 +7,7 @@ This package provides you with a gazebo plugin that allows you to simulate the u
 
 Add in your robot model the following, as done in [This sample model](https://github.com/MOCAP4ROS2-Project/mocap4ros2_gazebo/blob/main/models/waffle.model):
 
-```
+```xml
     <link name="base_mocap">
         <pose>"0 0 0.577 0 0 0</pose>
         <inertial>
@@ -33,22 +33,37 @@ Add in your robot model the following, as done in [This sample model](https://gi
     <gazebo>
     <plugin name="gazebo_ros_mocap" filename="libgazebo_ros_mocap.so">
       <model_name>robot</model_name>
-      <link_name>base_footprint</link_name>
+      <rigid_link>base_footprint</rigid_link>
     </plugin>
     </gazebo>
 ```
 
+If what you want is to add markers, you can do so by adding the following line in your plugin. For this case, we would add a marker in base_footprint:
+```xml
+<plugin name="gazebo_ros_mocap" filename="libgazebo_ros_mocap.so">
+    <model_name>robot</model_name>
+    <marker_link>base_link</marker_link>
+</plugin>
+ ```
+
+And if you have 3 or more markers within your environment, you can create new rigid_bodies from them. To do this, you have to choose the orientation through a link and add the index of the markers you want to add.
+```bash
+ros2 service call /create_rigid_body mocap4r2_msgs/srv/CreateRigidBody 'rigid_body_name: '\'new_rigid''\''
+link_parent: '\'base_link''\''
+markers: [1, 2, 5, 8]'
+```
+
 ## Run the sample
 
 To see it in action, just type:
 
-```
+```bash
 ros2 launch gazebo_mocap4r2_plugin tb3_simulation_launch.py
 ```
 
 Open gzclient in other terminal:
 
-```
+```bash
 gzclient
 ```
 

models/waffle.model

@@ -111,25 +111,25 @@
           <mass>0.125</mass>
         </inertial>
 
-        <collision name="mocap_sensor_collision">
+        <!-- <collision name="mocap_sensor_collision">
           <pose>-0.052 0 0.160 0 0 0</pose>
           <geometry>
             <cylinder>
               <radius>0.0508</radius>
               <length>0.055</length>
             </cylinder>
           </geometry>
-        </collision>
+        </collision> -->
 
-        <visual name="mocap_sensor_visual">
+        <!-- <visual name="mocap_sensor_visual">
           <pose>-0.064 0 0.141 0 0 0</pose>
           <geometry>
             <mesh>
               <uri>model://gazebo_mocap_plugin/meshes/rigid_body.dae</uri>
               <scale>0.001 0.001 0.001</scale>
             </mesh>
           </geometry>
-        </visual>
+        </visual> -->
       </link>
 
       <link name="base_scan">
@@ -557,13 +557,10 @@
       </plugin>
 
       <plugin name="gazebo_ros_mocap" filename="libgazebo_ros_mocap.so">
-        <link_name>base_mocap</link_name>
-        <!--ros>
-            <remapping>~/out:=joint_states</remapping>
-        </ros>
-        <update_rate>30</update_rate>
-        <joint_name>wheel_left_joint</joint_name>
-        <joint_name>wheel_right_joint</joint_name-->
+        <rigid_link>base_link</rigid_link>
+        <rigid_link>base_mocap</rigid_link>
+        <marker_link>camera_link</marker_link>
+        <rigid_link>base_footprint</rigid_link>
       </plugin>
     </model>
 </sdf>