[ROS navigation] Base Local Planner local planning - TrajectoryPlanner source code interpretation-2


Record learning and read the understanding of ROS Navigation source code. This paper is the Base Local Planner local planning source code learning record, mainly including text summary, drawing structure diagram description and code annotation. We are still learning. If you have any mistakes, please correct them and make progress together.

The local planner takes the current speed as a reference, generates a reasonable and achievable speed sampling range, and determines the next step speed. So how to filter? It uses the sampling speed to generate the corresponding simulation path. With the help of costmap, it evaluates the path cost from the aspects of obstacles, distance from the target and distance from the global planning path, selects the optimal cost path, publishes its corresponding sampling speed to the robot, and controls its movement. If in the process of circular generation of the forward path, the forward path cannot be obtained due to obstacles in the front, then enter the escape mode, continuously retreat, rotate, leave a distance, and then carry out forward planning, and move forward. When rotating in place, pay attention to vibration control to prevent the robot from rotating back and forth frequently.


[structure diagram]


[related documents]

  • base_local_planner/src/trajectory_planner_ros.cpp
  • base_local_planner/src/trajectory_planner.cpp
  • base_local_planner/src/map_grid.cpp
  • base_local_planner/src/map_cell.cpp
  • base_local_planner/src/costmap_model.cpp

This article records the reading and understanding of the trajectory planner class defined in trajectory_planner.cpp


Code analysis

trajectory_planner.cpp

Catalogues

TrajectoryPlanner::updatePlan | enter the global planning path
TrajectoryPlanner::findBestPath | local planner core function
TrajectoryPlanner::createTrajectories | generate all paths within the given speed range
TrajectoryPlanner::generateTrajectorie | generate a single path and return the cost
TrajectoryPlanner::checkTrajectorie and scoreTrajectorie | generate a single path with the given speed and return the cost

<1> TrajectoryPlanner::updatePlan

After the Movebase calls the global planner to generate the global path, it is passed into the TrajectoryPlanner ROS encapsulation class, and then passed into the real local planner TrajectoryPlanner class through this function, and the final point of the global path is the most targeted point final_goal.

  void TrajectoryPlanner::updatePlan(const vector<geometry_msgs::PoseStamped>& new_plan, bool compute_dists){
    global_plan_.resize(new_plan.size());
    for(unsigned int i = 0; i < new_plan.size(); ++i){
      global_plan_[i] = new_plan[i];
    }

    if( global_plan_.size() > 0 ){
      geometry_msgs::PoseStamped& final_goal_pose = global_plan_[ global_plan_.size() - 1 ];
      final_goal_x_ = final_goal_pose.pose.position.x;
      final_goal_y_ = final_goal_pose.pose.position.y;
      final_goal_position_valid_ = true;
    } else {
      final_goal_position_valid_ = false;
    }

The default value of compute dists is false, which means that the local planner does not recalculate path map and goal map when updating the global plan.

    if (compute_dists) {
      //reset the map for new operations
      path_map_.resetPathDist();
      goal_map_.resetPathDist();

      //make sure that we update our path based on the global plan and compute costs
      path_map_.setTargetCells(costmap_, global_plan_);
      goal_map_.setLocalGoal(costmap_, global_plan_);
      ROS_DEBUG("Path/Goal distance computed");
    }
  }

<2> TrajectoryPlanner::findBestPath

The whole process of local planning is embodied in findBestPath function, which can generate the next possible route within the scope, select the optimal path, and return the speed of the next step corresponding to the path.

First, record the current pose (under global System) and speed, and set the mark bit of the robot's current position to true.

Moreover, the setTargetCells function /setLocalGoal function is updated after the value of path_map_ and goal_map_ are reset. These two maps are specially used "maps" in the local planner, i.e. MapGrid class. Like costmap, they are all organized in cells. Path map records the distance between each cell and the cell on the global planning path, and goal map records the distance between each cell and the target cell. When calculating the cost, these two factors are taken into account to ensure that the results of local planning are not only Fit the overall planning path without deviation from the goal.

The contents of these two functions and the MapGrid class are recorded in the next article.

  Trajectory TrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
      tf::Stamped<tf::Pose>& drive_velocities){

    //Transform the current robot position and direction into a float vector
    Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
    Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));

    //Reset the map, clear all obstacle information and map content
    path_map_.resetPathDist();
    goal_map_.resetPathDist();

    //Get robot footprint with current pose
    std::vector<base_local_planner::Position2DInt> footprint_list =
        footprint_helper_.getFootprintCells(
            pos,
            footprint_spec_,
            costmap_,
            true);

    //Mark all cell s in the initial footprint with "within robot = true" to indicate in the robot footprint
    for (unsigned int i = 0; i < footprint_list.size(); ++i) {
      path_map_(footprint_list[i].x, footprint_list[i].y).within_robot = true;
    }

    //Ensure that the path is updated according to the global plan and the cost is calculated
    //Update which cell s on the map are on the global planning path, and set the target ﹣ dist to 0
    //And calculate the target of all points on the map through their relative positions with other points
    path_map_.setTargetCells(costmap_, global_plan_);
    goal_map_.setLocalGoal(costmap_, global_plan_);
    ROS_DEBUG("Path/Goal distance computed");

