From b46122b6b6ad778f96f6b659b39cba4e54f15957 Mon Sep 17 00:00:00 2001
From: Franco Cipollone <franco.c@ekumenlabs.com>
Date: Wed, 24 Apr 2024 23:32:04 -0300
Subject: [PATCH] Adds joint states info to complete tf tree.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
---
 README.md                            |  8 +++++++-
 andino_gz/config/bridge_config.yaml  |  5 +++++
 andino_gz/launch/andino_gz.launch.py | 13 +++++++++++++
 andino_gz/urdf/andino_gz.urdf.xacro  | 11 +++++++++++
 4 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 95f4464..239a4f5 100644
--- a/README.md
+++ b/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
diff --git a/andino_gz/config/bridge_config.yaml b/andino_gz/config/bridge_config.yaml
index c845568..16fd601 100644
--- a/andino_gz/config/bridge_config.yaml
+++ b/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"
diff --git a/andino_gz/launch/andino_gz.launch.py b/andino_gz/launch/andino_gz.launch.py
index 74220e8..30fdec4 100644
--- a/andino_gz/launch/andino_gz.launch.py
+++ b/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,
diff --git a/andino_gz/urdf/andino_gz.urdf.xacro b/andino_gz/urdf/andino_gz.urdf.xacro
index 10e153f..b1a4d29 100644
--- a/andino_gz/urdf/andino_gz.urdf.xacro
+++ b/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>