eband_local_planner学习2

在move_base中CONTROLLING中调用了

tc_->computeVelocityCommands(cmd_vel)

bool EBandPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
    {
      // check if plugin initialized
      if(!initialized_)
      {
        ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
        return false;
      }

      // instantiate local variables
      //std::vector<geometry_msgs::PoseStamped> local_plan;
      tf::Stamped<tf::Pose> global_pose;
      geometry_msgs::PoseStamped global_pose_msg;
      std::vector<geometry_msgs::PoseStamped> tmp_plan;

      // get curent robot position
      ROS_DEBUG("Reading current robot Position from costmap and appending it to elastic band.");
      if(!costmap_ros_->getRobotPose(global_pose))
      {
        ROS_WARN("Could not retrieve up to date robot pose from costmap for local planning.");
        return false;
      }

      // convert robot pose to frame in plan and set position in band at which to append
      tf::poseStampedTFToMsg(global_pose, global_pose_msg);
      tmp_plan.assign(1, global_pose_msg);
      eband_local_planner::AddAtPosition add_frames_at = add_front;

      // set it to elastic band and let eband connect it
      if(!eband_->addFrames(tmp_plan, add_frames_at))
      {
        ROS_WARN("Could not connect robot pose to existing elastic band.");
        return false;
      }

      // get additional path-frames which are now in moving window
      ROS_DEBUG("Checking for new path frames in moving window");
      std::vector<int> plan_start_end_counter = plan_start_end_counter_;
      std::vector<geometry_msgs::PoseStamped> append_transformed_plan;
      // transform global plan to the map frame we are working in - careful this also cuts the plan off (reduces it to local window)
      if(!eband_local_planner::transformGlobalPlan(*tf_, global_plan_, *costmap_ros_, costmap_ros_->getGlobalFrameID(), transformed_plan_, plan_start_end_counter))
      {
        // if plan could not be tranformed abort control and local planning
        ROS_WARN("Could not transform the global plan to the frame of the controller");
        return false;
      }

      // also check if there really is a plan
      if(transformed_plan_.empty())
      {
        // if global plan passed in is empty... we won't do anything
        ROS_WARN("Transformed plan is empty. Aborting local planner!");
        return false;
      }

      ROS_DEBUG("Retrieved start-end-counts are: (%d, %d)", plan_start_end_counter.at(0), plan_start_end_counter.at(1));
      ROS_DEBUG("Current start-end-counts are: (%d, %d)", plan_start_end_counter_.at(0), plan_start_end_counter_.at(1));

      // identify new frames - if there are any
      append_transformed_plan.clear();
      // did last transformed plan end futher away from end of complete plan than this transformed plan?
      if(plan_start_end_counter_.at(1) > plan_start_end_counter.at(1)) // counting from the back (as start might be pruned)
      {
        // new frames in moving window
        if(plan_start_end_counter_.at(1) > plan_start_end_counter.at(0)) // counting from the back (as start might be pruned)
        {
          // append everything
          append_transformed_plan = transformed_plan_;
        }
        else
        {
          // append only the new portion of the plan
          int discarded_frames = plan_start_end_counter.at(0) - plan_start_end_counter_.at(1);
          ROS_ASSERT(transformed_plan_.begin() + discarded_frames + 1 >= transformed_plan_.begin());
          ROS_ASSERT(transformed_plan_.begin() + discarded_frames + 1 < transformed_plan_.end());
          append_transformed_plan.assign(transformed_plan_.begin() + discarded_frames + 1, transformed_plan_.end());
        }

        // set it to elastic band and let eband connect it
        ROS_DEBUG("Adding %d new frames to current band", (int) append_transformed_plan.size());
        add_frames_at = add_back;
        if(eband_->addFrames(append_transformed_plan, add_back))
        {
          // appended frames succesfully to global plan - set new start-end counts
          ROS_DEBUG("Sucessfully added frames to band");
          plan_start_end_counter_ = plan_start_end_counter;
        }
        else {
          ROS_WARN("Failed to add frames to existing band");
          return false;
        }
      }
      else
        ROS_DEBUG("Nothing to add");

      // update Elastic Band (react on obstacle from costmap, ...)
      ROS_DEBUG("Calling optimization method for elastic band");
      std::vector<eband_local_planner::Bubble> current_band;
      if(!eband_->optimizeBand())
      {
        ROS_WARN("Optimization failed - Band invalid - No controls availlable");
        // display current band
        if(eband_->getBand(current_band))
          eband_visual_->publishBand("bubbles", current_band);
        return false;
      }

      // get current Elastic Band and
      eband_->getBand(current_band);
      
      // set it to the controller
      if(!eband_trj_ctrl_->setBand(current_band))
      {
        ROS_DEBUG("Failed to to set current band to Trajectory Controller");
        return false;
      }

      // set Odometry to controller
      if(!eband_trj_ctrl_->setOdometry(base_odom_))
      {
        ROS_DEBUG("Failed to to set current odometry to Trajectory Controller");
        return false;
      }

      // get resulting commands from the controller
      geometry_msgs::Twist cmd_twist;
      if(!eband_trj_ctrl_->getTwist(cmd_twist, goal_reached_))
      {
        ROS_DEBUG("Failed to calculate Twist from band in Trajectory Controller");
        return false;
      }


      // set retrieved commands to reference variable
      ROS_DEBUG("Retrieving velocity command: (%f, %f, %f)", cmd_twist.linear.x, cmd_twist.linear.y, cmd_twist.angular.z);
      cmd_vel = cmd_twist;


      // publish plan
      std::vector<geometry_msgs::PoseStamped> refined_plan;
      if(eband_->getPlan(refined_plan))
        // TODO publish local and current gloabl plan
        base_local_planner::publishPlan(refined_plan, g_plan_pub_);
      //base_local_planner::publishPlan(local_plan, l_plan_pub_, 0.0, 0.0, 1.0, 0.0);

      // display current band
      if(eband_->getBand(current_band))
        eband_visual_->publishBand("bubbles", current_band);

      return true;
    }