Next, call the createTrajectories function, pass in the current pose, speed and acceleration limits, generate the track within the reasonable speed range, and score, the result will return to best.

    //Find the path with the lowest cost, and input the current position, speed and acceleration limits of the robot
    Trajectory best = createTrajectories(pos[0], pos[1], pos[2],
        vel[0], vel[1], vel[2],
        acc_lim_x_, acc_lim_y_, acc_lim_theta_); //Generate many possible tracks, and select the one with the highest score. This is the most critical step.
    ROS_DEBUG("Trajectories created");

    /*
    //If we want to print a ppm file to draw goal dist
    char buf[4096];
    sprintf(buf, "base_local_planner.ppm");
    FILE *fp = fopen(buf, "w");
    if(fp){
      fprintf(fp, "P3\n");
      fprintf(fp, "%d %d\n", map_.size_x_, map_.size_y_);
      fprintf(fp, "255\n");
      for(int j = map_.size_y_ - 1; j >= 0; --j){
        for(unsigned int i = 0; i < map_.size_x_; ++i){
          int g_dist = 255 - int(map_(i, j).goal_dist);
          int p_dist = 255 - int(map_(i, j).path_dist);
          if(g_dist < 0)
            g_dist = 0;
          if(p_dist < 0)
            p_dist = 0;
          fprintf(fp, "%d 0 %d ", g_dist, 0);
        }
        fprintf(fp, "\n");
      }
      fclose(fp);
    }
    */

Judge the returned results. If the cost is negative, it means all paths are unavailable. If the cost is not negative, it means finding an effective path and filling the speed for drive ⅶ velocities before returning.

    if(best.cost_ < 0){
      drive_velocities.setIdentity(); 
    }
    else{
      tf::Vector3 start(best.xv_, best.yv_, 0);
      drive_velocities.setOrigin(start);
      tf::Matrix3x3 matrix;
      matrix.setRotation(tf::createQuaternionFromYaw(best.thetav_));
      drive_velocities.setBasis(matrix);
    }

    return best; //Return to optimal trajectory
  }

<3> TrajectoryPlanner::createTrajectories

First of all, calculate the feasible range of linear speed and angular speed. Here, first, limit the maximum linear speed, that is, ensure that the speed neither exceeds the preset maximum speed limit, nor exceeds the "distance between the starting point and the target straight line / total simulation time".

  Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta,
      double vx, double vy, double vtheta,
      double acc_x, double acc_y, double acc_theta) {
          
    //Declare maximum / small line speed, maximum / small angle speed
    double max_vel_x = max_vel_x_, max_vel_theta;
    double min_vel_x, min_vel_theta;

    //If the final goal is valid (global planning is not empty)
    if( final_goal_position_valid_ ){
      //Calculate the distance between the current location and the target location: final ABCD goal ABCD dist
      double final_goal_dist = hypot( final_goal_x_ - x, final_goal_y_ - y );
      //Maximum speed: at the preset maximum speed and
      max_vel_x = min( max_vel_x, final_goal_dist / sim_time_ );
    }

Continue to calculate the upper and lower limits of linear speed and angular speed. The limit used is the speed range reached by the maximum acceleration and deceleration speed in a period of time from the current speed. Here we make a judgment, that is, whether to use the dwa method:

① Using the dwa method, we use the periodic SIM ﹣ period ﹣ of the forward simulation of the trajectory (a time interval dedicated to the calculation speed of dwa method);

② If dwa method is not used, the whole simulation time is used_

When generating the range, pay attention to using the pre-determined speed and angular speed range parameters for protection.

    if (dwa_) {
      //Using the dwa window method, SIM ﹣ period ﹣ is the time taken by dwa to calculate the maximum and minimum speed
      //The range of velocity and angular velocity is calculated, and the acceleration limit condition is introduced
      max_vel_x = max(min(max_vel_x, vx + acc_x * sim_period_), min_vel_x_);
      min_vel_x = max(min_vel_x_, vx - acc_x * sim_period_);

      max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_period_);
      min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_period_);
    } else {
      //Do not use dwa window method
      //Calculate the range of velocity and angular velocity, and introduce the acceleration limit condition (using SIM? Time ±)
      max_vel_x = max(min(max_vel_x, vx + acc_x * sim_time_), min_vel_x_);
      min_vel_x = max(min_vel_x_, vx - acc_x * sim_time_);

      max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_time_);
      min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_time_);
    }

Next, according to the preset number of samples of linear speed and angular speed, and the range calculated above, the sampling interval is calculated respectively, and the minimum linear speed and angular speed in the range are taken as the initial sampling speed. The case of omnidirectional robot is not considered, i.e. it cannot move in y direction, so the sampling on vy is not considered temporarily.

    double dvx = (max_vel_x - min_vel_x) / (vx_samples_ - 1);//Calculate speed sample interval
    double dvtheta = (max_vel_theta - min_vel_theta) / (vtheta_samples_ - 1);//Calculate the sampling interval of angular velocity

    double vx_samp = min_vel_x;//x-direction speed sampling point
    double vtheta_samp = min_vel_theta;//Sampling point of angular velocity
    double vy_samp = 0.0;
     

