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, 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))
class ITargetActionServer():
__metaclass__ = abc.ABCMeta
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 start(self):
self.action_server.start()
def __goal_callback(self):
target_goal = self.action_server.accept_new_goal()
rospy.loginfo("Target goal received: " + str(target_goal))
self.execute(target_goal)
@abc.abstractmethod
def execute(self, target_goal):
"""
Run your gaze algorithm to make an end effector on the robot target a particular entity.
"""
def send_feedback(self, distance_to_target):
""" Call this method to send feedback about the distance to the target """
feedback = TargetFeedback()
feedback.distance_to_target = distance_to_target
self.action_server.publish_feedback(feedback)
rospy.loginfo("Target feedback. distance_to_target: {0}".format(distance_to_target))
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, 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")
class ITargetActionServer():
__metaclass__ = abc.ABCMeta
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 start(self):
self.action_server.start()
def __goal_callback(self):
target_goal = self.action_server.accept_new_goal()
rospy.loginfo("Target goal received: " + str(target_goal))
self.execute(target_goal)
@abc.abstractmethod
def execute(self, target_goal):
"""
Run your gaze algorithm to make an end effector on the robot target a particular entity.
"""
def send_feedback(self, distance_to_target):
""" Call this method to send feedback about the distance to the target """
feedback = TargetFeedback()
feedback.distance_to_target = distance_to_target
self.action_server.publish_feedback(feedback)
rospy.loginfo("Target feedback. distance_to_target: {0}".format(
distance_to_target))
class MotionControllerNode:
"""
This is a port of the AMR C++ MotionControllerNode
"""
CONTROLLER_TYPE_DIFF = 'diff'
CONTROLLER_TYPE_OMNI = 'omni'
CONTROLLER_TYPE_UNSPECIFIED = 'unsp'
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 _move_to_callback(self, move_to_goal):
"""
Triggered with a request to move_to action server
"""
rospy.loginfo('Received [move_to] action command.')
if not self._set_new_goal(move_to_goal):
return
else:
rate = rospy.Rate(self._controller_frequency)
while not rospy.is_shutdown():
if not self._move_to_server.is_active():
# Exit if the goal was aborted
return
if self._move_to_server.is_preempt_requested():
# Process pending preemption requests
rospy.loginfo('Action preemption requested.')
if (self._move_to_server.is_new_goal_available()
and self._set_new_goal(
self._move_to_server.accept_new_goal())):
# New goal already set
pass
else:
# No new goals, preempt explicitly and exit the callback
self._publish_zero_velocity()
self._move_to_server.set_preempted()
return
if not self._move_towards_goal():
# Finish execution if the goal was reached
self._move_to_server.set_succeeded(MoveToResult(),
'Goal reached.')
self._publish_zero_velocity()
return
rate.sleep()
self._move_to_server.set_aborted(
MoveToResult(), 'Aborted. The node has been killed.')
def _simple_goal_callback(self, target_pose):
"""
Wrapper for simple goal action. Forwards as a request to the move_to
action server. Has to be tested!
"""
rospy.loginfo(
'Received target pose through the "simple goal" topic.'
'Wrapping it in the action message and forwarding to the server.')
rospy.logwarn('Simple goal control is yet to be tested!')
action_goal = MoveToActionGoal()
action_goal.header.stamp = rospy.Time.now()
action_goal.goal.target_pose = target_pose
self._action_goal_publisher.publish(action_goal)
def _obstacles_callback(self, obstacle_msg):
rospy.logwarn(
'An obstacle was detected. Will stop the robot and cancel the current action.'
)
if self._move_to_server.is_active():
self._move_to_server.set_aborted(
MoveToResult(), 'Obstacle encountered, aborting...')
self._publish_zero_velocity()
def _move_towards_goal(self):
try:
time = self._tf.getLatestCommonTime("odom", "base_footprint")
position, quaternion = self._tf.lookupTransform(
"odom", "base_footprint", time)
except Exception as ex:
rospy.logwarn(
'Transform lookup failed (\odom -> \base_footprint). '
'Reason: {0}.'.format(ex.message))
return True
current_pose = Pose2D()
current_pose.x, current_pose.y = position[0], position[1]
current_pose.theta = tf.transformations.euler_from_quaternion(
quaternion)[2]
velocity = self._velocity_controller.compute_velocity(current_pose)
if self._velocity_controller.is_target_reached():
rospy.loginfo('The goal was reached')
return False
else:
self._publish_velocity(velocity)
return True
def _set_new_goal(self, new_goal):
"""
Set new target pose as given in the goal message.
Checks if the orientation provided in the target pose is valid.
Publishes the goal pose for the visualization purposes.
Returns true if the goal was accepted.
"""
if not self._is_quaternion_valid(
new_goal.target_pose.pose.orientation):
rospy.logwarn('Aborted. Target pose has invalid quaternion.')
self._move_to_server.set_aborted(
MoveToResult(), 'Aborted. Target pose has invalid quaternion.')
return False
else:
x = new_goal.target_pose.pose.position.x
y = new_goal.target_pose.pose.position.y
yaw = tf.transformations.euler_from_quaternion([
new_goal.target_pose.pose.orientation.x,
new_goal.target_pose.pose.orientation.y,
new_goal.target_pose.pose.orientation.z,
new_goal.target_pose.pose.orientation.w
])[2]
pose = Pose2D(x, y, yaw)
self._velocity_controller.set_target_pose(pose)
self._current_goal_publisher.publish(new_goal.target_pose)
rospy.loginfo('New target pose: {0}'.format(pose))
return True
def _publish_zero_velocity(self):
self._publish_velocity(Velocity())
def _publish_velocity(self, vel):
self._velocity_publisher.publish(vel.get_twist())
def _is_quaternion_valid(self, q):
if any([math.isinf(a) or math.isnan(a) for a in [q.x, q.y, q.z, q.w]]):
rospy.logwarn('Quaternion has NaN\'s or infinities.')
return False
# TODO: check quaternion length and rotation test
return True
pass
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.")
def __init__(self, name):
SimpleActionServer.__init__(self, name, TestRequestAction, self.execute_cb, False)
self.start()
def __init__(self,name):
action_spec=TestAction
SimpleActionServer.__init__(self,name,action_spec,self.goal_callback, False);
self.start()
rospy.loginfo("Creating SimpleActionServer [%s]\n", name);
class ActionServerWrapper():
"""SMACH container wrapper with actionlib ActionServer.
Use this class to associate an action server with a smach
L{StateMachine<smach.state_machine.StateMachine>}. This allows invocation
of the state machine over the actionlib API/protocol.
This class delegates to a provided SMACH container and associates it with an
action server. The user can specify lists of outcomes which correspond to
different action result statuses (SUCCEEDED, ABORTED, PREEMPTED). Once the
delegate state machine leaves one of these outcomes, this wrapper class will
cause the state machine to terminate, and cause the action server to return
a result.
Note that this class does not inherit from L{smach.State<smach.State>} and
can only be used as a top-level container.
"""
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.")
def run_server(self):
"""Run the state machine as an action server.
Note that this method does not block.
"""
# Register action server callbacks
#self._action_server.register_goal_callback(self.goal_cb)
self._action_server.register_preempt_callback(self.preempt_cb)
# Stat server (because we disabled auto-start to register the callbacks)
self._action_server.start()
rospy.loginfo("Started SMACH action server wrapper, adversiting as '%s'" % self._server_name)
### State machine callbacks
def transition_cb(self, userdata, active_states):
"""Transition callback passed to state machine.
This method is called each time the state machine transitions.
"""
rospy.logdebug("Publishing action feedback.")
# Publish action feedback
self.publish_feedback(userdata)
def termination_cb(self, userdata, terminal_states, container_outcome):
"""Termination callback passed to state machine.
This callback receives the final state and state machine outcome as
specified by the state-outcome map given to the delegate container
on construction (see L{ActionServerWrapper.__init__}).
Remember that in this context, the SMACH container is just a single state
object, which has an outcome like any other state; it is this outcome on
which we switch here. This method will determine from the state machine
outcome which result should be returned to the action client for this goal.
"""
rospy.logdebug("Wrapped state machine has terminated with final state: "+str(terminal_states)+" and container outcome: "+str(container_outcome))
def publish_feedback(self, userdata):
"""Publish the feedback message in the userdata db.
Note that this feedback is independent of smach.
"""
if self._feedback_key in userdata:
# This was spewing errors after the fix to ticket #5033 was submitted
# in the case when _feedback_key is not set
# For now, the fix is just checking if it exists, and not publishing otherwise
# The spewage used to not happen because we were looking in self.userdata
# and the constructor of this class sets the feedback key there to an empty struct
# TODO figure out what the hell is going on here.
self._action_server.publish_feedback(userdata[self._feedback_key])
### Action server callbacks
def execute_cb(self, goal):
"""Action server goal callback
This method is called when the action server associated with this state
machine receives a goal. This puts the goal into the userdata,
which is the userdata of the contained state.
"""
# If the state machine is running, we should preempt it before executing it
# it again.
rospy.logdebug("Starting wrapped SMACH container")
# Accept goal
#goal = self._action_server.accept_new_goal()
# Expand the goal into the root userdata for this server
if self._expand_goal_slots:
for slot in goal.__slots__:
self.userdata[slot] = getattr(goal, slot)
# Store the goal in the container local userdate
self.userdata[self._goal_key] = goal
# Store mapped goal slots in local userdata
for from_key,to_key in ((k,self._goal_slots_map[k]) for k in self._goal_slots_map):
self.userdata[to_key] = getattr(goal,from_key)
# Run the state machine (this blocks)
try:
container_outcome = self.wrapped_container.execute(
smach.Remapper(
self.userdata,
self.wrapped_container.get_registered_input_keys(),
self.wrapped_container.get_registered_output_keys(),
{}))
except smach.InvalidUserCodeError as ex:
rospy.logerr("Exception thrown while executing wrapped container.")
self._action_server.set_aborted()
return
except:
rospy.logerr("Exception thrown:while executing wrapped container: " + traceback.format_exc())
self._action_server.set_aborted()
return
# Grab the (potentially) populated result from the userdata
result = self.userdata[self._result_key]
# Store mapped slots in result
for from_key, to_key in ((k,self._result_slots_map[k]) for k in self._result_slots_map):
setattr(result, from_key, self.userdata[to_key])
# If any of the result members have been returned to the parent ud
# scope, overwrite the ones from the full structure
if self._pack_result_slots:
for slot in result.__slots__:
if slot in self.userdata:
setattr(result, slot, self.userdata[slot])
# Set terminal state based on state machine state outcome
if container_outcome in self._succeeded_outcomes:
rospy.loginfo('SUCCEEDED')
self._action_server.set_succeeded(result)
elif container_outcome in self._preempted_outcomes:
rospy.loginfo('PREEMPTED')
self._action_server.set_preempted(result)
else: #if container_outcome in self._aborted_outcomes:
rospy.loginfo('ABORTED')
self._action_server.set_aborted(result)
def preempt_cb(self):
"""Action server preempt callback.
This method is called when the action client preempts an active goal.
In this case, the StateMachine needs to propagate said preemption to
the currently active delegate action (the current state).
"""
rospy.loginfo("Preempt on state machine requested!")
self.wrapped_container.request_preempt()
def __init__(self, name):
SimpleActionServer.__init__(self, name, TestRequestAction, self.execute_cb, False)
self.start()
def __init__(self, name):
action_spec = TestAction
SimpleActionServer.__init__(self, name, action_spec,
self.goal_callback, False)
self.start()
rospy.loginfo("Creating SimpleActionServer [%s]\n", name)
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.')
class ActionServerWrapper():
"""SMACH container wrapper with actionlib ActionServer.
Use this class to associate an action server with a smach
L{StateMachine<smach.state_machine.StateMachine>}. This allows invocation
of the state machine over the actionlib API/protocol.
This class delegates to a provided SMACH container and associates it with an
action server. The user can specify lists of outcomes which correspond to
different action result statuses (SUCCEEDED, ABORTED, PREEMPTED). Once the
delegate state machine leaves one of these outcomes, this wrapper class will
cause the state machine to terminate, and cause the action server to return
a result.
Note that this class does not inherit from L{smach.State<smach.State>} and
can only be used as a top-level container.
"""
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."
)
def run_server(self):
"""Run the state machine as an action server.
Note that this method does not block.
"""
# Register action server callbacks
#self._action_server.register_goal_callback(self.goal_cb)
self._action_server.register_preempt_callback(self.preempt_cb)
# Stat server (because we disabled auto-start to register the callbacks)
self._action_server.start()
rospy.loginfo(
"Started SMACH action server wrapper, adversiting as '%s'" %
self._server_name)
### State machine callbacks
def transition_cb(self, userdata, active_states):
"""Transition callback passed to state machine.
This method is called each time the state machine transitions.
"""
rospy.logdebug("Publishing action feedback.")
# Publish action feedback
self.publish_feedback(userdata)
def termination_cb(self, userdata, terminal_states, container_outcome):
"""Termination callback passed to state machine.
This callback receives the final state and state machine outcome as
specified by the state-outcome map given to the delegate container
on construction (see L{ActionServerWrapper.__init__}).
Remember that in this context, the SMACH container is just a single state
object, which has an outcome like any other state; it is this outcome on
which we switch here. This method will determine from the state machine
outcome which result should be returned to the action client for this goal.
"""
rospy.logdebug(
"Wrapped state machine has terminated with final state: " +
str(terminal_states) + " and container outcome: " +
str(container_outcome))
def publish_feedback(self, userdata):
"""Publish the feedback message in the userdata db.
Note that this feedback is independent of smach.
"""
if self._feedback_key in userdata:
# This was spewing errors after the fix to ticket #5033 was submitted
# in the case when _feedback_key is not set
# For now, the fix is just checking if it exists, and not publishing otherwise
# The spewage used to not happen because we were looking in self.userdata
# and the constructor of this class sets the feedback key there to an empty struct
# TODO figure out what the hell is going on here.
self._action_server.publish_feedback(userdata[self._feedback_key])
### Action server callbacks
def execute_cb(self, goal):
"""Action server goal callback
This method is called when the action server associated with this state
machine receives a goal. This puts the goal into the userdata,
which is the userdata of the contained state.
"""
# If the state machine is running, we should preempt it before executing it
# it again.
rospy.logdebug("Starting wrapped SMACH container")
# Accept goal
#goal = self._action_server.accept_new_goal()
# Expand the goal into the root userdata for this server
if self._expand_goal_slots:
for slot in goal.__slots__:
self.userdata[slot] = getattr(goal, slot)
# Store the goal in the container local userdate
self.userdata[self._goal_key] = goal
# Store mapped goal slots in local userdata
for from_key, to_key in ((k, self._goal_slots_map[k])
for k in self._goal_slots_map):
self.userdata[to_key] = getattr(goal, from_key)
# Run the state machine (this blocks)
try:
container_outcome = self.wrapped_container.execute(
smach.Remapper(
self.userdata,
self.wrapped_container.get_registered_input_keys(),
self.wrapped_container.get_registered_output_keys(), {}))
except smach.InvalidUserCodeError as ex:
rospy.logerr("Exception thrown while executing wrapped container.")
self._action_server.set_aborted()
return
except:
rospy.logerr(
"Exception thrown:while executing wrapped container: " +
traceback.format_exc())
self._action_server.set_aborted()
return
# Grab the (potentially) populated result from the userdata
result = self.userdata[self._result_key]
# Store mapped slots in result
for from_key, to_key in ((k, self._result_slots_map)
for k in self._result_slots_map):
setattr(result, from_key, self.userdata[to_key])
# If any of the result members have been returned to the parent ud
# scope, overwrite the ones from the full structure
if self._pack_result_slots:
for slot in result.__slots__:
if slot in self.userdata:
setattr(result, slot, self.userdata[slot])
# Set terminal state based on state machine state outcome
if container_outcome in self._succeeded_outcomes:
rospy.loginfo('SUCCEEDED')
self._action_server.set_succeeded(result)
elif container_outcome in self._preempted_outcomes:
rospy.loginfo('PREEMPTED')
self._action_server.set_preempted(result)
else: #if container_outcome in self._aborted_outcomes:
rospy.loginfo('ABORTED')
self._action_server.set_aborted(result)
def preempt_cb(self):
"""Action server preempt callback.
This method is called when the action client preempts an active goal.
In this case, the StateMachine needs to propagate said preemption to
the currently active delegate action (the current state).
"""
rospy.loginfo("Preempt on state machine requested!")
self.wrapped_container.request_preempt()
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:
#Create omni controller
"""
========================= 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.
"""
self._velocity_controller = OmniVelocityController(max_linear_velocity,
max_linear_acceleration,
linear_tolerance,
max_angular_velocity,
max_angular_acceleration,
angular_tolerance,
self._controller_frequency)
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.')
class MotionControllerNode:
"""
This is a port of the AMR C++ MotionControllerNode
"""
CONTROLLER_TYPE_DIFF = 'diff'
CONTROLLER_TYPE_OMNI = 'omni'
CONTROLLER_TYPE_UNSPECIFIED = 'unsp'
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:
#Create omni controller
"""
========================= 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.
"""
self._velocity_controller = OmniVelocityController(max_linear_velocity,
max_linear_acceleration,
linear_tolerance,
max_angular_velocity,
max_angular_acceleration,
angular_tolerance,
self._controller_frequency)
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 _move_to_callback(self, move_to_goal):
"""
Triggered with a request to move_to action server
"""
rospy.loginfo('Received [move_to] action command.')
if not self._set_new_goal(move_to_goal):
return
else:
rate = rospy.Rate(self._controller_frequency)
while not rospy.is_shutdown():
if not self._move_to_server.is_active():
# Exit if the goal was aborted
return
#are there any preempted requests?
if self._move_to_server.is_preempt_requested():
rospy.loginfo('found a preempted request, ')
if ( self._move_to_server.is_new_goal_available() and
self._set_new_goal(self._move_to_server.accept_new_goal())
):
# New goal already set
pass
else:
# No new goals, preempt explicitly and exit the callback
self._publish_zero_velocity()
self._move_to_server.set_preempted()
return
if not self._move_towards_goal():
# Finish execution if the goal was reached
self._move_to_server.set_succeeded(MoveToResult(), 'Goal reached.')
self._publish_zero_velocity()
return
rate.sleep()
self._move_to_server.set_aborted(MoveToResult(), 'Aborted. The node has been killed.')
def _simple_goal_callback(self, target_pose):
"""
Wrapper for simple goal action. Forwards as a request to the move_to
action server. Has to be tested!
"""
rospy.loginfo('Received target pose through the "simple goal" topic.'
'Wrapping it in the action message and forwarding to the server.')
rospy.logwarn('Simple goal control is yet to be tested!')
action_goal = MoveToActionGoal()
action_goal.header.stamp = rospy.Time.now()
action_goal.goal.target_pose = target_pose
self._action_goal_publisher.publish(action_goal)
def _obstacles_callback(self, obstacle_msg):
rospy.logwarn('An obstacle was detected. Will stop the robot and cancel the current action.')
if self._move_to_server.is_active():
self._move_to_server.set_aborted(MoveToResult(), 'Obstacle encountered, aborting...')
self._publish_zero_velocity()
def _move_towards_goal(self):
try:
time = self._tf.getLatestCommonTime("odom", "base_footprint")
position, quaternion = self._tf.lookupTransform("odom", "base_footprint", time)
except Exception as ex:
rospy.logwarn('Transform lookup failed (\odom -> \base_footprint). '
'Reason: {0}.'.format(ex.message))
return True
current_pose = Pose2D()
current_pose.x, current_pose.y = position[0], position[1]
current_pose.theta = tf.transformations.euler_from_quaternion(quaternion)[2]
velocity = self._velocity_controller.compute_velocity(current_pose)
if self._velocity_controller.is_target_reached():
rospy.loginfo('The goal was reached')
return False
else:
self._publish_velocity(velocity)
return True
def _set_new_goal(self, new_goal):
"""
Set new target pose as given in the goal message.
Checks if the orientation provided in the target pose is valid.
Publishes the goal pose for the visualization purposes.
Returns true if the goal was accepted.
"""
if not self._is_quaternion_valid(new_goal.target_pose.pose.orientation):
rospy.logwarn('Aborted. Target pose has invalid quaternion.')
self._move_to_server.set_aborted(MoveToResult(),'Aborted. Target pose has invalid quaternion.')
return False
else:
x = new_goal.target_pose.pose.position.x
y = new_goal.target_pose.pose.position.y
yaw = tf.transformations.euler_from_quaternion([new_goal.target_pose.pose.orientation.x,
new_goal.target_pose.pose.orientation.y,
new_goal.target_pose.pose.orientation.z,
new_goal.target_pose.pose.orientation.w])[2]
pose = Pose2D(x, y, yaw)
self._velocity_controller.set_target_pose(pose)
self._current_goal_publisher.publish(new_goal.target_pose)
rospy.loginfo('New target pose: {0}'.format(pose))
return True
def _publish_zero_velocity(self):
self._publish_velocity(Velocity())
def _publish_velocity(self, vel):
self._velocity_publisher.publish(vel.get_twist())
def _is_quaternion_valid(self, q):
if any([math.isinf(a) or math.isnan(a) for a in [q.x, q.y, q.z, q.w]]):
rospy.logwarn('Quaternion has NaN\'s or infinities.')
return False
# TODO: check quaternion length and rotation test
return True
pass
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."
)
