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]