In order to iteratively compare the cost of different paths generated by different sampling speeds, it is declared that the cost of best traj and comp traj is - 1

	Trajectory* best_traj = &traj_one;
    best_traj->cost_ = -1.0;//The cost of initializing an optimal path first = - 1

    Trajectory* comp_traj = &traj_two; //If the generated track costs less than the best traj, select it
    comp_traj->cost_ = -1.0;//First, initialize the cost of a current "comparison" path = - 1, and then use the generateTrajectory function to generate the path and store the new cost in it

    Trajectory* swap = NULL; //Pointer for exchange

When the robot is not in the escape state, it starts to traverse all the linear velocity and angular velocity, and calls the generateTrajectory function in the class to generate tracks with them. In the double iteration, the outer traverse linear velocity (positive) and the inner traverse angular velocity. When traversing, when the angle velocity is 0, i.e. straight-line forward, it can avoid jumping over this special case due to the setting of sampling interval.

At the same time, the path with the least cost is obtained by iterative generation and edge comparison, which is stored in the best_traj.

After the execution is completed, the judgment structure of "not in escape state" is also jumped out.

    double impossible_cost = path_map_.obstacleCosts();

    //if we're performing an escape we won't allow moving forward
    if (!escaping_) {
      //[cycle all speeds and angular speeds, scoring]
      //Outer cycle all x speeds
      for(int i = 0; i < vx_samples_; ++i) {
        //Internal ergodic angular velocity of x-speed cycle
        //① Angular velocity = 0
        vtheta_samp = 0;
        //Input the current pose, velocity and acceleration limits, and start the sampling with x-direction velocity, y-direction velocity and angular velocity at the cost of comp_traj
        generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
            acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);//How to implement the generateTrajectory function - focus! ]Parameters: position, sampling starting point, acceleration

        //Compare the score of the generated path and the current optimal path. If the generated path score is smaller, exchange the current path and the optimal path
        //In this case, the cost of the first path generation will be assigned to the best ﹣ traj
        if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
          swap = best_traj;
          best_traj = comp_traj;
          comp_traj = swap;
        }

        //② Angular velocity = lower bound
        vtheta_samp = min_vel_theta;
        //The next iteration cycle generates the path and scoring of all angular velocities
        for(int j = 0; j < vtheta_samples_ - 1; ++j){
          generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
              acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

          //Similarly, if the cost of the new path is less, exchange with the best_traj
          if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
            swap = best_traj;
            best_traj = comp_traj;
            comp_traj = swap;
          }
          //Ergodic angular velocity
          vtheta_samp += dvtheta;
        }
        //Ergodic x-speed
        vx_samp += dvx;
      }

The nonomnidirectional robot will skip this judgment structure without considering the y-direction linear velocity.

      //Only for holonomic robots, the cycle y speed is iterated, and general robots have no y speed
      if (holonomic_robot_) {
        //explore trajectories that move forward but also strafe slightly
        vx_samp = 0.1;
        vy_samp = 0.1;
        vtheta_samp = 0.0;
        generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
            acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

        //if the new trajectory is better... let's take it
        if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
          swap = best_traj;
          best_traj = comp_traj;
          comp_traj = swap;
        }

        vx_samp = 0.1;
        vy_samp = -0.1;
        vtheta_samp = 0.0;
        generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
            acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

        //if the new trajectory is better... let's take it
        if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
          swap = best_traj;
          best_traj = comp_traj;
          comp_traj = swap;
        }
      }
    } // end if not escaping

Next, continue to consider the case of linear velocity = 0 (rotation in place), to consider when this will happen:

  • In trajectoryplanner ROS, if the position has reached the target (within the error range) and the attitude has reached, it will directly send 0 speed; if the attitude has not reached, it will call the deceleration function and the in-situ rotation function, and call the checkTrajectory function to check the validity until it rotates to the target attitude. The checkTrajectory function calls scoreTrajectory and generateTrajectory, and does not call the createTrajectory function. Therefore, the rotation in place near the target will not enter this part of the function at all.

  • In addition, because the path scoring mechanism of the local Planner (later) is: "distance from the target point" and "deviation from the global path", both of which only consider the cell at the end of the path, rather than the comprehensive effect of all cells on the path, the robot moves to a cell, even if there is any barrier free path that can move to the target again, its final score is one It must be higher than the path score of rotating in place.

So, the in-situ rotation here is the in-situ rotation in the process of moving, when the target is not reached, and when the robot encounters obstacles in the process of moving, there is no way to go ahead, so it has to turn in-situ, or even the in-situ rotation can not be satisfied. It needs to back a distance from the escape state, and then turn in situ to adjust the direction, and prepare for the next action. One possibility is that the robot has encountered a sudden obstacle in front of it that is not on the map.

