Make publisher publish to multiple topics

build_multiverse_ws2.sh

@@ -12,8 +12,14 @@ if [ -z "$ROS2_DISTRO" ]; then
     exit 1
 fi
 
+cd $(dirname $0)
+
+MULTIVERSE_DIR=$PWD/multiverse
+
 # Build the workspace
 cd $(dirname $0)/multiverse_ws2
 rosdep install --from-paths src --ignore-src -r -y --rosdistro $ROS2_DISTRO
 . /opt/ros/$ROS2_DISTRO/setup.sh
 colcon build --symlink-install
+
+ln -sf ${MULTIVERSE_DIR}/lib/libstdc++/libmultiverse_client_json.so ${MULTIVERSE_DIR}/../multiverse_ws2/install/multiverse_control/lib/libmultiverse_client_json.so

multiverse/modules/multiverse_connectors/src/mujoco/scripts/mujoco_compile.py

@@ -714,7 +714,7 @@ def apply_key_frame(self, entity_xml_path: str, apply: Dict[str, Dict[str, Any]]
             body_id = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_BODY, body_name)
             dof_adr = m.body_dofadr[body_id]
             dof_num = m.body_dofnum[body_id]
-            if dof_adr == -1 or dof_num not in [3, 6]:
+            if body_id == 1 or dof_adr == -1 or dof_num not in [3, 6]:
                 continue
 
             if isinstance(body_attributes, dict) and body_attributes.get("pos") is not None:

multiverse/modules/multiverse_connectors/src/ros/scripts/multiverse_ros_run.py

@@ -96,7 +96,7 @@ def start(self):
                 rospy.signal_shutdown(exit_str)
             elif INTERFACE == Interface.ROS2:
                 print(exit_str)
-                rclpy.shutdown()
+                # rclpy.shutdown()
         finally:
             for subscriber in subscriber_list:
                 subscriber.stop()

multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_node_properties.py

@@ -40,7 +40,7 @@ def ros_node_name(self, ros_node_name: str):
         self._ros_node_name = ros_node_name
 
     def create_publisher(self) -> MultiversePublisher:
-        topic_name = self.ros_node_prop.pop("topic")
+        topic_name = self.ros_node_prop.pop("topic") if "topic" in self.ros_node_prop else None
         rate = self.ros_node_prop.pop("rate")
         publisher_name = f"{self.ros_node_name}Publisher"
         for subclass in MultiversePublisher.__subclasses__():

multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/image_depth_publisher.py

@@ -15,7 +15,7 @@
 
 class ImageDepthPublisher(MultiversePublisher):
     _use_meta_data = False
-    _msg_type = Image
+    _msg_types = [Image]
     _body_name: str
     _frame_id: str
 
