Grasp_object error

Hello! When I tried to run ros2 launch stretch_demos grasp_object.launch.py, an error occurred after the object was detected. I have tried the script, but it still doesn’t work. May I ask what happened?
I hope you can help me answer, thank you!

hello-robot@stretch-se3-3102:~/ament_ws$ ros2 launch stretch_demos grasp_object.launch.py 
[INFO] [launch]: All log files can be found below /home/hello-robot/.ros/log/2025-04-01-05-44-45-050106-stretch-se3-3102-7858
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [launch.user]: D435i launched in high resolution
[INFO] [realsense2_camera_node-1]: process started with pid [7860]
[INFO] [stretch_driver-5]: process started with pid [7868]
[INFO] [d435i_accel_correction-2]: process started with pid [7862]
[INFO] [joint_state_publisher-3]: process started with pid [7864]
[INFO] [robot_state_publisher-4]: process started with pid [7866]
[INFO] [funmap-6]: process started with pid [7870]
[INFO] [grasp_object-7]: process started with pid [7872]
[INFO] [static_transform_publisher-8]: process started with pid [7874]
[static_transform_publisher-8] [INFO] [1743511485.229607637] [static_transform_publisher_mvr5SRaR1TUpXZS7]: Spinning until stopped - publishing transform
[static_transform_publisher-8] translation: ('0.000000', '0.000000', '0.000000')
[static_transform_publisher-8] rotation: ('0.500000', '0.500000', '-0.500000', '0.500000')
[static_transform_publisher-8] from 'link_straight_gripper' to 'link_straight_gripper_aligned'
[d435i_accel_correction-2] [INFO] [1743511485.751144308] [d435i_accel_correction_node]: d435i_accel_correction_node started
[realsense2_camera_node-1] [INFO] [1743511485.933741490] [camera]: RealSense ROS v4.55.1
[realsense2_camera_node-1] [INFO] [1743511485.933844475] [camera]: Built with LibRealSense v2.55.1
[realsense2_camera_node-1] [INFO] [1743511485.933855178] [camera]: Running with LibRealSense v2.55.1
[realsense2_camera_node-1] [INFO] [1743511486.041612379] [camera]: Device with serial number 250122075290 was found.
[realsense2_camera_node-1] 
[realsense2_camera_node-1] [INFO] [1743511486.041702444] [camera]: Device with physical ID /sys/devices/pci0000:00/0000:00:0d.0/usb2/2-1/2-1:1.0/video4linux/video0 was found.
[realsense2_camera_node-1] [INFO] [1743511486.041721637] [camera]: Device with name Intel RealSense D435IF was found.
[realsense2_camera_node-1] [INFO] [1743511486.041901836] [camera]: Device with port number 2-1 was found.
[realsense2_camera_node-1] [INFO] [1743511486.041925306] [camera]: Device USB type: 3.2
[realsense2_camera_node-1] [INFO] [1743511486.041938398] [camera]: Resetting device...
[grasp_object-7] [INFO] [1743511486.399886003] [stretch_funmap]: MoveBase __init__: self.debug_directory = None
[grasp_object-7] [INFO] [1743511486.442579906] [grasp_object]: grasp_object started
[stretch_driver-5] [INFO] [1743511486.574775608] [stretch_driver]: For use with S T R E T C H (TM) RESEARCH EDITION from Hello Robot Inc.
[stretch_driver-5] [INFO] [1743511486.575049888] [stretch_driver]: stretch_driver started
[funmap-6] [INFO] [1743511487.226339477] [funmap]: funmap started
[stretch_driver-5] Key mapped to End-Of-Arm Tool: eoa_wrist_dw3_tool_sg3
[stretch_driver-5] [INFO] [1743511488.297759503] [stretch_driver]: Changed to mode = position
[stretch_driver-5] [INFO] [1743511488.299451593] [stretch_driver]: broadcast_odom_tf = True
[stretch_driver-5] [INFO] [1743511488.327101227] [stretch_driver]: rate = 30.0 Hz
[stretch_driver-5] [INFO] [1743511488.330223024] [stretch_driver]: twist timeout = 0.5 s
[stretch_driver-5] [INFO] [1743511488.332149994] [stretch_driver]: base_frame_id = base_link
[stretch_driver-5] [INFO] [1743511488.333829564] [stretch_driver]: odom_frame_id = odom
[grasp_object-7] [INFO] [1743511488.483369271] [grasp_object]: Node grasp_object connected to /home_the_robot service.
[grasp_object-7] [INFO] [1743511488.484494359] [grasp_object]: Node grasp_object connected to /stow_the_robot service.
[grasp_object-7] [INFO] [1743511488.485868545] [grasp_object]: Node grasp_object connected to /stop_the_robot service.
[grasp_object-7] [INFO] [1743511488.487097863] [grasp_object]: Node grasp_object connected to /switch_to_trajectory_mode service.
[grasp_object-7] [INFO] [1743511488.492077098] [grasp_object]: Node grasp_object connected to /switch_to_position_mode service.
[grasp_object-7] [INFO] [1743511488.494246753] [grasp_object]: Node grasp_object connected to /switch_to_navigation_mode service.
[grasp_object-7] [INFO] [1743511488.494623514] [grasp_object]: Using the following directory for debugging files: /home/hello-robot/stretch_user/debug/
[grasp_object-7] [INFO] [1743511488.496999069] [grasp_object]: Waiting for /funmap/trigger_reach_until_contact' service
[funmap-6] [INFO] [1743511488.523573086] [funmap]: Node funmap connected to /home_the_robot service.
[funmap-6] [INFO] [1743511488.524851038] [funmap]: Node funmap connected to /stow_the_robot service.
[funmap-6] [INFO] [1743511488.528046096] [funmap]: Node funmap connected to /stop_the_robot service.
[funmap-6] [INFO] [1743511488.529353169] [funmap]: Node funmap connected to /switch_to_trajectory_mode service.
[funmap-6] [INFO] [1743511488.530427523] [funmap]: Node funmap connected to /switch_to_position_mode service.
[funmap-6] [INFO] [1743511488.531801789] [funmap]: Node funmap connected to /switch_to_navigation_mode service.
[funmap-6] [INFO] [1743511488.532064656] [funmap]: Node funmap waiting to receive first point cloud.
[realsense2_camera_node-1] [INFO] [1743511492.130483802] [camera]: Device with serial number 250122075290 was found.
[realsense2_camera_node-1] 
[realsense2_camera_node-1] [INFO] [1743511492.130549741] [camera]: Device with physical ID /sys/devices/pci0000:00/0000:00:0d.0/usb2/2-1/2-1:1.0/video4linux/video0 was found.
[realsense2_camera_node-1] [INFO] [1743511492.130563547] [camera]: Device with name Intel RealSense D435IF was found.
[realsense2_camera_node-1] [INFO] [1743511492.130775869] [camera]: Device with port number 2-1 was found.
[realsense2_camera_node-1] [INFO] [1743511492.130793232] [camera]: Device USB type: 3.2
[realsense2_camera_node-1] [INFO] [1743511492.130845786] [camera]: getParameters...
[realsense2_camera_node-1] [INFO] [1743511492.488820962] [camera]: JSON file is loaded! (/home/hello-robot/ament_ws/install/stretch_core/share/stretch_core/config/HighAccuracyPreset.json)
[realsense2_camera_node-1] [INFO] [1743511492.488893856] [camera]: Device Name: Intel RealSense D435IF
[realsense2_camera_node-1] [INFO] [1743511492.488903843] [camera]: Device Serial No: 250122075290
[realsense2_camera_node-1] [INFO] [1743511492.488912199] [camera]: Device physical port: /sys/devices/pci0000:00/0000:00:0d.0/usb2/2-1/2-1:1.0/video4linux/video0
[realsense2_camera_node-1] [INFO] [1743511492.488922692] [camera]: Device FW version: 5.16.0.1
[realsense2_camera_node-1] [INFO] [1743511492.488931295] [camera]: Device Product ID: 0x0B3A
[realsense2_camera_node-1] [INFO] [1743511492.488938916] [camera]: Sync Mode: On
[realsense2_camera_node-1] [WARN] [1743511492.627739438] [camera]: re-enable the stream for the change to take effect.
[realsense2_camera_node-1] [WARN] [1743511492.628136305] [camera]: re-enable the stream for the change to take effect.
[realsense2_camera_node-1] [WARN] [1743511492.633603953] [camera]: re-enable the stream for the change to take effect.
[realsense2_camera_node-1] [ERROR] [1743511492.634021138] [camera]: Given value, 63 is invalid. Set ROS param back to: 100
[realsense2_camera_node-1] [INFO] [1743511492.638032190] [camera]: Stopping Sensor: Depth Module
[realsense2_camera_node-1] [INFO] [1743511492.638097525] [camera]: Stopping Sensor: RGB Camera
[realsense2_camera_node-1] [INFO] [1743511492.638111434] [camera]: Stopping Sensor: Motion Module
[realsense2_camera_node-1] [INFO] [1743511492.649061431] [camera]: Starting Sensor: Depth Module
[realsense2_camera_node-1] [INFO] [1743511492.651996841] [camera]: Open profile: stream_type: Depth(0), Format: Z16, Width: 1280, Height: 720, FPS: 15
[realsense2_camera_node-1] [INFO] [1743511492.657391078] [camera]: Starting Sensor: RGB Camera
[realsense2_camera_node-1] [INFO] [1743511492.661364656] [camera]: Open profile: stream_type: Color(0), Format: RGB8, Width: 1280, Height: 720, FPS: 15
[realsense2_camera_node-1] [INFO] [1743511492.674872883] [camera]: Starting Sensor: Motion Module
[realsense2_camera_node-1] [INFO] [1743511492.676556168] [camera]: Open profile: stream_type: Accel(0)Format: MOTION_XYZ32F, FPS: 100
[realsense2_camera_node-1] [INFO] [1743511492.676589106] [camera]: Open profile: stream_type: Gyro(0)Format: MOTION_XYZ32F, FPS: 200
[realsense2_camera_node-1] [INFO] [1743511492.678411198] [camera]: RealSense Node Is Up!
[funmap-6] [INFO] [1743511493.440038403] [funmap]: Node funmap received first point cloud, so continuing.
[funmap-6] [INFO] [1743511493.465111959] [stretch_funmap]: MoveBase __init__: self.debug_directory = /home/hello-robot/stretch_user/debug/
[grasp_object-7] [INFO] [1743511493.506473186] [grasp_object]: Node grasp_object connected to /funmap/trigger_reach_until_contact.
[grasp_object-7] [INFO] [1743511493.508529158] [grasp_object]: Waiting for /funmap/trigger_lower_until_contact' service
[grasp_object-7] [INFO] [1743511493.509089139] [grasp_object]: Node grasp_object connected to /funmap/trigger_lower_until_contact.
[grasp_object-7] [INFO] [1743511493.509550982] [grasp_object]: Grasp object node is ready!
[realsense2_camera_node-1] [WARN] [1743511493.649552763] [camera]: XXX Hardware Notification:Motion Module failure,1.74351e+12,Error,Hardware Error
[realsense2_camera_node-1] [WARN] [1743511493.649620226] [camera]: Hardware Notification:Motion Module failure,1.74351e+12,Error,Hardware Error
[grasp_object-7] [INFO] [1743511497.515296336] [grasp_object]: Stow the arm.
[stretch_driver-5] [INFO] [1743511497.523686673] [stretch_driver]: Received stow_the_robot service call.
[stretch_driver-5] [INFO] [1743511497.524164554] [stretch_driver]: Changed to mode = stowing
[stretch_driver-5] --------- Stowing Arm ----
[stretch_driver-5] --------- Stowing eoa_wrist_dw3_tool_sg3 ----
[stretch_driver-5] --------- Stowing Lift ----
[stretch_driver-5] [INFO] [1743511505.241593372] [stretch_driver]: Changed to mode = position
[grasp_object-7] [INFO] [1743511505.324322919] [stretch_funmap]: ManipulationView __init__: self.debug_directory = /home/hello-robot/stretch_user/debug/
[stretch_driver-5] [INFO] [1743511505.329160291] [stretch_driver]: Received goal request
[stretch_driver-5] [INFO] [1743511505.333627950] [stretch_driver]: stretch_driver joint_traj action: New trajectory received with joint_names = ['joint_head_pan', 'joint_head_tilt']
[stretch_driver-5] -------------------------------------------
[stretch_driver-5] Finished goal
[stretch_driver-5] [INFO] [1743511506.208613293] [stretch_driver]: stretch_driver joint_traj action: Achieved all target points.
[grasp_object-7] [INFO] [1743511512.456261518] [stretch_funmap]: Found a grasp target
[grasp_object-7] [INFO] [1743511512.458599139] [stretch_funmap]: ManipulationView get_grasp_target : directory = /home/hello-robot/stretch_user/debug/get_grasp_target/
[grasp_object-7] [INFO] [1743511512.458839792] [stretch_funmap]: ManipulationView get_grasp_target : filename = grasp_target_20250401054512.png
[grasp_object-7] [INFO] [1743511512.463262659] [stretch_funmap]: object_planar_distance_m = 0.6029866060027534
[grasp_object-7] [INFO] [1743511512.463520012] [grasp_object]: come here********************************
[grasp_object-7] [INFO] [1743511512.464012466] [grasp_object]: Raise tool to pregrasp height.
[stretch_driver-5] [INFO] [1743511512.466833539] [stretch_driver]: Received goal request
[stretch_driver-5] [INFO] [1743511512.469649087] [stretch_driver]: stretch_driver joint_traj action: New trajectory received with joint_names = ['joint_lift']
[stretch_driver-5] -------------------------------------------
[stretch_driver-5] Finished goal
[stretch_driver-5] [INFO] [1743511519.537440508] [stretch_driver]: stretch_driver joint_traj action: Achieved all target points.
[grasp_object-7] Exception in thread Thread-1 (spin_thread):
[grasp_object-7] Traceback (most recent call last):
[grasp_object-7]   File "/usr/lib/python3.10/threading.py", line 1016, in _bootstrap_inner
[grasp_object-7]     self.run()
[grasp_object-7]   File "/usr/lib/python3.10/threading.py", line 953, in run
[grasp_object-7]     self._target(*self._args, **self._kwargs)
[grasp_object-7]   File "/home/hello-robot/ament_ws/install/hello_helpers/lib/python3.10/site-packages/hello_helpers/hello_misc.py", line 125, in spin_thread
[grasp_object-7]     executor.spin()
[grasp_object-7]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 294, in spin
[grasp_object-7]     self.spin_once()
[grasp_object-7]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 794, in spin_once
[grasp_object-7]     self._spin_once_impl(timeout_sec)
[grasp_object-7]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 791, in _spin_once_impl
[grasp_object-7]     future.result()
[grasp_object-7]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/task.py", line 94, in result
[grasp_object-7]     raise self.exception()
[grasp_object-7]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/task.py", line 239, in __call__
[grasp_object-7]     self._handler.send(None)
[grasp_object-7]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 437, in handler
[grasp_object-7]     await call_coroutine(entity, arg)
[grasp_object-7]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 391, in _execute_service
[grasp_object-7]     response = await await_or_execute(srv.callback, request, srv.srv_type.Response())
[grasp_object-7]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 107, in await_or_execute
[grasp_object-7]     return callback(*args)
[grasp_object-7]   File "/home/hello-robot/ament_ws/install/stretch_demos/lib/python3.10/site-packages/stretch_demos/grasp_object.py", line 126, in trigger_grasp_object_callback
[grasp_object-7]     pregrasp_yaw = self.manipulation_view.get_pregrasp_yaw(grasp_target, self.tf2_buffer)
[grasp_object-7]   File "/home/hello-robot/ament_ws/build/stretch_funmap/stretch_funmap/manipulation_planning.py", line 441, in get_pregrasp_yaw
[grasp_object-7]     gripper_frame = self.gripper_links[self.tool]
[grasp_object-7] KeyError: 'eoa_wrist_dw3_tool_sg3'
[grasp_object-7] MaxHeightImage information:
[grasp_object-7]      image.shape = (67, 25)
[grasp_object-7]      image.dtype = uint8
[grasp_object-7]      m_per_pix = 0.006
[grasp_object-7]      m_per_height_unit = 0.00027667984189723324
[grasp_object-7]      voi.x_in_m = 0.15
[grasp_object-7]      voi.y_in_m = 0.4
[grasp_object-7]      voi.z_in_m = 0.07
[grasp_object-7] MaxHeightImage information:
[grasp_object-7]      image.shape = (434, 350)
[grasp_object-7]      image.dtype = uint8
[grasp_object-7]      m_per_pix = 0.006
[grasp_object-7]      m_per_height_unit = 0.004566929133858267
[grasp_object-7]      voi.x_in_m = 2.1
[grasp_object-7]      voi.y_in_m = 2.6
[grasp_object-7]      voi.z_in_m = 1.16
[grasp_object-7] MaxHeightImage information:
[grasp_object-7]      image.shape = (334, 434)
[grasp_object-7]      image.dtype = uint8
[grasp_object-7]      m_per_pix = 0.006
[grasp_object-7]      m_per_height_unit = 0.004566929133858267
[grasp_object-7]      voi.x_in_m = 2.6
[grasp_object-7]      voi.y_in_m = 2.0
[grasp_object-7]      voi.z_in_m = 1.16
[grasp_object-7] MaxHeightImage saving to base_filename = /home/hello-robot/stretch_user/debug/grasp_object/look_at_surface_20250401054510
[grasp_object-7] Finished saving.
[grasp_object-7] segment_max_height_image.py : find_floor
[stretch_driver-5] New collision pair event: link_gripper_s3_body_TO_link_head_tilt [1743511519.661856]
[INFO] [grasp_object-7]: process has finished cleanly [pid 7872]

Hi @hhhh sorry for the delay in the response. This is due to this bug. We are workin on pushing the fix soon.

Thankyou for reporting.

Ok, thank you for your concern.

@hhhh Hey thankyou for your patience. This bug has been fixed through this PR.
You can pull the latest updates in the stretch_ros2 repository and rebuild the stretch_funmap package. Please let us know if your still facing issues.