In dealing with this situation, because the limit range of the angular speed when the robot rotates in place is more strict than the limit range of the angular speed when it moves, the chassis cannot deal with the too small in place speed, so we should pay attention to dealing with this limit. Also call the generateTrajectory function to generate the path.

    //Next, the trajectory of the robot rotating in place is generated
    vtheta_samp = min_vel_theta;
    vx_samp = 0.0;
    vy_samp = 0.0;

    double heading_dist = DBL_MAX;
    //Cycle all angular speeds
    for(int i = 0; i < vtheta_samples_; ++i) {
      //Forced minimum rotation speed in place
      double vtheta_samp_limited = vtheta_samp > 0 ? max(vtheta_samp, min_in_place_vel_th_)
        : min(vtheta_samp, -1.0 * min_in_place_vel_th_);
      //Path generating ergodic angular velocity
      generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp_limited,
          acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

In the same way, the best traj is updated with a better path in the iterative process, but when dealing with the in-situ rotation, the cost of the path is always the same because the cell is always in place, so the focus is on the second if, which first obtains the coordinates of the end point of the path, in fact, the x and y coordinates in place, plus the trigonometric function of the attitude after the heading'lookahead'rotation. This coordinate is the same as the original one The coordinate relationship of the earth is as follows:

Then, with the help of the coordinate of the calculation results and the help of the goal map, the result corresponding to the minimum value is taken to update the best traj.

For this screening strategy, my understanding is: this calculated cell is equivalent to the "facing" cell after the robot rotates in place, and the distance between it and the robot is heading ﹐ lookahead ﹐ because the barrier cell value on the goal ﹐ map ﹐ is obstacleCosts (larger than the normal cell), so after iteration, it is inevitable to screen out the situation where the calculation result is an obstacle, that is, after the robot rotates At the same time, the shortest distance between the robot and the target point can be obtained after screening, which ensures that the robot moves forward after rotation and the distance from the target point is shorter.

(the path scoring strategy of the TrajectoryPlanner is a combination of the following three: ① distance from the target point, ② distance from the global planning, ③ obstacles. In the case of the above-mentioned in situ rotation, it may be unexpected obstacles. At this time, ① is meaningless, and this screening strategy can take ② and ③ into account, which is also reasonable.)

      //If the new path costs less
      if(comp_traj->cost_ >= 0
          && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0 || best_traj->yv_ != 0.0)
          && (vtheta_samp > dvtheta || vtheta_samp < -1 * dvtheta)){
        //Get the end of the new path (in place)
        double x_r, y_r, th_r;
        comp_traj->getEndpoint(x_r, y_r, th_r);
        //Coordinate calculation
        x_r += heading_lookahead_ * cos(th_r);
        y_r += heading_lookahead_ * sin(th_r);
        unsigned int cell_x, cell_y;

        //Convert to map coordinate system and judge the distance from the target point
        if (costmap_.worldToMap(x_r, y_r, cell_x, cell_y)) {
          double ahead_gdist = goal_map_(cell_x, cell_y).target_dist;
          //Take the one with the smallest distance and put it into the best_traj
          if (ahead_gdist < heading_dist) {
            if (vtheta_samp < 0 && !stuck_left) {
              swap = best_traj;
              best_traj = comp_traj;
              comp_traj = swap;
              heading_dist = ahead_gdist;
            }
            else if(vtheta_samp > 0 && !stuck_right) {
              swap = best_traj;
              best_traj = comp_traj;
              comp_traj = swap;
              heading_dist = ahead_gdist;
            }
          }
        }
      }
      //Angular velocity traversal
      vtheta_samp += dvtheta;
    }

The following work can be summarized as follows: if the track cost is non negative, i.e. an effective track is found, it will be returned; if no effective track is found, it will enter the escape state, and then it will go back and rotate in place; if no effective path is found for the next step, it will go back and rotate again, and it will not exit the escape state until the backward distance or the angle of rotation reaches a certain standard New planning forward trajectory. In addition, shock suppression.

    //Check whether the score of the optimal trajectory is greater than 0
    if (best_traj->cost_ >= 0) {
      //Restrain the impact of vibration: when the robot moves in a certain direction, it is invalid to mark the opposite direction of the next cycle
      //Until the robot leaves a certain distance from the position marked with vibration and returns to the optimal trajectory.
      
      //If the sampling speed of the track is ≤ 0
      if ( ! (best_traj->xv_ > 0)) {
        //If the angular velocity of the track is less than 0
        if (best_traj->thetav_ < 0) {
          if (rotating_right) {
            stuck_right = true;
          }
          rotating_right = true;
        } else if (best_traj->thetav_ > 0) {
          if (rotating_left){
            stuck_left = true;
          }
          rotating_left = true;
        } else if(best_traj->yv_ > 0) {
          if (strafe_right) {
            stuck_right_strafe = true;
          }
          strafe_right = true;
        } else if(best_traj->yv_ < 0){
          if (strafe_left) {
            stuck_left_strafe = true;
          }
          strafe_left = true;
        }

        //set the position we must move a certain distance away from
        prev_x_ = x;
        prev_y_ = y;
      }

      double dist = hypot(x - prev_x_, y - prev_y_);
      if (dist > oscillation_reset_dist_) {
        rotating_left = false;
        rotating_right = false;
        strafe_left = false;
        strafe_right = false;
        stuck_left = false;
        stuck_right = false;
        stuck_left_strafe = false;
        stuck_right_strafe = false;
      }

      dist = hypot(x - escape_x_, y - escape_y_);
      if(dist > escape_reset_dist_ ||
          fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_){
        escaping_ = false;
      }

      return *best_traj;
    }

