class AveragingSVR2(object): def __init__(self): self._action = SimpleActionServer('averaging', AveragingAction, auto_start=False) self._action.register_preempt_callback(self.preempt_cb) self._action.register_goal_callback(self.goal_cb) self.reset_numbers() rospy.Subscriber('number', Float32, self.execute_loop) self._action.start() def std_dev(self, lst): ave = sum(lst) / len(lst) return sum([x * x for x in lst]) / len(lst) - ave**2 def goal_cb(self): self._goal = self._action.accept_new_goal() rospy.loginfo('goal callback %s' % (self._goal)) def preempt_cb(self): rospy.loginfo('preempt callback') self.reset_numbers() self._action.set_preempted(text='message for preempt') def reset_numbers(self): self._samples = [] def execute_loop(self, msg): if (not self._action.is_active()): return self._samples.append(msg.data) feedback = AveragingAction().action_feedback.feedback feedback.sample = len(self._samples) feedback.data = msg.data feedback.mean = sum(self._samples) / len(self._samples) feedback.std_dev = self.std_dev(self._samples) self._action.publish_feedback(feedback) ## sending result if (len(self._samples) >= self._goal.samples): result = AveragingAction().action_result.result result.mean = sum(self._samples) / len(self._samples) result.std_dev = self.std_dev(self._samples) rospy.loginfo('result: %s' % (result)) self.reset_numbers() if (result.mean > 0.5): self._action.set_succeeded(result=result, text='message for succeeded') else: self._action.set_aborted(result=result, text='message for aborted')
class MockNavigation(): def __init__(self, move_base_topic): self.robot_pose_ = PoseStamped() self.listener = tf.TransformListener() self.navigation_succedes = True self.reply = False self.preempted = 0 self.moves_base = False self.target_position = Point() self.target_orientation = Quaternion() self.move_base_as_ = SimpleActionServer( move_base_topic, MoveBaseAction, execute_cb = self.move_base_cb, auto_start = False) self.move_base_as_.start() def __del__(self): self.move_base_as_.__del__() def move_base_cb(self, goal): rospy.loginfo('move_base_cb') self.target_position = goal.target_pose.pose.position self.target_orientation = goal.target_pose.pose.orientation self.moves_base = True while not self.reply: rospy.sleep(0.2) (trans, rot) = self.listener.lookupTransform('/map', '/base_footprint', rospy.Time(0)) self.robot_pose_.pose.position.x = trans[0] self.robot_pose_.pose.position.y = trans[1] feedback = MoveBaseFeedback() feedback.base_position.pose.position.x = \ self.robot_pose_.pose.position.x feedback.base_position.pose.position.y = \ self.robot_pose_.pose.position.y self.move_base_as_.publish_feedback(feedback) if self.move_base_as_.is_preempt_requested(): self.preempted += 1 rospy.loginfo("Preempted!") self.moves_base = False self.move_base_as_.set_preempted() return None if self.move_base_as_.is_new_goal_available(): self.preempted += 1 self.move_base_cb(self.move_base_as_.accept_new_goal()) return None else: result = MoveBaseResult() self.reply = False self.preempted = 0 self.moves_base = False self.target_position = Point() self.target_orientation = Quaternion() if self.navigation_succedes: self.move_base_as_.set_succeeded(result) else: self.move_base_as_.set_aborted(result)
class move_base_fake_node: def __init__(self): self.action_server = SimpleActionServer( 'move_base', MoveBaseAction, execute_cb=self.execute_callback, auto_start=False ) # Simple action server will pretend to be move_base # Movement goal state self.goal = None # Is a goal set self.valid_goal = False # Is this a valid goal self.current_goal_started = False # Has the goal been started (i.e. have we told our Bug algorithm to use this point and start) self.current_goal_complete = False # Has the Bug algorithm told us it completed self.position = None # move_base feedback reports the current direction ## TO DO!! # Need a service provided by this node or something for the Bug algorithm to tell us it is done # Bug service to start and stop Bug algorithm # Bug service to set a new goal in Bug algorithm # rospy.wait_for_service() # rospy.wait_for_service() self.subscriber_odometry = rospy.Subscriber( 'odom/', Odometry, self.callback_odometry ) # We need to read the robots current point for the feedback self.subscriber_simple_goal = rospy.Subscriber( '/move_base_simple/goal/', PoseStamped, self.callback_simple_goal ) # Our return goal is done with /move_base_simple/goal/ self.goal_pub = rospy.Publisher( '/move_base/goal/', MoveBaseActionGoal, queue_size=10) # /move_base_simple/goal/ gets published here self.action_server.start() def execute_callback(self, move_base_goal): self.goal = move_base_goal.target_pose.pose # Set the provided goal as the current goal rospy.logdebug('[Move Base Fake] Execute Callback: {}'.format( str(self.goal.position))) self.valid_goal = True # Assume it is valid self.current_goal_started = False # It hasnt started yet self.current_goal_complete = False # It hasnt been completed r = rospy.Rate(1) while not rospy.is_shutdown(): # Always start by checking if there is a new goal that preempts the current one if self.action_server.is_preempt_requested(): ## TO DO!! # Tell Bug algorithm to stop before changing or stopping goal if self.action_server.is_new_goal_available(): new_goal = self.action_server.accept_new_goal( ) # There is a new goal rospy.logdebug('[Move Base Fake] New Goal: {}'.format( str(self.goal.position))) self.goal = new_goal.target_pose.pose # Set the provided goal as the current goal self.valid_goal = True # Assume it is valid self.current_goal_started = False # It hasnt started yet self.current_goal_complete = False # It hasnt been completed else: self.action_server.set_preempted( ) # No new goal, we've just been told to stop self.goal = None # Stop everything self.valid_goal = False self.current_goal_started = False self.current_goal_complete = False return # Start goal if self.valid_goal and not self.current_goal_started: rospy.logdebug('[Move Base Fake] Starting Goal') ## TO DO !! # Call the Bug services/topics etc to tell Bug algorithm new target point and then to start self.current_goal_started = True # Only start once # Feedback ever loop just reports current location feedback = MoveBaseFeedback() feedback.base_position.pose.position = self.position self.action_server.publish_feedback(feedback) # Completed is set in a callback that you need to link to a service or subscriber if self.current_goal_complete: rospy.logdebug('[Move Base Fake] Finishing Goal') ## TO DO!! # Tell Bug algorithm to stop before changing or stopping goal self.goal = None # Stop everything self.valid_goal = False self.current_goal_started = False self.current_goal_complete = False self.action_server.set_succeeded( MoveBaseResult(), 'Goal reached') # Send success message return r.sleep() # Shutdown rospy.logdebug('[Move Base Fake] Shutting Down') ## TO DO!! # Tell Bug algorithm to stop before changing or stopping goal self.goal = None # Stop everything self.valid_goal = False self.current_goal_started = False self.current_goal_complete = False # you need to connect this to something being called/published from the Bug algorithm def callback_complete(self, success): # TO DO!! # Implement some kind of service or subscriber so the Bug algorithm can tell this node it is complete self.current_goal_complete = success.data def callback_odometry(self, odom): self.position = odom.pose.pose.position # Simple goals get republished to the correct topic def callback_simple_goal(self, goal): rospy.logdebug('[Move Base Fake] Simple Goal: {}'.format( str(goal.pose.position))) action_goal = MoveBaseActionGoal() action_goal.header.stamp = rospy.Time.now() action_goal.goal.target_pose = goal self.goal_pub.publish(action_goal)
class MotionService(object): def __init__(self): rospy.init_node('motion_service') # Load config config_loader = RobotConfigLoader() try: robot_config_name = rospy.get_param(rospy.get_name() + '/robot_config') except KeyError: rospy.logwarn('Could not find robot config param in /' + rospy.get_name +'/robot_config. Using default config for ' 'Thor Mang.') robot_config_name = 'thor' config_loader.load_xml_by_name(robot_config_name + '_config.xml') # Create publisher for first target if len(config_loader.targets) > 0: self._motion_publisher = MotionPublisher( config_loader.robot_config, config_loader.targets[0].joint_state_topic, config_loader.targets[0].publisher_prefix) else: rospy.logerr('Robot config needs to contain at least one valid target.') self._motion_data = MotionData(config_loader.robot_config) # Subscriber to start motions via topic self.motion_command_sub = rospy.Subscriber('motion_command', ExecuteMotionCommand, self.motion_command_request_cb) # Create action server self._action_server = SimpleActionServer(rospy.get_name() + '/motion_goal', ExecuteMotionAction, None, False) self._action_server.register_goal_callback(self._execute_motion_goal) self._action_server.register_preempt_callback(self._preempt_motion_goal) self._preempted = False def _execute_motion_goal(self): goal = self._action_server.accept_new_goal() # Check if motion exists if goal.motion_name not in self._motion_data: result = ExecuteMotionResult() result.error_code = [ExecuteMotionResult.MOTION_UNKNOWN] result.error_string = "The requested motion is unknown." self._action_server.set_aborted(result) return # check if a valid time_factor is set, otherwise default to 1.0 if goal.time_factor <= 0.0: goal.time_factor = 1.0 self._preempted = False self._motion_publisher.publish_motion(self._motion_data[goal.motion_name], goal.time_factor, self._trajectory_finished) print '[MotionService] New action goal received.' def _preempt_motion_goal(self): self._motion_publisher.stop_motion() print '[MotionService] Preempting goal.' self._preempted = True def _trajectory_finished(self, error_codes, error_groups): result = ExecuteMotionResult() result.error_code = error_codes error_string = "" for error_code, error_group in zip(error_codes, error_groups): error_string += error_group + ': ' + str(error_code) + '\n' result.error_string = error_string if self._preempted: self._action_server.set_preempted(result) else: self._action_server.set_succeeded(result) print '[MotionService] Trajectory finished with error code: \n', error_string def _execute_motion(self, request): response = ExecuteMotionResponse() # check if a motion with this name exists if request.motion_name not in self._motion_data: print '[MotionService] Unknown motion name: "%s"' % request.motion_name response.ok = False response.finish_time.data = rospy.Time.now() return response # check if a valid time_factor is set, otherwise default to 1.0 if request.time_factor <= 0.0: request.time_factor = 1.0 # find the duration of the requested motion motion_duration = 0.0 for poses in self._motion_data[request.motion_name].values(): if len(poses) > 0: endtime = poses[-1]['starttime'] + poses[-1]['duration'] motion_duration = max(motion_duration, endtime) motion_duration *= request.time_factor # execute motion print '[MotionService] Executing motion: "%s" (time factor: %.3f, duration %.2fs)' % (request.motion_name, request.time_factor, motion_duration) self._motion_publisher.publish_motion(self._motion_data[request.motion_name], request.time_factor) # reply with ok and the finish_time of this motion response.ok = True response.finish_time.data = rospy.Time.now() + rospy.Duration.from_sec(motion_duration) return response def start_server(self): rospy.Service(rospy.get_name() + '/execute_motion', ExecuteMotion, self._execute_motion) self._action_server.start() print '[MotionService] Waiting for calls...' rospy.spin() def motion_command_request_cb(self, command): print "[MotionService] Initiate motion command via topic ..." request = ExecuteMotion() request.motion_name = command.motion_name request.time_factor = command.time_factor self._execute_motion(request) print "[MotionService] Motion command complete"
class ActionPerformer(Node, ABC): def __init__(self, name: str): super().__init__(name, "action") self.name = name # Service for action point computation self._action_point_server = Service(get_action_point_service(name), ComputeActionPoint, self._compute_action_point) # Action server for this action self._action_server = SimpleActionServer(get_action_server(name), PerformAction, auto_start=False) self._action_server.register_goal_callback(self._on_goal) self._action_server.register_preempt_callback(self._on_preempt) self._action_server.start() # Function managing the action def returns(self, state=ActionStatus.DONE): ''' Function to call when the action is interrupted with a given state ''' # Create result message result = PerformResult() result.state = state # Send back to client self._action_server.set_succeeded(result) # Hidden handlers def _compute_action_point( self, request: ComputeActionPointRequest) -> ComputeActionPointResponse: # Extract data from request and call performer's function result: ActionPoint = self.compute_action_point( Argumentable().from_list(request.args), request.robot_pos) response = ComputeActionPointResponse() response.action_point = result return response def _on_goal(self): if self._action_server.is_new_goal_available(): goal: PerformGoal = self._action_server.accept_new_goal() # run action with given args self.start(Argumentable().from_list(goal.arguments)) def _on_preempt(self): self.cancel() self._action_server.set_preempted() # Function to be defined by inherited actions @abstractmethod def compute_action_point(self, args: Argumentable, robot_pos: Pose2D) -> ActionPoint: pass @abstractmethod def start(self, args: Argumentable): pass def cancel(self): pass
class MotionService(object): def __init__(self): rospy.init_node('motion_service') # Load config config_loader = RobotConfigLoader() try: robot_config_name = rospy.get_param(rospy.get_name() + '/robot_config') except KeyError: rospy.logwarn('Could not find robot config param in /' + rospy.get_name + '/robot_config. Using default config for ' 'Thor Mang.') robot_config_name = 'thor' config_loader.load_xml_by_name(robot_config_name + '_config.xml') # Create publisher for first target if len(config_loader.targets) > 0: self._motion_publisher = MotionPublisher( config_loader.robot_config, config_loader.targets[0].joint_state_topic, config_loader.targets[0].publisher_prefix) else: rospy.logerr( 'Robot config needs to contain at least one valid target.') self._motion_data = MotionData(config_loader.robot_config) # Subscriber to start motions via topic self.motion_command_sub = rospy.Subscriber( 'motion_command', ExecuteMotionCommand, self.motion_command_request_cb) # Create action server self._action_server = SimpleActionServer( rospy.get_name() + '/motion_goal', ExecuteMotionAction, None, False) self._action_server.register_goal_callback(self._execute_motion_goal) self._action_server.register_preempt_callback( self._preempt_motion_goal) self._preempted = False def _execute_motion_goal(self): goal = self._action_server.accept_new_goal() # Check if motion exists if goal.motion_name not in self._motion_data: result = ExecuteMotionResult() result.error_code = [ExecuteMotionResult.MOTION_UNKNOWN] result.error_string = "The requested motion is unknown." self._action_server.set_aborted(result) return # check if a valid time_factor is set, otherwise default to 1.0 if goal.time_factor <= 0.0: goal.time_factor = 1.0 self._preempted = False self._motion_publisher.publish_motion( self._motion_data[goal.motion_name], goal.time_factor, self._trajectory_finished) print '[MotionService] New action goal received.' def _preempt_motion_goal(self): self._motion_publisher.stop_motion() print '[MotionService] Preempting goal.' self._preempted = True def _trajectory_finished(self, error_codes, error_groups): result = ExecuteMotionResult() result.error_code = error_codes error_string = "" for error_code, error_group in zip(error_codes, error_groups): error_string += error_group + ': ' + str(error_code) + '\n' result.error_string = error_string if self._preempted: self._action_server.set_preempted(result) else: self._action_server.set_succeeded(result) print '[MotionService] Trajectory finished with error code: \n', error_string def _execute_motion(self, request): response = ExecuteMotionResponse() # check if a motion with this name exists if request.motion_name not in self._motion_data: print '[MotionService] Unknown motion name: "%s"' % request.motion_name response.ok = False response.finish_time.data = rospy.Time.now() return response # check if a valid time_factor is set, otherwise default to 1.0 if request.time_factor <= 0.0: request.time_factor = 1.0 # find the duration of the requested motion motion_duration = 0.0 for poses in self._motion_data[request.motion_name].values(): if len(poses) > 0: endtime = poses[-1]['starttime'] + poses[-1]['duration'] motion_duration = max(motion_duration, endtime) motion_duration *= request.time_factor # execute motion print '[MotionService] Executing motion: "%s" (time factor: %.3f, duration %.2fs)' % ( request.motion_name, request.time_factor, motion_duration) self._motion_publisher.publish_motion( self._motion_data[request.motion_name], request.time_factor) # reply with ok and the finish_time of this motion response.ok = True response.finish_time.data = rospy.Time.now() + rospy.Duration.from_sec( motion_duration) return response def start_server(self): rospy.Service(rospy.get_name() + '/execute_motion', ExecuteMotion, self._execute_motion) self._action_server.start() print '[MotionService] Waiting for calls...' rospy.spin() def motion_command_request_cb(self, command): print "[MotionService] Initiate motion command via topic ..." request = ExecuteMotion() request.motion_name = command.motion_name request.time_factor = command.time_factor self._execute_motion(request) print "[MotionService] Motion command complete"
class Server(object): def __init__(self, name): rospy.loginfo("Starting '{test}'.".format(test=name)) self._as = SimpleActionServer(name, WayPointNavAction, auto_start=False) self._as.register_goal_callback(self.execute_cb) self._as.register_preempt_callback(self.preempt_cb) self.listener = tf.TransformListener() self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) self.sub = None self._as.start() rospy.loginfo("Started '{test}'.".format(test=name)) def execute_cb(self): self.goal = self._as.accept_new_goal() print self.goal self.sub = rospy.Subscriber("/orb_slam/pose", PoseStamped, self.pose_cb) def preempt_cb(self, *args): self.sub.unregister() self.sub = None t = Twist() self.pub.publish(t) self._as.set_preempted(WayPointNavResult()) def pose_cb(self, msg): dist = self.get_dist(self.goal.goal, msg) t = Twist() new_goal = self._transform(self.goal.goal, "camera_pose") print new_goal print "Dist", dist theta = self.get_theta(new_goal.pose) print "Theta", theta if dist < 0.02: self._as.set_succeeded(WayPointNavResult()) self.sub.unregister() self.sub = None t.linear.x = 0.0 t.angular.z = 0.0 else: t.linear.x = 0.03 t.angular.z = theta * .6 self.pub.publish(t) def get_dist(self, pose1, pose2): return np.sqrt(np.power(pose1.pose.position.x-pose2.pose.position.x, 2)+np.power(pose1.pose.position.y-pose2.pose.position.y, 2)) def _transform(self, msg, target_frame): if msg.header.frame_id != target_frame: try: t = self.listener.getLatestCommonTime(target_frame, msg.header.frame_id) msg.header.stamp = t return self.listener.transformPose(target_frame, msg) except (tf.Exception, tf.LookupException, tf.ConnectivityException) as ex: rospy.logwarn(ex) return None else: return msg def get_theta(self, pose): return np.arctan2(pose.position.y, pose.position.x)