inline void WaypointFollower::turnVelocityInGoal(VELOCITY_COMMAND & cmd) {
double final_turning_angle = 0.0;
final_turning_angle = NORMALIZE(this->final_gaz - this->az);
ROS_DEBUG_NAMED("velo","Normalize(%f - %f) = %f", this->final_gaz, this->az, final_turning_angle);
ROS_DEBUG_NAMED("velo","Reaching final angle from %f to %f = %f", this->az, this->final_gaz, final_turning_angle);
cmd.vel.px = 0;
if (fabs(final_turning_angle) < this->motionParams.az_success_distance) {
cmd.vel.pa = 0;
ROS_INFO("Goal reached at angle %f < %f", fabs(final_turning_angle) , this->motionParams.az_success_distance);
this->goal_ready = false;
this->robot_movement_allowed = false; // wait for new goal
} else {
cmd.vel.pa = ABSMAX( ( final_turning_angle * this->motionParams.pid_rot_kp ), this->motionParams.max_rot_vel );
}
}
void NumPrintArrAbsMaxColor(char *name, real *arr, int l) {
int i = 0;
char name10[11] = {0};
for (i = 0; i < 10; i++) {
if (i < strlen(name))
name10[i] = name[i];
else
name10[i] = ' ';
}
printf("%10s: ", name10);
real x = ABSMAX(arr, l);
for (i = 0; i < l; i++) {
if (ABS(arr[i]) == x)
printfc('r', 'k', "%12.6g ", arr[i]);
else if (-NUM_EPS <= arr[i] && arr[i] <= NUM_EPS)
printf("%12.6g ", 0.0);
else
printf("%12.6g ", arr[i]);
}
printf("\n");
fflush(stdout);
}
/**
* Calculates forward and turning velocities
*/
inline void WaypointFollower::updateVelocityBetweenWaypoints( VELOCITY_COMMAND & cmd, double current_distance_to_goal)
{
double turning_angle = 0.0;
double local_trans_angle_range = 0.0, angle_towards_goal_absolute = 0.0;
double start_driving_forward_turning_angle = 0.0, start_driving_backward_turning_angle = 0.0;
// absolute angle the robot has to take to reach the goal if he moves forward
angle_towards_goal_absolute = atan2(this->gy - this->y, this->gx - this->x);
ROS_DEBUG_NAMED("velo", "Localplanner (%f, %f, %f) -> (%f, %f, %f) ", this->x, this->y, this->az, this->gx, this->gy, angle_towards_goal_absolute );
// angular difference between current robot angle and angle_towards_goal_absolute if robot drives
// forward - turning_angle_forward must be within [-PI, .. , PI]
start_driving_forward_turning_angle = NORMALIZE(angle_towards_goal_absolute - this->az);
// angular difference between current robot angle and angle_towards_goal_absolute if robot drives
// backward - a clockwise forward-angle results in a counterclockwise backward-angle and vice versa
start_driving_backward_turning_angle = NORMALIZE(start_driving_forward_turning_angle + M_PI);
bool moved_away_from_target = false;
// check if we are moving away from the target
if (current_distance_to_goal > (this->last_known_distance_to_goal + this->motionParams.success_distance)) {
moved_away_from_target = true; // if we moved beyond the goal already stop driving
}
// if we got a new waypoint, compute the new driving direction (forward or backward) to reach it
if (!this->direction_already_computed_p || moved_away_from_target) {
updateDrivingDirection(angle_towards_goal_absolute, current_distance_to_goal, start_driving_forward_turning_angle);
this->direction_already_computed_p = true;
} // endif ! direction_already_computed
// set turning_angle according to movement direction
turning_angle = ( this->driving_direction == DRIVING_DIRECTION_FORWARD
? start_driving_forward_turning_angle
: start_driving_backward_turning_angle );
// whether the robot will have to change directions in the next WP
XYTH_COORD over_next_wp = this->waypoint_queue.front();
double angle_towards_over_next_wp = fabs(atan2(over_next_wp.y - this->y, over_next_wp.x - this->x));
bool pathchanges = NORMALIZE(angle_towards_over_next_wp - this->az) > 0.7;
// local_trans_angle_range is the angle outside which the robot should just turn and not yet move forward
// allow moving forward within a range bounded by 0.1 and maximally 0.75 (45 degrees) in general,
// or max according to params and distance to next goal (less tolerant near goal)
if (this->waypoint_queue.empty()) {
local_trans_angle_range = 0.2;
} else {
// value between 0.3 and 1.2
local_trans_angle_range = MINMAX(this->motionParams.trans_angle_range * fmin(current_distance_to_goal * 3, 1.0), 0.4, 1.2);
}
if (fabs(turning_angle) > local_trans_angle_range )
{ // rotation without driving, because turning_angle is large in comparison to current_distance to goal, or we are at goal
cmd.vel.px = 0.00;
// set rotation speed to a value that is proportional to turning_angle, but below a certain cut-off value (max_rot_vel)
cmd.vel.pa = ABSMAX( ( turning_angle * this->motionParams.pid_rot_kp), this->motionParams.max_rot_vel );
ROS_DEBUG_NAMED("velo", "Turning because angle %f > %f : %f", fabs(turning_angle), local_trans_angle_range, cmd.vel.pa);
} else { // rotating and driving simultaneously, because turning_angle is not large in comparison to current_distance to goal
// set translation speed proportional to current_distance to the goal
// TODO (maybe): if goal angle is similar to current angle (no hard edge ahead) and next waypoint is not the last, then don't break down as much.
//ROS_DEBUG_NAMED("velo", "max-velo : %f", this->motionParams.reduced_trans_vel / 10);
if (humanOnPath) {
cmd.vel.px = 0;
} else if (this->waypoint_queue.empty()) {
// last waypoint, slow down. px is in dm/s, not m/s!!
cmd.vel.px = this->driving_direction *
fmin(this->motionParams.reduced_trans_vel / 10 /*dm/s*/, ( current_distance_to_goal * 3 * this->motionParams.pid_trans_kp ));
// } else if (this->waypoint_queue.size() < 3) {
// // last few waypoints, slow down. px is in dm/s, not m/s!!
// cmd.vel.px = this->driving_direction *
// fmin(this->motionParams.reduced_trans_vel / 10 /*dm/s*/, ( current_distance_to_goal * 4 * this->motionParams.pid_trans_kp ));
// } else if (pathchanges) {
// cmd.vel.px = this->driving_direction *
// fmin(this->motionParams.reduced_trans_vel / 10 /*dm/s*/, ( fmax(1, (this->waypoint_queue.size() * 0.1)) * this->motionParams.pid_trans_kp ));
} else { // more speed in middle waypoints
cmd.vel.px = this->driving_direction *
fmin(this->motionParams.reduced_trans_vel / 10 /*dm/s*/, ( fmax(1, (this->waypoint_queue.size() * 0.2)) * this->motionParams.pid_trans_kp ));
}
// set rotation speed to a value that is proportional, but below a certain cut-off value (max_rot_vel)
cmd.vel.pa = ABSMAX( ( turning_angle * this->motionParams.pid_rot_kp ), this->motionParams.wp_max_rot_vel );
}
ROS_DEBUG_NAMED("velo", "Command %f, %f ", cmd.vel.px, cmd.vel.pa);
} // end updateVelocityBetweenWaypoints
/**************************************************************************
* returns a velocity update command to reach the goal position
* iterates over waypoints
* ************************************************************************** */
void WaypointFollower::updateVelocities(VELOCITY_COMMAND &cmd)
{
// set velocity mode
cmd.state = 1;
cmd.vel.px = 0;
cmd.vel.py = 0;
cmd.vel.pa = 0;
if (this->goal_ready == false ||
this->position_initialized == false ||
this->motionParamsSet == false) {
return;
}
if (this->robot_movement_allowed == false) {
ROS_ERROR("Robot blocked %d, %d, %d.", this->goal_ready, this->position_initialized, this->robot_movement_allowed);
return;
}
timeval timeVal;
if (gettimeofday(&timeVal, NULL) != 0) {
ROS_ERROR("Fatal error when reading System time ");
return;
}
// penalty for robot-robot when waiting for human-robot
if (waitUntil > 0) { // check whether robot shall wait a little longer
if (timeVal.tv_sec >= waitUntil) {
waitUntil = -1; // wait time has passed
} else {
ROS_DEBUG("Waiting %ld more seconds...", waitUntil - timeVal.tv_sec);
return;
}
}
// TODO: if we think we stand still and position has not changed (by pushing), don't recalculate
double current_distance_to_goal = DISTANCE2D(this->x,this->y,this->gx, this->gy);
ROS_DEBUG_NAMED("velo", "Distance to goal: %f, waypoints: %zu", current_distance_to_goal, this->waypoint_queue.size());
if (this->humanOnPath == true) { // stop if human is in the way, but allow rotating on spot
double turning_angle = NORMALIZE(this->gaz - this->az);
ROS_DEBUG_NAMED("velo", "Turning angle: %f", turning_angle);
pathBlockedTimestamp = timeVal.tv_sec;
if (fabs(turning_angle) < this->motionParams.az_success_distance * 2) {
cmd.vel.pa = 0;
} else {
cmd.vel.pa = ABSMAX( ( turning_angle * this->motionParams.pid_rot_kp / 3 ), this->motionParams.max_rot_vel );
}
return;
} else {
pathBlockedTimestamp = -1;
}
if (this->direction_already_computed_p == false) {
// required to detect from call to call when we move accross a waypoint meanwhile
this->last_known_distance_to_goal = current_distance_to_goal;
}
if (this->waypoint_queue.size() > 2) {
// if turning made us leave goal point very far, return to goal
turning_in_goal = false;
}
/* either we are at the end (within single success distance)
* or we are on a waypoint (within 3x success distance)
* or we need to continue and correct PID velocities
*/
if (turning_in_goal == true || (this->waypoint_queue.empty() && (current_distance_to_goal < this->motionParams.success_distance))) {
ROS_DEBUG_NAMED("velo", "Success: (%f, %f) in delta %f of (%f, %f)", this->gx, this->gy, current_distance_to_goal, this->x, this->y);
turning_in_goal = true;
// just turn from now on, even if it makes us go away from goal a bit
turnVelocityInGoal(cmd);
} else {
// either more waypoints are left or we are not quite in final location
if ( ! this->waypoint_queue.empty()) {
int skip = 0;
if (current_distance_to_goal < this->motionParams.wp_success_distance) {
skip = 1;
}
// prune points from the beginning
skip += this->prunePlan();
this->updateGoalForNextWaypoint(skip);
current_distance_to_goal = DISTANCE2D(this->x, this->y, this->gx, this->gy);
this->last_known_distance_to_goal = current_distance_to_goal;
}
// for direction calculations
updateVelocityBetweenWaypoints(cmd, current_distance_to_goal);
this->last_known_distance_to_goal = current_distance_to_goal;
}
}
