car_controller.cpp代码解释

 这里是未到达导航点的情况，如果未到达会反复运行这里的代码

  if (!goal_reached)
  {
    for (int i = 0; i < map_path.poses.size(); i++)
    {
      geometry_msgs::PoseStamped map_path_pose = map_path.poses[i];
      geometry_msgs::PoseStamped odom_path_pose;
      // geometry_msgs::PoseStamped odom_path_pose = map_path.poses[i];
      try
      {
        tf_listener.transformPose("odom", ros::Time(0), map_path_pose, "map", odom_path_pose);
        geometry_msgs::Point odom_path_wayPt = odom_path_pose.pose.position;
        bool _isForwardWayPt = isForwardWayPt(odom_path_wayPt, carPose);

        if (_isForwardWayPt && start_flag == false)
        {
          start_flag = true;
        }
        if (start_flag == false)
        {
          continue;
        }

        if (_isForwardWayPt || 1)
        {
          bool _isWayPtAwayFromLfwDist = isWayPtAwayFromLfwDist(odom_path_wayPt, carPose_pos);
          if (_isWayPtAwayFromLfwDist)
          {
            forwardPt = odom_path_wayPt;
            foundForwardPt = true;
            break;
          }
        }

        if (car2goal_dist < Lfw)
        {
          forwardPt = odom_goal_pos;
          foundForwardPt = true;
        }
      }
      catch (tf::TransformException &ex)
      {
        ROS_ERROR("%s", ex.what());
        ros::Duration(1.0).sleep();
      }
    }
  }
  else if (goal_reached)
  {
    forwardPt = odom_goal_pos;
    foundForwardPt = false;
    // ROS_INFO("goal REACHED!");
  }

如果 未达到目标：  
    遍历 地图路径 中的所有姿势：  
        获取 当前地图路径姿势  
 
        尝试：  
            将 当前地图路径姿势 从 'map' 坐标系 转换到 'odom' 坐标系，结果存储在 里程计路径姿势  
            获取 转换后的里程计路径点  
            判断 该点是否在前向方向上（相对于车辆当前位置）：  
                如果是，并且 起始标志 为假：  
                    设置 起始标志 为真  
                如果 起始标志 为假：  
                    跳过当前循环迭代  
                如果 该点在前向方向上（或总是为真，因为这里有个硬编码的1）：  
                    判断 该点是否远离左侧前方距离（基于车辆位置和点位置）：  
                        如果是：  
                            将该点作为 前向点  
                            设置 找到前向点 标志 为真  
                            跳出循环  
            判断 车辆到目标的距离 是否小于 左侧前方距离：  
                如果是：  
                    将 目标位置 作为 前向点  
                    设置 找到前向点 标志 为真  
        捕获 坐标转换异常：  
            打印 错误信息  
            等待 1 秒  
否则 如果 已达到目标：  
    将 目标位置 作为 前向点  
    设置 找到前向点 标志 为假  
    // （可选）打印 "目标已到达！"