一.得到机器人的位姿并 加入到 elastic_band_

if(!costmap_ros_->getRobotPose(global_pose))
      {
        ROS_WARN("Could not retrieve up to date robot pose from costmap for local planning.");
        return false;
      }

      // convert robot pose to frame in plan and set position in band at which to append
      tf::poseStampedTFToMsg(global_pose, global_pose_msg);
      tmp_plan.assign(1, global_pose_msg);
      eband_local_planner::AddAtPosition add_frames_at = add_front;

      // set it to elastic band and let eband connect it
      if(!eband_->addFrames(tmp_plan, add_frames_at))    //  执行插入
      {
        ROS_WARN("Could not connect robot pose to existing elastic band.");
        return false;
      }

二.优化 elastic_band_

eband_->optimizeBand();
optimizeBand(elastic_band_);   //实际调用

bool EBandPlanner::optimizeBand(std::vector<Bubble>& band)
  {
    // check if plugin initialized
    if(!initialized_)
    {
      ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
      return false;
    }

    // check whether band and costmap are in the same frame
    if(band.front().center.header.frame_id != costmap_ros_->getGlobalFrameID())
    {
      ROS_ERROR("Elastic Band expects plan for optimization in the %s frame, the plan was sent in the %s frame.",
          costmap_ros_->getGlobalFrameID().c_str(), band.front().center.header.frame_id.c_str());
      return false;
    }

    double distance;
    for(int i = 0; i < ((int) band.size()); i++)
    {
      // update Size of Bubbles in band by calculating Dist to nearest Obstacle [depends kinematic, environment]
      if(!calcObstacleKinematicDistance(band.at(i).center.pose, distance))
      {
        ROS_DEBUG("Optimization (Elastic Band) - Calculation of Distance failed. Frame %d of %d Probably outside map coordinates.",
            i, ((int) band.size()) );
        return false;
      }

      if(distance == 0.0)
      {
        // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid
        ROS_DEBUG("Optimization (Elastic Band) - Calculation of Distance failed. Frame %d of %d in collision. Plan invalid. Trying to refine band.",
            i, ((int) band.size()) );
        // TODO if frame in collision try to repair band instead of aborting everything
        return false;
      }

      band.at(i).expansion = distance;
    }
    ROS_DEBUG("before refineBand.size() %d.", (int) band.size());
    // close gaps and remove redundant bubbles
    if(!refineBand(band))
    {
      ROS_DEBUG("Elastic Band is broken. Could not close gaps in band. Global replanning needed.");
      return false;
    }
    ROS_DEBUG("refineBand.size() %d.", (int) band.size());
    // get a copy of current (valid) band
    std::vector<Bubble> tmp_band = band;

    // now optimize iteratively (for instance by miminizing the energy-function of the full system)
    for(int i = 0; i < num_optim_iterations_; i++)
    {
      ROS_DEBUG("Inside optimization: Cycle no %d", i);

      // calculate forces and apply changes
      if(!modifyBandArtificialForce(tmp_band))
      {
        ROS_DEBUG("Optimization failed while trying to modify Band.");
        // something went wrong -> discard changes and stop process
        return false;
      }

      // check whether band still valid - refine if neccesarry
      if(!refineBand(tmp_band))
      {
        ROS_DEBUG("Optimization failed while trying to refine modified band");
        // modified band is not valid anymore -> discard changes and stop process
        return false;
      }
    }

    // copy changes back to band
    band = tmp_band;
    return true;
  }

