Framework to view the buoy on RViz

buoy_description/hooks/buoy_description.dsv.in

@@ -1 +1 @@
-prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share/@PROJECT_NAME@/models
+prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share

buoy_description/models/mbari_wec/model.sdf.em

@@ -92,10 +92,10 @@ pto_scale = pto_inner_radius / pto_stl_inner_radius
           <izz>1476</izz>
         </inertia>
       </inertial>
-      <visual name="visual">
+      <visual name="visual_Buoy">
         <geometry>
           <mesh>
-            <uri>meshes/buoy_float.stl</uri>
+            <uri>package://buoy_description/models/mbari_wec/meshes/buoy_float.stl</uri>
           </mesh>
         </geometry>
         <!--color-->
@@ -140,10 +140,10 @@ pto_scale = pto_inner_radius / pto_stl_inner_radius
           <izz>7.28</izz>
         </inertia>
       </inertial>
-      <visual name="visual">
+      <visual name="visual_PTO">
         <geometry>
           <mesh>
-            <uri>meshes/pto.stl</uri>
+            <uri>package://buoy_description/models/mbari_wec/meshes/pto.stl</uri>
           </mesh>
         </geometry>
         <material>
@@ -155,7 +155,7 @@ pto_scale = pto_inner_radius / pto_stl_inner_radius
       <collision name="collision">
         <geometry>
           <mesh>
-            <uri>meshes/pto_collision.stl</uri>
+            <uri>package://buoy_description/models/mbari_wec/meshes/pto_collision.stl</uri>
             <scale>@(pto_scale) @(pto_scale) 1.0</scale>
           </mesh>
         </geometry>
@@ -189,10 +189,10 @@ pto_scale = pto_inner_radius / pto_stl_inner_radius
           <izz>0.0216</izz>
         </inertia>
       </inertial>
-      <visual name="visual">
+      <visual name="visual_Piston">
         <geometry>
           <mesh>
-            <uri>meshes/rod_and_piston.stl</uri>
+            <uri>package://buoy_description/models/mbari_wec/meshes/rod_and_piston.stl</uri>
           </mesh>
         </geometry>
         <!--color-->
@@ -219,7 +219,7 @@ pto_scale = pto_inner_radius / pto_stl_inner_radius
           <izz>@(tether_top_link_mm.izz())</izz>
         </inertia>
       </inertial>
-      <visual name="visual">
+      <visual name="visual_tether_top_@(link_index)">
         <geometry>
           <cylinder>
             <radius>@(tether_radius)</radius>
@@ -286,7 +286,7 @@ pto_scale = pto_inner_radius / pto_stl_inner_radius
           <izz>@(tether_bottom_link_mm.izz())</izz>
         </inertia>
       </inertial>
-      <visual name="visual">
+      <visual name="visual_tether_bottom_@(link_index)">
         <geometry>
           <cylinder>
             <radius>@(tether_radius)</radius>
@@ -356,10 +356,10 @@ pto_scale = pto_inner_radius / pto_stl_inner_radius
           <izz>613.52</izz>
         </inertia>
       </inertial>
-      <visual name="visual">
+      <visual name="visual_HeaveCone">
         <geometry>
           <mesh>
-            <uri>meshes/heave_cone.stl</uri>
+            <uri>package://buoy_description/models/mbari_wec/meshes/heave_cone.stl</uri>
           </mesh>
         </geometry>
         <!--color-->
@@ -394,10 +394,10 @@ pto_scale = pto_inner_radius / pto_stl_inner_radius
           <izz>19.9</izz>
         </inertia>
       </inertial>
-      <visual name="visual">
+      <visual name="visual_Trefoil">
         <geometry>
           <mesh>
-            <uri>meshes/trefoil.stl</uri>
+            <uri>package://buoy_description/models/mbari_wec/meshes/trefoil.stl</uri>
           </mesh>
         </geometry>
         <!--color-->

buoy_gazebo/CMakeLists.txt

@@ -86,6 +86,7 @@ install(DIRECTORY
   worlds
   launch
   gazebo
+  rviz
   DESTINATION share/${PROJECT_NAME}/
 )
 

buoy_gazebo/launch/mbari_wec.launch.py

@@ -18,33 +18,113 @@
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.actions import IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 
 
 def generate_launch_description():
 
     pkg_ros_ign_gazebo = get_package_share_directory('ros_ign_gazebo')
     pkg_buoy_gazebo = get_package_share_directory('buoy_gazebo')
