7.6.1. grasppose_estimator.FindGrasppointServer

class grasppose_estimator.FindGrasppointServer(model_dir)

Bases: object

Computes the grasp_poses for the object to grasp based on annotated grasps.

The FindGrasppointServer uses the grasp checker to calculate the grasp poses for the object, based on grasp-annotations and the estimated object poses. The server returns the grasp poses, the bounding box of the object to grasp and the object name of the object to grasp. The server adds markers to the rviz visualization to show the grasp pose and the bounding box of the object.

models_metadata

The metadata of the known objects. Contains the bounding box information.

Type:: dict

server

This action server. Computes grasp poses for the object to grasp.

Type:: actionlib.SimpleActionServer

marker_pub

The publisher for the rviz markers.

Type:: rospy.Publisher

cam_info

The camera info message containing the camera parameters.

Type:: sensor_msgs.msg.CameraInfo

timeout

The timeout duration for setting up the action servers and waiting for results.

Type:: float

Parameters:

object_to_grasp (str) – The name of the object to grasp. If specified, the server will grasp this object. If not specified (empty string), the server will grasp the closest object.
object_poses (list of geometry_msgs.msg.Pose) – The poses of the objects detected by the pose estimator.
depth (sensor_msgs.msg.Image) – The depth image of the scene.
class_names (list of str) – The class names of the objects detected by the pose estimator.

Returns:

The result of the FindGrasppoint action server. Contains the grasp poses, the bounding box of the object and the object name.

Return type:

grasping_pipeline_msgs.msg.FindGrasppointResult

__init__(model_dir)

Initializes the FindGrasppointServer.

Loads the model metadata from the models_metadata.yml file in the model_dir. This file contains the bounding box information for known objects. The server then waits for the camera info message. After that it initializes the action server and the rviz marker publisher. Finally it starts the action server.

Parameters:: model_dir (str) – Path to the directory containing the model metadata file.

Methods

`__init__`(model_dir)	Initializes the FindGrasppointServer.
`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`(goal)	Executes the FindGrasppoint action server.
`get_bb_for_known_objects`(object_pose, ...)	Generates a bounding box for known objects based on the object pose and object name.
`get_closest_objects`(object_poses)	Returns a sorted list with indices of the closest objects.
`transform_to_kdl`(pose)	Converts a geometry_msgs.msg.Pose to a PyKDL.Frame.

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(goal)

Executes the FindGrasppoint action server.

If an object to grasp is specified in the goal, the server will estimate the grasp pose for the specified object. Otherwise, it will estimate the grasp pose for the closest object. The server uses the grasp checker which uses grasp-annotations to calculate grasp poses from the estimated object pose. In the end the server will add markers to the rviz visualization to show the grasp pose and the bounding box of the object.

Parameters:: goal (grasping_pipeline_msgs.msg.FindGrasppointGoal) – The goal of the FindGrasppoint action server. Contains the object to grasp.
Returns:: The result of the FindGrasppoint action server. Contains the grasp poses, the bounding box of the object and the object name.
Return type:: grasping_pipeline_msgs.msg.FindGrasppointResult

get_bb_for_known_objects(object_pose, object_name, frame_id, stamp)

Generates a bounding box for known objects based on the object pose and object name.

Loads the bounding box information for the object from the models_metadata.yml file.

Parameters:

object_pose (geometry_msgs.msg.PoseStamped) – The pose of the object.
object_name (str) – The name of the object. Used to look up the bounding box information.
frame_id (str) – The frame id of the object pose.
stamp (rospy.Time) – The timestamp of the object pose.

Returns:

The bounding box for the object with frame id and timestamp.

Return type:

grasping_pipeline_msgs.msg.BoundingBoxStamped

get_closest_objects(object_poses)

Returns a sorted list with indices of the closest objects. The closest object is at index 0.

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 indices of the closest objects in the estimator result.
Return type:: list of int

transform_to_kdl(pose)

Converts a geometry_msgs.msg.Pose to a PyKDL.Frame.

Parameters:: pose (geometry_msgs.msg.Pose) – The pose to convert.
Returns:: The converted pose.
Return type:: PyKDL.Frame