7.2.1. direct_grasppose_estimator.DirectGraspposeEstimatorCaller

class direct_grasppose_estimator.DirectGraspposeEstimatorCaller

Bases: object

Calls a service that directly estimates the grasppose.

Calls a service that directly estimates the grasppose without needing an object pose and annotations. The topic of the service is defined in the parameter ‘/grasping_pipeline/grasppoint_estimator_topic’. The results are published as a marker in the rviz visualization with the topic ‘/grasping_pipeline/grasp_marker’.

If an object to grasp is specified, the service will only estimate the grasppose of the specified object. If no object is specified, the service will only estimate the grasppose of the closest object to the camera.

Parameters:

rgb (sensor_msgs.msg.Image) – The RGB image of the scene.
depth (sensor_msgs.msg.Image) – The depth image of the scene.
mask_detections (list of sensor_msgs.msg.Image) – The object masks.
bb_detections (list of sensor_msgs.msg.RegionOfInterest) – The 2D bounding boxes of the objects.
class_names (list of str) – The class names of the objects.
object_to_grasp (str) – The name of the object to grasp. If empty, the closest object to the camera is grasped.

bridge

The OpenCV bridge to convert images.

Type:: CvBridge

srv

This service. Calls the direct grasppose estimator when called.

Type:: rospy.Service

cam_info

The camera information/intrinsics.

Type:: sensor_msgs.msg.CameraInfo

marker_pub

The publisher that publishes the markers in the rviz visualization.

Type:: rospy.Publisher

Raises:

rospy.ServiceException – If the direct grasppose estimator service times out or fails to estimate the grasppose or if no mask or bounding box detections are provided.

Returns:

grasp_poses (list of geometry_msgs.msg.PoseStamped) – The estimated graspposes.
grasp_object_bb (grasping_pipeline_msgs.msg.BoundingBoxStamped) – The bounding box of the object to grasp.
grasp_object_name (str) – The name of the object to grasp.

__init__()

Methods

`__init__`()
`add_bb_marker`(object_bb_stamped)	Adds a marker to the rviz visualization to show the bounding box of the object.
`add_marker`(pose_goal)	Adds a marker to the rviz visualization to show the grasp pose.
`execute`(req)	Calls the direct grasppose estimator service and returns the grasppose.
`get_3D_bbs`(depth, masks, bbs_2d)	Extracts the 3D bounding boxes of the objects.
`get_bb_3D_from_bb`(depth, depth_np, bb_2d)	Extracts the 3D bounding box of the object from the 2D bounding box.
`get_bb_3D_from_mask`(depth, depth_np, mask)	Extracts the 3D bounding box of the object from the mask.
`get_bb_center_poses`(bbs)	Extracts the center poses of the bounding boxes.
`get_closest_object`(object_poses)	Returns the index of the closest object in the estimator result.

add_bb_marker(object_bb_stamped)

Adds a marker to the rviz visualization to show the bounding box of the object.

The marker is a blue bounding box. The topic of the marker is ‘/grasping_pipeline/grasp_marker’. The marker is published in the namespace ‘bb_marker’ with id 0.

Parameters:: object_bb_stamped (grasping_pipeline_msgs.msg.BoundingBoxStamped) – The bounding box of the object.

add_marker(pose_goal)

Adds a marker to the rviz visualization to show the grasp pose.

The marker is an arrow pointing in the direction of the grasp pose. The arrow is red. The topic of the marker is ‘/grasping_pipeline/grasp_marker’. The marker is published in the namespace ‘grasp_marker’ with id 0.

Parameters:: pose_goal (geometry_msgs.msg.PoseStamped) – The pose of the grasp.

execute(req)

Calls the direct grasppose estimator service and returns the grasppose.

If an object to grasp is specified, the service will only estimate the grasppose of the specified object. If no object is specified, the service will only estimate the grasppose of the closest object to the camera.

If no mask or bounding box detections are provided, the service will raise an error.

Parameters:: req (grasping_pipeline_msgs.srv.CallDirectGraspPoseEstimatorRequest) – The request to the service. Contains the RGB image, depth image, mask detections, bounding box detections, class names, and (optionally) the name of the object to grasp.
Raises:: rospy.ServiceException – If the direct grasppose estimator service times out or fails to estimate the grasppose or if no mask or bounding box detections are provided.
Returns:: The response of the service. Contains the name of the object to grasp, the bounding box of the object, and the estimated graspposes.
Return type:: grasping_pipeline_msgs.srv.CallDirectGraspPoseEstimatorResponse

get_3D_bbs(depth, masks, bbs_2d)

Extracts the 3D bounding boxes of the objects.

If masks are provided, the 3D bounding boxes are extracted from the masks and the depth image. Otherwise, if 2D bounding boxes are provided, the 3D bounding boxes are extracted from the 2D bounding boxes. If neither masks nor 2D bounding boxes are provided, an error is raised.

Parameters:

depth (sensor_msgs.msg.Image) – The depth image.
masks (list of sensor_msgs.msg.Image) – The object masks.
bbs_2d (list of sensor_msgs.msg.RegionOfInterest) – The 2D bounding boxes of the objects.

get_bb_3D_from_bb(depth, depth_np, bb_2d)

Extracts the 3D bounding box of the object from the 2D bounding box.

Parameters:

depth (sensor_msgs.msg.Image) – The depth image.
depth_np (numpy.ndarray) – The depth image as a numpy array.
bb_2d (sensor_msgs.msg.RegionOfInterest) – The 2D bounding box of the object.

get_bb_3D_from_mask(depth, depth_np, mask)

Extracts the 3D bounding box of the object from the mask.

Parameters:

depth (sensor_msgs.msg.Image) – The depth image.
depth_np (numpy.ndarray) – The depth image as a numpy array.
mask (sensor_msgs.msg.Image) – The object mask.

Returns:

The 3D bounding box of the object.

Return type:

grasping_pipeline_msgs.msg.BoundingBoxStamped

get_bb_center_poses(bbs)

Extracts the center poses of the bounding boxes.

Parameters:: bbs (list of) – A list of bounding boxes.
Returns:: A list of the center poses of the bounding boxes.
Return type:: list

get_closest_object(object_poses)

Returns the index of the closest object in the estimator result.

The closest object is determined by the distance to the camera. The distance is calculated as the euclidean distance from the camera to the object pose.

Parameters:: estimator_result (GenericImgProcAnnotatorResult) – The result of the pose estimator.
Raises:: ValueError – If no close object is found.
Returns:: The index of the closest object in the estimator result.
Return type:: int