+    pkg_buoy_description = get_package_share_directory('buoy_description')
+    model_dir = 'mbari_wec'
+    model_name = 'MBARI_WEC'
+    sdf_file = os.path.join(pkg_buoy_description, 'models', model_dir, 'model.sdf')
+
+    with open(sdf_file, 'r') as infp:
+        robot_desc = infp.read()
+
+    gazebo_world_file_launch_arg = DeclareLaunchArgument(
+        'world_file', default_value=['mbari_wec.sdf'],
+        description='Gazebo world filename.sdf'
+    )
+
+    gazebo_world_name_launch_arg = DeclareLaunchArgument(
+        'world_name', default_value=['world_demo'],
+        description='Gazebo <world name>'
+    )
+
+    rviz_launch_arg = DeclareLaunchArgument(
+        'rviz', default_value='false',
+        description='Open RViz.'
+    )
 
     gazebo = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py'),
         ),
+        launch_arguments={'ign_args': PathJoinSubstitution([
+            pkg_buoy_gazebo,
+            'worlds',
+            LaunchConfiguration('world_file')
+        ])}.items(),
     )
 
-    bridge = Node(package='ros_ign_bridge',
-                  executable='parameter_bridge',
-                  arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'],
-                  output='screen')
+    # Bridge to forward tf and joint states to ros2
+    link_pose_gz_topic = '/model/' + model_name + '/pose'
+    bridge = Node(
+        package='ros_ign_bridge',
+        executable='parameter_bridge',
+        arguments=[
+            # Clock (Gazebo -> ROS2)
+            '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
+            # Joint states (Gazebo -> ROS2)
+            ['/world/', LaunchConfiguration('world_name'), '/model/', model_name, '/joint_state',
+             '@', 'sensor_msgs/msg/JointState', '[', 'ignition.msgs.Model'],
+            # Link poses (Gazebo -> ROS2)
+            link_pose_gz_topic + '@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V',
+            link_pose_gz_topic + '_static@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V',
+        ],
+        remappings=[
+            (['/world/', LaunchConfiguration('world_name'), '/model/', model_name, '/joint_state'],
+                'joint_states'),
+            (link_pose_gz_topic, '/tf'),
+            (link_pose_gz_topic + '_static', '/tf_static'),
+        ],
+        output='screen'
+    )
+
+    # Mapping initial frame
+    tf_buoy = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        arguments=['0', '0', '0', '0', '0', '0', 'MBARI_WEC/Buoy', 'MBARI_WEC'],
+        condition=IfCondition(LaunchConfiguration('rviz')),
+    )
+
+    # Get the parser plugin convert sdf to urdf using robot_description topic
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='both',
+        parameters=[
+            {'use_sim_time': True},
+            {'robot_description': robot_desc},
+        ]
+    )
+
+    # Launch rviz
+    rviz = Node(
+        package='rviz2',
+        executable='rviz2',
+        arguments=['-d', os.path.join(pkg_buoy_gazebo, 'rviz', 'mbari_wec.rviz')],
+        condition=IfCondition(LaunchConfiguration('rviz')),
+        parameters=[
+            {'use_sim_time': True},
+        ]
+    )
 
     return LaunchDescription([
-        DeclareLaunchArgument(
-          'ign_args',
-          default_value=[os.path.join(pkg_buoy_gazebo, 'worlds', 'mbari_wec.sdf'), ''],
-          description='Ignition Gazebo arguments'),
+        gazebo_world_file_launch_arg,
+        gazebo_world_name_launch_arg,
+        rviz_launch_arg,
         gazebo,
-        bridge
+        bridge,
+        tf_buoy,
+        robot_state_publisher,
+        rviz
     ])

buoy_gazebo/package.xml

@@ -14,7 +14,10 @@
   <depend>buoy_examples</depend>
   <depend>buoy_msgs</depend>
   <depend>ignition-gazebo6</depend>
+  <depend>robot_state_publisher</depend>
   <depend>ros_ign_gazebo</depend>
+  <depend>rviz2</depend>
+  <depend>sdformat_urdf</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

buoy_gazebo/rviz/mbari_wec.rviz

@@ -0,0 +1,159 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 85
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+        - /RobotModel1/Description Topic1
+        - /RobotModel1/Links1
+      Splitter Ratio: 0.5
+    Tree Height: 1566
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: true
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+      Name: RobotModel
+      TF Prefix: MBARI_WEC
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: MBARI_WEC/odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 11.199999809265137
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.785398006439209
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398006439209
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 1883
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000015a000006b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006b00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000002d000000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000044000006b4000000eb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000006b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000044000006b4000000b900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004680000003efc0100000002fb0000000800540069006d0065010000000000000468000002c500fffffffb0000000800540069006d0065010000000000000450000000000000000000000468000006b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1128
+  X: 1992
+  Y: 0