Auto Ware 代码解析系列-astar_planner节点

节点的主函数代码如下：

int main(int argc, char **argv)
{
  ros::init(argc, argv, "velocity_set");

  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");

  bool use_crosswalk_detection;
  std::string points_topic;
  private_nh.param("use_crosswalk_detection", use_crosswalk_detection, true);
  private_nh.param<std::string>("points_topic", points_topic, "points_lanes");

  // class声明类
  CrossWalk crosswalk;
  VelocitySetPath vs_path;
  VelocitySetInfo vs_info;

  // velocity set subscriber
  ros::Subscriber waypoints_sub = nh.subscribe("base_waypoints", 1, &VelocitySetPath::waypointsCallback, &vs_path);
  ros::Subscriber current_vel_sub = nh.subscribe("current_velocity", 1, &VelocitySetPath::currentVelocityCallback, &vs_path);

  // velocity set info subscriber
  ros::Subscriber config_sub = nh.subscribe("config/velocity_set", 1, &VelocitySetInfo::configCallback, &vs_info);
  ros::Subscriber points_sub = nh.subscribe(points_topic, 1, &VelocitySetInfo::pointsCallback, &vs_info);
  ros::Subscriber localizer_sub = nh.subscribe("localizer_pose", 1, &VelocitySetInfo::localizerPoseCallback, &vs_info);
  ros::Subscriber control_pose_sub = nh.subscribe("current_pose", 1, &VelocitySetInfo::controlPoseCallback, &vs_info);
  ros::Subscriber closest_waypoint_sub = nh.subscribe("closest_waypoint", 1, &VelocitySetInfo::closestWaypointCallback, &vs_info);

  // vector map subscriber
  ros::Subscriber sub_dtlane = nh.subscribe("vector_map_info/cross_walk", 1, &CrossWalk::crossWalkCallback, &crosswalk);
  ros::Subscriber sub_area = nh.subscribe("vector_map_info/area", 1, &CrossWalk::areaCallback, &crosswalk);
  ros::Subscriber sub_line = nh.subscribe("vector_map_info/line", 1, &CrossWalk::lineCallback, &crosswalk);
  ros::Subscriber sub_point = nh.subscribe("vector_map_info/point", 1, &CrossWalk::pointCallback, &crosswalk);

  // publisher
  ros::Publisher detection_range_pub = nh.advertise<visualization_msgs::MarkerArray>("detection_range", 0);
  ros::Publisher temporal_waypoints_pub = nh.advertise<waypoint_follower::lane>("temporal_waypoints", 1000, true);
  ros::Publisher obstacle_pub = nh.advertise<visualization_msgs::Marker>("obstacle", 0);

  ros::Rate loop_rate(LOOP_RATE);
  while (ros::ok())
  {
    ros::spinOnce();
//四个回调函数是否接收到数据，包括cross_walk,area,line,point，同时检测是否设置路径点
    if (crosswalk.loaded_all && !crosswalk.set_points)
      crosswalk.setCrossWalkPoints();

//是否获成功获取机器人的位置和是否用户在设置路径
    if (!vs_info.getSetPose() || !vs_path.getSetPath())
    {
      loop_rate.sleep();
      continue;
    }
///是否开启人行横道的检测
    if (use_crosswalk_detection)
      crosswalk.setDetectionWaypoint(crosswalk.findClosestCrosswalk(vs_info.getClosestWaypoint(), vs_path.getPrevWaypoints(), STOP_SEARCH_DISTANCE));

    int obstacle_waypoint = -1;
    EControl detection_result = obstacleDetection(vs_info.getClosestWaypoint(), vs_path.getPrevWaypoints(), crosswalk, vs_info, detection_range_pub, obstacle_pub, &obstacle_waypoint);

    changeWaypoints(vs_info, detection_result, vs_info.getClosestWaypoint(), obstacle_waypoint, temporal_waypoints_pub, &vs_path);

    vs_info.clearPoints();

    loop_rate.sleep();
  }

  return 0;
}