1. refineBand(band)
2. modifyBandArtificialForce(tmp_band)

while( (i>0) && (i < ((int) band.size() - 1)) )
{
     calcInternalForces(i, band, band.at(i), internal_forces.at(i));
     calcExternalForces(i, band.at(i), external_forces.at(i));
}

InternalForces + ExternalForces

// sum up external and internal forces over all bubbles
      forces.at(i).wrench.force.x = internal_forces.at(i).wrench.force.x + external_forces.at(i).wrench.force.x;
      forces.at(i).wrench.force.y = internal_forces.at(i).wrench.force.y + external_forces.at(i).wrench.force.y;
      forces.at(i).wrench.force.z = internal_forces.at(i).wrench.force.z + external_forces.at(i).wrench.force.z;

      forces.at(i).wrench.torque.x = internal_forces.at(i).wrench.torque.x + external_forces.at(i).wrench.torque.x;
      forces.at(i).wrench.torque.y = internal_forces.at(i).wrench.torque.y + external_forces.at(i).wrench.torque.y;
      forces.at(i).wrench.torque.z = internal_forces.at(i).wrench.torque.z + external_forces.at(i).wrench.torque.z;

抑制切向力

suppressTangentialForces(i, band, forces.at(i))

applyForces(i, band, forces)

------------------------------------------------------------

calcInternalForces (i, band, band. at (i), internal_forces. at (i))

calcBubbleDistance(curr_bubble.center.pose, band[bubble_num-1].center.pose, distance1)
calcBubbleDistance(curr_bubble.center.pose, band[bubble_num+1].center.pose, distance2)
calcBubbleDifference(curr_bubble.center.pose, band[bubble_num-1].center.pose, difference1)
calcBubbleDifference(curr_bubble.center.pose, band[bubble_num+1].center.pose, difference2)

wrench.force.x = internal_force_gain_*(difference1.linear.x/distance1 + difference2.linear.x/distance2);
wrench.force.y = internal_force_gain_*(difference1.linear.y/distance1 + difference2.linear.y/distance2);
wrench.force.z = internal_force_gain_*(difference1.linear.z/distance1 + difference2.linear.z/distance2);
wrench.torque.x = internal_force_gain_*(difference1.angular.x/distance1 + difference2.angular.x/distance2);
wrench.torque.y = internal_force_gain_*(difference1.angular.y/distance1 + difference2.angular.y/distance2);
wrench.torque.z = internal_force_gain_*(difference1.angular.z/distance1 + difference2.angular.z/distance2);

------------------------------------------------------------

calcExternalForces (i, band. at (i), external_forces. at (i))

