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