Autoware小问题之三——open planner不能多点连续导航

#ps：许久未更。autoware ROS1教程这些这一两年逐渐多了许多，大多都千篇一律（或许核心内容各自都不公布），虽然手上有一份自己写的教程手册，大家在网上搜搜找找基本上都能找到一点，就不用公布了（60页的内容，主要是懒的公布出来）

正文

回到正文，原始autoware.ai1.13代码中open planner导航方式可能或许应该是不支持连续导航的，在导航到第一个目标点后，需要重新运行全局路径导航节点，局部导航节点以及pure persuit节点才能导航到下一个点。需要我们修改部分代码才能不用重新运行节点直接导航到下一个点。

修改代码

1   op_global_planner_core.cpp的MainLoop()

void GlobalPlanner::MainLoop()
{

        if(m_GoalsPos.size() > 0)
        {
            if(m_GeneratedTotalPaths.size() > 0 && m_GeneratedTotalPaths.at(0).size() > 3)
            {
                //修改这个if条件
                // if(m_params.bEnableReplanning)
               //改成下面的if
                if(m_params.bEnableReplanning && m_iCurrentGoalIndex<m_GoalsPos.size()-1)
                {
                    PlannerHNS::RelativeInfo info;
                    bool ret = PlannerHNS::PlanningHelpers::GetRelativeInfoRange(m_GeneratedTotalPaths, m_CurrentPose, 0.75, info);
                    if(ret == true && info.iGlobalPath >= 0 &&  info.iGlobalPath < m_GeneratedTotalPaths.size() && info.iFront > 0 && info.iFront < m_GeneratedTotalPaths.at(info.iGlobalPath).size())
                    {
                        double remaining_distance =    m_GeneratedTotalPaths.at(info.iGlobalPath).at(m_GeneratedTotalPaths.at(info.iGlobalPath).size()-1).cost - (m_GeneratedTotalPaths.at(info.iGlobalPath).at(info.iFront).cost + info.to_front_distance);
                        if(remaining_distance <= REPLANNING_DISTANCE)
                        {
                            bMakeNewPlan = true;
                            if(m_GoalsPos.size() > 0)
                                m_iCurrentGoalIndex = (m_iCurrentGoalIndex + 1) % m_GoalsPos.size();
                            std::cout << "Current Goal Index = " << m_iCurrentGoalIndex << std::endl << std::endl;
                        }
                    }
                }
            }

}

2   BehaviorStateMachine.cpp的MissionAccomplishedStateII::GetNextState()函数

BehaviorStateMachine* MissionAccomplishedStateII::GetNextState()
{
    // -------------原有代码--------------------
    // return FindBehaviorState(this->m_Behavior);


    //需要改成以下的
	PreCalculatedConditions* pCParams = GetCalcParams();

    if(pCParams->currentGoalID == -1)
        return FindBehaviorState(this->m_Behavior);

    else
    {
        pCParams->prevGoalID = pCParams->currentGoalID;
        return FindBehaviorState(FORWARD_STATE);
    }
}

3. pure_pursuit模块在final_waypoints为空时会出异常

先在pure_pursuit_core.h中添加  ros::Subscriber sub5_;,并增加回调函数声明

private:
  // handle
  ros::NodeHandle nh_;
  ros::NodeHandle private_nh_;

  std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;

  // class
  PurePursuit pp_;

  // publisher
  ros::Publisher pub1_, pub2_,
    pub11_, pub12_, pub13_, pub14_, pub15_, pub16_, pub17_, pub18_;

  // subscriber
  ros::Subscriber sub1_, sub2_, sub3_, sub4_;

//add下面句话
  ros::Subscriber sub5_;

..........



  // callbacks
  void callbackFromConfig(
    const autoware_config_msgs::ConfigWaypointFollowerConstPtr& config);
  void callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg);
  void callbackFromCurrentVelocity(
    const geometry_msgs::TwistStampedConstPtr& msg);
  void callbackFromWayPoints(const autoware_msgs::LaneConstPtr& msg_input);

//add下面函数声明
  void loopWaypointsCallback(const autoware_msgs::LaneConstPtr &msg);

pure_pursuit_core.cpp添加sub5_的定义

void PurePursuitNode::initForROS()
{

.......

  // setup subscriber
  sub1_ = nh_.subscribe("final_waypoints", 10, &PurePursuitNode::callbackFromWayPoints, this);
  sub2_ = nh_.subscribe("current_pose", 10, &PurePursuitNode::callbackFromCurrentPose, this);
  sub3_ = nh_.subscribe("config/waypoint_follower", 10, &PurePursuitNode::callbackFromConfig, this);
  sub4_ = nh_.subscribe("current_velocity", 10, &PurePursuitNode::callbackFromCurrentVelocity, this);
  sub5_ = nh_.subscribe("loop_waypoints", 10, &PurePursuitNode::loopWaypointsCallback, this); //add

......

}

pure_pursuit_core.cpp 中添加sub5_的回调函数

static autoware_msgs::Lane loopWaypoints;
void PurePursuitNode::loopWaypointsCallback(const autoware_msgs::LaneConstPtr &msg)
{
  loopWaypoints = *msg;
}

pure_pursuit_core.cpp 中修改callbackFromWayPoints函数，改成以下样子

void PurePursuitNode::callbackFromWayPoints(
  const autoware_msgs::LaneConstPtr& msg)
{
  autoware_msgs::Lane::Ptr msg_output(new autoware_msgs::Lane);
  *msg_output = *msg;
  if (!loopWaypoints.waypoints.empty())
  {
    msg_output->waypoints.insert(msg_output->waypoints.end(), loopWaypoints.waypoints.begin(), loopWaypoints.waypoints.end());
    loopWaypoints.waypoints.clear();
  }
  if (msg_output->waypoints.empty())    return;  // 如果waypoints为空，pp_会出异常      

  if (!msg_output->waypoints.empty())
    command_linear_velocity_ = msg_output->waypoints.at(0).twist.twist.linear.x;   // 从航点中得到速度
  else
    command_linear_velocity_ = 0;

  pp_.setCurrentWaypoints(msg_output->waypoints);
  is_waypoint_set_ = true;
}

最后需要编译一下

AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release