Look at the above code. When the track cost is non negative, i.e. the track is effective, enter the following process. The nested if structures are used to record "concussion" and "escape", which do not affect the release of the track. As long as the track is effective, return *best_traj will be executed to return the track.

The first part of these if blocks is to record the vibration: "when the sampling speed v ≤ 0", because the linear speed sampling range is positive, when the non positive sampling speed occurs, it is only possible that the last traj is invalid, the backward command is issued, the escape mode is entered, and then the cycle is renewed, the forward trajectory planning is skipped, and the in-situ rotation mode is entered, resulting in the sampling speed = 0

So it can be said that when rotating in place:

  • If the angular velocity is less than 0, the mark "rotating" right is true
  • If the angular velocity is > 0, the mark "rotating" left is true
  • And record the current rotation position

As like as two peas, we can jump out of the code and skip the part of the omnidirectional robot, and find that the same code has been shown again. Once again, it will cause (but I don't quite understand why this design is what makes the same code appear two times...) :

  • If the angular velocity is less than 0, mark stuck'u right as true
  • If angular velocity > 0, mark stuck'u left as true
  • And record the current rotation position

The above flag bit will not return to false until the robot leaves the recorded position for a certain distance.

Compare with the previous code of rotating in place: it can be found that once the robot rotates at a negative angular speed and stuck'u right is set to true, it can't update the best'traj with the positive angular speed before it leaves the current position for a certain distance, and can only update it with the negative angular speed.

It can be seen that vibration suppression can prevent the robot from turning around in a small range.

            if (vtheta_samp < 0 && !stuck_left) {
             swap = best_traj;
             best_traj = comp_traj;
             comp_traj = swap;
             heading_dist = ahead_gdist;
           }
           else if(vtheta_samp > 0 && !stuck_right) {
             swap = best_traj;
             best_traj = comp_traj;
             comp_traj = swap;
             heading_dist = ahead_gdist;
    //[ignore omni-directional robot, skip this passage]
    if (holonomic_robot_) {
      //if we can't rotate in place or move forward... maybe we can move sideways and rotate
      vtheta_samp = min_vel_theta;
      vx_samp = 0.0;

      //loop through all y velocities
      for(unsigned int i = 0; i < y_vels_.size(); ++i){
        vtheta_samp = 0;
        vy_samp = y_vels_[i];
        //sample completely horizontal trajectories
        generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
            acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

        //if the new trajectory is better... let's take it
        if(comp_traj->cost_ >= 0 && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0)){
          double x_r, y_r, th_r;
          comp_traj->getEndpoint(x_r, y_r, th_r);
          x_r += heading_lookahead_ * cos(th_r);
          y_r += heading_lookahead_ * sin(th_r);
          unsigned int cell_x, cell_y;

          //make sure that we'll be looking at a legal cell
          if(costmap_.worldToMap(x_r, y_r, cell_x, cell_y)) {
            double ahead_gdist = goal_map_(cell_x, cell_y).target_dist;
            if (ahead_gdist < heading_dist) {
              //if we haven't already tried strafing left since we've moved forward
              if (vy_samp > 0 && !stuck_left_strafe) {
                swap = best_traj;
                best_traj = comp_traj;
                comp_traj = swap;
                heading_dist = ahead_gdist;
              }
              //if we haven't already tried rotating right since we've moved forward
              else if(vy_samp < 0 && !stuck_right_strafe) {
                swap = best_traj;
                best_traj = comp_traj;
                comp_traj = swap;
                heading_dist = ahead_gdist;
              }
            }
          }
        }
      }
    }

    if (best_traj->cost_ >= 0) {
      if (!(best_traj->xv_ > 0)) {
        if (best_traj->thetav_ < 0) {
          if (rotating_right){
            stuck_right = true;
          }
          rotating_left = true;
        } else if(best_traj->thetav_ > 0) {
          if(rotating_left){
            stuck_left = true;
          }
          rotating_right = true;
        } else if(best_traj->yv_ > 0) {
          if(strafe_right){
            stuck_right_strafe = true;
          }
          strafe_left = true;
        } else if(best_traj->yv_ < 0) {
          if(strafe_left){
            stuck_left_strafe = true;
          }
          strafe_right = true;
        }

        //set the position we must move a certain distance away from
        prev_x_ = x;
        prev_y_ = y;

      }

      double dist = hypot(x - prev_x_, y - prev_y_);
      if(dist > oscillation_reset_dist_) {
        rotating_left = false;
        rotating_right = false;
        strafe_left = false;
        strafe_right = false;
        stuck_left = false;
        stuck_right = false;
        stuck_left_strafe = false;
        stuck_right_strafe = false;
      }

      dist = hypot(x - escape_x_, y - escape_y_);
      if(dist > escape_reset_dist_ || fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_) {
        escaping_ = false;
      }

      return *best_traj;
    }

The effective part of the trajectory ends. When the cost of the trajectory is negative, it is invalid. Execute the next part, set a negative speed, and generate the trajectory that makes the robot slow backward. Let's also judge the concussion distance here.

    vtheta_samp = 0.0;
    vx_samp = backup_vel_;//reverse speed
    vy_samp = 0.0;
    generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
        acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

    //if the new trajectory is better... let's take it
    /*
       if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
       swap = best_traj;
       best_traj = comp_traj;
       comp_traj = swap;
       }
       */
       
    swap = best_traj;
    best_traj = comp_traj;
    comp_traj = swap;

    double dist = hypot(x - prev_x_, y - prev_y_);
    if (dist > oscillation_reset_dist_) {
      rotating_left = false;
      rotating_right = false;
      strafe_left = false;
      strafe_right = false;
      stuck_left = false;
      stuck_right = false;
      stuck_left_strafe = false;
      stuck_right_strafe = false;
    }

If the end point of the trajectory generated by the backward speed is valid (> 2.0), enter the escape state, cycle backward and rotate, and record the escape position and attitude. Only when the escape position is a certain distance or a certain angle, can the escape state exit, and plan the forward speed again. Just like concussion judgment, escape judgment has been executed many times above, as long as the judgment is executed once before the release of best_traj.

If the end point is invalid, negative cost is returned, and the local planning fails.

    //Escape mode only if there is a valid destination
    if (!escaping_ && best_traj->cost_ > -2.0) {
      escape_x_ = x;
      escape_y_ = y;
      escape_theta_ = theta;
      escaping_ = true;
    }

    dist = hypot(x - escape_x_, y - escape_y_);

    if (dist > escape_reset_dist_ ||
        fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_) {
      escaping_ = false;
    }

If the backward track is in trouble, continue to back up, because a little back up will immediately enter the original rotation mode.

    //if the trajectory failed because the footprint hits something, we're still going to back up
    if(best_traj->cost_ == -1.0)
      best_traj->cost_ = 1.0;

    return *best_traj;

  }

