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