def __init__(self, service_topic, control_manager_diagnostics_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceGoToState,
self).__init__(input_keys=['goal'],
outcomes=['successed', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
self._control_manager_diagnostics_topic = rospy.resolve_name(
control_manager_diagnostics_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: mrs_msgs.srv.Vec4})
(msg_path, msg_topic,
fn) = rostopic.get_topic_type(self._control_manager_diagnostics_topic)
if msg_topic == self._control_manager_diagnostics_topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_cont_diag = ProxySubscriberCached(
{self._control_manager_diagnostics_topic: msg_type})
self._connected = True
Logger.loginfo(
'[ServiceGoToState]: Successfully subscribed to topic %s' %
self._control_manager_diagnostics_topic)
self._failed = False
def __init__(self):
'''Constructor'''
super(AttachObjectState,
self).__init__(outcomes=['done', 'failed'],
input_keys=['template_id', 'hand_side'],
output_keys=['template_pose'])
# Set up service for attaching object template
self._service_topic = "/stitch_object_template"
self._srv = ProxyServiceCaller(
{self._service_topic: SetAttachedObjectTemplate})
# Set up subscribers for listening to the latest wrist poses
self._wrist_pose_topics = dict()
self._wrist_pose_topics['left'] = '/flor/l_arm_current_pose'
self._wrist_pose_topics['right'] = '/flor/r_arm_current_pose'
self._sub = ProxySubscriberCached({
self._wrist_pose_topics['left']:
PoseStamped,
self._wrist_pose_topics['right']:
PoseStamped
})
self._sub.make_persistant(self._wrist_pose_topics['left'])
self._sub.make_persistant(self._wrist_pose_topics['right'])
self._failed = False
def __init__(self, plan_keeper_wall_definition_in_service_topic,
plan_keeper_building_rules_in_service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceSetWallDefinitionAndConstraintsIntoPlanKeeper,
self).__init__(
input_keys=['brick_rules', 'brick_positions', 'brick_ids'],
outcomes=['successed', 'failed'])
# Store state parameter for later use.
self._plan_keeper_wall_definition_in_service_topic = rospy.resolve_name(
plan_keeper_wall_definition_in_service_topic)
self._plan_keeper_building_rules_in_service_topic = rospy.resolve_name(
plan_keeper_building_rules_in_service_topic)
# Create proxy service client
self._srv_wall_definition = ProxyServiceCaller({
self._plan_keeper_wall_definition_in_service_topic:
plan_keeper.srv.SetWallDefinition
})
self._srv_building_rules = ProxyServiceCaller({
self._plan_keeper_building_rules_in_service_topic:
plan_keeper.srv.SetBuildingRules
})
self._failed = False
def __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceSetPlanIntoPlanKeeper, self).__init__(input_keys=['plans'],
outcomes = ['successed', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
# Create proxy service client
self._srv = ProxyServiceCaller({self._service_topic: plan_keeper.srv.RobotPlan})
self._failed = False
def __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceResetMapState,
self).__init__(outcomes=['finished', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: std_srvs.srv.Trigger})
self._failed = False
def __init__(self, request):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(MavrosArmState, self).__init__(outcomes=['finished', 'failed'])
# Store state parameter for later use.
self._request = request
self._service_topic = rospy.resolve_name("mavros/cmd/arming")
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: mavros_msgs.srv.CommandBool})
self._failed = False
def __init__(self):
'''Constructor'''
super(GetTemplatePoseState,
self).__init__(outcomes=['done', 'failed'],
input_keys=['template_id'],
output_keys=['template_pose'])
self._service_topic = "/template_info"
self._srv = ProxyServiceCaller(
{self._service_topic: GetTemplateStateAndTypeInfo})
self._srv_result = None
self._failed = False
def __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceIsBrickPlaceable, self).__init__(input_keys=['brick_id'],
output_keys=[],
outcomes = ['placeable', 'wait', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
# Create proxy service client
self._srv = ProxyServiceCaller({self._service_topic: plan_keeper.srv.IsBrickPlaceable})
self._failed = False
self._placeable = False
def __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceGetWallBrickPositionsState,
self).__init__(outcomes=['finished', 'failed'],
output_keys=['mapped_objects'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: brick_mapping.srv.GetMappedArenaObjects})
self._failed = False
def __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceSetBrickPlacedIntoPlanKeeper, self).__init__(input_keys=['brick_id','placing_result'],
outcomes = ['successed', 'failed'])
# Store state parameter for later use.
self._service_topic = service_topic
# Create proxy service client
self._srv = ProxyServiceCaller({self._service_topic: plan_keeper.srv.BrickResult})
self._failed = False
Logger.loginfo("[ServiceSetBrickPlacedIntoPlanKeeper]: init with service_topic %s"%(str(self._service_topic)))
def __init__(self):
'''Constructor'''
super(GetTemplateGraspState, self).__init__(outcomes = ['done', 'failed', 'not_available'],
input_keys = ['template_id', 'hand_side', 'preference'],
output_keys = ['grasp'])
self._service_topic = "/template_info"
self._srv = ProxyServiceCaller({self._service_topic: GetTemplateStateAndTypeInfo})
self._srv_result = None
self._hand_side = None
self._failed = False
self._done = False
def __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceGetExistingCtopPlan,
self).__init__(input_keys=[],
output_keys=['plans'],
outcomes=['successed', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: ctop_planner.srv.GetExistingPlanCTOP})
self._failed = False
def __init__(self, usability_name='', usability_id=100):
'''Constructor'''
super(GetTemplateUsabilityState,
self).__init__(outcomes=['done', 'failed'],
input_keys=['template_id'],
output_keys=['usability_pose'])
# Set up service for attaching object template
self._service_topic = "/usability_pose"
self._srv = ProxyServiceCaller(
{self._service_topic: GetUsabilityInWristFrame})
self._usability_id = usability_id
self._usability_name = usability_name
self._failed = False
def __init__(self, target_frame):
'''Constructor'''
super(GetPoseInFrameState, self).__init__(outcomes=['done', 'failed'],
input_keys=['pose_in'],
output_keys=['pose_out'])
self._target_frame = target_frame
# Set up service for requesting the transformation
self._service_topic = '/worldmodel_main/get_pose_in_frame_service'
self._srv = ProxyServiceCaller({self._service_topic: GetPoseInFrame})
self._srv_result = None
self._failed = False
self._done = False
def __init__(self, set_yaw_service_topic,
control_manager_diagnostics_topic, odometry_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceRotateRelativeToGoToState,
self).__init__(input_keys=[
'goal_tracker_point', 'frame_id', 'yaw_relative_to_goal'
],
output_keys=['desired_yaw'],
outcomes=['successed', 'failed'])
# Store state parameter for later use.
self._set_yaw_service_topic = rospy.resolve_name(set_yaw_service_topic)
self._control_manager_diagnostics_topic = rospy.resolve_name(
control_manager_diagnostics_topic)
self._odometry_topic = rospy.resolve_name(odometry_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._set_yaw_service_topic: mrs_msgs.srv.Vec1})
(msg_path, msg_topic,
fn) = rostopic.get_topic_type(self._control_manager_diagnostics_topic)
if msg_topic == self._control_manager_diagnostics_topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_cont_diag = ProxySubscriberCached(
{self._control_manager_diagnostics_topic: msg_type})
self._connected = True
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: Successfully subscribed to topic %s'
% self._control_manager_diagnostics_topic)
(msg_path_odom, msg_topic_odom,
fn_odom) = rostopic.get_topic_type(self._odometry_topic)
if msg_topic_odom == self._odometry_topic:
msg_type_odom = self._get_msg_from_path(msg_path_odom)
self._sub_odom = ProxySubscriberCached(
{self._odometry_topic: msg_type_odom})
self._connected_odom = True
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: Successfully subscribed to topic %s'
% self._odometry_topic)
self._desired_yaw = 0
self._sended = False
self._failed = False
def __init__(self, planning_group):
'''
Constructor
'''
super(CurrentJointPositionsState,
self).__init__(outcomes=['retrieved', 'failed'],
output_keys=['joint_positions'])
self._srv_topic = '/flor/planning/upper_body/get_group_state'
self._srv = ProxyServiceCaller(
{self._srv_topic: GetCurrentPlanningGroupState})
self._planning_group = planning_group
self._srv_result = None
self._failed = False
def __init__(self, model_name):
'''Constructor'''
super(AddGazeboModel, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['gazebo_model_pose'])
self._srv_result = None
self._service_name = '/gazebo/spawn_urdf_model'
self._failed = False
self._done = False
self._srv = ProxyServiceCaller({self._service_name:SpawnModel})
self._model_reference_frame = 'base'
self._gazebo_model_pose = None
self._model_name = model_name
def __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceGetCtopPlan,
self).__init__(input_keys=[
'num_robots', 'remaining_time_s', 'max_planning_time_s',
'times_finish_actual_s', 'builded_brick_ids'
],
output_keys=['plans'],
outcomes=['successed', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: ctop_planner.srv.PlanCTOP})
self._failed = False
def __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceScanArena, self).__init__(
input_keys=[
'scanning_robots', 'scanning_altitude', 'scanning_speed',
'scanning_max_acc', 'scanning_turning_speed', 'robot_id'
],
output_keys=['scanning_trajectory', 'scanning_trajectory_start'],
outcomes=['successed', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: arena_scan_planner.srv.ArenaScanTrajectory})
self._failed = False
def __init__(self, service_topic, msg_text, state_name=None):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceStringState,
self).__init__(outcomes=['finished', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
if state_name is None:
self._state_name = ""
else:
self._state_name = '[{}]: '.format(state_name)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: mrs_msgs.srv.String})
self._msg_text = msg_text
self._failed = False
def __init__(self, service_name):
'''Constructor'''
super(CallBirlService, self).__init__(outcomes = ['done', 'failed'])
self._srv_result = None
self._service_name = service_name
self._hand_side = None
self._failed = False
self._done = False
self._srv = ProxyServiceCaller({self._service_name:Add_Gazebo_Model})
self._model_reference_frame = None
self._pose = None
self._model_name = None
def __init__(self):
'''Constructor'''
super(GetTemplatePoseState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['template_id'],
output_keys = ['template_pose'])
self._service_topic = "/template_info"
self._srv = ProxyServiceCaller({self._service_topic: GetTemplateStateAndTypeInfo})
self._srv_result = None
self._failed = False
def __init__(self, service_topic_see_type, service_topic_layer):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceSetBrickDetectionTypeAndWallLayer,
self).__init__(input_keys=['see_type', 'wall_layer'],
outcomes=['successed', 'failed'])
# Store state parameter for later use.
self._service_topic_see_type = rospy.resolve_name(
service_topic_see_type)
self._service_topic_layer = rospy.resolve_name(service_topic_layer)
# Create proxy service client
wait_time = 30
rospy.loginfo("waiting for service " + self._service_topic_see_type)
rospy.wait_for_service(self._service_topic_see_type, wait_time)
self._srv_detection_type = ProxyServiceCaller(
{self._service_topic_see_type: mbzirc_msgs.srv.DetectionType})
self._srv_layer = ProxyServiceCaller(
{self._service_topic_layer: mbzirc_msgs.srv.DetectionType})
self._failed = False
class GetTemplatePoseState(EventState):
'''
Requests the pose of a template from the template server.
># template_id int ID of the template to get the pose for.
#> grasp PoseStamped The current pose of the template.
<= done Pose data is available.
<= failed Failed to get pose information.
'''
def __init__(self):
'''Constructor'''
super(GetTemplatePoseState,
self).__init__(outcomes=['done', 'failed'],
input_keys=['template_id'],
output_keys=['template_pose'])
self._service_topic = "/template_info"
self._srv = ProxyServiceCaller(
{self._service_topic: GetTemplateStateAndTypeInfo})
self._srv_result = None
self._failed = False
def execute(self, userdata):
if self._failed or self._srv_result is None:
return 'failed'
try:
userdata.template_pose = self._srv_result.template_state_information.pose
except Exception as e:
Logger.logwarn(
'Failed to retrieve pose information from service result:\n%s'
% str(e))
return 'failed'
return 'done'
def on_enter(self, userdata):
try:
self._srv_result = self._srv.call(
self._service_topic,
GetTemplateStateAndTypeInfoRequest(
userdata.template_id,
0)) # We don't care about the hand side
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
def __init__(self, service_topic, flying_altitude):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceGetNextBrickInPlan,
self).__init__(output_keys=[
'grasp_position', 'placing_position', 'drop_wait_position',
'next_to_wall_wait_position', 'brick_type',
'brick_see_wall_typed', 'brick_id', 'wall_layer'
],
outcomes=['successed', 'finished', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
self._robot_name = rospy.get_namespace()[
1:-1] # removing backslash from beggining and end
self._flying_altitude = flying_altitude
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: plan_keeper.srv.BrickPlan})
self._failed = False
self._finished = False
def __init__(self, service_topic, brick_estimation_closest_wall_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(SpiralAroundPointState, self).__init__(
input_keys=['start_spiral_tracker_point', 'frame_id'],
outcomes=['successed', 'failed', 'over_time'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
self._closest_wall_topic = rospy.resolve_name(
brick_estimation_closest_wall_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: mrs_msgs.srv.ReferenceStampedSrv})
self._sub = rospy.Subscriber(self._closest_wall_topic,
mbzirc_msgs.msg.MbzircBrick,
self.nearest_wall_callback)
self._start_point = None
self._spiral_points = None
self._failed = False
self.received = False
def __init__(self, service_topic_follow,service_topic_set_trajectory,control_manager_diagnostics_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceFollowTrajectory, self).__init__(input_keys=['scanning_trajectory','frame_id'],
outcomes = ['successed', 'failed'])
# Store state parameter for later use.
self._service_topic_follow = rospy.resolve_name(service_topic_follow)
self._service_topic_set_trajectory = rospy.resolve_name(service_topic_set_trajectory)
self._control_manager_diagnostics_topic = rospy.resolve_name(control_manager_diagnostics_topic)
# Create proxy service client
self._srv_follow = ProxyServiceCaller({self._service_topic_follow: std_srvs.srv.Trigger})
self._srv_set_trajectory = ProxyServiceCaller({self._service_topic_set_trajectory: mrs_msgs.srv.TrajectoryReferenceSrv})
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._control_manager_diagnostics_topic)
if msg_topic == self._control_manager_diagnostics_topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_cont_diag = ProxySubscriberCached({self._control_manager_diagnostics_topic: msg_type})
self._connected = True
Logger.loginfo('[ServiceFollowTrajectory]: Successfully subscribed to topic %s' % self._control_manager_diagnostics_topic)
self._failed = False
def __init__(self, side, controller='traj_controller'):
'''
Constructor
'''
super(QueryJointPositionsState,
self).__init__(outcomes=['retrieved', 'failed'],
output_keys=['joint_config'])
if not rospy.has_param("behavior/joint_controllers_name"):
Logger.logerr(
"Need to specify parameter behavior/joint_controllers_name at the parameter server"
)
return
controller_namespace = rospy.get_param(
"behavior/joint_controllers_name")
self._srv_topic = '/' + controller_namespace + '/' + side + '_arm_' + \
controller + '/query_state'
self._srv = ProxyServiceCaller({self._srv_topic: QueryTrajectoryState})
self._srv_result = None
self._failed = False
def __init__(self, planning_group):
'''
Constructor
'''
super(CurrentJointPositionsState, self).__init__(outcomes=['retrieved', 'failed'],
output_keys=['joint_positions'])
self._srv_topic = '/flor/planning/upper_body/get_group_state'
self._srv = ProxyServiceCaller({self._srv_topic: GetCurrentPlanningGroupState})
self._planning_group = planning_group
self._srv_result = None
self._failed = False
def __init__(self, usability_name = '', usability_id = 100):
'''Constructor'''
super(GetTemplateUsabilityState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['template_id'],
output_keys = ['usability_pose'])
# Set up service for attaching object template
self._service_topic = "/usability_pose"
self._srv = ProxyServiceCaller({
self._service_topic: GetUsabilityInWristFrame})
self._usability_id = usability_id
self._usability_name = usability_name
self._failed = False
def __init__(self, target_frame):
'''Constructor'''
super(GetPoseInFrameState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['pose_in'],
output_keys = ['pose_out'])
self._target_frame = target_frame
# Set up service for requesting the transformation
self._service_topic = '/worldmodel_main/get_pose_in_frame_service'
self._srv = ProxyServiceCaller({self._service_topic: GetPoseInFrame})
self._srv_result = None
self._failed = False
self._done = False
class GetTemplatePoseState(EventState):
'''
Requests the pose of a template from the template server.
># template_id int ID of the template to get the pose for.
#> grasp PoseStamped The current pose of the template.
<= done Pose data is available.
<= failed Failed to get pose information.
'''
def __init__(self):
'''Constructor'''
super(GetTemplatePoseState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['template_id'],
output_keys = ['template_pose'])
self._service_topic = "/template_info"
self._srv = ProxyServiceCaller({self._service_topic: GetTemplateStateAndTypeInfo})
self._srv_result = None
self._failed = False
def execute(self, userdata):
if self._failed or self._srv_result is None:
return 'failed'
try:
userdata.template_pose = self._srv_result.template_state_information.pose
except Exception as e:
Logger.logwarn('Failed to retrieve pose information from service result:\n%s' % str(e))
return 'failed'
return 'done'
def on_enter(self, userdata):
try:
self._srv_result = self._srv.call(self._service_topic, GetTemplateStateAndTypeInfoRequest(userdata.template_id, 0)) # We don't care about the hand side
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
def set_up_affordance_service(self):
hands_in_use = self.determine_hands_in_use()
# Set up services for transforming cartesian affordances to wrist frame
self._service_topics = dict()
for hand in hands_in_use:
topic = '/manipulation_control/%s_hand/wrist_affordance_srv' % hand[
0]
self._service_topics[hand] = topic
self._srv = ProxyServiceCaller({
t: GetAffordanceInWristFrame
for t in self._service_topics.values()
})
def __init__(self, side, controller = 'traj_controller'):
'''
Constructor
'''
super(QueryJointPositionsState, self).__init__(outcomes = ['retrieved', 'failed'],
output_keys = ['joint_config'])
if not rospy.has_param("behavior/joint_controllers_name"):
Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server")
return
controller_namespace = rospy.get_param("behavior/joint_controllers_name")
self._srv_topic = '/' + controller_namespace + '/' + side + '_arm_' + \
controller + '/query_state'
self._srv = ProxyServiceCaller({self._srv_topic: QueryTrajectoryState})
self._srv_result = None
self._failed = False
def __init__(self):
'''Constructor'''
super(AttachObjectState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['template_id', 'hand_side'],
output_keys = ['template_pose'])
# Set up service for attaching object template
self._service_topic = "/stitch_object_template"
self._srv = ProxyServiceCaller({
self._service_topic: SetAttachedObjectTemplate})
# Set up subscribers for listening to the latest wrist poses
self._wrist_pose_topics = dict()
self._wrist_pose_topics['left'] = '/flor/l_arm_current_pose'
self._wrist_pose_topics['right'] = '/flor/r_arm_current_pose'
self._sub = ProxySubscriberCached({
self._wrist_pose_topics['left']: PoseStamped,
self._wrist_pose_topics['right']: PoseStamped})
self._sub.make_persistant(self._wrist_pose_topics['left'])
self._sub.make_persistant(self._wrist_pose_topics['right'])
self._failed = False
class QueryJointPositionsState(EventState):
'''
Retrieves the current positions of the joints of the specified planning group.
-- side string One of left, right
-- controller string Specifify the trajectory controller suffix
Example: 'traj_hybrid_controller'
#> joint_config dict Trajectory joint positions at current time.
joint_names string[] : joint_values[]
<= retrieved Successfully determined the current joint values.
<= failed Failed to send service call or got no result.
'''
TRAJ_CONTROLLER = 'traj_controller'
HYBRID_CONTROLLER = 'traj_hybrid_controller'
def __init__(self, side, controller='traj_controller'):
'''
Constructor
'''
super(QueryJointPositionsState,
self).__init__(outcomes=['retrieved', 'failed'],
output_keys=['joint_config'])
if not rospy.has_param("behavior/joint_controllers_name"):
Logger.logerr(
"Need to specify parameter behavior/joint_controllers_name at the parameter server"
)
return
controller_namespace = rospy.get_param(
"behavior/joint_controllers_name")
self._srv_topic = '/' + controller_namespace + '/' + side + '_arm_' + \
controller + '/query_state'
self._srv = ProxyServiceCaller({self._srv_topic: QueryTrajectoryState})
self._srv_result = None
self._failed = False
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
else:
return 'retrieved'
def on_enter(self, userdata):
self._failed = False
try:
self._srv_result = self._srv.call(
self._srv_topic,
QueryTrajectoryStateRequest(time=rospy.Time.now()))
joint_names = self._srv_result.name
joint_values = list(
self._srv_result.position) # cast tuple to list
userdata.joint_config = {
'joint_names': joint_names,
'joint_values': joint_values
}
except Exception as e:
Logger.logwarn('Could not retrieve current joint positions!\n%s' %
str(e))
self._failed = True
class GetTemplateAffordanceState(EventState):
'''
Requests affordance information for a template from the template server.
-- identifier string Name to identify the requested affordance.
># template_id int ID of the template to manipulate.
># hand_side string Hand side for which the state will ask the template server for affordances {left, right, both}
#> affordance Affordance A message as defined in vigir_object_template_msgs containing all required information.
<= done Affordance data is available.
<= failed Failed to get affordance information.
<= not_available The requested preference is not available (index too high).
'''
def __init__(self, identifier):
'''Constructor'''
super(GetTemplateAffordanceState, self).__init__(outcomes = ['done', 'failed', 'not_available'],
input_keys = ['template_id', 'hand_side'],
output_keys = ['affordance'])
self._service_topic = "/template_info"
self._srv = ProxyServiceCaller({self._service_topic: GetTemplateStateAndTypeInfo})
self._srv_result = None
self._identifier = identifier
self._hand_side = None
self._failed = False
self._done = False
def execute(self, userdata):
if self._failed or self._srv_result is None:
return 'failed'
if self._done:
return 'done'
try:
choices = self._srv_result.template_type_information.affordances
if not self._identifier in [a.name for a in choices]:
return 'not_available'
chosen_affordance = next(a for a in choices if a.name == self._identifier)
userdata.affordance = chosen_affordance
Logger.loginfo('Using grasp with ID: %s' % str(chosen_affordance.id))
except Exception as e:
Logger.logwarn('Failed to retrieve affordance information from service result:\n%s' % str(e))
self._failed = True
return 'failed'
self._done = True
return 'done'
def on_enter(self, userdata):
self._failed = False
self._done = False
self._srv_result = None
if userdata.hand_side == 'left':
self._hand_side = GetTemplateStateAndTypeInfoRequest.LEFT_HAND
elif userdata.hand_side == 'right':
self._hand_side = GetTemplateStateAndTypeInfoRequest.RIGHT_HAND
elif userdata.hand_side == 'both':
self._hand_side = GetTemplateStateAndTypeInfoRequest.BOTH_HANDS
else:
Logger.logwarn('Unexpected value of hand side: %s Expected {left, right, both}' % str(userdata.hand_side))
try:
self._srv_result = self._srv.call(self._service_topic, GetTemplateStateAndTypeInfoRequest(userdata.template_id, self._hand_side))
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
class AttachObjectState(EventState):
'''
Requests a grasp for a template from the template server.
># template_id int ID of the template to attach.
># hand_side string Hand side to which the template
should be attached {left, right}
#> template_pose PoseStamped Pose of the attached template in the
reference frame of the wrist (r/l_hand)
<= done Template has been attached.
<= failed Failed to attach template.
'''
def __init__(self):
'''Constructor'''
super(AttachObjectState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['template_id', 'hand_side'],
output_keys = ['template_pose'])
# Set up service for attaching object template
self._service_topic = "/stitch_object_template"
self._srv = ProxyServiceCaller({
self._service_topic: SetAttachedObjectTemplate})
# Set up subscribers for listening to the latest wrist poses
self._wrist_pose_topics = dict()
self._wrist_pose_topics['left'] = '/flor/l_arm_current_pose'
self._wrist_pose_topics['right'] = '/flor/r_arm_current_pose'
self._sub = ProxySubscriberCached({
self._wrist_pose_topics['left']: PoseStamped,
self._wrist_pose_topics['right']: PoseStamped})
self._sub.make_persistant(self._wrist_pose_topics['left'])
self._sub.make_persistant(self._wrist_pose_topics['right'])
self._failed = False
def execute(self, userdata):
if self._failed:
return 'failed'
return 'done'
def on_enter(self, userdata):
self._failed = False
# The frame ID to which the object template will be attached to
if userdata.hand_side == 'left':
frame_id = "l_hand"
elif userdata.hand_side == 'right':
frame_id = "r_hand"
else:
Logger.logwarn('Unexpected value of hand side: %s Expected {left, right}' % str(userdata.hand_side))
self._failed = True
return
# Get the current wrist pose, which is required by the stitching service
try:
wrist_pose_topic = self._wrist_pose_topics[userdata.hand_side]
current_wrist_pose = self._sub.get_last_msg(wrist_pose_topic)
# Set the PoseStamped message's frame ID to the one
# that the object template should be attached to:
current_wrist_pose.header.frame_id = frame_id
except Exception as e:
Logger.logwarn('Failed to get the latest hand pose:\n%s' % str(e))
self._failed = True
return
# Send the stiching request and write response to userdata
try:
request = SetAttachedObjectTemplateRequest(template_id = userdata.template_id,
pose = current_wrist_pose)
self._srv_result = self._srv.call(self._service_topic, request)
# The result contains the template's pose, mass, and center of mass
userdata.template_pose = self._srv_result.template_pose
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
return
class CurrentJointPositionsState(EventState):
'''
Retrieves the current positions of the joints of the specified planning group.
-- planning_group string Name of the planning group.
#> joint_positions float[] Current joint values. Length of the list depends on the number of joints of the planning group.
<= retrieved Successfully determined the current joint values.
<= failed Failed to send service call or got no result.
'''
def __init__(self, planning_group):
'''
Constructor
'''
super(CurrentJointPositionsState,
self).__init__(outcomes=['retrieved', 'failed'],
output_keys=['joint_positions'])
self._srv_topic = '/flor/planning/upper_body/get_group_state'
self._srv = ProxyServiceCaller(
{self._srv_topic: GetCurrentPlanningGroupState})
self._planning_group = planning_group
self._srv_result = None
self._failed = False
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._srv_result is not None:
userdata.joint_positions = self._srv_result.position
if len(self._srv_result.position) == 0:
Logger.logwarn("Lookup of joint positions of group %s failed" %
self._planning_group)
self._failed = True
return 'failed'
else:
return 'retrieved'
def on_enter(self, userdata):
self._failed = False
try:
self._srv_result = self._srv.call(
self._srv_topic,
GetCurrentPlanningGroupStateRequest(self._planning_group))
except Exception as e:
Logger.logwarn('Could not retrieve current joint positions!')
rospy.logwarn(str(e))
self._failed = True
class QueryJointPositionsState(EventState):
'''
Retrieves the current positions of the joints of the specified planning group.
-- side string One of left, right
-- controller string Specifify the trajectory controller suffix
Example: 'traj_hybrid_controller'
#> joint_config dict Trajectory joint positions at current time.
joint_names string[] : joint_values[]
<= retrieved Successfully determined the current joint values.
<= failed Failed to send service call or got no result.
'''
TRAJ_CONTROLLER = 'traj_controller'
HYBRID_CONTROLLER = 'traj_hybrid_controller'
def __init__(self, side, controller = 'traj_controller'):
'''
Constructor
'''
super(QueryJointPositionsState, self).__init__(outcomes = ['retrieved', 'failed'],
output_keys = ['joint_config'])
if not rospy.has_param("behavior/joint_controllers_name"):
Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server")
return
controller_namespace = rospy.get_param("behavior/joint_controllers_name")
self._srv_topic = '/' + controller_namespace + '/' + side + '_arm_' + \
controller + '/query_state'
self._srv = ProxyServiceCaller({self._srv_topic: QueryTrajectoryState})
self._srv_result = None
self._failed = False
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
else:
return 'retrieved'
def on_enter(self, userdata):
self._failed = False
try:
self._srv_result = self._srv.call(
self._srv_topic,
QueryTrajectoryStateRequest(time = rospy.Time.now()))
joint_names = self._srv_result.name
joint_values = list(self._srv_result.position) # cast tuple to list
userdata.joint_config = {'joint_names' : joint_names,
'joint_values': joint_values}
except Exception as e:
Logger.logwarn('Could not retrieve current joint positions!\n%s' % str(e))
self._failed = True
class GetTemplatePregraspState(EventState):
'''
Requests pregrasp information for a template from the template server.
># template_id int ID of the template to grasp.
># hand_side string Hand side for which the state will ask the template server for pregrasp poses {left, right, both}
># preference int Index of the preferred pregrasp.
#> pre_grasp PoseStamped Target endeffector pose for pregrasp.
<= done Pregrasp data is available.
<= failed Failed to get pregrasp information.
<= not_available The requested preference is not available (index too high).
'''
def __init__(self):
'''Constructor'''
super(GetTemplatePregraspState, self).__init__(outcomes = ['done', 'failed', 'not_available'],
input_keys = ['template_id', 'hand_side', 'preference'],
output_keys = ['pre_grasp'])
self._service_topic = "/instantiated_grasp_info"
self._srv = ProxyServiceCaller({self._service_topic: GetInstantiatedGraspInfo})
self._srv_result = None
self._hand_side = None
self._failed = False
self._done = False
self._not_available = False
def execute(self, userdata):
if self._failed or self._srv_result is None:
return 'failed'
if self._done:
return 'done'
if self._not_available:
return 'not_available'
try:
options = self._srv_result.pre_grasp_information.grasps
if len(options) <= userdata.preference:
Logger.logwarn('The option with index %d is not available. There are %d options total.' % (userdata.preference, len(options)))
self._not_available = True
return 'not_available'
chosen_pregrasp = options[userdata.preference]
userdata.pre_grasp = chosen_pregrasp.grasp_pose
Logger.loginfo('Using grasp with ID: %s' % str(chosen_pregrasp.id))
except Exception as e:
Logger.logwarn('Failed to retrieve grasp information from service result:\n%s' % str(e))
self._failed = True
return 'failed'
self._done = True
return 'done'
def on_enter(self, userdata):
self._failed = False
self._done = False
self._not_available = False
if userdata.hand_side == 'left':
self._hand_side = GetInstantiatedGraspInfoRequest.LEFT_HAND
elif userdata.hand_side == 'right':
self._hand_side = GetInstantiatedGraspInfoRequest.RIGHT_HAND
elif userdata.hand_side == 'both':
self._hand_side = GetInstantiatedGraspInfoRequest.BOTH_HANDS
else:
Logger.logwarn('Unexpected value of hand side: %s Expected {left, right, both}' % str(userdata.hand_side))
try:
self._srv_result = self._srv.call(self._service_topic, GetInstantiatedGraspInfoRequest(int(userdata.template_id), self._hand_side))
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
class GetPoseInFrameState(EventState):
'''
Transforms the given pose from its current frame to a target frame.
-- target_frame string The frame to which the pose will be transformed
For example, 'utorso', 'world', etc.
># pose_in PoseStamped The pose to be transformed to a target frame
Its current frame is included in the message
#> pose_out PoseStamped The same pose but in the target ref frame
<= done The pose was transformed successfully.
<= failed Failed to transform pose to target frame.
'''
def __init__(self, target_frame):
'''Constructor'''
super(GetPoseInFrameState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['pose_in'],
output_keys = ['pose_out'])
self._target_frame = target_frame
# Set up service for requesting the transformation
self._service_topic = '/worldmodel_main/get_pose_in_frame_service'
self._srv = ProxyServiceCaller({self._service_topic: GetPoseInFrame})
self._srv_result = None
self._failed = False
self._done = False
def execute(self, userdata):
if self._failed:
return 'failed'
if self._done:
return 'done'
error = self._srv_result.error_code # message is empty if success
if error:
Logger.logwarn('Pose transformation failed because: %s' % error)
self._failed = True
return 'failed'
else:
userdata.pose_out = self._srv_result.transformed_pose
return 'done'
def on_enter(self, userdata):
self._failed = False
self._done = False
# Send the transformation request
try:
Logger.loginfo('Requesting transformation from frame %s to %s' %
(userdata.pose_in.header.frame_id, self._target_frame))
request = GetPoseInFrameRequest(pose = userdata.pose_in,
target_frame = self._target_frame)
self._srv_result = self._srv.call(self._service_topic, request)
except Exception as e:
Logger.logwarn('Failed to send service request:\n%s' % str(e))
self._failed = True
return
class GetTemplateStandPoseState(EventState):
'''
Requests the pose where to stand in front of a template from the template server.
># template_id int ID of the template to get the stand pose for.
># hand_side string Hand side for which the state will ask the template server for poses {left, right, both}
># preference int Index of the preferred stand pose.
#> grasp PoseStamped Pelvis pose of the robot required to perform grasping.
<= done Pose data is available.
<= failed Failed to get stand pose.
<= not_available The requested preference is not available (index too high).
'''
def __init__(self):
'''Constructor'''
super(GetTemplateStandPoseState, self).__init__(outcomes = ['done', 'failed', 'not_available'],
input_keys = ['template_id', 'hand_side', 'preference'],
output_keys = ['stand_pose'])
self.hand_side = int()
self._service_topic = "/template_info"
self._srv = ProxyServiceCaller({self._service_topic: GetTemplateStateAndTypeInfo})
self._srv_result = None
self._hand_side = None
self._failed = False
self._done = False
self._not_available = False
def execute(self, userdata):
if self._failed or self._srv_result is None:
return 'failed'
if self._done:
return 'done'
if self._not_available:
return 'not_available'
try:
choices = self._srv_result.template_type_information.stand_poses
Logger.loginfo("There are %d stand pose choices and want to select entry %d" % (len(choices), userdata.preference))
if len(choices) <= userdata.preference:
self._not_available = True
return 'not_available'
userdata.stand_pose = choices[userdata.preference].pose
except Exception as e:
Logger.logwarn('Failed to retrieve pose information from service result:\n%s' % str(e))
self._failed = True
return 'failed'
self._done = True
return 'done'
def on_enter(self, userdata):
self._failed = False
self._done = False
self._not_available = False
if userdata.hand_side == 'left':
self._hand_side = GetTemplateStateAndTypeInfoRequest.LEFT_HAND
elif userdata.hand_side == 'right':
self._hand_side = GetTemplateStateAndTypeInfoRequest.RIGHT_HAND
elif userdata.hand_side == 'both':
self._hand_side = GetTemplateStateAndTypeInfoRequest.BOTH_HANDS
else:
Logger.logwarn('Unexpected value of hand side: %s Expected {left, right, both}' % str(userdata.hand_side))
try:
self._srv_result = self._srv.call(self._service_topic, GetTemplateStateAndTypeInfoRequest(userdata.template_id, self._hand_side))
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
class GetTemplateUsabilityState(EventState):
'''
Requests a grasp for a template from the template server.
-- usability_name string Name of the template usability (e.g. 'tip')
-- usability_id int ID of the template usability.
Set to 100 if requesting by name instead.
># template_id int ID of the template of interest
#> usability_pose PoseStamped Pose of the template's usability in the
reference frame of the wrist (r/l_hand)
<= done The usability has been retrieved.
<= failed Failed to get the template's usability.
'''
def __init__(self, usability_name = '', usability_id = 100):
'''Constructor'''
super(GetTemplateUsabilityState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['template_id'],
output_keys = ['usability_pose'])
# Set up service for attaching object template
self._service_topic = "/usability_pose"
self._srv = ProxyServiceCaller({
self._service_topic: GetUsabilityInWristFrame})
self._usability_id = usability_id
self._usability_name = usability_name
self._failed = False
def execute(self, userdata):
'''Move along people, nothing to see here!'''
if self._failed:
return 'failed'
else:
return 'done'
def on_enter(self, userdata):
'''Send the usability request and write response to userdata.'''
self._failed = False
if self._usability_id != 100 and self._usability_name != '':
Logger.logwarn('Not clear if you are requesting usability by ID (%d) + \
or name (%s)' % (self._usability_id, self._usability_name))
try:
request = GetUsabilityInWristFrameRequest(
template_id = userdata.template_id,
usability_id = self._usability_id,
usability_name = self._usability_name)
self._srv_result = self._srv.call(self._service_topic, request)
userdata.usability_pose = self._srv_result.wrist_usability
print request, self._srv_result.wrist_usability # debug
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
return
class CurrentJointPositionsState(EventState):
'''
Retrieves the current positions of the joints of the specified planning group.
-- planning_group string Name of the planning group.
#> joint_positions float[] Current joint values. Length of the list depends on the number of joints of the planning group.
<= retrieved Successfully determined the current joint values.
<= failed Failed to send service call or got no result.
'''
def __init__(self, planning_group):
'''
Constructor
'''
super(CurrentJointPositionsState, self).__init__(outcomes=['retrieved', 'failed'],
output_keys=['joint_positions'])
self._srv_topic = '/flor/planning/upper_body/get_group_state'
self._srv = ProxyServiceCaller({self._srv_topic: GetCurrentPlanningGroupState})
self._planning_group = planning_group
self._srv_result = None
self._failed = False
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._srv_result is not None:
userdata.joint_positions = self._srv_result.position
if len(self._srv_result.position) == 0:
Logger.logwarn("Lookup of joint positions of group %s failed" % self._planning_group)
self._failed = True
return 'failed'
else:
return 'retrieved'
def on_enter(self, userdata):
self._failed = False
try:
self._srv_result = self._srv.call(
self._srv_topic,
GetCurrentPlanningGroupStateRequest(self._planning_group))
except Exception as e:
Logger.logwarn('Could not retrieve current joint positions!')
rospy.logwarn(str(e))
self._failed = True