add ovis integration

README.md

@@ -46,6 +46,7 @@ Before installing the robot software, you need to install the simulator : https:
 git clone https://github.com/clubcapra/rove.git
 cd rove
 vcs import src < rove.repos
+echo "export GZ_VERSION=harmonic" >> ~/.bashrc && source ~/.bashrc
 sudo rosdep init
 rosdep update
 rosdep install --from-paths src --ignore-src -r -y

rove.repos

@@ -6,4 +6,8 @@ repositories:
   ovis_ros2:
     type: git
     url: https://github.com/clubcapra/ovis_ros2
-    version: new-env-with-arm
+    version: fix-gazebo-joy
+  gz_ros2_control:
+    type: git
+    url: https://github.com/clubcapra/gz_ros2_control.git
+    version: iron

src/rove_bringup/launch/sim.launch.py

@@ -3,109 +3,129 @@
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import IncludeLaunchDescription
-
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import Command
 from launch_ros.actions import Node
 from launch_ros.parameter_descriptions import ParameterValue
-from math import pi
 
 
 def generate_launch_description():
     # Get the launch directory
-    pkg_rove_bringup = get_package_share_directory('rove_bringup')
-    pkg_rove_description = get_package_share_directory('rove_description')
-    pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
+    pkg_rove_bringup = get_package_share_directory("rove_bringup")
+    pkg_rove_description = get_package_share_directory("rove_description")
+    pkg_ros_gz_sim = get_package_share_directory("ros_gz_sim")
+    pkg_ovis = get_package_share_directory("ovis_bringup")
 
     # Get the URDF file
-    urdf_path = os.path.join(pkg_rove_description, 'urdf', 'rove.urdf.xacro')
-    robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str)
+    urdf_path = os.path.join(pkg_rove_description, "urdf", "rove.urdf.xacro")
+    robot_desc = ParameterValue(Command(["xacro ", urdf_path]), value_type=str)
 
     # Get simulation file
-    world_file_name = 'worlds/base_world.world'
+    world_file_name = "worlds/base_world.world"
     world = os.path.join(pkg_rove_description, world_file_name)
 
     # Setup to launch the simulator and Gazebo world
     gz_sim = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
-        launch_arguments={'gz_args': "-v 4 -r " + world}.items(),
+            os.path.join(pkg_ros_gz_sim, "launch", "gz_sim.launch.py")
+        ),
+        launch_arguments={"gz_args": "-v 4 -r " + world}.items(),
     )
 
-    walls_file_path = os.path.join(pkg_rove_description, 'worlds', 'walls.sdf')
+    walls_file_path = os.path.join(pkg_rove_description, "worlds", "walls.sdf")
     spawn_walls = Node(
-        package='ros_gz_sim',
-        executable='create',
-        arguments=['-file', walls_file_path,
-                   '-name', 'walls',
-                   '-x', '0',
-                   '-y', '0',
-                   '-z', '0'],
-        output='screen',
+        package="ros_gz_sim",
+        executable="create",
+        arguments=[
+            "-file",
+            walls_file_path,
+            "-name",
+            "walls",
+            "-x",
+            "0",
+            "-y",
+            "0",
+            "-z",
+            "0",
+        ],
+        output="screen",
     )
 
-    actor_file_path = os.path.join(pkg_rove_description, 'worlds', 'actor.sdf')
+    actor_file_path = os.path.join(pkg_rove_description, "worlds", "actor.sdf")
     spawn_actor = Node(
-        package='ros_gz_sim',
-        executable='create',
-        arguments=['-file', actor_file_path,
-                   '-name', 'actor',
-                   '-topic', 'actor_pose',
-                   '-x', '0',
-                   '-y', '0',
-                   '-z', '0.1'],
-        output='screen',
+        package="ros_gz_sim",
+        executable="create",
+        arguments=[
+            "-file",
+            actor_file_path,
+            "-name",
+            "actor",
+            "-x",
+            "0",
+            "-y",
+            "0",
+            "-z",
+            "0.1",
+        ],
+        output="screen",
     )
 
