Skip to content

Updates stretch_mujoco_driver to use sim.pull_lidar_points() #60

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
hello-robot-shehab wants to merge 3 commits into
base: jazzy
Choose a base branch
from
Open

Updates stretch_mujoco_driver to use sim.pull_lidar_points() #60

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8bbcae7
Updates stretch_mujoco_driver to use sim.pull_hemi_lidar_points() to …
hello-robot-shehab Aug 13, 2026
d568611
Cameras now work with the latest camera changes in stretch4_mujoco
hello-robot-shehab Aug 13, 2026
7d74b62
Updated lidar and camera topics to use Best Effort reliability
hello-robot-shehab Aug 17, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
70 changes: 69 additions & 1 deletion stretch_simulation/rviz/stretch_sim.rviz
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -438,7 +438,7 @@ Visualization Manager:
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Best Effort
Value: /camera_head/left/image_raw
Value: /cameras_head/left/image_raw
Value: true
Visibility:
Grid: true
Expand Down Expand Up @@ -483,6 +483,74 @@ Visualization Manager:
Use Fixed Frame: true
Use rainbow: true
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: 0; 220; 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: LidarPointsLeft
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Best Effort
Value: /lidar_points_left
Use Fixed Frame: true
Use rainbow: true
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; 180; 0
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: LidarPointsRight
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Best Effort
Value: /lidar_points_right
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Expand Down
102 changes: 48 additions & 54 deletions stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -657,39 +657,32 @@ def publish_camera_and_lidar(self, current_time: TimeMsg | None = None):
self.camera_compressed_publishers[camera.name].publish(ros_image_compressed)

if camera.is_depth:
# if camera == StretchCameras.cam_gripper_se4_stereo_depth:
# pointcloud_msg = create_pointcloud_rgb_msg(
# camera_info_msg=camera_info,
# rgb_image=camera_data.get_camera_data(
# StretchCameras.cam_gripper_se4_stereo_depth
# ),
# depth_image=frame,
# )
# elif camera == StretchCameras.cam_hemilidar_left:
# pointcloud_msg = create_pointcloud_rgb_msg(
# camera_info_msg=camera_info,
# rgb_image=camera_data.get_camera_data(
# StretchCameras.cam_nav_rgb_se4_left, auto_rotate=False
# ),
# depth_image=camera_data.get_camera_data(
# StretchCameras.cam_hemilidar_left, auto_rotate=False
# ),
# )
# elif camera == StretchCameras.cam_hemilidar_right:
# pointcloud_msg = create_pointcloud_rgb_msg(
# camera_info_msg=camera_info,
# rgb_image=camera_data.get_camera_data(
# StretchCameras.cam_nav_rgb_se4_right, auto_rotate=False
# ),
# depth_image=camera_data.get_camera_data(
# StretchCameras.cam_hemilidar_right, auto_rotate=False
# ),
# )
# else:
# pointcloud_msg = create_pointcloud_msg(camera_info, frame)
pointcloud_msg = create_pointcloud_msg(camera_info, frame)
self.pointcloud_publishers[camera.name].publish(pointcloud_msg)

# Publish lidar point clouds
try:
hesai_pts = self.sim.pull_lidar_points()
if isinstance(hesai_pts, dict):
left_pts = hesai_pts.get("left")
right_pts = hesai_pts.get("right")

if left_pts is not None and len(left_pts) > 0:
header = Header()
header.frame_id = "base_footprint"
header.stamp = current_time
cloud_msg_left = pc2.create_cloud_xyz32(header, left_pts)
self.lidar_left_pub.publish(cloud_msg_left)

if right_pts is not None and len(right_pts) > 0:
header = Header()
header.frame_id = "base_footprint"
header.stamp = current_time
cloud_msg_right = pc2.create_cloud_xyz32(header, right_pts)
self.lidar_right_pub.publish(cloud_msg_right)
except Exception as e:
self.logger.error(f"Error publishing lidar points: {e}")

# CHANGE MODES ################
def change_mode(self, new_mode, code_to_run=None):
# self.robot_mode_rwlock.acquire_write()
Expand Down Expand Up @@ -1026,6 +1019,23 @@ def ros_setup(self):
"/scan_filtered",
qos_profile=QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
)
self.lidar_left_pub = self.create_publisher(
PointCloud2,
"/lidar_points_left",
qos_profile=QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
)
self.lidar_right_pub = self.create_publisher(
PointCloud2,
"/lidar_points_right",
qos_profile=QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
)