// calculate delta-poses (on upper edge of bubble) for x-direction
    edge = curr_bubble.center.pose;
    edge.position.x = edge.position.x + curr_bubble.expansion;
    // get expansion on bubble at this point
    if(!calcObstacleKinematicDistance(edge, distance1))
    {
      ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num);
      // we cannot calculate external forces for this bubble - but still continue for the other bubbles
      return true;
    }
    // calculate delta-poses (on lower edge of bubble) for x-direction
    edge.position.x = edge.position.x - 2.0*curr_bubble.expansion;
    // get expansion on bubble at this point
    if(!calcObstacleKinematicDistance(edge, distance2))
    {
      ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num);
      // we cannot calculate external forces for this bubble - but still continue for the other bubbles
      return true;
    }

    // calculate difference-quotient (approx. of derivative) in x-direction
    if(curr_bubble.expansion <= tiny_bubble_expansion_)
    {
      // avoid division by (almost) zero to avoid numerical problems
      wrench.force.x = -external_force_gain_*(distance2 - distance1)/(2.0*tiny_bubble_expansion_);
      // actually we should never end up here - band should have been considered as broken
      ROS_DEBUG("Calculating external forces on broken band. Bubble should have been removed. Local Planner probably ill configured");
    }
    else
      wrench.force.x = -external_force_gain_*(distance2 - distance1)/(2.0*curr_bubble.expansion);
    // TODO above equations skip term to make forces continuous at end of influence region - test to add corresponding term


    // calculate delta-poses (on upper edge of bubble) for y-direction
    edge = curr_bubble.center.pose;
    edge.position.y = edge.position.y + curr_bubble.expansion;
    // get expansion on bubble at this point
    if(!calcObstacleKinematicDistance(edge, distance1))
    {
      ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num);
      // we cannot calculate external forces for this bubble - but still continue for the other bubbles
      return true;
    }
    // calculate delta-poses (on lower edge of bubble) for x-direction
    edge.position.y = edge.position.y - 2.0*curr_bubble.expansion;
    // get expansion on bubble at this point
    if(!calcObstacleKinematicDistance(edge, distance2))
    {
      ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num);
      // we cannot calculate external forces for this bubble - but still continue for the other bubbles
      return true;
    }

    // calculate difference-quotient (approx. of derivative) in x-direction
    if(curr_bubble.expansion <= tiny_bubble_expansion_)
    {
      // avoid division by (almost) zero to avoid numerical problems
      wrench.force.y = -external_force_gain_*(distance2 - distance1)/(2.0*tiny_bubble_expansion_);
      // actually we should never end up here - band should have been considered as broken
      ROS_DEBUG("Calculating external forces on broken band. Bubble should have been removed. Local Planner probably ill configured");
    }
    else
      wrench.force.y = -external_force_gain_*(distance2 - distance1)/(2.0*curr_bubble.expansion);
    // TODO above equations skip term to make forces continuous at end of influence region - test to add corresponsing term


    // no force in z-direction
    wrench.force.z = 0.0;


    // no torque around x and y axis
    wrench.torque.x = 0.0;
    wrench.torque.y = 0.0;


    // calculate delta-poses (on upper edge of bubble) for x-direction
    PoseToPose2D(curr_bubble.center.pose, edge_pose2D);
    edge_pose2D.theta = edge_pose2D.theta + (curr_bubble.expansion/getCircumscribedRadius(*costmap_ros_));
    edge_pose2D.theta = angles::normalize_angle(edge_pose2D.theta);
    PoseToPose2D(edge, edge_pose2D);
    // get expansion on bubble at this point
    if(!calcObstacleKinematicDistance(edge, distance1))
    {
      ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num);
      // we cannot calculate external forces for this bubble - but still continue for the other bubbles
      return true;
    }
    // calculate delta-poses (on lower edge of bubble) for x-direction
    edge_pose2D.theta = edge_pose2D.theta - 2.0*(curr_bubble.expansion/getCircumscribedRadius(*costmap_ros_));
    edge_pose2D.theta = angles::normalize_angle(edge_pose2D.theta);
    PoseToPose2D(edge, edge_pose2D);
    // get expansion on bubble at this point
    if(!calcObstacleKinematicDistance(edge, distance2))
    {
      ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num);
      // we cannot calculate external forces for this bubble - but still continue for the other bubbles
      return true;
    }

    // calculate difference-quotient (approx. of derivative) in x-direction
    if(curr_bubble.expansion <= tiny_bubble_expansion_)
    {
      // avoid division by (almost) zero to avoid numerical problems
      wrench.torque.z = -external_force_gain_*(distance2 - distance1)/(2.0*tiny_bubble_expansion_);
      // actually we should never end up here - band should have been considered as broken
      ROS_DEBUG("Calculating external forces on broken band. Bubble should have been removed. Local Planner probably ill configured");
    }
    else
      wrench.torque.z = -external_force_gain_*(distance2 - distance1)/(2.0*curr_bubble.expansion);
    // TODO above equations skip term to make forces continuous at end of influence region - test to add corresponsing term