-    yaw = -pi / 2
-
     # Spawn robot
     spawn_rove = Node(
-        package='ros_gz_sim',
-        executable='create',
+        package="ros_gz_sim",
+        executable="create",
         arguments=[
-            '-name', 'rove',
-            '-topic', 'robot_description',
-            '-x', '0',
-            '-y', '0',
-            '-z', '0.1',
-            '-Y', str(yaw),
+            "-name",
+            "rove",
+            "-topic",
+            "robot_description",
+            "-x",
+            "0",
+            "-y",
+            "0",
+            "-z",
+            "0.1",
         ],
-        output='screen',
+        output="screen",
     )
 
     # Takes the description and joint angles as inputs and publishes
     # the 3D poses of the robot links
     robot_state_publisher = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        output='both',
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="both",
         parameters=[
-            {'robot_description': robot_desc},
-            {"use_sim_time": True, }
-        ]
+            {"robot_description": robot_desc},
+            {
+                "use_sim_time": True,
+            },
+        ],
     )
 
     # fake human tracker
     human_tracker = Node(
-            package='rove_navigation',
-            executable='green_person_tracker',
-            name='green_person_tracker',
-            output='screen',
+        package="rove_navigation",
+        executable="green_person_tracker",
+        name="green_person_tracker",
+        output="screen",
     )
 
     # Bridge ROS topics and Gazebo messages for establishing communication
     bridge = Node(
-        package='ros_gz_bridge',
-        executable='parameter_bridge',
-        parameters=[{
-            'config_file': os.path.join(pkg_rove_description, 'config',
-                                        'default_bridge.yaml'),
-            'qos_overrides./tf_static.publisher.durability': 'transient_local',
-            "use_sim_time": True,
-        }],
-        output='screen'
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        parameters=[
+            {
+                "config_file": os.path.join(
+                    pkg_rove_description, "config", "default_bridge.yaml"
+                ),
+                "qos_overrides./tf_static.publisher.durability": "transient_local",
+                "use_sim_time": True,
+            }
+        ],
+        output="screen",
     )
 
     common = IncludeLaunchDescription(
@@ -117,13 +137,43 @@ def generate_launch_description():
         }.items(),
     )
 
-    return LaunchDescription([
+    static_tf_ovis = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        arguments=[
+            "0.25",
+            "0",
+            "0.25",
+            "0",
+            "0",
+            "3.14",
+            "base_link",
+            "ovis_base_link",
+        ],
+    )
+
+    ovis = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ovis, "launch", "sim.launch.py"),
+        ),
+        launch_arguments={
+            "with_rove": "true",
+            "with_joy": "false",
+            "ovis_base_origin": "0.25 0 0.25 0 0 3.14",
+        }.items(),
+    )
+
+    return LaunchDescription(
+        [
             gz_sim,
             bridge,
             robot_state_publisher,
             spawn_walls,
             spawn_actor,
             spawn_rove,
             common,
-            human_tracker,
-            ])
+            # human_tracker,
+            ovis,
+            static_tf_ovis,
+        ]
+    )

src/rove_description/config/basic.rviz

@@ -5,6 +5,7 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /DepthCloud1/Auto Size1
+        - /RobotModel2/Status1
       Splitter Ratio: 0.5
     Tree Height: 325
   - Class: rviz_common/Selection
@@ -472,6 +473,79 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       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: /ovis/robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ovis_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ovis_end_effector:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ovis_link_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ovis_link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ovis_link_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ovis_link_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ovis_link_5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ovis_link_6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ovis_link_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -518,25 +592,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 36.84917449951172
+      Distance: 5.473262786865234
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.4458295702934265
-        Y: 3.5056796073913574
-        Z: -1.5910545587539673
+        X: 2.4571151733398438
+        Y: -1.7171390056610107
+        Z: -1.659576654434204
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.914797306060791
+      Pitch: 0.8547973036766052
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 4.8786468505859375
+      Yaw: 3.2836241722106934
     Saved: ~
 Window Geometry:
   Camera:

src/rove_description/config/default_bridge.yaml

@@ -73,4 +73,18 @@
   gz_topic_name: "/camera_info"
   ros_type_name: "sensor_msgs/msg/CameraInfo"
   gz_type_name: "gz.msgs.CameraInfo"
-  direction: GZ_TO_ROS
+  direction: GZ_TO_ROS
+
+# Bridge for dynamic transforms
+- ros_topic_name: "/tf"
+  gz_topic_name: "/tf"
+  ros_type_name: "tf2_msgs/msg/TFMessage"
+  gz_type_name: "gz.msgs.Pose_V"
+  direction: BIDIRECTIONAL
+
+# Bridge for static transforms
+- ros_topic_name: "/tf_static"
+  gz_topic_name: "/tf_static"
+  ros_type_name: "tf2_msgs/msg/TFMessage"
+  gz_type_name: "gz.msgs.Pose_V"
+  direction: BIDIRECTIONAL