diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 5c2bfe0aa..3377b8a88 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -24,6 +24,7 @@ jobs: bash format.sh --test code_docstring: + if: github.event_name != 'pull_request' || github.head_ref != 'main' runs-on: ubuntu-latest steps: - uses: actions/checkout@v2 @@ -200,6 +201,47 @@ jobs: cp ./examples/Basic_MetaDrive_Usages.ipynb ./tests/test_ipynb/ pytest --nbmake -n=auto ./tests/test_ipynb/ + test_ros: + runs-on: ubuntu-22.04 +# container: +# image: ubuntu:jammy + steps: + - name: Set up ROS2 humble + uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: humble + - name: checkout code + uses: actions/checkout@v2 + - name: Blackbox tests + run: | + + ls -al + pwd + source /opt/ros/humble/setup.bash + + sudo rosdep update + + pwd + + pip install pyzmq + pip install gymnasium==0.28 + ls -al + pip install -e . + python -m metadrive.pull_asset + + cd bridges/ros_bridge + ls -al + python _delete_rviz_line.py + sudo rosdep install --from-paths src --ignore-src -y --rosdistro humble + pwd + colcon build + source install/setup.bash + pip install pyzmq + nohup ros2 launch metadrive_example_bridge metadrive_example_bridge.launch.py > ros.txt 2>&1 & + python ros_socket_server.py --test + +# python ros_socket_server.py --test + # - name: Upload coverage to Codecov # uses: codecov/codecov-action@v1 # with: diff --git a/.gitignore b/.gitignore index 2a377f8fb..ac0ae4791 100644 --- a/.gitignore +++ b/.gitignore @@ -39,3 +39,8 @@ **/test_read_copy_waymo /metadrive/tests/vis_functionality/*.png **/assets/ + +/bridges/ros_bridge/build +/bridges/ros_bridge/install +/bridges/ros_bridge/log + diff --git a/bridges/ros_bridge/README.md b/bridges/ros_bridge/README.md new file mode 100644 index 000000000..6f2cdf703 --- /dev/null +++ b/bridges/ros_bridge/README.md @@ -0,0 +1,43 @@ +# Connecting MetaDrive Simulator with ROS2 + +We provide an example bridge for connecting MetaDrive and ROS2 using zmq sockets, which publishes messages for camera +images, LiDAR point clouds, and object 3D bounding boxes. + +## Installation + +To install the bridge, first [install ROS2 **humble**](https://docs.ros.org/en/humble/Installation.html) and follow the scripts +below: + +```bash +cd bridges/ros_bridge + +# activate env, ${ROS_DISTRO} is something like foxy, iron, humble +source /opt/ros/${ROS_DISTRO}/setup.bash +# You may need to run init, if you are installing ros2 for the first time +sudo rosdep init +# zmq should be installed with system python interpreter +pip install pyzmq +rosdep update +rosdep install --from-paths src --ignore-src -y + +# build +colcon build +source install/setup.bash +``` + +## Usage + +```bash +# Terminal 1, launch ROS publishers +ros2 launch metadrive_example_bridge metadrive_example_bridge.launch.py +# Terminal 2, launch socket server +python ros_socket_server.py + +``` + +[Demo Video](https://www.youtube.com/watch?v=WWwdnURnOBM&list=TLGGdRGbC4RGzhAxNzEwMjAyMw) + + +## Known Issues +If you are using the `conda`, it is very likely that the interpreter will not match the system interpreter +and will be incompatible with ROS 2 binaries. \ No newline at end of file diff --git a/bridges/ros_bridge/_delete_rviz_line.py b/bridges/ros_bridge/_delete_rviz_line.py new file mode 100644 index 000000000..7aa9c5af9 --- /dev/null +++ b/bridges/ros_bridge/_delete_rviz_line.py @@ -0,0 +1,28 @@ +""" +Only for github CI using! The visualization tool is not available on github server, remove it. +""" +import os + + +def delete_line_from_file(file_name, line_number): + # Check if the line number is valid + if line_number < 1: + return + + lines = [] + with open(file_name, 'r') as f: + lines = f.readlines() + + # Check if the line number is beyond the file's length + if line_number > len(lines): + return + + with open(file_name, 'w') as f: + for idx, line in enumerate(lines): + if idx + 1 != line_number: + f.write(line) + + +# Usage +if __name__ == '__main__': + delete_line_from_file(os.path.join(os.path.dirname(__file__), 'src', "metadrive_example_bridge", 'package.xml'), 10) diff --git a/bridges/ros_bridge/ros_socket_server.py b/bridges/ros_bridge/ros_socket_server.py new file mode 100644 index 000000000..414bec44c --- /dev/null +++ b/bridges/ros_bridge/ros_socket_server.py @@ -0,0 +1,169 @@ +# launch sockets to send sensor readings to ROS +import argparse +import struct + +import numpy as np +import zmq + +from metadrive.constants import HELP_MESSAGE +from metadrive.engine.asset_loader import AssetLoader +from metadrive.envs.scenario_env import ScenarioEnv +from metadrive.policy.replay_policy import ReplayEgoCarPolicy + + +class RosSocketServer(): + + def __init__(self): + self.context = zmq.Context().instance() + self.context.setsockopt(zmq.IO_THREADS, 2) + self.img_socket = self.context.socket(zmq.PUSH) + self.img_socket.setsockopt(zmq.SNDBUF, 4194304) + self.img_socket.bind("ipc:///tmp/rgb_camera") + self.img_socket.set_hwm(5) + self.lidar_socket = self.context.socket(zmq.PUSH) + self.lidar_socket.setsockopt(zmq.SNDBUF, 4194304) + self.lidar_socket.bind("ipc:///tmp/lidar") + self.lidar_socket.set_hwm(5) + self.obj_socket = self.context.socket(zmq.PUSH) + self.obj_socket.setsockopt(zmq.SNDBUF, 4194304) + self.obj_socket.bind("ipc:///tmp/obj") + self.obj_socket.set_hwm(5) + + def run(self, test=False): + config = dict( + use_render=True if not test else False, # need this on to get the camera + num_scenarios=1, + horizon=1000, + image_observation=True if not test else False, + manual_control=False, + agent_policy=ReplayEgoCarPolicy, + rgb_clip=False, + show_logo=False, + show_fps=False, + show_interface=False, + physics_world_step_size=0.02, + vehicle_config=dict( + image_source="main_camera", + show_navi_mark=False, + ), + data_directory=AssetLoader.file_path("nuscenes", return_raw_style=False), + ) + + env = ScenarioEnv(config) + + try: + + env.reset() + print(HELP_MESSAGE) + env.vehicle.expert_takeover = False + while True: + o = env.step([0, 0]) + if test: + image_data = np.zeros((512, 512, 3)) # fake data for testing + image_data[::16, :, :] = 255 + else: + image_data = o[0]['image'][..., -1] + # send via socket + image_data = image_data.astype(np.uint8) + dim_data = struct.pack('ii', image_data.shape[1], image_data.shape[0]) + image_data = bytearray(image_data) + # concatenate the dimensions and image data into a single byte array + image_data = dim_data + image_data + try: + self.img_socket.send(image_data, zmq.NOBLOCK) + except zmq.error.Again: + msg = "ros_socket_server: error sending image" + if test: + raise ValueError(msg) + else: + print(msg) + del image_data # explicit delete to free memory + + lidar_data, objs = env.vehicle.lidar.perceive( + env.vehicle, + env.engine.physics_world.dynamic_world, + env.vehicle.config["lidar"]["num_lasers"], + env.vehicle.config["lidar"]["distance"], + height=1.0, + ) + + ego_x = env.vehicle.position[0] + ego_y = env.vehicle.position[1] + ego_theta = np.arctan2(env.vehicle.heading[1], env.vehicle.heading[0]) + + num_data = struct.pack('i', len(objs)) + obj_data = [] + for obj in objs: + obj_x = obj.position[0] + obj_y = obj.position[1] + obj_theta = np.arctan2(obj.heading[1], obj.heading[0]) + + obj_x = obj_x - ego_x + obj_y = obj_y - ego_y + obj_x_new = np.cos(-ego_theta) * obj_x - np.sin(-ego_theta) * obj_y + obj_y_new = np.sin(-ego_theta) * obj_x + np.cos(-ego_theta) * obj_y + + obj_data.append(obj_x_new) + obj_data.append(obj_y_new) + obj_data.append(obj_theta - ego_theta) + obj_data.append(obj.LENGTH) + obj_data.append(obj.WIDTH) + obj_data.append(obj.HEIGHT) + obj_data = np.array(obj_data, dtype=np.float32) + obj_data = bytearray(obj_data) + obj_data = num_data + obj_data + try: + self.obj_socket.send(obj_data, zmq.NOBLOCK) + except zmq.error.Again: + msg = "ros_socket_server: error sending objs" + if test: + raise ValueError(msg) + else: + print(msg) + del obj_data # explicit delete to free memory + + # convert lidar data to xyz + lidar_data = np.array(lidar_data) * env.vehicle.config["lidar"]["distance"] + lidar_range = env.vehicle.lidar._get_lidar_range( + env.vehicle.config["lidar"]["num_lasers"], env.vehicle.lidar.start_phase_offset + ) + point_x = lidar_data * np.cos(lidar_range) + point_y = lidar_data * np.sin(lidar_range) + point_z = np.ones(lidar_data.shape) # assume height = 1.0 + lidar_data = np.stack([point_x, point_y, point_z], axis=-1).astype(np.float32) + dim_data = struct.pack('i', len(lidar_data)) + lidar_data = bytearray(lidar_data) + # concatenate the dimensions and lidar data into a single byte array + lidar_data = dim_data + lidar_data + try: + self.lidar_socket.send(lidar_data, zmq.NOBLOCK) + except zmq.error.Again: + msg = "ros_socket_server: error sending lidar" + if test: + raise ValueError(msg) + else: + print(msg) + del lidar_data # explicit delete to free memory + + if o[2]: # done + break + + except Exception as e: + import traceback + traceback.print_exc() + raise e + + finally: + env.close() + + +def main(test=False): + server = RosSocketServer() + server.run(test) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--test", action="store_true") + args = parser.parse_args() + main(args.test) diff --git a/bridges/ros_bridge/src/metadrive_example_bridge/launch/metadrive_example_bridge.launch.py b/bridges/ros_bridge/src/metadrive_example_bridge/launch/metadrive_example_bridge.launch.py new file mode 100644 index 000000000..0d6b8790a --- /dev/null +++ b/bridges/ros_bridge/src/metadrive_example_bridge/launch/metadrive_example_bridge.launch.py @@ -0,0 +1,26 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + ld = LaunchDescription() + camera_node = Node(package="metadrive_example_bridge", executable="camera_bridge", name='camera_bridge') + ld.add_action(camera_node) + lidar_node = Node(package="metadrive_example_bridge", executable="lidar_bridge", name='lidar_bridge') + ld.add_action(lidar_node) + obj_node = Node(package="metadrive_example_bridge", executable="obj_bridge", name='obj_bridge') + ld.add_action(obj_node) + + pkg_dir = get_package_share_directory('metadrive_example_bridge') + rviz_node = Node( + package='rviz2', executable='rviz2', name='rviz2', arguments=['-d', [os.path.join(pkg_dir, 'default.rviz')]] + ) + ld.add_action(rviz_node) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/bridges/ros_bridge/src/metadrive_example_bridge/metadrive_example_bridge/__init__.py b/bridges/ros_bridge/src/metadrive_example_bridge/metadrive_example_bridge/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/bridges/ros_bridge/src/metadrive_example_bridge/metadrive_example_bridge/camera_bridge.py b/bridges/ros_bridge/src/metadrive_example_bridge/metadrive_example_bridge/camera_bridge.py new file mode 100644 index 000000000..a7a87292e --- /dev/null +++ b/bridges/ros_bridge/src/metadrive_example_bridge/metadrive_example_bridge/camera_bridge.py @@ -0,0 +1,76 @@ +import struct + +import cv2 +import numpy as np +import rclpy +import zmq +from builtin_interfaces.msg import Time +from cv_bridge import CvBridge +from rclpy.node import Node +from sensor_msgs.msg import Image +from std_msgs.msg import Header + + +class CameraPublisher(Node): + cv_bridge = CvBridge() + + def __init__(self): + super().__init__('camera_publisher') + self.publisher_ = self.create_publisher(Image, 'metadrive/image', qos_profile=10) + timer_period = 0.05 # seconds + context = zmq.Context().instance() + self.socket = context.socket(zmq.PULL) + self.socket.setsockopt(zmq.CONFLATE, 1) + self.socket.set_hwm(5) + self.socket.connect("ipc:///tmp/rgb_camera") # configured in gamerunner.py + self.timer = self.create_timer(timer_period, self.timer_callback) + self.i = 0 + + def timer_callback(self): + image_buffer_msg = self.socket.recv() + # read the first 32bit int as W + # second as H to handle different resolutions + W, H = struct.unpack('ii', image_buffer_msg[:8]) + # read the rest of the message as the image buffer + image_buffer = image_buffer_msg[8:] + image = np.frombuffer(image_buffer, dtype=np.uint8).reshape((H, W, 3)) + image = image[:, :, :3] # remove the alpha channel + image = cv2.resize(image, (W, H)) + img_msg = CameraPublisher.cv_bridge.cv2_to_imgmsg(image) + img_msg.header = self.get_msg_header() + self.publisher_.publish(img_msg) + self.i += 1 + + def get_msg_header(self): + """ + Get a filled ROS message header + :return: ROS message header + :rtype: std_msgs.msg.Header + """ + header = Header() + header.frame_id = "map" + t = self.get_clock().now() + t = t.seconds_nanoseconds() + time = Time() + time.sec = t[0] + time.nanosec = t[1] + header.stamp = time + return header + + +def main(args=None): + rclpy.init(args=args) + + camera_publisher = CameraPublisher() + + rclpy.spin(camera_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + camera_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/bridges/ros_bridge/src/metadrive_example_bridge/metadrive_example_bridge/lidar_bridge.py b/bridges/ros_bridge/src/metadrive_example_bridge/metadrive_example_bridge/lidar_bridge.py new file mode 100644 index 000000000..5b8d31047 --- /dev/null +++ b/bridges/ros_bridge/src/metadrive_example_bridge/metadrive_example_bridge/lidar_bridge.py @@ -0,0 +1,127 @@ +import ctypes +import struct +import sys + +import numpy as np +import rclpy +import zmq +from builtin_interfaces.msg import Time +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2, PointField +from std_msgs.msg import Header + +_DATATYPES = {} +_DATATYPES[PointField.INT8] = ('b', 1) +_DATATYPES[PointField.UINT8] = ('B', 1) +_DATATYPES[PointField.INT16] = ('h', 2) +_DATATYPES[PointField.UINT16] = ('H', 2) +_DATATYPES[PointField.INT32] = ('i', 4) +_DATATYPES[PointField.UINT32] = ('I', 4) +_DATATYPES[PointField.FLOAT32] = ('f', 4) +_DATATYPES[PointField.FLOAT64] = ('d', 8) + + +class LidarPublisher(Node): + + def __init__(self): + super().__init__('lidar_publisher') + self.publisher_ = self.create_publisher(PointCloud2, 'metadrive/lidar', qos_profile=10) + timer_period = 0.05 # seconds + context = zmq.Context().instance() + self.socket = context.socket(zmq.PULL) + self.socket.setsockopt(zmq.CONFLATE, 1) + self.socket.set_hwm(5) + self.socket.connect("ipc:///tmp/lidar") # configured in gamerunner.py + self.timer = self.create_timer(timer_period, self.timer_callback) + self.i = 0 + + def _get_struct_fmt(self, is_bigendian, fields, field_names=None): + fmt = '>' if is_bigendian else '<' + + offset = 0 + for field in (f for f in sorted(fields, key=lambda f: f.offset) + if field_names is None or f.name in field_names): + if offset < field.offset: + fmt += 'x' * (field.offset - offset) + offset = field.offset + if field.datatype not in _DATATYPES: + print('Skipping unknown PointField datatype [{}]' % field.datatype, file=sys.stderr) + else: + datatype_fmt, datatype_length = _DATATYPES[field.datatype] + fmt += field.count * datatype_fmt + offset += field.count * datatype_length + + return fmt + + def timer_callback(self): + lidar_buffer_msg = self.socket.recv() + # read the first 32bit int as W + # second as H to handle different resolutions + length = struct.unpack('i', lidar_buffer_msg[:4])[0] + # read the rest of the message as the image buffer + lidar_buffer = lidar_buffer_msg[4:] + lidar = np.frombuffer(lidar_buffer, dtype=np.float32) + lidar = lidar.reshape(-1, 3) + assert lidar.shape[0] == length + fields = [ + PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), + ] + + header = self.get_msg_header() + cloud_struct = struct.Struct(self._get_struct_fmt(False, fields)) + buff = ctypes.create_string_buffer(cloud_struct.size * len(lidar)) + point_step, pack_into = cloud_struct.size, cloud_struct.pack_into + offset = 0 + for p in lidar: + pack_into(buff, offset, *p) + offset += point_step + + point_cloud_msg = PointCloud2( + header=header, + height=1, + width=len(lidar), + is_dense=False, + is_bigendian=False, + fields=fields, + point_step=cloud_struct.size, + row_step=cloud_struct.size * len(lidar), + data=buff.raw + ) + self.publisher_.publish(point_cloud_msg) + self.i += 1 + + def get_msg_header(self): + """ + Get a filled ROS message header + :return: ROS message header + :rtype: std_msgs.msg.Header + """ + header = Header() + header.frame_id = "map" + t = self.get_clock().now() + t = t.seconds_nanoseconds() + time = Time() + time.sec = t[0] + time.nanosec = t[1] + header.stamp = time + return header + + +def main(args=None): + rclpy.init(args=args) + + camera_publisher = LidarPublisher() + + rclpy.spin(camera_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + camera_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/bridges/ros_bridge/src/metadrive_example_bridge/metadrive_example_bridge/obj_bridge.py b/bridges/ros_bridge/src/metadrive_example_bridge/metadrive_example_bridge/obj_bridge.py new file mode 100644 index 000000000..b03b36da6 --- /dev/null +++ b/bridges/ros_bridge/src/metadrive_example_bridge/metadrive_example_bridge/obj_bridge.py @@ -0,0 +1,81 @@ +import struct + +import numpy as np +import rclpy +import zmq +from builtin_interfaces.msg import Time +from geometry_msgs.msg import Point, Pose, Quaternion, Vector3 +from rclpy.node import Node +from std_msgs.msg import Header +from vision_msgs.msg import BoundingBox3D, BoundingBox3DArray + + +class ObjectPublisher(Node): + + def __init__(self): + super().__init__('obj_publisher') + self.publisher_ = self.create_publisher(BoundingBox3DArray, 'metadrive/object', qos_profile=10) + timer_period = 0.05 # seconds + context = zmq.Context().instance() + self.socket = context.socket(zmq.PULL) + self.socket.setsockopt(zmq.CONFLATE, 1) + self.socket.set_hwm(5) + self.socket.connect("ipc:///tmp/obj") # configured in gamerunner.py + self.timer = self.create_timer(timer_period, self.timer_callback) + self.i = 0 + + def timer_callback(self): + obj_buffer_msg = self.socket.recv() + # read the first 32bit int as W + # second as H to handle different resolutions + num_obj = struct.unpack('i', obj_buffer_msg[:4])[0] + # read the rest of the message as the image buffer + obj_buffer = obj_buffer_msg[4:] + obj_infos = np.frombuffer(obj_buffer, dtype=np.float32).reshape((num_obj, 6)) + bboxes = [] + for obj_info in obj_infos: + obj_info = obj_info.tolist() + size = Vector3(x=obj_info[3], y=obj_info[4], z=obj_info[5]) + pos = Point(x=obj_info[0], y=obj_info[1], z=0.0) + rot = Quaternion(x=0.0, y=0.0, z=np.sin(obj_info[2] / 2.0), w=np.cos(obj_info[2] / 2.0)) + pose = Pose(position=pos, orientation=rot) + bbox = BoundingBox3D(center=pose, size=size) + bboxes.append(bbox) + obj_msg = BoundingBox3DArray(header=self.get_msg_header(), boxes=bboxes) + obj_msg.header = self.get_msg_header() + self.publisher_.publish(obj_msg) + self.i += 1 + + def get_msg_header(self): + """ + Get a filled ROS message header + :return: ROS message header + :rtype: std_msgs.msg.Header + """ + header = Header() + header.frame_id = "map" + t = self.get_clock().now() + t = t.seconds_nanoseconds() + time = Time() + time.sec = t[0] + time.nanosec = t[1] + header.stamp = time + return header + + +def main(args=None): + rclpy.init(args=args) + + obj_publisher = ObjectPublisher() + + rclpy.spin(obj_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + obj_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/bridges/ros_bridge/src/metadrive_example_bridge/package.xml b/bridges/ros_bridge/src/metadrive_example_bridge/package.xml new file mode 100644 index 000000000..acda96270 --- /dev/null +++ b/bridges/ros_bridge/src/metadrive_example_bridge/package.xml @@ -0,0 +1,14 @@ + + + + metadrive_example_bridge + 0.0.0 + example ros2 bridge for metadrive + Zhizheng Liu + MIT + vision_msgs + vision_msgs_rviz_plugins + + ament_python + + diff --git a/bridges/ros_bridge/src/metadrive_example_bridge/resource/metadrive_example_bridge b/bridges/ros_bridge/src/metadrive_example_bridge/resource/metadrive_example_bridge new file mode 100644 index 000000000..e69de29bb diff --git a/bridges/ros_bridge/src/metadrive_example_bridge/rviz/default.rviz b/bridges/ros_bridge/src/metadrive_example_bridge/rviz/default.rviz new file mode 100644 index 000000000..b9335a57e --- /dev/null +++ b/bridges/ros_bridge/src/metadrive_example_bridge/rviz/default.rviz @@ -0,0 +1,208 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /BoundingBox3DArray1 + Splitter Ratio: 0.5 + Tree Height: 701 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /metadrive/image + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 5 + Size (m): 0.30000001192092896 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /metadrive/lidar + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Value: true + - Alpha: 1 + Class: vision_msgs_rviz_plugins/BoundingBox3DArray + Color: 0; 255; 0 + Enabled: true + Line Width: 0.05000000074505806 + Name: BoundingBox3DArray + Namespaces: + {} + Only Edge: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /metadrive/object + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - 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 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 134.39999389648438 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.52587890625e-05 + Y: 2.6702880859375e-05 + Z: 7.62939453125e-06 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.0347968339920044 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 3.1315996646881104 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 2032 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000047400000701fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000003ab0000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000004200000034a0000004500ffffff000000010000014d00000701fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000069000007010000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7000000051fc0100000002fb0000000800540069006d0065010000000000000e700000046000fffffffb0000000800540069006d00650100000000000004500000000000000000000008970000070100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 3696 + X: 144 + Y: 54 diff --git a/bridges/ros_bridge/src/metadrive_example_bridge/setup.cfg b/bridges/ros_bridge/src/metadrive_example_bridge/setup.cfg new file mode 100644 index 000000000..00d9a9f2b --- /dev/null +++ b/bridges/ros_bridge/src/metadrive_example_bridge/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/metadrive_example_bridge +[install] +install_scripts=$base/lib/metadrive_example_bridge diff --git a/bridges/ros_bridge/src/metadrive_example_bridge/setup.py b/bridges/ros_bridge/src/metadrive_example_bridge/setup.py new file mode 100644 index 000000000..b732c3c77 --- /dev/null +++ b/bridges/ros_bridge/src/metadrive_example_bridge/setup.py @@ -0,0 +1,30 @@ +from setuptools import setup +from glob import glob + +package_name = 'metadrive_example_bridge' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, glob('launch/*.launch.py')), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name, glob('rviz/*.rviz')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Zhizheng Liu', + maintainer_email='zhizheng@cs.ucla.edu', + description='example ros2 bridge for metadrive', + license='MIT', + entry_points={ + 'console_scripts': [ + 'camera_bridge = metadrive_example_bridge.camera_bridge:main', + 'lidar_bridge = metadrive_example_bridge.lidar_bridge:main', + 'obj_bridge = metadrive_example_bridge.obj_bridge:main' + ], + }, +) diff --git a/metadrive/engine/base_engine.py b/metadrive/engine/base_engine.py index f71aec015..2a161fe15 100644 --- a/metadrive/engine/base_engine.py +++ b/metadrive/engine/base_engine.py @@ -544,7 +544,7 @@ def agents(self): def setup_main_camera(self): from metadrive.engine.core.main_camera import MainCamera # Not we should always enable main camera if image obs is required! Or RGBCamera will return incorrect result - if self.mode == RENDER_MODE_ONSCREEN or (RENDER_MODE_OFFSCREEN and + if self.mode == RENDER_MODE_ONSCREEN or (self.mode == RENDER_MODE_OFFSCREEN and self.global_config["vehicle_config"]["image_source"] == "main_camera"): return MainCamera(self, self.global_config["camera_height"], self.global_config["camera_dist"]) else: diff --git a/setup.py b/setup.py index 29530765a..d48d1e12f 100644 --- a/setup.py +++ b/setup.py @@ -92,6 +92,10 @@ def is_win(): "gym>=0.19.0, <=0.26.0" ] +ros_requirement = [ + "zmq" +] + setup( name="metadrive-simulator", python_requires='>=3.6, <3.12', # do version check with assert @@ -107,6 +111,7 @@ def is_win(): "nuplan": nuplan_requirement, "waymo": waymo_requirement, "gym": gym_requirement, + "ros": ros_requirement, "all": nuplan_requirement + cuda_requirement }, include_package_data=True,