def __init__(self): self._server_name = server_name self._action_spec = action_spec self._action_server = SimpleActionServer( self._server_name, self._action_spec, execute_cb = self.execute_cb, auto_start = False)
def __init__(self, action_server_name): self.action_server_name = action_server_name self.action_server = SimpleActionServer(self.action_server_name, TargetAction, auto_start=False) self.action_server.register_goal_callback(self.__goal_callback) self.origin_frame = rospy.get_param('~origin_frame', 'base_link') self.success_distance = rospy.get_param('~success_distance', 0.2) self.end_effector_frame = rospy.get_param('~end_effector_frame', 'gaze') self.rate = rospy.Rate(rospy.get_param('~hz', 10))
def __init__(self, name): rospy.loginfo("Starting %s" % name) self.client = None action_list = rospy.get_param("han_action_dispatcher")["han_actions"] self.default_action = rospy.get_param("~default_action") self.dyn_srv = DynServer(HanActionDispatcherConfig, self.dyn_callback) self.servers = {} for a in action_list.items(): name = a[0] self.servers[name] = SimpleActionServer( name, MoveBaseAction, execute_cb=(lambda x: lambda msg: self.execute_cb( msg, x[0], x[1]["action"]))(a), auto_start=False) self.servers[name].register_preempt_callback(self.preempt_cb) self.servers[name].start() rospy.loginfo("done")
def __init__(self): rospy.init_node(NODE) """ Parameters """ max_linear_velocity = rospy.get_param('max_linear_velocity', 0.3) max_linear_acceleration = rospy.get_param('max_linear_acceleration', 0.05) linear_tolerance = rospy.get_param('linear_tolerance', 0.02) max_angular_velocity = rospy.get_param('max_angular_velocity', 0.2) max_angular_acceleration = rospy.get_param('max_angular_acceleration', 0.03) angular_tolerance = rospy.get_param('angular_tolerance', 0.02) abort_if_obstacle_detected = rospy.get_param( 'abort_if_obstacle_detected', True) self._controller_frequency = rospy.get_param('controller_frequency', 10.0) controller_type = rospy.get_param('~controller', self.CONTROLLER_TYPE_UNSPECIFIED) if controller_type == self.CONTROLLER_TYPE_DIFF: #Create diff controller self._velocity_controller = DiffVelocityController( max_linear_velocity, linear_tolerance, max_angular_velocity, angular_tolerance) elif controller_type == self.CONTROLLER_TYPE_OMNI: self._velocity_controller = OmniVelocityController( max_linear_velocity, linear_tolerance, max_angular_velocity, angular_tolerance) """ ========================= YOUR CODE HERE ========================= Instructions: create an instance of OmniVelocityController. Hint: you may copy-paste from the DiffVelocityController case and adjust the arguments in the call to the constructor to conform to what you have implemented in that class. """ elif controller_type == self.CONTROLLER_TYPE_UNSPECIFIED: rospy.logerr('Controller type not specified. ' 'Check the [controller] launch parameter') exit() else: #Unknown controller rospy.logerr('Requested controller type "{0}" unknown. ' 'Check the [controller] launch parameter'.format( controller_type)) exit() """ Publishers """ self._velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10) self._current_goal_publisher = rospy.Publisher(NODE + '/current_goal', PoseStamped, latch=True, queue_size=0) self._action_goal_publisher = rospy.Publisher(NODE + '/move_to/goal', MoveToActionGoal, queue_size=1) """ Subscribers """ self._simple_goal_subscriber = rospy.Subscriber( NODE + '/move_to_simple/goal', PoseStamped, self._simple_goal_callback, queue_size=1) if abort_if_obstacle_detected: self._obstacles_subscriber = rospy.Subscriber( 'obstacles', Obstacle, self._obstacles_callback, queue_size=100) """ Action server """ self._move_to_server = SimpleActionServer(NODE + '/move_to', MoveToAction, self._move_to_callback, auto_start=False) self._move_to_server.start() self._tf = tf.TransformListener() rospy.loginfo('Started [motion_controller] node.')
def __init__(self, server_name, action_spec, wrapped_container, succeeded_outcomes=[], aborted_outcomes=[], preempted_outcomes=[], goal_key='action_goal', feedback_key='action_feedback', result_key='action_result', goal_slots_map={}, feedback_slots_map={}, result_slots_map={}, expand_goal_slots=False, pack_result_slots=False): """Constructor. @type server_name: string @param server_name: The name of the action server that this container will present. @type action_spec: actionlib action msg @param action_spec: The type of action this server will present @type wrapped_container: L{StateMachine} @param wrapped_container: The state machine to manipulate @type succeeded_outcomes: array of strings @param succeeded_outcomes: Array of terminal state labels which, when left, should cause the action server to return SUCCEEDED as a result status. @type aborted_outcomes: array of strings @param aborted_outcomes: Array of terminal state labels which, when left, should cause the action server to return ABORTED as a result status. @type preempted_outcomes: array of strings @param preempted_outcomes: Array of terminal state labels which, when left, should cause the action server to return PREEMPTED as a result status. @type goal_key: string @param goal_key: The userdata key into which the action goal should be stuffed when the action server receives one. @type feedback_key: string @param feedback_key: The userdata key into which the SMACH container can put feedback information relevant to the action. @type result_key: string @param result_key: The userdata key into which the SMACH container can put result information from this action. """ # Store state machine self.wrapped_container = wrapped_container """State machine that this wrapper talks to.""" # Register state machine callbacks self.wrapped_container.register_transition_cb(self.transition_cb) self.wrapped_container.register_termination_cb(self.termination_cb) # Grab reference to state machine user data (the user data in the children # states scope) self.userdata = smach.UserData() # Store special userdata keys self._goal_key = goal_key self._feedback_key = feedback_key self._result_key = result_key self._goal_slots_map = goal_slots_map self._feedback_slots_map = feedback_slots_map self._result_slots_map = result_slots_map self._expand_goal_slots = expand_goal_slots self._pack_result_slots = pack_result_slots # Store goal, result, and feedback types self.userdata[self._goal_key] = copy.copy( action_spec().action_goal.goal) self.userdata[self._result_key] = copy.copy( action_spec().action_result.result) self.userdata[self._feedback_key] = copy.copy( action_spec().action_feedback.feedback) # Action info self._server_name = server_name self._action_spec = action_spec # Construct action server (don't start it until later) self._action_server = SimpleActionServer(self._server_name, self._action_spec, execute_cb=self.execute_cb, auto_start=False) # Store and check the terminal outcomes self._succeeded_outcomes = set(succeeded_outcomes) self._aborted_outcomes = set(aborted_outcomes) self._preempted_outcomes = set(preempted_outcomes) # Make sure the sets are disjoint card_of_unions = len(self._succeeded_outcomes | self._aborted_outcomes | self._preempted_outcomes) sum_of_cards = (len(self._succeeded_outcomes) + len(self._aborted_outcomes) + len(self._preempted_outcomes)) if card_of_unions != sum_of_cards: rospy.logerr( "Succeeded, aborted, and preempted outcome lists were not mutually disjoint... expect undefined behavior." )