def turn_angle_trap_profile(self, goal_angle):
"""Continually sends commands to move a robot straight forward
until the specified distance has been reached, using a trapezoidal
velocity profile.
"""
goal_reached = False
# make sure TF transform exists
if not self.check_frames():
goal_reached = False
self._action_server.set_aborted()
return goal_reached
(start_position, start_rotation, time) = self.get_frame_transform()
# constrain angle for shortest turn if configured
if self.truncate_angle:
goal_angle = self.normalize_angle(goal_angle)
# setup the base command
base_command = Twist()
x_start = start_position.x
y_start = start_position.y
# keep track of the distance traveled
last_angle = start_rotation
turn_angle = 0
self.angular_vel = 0
rate = rospy.Rate(self.update_rate)
period = 1.0 / self.update_rate
# setup the velocity controllers
profile = VelocityProfiler(self.precision_angle, self.max_turn,
self.accel_turn, period)
finished = False
loop_num = 0
# enter the loop to move requested distance
while not finished:
# get the current position
(position, rotation, time) = self.get_frame_transform()
# check for ROS
if rospy.is_shutdown():
rospy.loginfo('%s: ROS Shutdown' % self._action_name)
self._action_server.set_aborted()
goal_reached = False
break
# check for goal canceled
if self._action_server.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._action_server.set_preempted()
goal_reached = False
break
# compute the amount of rotation since the last loop
delta_angle = self.normalize_angle(rotation - last_angle)
turn_angle += delta_angle
last_angle = rotation
angle_left = abs(goal_angle) - abs(turn_angle)
if (goal_angle < 0):
angle_left = -angle_left
# call different functions here
if self.using_distance_feedback:
angle_age = rospy.Time.now() - time
# distance_left is recalculated based on how old the measurement is
measurement_secs = angle_age.to_sec()
(command, finished, goal_reached) = \
profile.next_profile_step_with_feedback(angle_left,
self.angular_vel,
measurement_secs,
loop_num)
else:
(command, finished, goal_reached) = \
profile.next_profile_step_ballistic(angle_left, loop_num)
# rospy.loginfo('goal_angle %f, turn_angle %f, angle left %f' %
# (goal_angle, turn_angle, angle_left))
actual_accel = abs(command - base_command.angular.z) / period
last_vel = self.angular_vel
if (actual_accel > self.accel_turn):
rospy.logwarn('%s: Angular acceleration limit exceeded! Limit %g, Acceleration %g, Velocity %g' %
(self._action_name, self.accel_turn,
actual_accel, last_vel))
base_command.angular.z = command;
self.angular_vel = command;
# publish the base velocities command
self._velocity_command.publish(base_command)
# publish action feedback
self._action_feedback.current_angle = turn_angle
self._action_server.publish_feedback(self._action_feedback)
loop_num += 1
rate.sleep()
# stop the robot when done
base_command = Twist()
self._velocity_command.publish(base_command)
rospy.sleep(self.move_delay)
# get the current position
(position, rotation, time) = self.get_frame_transform()
# compute the amount of rotation since the last loop
delta_angle = self.normalize_angle(rotation - last_angle)
turn_angle += delta_angle
# publish action feedback
self._action_feedback.current_angle = turn_angle
self._action_server.publish_feedback(self._action_feedback)
return goal_reached
def drive_forward_trap_profile(self, goal_distance):
"""Continually sends commands to move a robot straight forward
until the specified distance has been reached, using a trapezoidal
velocity profile.
"""
goal_reached = False
# make sure TF transform exists
if not self.check_frames():
goal_reached = False
self._action_server.set_aborted()
return goal_reached
(start_position, start_rotation, time) = self.get_frame_transform()
# setup the base command
base_command = Twist()
x_start = start_position.x
y_start = start_position.y
# keep track of the distance traveled
distance = 0
self.linear_vel = 0
rate = rospy.Rate(self.update_rate)
period = 1.0 / self.update_rate
# setup the velocity controllers
profile = VelocityProfiler(self.precision_distance,
self.max_speed,
self.accel_speed,
period)
finished = False
loop_num = 0
# enter the loop to move requested distance
while not finished:
# get the current position
(position, rotation, time) = self.get_frame_transform()
# check for ROS
if rospy.is_shutdown():
rospy.loginfo('%s: ROS Shutdown' % self._action_name)
self._action_server.set_aborted()
goal_reached = False
break
# check for goal canceled
if self._action_server.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._action_server.set_preempted()
goal_reached = False
break
# compute the Euclidean distance from the start
distance = math.sqrt(pow((position.x - x_start), 2) +
pow((position.y - y_start), 2))
distance_left = abs(goal_distance) - abs(distance)
if (goal_distance < 0):
distance_left = -distance_left
distance = -distance
# call different functions here
if self.using_distance_feedback:
now = rospy.Time.now()
distance_age = now - time
# print(now)
# print(time)
# distance_left is recalculated based on how old the measurement is
measurement_secs = distance_age.to_sec()
(command, finished, goal_reached) = \
profile.next_profile_step_with_feedback(distance_left,
self.linear_vel,
measurement_secs,
loop_num)
else:
(command, finished, goal_reached) = \
profile.next_profile_step_ballistic(distance_left,
loop_num)
# needed a fudge factor for floating point aliasing, I think 0.01% is close enough
actual_accel = (0.9999 * abs(command - base_command.linear.x))/ period
last_vel = self.linear_vel
base_command.linear.x = command
self.linear_vel = command
if (actual_accel > self.accel_speed):
rospy.logwarn('%s: Linear Acceleration limit exceeded! Limit %g, Acceleration %g, Velocity %g' %
(self._action_name, self.accel_speed,
actual_accel, last_vel))
# publish the base velocities command
self._velocity_command.publish(base_command)
# publish action feedback
self._action_feedback.current_distance = distance
self._action_server.publish_feedback(self._action_feedback)
loop_num += 1
rate.sleep()
# stop the robot when done
base_command = Twist()
self._velocity_command.publish(base_command)
rospy.sleep(self.move_delay)
# get the current position
(position, rotation, time) = self.get_frame_transform()
# compute the Euclidean distance from the start
distance = math.sqrt(pow((position.x - x_start), 2) +
pow((position.y - y_start), 2))
if (goal_distance < 0):
distance = -distance
# publish action feedback
self._action_feedback.current_distance = distance
self._action_server.publish_feedback(self._action_feedback)
return goal_reached