------------------------------------------------------------

抑制切向力

suppressTangentialForces(i, band, forces.at(i))

// get pose-difference from following to preceding bubble -> "direction of the band in this bubble"
    if(!calcBubbleDifference(band[bubble_num+1].center.pose, band[bubble_num-1].center.pose, difference))
      return false;

    // "project wrench" in middle bubble onto connecting vector
    // scalar wrench*difference
    scalar_fd = forces.wrench.force.x*difference.linear.x + forces.wrench.force.y*difference.linear.y +
      forces.wrench.force.z*difference.linear.z + forces.wrench.torque.x*difference.angular.x +
      forces.wrench.torque.y*difference.angular.y + forces.wrench.torque.z*difference.angular.z;

    // abs of difference-vector: scalar difference*difference
    scalar_dd = difference.linear.x*difference.linear.x + difference.linear.y*difference.linear.y + difference.linear.z*difference.linear.z +
      difference.angular.x*difference.angular.x + difference.angular.y*difference.angular.y + difference.angular.z*difference.angular.z;

    // avoid division by (almost) zero -> check if bubbles have (almost) same center-pose
    if(scalar_dd <= tiny_bubble_distance_)
    {
      // there are redundant bubbles, this should normally not hapen -> probably error in band refinement
      ROS_DEBUG("Calculating tangential forces for redundant bubbles. Bubble should have been removed. Local Planner probably ill configured");
    }

    // calculate orthogonal components
    forces.wrench.force.x = forces.wrench.force.x - scalar_fd/scalar_dd * difference.linear.x;
    forces.wrench.force.y = forces.wrench.force.y - scalar_fd/scalar_dd * difference.linear.y;
    forces.wrench.force.z = forces.wrench.force.z - scalar_fd/scalar_dd * difference.linear.z;
    forces.wrench.torque.x = forces.wrench.torque.x - scalar_fd/scalar_dd * difference.angular.x;
    forces.wrench.torque.y = forces.wrench.torque.y - scalar_fd/scalar_dd * difference.angular.y;
    forces.wrench.torque.z = forces.wrench.torque.z - scalar_fd/scalar_dd * difference.angular.z;

------------------------------------------------------------

applyForces(i, band, forces)

// move bubble
    bubble_pose = band.at(bubble_num).center.pose;
    PoseToPose2D(bubble_pose, bubble_pose2D);

    // move according to bubble_new = bubble_old + alpha*force -> we choose alpha to be the current expansion of the modified bubble
    bubble_jump.linear.x = band.at(bubble_num).expansion*forces.at(bubble_num).wrench.force.x;
    bubble_jump.linear.y = band.at(bubble_num).expansion*forces.at(bubble_num).wrench.force.y;
    bubble_jump.linear.z = 0.0;
    bubble_jump.angular.x = 0.0;
    bubble_jump.angular.y = 0.0;
    bubble_jump.angular.z = band.at(bubble_num).expansion/getCircumscribedRadius(*costmap_ros_) * forces.at(bubble_num).wrench.torque.z;
    bubble_jump.angular.z = angles::normalize_angle(bubble_jump.angular.z);

    // apply changes to calc tmp bubble position
    new_bubble_pose2D.x = bubble_pose2D.x + bubble_jump.linear.x;
    new_bubble_pose2D.y = bubble_pose2D.y + bubble_jump.linear.y;
    new_bubble_pose2D.theta = bubble_pose2D.theta + bubble_jump.angular.z;
    new_bubble_pose2D.theta = angles::normalize_angle(new_bubble_pose2D.theta);

    // apply changes to local copy
    Pose2DToPose(new_bubble_pose, new_bubble_pose2D);
    new_bubble.center.pose = new_bubble_pose;