active_cameras = [
StretchCameras.cam_nav_rgb_se4_center
if camera == StretchCameras.cam_nav_rgb_se4_center_low_rez
else camera
for camera in self.sim._cameras_to_use
]

self.camera_publishers = {
camera.name: self.create_publisher(
Expand All @@ -1035,7 +1045,7 @@ def ros_setup(self):
depth=1, reliability=ReliabilityPolicy.BEST_EFFORT
),
)
for camera in self.sim._cameras_to_use
for camera in active_cameras
}
self.camera_compressed_publishers = {
camera.name: self.create_publisher(
Expand All @@ -1045,7 +1055,7 @@ def ros_setup(self):
depth=1, reliability=ReliabilityPolicy.BEST_EFFORT
),
)
for camera in self.sim._cameras_to_use
for camera in active_cameras
}
self.pointcloud_publishers = {
camera.name: self.create_publisher(
Expand All @@ -1055,18 +1065,18 @@ def ros_setup(self):
depth=1, reliability=ReliabilityPolicy.BEST_EFFORT
),
)
for camera in self.sim._cameras_to_use
for camera in active_cameras
if camera.is_depth
}
self.camera_info_publishers = {
camera.name: self.create_publisher(
CameraInfo,
get_camera_info_topic_name(camera),
qos_profile=QoSProfile(
depth=1, reliability=ReliabilityPolicy.BEST_EFFORT
depth=1, reliability=ReliabilityPolicy.RELIABLE
),
)
for camera in self.sim._cameras_to_use
for camera in active_cameras
}

self.clock_pub = self.create_publisher(
Expand Down Expand Up @@ -1396,12 +1406,8 @@ def get_camera_topic_name(camera: StretchCameras):
return "/cameras_head/left/image_raw"
if camera == StretchCameras.cam_nav_rgb_se4_right:
return "/cameras_head/right/image_raw"
if camera == StretchCameras.cam_nav_rgb_se4_center:
if camera == StretchCameras.cam_nav_rgb_se4_center or camera == StretchCameras.cam_nav_rgb_se4_center_low_rez:
return "/camera_head/center/image_raw"
if camera == StretchCameras.cam_hemilidar_left:
return "/depth/left/depth"
if camera == StretchCameras.cam_hemilidar_right:
return "/depth/right/depth"

raise NotImplementedError(f"Camera {camera} image topic mapping is not implemented")

Expand All @@ -1421,12 +1427,8 @@ def get_camera_info_topic_name(camera: StretchCameras):
return "/cameras_head/left/camera_info"
if camera == StretchCameras.cam_nav_rgb_se4_right:
return "/cameras_head/right/camera_info"
if camera == StretchCameras.cam_nav_rgb_se4_center:
if camera == StretchCameras.cam_nav_rgb_se4_center or camera == StretchCameras.cam_nav_rgb_se4_center_low_rez:
return "/camera_head/center/camera_info"
if camera == StretchCameras.cam_hemilidar_left:
return "/depth/left/camera_info"
if camera == StretchCameras.cam_hemilidar_right:
return "/depth/right/camera_info"

raise NotImplementedError(f"Camera {camera} camera_info topic mapping is not implemented")

Expand All @@ -1438,10 +1440,6 @@ def get_camera_pointcloud_topic_name(camera: StretchCameras):
"""
if camera == StretchCameras.cam_gripper_se4_stereo_depth:
return "/gripper_camera/depth/color/points"
if camera == StretchCameras.cam_hemilidar_left:
return "/lidar_points_left"
if camera == StretchCameras.cam_hemilidar_right:
return "/lidar_points_right"

raise NotImplementedError(f"Camera {camera} pointcloud topic mapping is not implemented")

Expand All @@ -1461,12 +1459,8 @@ def get_camera_frame(camera: StretchCameras):
return "camera_left_optical_link"
if camera == StretchCameras.cam_nav_rgb_se4_right:
return "camera_right_optical_link"
if camera == StretchCameras.cam_nav_rgb_se4_center:
if camera == StretchCameras.cam_nav_rgb_se4_center or camera == StretchCameras.cam_nav_rgb_se4_center_low_rez:
return "camera_center_optical_link"
if camera == StretchCameras.cam_hemilidar_left:
return "lidar_left_link"
if camera == StretchCameras.cam_hemilidar_right:
return "lidar_right_link"

raise NotImplementedError(f"Camera {camera} frame is not implemented")

Expand Down