This function has a lot of contents. Draw a picture to summarize the main part:


After entering the escape state, the following (white dot represents the exit condition of detection):


<4> TrajectoryPlanner::generateTrajectorie

This function generates a single path and its cost according to the given velocity and angular velocity sampling.

  void TrajectoryPlanner::generateTrajectory(
      double x, double y, double theta,
      double vx, double vy, double vtheta,
      double vx_samp, double vy_samp, double vtheta_samp,
      double acc_x, double acc_y, double acc_theta,
      double impossible_cost,
      Trajectory& traj) {

    boost::mutex::scoped_lock l(configuration_mutex_); //Local range lock 

    // Record the initial position, velocity and angular velocity
    double x_i = x; 
    double y_i = y;
    double theta_i = theta;
    double vx_i, vy_i, vtheta_i;
    vx_i = vx;
    vy_i = vy;
    vtheta_i = vtheta;

    //Non omnidirectional robot i.e. linear velocity
    double vmag = hypot(vx_samp, vy_samp);

The calculation method of simulation steps and the corresponding time of each step is slightly different.

    int num_steps;
    if(!heading_scoring_) {
      //SIM ﹣ granularity ﹣ distance interval between simulation points
      //Steps = max (velocity mode × total simulation time / distance interval, angular velocity / angular velocity interval), rounded
      num_steps = int(max((vmag * sim_time_) / sim_granularity_, fabs(vtheta_samp) / angular_sim_granularity_) + 0.5);
    } else {
      //Steps = total simulation time / distance interval, rounded
      num_steps = int(sim_time_ / sim_granularity_ + 0.5);
    }

    //At least one step
    if(num_steps == 0) {
      num_steps = 1;
    }

    //Time of each step
    double dt = sim_time_ / num_steps;
    double time = 0.0;

    //Declaration track
    traj.resetPoints();
    traj.xv_ = vx_samp;//Line speed
    traj.yv_ = vy_samp;
    traj.thetav_ = vtheta_samp;//angular velocity
    traj.cost_ = -1.0;//Default cost - 1.0

    //Cost of initializing tracks
    double path_dist = 0.0;//Path distance
    double goal_dist = 0.0;//Target distance
    double occ_cost = 0.0;//Barrier cost
    double heading_diff = 0.0;//Heading angle

Next, the trajectory is generated circularly and the corresponding generation value of the trajectory is calculated. First, the current point is converted from global system to map system. If it cannot be converted to map system, the trajectory generation is directly ended and the cost is - 1.0.

    for(int i = 0; i < num_steps; ++i){
      //Coordinates on the map of the storage point
      unsigned int cell_x, cell_y;

      //Don't want the path to run out of a known map
      //Loop the points on the global path in turn, starting at the current location
      //Convert the current position (x ﹣ I, y ﹣ I) to the map. If it cannot be converted, it means that the path point is not on the map. Set the cost to - 1.0, and return
      if(!costmap_.worldToMap(x_i, y_i, cell_x, cell_y)){
        traj.cost_ = -1.0;
        return;
      }

After conversion to the map, we start to consider the size of the robot, expand the point to the robot's footprints at the point, and call the footprintCost function to obtain the cost of the robot's footprints at the point. If - 1 is returned, it means that the footprints are in trouble, and directly return - 1. This function is declared in the WorldModel class and defined in its derived class CostmapModel. It expands the point where the robot is to the footprint, and returns the cost corresponding to the footprint in combination with the costmap value of the local planner. The specific content will be recorded in the next section.

The maximum cost of the points on the path is recorded in the OCC ﹣ cost, which completes the collision detection.

      //Check the validity of the points on the current path, each path point → robot's footprints, check whether there is an obstacle in the polygon of footprints, and return a negative value if there is an obstacle
      double footprint_cost = footprintCost(x_i, y_i, theta_i);

      //If a negative value is returned, it means that the robot is in trouble in the path. return directly
      if(footprint_cost < 0){
        traj.cost_ = -1.0;
        return;
        //TODO: Really look at getMaxSpeedToStopInTime... dues to discretization errors and high acceleration limits,
        //it can actually cause the robot to hit obstacles. There may be something to be done to fix, but I'll have to
        //come back to it when I have time. Right now, pulling it out as it'll just make the robot a bit more conservative,
        //but safe.
        /*
        double max_vel_x, max_vel_y, max_vel_th;
        //we want to compute the max allowable speeds to be able to stop
        //to be safe... we'll make sure we can stop some time before we actually hit
        getMaxSpeedToStopInTime(time - stop_time_buffer_ - dt, max_vel_x, max_vel_y, max_vel_th);

        //check if we can stop in time
        if(fabs(vx_samp) < max_vel_x && fabs(vy_samp) < max_vel_y && fabs(vtheta_samp) < max_vel_th){
          ROS_ERROR("v: (%.2f, %.2f, %.2f), m: (%.2f, %.2f, %.2f) t:%.2f, st: %.2f, dt: %.2f", vx_samp, vy_samp, vtheta_samp, max_vel_x, max_vel_y, max_vel_th, time, stop_time_buffer_, dt);
          //if we can stop... we'll just break out of the loop here.. no point in checking future points
          break;
        }
        else{
          traj.cost_ = -1.0;
          return;
        }
        */
      }

      //Update OCC cost: Max (max (OCC cost, cost of path footprint, cost of current location)
      //In other words, the maximum obstacle cost of all path points is set as the OCC cost of the path
      occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));

