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