Fix bug tf and add ros1 and ros2

Multiverse-Framework · Jun 3, 2024 · 56a0052 · 56a0052
1 parent ceccd63
commit 56a0052
Show file tree

Hide file tree

Showing 3 changed files with 148 additions and 15 deletions.
diff --git a/...s/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/multiverse_publisher.py b/...s/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/multiverse_publisher.py
@@ -14,7 +14,6 @@
 
 from ..multiverse_node import MultiverseNode, SocketAddress, MultiverseMetaData
 
-
 class MultiversePublisher(MultiverseNode):
     _use_meta_data: bool = False
     _publisher: Publisher
@@ -39,6 +38,8 @@ def __init__(
             duration_in_seconds = 1.0 / rate
             secs = int(duration_in_seconds)
             nsecs = int((duration_in_seconds - secs) * 1e9)
+            r = rospy.Rate(1)
+            r.sleep()
             rospy.Timer(
                 period=rospy.Duration(secs=secs, nsecs=nsecs),
                 callback=self._publisher_callback

diff --git a/multiverse/resources/demos/ur5e_with_task_board/config/rviz.rviz b/multiverse/resources/demos/ur5e_with_task_board/config/rviz.rviz
@@ -3,11 +3,9 @@ Panels:
     Help Height: 138
     Name: Displays
     Property Tree Widget:
-      Expanded:
-        - /RobotModel1
-        - /RobotModel2/Status1
+      Expanded: ~
       Splitter Ratio: 0.3831101953983307
-    Tree Height: 1576
+    Tree Height: 774
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -168,6 +166,117 @@ Visualization Manager:
       Update Interval: 0
       Value: true
       Visual Enabled: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      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
+        task_board_tool:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: task_board_tool_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      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:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        gripper-2F-85:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        left_coupler:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_driver:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_follower:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_pad:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_silicone_pad:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_spring_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_coupler:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_driver:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_follower:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_pad:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_silicone_pad:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_spring_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: gripper_2F_85_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -196,33 +305,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 3.509185552597046
+      Distance: 2.9916720390319824
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 0.009522239677608013
-        Y: 0.06332888454198837
-        Z: 0.1418982744216919
+        X: -0.5991308689117432
+        Y: 0.04480421915650368
+        Z: 0.684747040271759
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5297964811325073
+      Pitch: 0.4197966754436493
       Target Frame: <Fixed Frame>
-      Yaw: 5.81358528137207
+      Yaw: 5.813584804534912
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 2086
+  Height: 1163
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000037700000720fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000007200000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000720fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000007200000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000f000000005efc0100000002fb0000000800540069006d0065010000000000000f00000006dc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000a120000072000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001d1000003cdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003cd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000003cdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003cd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000005efc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000444000003cd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -231,6 +340,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 3840
+  Width: 1920
   X: 0
   Y: 0
diff --git a/multiverse/resources/muv/ur5e_with_task_board.muv b/multiverse/resources/muv/ur5e_with_task_board.muv
@@ -84,6 +84,20 @@ multiverse_clients:
 
   ros:
     ros_nodes:
+      publishers:
+        tf:
+          - meta_data:
+              world_name: world
+              length_unit: m
+              angle_unit: rad
+              mass_unit: kg
+              time_unit: s
+              handedness: rhs
+            port: 7301
+            topic: /tf
+            rate: 60
+            root_frame_id: map
+
       subscribers:
         data:
           - meta_data:
@@ -129,6 +143,15 @@ multiverse_clients:
               - joint_state_controller
                 joint_trajectory_controller
 
+    ros_run:
+      rviz:
+        config: ur5e_with_task_board/config/rviz.rviz
+        robot_descriptions:
+          ur5e_description: ur5e/urdf/ur5e.urdf
+          gripper_2F_85_description: gripper-2F-85/urdf/gripper-2F-85.urdf
+          task_board_description: task_board/urdf/task_board.urdf
+          task_board_tool_description: task_board/urdf/task_board_tool.urdf
+
   ros2:
     ros_nodes:
       publishers:
@@ -140,7 +163,7 @@ multiverse_clients:
               mass_unit: kg
               time_unit: s
               handedness: rhs
-            port: 7301
+            port: 7801
             topic: /tf
             rate: 60
             root_frame_id: map