Next, consider the distance from the global path and target point:

  • If simple ﹣ attractor ﹣ is on, a simple tracking strategy is adopted, which only considers the linear distance ^ 2 from the target point
  • If the simple ﹣ attractor ﹣ is not turned on, the distance between the point and the target point and the global planning path is obtained with the help of the goal ﹣ map ﹣ and the path ﹣ map ﹣ if the distance exceeds the range, it means that there is no clear target path, the path is invalid, and the direct return is - 2.0

After iteration, both goal dist and path dist store the corresponding cost of the last point on the path. That is to say, if the path is effective, the distance between the global path and the target point is only related to the end point of the path.

If you turn on heading "scoring", you can score only once when the path is generated from the beginning and reaches a specific time. The specific process of headingDiff function is as follows:
and

  • From the end point (target point) of the global path, the current point and each point on the global path are connected in turn to obtain cost. If cost is positive (barrier free), it is calculated immediately: the difference between the direction of the connection between the current point and the iterative point and the attitude of the current point is returned to the heading_diff and the connection is stopped;
  • If all connections have a negative cost, the maximum value is returned.

The rounds scored for orientation will not update the goal and path dist.

      /*********************Update path and target distance**********************/
      //If you just want to follow blindly, follow the steps below (find the triangle distance from the target). Otherwise, see the back
      if (simple_attractor_) {
        //Target distance
        goal_dist = (x_i - global_plan_[global_plan_.size() -1].pose.position.x) *
          (x_i - global_plan_[global_plan_.size() -1].pose.position.x) +
          (y_i - global_plan_[global_plan_.size() -1].pose.position.y) *
          (y_i - global_plan_[global_plan_.size() -1].pose.position.y);
      } else {
        //Not only blind follow, "update path distance and target distance"
        bool update_path_and_goal_distances = true;
        //If there is a heading scoring, we need to set the heading angle and score the path distance and target distance of a certain point in the path
        //If scoring for orientation
        if (heading_scoring_) { 
          /**********Here we can find out whether this time is the time to mark the heading**********/
          //Heading? Scoring? Timestep
          //In other words, only after passing a certain time on the path can we score the orientation once
          //If the time interval of heading ﹐ scoring ﹐ timestep ﹐ time < heading ﹐ scoring ﹐ timestep +
          if (time >= heading_scoring_timestep_ && time < heading_scoring_timestep_ + dt) {
            //Find the heading angle
            heading_diff = headingDiff(cell_x, cell_y, x_i, y_i, theta_i);
          } else {
            //Do not update path and target distance
            update_path_and_goal_distances = false;
          }
        }

        if (update_path_and_goal_distances) { 
          //Update path distance and target distance (only end cell is considered)
          path_dist = path_map_(cell_x, cell_y).target_dist;
          goal_dist = goal_map_(cell_x, cell_y).target_dist;

          //If the target distance or path distance is greater than or equal to the impossibility_cost (map size), the cost is set to - 2.0
          if(impossible_cost <= goal_dist || impossible_cost <= path_dist){
          //            ROS_DEBUG("No path to goal with goal distance = %f, path_distance = %f and max cost = %f",
          //                goal_dist, path_dist, impossible_cost);
            traj.cost_ = -2.0;
            return;
          }
        }
      }
      /*****************************************************************/

