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; } }