def get_cam_pose(self, height, rotation, left=True):
fbp = FullBodyPosture()
fbp.type = fbp.CAMERA
fbp.camera_pos = self.height_to_cam_pose(height)
if left:
t_base_footprint___camera = PyKDL.Frame(
PyKDL.Rotation(1, 0, 0, 0, 0, 1, 0, -1, 0))
else:
t_base_footprint___camera = PyKDL.Frame(
PyKDL.Rotation(-1, 0, 0, 0, 0, -1, 0, -1, 0))
t_camera___camera_goal = PyKDL.Frame(PyKDL.Rotation.RotX(rotation))
t_base_footprint___camera_goal = t_base_footprint___camera * t_camera___camera_goal
fbp.camera_pos.pose.orientation = kdl_to_pose(
t_base_footprint___camera_goal).orientation
# fbp.camera_pos.pose.orientation = transform_pose('camera_link', fbp.camera_pos).pose.orientation
return fbp
def query_detect_shelf_layers_path_cb(self, data):
"""
:type data: QueryDetectShelfLayersPathRequest
:rtype: QueryDetectShelfLayersPathResponse
"""
self.print_with_prefix('called', 'query_detect_shelf_layers_path')
r = QueryDetectShelfLayersPathResponse()
if data.id in self.shelf_ids:
r.error = QueryDetectShelfLayersPathResponse.SUCCESS
base_pose = PoseStamped()
base_pose.header.frame_id = '/map'
base_pose.pose.position = Point(
rospy.get_param('shelf_detection_path/position/x'),
rospy.get_param('shelf_detection_path/position/y'),
rospy.get_param('shelf_detection_path/position/z'))
base_pose.pose.orientation = Quaternion(
rospy.get_param('shelf_detection_path/orientation/x'),
rospy.get_param('shelf_detection_path/orientation/y'),
rospy.get_param('shelf_detection_path/orientation/z'),
rospy.get_param('shelf_detection_path/orientation/w'))
for k in range(5):
posture = FullBodyPosture()
posture.base_pos = base_pose
posture.joints.append(
JointPosition(
'Torso_lin1',
rospy.get_param('shelf_detection_path/Torso_lin1')))
posture.joints.append(
JointPosition(
'Torso_lin2',
rospy.get_param('shelf_detection_path/Torso_lin2/gain')
* k))
posture.joints.append(
JointPosition(
'Torso_rot_1',
rospy.get_param('shelf_detection_path/Torso_rot_1')))
r.path.postures.append(posture)
else:
r.error = QueryDetectShelfLayersPathResponse.INVALID_ID
return r
def get_detect_shelf_layers_path(self, shelf_system_id):
"""
:type shelf_system_id: str
:rtype: FullBodyPath
"""
shelf_system_width = self.knowrob.get_shelf_layer_width(
shelf_system_id)
# shelf_system_height = self.knowrob.get_shelf_system_height(shelf_system_id)
full_body_path = FullBodyPath()
# via points
for via_point in self.get_via_points(shelf_system_id):
full_body_pose = FullBodyPosture()
full_body_pose.base_pos = via_point
full_body_pose.type = FullBodyPosture.BASE
full_body_path.postures.append(full_body_pose)
# calculate base pose
base_pose = self.base_pose_in_front_of_shelf(shelf_system_id,
shelf_system_width / 2,
y=-0.85)
base_pose = transform_pose(
self.knowrob.get_perceived_frame_id(shelf_system_id), base_pose)
if self.is_left(shelf_system_id):
base_pose.pose.position.x += 0.5
else:
base_pose.pose.position.x -= 0.5
base_pose = transform_pose('map', base_pose)
full_body_pose = FullBodyPosture()
full_body_pose.base_pos = base_pose
full_body_pose.type = FullBodyPosture.BASE
full_body_path.postures.append(full_body_pose)
full_body_pose = FullBodyPosture()
if self.is_left(shelf_system_id):
full_body_pose.goal_joint_state = self.get_floor_detection_pose_left(
)
else:
full_body_pose.goal_joint_state = self.get_floor_detection_pose_right(
)
full_body_pose.type = FullBodyPosture.JOINT
full_body_path.postures.append(full_body_pose)
full_body_pose = FullBodyPosture()
full_body_pose.base_pos = self.cam_pose_in_front_of_shelf(
shelf_system_id, x=shelf_system_width / 2, y=-0.55)
full_body_pose.type = FullBodyPosture.CAM_FOOTPRINT
full_body_path.postures.append(full_body_pose)
full_body_pose = FullBodyPosture()
full_body_pose.type = FullBodyPosture.CAMERA
full_body_pose.camera_pos.header.frame_id = 'camera_link'
full_body_pose.camera_pos.pose.position = Point(0, 1.364, 0)
full_body_pose.camera_pos.pose.orientation = Quaternion(0, 0, 0, 1)
full_body_path.postures.append(full_body_pose)
return full_body_path
def get_detect_facings_path(self, shelf_layer_id):
"""
:type shelf_layer_id: str
:rtype: FullBodyPath
"""
shelf_system_id = self.knowrob.get_shelf_system_from_layer(
shelf_layer_id)
# shelf_system_frame_id = self.knowrob.get_perceived_frame_id(shelf_system_id)
shelf_system_width = self.knowrob.get_shelf_system_width(
shelf_system_id)
shelf_layer_frame_id = self.knowrob.get_perceived_frame_id(
shelf_layer_id)
full_body_path = FullBodyPath()
# TODO consider left right cases
# joints
# TODO layer above
shelf_layer_height = lookup_pose('map',
shelf_layer_frame_id).pose.position.z
# TODO tune this number
to_low_offset = 0.05
other_offset = 0.05
full_body_pose = FullBodyPosture()
full_body_pose.type = FullBodyPosture.CAMERA
if self.layer_too_low(shelf_layer_height):
goal_angle = radians(-20)
full_body_pose = self.get_cam_pose(
shelf_layer_height + to_low_offset, goal_angle,
self.is_left(shelf_system_id))
else:
goal_angle = radians(0)
full_body_pose = self.get_cam_pose(
shelf_layer_height + other_offset, goal_angle,
self.is_left(shelf_system_id))
full_body_path.postures.append(full_body_pose)
# base poses
# start base poses
start_base_pose = self.cam_pose_in_front_of_layer(
shelf_layer_id, goal_angle=goal_angle)
start_base_pose = transform_pose('map', start_base_pose)
start_full_body_pose = FullBodyPosture()
start_full_body_pose.type = FullBodyPosture.CAM_FOOTPRINT
start_full_body_pose.base_pos = start_base_pose
end_base_pose = self.cam_pose_in_front_of_layer(shelf_layer_id,
x=shelf_system_width,
goal_angle=goal_angle)
end_base_pose = transform_pose('map', end_base_pose)
end_full_body_pose = FullBodyPosture()
end_full_body_pose.type = FullBodyPosture.CAM_FOOTPRINT
end_full_body_pose.base_pos = end_base_pose
if self.is_left(shelf_system_id):
full_body_path.postures.append(end_full_body_pose)
full_body_path.postures.append(start_full_body_pose)
else:
full_body_path.postures.append(start_full_body_pose)
full_body_path.postures.append(end_full_body_pose)
return full_body_path
def query_count_products_posture_cb(self, data):
"""
:type data: QueryCountProductsPostureRequest
:rtype: QueryCountProductsPostureResponse
"""
self.print_with_prefix('called', 'query_count_products_posture')
r = QueryCountProductsPostureResponse()
if data.id in self.facing_ids:
r.error = QueryCountProductsPostureResponse.SUCCESS
if data.id == 'facing_92c6c2':
r.posture = FullBodyPosture()
r.posture.base_pos.header.frame_id = '/map'
r.posture.base_pos.pose.position = Point(
rospy.get_param(
'count_products_posture/facing_92c6c2/position/x'),
rospy.get_param(
'count_products_posture/facing_92c6c2/position/y'),
rospy.get_param(
'count_products_posture/facing_92c6c2/position/z'))
r.posture.base_pos.pose.orientation = Quaternion(
rospy.get_param(
'count_products_posture/facing_92c6c2/orientation/x'),
rospy.get_param(
'count_products_posture/facing_92c6c2/orientation/y'),
rospy.get_param(
'count_products_posture/facing_92c6c2/orientation/z'),
rospy.get_param(
'count_products_posture/facing_92c6c2/orientation/w'))
r.posture.joints.append(
JointPosition(
'Torso_lin1',
rospy.get_param(
'count_products_posture/facing_92c6c2/Torso_lin1'))
)
r.posture.joints.append(
JointPosition(
'Torso_lin2',
rospy.get_param(
'count_products_posture/facing_92c6c2/Torso_lin2'))
)
r.posture.joints.append(
JointPosition(
'Torso_rot_1',
rospy.get_param(
'count_products_posture/facing_92c6c2/Torso_rot_1')
))
elif data.id == 'facing_77bce2':
r.posture = FullBodyPosture()
r.posture.base_pos.header.frame_id = '/map'
r.posture.base_pos.pose.position = Point(
rospy.get_param(
'count_products_posture/facing_77bce2/position/x'),
rospy.get_param(
'count_products_posture/facing_77bce2/position/y'),
rospy.get_param(
'count_products_posture/facing_77bce2/position/z'))
r.posture.base_pos.pose.orientation = Quaternion(
rospy.get_param(
'count_products_posture/facing_77bce2/orientation/x'),
rospy.get_param(
'count_products_posture/facing_77bce2/orientation/y'),
rospy.get_param(
'count_products_posture/facing_77bce2/orientation/z'),
rospy.get_param(
'count_products_posture/facing_77bce2/orientation/w'))
r.posture.joints.append(
JointPosition(
'Torso_lin1',
rospy.get_param(
'count_products_posture/facing_77bce2/Torso_lin1'))
)
r.posture.joints.append(
JointPosition(
'Torso_lin2',
rospy.get_param(
'count_products_posture/facing_77bce2/Torso_lin2'))
)
r.posture.joints.append(
JointPosition(
'Torso_rot_1',
rospy.get_param(
'count_products_posture/facing_77bce2/Torso_rot_1')
))
elif data.id == 'facing_c4a62e':
r.posture = FullBodyPosture()
r.posture.base_pos.header.frame_id = '/map'
r.posture.base_pos.pose.position = Point(
rospy.get_param(
'count_products_posture/facing_c4a62e/position/x'),
rospy.get_param(
'count_products_posture/facing_c4a62e/position/y'),
rospy.get_param(
'count_products_posture/facing_c4a62e/position/z'))
r.posture.base_pos.pose.orientation = Quaternion(
rospy.get_param(
'count_products_posture/facing_c4a62e/orientation/x'),
rospy.get_param(
'count_products_posture/facing_c4a62e/orientation/y'),
rospy.get_param(
'count_products_posture/facing_c4a62e/orientation/z'),
rospy.get_param(
'count_products_posture/facing_c4a62e/orientation/w'))
r.posture.joints.append(
JointPosition(
'Torso_lin1',
rospy.get_param(
'count_products_posture/facing_c4a62e/Torso_lin1'))
)
r.posture.joints.append(
JointPosition(
'Torso_lin2',
rospy.get_param(
'count_products_posture/facing_c4a62e/Torso_lin2'))
)
r.posture.joints.append(
JointPosition(
'Torso_rot_1',
rospy.get_param(
'count_products_posture/facing_c4a62e/Torso_rot_1')
))
else:
r.error = QueryCountProductsPostureResponse.INVALID_ID
return r
def query_detect_facings_path_cb(self, data):
"""
:type data: QueryDetectFacingsPathRequest
:rtype: QueryDetectFacingsPathResponse
"""
self.print_with_prefix('called', 'query_detect_facings_path')
r = QueryDetectFacingsPathResponse()
if data.id in self.layer_ids:
r.error = QueryDetectFacingsPathResponse.SUCCESS
# Start
posture = FullBodyPosture()
posture.base_pos.header.frame_id = '/map'
posture.base_pos.pose.position = Point(
rospy.get_param('detect_facing_path/start/position/x'),
rospy.get_param('detect_facing_path/start/position/y'),
rospy.get_param('detect_facing_path/start/position/z'))
posture.base_pos.pose.orientation = Quaternion(
rospy.get_param('detect_facing_path/start/orientation/x'),
rospy.get_param('detect_facing_path/start/orientation/y'),
rospy.get_param('detect_facing_path/start/orientation/z'),
rospy.get_param('detect_facing_path/start/orientation/w'))
posture.joints.append(
JointPosition(
'Torso_lin1',
rospy.get_param('detect_facing_path/start/Torso_lin1')))
posture.joints.append(
JointPosition(
'Torso_lin2',
rospy.get_param('detect_facing_path/start/Torso_lin2')))
posture.joints.append(
JointPosition(
'Torso_rot_1',
rospy.get_param('detect_facing_path/start/Torso_rot_1')))
r.path.postures.append(posture)
# End
posture = FullBodyPosture()
posture.base_pos.header.frame_id = '/map'
posture.base_pos.pose.position = Point(
rospy.get_param('detect_facing_path/end/position/x'),
rospy.get_param('detect_facing_path/end/position/y'),
rospy.get_param('detect_facing_path/end/position/z'))
posture.base_pos.pose.orientation = Quaternion(
rospy.get_param('detect_facing_path/end/orientation/x'),
rospy.get_param('detect_facing_path/end/orientation/y'),
rospy.get_param('detect_facing_path/end/orientation/z'),
rospy.get_param('detect_facing_path/end/orientation/w'))
posture.joints.append(
JointPosition(
'Torso_lin1',
rospy.get_param('detect_facing_path/end/Torso_lin1')))
posture.joints.append(
JointPosition(
'Torso_lin2',
rospy.get_param('detect_facing_path/end/Torso_lin2')))
posture.joints.append(
JointPosition(
'Torso_rot_1',
rospy.get_param('detect_facing_path/end/Torso_rot_1')))
r.path.postures.append(posture)
else:
r.error = QueryDetectFacingsPathResponse.INVALID_ID
return r