After detection, if the footprints of this point do not encounter obstacles, and the goal ﹣ dist and path ﹣ dist of this point exist, the trajectory is added.

The velocity of the point is calculated by the function. The velocity calculation function makes the current velocity approach the sampling velocity with the acceleration ACC ﹣ UX in dt time, and it will not change after reaching the sampling velocity. Therefore, in fact, each track is a process of moving from the current speed to a stable sampling speed.

Next, calculate the coordinates of the next path point through the track deduction formula, and add several scoring items in proportion to get the cost of the current path, enter the next iteration, fill in the path coordinates repeatedly, and update the path cost.

It should be noted that the calculation of speed here has nothing to do with the speed we released to the robot. The speed here is just to calculate the next point and get the path, while the speed we really released to the robot is the sampling speed. In the real world, the process of robot's current speed – > sampling speed corresponds to the trajectory of this simulation on our map.

      //This point works. Add a trajectory
      traj.addPoint(x_i, y_i, theta_i);

      //Calculation speed, angular speed
      //The function of computeNewVelocity: no matter the speed is greater than or less than VX? Samp, let it approach VX? Samp with the acceleration ACC? X, and then stabilize in VX? Samp
      vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt);
      vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt);
      vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt);

      //Calculate the next position and attitude through the calculated speed
      x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt);
      y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt);
      theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt);

      //Increase time
      time += dt;
    } // end for i < numsteps

    //ROS_INFO("OccCost: %f, vx: %.2f, vy: %.2f, vtheta: %.2f", occ_cost, vx_samp, vy_samp, vtheta_samp);
    //Calculate the cost of the generated path
    double cost = -1.0; 
    if (!heading_scoring_) {
      //Cost = path distance + target distance + obstacle cost (multiplied by respective proportion)
      cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost;
    } else {
      //Cost = path distance + target distance + obstacle cost + heading angle (cost will be increased if there is yaw)
      cost = occdist_scale_ * occ_cost + pdist_scale_ * path_dist + 0.3 * heading_diff + goal_dist * gdist_scale_;
    }
    //Cost of setting current path
    traj.cost_ = cost;
  }

<5> Trajectoryplanner:: checktrajectorie and scoreTrajectorie

The two functions do not work. checkTrajectory calls scoreTrajectory and scoreTrajectory calls generateTrajectory to generate a single path and return the cost. They are paths of corresponding speed generated by the local planner when it is close enough to the target.

  bool TrajectoryPlanner::checkTrajectory(double x, double y, double theta, double vx, double vy,
      double vtheta, double vx_samp, double vy_samp, double vtheta_samp){
    Trajectory t;

    double cost = scoreTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp);

    //if the trajectory is a legal one... the check passes
    if(cost >= 0) {
      return true;
    }
    ROS_WARN("Invalid Trajectory %f, %f, %f, cost: %f", vx_samp, vy_samp, vtheta_samp, cost);

    //otherwise the check fails
    return false;
  }
  double TrajectoryPlanner::scoreTrajectory(double x, double y, double theta, double vx, double vy,
      double vtheta, double vx_samp, double vy_samp, double vtheta_samp) {
    Trajectory t;
    double impossible_cost = path_map_.obstacleCosts();
    generateTrajectory(x, y, theta,
                       vx, vy, vtheta,
                       vx_samp, vy_samp, vtheta_samp,
                       acc_lim_x_, acc_lim_y_, acc_lim_theta_,
                       impossible_cost, t);

    // return the cost.
    return double( t.cost_ );
  }


[Something]

This part of the content often looks new, and each time it looks, it will produce new problems and overturn some of the previous understandings. This time, I have recorded some of my understanding. There must be some inaccuracies and imperfections in the understanding. You are welcome to make corrections and discuss with us.




Neo 2020.3

Published 5 original articles, won praise 0, visited 202
Private letter follow

Tags: angular less

Posted on Fri, 06 Mar 2020 21:25:15 -0800 by Boris Senker