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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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