@@ -41,51 +41,51 @@ def __init__(
         self._frame_id = str(kwargs.get("frame_id", "map"))
         image_res = str(kwargs["image_res"])
         if image_res == "3840_2160":
-            self._msg.width = 3840
-            self._msg.height = 2160
+            self._msgs[0].width = 3840
+            self._msgs[0].height = 2160
             self.request_meta_data["receive"][self._camera_name] = ["depth_3840_2160"]
         elif image_res == "1280_1024":
-            self._msg.width = 1280
-            self._msg.height = 1024
+            self._msgs[0].width = 1280
+            self._msgs[0].height = 1024
             self.request_meta_data["receive"][self._camera_name] = ["depth_1280_1024"]
         elif image_res == "640_480":
-            self._msg.width = 640
-            self._msg.height = 480
+            self._msgs[0].width = 640
+            self._msgs[0].height = 480
             self.request_meta_data["receive"][self._camera_name] = ["depth_640_480"]
         elif image_res == "128_128":
-            self._msg.width = 128
-            self._msg.height = 128
+            self._msgs[0].width = 128
+            self._msgs[0].height = 128
             self.request_meta_data["receive"][self._camera_name] = ["depth_128_128"]
         else:
             raise Exception(f"Invalid image resolution {image_res}.")
 
         if INTERFACE == Interface.ROS1:
-            self._msg.header.stamp = rospy.Time.now()
-            self._msg.header.seq = 0
+            self._msgs[0].header.stamp = rospy.Time.now()
+            self._msgs[0].header.seq = 0
         elif INTERFACE == Interface.ROS2:
-            self._msg.header.stamp = self.get_clock().now().to_msg()
-        self._msg.header.frame_id = self._frame_id
+            self._msgs[0].header.stamp = self.get_clock().now().to_msg()
+        self._msgs[0].header.frame_id = self._frame_id
 
-        self._msg.encoding = "16UC1"
-        self._msg.is_bigendian = False
-        self._msg.step = 1
+        self._msgs[0].encoding = "16UC1"
+        self._msgs[0].is_bigendian = False
+        self._msgs[0].step = 1
 
     def _bind_response_meta_data(self, response_meta_data) -> None:
         if response_meta_data.get("receive") is None:
             return
 
     def _bind_receive_data(self, receive_data: List[float]) -> None:
-        if len(receive_data) != self._msg.height * self._msg.width + 1:
+        if len(receive_data) != self._msgs[0].height * self._msgs[0].width + 1:
             self.logwarn(
                 f"Invalid data length {len(receive_data)} for camera {self._camera_name}."
             )
             return
 
         if INTERFACE == Interface.ROS1:
-            self._msg.header.stamp = rospy.Time.now()
-            self._msg.header.seq += 1
+            self._msgs[0].header.stamp = rospy.Time.now()
+            self._msgs[0].header.seq += 1
         elif INTERFACE == Interface.ROS2:
-            self._msg.header.stamp = self.get_clock().now().to_msg()
+            self._msgs[0].header.stamp = self.get_clock().now().to_msg()
 
         data_array = numpy.array(receive_data[1:], dtype=numpy.uint16)
-        self._msg.data = data_array.tobytes()
+        self._msgs[0].data = data_array.tobytes()

multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/image_rgb_publisher.py

@@ -15,7 +15,7 @@
 
 class ImageRgbPublisher(MultiversePublisher):
     _use_meta_data = False
-    _msg_type = Image
+    _msg_types = [Image]
     _body_name: str
     _frame_id: str
 
@@ -41,51 +41,51 @@ def __init__(
         self._frame_id = str(kwargs.get("frame_id", "map"))
         image_res = str(kwargs["image_res"])
         if image_res == "3840_2160":
-            self._msg.width = 3840
-            self._msg.height = 2160
+            self._msgs[0].width = 3840
+            self._msgs[0].height = 2160
             self.request_meta_data["receive"][self._camera_name] = ["rgb_3840_2160"]
         elif image_res == "1280_1024":
-            self._msg.width = 1280
-            self._msg.height = 1024
+            self._msgs[0].width = 1280
+            self._msgs[0].height = 1024
             self.request_meta_data["receive"][self._camera_name] = ["rgb_1280_1024"]
         elif image_res == "640_480":
-            self._msg.width = 640
-            self._msg.height = 480
+            self._msgs[0].width = 640
+            self._msgs[0].height = 480
             self.request_meta_data["receive"][self._camera_name] = ["rgb_640_480"]
         elif image_res == "128_128":
-            self._msg.width = 128
-            self._msg.height = 128
+            self._msgs[0].width = 128
+            self._msgs[0].height = 128
             self.request_meta_data["receive"][self._camera_name] = ["rgb_128_128"]
         else:
             raise Exception(f"Invalid image resolution {image_res}.")
 
         if INTERFACE == Interface.ROS1:
-            self._msg.header.stamp = rospy.Time.now()
-            self._msg.header.seq = 0
+            self._msgs[0].header.stamp = rospy.Time.now()
+            self._msgs[0].header.seq = 0
         elif INTERFACE == Interface.ROS2:
-            self._msg.header.stamp = self.get_clock().now().to_msg()
-        self._msg.header.frame_id = self._frame_id
+            self._msgs[0].header.stamp = self.get_clock().now().to_msg()
+        self._msgs[0].header.frame_id = self._frame_id
 
-        self._msg.encoding = "rgb8"
-        self._msg.is_bigendian = False
-        self._msg.step = 3
+        self._msgs[0].encoding = "rgb8"
+        self._msgs[0].is_bigendian = False
+        self._msgs[0].step = 3
 
     def _bind_response_meta_data(self, response_meta_data) -> None:
         if response_meta_data.get("receive") is None:
             return
 
     def _bind_receive_data(self, receive_data: List[float]) -> None:
-        if len(receive_data) != self._msg.height * self._msg.width * 3 + 1:
+        if len(receive_data) != self._msgs[0].height * self._msgs[0].width * 3 + 1:
             self.logwarn(
                 f"Invalid data length {len(receive_data)} for camera {self._camera_name}."
             )
             return
 
         if INTERFACE == Interface.ROS1:
-            self._msg.header.stamp = rospy.Time.now()
-            self._msg.header.seq += 1
+            self._msgs[0].header.stamp = rospy.Time.now()
+            self._msgs[0].header.seq += 1
         elif INTERFACE == Interface.ROS2:
-            self._msg.header.stamp = self.get_clock().now().to_msg()
+            self._msgs[0].header.stamp = self.get_clock().now().to_msg()
 
         data_array = numpy.array(receive_data[1:], dtype=numpy.uint8)
-        self._msg.data = data_array.tobytes()
+        self._msgs[0].data = data_array.tobytes()

multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/multiverse_publisher.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 
-from typing import Dict
+from typing import Dict, Any, List, Optional
 
 from ... import Interface, INTERFACE
 
@@ -16,14 +16,14 @@
 
 class MultiversePublisher(MultiverseNode):
     _use_meta_data: bool = False
-    _publisher: Publisher
-    _msg_type = None
-    _msg = None
+    _msg_types: List[Any] = []
+    _msgs: List[Any] = []
+    __publishers: List[Publisher] = []
 
     def __init__(
             self,
             client_addr: SocketAddress,
-            topic_name: str,
+            topic_name: str | List[str],
             rate: float = 60.0,
             multiverse_meta_data: MultiverseMetaData = MultiverseMetaData(),
             **kwargs: Dict
@@ -32,21 +32,27 @@ def __init__(
             client_addr=client_addr,
             multiverse_meta_data=multiverse_meta_data
         )
+        if isinstance(topic_name, str):
+            self.create_publishers([topic_name], [self._msg_type], rate)
+        elif isinstance(topic_name, list):
+            self.create_publishers(topic_name, self._msg_types, rate)
+
+    def create_publishers(self, topic_names: List[str], msg_types: List[Any], rate: float) -> None:
         if INTERFACE == Interface.ROS1:
-            self._msg = self._msg_type()
-            self._publisher = Publisher(topic_name, self._msg_type, queue_size=100)
+            for topic_name, msg_type in zip(topic_names, msg_types):
+                self._msgs.append(msg_type())
+                self.__publishers.append(rospy.Publisher(topic_name, msg_type, queue_size=100))
             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
             )
         elif INTERFACE == Interface.ROS2:
-            self._msg = self._msg_type()
-            self._publisher = self.create_publisher(self._msg_type, topic_name, 100)
+            for topic_name, msg_type in zip(topic_names, msg_types):
+                self._msgs.append(msg_type())
+                self.__publishers.append(self.create_publisher(msg_type, topic_name, 100))
             self.create_timer(
                 timer_period_sec=1.0 / rate,
                 callback=self._publisher_callback
@@ -72,4 +78,5 @@ def _publisher_callback(self, _=None) -> None:
             return
 
     def _publish(self) -> None:
-        self._publisher.publish(self._msg)
+        for msg, publisher in zip(self._msgs, self.__publishers):
+            publisher.publish(msg)

multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/odom_publisher.py

@@ -3,6 +3,8 @@
 from typing import List, Dict
 
 from nav_msgs.msg import Odometry
+from geometry_msgs.msg import TransformStamped
+from tf2_msgs.msg import TFMessage
 
 from .multiverse_publisher import Interface, INTERFACE
 
@@ -14,28 +16,31 @@
 
 class OdomPublisher(MultiversePublisher):
     _use_meta_data = False
-    _msg_type = Odometry
+    _msg_types = [Odometry, TFMessage]
     _body_name: str
     _frame_id: str
 
     def __init__(
             self,
             client_addr: SocketAddress,
-            topic_name: str = "/tf",
             rate: float = 60.0,
             multiverse_meta_data: MultiverseMetaData = MultiverseMetaData(),
             **kwargs: Dict
     ) -> None:
+        if "body" not in kwargs:
+            raise Exception("body not found.")
+        if "odom_topic" not in kwargs:
+            raise Exception("odom_topic not found.")
+        if "tf_topic" not in kwargs:
+            raise Exception("tf_topic not found.")
+        self._body_name = str(kwargs["body"])
+        self._frame_id = str(kwargs.get("frame_id", "map"))
         super().__init__(
             client_addr=client_addr,
-            topic_name=topic_name,
+            topic_name=[kwargs["odom_topic"], kwargs["tf_topic"]],
             rate=rate,
             multiverse_meta_data=multiverse_meta_data,
         )
-        if "body" not in kwargs:
-            raise Exception("Body not found.")
-        self._body_name = str(kwargs["body"])
-        self._frame_id = str(kwargs.get("frame_id", "map"))
         self.request_meta_data["receive"][self._body_name] = [
             "position",
             "quaternion",
@@ -47,29 +52,49 @@ def _bind_response_meta_data(self, response_meta_data) -> None:
             return
 
         if INTERFACE == Interface.ROS1:
-            self._msg.header.seq += 1
-        self._msg.header.frame_id = self._frame_id
-        self._msg.child_frame_id = self._body_name
-        self._msg.pose.covariance = [0.0] * 36
-        self._msg.twist.covariance = [0.0] * 36
+            self._msgs[0].header.seq += 1
+        self._msgs[0].header.frame_id = self._frame_id
+        self._msgs[0].child_frame_id = self._body_name
+        self._msgs[0].pose.covariance = [0.0] * 36
+        self._msgs[0].twist.covariance = [0.0] * 36
+
+        self._msgs[1].transforms.clear()
+        tf_msg = TransformStamped()
+        tf_msg.header.frame_id = self._frame_id
+        tf_msg.child_frame_id = self._body_name
+        self._msgs[1].transforms.append(tf_msg)
 
     def _bind_receive_data(self, receive_data: List[float]) -> None:
         if len (receive_data) != 14:
             return
         if INTERFACE == Interface.ROS1:
-            self._msg.header.stamp = rospy.Time.now()
+            self._msgs[0].header.stamp = rospy.Time.now()
         elif INTERFACE == Interface.ROS2:
-            self._msg.header.stamp = self.get_clock().now().to_msg()
-        self._msg.pose.pose.position.x = receive_data[1]
-        self._msg.pose.pose.position.y = receive_data[2]
-        self._msg.pose.pose.position.z = receive_data[3]
-        self._msg.pose.pose.orientation.w = receive_data[4]
-        self._msg.pose.pose.orientation.x = receive_data[5]
-        self._msg.pose.pose.orientation.y = receive_data[6]
-        self._msg.pose.pose.orientation.z = receive_data[7]
-        self._msg.twist.twist.linear.x = receive_data[8]
-        self._msg.twist.twist.linear.y = receive_data[9]
-        self._msg.twist.twist.linear.z = receive_data[10]
-        self._msg.twist.twist.angular.x = receive_data[11]
-        self._msg.twist.twist.angular.y = receive_data[12]
-        self._msg.twist.twist.angular.z = receive_data[13]
+            self._msgs[0].header.stamp = self.get_clock().now().to_msg()
+        self._msgs[0].pose.pose.position.x = receive_data[1]
+        self._msgs[0].pose.pose.position.y = receive_data[2]
+        self._msgs[0].pose.pose.position.z = receive_data[3]
+        self._msgs[0].pose.pose.orientation.w = receive_data[4]
+        self._msgs[0].pose.pose.orientation.x = receive_data[5]
+        self._msgs[0].pose.pose.orientation.y = receive_data[6]
+        self._msgs[0].pose.pose.orientation.z = receive_data[7]
+        self._msgs[0].twist.twist.linear.x = receive_data[8]
+        self._msgs[0].twist.twist.linear.y = receive_data[9]
+        self._msgs[0].twist.twist.linear.z = receive_data[10]
+        self._msgs[0].twist.twist.angular.x = receive_data[11]
+        self._msgs[0].twist.twist.angular.y = receive_data[12]
+        self._msgs[0].twist.twist.angular.z = receive_data[13]
+
+        if INTERFACE == Interface.ROS1:
+            self._msgs[1].transforms[0].header.stamp = rospy.Time.now()
+            self._msgs[1].transforms[0].header.seq = self._seq
+        elif INTERFACE == Interface.ROS2:
+            self._msgs[1].transforms[0].header.stamp = self.get_clock().now().to_msg()
+
+        self._msgs[1].transforms[0].transform.translation.x = receive_data[1]
+        self._msgs[1].transforms[0].transform.translation.y = receive_data[2]
+        self._msgs[1].transforms[0].transform.translation.z = receive_data[3]
+        self._msgs[1].transforms[0].transform.rotation.w = receive_data[4]
+        self._msgs[1].transforms[0].transform.rotation.x = receive_data[5]
+        self._msgs[1].transforms[0].transform.rotation.y = receive_data[6]
+        self._msgs[1].transforms[0].transform.rotation.z = receive_data[7]