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