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
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
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 ServiceRotateRelativeToGoToState(EventState): ''' State for calling goto service in MRS system. -- set_yaw_service_topic string service topic for setting yaw. -- control_manager_diagnostics_topic string service topic for control diagnostics. ># goal_tracker_point mrs_msgs.srv.ReferenceStampedSrvRequest Target position in Reference style <= successed Whenever the calling for successful. <= failed When the calling failed. ''' 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 execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if not self._connected: Logger.logerr( '[ServiceRotateRelativeToGoToState]: not connected to %s' % self._control_manager_diagnostics_topic) return 'failed' if not self._connected_odom: Logger.logerr( '[ServiceRotateRelativeToGoToState]: not connected to %s' % self._odometry_topic) return 'failed' diff_time = rospy.get_time() - self._start_time if self._sub_cont_diag.has_msg(self._control_manager_diagnostics_topic ) and self._sub_odom.has_msg( self._odometry_topic): Logger.loginfo( '[ServiceRotateRelativeToGoToState]: has_diagnostics msg at time %s' % (str(diff_time))) message = self._sub_cont_diag.get_last_msg( self._control_manager_diagnostics_topic) message_odom = self._sub_odom.get_last_msg(self._odometry_topic) currentPosition = message_odom.pose.pose.position target_position = userdata.goal_tracker_point.position self._desired_yaw = math.atan2( target_position.y - currentPosition.y, target_position.x - currentPosition.x) self._sub_cont_diag.remove_last_msg( self._control_manager_diagnostics_topic) self._sub_odom.remove_last_msg(self._odometry_topic) rospy.loginfo_throttle( 2, '[ServiceRotateRelativeToGoToState]: currentPosition %s %s' % (str(currentPosition.x), str(currentPosition.y))) rospy.loginfo_throttle( 2, '[ServiceRotateRelativeToGoToState]: target_position %s %s' % (str(target_position.x), str(target_position.y))) rospy.loginfo_throttle( 2, '[ServiceRotateRelativeToGoToState]: self._desired_yaw %s' % (str(self._desired_yaw))) if not self._sended: # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._set_yaw_service_topic): Logger.logerr( '[ServiceRotateRelativeToGoToState]: Topic \'{}\' not yet registered!' .format(self._set_yaw_service_topic)) self._failed = True return try: service_request = mrs_msgs.srv.Vec1Request() service_request.goal = self._desired_yaw service_result = self._srv.call( self._set_yaw_service_topic, service_request) Logger.loginfo( '[ServiceRotateRelativeToGoToState]: sended desired yaw' ) if not service_result.success: self._failed = True Logger.logwarn( '[ServiceRotateRelativeToGoToState]: Calling \'{}\' was not successful: {} for reference position (x,y,z) = ({},{},{})' .format(self._service_topic, str(service_result.message), service_request.goal)) else: self._sended = True self._start_time = rospy.get_time() userdata.desired_yaw = self._desired_yaw Logger.loginfo( '[ServiceRotateRelativeToGoToState]: call result {}' .format(str(service_result.message))) except Exception as e: Logger.logerr( '[ServiceRotateRelativeToGoToState]: Failed to call \'{}\' service request: {}' .format(self._set_yaw_service_topic, str(e))) self._failed = True diff_time = rospy.get_time() - self._start_time if diff_time >= 1.0 and not message.tracker_status.have_goal and self._sended: Logger.loginfo( '[ServiceRotateRelativeToGoToState]: Successfully ended following of trajectory %s' % self._control_manager_diagnostics_topic) return 'successed' else: return if self._failed: return 'failed' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._start_time = rospy.get_time() if not self._connected: (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 previously unavailable topic %s' % self._control_manager_diagnostics_topic) else: Logger.logwarn( '[ServiceRotateRelativeToGoToState]: Topic %s still not available, giving up.' % self._control_manager_diagnostics_topic) if not self._connected_odom: (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) else: Logger.logwarn( '[ServiceRotateRelativeToGoToState]: Topic %s still not available, giving up.' % self._odometry_topic) if self._connected and self._sub_cont_diag.has_msg( self._control_manager_diagnostics_topic): Logger.loginfo( '[ServiceRotateRelativeToGoToState]: Waiting for msg from topic %s' % self._control_manager_diagnostics_topic) self._sub_cont_diag.remove_last_msg( self._control_manager_diagnostics_topic) if self._connected_odom and self._sub_odom.has_msg( self._odometry_topic): Logger.loginfo( '[ServiceRotateRelativeToGoToState]: Waiting for msg from topic %s' % self._odometry_topic) self._sub_odom.remove_last_msg(self._odometry_topic) self._failed = False def _get_msg_from_path(self, msg_path): msg_import = msg_path.split('/') msg_module = '%s.msg' % (msg_import[0]) package = __import__(msg_module, fromlist=[msg_module]) clsmembers = inspect.getmembers( package, lambda member: inspect.isclass(member) and member. __module__.endswith(msg_import[1])) return clsmembers[0][1]
class ServiceSetWallDefinitionAndConstraintsIntoPlanKeeper(EventState): ''' State for saving the plans for robots in Wall challenge in MBZIRC 2020 into plan_keeper node. -- service_topic string Service_topic_name. #> brick_rules Binary rules of bricks. #> brick_positions Array of brick positions in the wall. #> brick_ids Array of brick ids ordered same as brick_positions. <= successed Whenever the calling for successful. <= failed When the calling failed. ''' 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 execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if self._failed: return 'failed' else: return 'successed' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._failed = False # Check if the ProxyServiceCaller has been registered for setting wall definition if not self._srv_wall_definition.is_available( self._plan_keeper_wall_definition_in_service_topic): Logger.logerr( 'ProxyServiceCaller: Topic \'{}\' not yet registered!'.format( self._plan_keeper_wall_definition_in_service_topic)) self._failed = True return try: service_request = plan_keeper.srv.SetWallDefinitionRequest() service_request.brick_positions = userdata.brick_positions service_request.brick_ids = userdata.brick_ids service_result = self._srv_wall_definition.call( self._plan_keeper_wall_definition_in_service_topic, service_request) if not service_result.success: self._failed = True Logger.logwarn('Calling \'{}\' was not successful: {}'.format( self._plan_keeper_wall_definition_in_service_topic, str(service_result.message))) else: Logger.loginfo(service_result.message) except Exception as e: Logger.logerr('Failed to call \'{}\' service request: {}'.format( self._plan_keeper_wall_definition_in_service_topic, str(e))) self._failed = True # Check if the ProxyServiceCaller has been registered for setting constraints if not self._srv_building_rules.is_available( self._plan_keeper_building_rules_in_service_topic): Logger.logerr( 'ProxyServiceCaller: Topic \'{}\' not yet registered!'.format( self._plan_keeper_building_rules_in_service_topic)) self._failed = True return try: service_request = plan_keeper.srv.SetBuildingRulesRequest() service_request.brick_rules = userdata.brick_rules service_result = self._srv_building_rules.call( self._plan_keeper_building_rules_in_service_topic, service_request) if not service_result.success: self._failed = True Logger.logwarn('Calling \'{}\' was not successful: {}'.format( self._plan_keeper_building_rules_in_service_topic, str(service_result.message))) else: Logger.loginfo(service_result.message) except Exception as e: Logger.logerr('Failed to call \'{}\' service request: {}'.format( self._plan_keeper_building_rules_in_service_topic, str(e))) self._failed = True
class SpiralAroundPointState(EventState): ''' State for calling goto service in MRS system. -- service_topic string Service_topic_name. ># goal_tracker_point mrs_msgs.srv.ReferenceStampedSrvRequest Target position in Reference style ># frame_id string frame id of targetreference <= successed Whenever the calling for successful. <= failed When the calling failed. ''' 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 nearest_wall_callback(self, message): #rospy.loginfo_throttle(5,"[SpiralAroundPointState]: received nearest wall %s"%(str(message.header.stamp))) #rospy.loginfo("rospy.get_rostime() %s"%(str(rospy.get_rostime()))) #rospy.loginfo("message.header.stamp %s"%(str(message.header.stamp))) #rospy.loginfo("diff %s"%(str(rospy.get_rostime()-message.header.stamp))) if rospy.get_rostime() - message.header.stamp < rospy.Duration( secs=TIME_DURATION): rospy.loginfo_throttle( 0.5, "[SpiralAroundPointState]: received nearest wall reacently %s" % (str(message.header.stamp))) self.received = True def execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if self.received: rospy.loginfo( "[SpiralAroundPointState]: successfully ended with detection") return 'successed' diff_time = rospy.get_time() - self._start_time if diff_time > TIME_DURATION: rospy.loginfo( "[SpiralAroundPointState]: spiral ended with over_time without detection" ) return 'over_time' time_portion = diff_time / TIME_DURATION spiral_idx = int(len(self._spiral_points) * time_portion) spiral_idx = min(spiral_idx, len(self._spiral_points) - 1) spiral_idx = max(spiral_idx, 0) current_target = self._spiral_points[spiral_idx] # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._service_topic): Logger.logerr( '[SpiralAroundPointState]: Topic \'{}\' not yet registered!'. format(self._service_topic)) self._failed = True return try: service_request = mrs_msgs.srv.ReferenceStampedSrvRequest() service_request.header = std_msgs.msg.Header() service_request.header.stamp = rospy.Time.now() service_request.header.seq = 0 service_request.header.frame_id = userdata.frame_id service_request.reference.position.x = current_target[0] service_request.reference.position.y = current_target[1] service_request.reference.position.z = userdata.start_spiral_tracker_point.z service_request.reference.yaw = userdata.start_spiral_tracker_point.yaw service_result = self._srv.call(self._service_topic, service_request) if not service_result.success: self._failed = True Logger.logwarn( '[SpiralAroundPointState]: {} Calling \'{}\' was not successful: {} for reference position (x,y,z) = ({},{},{})' .format(time_portion, self._service_topic, str(service_result.message), service_request.reference.position.x, service_request.reference.position.y, service_request.reference.position.z)) else: Logger.loginfo('[SpiralAroundPointState]: {}'.format( str(service_result.message))) except Exception as e: Logger.logerr( '[SpiralAroundPointState]: Failed to call \'{}\' service request: {}' .format(self._service_topic, str(e))) self._failed = True if self._failed: return 'failed' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._start_point = userdata.start_spiral_tracker_point start_direction = self._start_point.yaw - math.pi / 2.0 self._spiral_points = self.get_spiral_points( [self._start_point.x, self._start_point.y], START_RADIUS, MAX_RADIUS, RADIUS_STEP, start_direction, ROT_STEP, NUM_ROTATIONS) self._start_time = rospy.get_time() self._failed = False self.received = False def get_spiral_points(self, pos, start_radius, max_radius, radius_step, start_direction, rot_step, num_rotations): rot = start_direction radius = start_radius #speed = len_step/duration_s num_points = (2.0 * math.pi * num_rotations) / rot_step spiral_points = [] if PLOT: plt.plot([pos[0]], [pos[0]], 'og') for i in range(int(num_points)): radius += radius_step radius = min(radius, max_radius) radius = max(radius, start_radius) rot += rot_step posx = pos[0] + radius * math.cos(rot) posy = pos[1] + radius * math.sin(rot) spiral_points.append([posx, posy]) if PLOT: plt.plot([posx], [posy], '.r') if PLOT: plt.axis('equal') plt.show() return spiral_points
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 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 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 AddGazeboModel(EventState): ''' Call a service given a name. -- model_name string Name of the calling service ># gazebo_model_pose Pose gaezbo_model_pose <= done Affordance data is available. <= failed Failed to get affordance information. ''' BOX_MALE = 'box_male' BOX_FEMALE = 'box_female' 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 execute(self, userdata): if self._failed or self._srv_result is None: return 'failed' if self._done: return 'done' self._done = True return 'done' def on_enter(self, userdata): self._gazebo_model_pose = userdata.gazebo_model_pose if self._gazebo_model_pose is not None: if self._model_name == "box_male": # Get male box Path model_path = rospkg.RosPack().get_path('birl_baxter_description') + "/urdf/box/" # Load male box SDF self._model_xml = '' with open(model_path + "box_male/robots/box_male.URDF", "r") as box_male_file: self._model_xml = box_male_file.read().replace('\n', '') try: self._srv_result = self._srv.call(self._service_name, SpawnModelRequest(self._model_name, self._model_xml, "/", self._gazebo_model_pose, self._model_reference_frame)) rospy.loginfo('loading male box succesfully') except Exception, e: rospy.logerr("Spawn URDF service call failed: {0}".format(e)) self._failed = True elif self._model_name == "box_female": # get path model_path = rospkg.RosPack().get_path('birl_baxter_description') + "/urdf/box/" # Load female box URDF box_female_xml = '' with open(model_path + "box_female/robots/box_female.URDF", "r") as box_female_file: box_female_xml = box_female_file.read().replace('\n', '') rospy.wait_for_service('/gazebo/spawn_urdf_model') try: self._srv_result = self._srv.call(self._service_name, SpawnModelRequest(self._model_name, self._model_xml, "/", self._gazebo_model_pose, self._model_reference_frame)) except Exception, e: rospy.logerr("Spawn URDF service call failed: {0}".format(e)) self._failed = True else:
class ServiceSetBrickDetectionTypeAndWallLayer(EventState): ''' State for saving the plans for robots in Wall challenge in MBZIRC 2020 into plan_keeper node. -- service_topic_see_type string Service topic name for seting see WALL/BRICK.. -- service_topic_layer string Service topic name for setting layer of wall ># see_type what to see ># wall_layer in which layer we are placing <= successed Whenever the calling for successful. <= failed When the calling failed. ''' BRICK_SEE_EVERYTHING = 0 BRICK_SEE_RED = 1 BRICK_SEE_GREEN = 2 BRICK_SEE_BLUE = 3 BRICK_SEE_ORANGE = 4 BRICK_SEE_WALL = 8 BRICK_SEE_WALL_HAVING_RED = 9 BRICK_SEE_WALL_HAVING_GREEN = 10 BRICK_SEE_WALL_HAVING_BLUE = 11 LAYER_0 = 0 LAYER_1 = 1 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 def execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if self._failed: return 'failed' else: return 'successed' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._failed = False # Check if the ProxyServiceCaller has been registered if not self._srv_detection_type.is_available( self._service_topic_see_type): Logger.logerr( '[ServiceSetBrickDetectionType]: Topic \'{}\' not yet registered!' .format(self._service_topic_see_type)) self._failed = True return try: service_request_see_type = mbzirc_msgs.srv.DetectionTypeRequest() service_request_see_type.type = userdata.see_type Logger.loginfo( '[ServiceSetBrickDetectionType]: setting see type %s' % (str(service_request_see_type.type))) service_result_see_type = self._srv_detection_type.call( self._service_topic_see_type, service_request_see_type) if not service_result_see_type.success: self._failed = True Logger.logwarn( '[ServiceSetBrickDetectionType]: Calling \'{}\' was not successful: {}' .format(self._service_topic_see_type, str(service_result_see_type.message))) else: Logger.loginfo(service_result_see_type.message) except Exception as e: Logger.logerr( '[ServiceSetBrickDetectionType]:Failed to call \'{}\' service request: {}' .format(self._service_topic_see_type, str(e))) self._failed = True #part for setting layer if not self._srv_layer.is_available(self._service_topic_layer): Logger.logerr( '[ServiceSetBrickDetectionType]: Topic \'{}\' not yet registered!' .format(self._service_topic_layer)) self._failed = True return try: service_request_layer = mbzirc_msgs.srv.DetectionTypeRequest() service_request_layer.type = userdata.wall_layer Logger.loginfo( '[ServiceSetBrickDetectionType]: setting slayer to %s' % (str(service_request_see_type.type))) service_result_layer = self._srv_layer.call( self._service_topic_layer, service_request_layer) if not service_result_layer.success: self._failed = True Logger.logwarn( '[ServiceSetBrickDetectionType]: Calling \'{}\' was not successful: {}' .format(self._service_topic_layer, str(service_result_layer.message))) else: Logger.loginfo(service_result_layer.message) except Exception as e: Logger.logerr( '[ServiceSetBrickDetectionType]:Failed to call \'{}\' service request: {}' .format(self._service_topic_layer, 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 CallBirlService(EventState): ''' Call a service given a name. -- service_name string Name of the calling service <= done Affordance data is available. <= failed Failed to get affordance information. ''' 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 execute(self, userdata): if self._failed or self._srv_result is None: return 'failed' if self._done: return 'done' self._done = True return 'done' def on_enter(self, userdata): self._failed = False self._done = False self._srv_result = None self._pose = Pose() self._pose.position.x = 0.6 self._pose.position.y = 0 self._pose.position.z = -0.115 self._pose.orientation.x = 0 self._pose.orientation.y = 0 self._pose.orientation.z = 0 self._pose.orientation.w = 1 self._model_reference_frame = String() self._model_reference_frame.data = "base" self._model_name = String() self._model_name.data = "box_female" try: self._srv_result = self._srv.call(self._service_name, Add_Gazebo_ModelRequest(self._model_name, self._pose, self._model_reference_frame)) 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 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 ServiceGetWallBrickPositionsState(EventState): ''' State for calling std_srvs/Trigger type of services. -- service_topic string Service_topic_name. <= finished Whenever the calling for successful. <= failed When the calling failed. ''' 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 execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if self._failed: return 'failed' else: return 'finished' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._failed = False # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._service_topic): Logger.logerr( '[ServiceGetWallBrickPositionsState]: Topic \'{}\' not yet registered!' .format(self._service_topic)) self._failed = True return try: service_request = brick_mapping.srv.GetMappedArenaObjectsRequest() service_result = self._srv.call(self._service_topic, service_request) Logger.loginfo( '[ServiceGetWallBrickPositionsState]: got mapped_objects from mapping' ) Logger.loginfo(str(service_result.mapped_objects)) userdata.mapped_objects = service_result.mapped_objects if not service_result.success: self._failed = True Logger.logwarn( '[ServiceGetWallBrickPositionsState]: Calling \'{}\' was not successful: {}' .format(self._service_topic, str(service_result.message))) else: Logger.loginfo('[ServiceGetWallBrickPositionsState]: ' + str(service_result.message)) except Exception as e: Logger.logerr( '[ServiceGetWallBrickPositionsState]: Failed to call \'{}\' service request: {}' .format(self._service_topic, 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 ServiceScanArena(EventState): ''' State for scanning arena in Wall challenge in MBZIRC 2020. -- service_topic string Service_topic_name. ># scanning_altitude int16 Time that remains to finish the challenge. <= successed Whenever the calling for successful. <= failed When the calling failed. ''' 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 execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if self._failed: return 'failed' else: return 'successed' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._failed = False # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._service_topic): Logger.logerr( '[ServiceScanArena]: Topic \'{}\' not yet registered!'.format( self._service_topic)) self._failed = True return try: Logger.loginfo('[ServiceScanArena]: create service_request for ' + self._service_topic) service_request = arena_scan_planner.srv.ArenaScanTrajectoryRequest( ) service_request.speed = userdata.scanning_speed service_request.acc = userdata.scanning_max_acc service_request.turning_speed = userdata.scanning_turning_speed service_request.altitude = userdata.scanning_altitude service_request.initial_speed = 0 service_request.loop = False service_request.arena_part_id = min(userdata.robot_id + 1, userdata.scanning_robots) service_request.num_robots = userdata.scanning_robots service_result = self._srv.call(self._service_topic, service_request) sweep_path = service_result.sweep_path userdata.scanning_trajectory = sweep_path userdata.scanning_trajectory_start = sweep_path[0] Logger.logwarn('"[ServiceScanArena]: sweep path has %d points' % (len(sweep_path))) Logger.logwarn( '"[ServiceScanArena]: sweep path start is at %f %f %f %f' % (sweep_path[0].position.x, sweep_path[0].position.y, sweep_path[0].position.z, sweep_path[0].heading)) if not service_result.ok: self._failed = True Logger.logwarn( '"[ServiceScanArena]: Calling \'{}\' was not successful: {}' .format(self._service_topic, str(service_result.message))) else: Logger.loginfo(service_result.message) except Exception as e: Logger.logerr( '"[ServiceScanArena]: Failed to call \'{}\' service request: {}' .format(self._service_topic, str(e))) self._failed = True
class ServiceGetNextBrickInPlan(EventState): ''' State for getting a plan for next brick in Wall challenge in MBZIRC 2020. -- service_topic string Service_topic_name. -- flying_altitude float Altitude when the robot will move. #> grasp_position TrackerPoint Position with bricks. #> placing_position TrackerPoint Position where to place brick after grasping. #> wall_position TrackerPoint Position where the robot should go before calling dropping. #> brick_type int8 Type of the brick. #> brick_id int16 Id of the brick. <= successed Whenever the calling for successful. <= failed When the calling failed. <= finished When plan is finished ''' 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 execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if self._finished: return 'finished' if self._failed: return 'failed' else: return 'successed' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._failed = False self._finished = False # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._service_topic): Logger.logerr( 'ProxyServiceCaller: Topic \'{}\' not yet registered!'.format( self._service_topic)) self._failed = True return try: service_request = plan_keeper.srv.BrickPlanRequest() service_request.robot_name = self._robot_name service_result = self._srv.call(self._service_topic, service_request) self._finished = service_result.finished if not service_result.success: self._failed = True Logger.logwarn('Calling \'{}\' was not successful: {}'.format( self._service_topic, str(service_result.message))) else: Logger.loginfo('[ServiceGetNextBrickInPlan]: {}'.format( service_result.message)) service_result.grasp_position.position.z = self._flying_altitude userdata.grasp_position = service_result.grasp_position userdata.placing_position = service_result.drop_position userdata.brick_type = service_result.brick_type userdata.brick_id = service_result.brick_id userdata.wall_layer = service_result.wall_layer userdata.brick_see_wall_typed = service_result.brick_see_wall_typed userdata.next_to_wall_wait_position = service_result.next_to_wall_wait_position drop_wait_position = copy.deepcopy( service_result.drop_wait_position) drop_wait_position.position.z = self._flying_altitude userdata.drop_wait_position = drop_wait_position except Exception as e: Logger.logerr('Failed to call \'{}\' service request: {}'.format( self._service_topic, str(e))) self._failed = True
class ServiceGetExistingCtopPlan(EventState): ''' State for getting an existing plan (planned by some other drone) for robots in Wall challenge in MBZIRC 2020. -- service_topic string Service_topic_name. #> plans Plans how to divide robots in challenge. <= successed Whenever the calling for successful. <= failed When the calling failed. ''' 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 execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if self._failed: return 'failed' else: return 'successed' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._failed = False # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._service_topic): Logger.logerr( 'ProxyServiceCaller: Topic \'{}\' not yet registered!'.format( self._service_topic)) self._failed = True return try: service_request = ctop_planner.srv.GetExistingPlanCTOPRequest() service_result = self._srv.call(self._service_topic, service_request) if not service_result.success: self._failed = True Logger.logwarn('Calling \'{}\' was not successful: {}'.format( self._service_topic, str(service_result.message))) else: Logger.loginfo(service_result.message) userdata.plans = service_result.plans except Exception as e: Logger.logerr('Failed to call \'{}\' service request: {}'.format( self._service_topic, str(e))) self._failed = True
class ServiceGetCtopPlan(EventState): ''' State for getting a plan for robots in Wall challenge in MBZIRC 2020. -- service_topic string Service_topic_name. ># num_robots int16 Number of robots for the planner. ># remaining_time_s int16 Time that remains to finish the challenge. ># max_planning_time_s int16 Time until we want to finish the challenge. ># times_finish_actual_s int16[] Time until the robots will be available. ># builded_brick_ids int16[] Array of already placed bricks. #> plans Plans how to divide robots in challenge. <= successed Whenever the calling for successful. <= failed When the calling failed. ''' 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 execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if self._failed: return 'failed' else: return 'successed' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._failed = False # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._service_topic): Logger.logerr( 'ProxyServiceCaller: Topic \'{}\' not yet registered!'.format( self._service_topic)) self._failed = True return try: service_request = ctop_planner.srv.PlanCTOPRequest() service_request.num_robots = userdata.num_robots service_request.remaining_time_s = userdata.remaining_time_s service_request.max_planning_time_s = userdata.max_planning_time_s service_request.times_finish_actual_s = userdata.times_finish_actual_s service_request.builded_brick_ids = userdata.builded_brick_ids service_result = self._srv.call(self._service_topic, service_request) if not service_result.success: self._failed = True Logger.logwarn('Calling \'{}\' was not successful: {}'.format( self._service_topic, str(service_result.message))) else: Logger.loginfo(service_result.message) userdata.plans = service_result.plans except Exception as e: Logger.logerr('Failed to call \'{}\' service request: {}'.format( self._service_topic, str(e))) self._failed = True
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 ServiceGoToAltitudeState(EventState): ''' State for calling goto_altitude service in MRS system. -- service_topic string Service_topic_name. -- control_manager_diagnostics_topic string control_manager_diagnostics_topic. ># goal double Target altitude. <= successed Whenever the calling for successful. <= failed When the calling failed. ''' 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(ServiceGoToAltitudeState, 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.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('[ServiceGoToAltitudeState]: Successfully subscribed to topic %s' % self._control_manager_diagnostics_topic) self._failed = False def execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if not self._connected: Logger.logerr('[ServiceGoToAltitudeState]: not connected to %s' % self._control_manager_diagnostics_topic) return 'failed' diff_time = rospy.get_time() - self._start_time if self._sub_cont_diag.has_msg(self._control_manager_diagnostics_topic): #Logger.loginfo('[ServiceGoToAltitudeState]: has diagnostics msg at time %s' % (str(diff_time))) message = self._sub_cont_diag.get_last_msg(self._control_manager_diagnostics_topic) self._sub_cont_diag.remove_last_msg(self._control_manager_diagnostics_topic) if diff_time >= 1.0 and not message.tracker_status.have_goal: Logger.loginfo('[ServiceGoToAltitudeState]: Successfully ended following of trajectory %s' % self._control_manager_diagnostics_topic) return 'successed' else: return if self._failed: return 'failed' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._start_time = rospy.get_time() if not self._connected: (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('[ServiceGoToAltitudeState]: Successfully subscribed to previously unavailable topic %s' % self._control_manager_diagnostics_topic) else: Logger.logwarn('[ServiceGoToAltitudeState]: Topic %s still not available, giving up.' % self._control_manager_diagnostics_topic) if self._connected and self._sub_cont_diag.has_msg(self._control_manager_diagnostics_topic): Logger.loginfo('[ServiceGoToAltitudeState]: Waiting for msg from topic %s' % self._control_manager_diagnostics_topic) self._sub_cont_diag.remove_last_msg(self._control_manager_diagnostics_topic) self._failed = False # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._service_topic): Logger.logerr('[ServiceGoToAltitudeState]: ProxyServiceCaller: Topic \'{}\' not yet registered!'.format(self._service_topic)) self._failed = True return try: service_request = mrs_msgs.srv.Vec1Request() service_request.goal = userdata.goal Logger.loginfo('[ServiceGoToAltitudeState]: calling goto_altitude %f'%(userdata.goal)) service_result = self._srv.call(self._service_topic, service_request) if not service_result.success: self._failed = True Logger.logwarn('[ServiceGoToAltitudeState]: Calling \'{}\' was not successful: {}'.format(self._service_topic, str(service_result.message))) else: Logger.loginfo('[ServiceGoToAltitudeState]: {}'.format(str(service_result.message))) except Exception as e: Logger.logerr('[ServiceGoToAltitudeState]: Failed to call \'{}\' service request: {}'.format(self._service_topic, str(e))) self._failed = True def _get_msg_from_path(self, msg_path): msg_import = msg_path.split('/') msg_module = '%s.msg' % (msg_import[0]) package = __import__(msg_module, fromlist=[msg_module]) clsmembers = inspect.getmembers(package, lambda member: inspect.isclass(member) and member.__module__.endswith(msg_import[1])) return clsmembers[0][1]
class ServiceFollowTrajectory(EventState): ''' State for start trajectory following -- service_topic_follow string Service_topic_name to start following -- service_topic_set_trajectory string Service_topic_name to fill trajectory ># scanning_trajectory Trajectory that is followed <= successed Whenever the calling for successful. <= failed When the calling failed. ''' 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 execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. #Logger.logerr('[ServiceFollowTrajectory]: execute') if not self._connected: Logger.logerr('[ServiceFollowTrajectory]: not connected to %s' % self._control_manager_diagnostics_topic) return 'failed' diff_time = rospy.get_time() - self._start_time if self._sub_cont_diag.has_msg(self._control_manager_diagnostics_topic): rospy.loginfo_throttle(5,'[ServiceFollowTrajectory]: has diagnostics msg at time %s' % (str(rospy.get_time()))) message = self._sub_cont_diag.get_last_msg(self._control_manager_diagnostics_topic) self._sub_cont_diag.remove_last_msg(self._control_manager_diagnostics_topic) if diff_time >= 1.0 and not message.tracker_status.tracking_trajectory: Logger.loginfo('[ServiceFollowTrajectory]: Successfully ended following of trajectory %s' % self._control_manager_diagnostics_topic) return 'successed' else: return if self._failed or diff_time >= len(userdata.scanning_trajectory) * 0.2 * 1.2: Logger.logerr('Failed follow trajectory \'{}\' within time {} s out of {} s'.format(self._service_topic_follow, str(diff_time),len(userdata.scanning_trajectory) * 0.2 * 1.2)) return 'failed' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. #Logger.logerr('[ServiceFollowTrajectory]: enter') self._start_time = rospy.get_time() if not self._connected: (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 previously unavailable topic %s' % self._control_manager_diagnostics_topic) else: Logger.logwarn('[ServiceFollowTrajectory]: Topic %s still not available, giving up.' % self._control_manager_diagnostics_topic) if self._connected and self._sub_cont_diag.has_msg(self._control_manager_diagnostics_topic): Logger.loginfo('[ServiceFollowTrajectory]: Waiting for msg from topic %s' % self._control_manager_diagnostics_topic) self._sub_cont_diag.remove_last_msg(self._control_manager_diagnostics_topic) self._failed = False # Check if the ProxyServiceCaller has been registered if not self._srv_follow.is_available(self._service_topic_follow): Logger.logerr('[ServiceFollowTrajectory]: Topic \'{}\' not yet registered!'.format(self._service_topic_follow)) self._failed = True return if not self._srv_set_trajectory.is_available(self._service_topic_set_trajectory): Logger.logerr('[ServiceFollowTrajectory]: Topic \'{}\' not yet registered!'.format(self._service_topic_set_trajectory)) self._failed = True return try: service_request_set_trajectory = mrs_msgs.srv.TrajectoryReferenceSrvRequest()#TrackerTrajectorySrvRequest() trajectory_msg = mrs_msgs.msg.TrajectoryReference() trajectory_msg.header = std_msgs.msg.Header() trajectory_msg.header.stamp = rospy.Time.now() trajectory_msg.header.seq = 0 trajectory_msg.header.frame_id = userdata.frame_id trajectory_msg.use_heading = True trajectory_msg.fly_now = True trajectory_msg.loop = False trajectory_msg.points = userdata.scanning_trajectory service_request_set_trajectory.trajectory = trajectory_msg service_result_set_trajectory = self._srv_set_trajectory.call(self._service_topic_set_trajectory,service_request_set_trajectory) Logger.logwarn('[ServiceFollowTrajectory]: Called \'{}\' in ServiceFollowTrajectory set trejectory'.format(self._service_topic_set_trajectory)) if not service_result_set_trajectory.success: self._failed = True Logger.logwarn('[ServiceFollowTrajectory]: Calling \'{}\' was not successful'.format(self._service_topic_set_trajectory)) else: Logger.loginfo('[ServiceFollowTrajectory]: Calling \'{}\' was successful'.format(self._service_topic_set_trajectory)) except Exception as e: Logger.logerr('Failed to call \'{}\' service request: {}'.format(self._service_topic_follow, str(e))) self._failed = True def _get_msg_from_path(self, msg_path): msg_import = msg_path.split('/') msg_module = '%s.msg' % (msg_import[0]) package = __import__(msg_module, fromlist=[msg_module]) clsmembers = inspect.getmembers(package, lambda member: inspect.isclass(member) and member.__module__.endswith(msg_import[1])) return clsmembers[0][1]
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 ServiceStringState(EventState): ''' State for calling std_srvs/Trigger type of services. -- service_topic string Service_topic_name. -- state_name string Name of the state used during printing. -- msg_text string text msg. <= finished Whenever the calling for successful. <= failed When the calling failed. ''' 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 execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if self._failed: return 'failed' else: return 'finished' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._failed = False # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._service_topic): Logger.logerr( '{}ProxyServiceCaller: Topic \'{}\' not yet registered!'. format(self._state_name, self._service_topic)) self._failed = True return try: service_request = mrs_msgs.srv.StringRequest() service_request.value = self._msg_text service_result = self._srv.call(self._service_topic, service_request) if not service_result.success: self._failed = True Logger.logwarn( '{}Calling \'{}\' was not successful: {}'.format( self._state_name, self._service_topic, str(service_result.message))) else: Logger.loginfo(self._state_name + str(service_result.message)) except Exception as e: Logger.logerr('{}Failed to call \'{}\' service request: {}'.format( self._state_name, self._service_topic, str(e))) self._failed = True
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 ServiceIsBrickPlaceable(EventState): ''' State for getting whether a brick can be placed in Wall challenge in MBZIRC 2020. -- service_topic string Service_topic_name. ># brick_id Id of current brick being placed <= placeable When we can place the brick. <= wait When we need to wait for placing. <= failed When the calling failed. ''' 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 execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if self._failed: return 'failed' else: if self._placeable: return 'placeable' else: return 'wait' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._failed = False self._placeable = False # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._service_topic): Logger.logerr('ProxyServiceCaller: Topic \'{}\' not yet registered!'.format(self._service_topic)) self._failed = True return try: service_request = plan_keeper.srv.IsBrickPlaceableRequest() service_request.brick_id = userdata.brick_id Logger.loginfo('ask if brick %d is placeable'%(userdata.brick_id)) service_result = self._srv.call(self._service_topic, service_request) if not service_result.success: self._failed = True Logger.logwarn('Calling \'{}\' was not successful: {}'.format(self._service_topic, str(service_result.message))) else: Logger.loginfo(service_result.message) self._placeable = service_result.placeable except Exception as e: Logger.logerr('Failed to call \'{}\' service request: {}'.format(self._service_topic, str(e))) self._failed = True
class MavrosSetModeState(EventState): ''' Request to switch FCU operation mode using mavros service: "mavros/set_mode". -- mode string Specifies to which mode the FCU shoul be switch. <= finished Whenever the switching for successful. <= failed When the switching failed. ''' def __init__(self, request): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(MavrosSetModeState, self).__init__(outcomes=['finished', 'failed']) # Store state parameter for later use. self._request = request self._service_topic = rospy.resolve_name("mavros/set_mode") # Create proxy service client self._srv = ProxyServiceCaller( {self._service_topic: mavros_msgs.srv.SetMode}) self._failed = False def execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if self._failed: return 'failed' else: return 'finished' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._failed = False # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._service_topic): Logger.logerr( '[MavrosSetMode]: ProxyServiceCaller: Topic \'{}\' not yet registered!' .format(self._service_topic)) self._failed = True return try: service_request = mavros_msgs.srv.SetModeRequest() service_request.custom_mode = self._request service_result = self._srv.call(self._service_topic, service_request) if not service_result.mode_sent: self._failed = True Logger.logwarn( '[MavrosSetMode]: Calling \'{}\' was not successful: {}'. format(self._service_topic, str(service_result.result))) else: Logger.loginfo( '[MavrosSetMode]: Mavros mode set to: {}'.format( self._request)) except Exception as e: Logger.logerr( '[MavrosSetMode]: Failed to call \'{}\' service request: {}'. format(self._service_topic, 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 ServiceSetMappedObjectsIntoPlanKeeper(EventState): ''' State for saving the plans for robots in Wall challenge in MBZIRC 2020 into plan_keeper node. -- service_topic string Service_topic_name. ># mapped_objects Mapped walls and bricks. <= successed Whenever the calling for successful. <= failed When the calling failed. ''' def __init__(self, service_topic): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(ServiceSetMappedObjectsIntoPlanKeeper, self).__init__(input_keys=['mapped_objects'], 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.SetMappedPositions}) self._failed = False def execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if self._failed: return 'failed' else: return 'successed' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._failed = False # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._service_topic): Logger.logerr( '[ServiceSetMappedObjectsIntoPlanKeeper]: Topic \'{}\' not yet registered!' .format(self._service_topic)) self._failed = True return try: service_request = plan_keeper.srv.SetMappedPositionsRequest() service_request.mapped_objects = userdata.mapped_objects Logger.loginfo( '[ServiceSetMappedObjectsIntoPlanKeeper]: sending mapped_objects to plan_keeper' ) Logger.loginfo(str(service_request.mapped_objects)) service_result = self._srv.call(self._service_topic, service_request) if not service_result.success: self._failed = True Logger.logwarn( '[ServiceSetMappedObjectsIntoPlanKeeper]: Calling \'{}\' was not successful: {}' .format(self._service_topic, str(service_result.message))) else: Logger.loginfo(service_result.message) except Exception as e: Logger.logerr( '[ServiceSetMappedObjectsIntoPlanKeeper]: Failed to call \'{}\' service request: {}' .format(self._service_topic, 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 MavrosArmState(EventState): ''' Request to arm or disarm a robot by calling the mavros service: "mavros/cmd/arming". -- request bool Request that specifies if a robot should be arm or disarm. <= finished Whenever the calling for successful. <= failed When the calling failed. ''' 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 execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if self._failed: return 'failed' else: return 'finished' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._failed = False # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._service_topic): Logger.logerr( 'ProxyServiceCaller: Topic \'{}\' not yet registered!'.format( self._service_topic)) self._failed = True return try: service_request = mavros_msgs.srv.CommandBoolRequest() service_request.value = self._request service_result = self._srv.call(self._service_topic, service_request) if not service_result.success: self._failed = True Logger.logwarn('Calling \'{}\' was not successful: {}'.format( self._service_topic, str(service_result.result))) else: if self._request: Logger.loginfo('[MavrosArmState]: Robot armed.') else: Logger.loginfo('[MavrosArmState]: Robot disarmed.') except Exception as e: Logger.logerr('Failed to call \'{}\' service request: {}'.format( self._service_topic, str(e))) self._failed = True