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