diff --git a/build_multiverse_ws2.sh b/build_multiverse_ws2.sh index f44071a31..8daa7ca03 100755 --- a/build_multiverse_ws2.sh +++ b/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 \ No newline at end of file diff --git a/multiverse/modules/multiverse_connectors/src/mujoco/scripts/mujoco_compile.py b/multiverse/modules/multiverse_connectors/src/mujoco/scripts/mujoco_compile.py index 66f23f64c..25b5ba4bd 100755 --- a/multiverse/modules/multiverse_connectors/src/mujoco/scripts/mujoco_compile.py +++ b/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: diff --git a/multiverse/modules/multiverse_connectors/src/ros/scripts/multiverse_ros_run.py b/multiverse/modules/multiverse_connectors/src/ros/scripts/multiverse_ros_run.py index d50bccdda..ac6490112 100755 --- a/multiverse/modules/multiverse_connectors/src/ros/scripts/multiverse_ros_run.py +++ b/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() diff --git a/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_node_properties.py b/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_node_properties.py index ef4c532db..43201607f 100644 --- a/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_node_properties.py +++ b/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__(): diff --git a/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/image_depth_publisher.py b/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/image_depth_publisher.py index 4d9aa713c..9360334df 100644 --- a/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/image_depth_publisher.py +++ b/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() diff --git a/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/image_rgb_publisher.py b/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/image_rgb_publisher.py index 5216a13f3..ef776ad97 100644 --- a/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/image_rgb_publisher.py +++ b/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() diff --git a/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/multiverse_publisher.py b/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/multiverse_publisher.py index 60a8fef55..cb7e312c1 100755 --- a/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/multiverse_publisher.py +++ b/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) diff --git a/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/odom_publisher.py b/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/odom_publisher.py index 1b4df60ff..0b2680990 100644 --- a/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/odom_publisher.py +++ b/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] \ No newline at end of file diff --git a/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/tf_publisher.py b/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/tf_publisher.py index 37bc8826c..ea76bbff1 100755 --- a/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/tf_publisher.py +++ b/multiverse/modules/multiverse_connectors/src/ros/src/multiverse_ros_socket/multiverse_node/multiverse_publishers/tf_publisher.py @@ -16,7 +16,7 @@ class TfPublisher(MultiversePublisher): _use_meta_data = True - _msg_type = TFMessage + _msg_types = [TFMessage] _root_frame_id: str _seq: int = 0 @@ -28,16 +28,16 @@ def __init__( multiverse_meta_data: MultiverseMetaData = MultiverseMetaData(), **kwargs: Dict ) -> None: + self._root_frame_id = kwargs.get("root_frame_id", "map") + if INTERFACE == Interface.ROS1: + self._seq = 0 super().__init__( topic_name=topic_name, rate=rate, client_addr=client_addr, multiverse_meta_data=multiverse_meta_data, ) - self._root_frame_id = kwargs.get("root_frame_id", "map") self.request_meta_data["receive"][""] = ["position", "quaternion"] - if INTERFACE == Interface.ROS1: - self._seq = 0 def _bind_response_meta_data(self, response_meta_data: Dict) -> None: objects = response_meta_data.get("receive") @@ -45,7 +45,7 @@ def _bind_response_meta_data(self, response_meta_data: Dict) -> None: if objects is None: return - self._msg.transforms.clear() + self._msgs[0].transforms.clear() for object_name, tf_data in objects.items(): tf_data = response_meta_data["receive"][object_name] @@ -76,7 +76,7 @@ def _bind_response_meta_data(self, response_meta_data: Dict) -> None: tf_msg.transform.rotation.x = quaternion[1] tf_msg.transform.rotation.y = quaternion[2] tf_msg.transform.rotation.z = quaternion[3] - self._msg.transforms.append(tf_msg) + self._msgs[0].transforms.append(tf_msg) if INTERFACE == Interface.ROS1: - self._seq += 1 + self._msgs[0] += 1 diff --git a/multiverse/resources/muv/pr2_ros2_control.muv b/multiverse/resources/muv/pr2_ros2_control.muv index 9f681642f..5f20dc8a1 100644 --- a/multiverse/resources/muv/pr2_ros2_control.muv +++ b/multiverse/resources/muv/pr2_ros2_control.muv @@ -97,19 +97,6 @@ multiverse_clients: ros2: ros_nodes: publishers: - tf: - - meta_data: - world_name: world - length_unit: m - angle_unit: rad - mass_unit: kg - time_unit: s - handedness: rhs - port: 7300 - topic: /tf - rate: 60 - root_frame_id: map - odom: - meta_data: world_name: world @@ -118,8 +105,9 @@ multiverse_clients: mass_unit: kg time_unit: s handedness: rhs - port: 7301 - topic: /odom + port: 7300 + odom_topic: /odom + tf_topic: /tf rate: 60 body: pr2 frame_id: map @@ -151,7 +139,7 @@ multiverse_clients: robot_description: /robot_description urdf: pr2/pr2.urdf config: pr2/config/ros2_control.yaml - tf_topic: /pr2/tf + tf_topic: /tf actuators: r_shoulder_pan_actuator: r_shoulder_pan_joint r_shoulder_lift_actuator: r_shoulder_lift_joint diff --git a/multiverse/resources/robots/pr2/config/rviz.rviz2 b/multiverse/resources/robots/pr2/config/rviz.rviz2 index 1292b48e4..e1b2d71c4 100644 --- a/multiverse/resources/robots/pr2/config/rviz.rviz2 +++ b/multiverse/resources/robots/pr2/config/rviz.rviz2 @@ -8,7 +8,7 @@ Panels: - /Status1 - /RobotModel1 Splitter Ratio: 0.5 - Tree Height: 847 + Tree Height: 840 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -467,6 +467,9 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + Mass Properties: + Inertia: false + Mass: false Name: RobotModel TF Prefix: "" Update Interval: 0 @@ -475,7 +478,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: base_footprint + Fixed Frame: map Frame Rate: 30 Name: root Tools: @@ -487,6 +490,9 @@ Visualization Manager: - 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 @@ -541,7 +547,7 @@ Window Geometry: Height: 1136 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000022300000416fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000416000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000416fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000416000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003aa0000041600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000022300000412fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f00000412000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000412fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f00000412000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003aa0000041200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: @@ -549,5 +555,5 @@ Window Geometry: Views: collapsed: false Width: 1848 - X: 1992 - Y: 27 + X: 712 + Y: 32 diff --git a/multiverse_ws2/src/multiverse_core/multiverse_control/CMakeLists.txt b/multiverse_ws2/src/multiverse_core/multiverse_control/CMakeLists.txt index 08644447e..6f54f1e94 100644 --- a/multiverse_ws2/src/multiverse_core/multiverse_control/CMakeLists.txt +++ b/multiverse_ws2/src/multiverse_core/multiverse_control/CMakeLists.txt @@ -86,10 +86,6 @@ install(TARGETS multiverse_control RUNTIME DESTINATION bin ) -execute_process( - COMMAND cp -rf ${MULTIVERSE_DIR}/lib/libstdc++/libmultiverse_client_json.so ${MULTIVERSE_DIR}/../multiverse_ws2/install/multiverse_control/lib/libmultiverse_client_json.so -) - # # EXPORTS ament_export_targets(export_multiverse_control HAS_LIBRARY_TARGET) ament_export_dependencies(${HW_IF_INCLUDE_DEPENDS})