Visualizing convex and surface

CMakeLists.txt

@@ -13,6 +13,9 @@ project(
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(visualization_msgs REQUIRED)
 find_package(mc_rtc REQUIRED)
 
 include_directories(include ${ament_INCLUDE_DIRS})
@@ -39,6 +42,20 @@ install(
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME})
 
-install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+add_executable(mc_convex_visualization src/mc_convex_visualization.cpp)
+target_link_libraries(mc_convex_visualization mc_rtc::mc_control)
+ament_target_dependencies(mc_convex_visualization rclcpp std_msgs tf2_ros
+                          visualization_msgs)
+
+add_executable(mc_surface_visualization src/mc_surface_visualization.cpp)
+target_link_libraries(mc_surface_visualization mc_rtc::mc_control)
+ament_target_dependencies(mc_surface_visualization rclcpp std_msgs tf2_ros
+                          visualization_msgs)
+
+install(
+  TARGETS mc_convex_visualization mc_surface_visualization
+  RUNTIME DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME})
 
 ament_package()

README.md

@@ -28,6 +28,34 @@ Where:
 - `publish_to` is the topic where the controller is subscribed to a control message (defaults to: `/command`)
 - `subscribe_to` is the topic where the robot is publishing its state through a `sensor_msgs/msg/JointState` message (defaults to: `/joint_state`)
 
+Visualization
+--
+
+### Convex visualization
+
+Display the collision convex shapes of a robot in RViz2:
+
+```bash
+ros2 launch mc_rtc_ros_control convex_visualization.launch.py
+ros2 launch mc_rtc_ros_control convex_visualization.launch.py robot:=JVRC1 frame_id:=world
+```
+
+Where:
+
+- `robot` is the mc_rtc robot module name (defaults to: `JVRC1`)
+- `frame_id` is the TF reference frame for the markers (defaults to: `map`)
+
+### Surface visualization
+
+Display the contact surfaces of a robot alongside its mesh in RViz2:
+
+```bash
+ros2 launch mc_rtc_ros_control surface_visualization.launch.py
+ros2 launch mc_rtc_ros_control surface_visualization.launch.py robot:=JVRC1 frame_id:=world
+```
+
+Parameters are the same as above. This node also publishes the robot URDF on `/robot_description` and static TF for all bodies so that the robot model is rendered in RViz2.
+
 Example
 --
 

launch/convex_visualization.launch.py

@@ -0,0 +1,60 @@
+# usr/bin/env python3
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+
+
+def launch_setup(context, *args, **kwargs):
+    robot = context.launch_configurations['robot']
+    frame_id = context.launch_configurations['frame_id']
+
+    pkg_share = get_package_share_directory('mc_rtc_ros_control')
+    rviz_config = os.path.join(pkg_share, 'rviz', 'convex_visualization.rviz')
+
+    convex_node = Node(
+        package='mc_rtc_ros_control',
+        executable='mc_convex_visualization',
+        name='mc_convex_visualization',
+        output='screen',
+        parameters=[
+            {
+                'robot': robot,
+                'frame_id': frame_id,
+            }
+        ],
+    )
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', rviz_config],
+    )
+
+    return [convex_node, rviz_node]
+
+
+def generate_launch_description():
+
+    robot = DeclareLaunchArgument(
+        'robot',
+        default_value='JVRC1',
+        description='Robot module name'
+    )
+
+    frame_id = DeclareLaunchArgument(
+        'frame_id',
+        default_value='map',
+        description='TF frame for markers'
+    )
+
+    declare_arguments = [robot, frame_id]
+
+    return LaunchDescription(declare_arguments + [
+        OpaqueFunction(function=launch_setup),
+    ])

launch/surface_visualization.launch.py

