def set_object_pose(self, object_info):
add_object_request = AddObjectRequest()
add_object_request.object_identifier = object_info.name
add_object_request.class_identifier = object_info.object_class
add_object_request.pose = ROSUtils.toROSPose(object_info.pose,
bStamped=True)
response = ROSUtils.guarded_service_call(self._add_object_service,
add_object_request,
self._add_object_service_name)
if response is None:
return False
return response.success
def set_robot_pose(self, robot_info):
add_object_request = AddObjectRequest()
add_object_request.object_identifier = robot_info.name
add_object_request.class_identifier = robot_info.name
add_object_request.pose = ROSUtils.toROSPose(robot_info.pose,
bStamped=True)
response = ROSUtils.guarded_service_call(self._add_object_service,
add_object_request,
self._add_object_service_name)
if response is None:
return False
# TODO we should also set the configuration here
return response.success
def _call_hfts_planner(self, object_name, context):
# TODO set parameters
object_info = context.getSceneInformation().getObjectInfo(object_name)
hfts_planner_request = PlanGraspMotionRequest()
hfts_planner_request.object_identifier = object_name
hfts_planner_request.model_identifier = object_info.object_class
hfts_planner_request.object_pose = ROSUtils.toROSPose(object_info.pose,
bStamped=True)
start_config = context.getRobotInformation().configuration
hfts_planner_request.start_configuration = ROSUtils.toJointState(
start_config)
response = ROSUtils.guarded_service_call(
self._integrated_hfts_service, hfts_planner_request,
self._integrated_hfts_service_name)
if response is None or not response.planning_success:
return None, None
return response.trajectory, response.grasp_pose
def planArmTrajectory(self, goal, context, paramPrefix, parameters):
# If this function is called, it means we are only asked to plan an arm motion
send_parameters(self.getParameters('ArmPlanner', paramPrefix),
paramPrefix, parameters, self._parameter_client)
self._scene_interface.synchronize_scene(context)
plan_arm_request = PlanArmMotionRequest()
start_configuration = context.getRobotInformation().configuration
if type(goal) is not ContextPose:
raise NotImplementedError(
'[IntegratedHFTSPlanner::planArmTrajectory] Goals of type %s not supported.'
% str(type(goal)))
plan_arm_request.target_pose = ROSUtils.toROSPose(goal, bStamped=True)
plan_arm_request.start_configuration = ROSUtils.toJointState(
start_configuration)
plan_arm_request.grasped_object = context.getRobotInformation(
).grasped_object
response = ROSUtils.guarded_service_call(self._plan_arm_motion_service,
plan_arm_request,
self._arm_service_name)
if response is not None and response.planning_success:
return ROSUtils.toContextTrajectory(response.trajectory)
return None