Merge pull request #698 from osrf/jessica/move_tf_to_ros

move tf to ros
osrf · Jul 26, 2023 · 76690b6 · 76690b6
2 parents bfc5c65 + 0aed330
commit 76690b6
Show file tree

Hide file tree

Showing 11 changed files with 832 additions and 7 deletions.
diff --git a/vrx_gz/src/vrx_gz/bridges.py b/vrx_gz/src/vrx_gz/bridges.py
@@ -38,6 +38,14 @@ def pose_static(model_name):
         ros_type='tf2_msgs/msg/TFMessage',
         direction=BridgeDirection.GZ_TO_ROS)
 
+def joint_states(world_name, model_name):
+    return Bridge(
+        gz_topic=f'/world/{world_name}/model/{model_name}/joint_state',
+        ros_topic='joint_states',
+        gz_type='ignition.msgs.Model',
+        ros_type='sensor_msgs/msg/JointState',
+        direction=BridgeDirection.GZ_TO_ROS)
+
 def cmd_vel(model_name):
     return Bridge(
         gz_topic=f'/model/{model_name}/cmd_vel',

diff --git a/vrx_gz/src/vrx_gz/launch.py b/vrx_gz/src/vrx_gz/launch.py
@@ -19,7 +19,9 @@
 from launch.actions import RegisterEventHandler
 from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
 from launch.actions import ExecuteProcess, EmitEvent
 from launch.events import Shutdown
 
@@ -202,6 +204,11 @@ def competition_bridges(world_name):
 def spawn(sim_mode, world_name, models, robot=None):
     if type(models) != list:
         models = [models]
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='true',
+        description='Use simulation (Gazebo) clock if true'),
 
     launch_processes = []
     for model in models:
@@ -237,13 +244,25 @@ def spawn(sim_mode, world_name, models, robot=None):
                 remappings=[bridge.remapping() for bridge in bridges],
             ))
 
-            # tf broadcaster
+            # tf broadcaster (sensors)
             nodes.append(Node(
                 package='vrx_ros',
                 executable='pose_tf_broadcaster',
                 output='screen',
             ))
 
+            # robot_state_publisher (tf for wamv)
+            model_dir = os.path.join(get_package_share_directory('vrx_gazebo'), 'models/wamv/tmp')
+            urdf_file = os.path.join(model_dir, 'model.urdf')
+            with open(urdf_file, 'r') as infp:
+                robot_desc = infp.read()
+            params = {'use_sim_time': use_sim_time, 'frame_prefix': 'wamv/', 'robot_description': robot_desc}
+            nodes.append(Node(package='robot_state_publisher',
+                                  executable='robot_state_publisher',
+                                  output='both',
+                                  parameters=[params],
+                                  remappings=[('/joint_states', '/wamv/joint_states')]))
+
             group_action = GroupAction([
                 PushRosNamespace(model.model_name),
                 *nodes

diff --git a/vrx_gz/src/vrx_gz/model.py b/vrx_gz/src/vrx_gz/model.py
@@ -70,6 +70,8 @@ def bridges(self, world_name):
             vrx_gz.bridges.pose(self.model_name),
             # pose static
             vrx_gz.bridges.pose_static(self.model_name),
+            # joint states
+            vrx_gz.bridges.joint_states(world_name, self.model_name),
             # comms tx
             # vrx_gz.bridges.comms_tx(self.model_name),
             # comms rx

diff --git a/vrx_urdf/vrx_gazebo/CMakeLists.txt b/vrx_urdf/vrx_gazebo/CMakeLists.txt
@@ -23,7 +23,6 @@ install(DIRECTORY config/
 install(PROGRAMS scripts/generate_wamv.py
   DESTINATION lib/${PROJECT_NAME})
 
-
 ament_python_install_package(
   vrx_gazebo
   PACKAGE_DIR src/vrx_gazebo