@@ -0,0 +1,60 @@
+# usr/bin/env python3
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+
+
+def launch_setup(context, *args, **kwargs):
+    robot = context.launch_configurations['robot']
+    frame_id = context.launch_configurations['frame_id']
+
+    pkg_share = get_package_share_directory('mc_rtc_ros_control')
+    rviz_config = os.path.join(pkg_share, 'rviz', 'surface_visualization.rviz')
+
+    surface_node = Node(
+        package='mc_rtc_ros_control',
+        executable='mc_surface_visualization',
+        name='mc_surface_visualization',
+        output='screen',
+        parameters=[
+            {
+                'robot': robot,
+                'frame_id': frame_id,
+            }
+        ],
+    )
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', rviz_config],
+    )
+
+    return [surface_node, rviz_node]
+
+
+def generate_launch_description():
+
+    robot = DeclareLaunchArgument(
+        'robot',
+        default_value='JVRC1',
+        description='Robot module name'
+    )
+
+    frame_id = DeclareLaunchArgument(
+        'frame_id',
+        default_value='map',
+        description='TF frame for markers'
+    )
+
+    declare_arguments = [robot, frame_id]
+
+    return LaunchDescription(declare_arguments + [
+        OpaqueFunction(function=launch_setup),
+    ])

package.xml

@@ -12,10 +12,14 @@
 
   <build_depend>std_msgs</build_depend>
   <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>visualization_msgs</build_depend>
   <build_depend>xacro</build_depend>
 
   <run_depend>std_msgs</run_depend>
   <run_depend>sensor_msgs</run_depend>
+  <run_depend>tf2_ros</run_depend>
+  <run_depend>visualization_msgs</run_depend>
   <run_depend>xacro</run_depend>
 
   <export>

rviz/convex_visualization.rviz

@@ -0,0 +1,54 @@
+Panels:
+  - Class: rviz_common/Displays
+    Name: Displays
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Class: rviz_default_plugins/Grid
+      Name: Grid
+      Enabled: true
+      Value: true
+    - Class: rviz_default_plugins/RobotModel
+      Name: Robot Model
+      Enabled: true
+      Value: true
+      Description Source: Topic
+      Description Topic:
+        Value: /robot_description
+        Depth: 5
+        Durability Policy: Transient Local
+        Reliability Policy: Reliable
+      Alpha: 0.5
+      TF Prefix: ""
+    - Class: rviz_default_plugins/MarkerArray
+      Name: Convex Markers
+      Enabled: true
+      Value: true
+      Topic:
+        Value: /convex_markers
+        Depth: 5
+        Durability Policy: Transient Local
+        Reliability Policy: Reliable
+    - Class: rviz_default_plugins/TF
+      Name: TF
+      Enabled: false
+      Value: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Tools:
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/FocusCamera
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 3
+      Name: Current View
+      Pitch: 0.3
+      Yaw: 0.5
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0.5

rviz/surface_visualization.rviz

@@ -0,0 +1,54 @@
+Panels:
+  - Class: rviz_common/Displays
+    Name: Displays
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Class: rviz_default_plugins/Grid
+      Name: Grid
+      Enabled: true
+      Value: true
+    - Class: rviz_default_plugins/RobotModel
+      Name: Robot Model
+      Enabled: true
+      Value: true
+      Description Source: Topic
+      Description Topic:
+        Value: /robot_description
+        Depth: 5
+        Durability Policy: Transient Local
+        Reliability Policy: Reliable
+      Alpha: 0.5
+      TF Prefix: ""
+    - Class: rviz_default_plugins/MarkerArray
+      Name: Surface Markers
+      Enabled: true
+      Value: true
+      Topic:
+        Value: /surface_markers
+        Depth: 5
+        Durability Policy: Transient Local
+        Reliability Policy: Reliable
+    - Class: rviz_default_plugins/TF
+      Name: TF
+      Enabled: false
+      Value: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Tools:
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/FocusCamera
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 3
+      Name: Current View
+      Pitch: 0.3
+      Yaw: 0.5
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0.5