Adds joint states info to complete tf tree. (#40)

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

README.md

@@ -53,7 +53,13 @@ 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 via:
+If you'd like to work from ROS you can launch the ros bridge by adding the corresponding flag
+
+  ```sh
+  ros2 launch andino_gz andino_gz.launch.py ros_bridge:=true
+  ```
+
+(Optional) Or launching it separately via:
 
   ```sh
   ros2 launch andino_gz gz_ros_bridge.launch.py

andino_gz/config/bridge_config.yaml

@@ -13,6 +13,11 @@
   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_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_type_name: "sensor_msgs/msg/CameraInfo"

andino_gz/launch/andino_gz.launch.py

@@ -14,6 +14,8 @@
 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.')
     jsp_gui_arg = DeclareLaunchArgument('jsp_gui', default_value='false', description='Run joint state publisher gui node.')
     world_name_arg = DeclareLaunchArgument('world_name', default_value='depot.sdf', description='Name of the world to load.')
@@ -50,6 +52,7 @@ def generate_launch_description():
         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'))
     )
 
@@ -62,13 +65,23 @@ def generate_launch_description():
         condition=IfCondition(LaunchConfiguration('jsp_gui'))
     )
 
+    # 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'))
+    )
+
     return LaunchDescription(
         [
             # Arguments and Nodes
+            ros_bridge_arg,
             jsp_gui_arg,
             rviz_arg,
             world_name_arg,
             gazebo,
+            ros_bridge,
             spawn_robot_and_rsp,
             jsp_gui,
             rviz,

andino_gz/urdf/andino_gz.urdf.xacro

@@ -17,6 +17,17 @@
     </plugin>
   </gazebo>
 
+  <gazebo>
+    <plugin
+      filename="ignition-gazebo-joint-state-publisher-system"
+      name="ignition::gazebo::systems::JointStatePublisher">
+      <joint_name>left_wheel_joint</joint_name>
+      <joint_name>right_wheel_joint</joint_name>
+      <joint_name>caster_wheel_joint</joint_name>
+      <joint_name>caster_rotation_joint</joint_name>
+    </plugin>
+  </gazebo>
+
   <gazebo reference="rplidar_laser_link">
     <sensor name="sensor_ray_front" type="gpu_lidar">
       <ray>