当前位置： 首页 > news >正文

解读代码Dftpav-main（3.1规划核心traj_server_ros.cpp26.3.8）

news 2026/4/2 0:23:10

这篇文章开始解读轨迹规划服务器traj_server_ros.cpp，代码位于项目的Plan/traj_planner/src/traj_server_ros.cpp

代码如下

/** * @file ssc_server.cc * @author HKUST Aerial Robotics Group * @brief implementation for ssc planner server * @version 0.1 * @date 2019-02 * @copyright Copyright (c) 2019 */ /** * @file traj_server_ros.cpp revised version * @brief for minco planner server * @version 0.2 * @date 2021-04 * @copyright Copyright (c) 2021 */ #include "plan_utils/traj_server_ros.h" namespace plan_utils { TrajPlannerServer::TrajPlannerServer(ros::NodeHandle nh, int ego_id) : nh_(nh), work_rate_(20.0), ego_id_(ego_id) { p_input_smm_buff_ = new moodycamel::ReaderWriterQueue<SemanticMapManager>( config_.kInputBufferSize); // p_smm_vis_ = new semantic_map_manager::Visualizer(nh, ego_id); p_traj_vis_ = new TrajVisualizer(nh, ego_id); nh.getParam("enable_urban", enable_urban_); p_planner_ = new plan_manage::TrajPlanner(nh, ego_id, enable_urban_); } //use this! TrajPlannerServer::TrajPlannerServer(ros::NodeHandle nh, double work_rate, int ego_id) : nh_(nh), work_rate_(work_rate), ego_id_(ego_id) { p_input_smm_buff_ = new moodycamel::ReaderWriterQueue<SemanticMapManager>( config_.kInputBufferSize); // p_smm_vis_ = new semantic_map_manager::Visualizer(nh, ego_id); p_traj_vis_ = new TrajVisualizer(nh, ego_id); nh.getParam("enable_urban", enable_urban_); nh.param("isparking", isparking,true); p_planner_ = new plan_manage::TrajPlanner(nh, ego_id, enable_urban_); parking_sub_ = nh_.subscribe("/move_base_simple/goal", 1, &TrajPlannerServer::ParkingCallback, this); if(!isparking){ trajplan = std::bind(&plan_manage::TrajPlanner::RunOnce,p_planner_); } else{ trajplan = std::bind(&plan_manage::TrajPlanner::RunOnceParking,p_planner_); } } void TrajPlannerServer::PushSemanticMap(const SemanticMapManager& smm) { if (p_input_smm_buff_) p_input_smm_buff_->try_enqueue(smm); } void TrajPlannerServer::Init(const std::string& config_path) { p_planner_->Init(config_path); nh_.param("use_sim_state", use_sim_state_, true); std::string traj_topic = std::string("/vis/agent_") + std::to_string(ego_id_) + std::string("/minco/exec_traj"); // to publish command to sim ctrl_signal_pub_ = nh_.advertise<vehicle_msgs::ControlSignal>("ctrl", 20); executing_traj_vis_pub_ = nh_.advertise<visualization_msgs::MarkerArray>(traj_topic, 1); debug_pub = nh_.advertise<std_msgs::Bool>("/DEBUG", 1); end_pt_.setZero(); } //call the main thread void TrajPlannerServer::Start() { // printf("\033[34mTrajPlannerServer]TrajPlannerServer::Start\n\033[0m"); if (is_replan_on_) { return; } is_replan_on_ = true; // only call once //@yuwei : set map interface to planner p_planner_->set_map_interface(&map_adapter_); printf("[TrajPlannerServer]Planner server started.\n"); std::thread(&TrajPlannerServer::MainThread, this).detach(); std::thread(&TrajPlannerServer::PublishThread, this).detach(); } void TrajPlannerServer::PublishThread(){ using namespace std::chrono; system_clock::time_point current_start_time{system_clock::now()}; system_clock::time_point next_start_time{current_start_time}; const milliseconds interval{static_cast<int>(10)}; // 50ms while (true) { current_start_time = system_clock::now(); next_start_time = current_start_time + interval; PublishData(); std::this_thread::sleep_until(next_start_time); } } void TrajPlannerServer::MainThread() { using namespace std::chrono; system_clock::time_point current_start_time{system_clock::now()}; system_clock::time_point next_start_time{current_start_time}; const milliseconds interval{static_cast<int>(1000.0 / work_rate_)}; // 50ms while (true) { current_start_time = system_clock::now(); next_start_time = current_start_time + interval; PlanCycleCallback(); std::this_thread::sleep_until(next_start_time); } } void TrajPlannerServer::PlanCycleCallback() { if (!is_replan_on_) { return; } while (p_input_smm_buff_->try_dequeue(last_smm_)) { is_map_updated_ = true; } /*find the latest smm if no smm, the process will return */ if (!is_map_updated_) { printf("No map is updated"); return; } // Update map auto map_ptr = std::make_shared<semantic_map_manager::SemanticMapManager>(last_smm_); map_adapter_.set_map(map_ptr); //=maptp, the mapinterface used in p_planner // PublishData(); //vis smm, vis lasttraj, pub control law double current_time = ros::Time::now().toSec(); if (executing_traj_!=nullptr && current_time > executing_traj_->at(final_traj_index_).end_time) { printf("[TrajPlannerServer]Mission complete.\n"); m.lock(); executing_traj_.release(); m.unlock(); p_planner_->release_initial_state(); return; } std_msgs::Bool debug; debug.data = 1; debug_pub.publish(debug); if (enable_urban_){ // the requency is high enough so that we will not consider event triggered replanning static int fsm_num = 0; fsm_num++; if (fsm_num > 100000||CheckReplan()) { if (next_traj_ == nullptr) { Replan(); // return; } if (next_traj_ !=nullptr) { m.lock(); executing_traj_ = std::move(next_traj_); next_traj_.release(); fsm_num = 0; final_traj_index_ = executing_traj_->size() - 1; exe_traj_index_ = 0; m.unlock(); p_traj_vis_->displayPolyTraj(executing_traj_); Display(); // ros::Duration(100.0).sleep(); return; } } } else{ CheckReplan(); } } void TrajPlannerServer::PublishData() { using common::VisualizationUtil; m.lock(); ros::Time current_time = ros::Time::now(); if(!is_map_updated_) { m.unlock(); return;} // smm visualization if(is_map_updated_) { // p_smm_vis_->VisualizeDataWithStamp(current_time, last_smm_); // p_smm_vis_->SendTfWithStamp(current_time, last_smm_); } if (executing_traj_ == nullptr ||exe_traj_index_ > final_traj_index_ || executing_traj_->at(exe_traj_index_).duration < 1e-5) { common::State state = ego_state; state.velocity = 0.0; state.steer = 0.0; state.acceleration = 0.0; state.curvature = 0.0; double t = current_time.toSec(); state.time_stamp = t; if(ctrl_state_hist_.size()==0){ ctrl_state_hist_.push_back(ego_state); } else{ FilterSingularityState(ctrl_state_hist_, &state); ctrl_state_hist_.push_back(state); if (ctrl_state_hist_.size() > 100) ctrl_state_hist_.erase(ctrl_state_hist_.begin()); } vehicle_msgs::ControlSignal ctrl_msg; vehicle_msgs::Encoder::GetRosControlSignalFromControlSignal( common::VehicleControlSignal(state), ros::Time(t), std::string("map"), &ctrl_msg); ctrl_signal_pub_.publish(ctrl_msg); m.unlock(); return; } // trajectory feedback { if (use_sim_state_) { common::State state; double t = current_time.toSec(); state.time_stamp = t; if (executing_traj_->at(exe_traj_index_).end_time <= t){ exe_traj_index_ += 1; } if (exe_traj_index_ > final_traj_index_){ m.unlock();return;} // std::cout << " exe_traj_index_ " << exe_traj_index_ << std::endl; // std::cout << " final_traj_index_ " << final_traj_index_ << std::endl; executing_traj_->at(exe_traj_index_).traj.GetState(t-executing_traj_->at(exe_traj_index_).start_time, &state); // std::cout<<"state: "<<state.vec_position.transpose()<<"\n"; FilterSingularityState(ctrl_state_hist_, &state); ctrl_state_hist_.push_back(state); if (ctrl_state_hist_.size() > 100) ctrl_state_hist_.erase(ctrl_state_hist_.begin()); vehicle_msgs::ControlSignal ctrl_msg; vehicle_msgs::Encoder::GetRosControlSignalFromControlSignal( common::VehicleControlSignal(state), ros::Time(t), std::string("map"), &ctrl_msg); //hzchzc /* vec_position: x: 9.96869580051 y: 58.806571087 z: 0.0 angle: 0.777951436039 curvature: 0.000542476085517 velocity: 9.70671770974 acceleration: 0.443944701338 steer: 0.00154605561188 */ // ctrl_msg.state.acceleration = 0; // ctrl_msg.state.curvature = 0; // ctrl_msg.state.acceleration = 0; // ctrl_msg.state.steer = 0; // std::cout<<"ctrl_msg: "<<ctrl_msg.state.vec_position.x<<" "<<ctrl_msg.state.vec_position.y<<std::endl; ctrl_signal_pub_.publish(ctrl_msg); } } // trajectory visualization /*{ { auto color = common::cmap["magenta"]; if (require_intervention_signal_) color = common::cmap["yellow"]; visualization_msgs::MarkerArray traj_mk_arr; if (require_intervention_signal_) { visualization_msgs::Marker traj_status; common::State state_begin; state_begin.time_stamp = executing_traj_->at(exe_traj_index_).start_time; // define the timestamp before call get state executing_traj_->at(exe_traj_index_).traj.GetState(0.0, &state_begin); Vec3f pos = Vec3f(state_begin.vec_position[0], state_begin.vec_position[1], 5.0); common::VisualizationUtil::GetRosMarkerTextUsingPositionAndString( pos, std::string("Intervention Needed!"), common::cmap["red"], Vec3f(5.0, 5.0, 5.0), 0, &traj_status); traj_mk_arr.markers.push_back(traj_status); } int num_traj_mks = static_cast<int>(traj_mk_arr.markers.size()); common::VisualizationUtil::FillHeaderIdInMarkerArray( current_time, std::string("map"), last_trajmk_cnt_, &traj_mk_arr); last_trajmk_cnt_ = num_traj_mks; executing_traj_vis_pub_.publish(traj_mk_arr); } }*/ m.unlock(); } // minco display void TrajPlannerServer::Display(){ // std::cout << "\033[34m[TrajPlannerServer]TrajPlannerServer::Display. \033[0m" << std::endl; p_traj_vis_->displayPolyH(p_planner_->display_hPolys()); p_traj_vis_->displayObs(p_planner_->display_vec_obs()); p_traj_vis_->displayKinoPath(p_planner_->display_kino_path()); p_traj_vis_->displayInnerList(p_planner_->display_InnterPs(), 0); p_traj_vis_->displayFitSurTrajs(p_planner_->display_surround_trajs()); } ErrorType TrajPlannerServer::FilterSingularityState( const vec_E<common::State>& hist, common::State* filter_state) { if (hist.empty()) { return kWrongStatus; } double duration = filter_state->time_stamp - hist.back().time_stamp; double max_steer = M_PI / 4.0; double singular_velocity = kBigEPS; double max_orientation_rate = tan(max_steer) / 2.85 * singular_velocity; double max_orientation_change = max_orientation_rate * duration; if (fabs(filter_state->velocity) < singular_velocity && fabs(normalize_angle(filter_state->angle - hist.back().angle)) > max_orientation_change) { // printf( // "[TrajPlannerServer]Detect singularity velocity %lf angle (%lf, %lf).\n", // filter_state->velocity, hist.back().angle, filter_state->angle); filter_state->angle = hist.back().angle; // printf("[TrajPlannerServer]Filter angle to %lf.\n", hist.back().angle); } return kSuccess; } bool TrajPlannerServer::CheckReplan(){ //return 1: replan 0: not if(executing_traj_==nullptr) return true; bool is_near = false; bool is_collision = false; bool is_close_turnPoint = false; double cur_time = ros::Time::now().toSec(); Eigen::Vector2d localTarget; localTarget = executing_traj_->back().traj.getPos(executing_traj_->back().duration); double totaltrajTime = 0.0; for(int i = 0; i<executing_traj_->size(); i++){ totaltrajTime += executing_traj_->at(i).duration; } //is close to turnPoint? if(exe_traj_index_ == final_traj_index_) is_close_turnPoint = false; else{ if((executing_traj_->at(exe_traj_index_).end_time - cur_time)<2.5) is_close_turnPoint = true; } //is near? if((executing_traj_->back().end_time - cur_time)<2*totaltrajTime / 3.0) is_near = true; else is_near = false; if(is_near && !is_close_turnPoint&&(localTarget-end_pt_.head(2)).norm()>0.1){ return true; } //collision-check for(int i = 0; i < executing_traj_->size(); i++){ for(double t = 0.0; t < executing_traj_->at(i).duration; t+=0.05){ Eigen::Vector2d pos; Eigen::Vector3d state; double yaw; pos = executing_traj_->at(i).traj.getPos(t); yaw = executing_traj_->at(i).traj.getAngle(t); state << pos[0],pos[1],yaw; map_adapter_.CheckIfCollisionUsingPosAndYaw(vp_,state,&is_collision); if(is_collision) return true; } } // executing_traj_/ return false; // map_adapter_.CheckIfCollisionUsingPosAndYaw } ErrorType TrajPlannerServer::Replan() { if (!is_replan_on_) return kWrongStatus; time_profile_tool_.tic(); // define the timestamp before call get state common::State desired_state; if(map_adapter_.GetEgoState(&desired_state)!=kSuccess){ return kWrongStatus; } desired_state.time_stamp = ros::Time::now().toSec()+Budget; if(executing_traj_ ==nullptr){ p_planner_->set_initial_state(desired_state); if (trajplan()!= kSuccess) { Display(); return kWrongStatus; } //wait double curt = ros::Time::now().toSec(); if(0){ // if(curt>desired_state.time_stamp){ ROS_WARN("exceed time budget"); return kWrongStatus; } else{ while(true){ curt = ros::Time::now().toSec(); if(curt>=desired_state.time_stamp){ break; } } } desired_state_hist_.clear(); desired_state_hist_.push_back(last_smm_.ego_vehicle().state()); ctrl_state_hist_.clear(); ctrl_state_hist_.push_back(last_smm_.ego_vehicle().state()); //record the hist state and control } else if(executing_traj_->at(exe_traj_index_).duration >= 0.0){ //locate the piece idx int pidx = exe_traj_index_; while(true){ if(desired_state.time_stamp<=executing_traj_->at(pidx).start_time+executing_traj_->at(pidx).duration){ break; } else{ pidx++; if(pidx>=executing_traj_->size()){ pidx--; break; } } } double t = desired_state.time_stamp - executing_traj_->at(pidx).start_time; executing_traj_->at(pidx).traj.GetState(t, &desired_state); FilterSingularityState(desired_state_hist_, &desired_state); desired_state_hist_.push_back(desired_state); if (desired_state_hist_.size() > 100) desired_state_hist_.erase(desired_state_hist_.begin()); p_planner_->set_initial_state(desired_state); if (trajplan() != kSuccess) { Display(); return kWrongStatus; } printf("\033[32m[TrajPlannerServer]Traj planner succeed in %lf ms.\n\033[0m", time_profile_tool_.toc()); //wait double curt = ros::Time::now().toSec(); if(curt>desired_state.time_stamp){ ROS_WARN("exceed time budget"); return kWrongStatus; } else{ while(true){ curt = ros::Time::now().toSec(); if(curt>=desired_state.time_stamp){ break; } } } } else{ return kWrongStatus; } next_traj_ = std::move(p_planner_->trajectory()); if(next_traj_->at(0).duration <= 1e-5){ next_traj_.release(); return kWrongStatus; } return kSuccess; } void TrajPlannerServer::ParkingCallback(const geometry_msgs::PoseStamped &msg) { std::cout << "Triggered parking mode!" << std::endl; end_pt_ << msg.pose.position.x, msg.pose.position.y, tf::getYaw(msg.pose.orientation), 1.0e-2; std::cout<<"end_pt: "<<end_pt_.transpose()<<std::endl; end_pt_ << -20.2606 ,-7.24047 ,-3.13672 , 0.01; have_parking_target_ = true; p_planner_->setParkingEnd(end_pt_); } } // namespace planning

1. 文件作用概述

  • 文件名：traj_server_ros.cpp（修订版，用于 MINCO 规划器服务器）
  • 命名空间：plan_utils
  • 核心类：TrajPlannerServer
  • 功能：
    1. 初始化轨迹规划器（MINCO）与 ROS 接口
    2. 接收语义地图更新（PushSemanticMap）
    3. 在独立线程中按设定频率执行规划周期（MainThread）
    4. 发布控制指令与轨迹可视化
    5. 支持停车模式与城市场景模式切换
    6. 处理停车目标点回调（ParkingCallback）

2. 构造函数

2.1 双构造函数

有两个构造函数，区别在于是否指定work_rate（工作频率）：

//两个构造函数 TrajPlannerServer::TrajPlannerServer(ros::NodeHandle nh, int ego_id) : nh_(nh), work_rate_(20.0), ego_id_(ego_id) { p_input_smm_buff_ = new moodycamel::ReaderWriterQueue<SemanticMapManager>( config_.kInputBufferSize); // p_smm_vis_ = new semantic_map_manager::Visualizer(nh, ego_id); p_traj_vis_ = new TrajVisualizer(nh, ego_id); nh.getParam("enable_urban", enable_urban_); p_planner_ = new plan_manage::TrajPlanner(nh, ego_id, enable_urban_); } //use this! TrajPlannerServer::TrajPlannerServer(ros::NodeHandle nh, double work_rate, int ego_id) : nh_(nh), work_rate_(work_rate), ego_id_(ego_id) { p_input_smm_buff_ = new moodycamel::ReaderWriterQueue<SemanticMapManager>( config_.kInputBufferSize); // p_smm_vis_ = new semantic_map_manager::Visualizer(nh, ego_id); p_traj_vis_ = new TrajVisualizer(nh, ego_id); nh.getParam("enable_urban", enable_urban_); nh.param("isparking", isparking,true); p_planner_ = new plan_manage::TrajPlanner(nh, ego_id, enable_urban_); parking_sub_ = nh_.subscribe("/move_base_simple/goal", 1, &TrajPlannerServer::ParkingCallback, this); if(!isparking){ trajplan = std::bind(&plan_manage::TrajPlanner::RunOnce,p_planner_); } else{ trajplan = std::bind(&plan_manage::TrajPlanner::RunOnceParking,p_planner_); } }
  • 第一个：固定 20 Hz
  • 第二个：可自定义频率（在park.cc中调用的是第二个）

第二个构造函数关键代码：

nh.getParam("enable_urban", enable_urban_); nh.param("isparking", isparking, true); p_planner_ = new plan_manage::TrajPlanner(nh, ego_id, enable_urban_); parking_sub_ = nh_.subscribe("/move_base_simple/goal", 1, &TrajPlannerServer::ParkingCallback, this); if (!isparking) { trajplan = std::bind(&plan_manage::TrajPlanner::RunOnce, p_planner_); } else { trajplan = std::bind(&plan_manage::TrajPlanner::RunOnceParking, p_planner_); }
  • 读取enable_urban（城市场景标志）与isparking（停车标志）。
  • 订阅/move_base_simple/goal话题，接收 Rviz 发送的停车目标点，触发ParkingCallback。
  • 根据isparking选择不同的规划函数：
    • RunOnce：普通轨迹生成
    • RunOnceParking：停车专用轨迹生成（末端姿态精调）

3. 关键成员变量

  • p_input_smm_buff_：无锁队列，缓存语义地图更新（生产者-消费者模式，对应函数4.1PushSemanticMap）。
  • p_traj_vis_：轨迹可视化器。
  • p_planner_：TrajPlanner实例（MINCO 优化器）。
  • work_rate_：规划线程频率（Hz）。
  • enable_urban_、isparking：模式标志。
  • trajplan：std::function，指向具体的规划函数（普通或停车）。
  • executing_traj_：当前正在执行的轨迹。
  • next_traj_：下一次要切换的轨迹（用于重规划）。
  • ctrl_signal_pub_：发布控制指令到ctrl话题。
  • executing_traj_vis_pub_：发布轨迹可视化 Marker。
  • 成员类型作用
    nh_ros::NodeHandleROS 节点句柄
    work_rate_double规划线程工作频率（Hz）
    ego_id_int自车 ID
    p_input_smm_buff_moodycamel::ReaderWriterQueue<SemanticMapManager>无锁队列缓存地图更新
    p_traj_vis_TrajVisualizer*轨迹可视化器
    p_planner_plan_manage::TrajPlanner*MINCO 轨迹优化器
    enable_urban_bool是否启用城市场景模式
    isparkingbool是否停车模式
    trajplanstd::function<ErrorType()>绑定具体规划函数（普通或停车）
    executing_traj_std::unique_ptr<TrajectoryBundle>当前执行轨迹
    next_traj_std::unique_ptr<TrajectoryBundle>下次切换的轨迹（重规划结果）
    ctrl_signal_pub_ros::Publisher发布控制指令（ctrl话题）
    executing_traj_vis_pub_ros::Publisher发布轨迹可视化 Marker
    parking_sub_ros::Subscriber订阅停车目标点（/move_base_simple/goal）

4. 主要方法

4.1PushSemanticMap(const SemanticMapManager& smm)

  • 功能：将最新的语义地图放入无锁队列，供规划线程消费。
  • 实现：p_input_smm_buff_->try_enqueue(smm)

4.2Init(const std::string& config_path)

  • 功能：初始化MINCO 规划器配置，读取use_sim_state（是否使用仿真状态），创建 ROS 发布器（ctrl、/vis/.../minco/exec_traj、/DEBUG）。
  • 关键操作：
    • p_planner_->Init(config_path)加载 MINCO 参数文件
    • 读取use_sim_state（是否使用仿真状态）
    • 创建ctrl_signal_pub_、executing_traj_vis_pub_、debug_pub

4.3Start()

  • 功能：启动规划与发布线程，开始运行规划循环。
    • 线程1：4.5MainThread：按work_rate_执行函数4.6PlanCycleCallback（规划周期）。
    • 线程2：4.4PublishThread：10 ms 周期发布控制指令与可视化数据。
  • 关键操作：
    • 设置地图接口p_planner_->set_map_interface(&map_adapter_)
    • 创建并分离MainThread与PublishThread

4.4 PublishThread()

  • 功能：以固定频率（10 ms）发布控制指令与可视化数据。
  • 实现：循环睡眠至下一个周期，调用 4.7PublishData()

4.5MainThread()

  • 功能：按work_rate_频率执行规划周期。
  • 实现：循环睡眠至下一个周期，调用函数4.6PlanCycleCallback()

4.6PlanCycleCallback()

核心规划逻辑：

  1. 从队列取出最新地图。
  2. 检查当前轨迹是否已完成（超时或到达终点）。
  3. 如果enable_urban_：
    • 高频运行，定期或事件触发重规划（CheckReplan）。
    • 若需重规划且next_traj_准备好，则切换为执行轨迹。
  4. 否则（非城市场景）仅检查重规划条件。

4.7PublishData()

  • 发布控制指令（根据executing_traj_插值得到状态）。
  • 处理轨迹完成、无轨迹、仿真状态等情况。
  • 使用函数4.9FilterSingularityState过滤奇异位形（防止角度突变）。
  • 发布轨迹可视化（注释部分可启用）。
  • 流程：
    • 若无地图更新则返回
    • 若无执行轨迹，发布零控制量
    • 若有执行轨迹，根据当前时间插值得到状态，滤波后发布控制指令
    • （注释部分）可发布轨迹可视化 Marker

4.8Display

调用可视化器显示 MINCO 的内部数据（多项式、障碍物、路径等）。

4.9FilterSingularityState(...)对应4.7

  • 功能：过滤奇异位形（防止速度接近零时角度突变）。
  • 逻辑：若速度很小且角度变化超过阈值，则将角度设为历史值。

4.10CheckReplan

判断是否需重规划：

  • 接近终点且目标点未到达
  • 检测到碰撞
  • 接近转弯点（避免中途切换导致不稳定）

4.11Replan

执行重规划，重新生成新轨迹：

  • 获取当前/预测状态作为初始条件
  • 调用trajplan（普通或停车模式）
  • 等待时间预算（确保实时性）
  • 生成next_traj_

4.12ParkingCallback

接收 Rviz 发送的geometry_msgs::PoseStamped作为停车目标点，设置到规划器。

  • 关键操作：
    • 解析 PoseStamped 为end_pt_（x, y, yaw, 容差）
    • 调用p_planner_->setParkingEnd(end_pt_)
    • 标记have_parking_target_ = true

5. 方法协作关系

  1. 初始化阶段：构造函数 →Init→Start
  2. 运行阶段：
    • MainThread循环调用PlanCycleCallback
    • PublishThread循环调用PublishData
    • PlanCycleCallback依赖PushSemanticMap提供的地图数据
    • Replan在需要时由PlanCycleCallback调用
    • ParkingCallback可随时修改停车目标，影响Replan结果
  3. 可视化：Display与PublishData中的可视化部分

6. 与park.cc及park.launch的关联

  • park.launch中isparking=true会让TrajPlannerServer使用RunOnceParking。
  • park.cc通过p_traj_server_->Init(traj_config_path)加载 MINCO 参数。
  • 停车目标可由 Rviz 手动发送，触发ParkingCallback设置end_pt_。

7. 模式切换逻辑

参数模式规划函数特点
isparking=false普通模式RunOnce常规轨迹优化
isparking=true停车模式RunOnceParking末端姿态精调，适合泊车
enable_urban=true城市场景高频重规划复杂交通，需频繁更新轨迹

8. 总结

TrajPlannerServer是一个高内聚、多线程、模式感知的 ROS 轨迹规划服务器类：

  • 数据输入：语义地图（无锁队列）、停车目标点（ROS 话题）
  • 核心处理：MINCO 优化、重规划判断、奇点滤波
  • 数据输出：控制指令（ctrl话题）、轨迹可视化（Marker）
  • 模式支持：普通、停车、城市场景
  • 实时性保障：双线程 + 固定周期 + 时间预算等待

这个类把复杂的轨迹优化与 ROS 系统集成封装成一个可配置的服务器，是 DFTPAV 能在停车等场景下稳定运行的关键。

9. 主要调用位置：park.cc

在src/Sim/app/planning_integrated/src/park.cc的main函数中：

p_traj_server_ = new plan_utils::TrajPlannerServer(nh, traj_planner_work_rate, ego_id); p_traj_server_->Init(traj_config_path); p_traj_server_->Start();
  • 这里创建了TrajPlannerServer对象（工作频率 20 Hz）。
  • 调用Init加载配置文件（如minco_config.pb.txt）。
  • 调用Start启动规划与发布线程。
  • 这是停车场景下轨迹规划服务器的启动入口。

其他可能的调用位置（同目录下的测试/实验文件）

在src/Sim/app/planning_integrated/src/目录下还有几个类似入口文件，它们可能复用或参考了TrajPlannerServer：

（1）test_traj_with_mpdm.cc

你之前贴出的代码内容与park.cc几乎一样，也是：

p_traj_server_ = new plan_utils::TrajPlannerServer(nh, traj_planner_work_rate, ego_id);

这说明MPDM 测试程序也使用了TrajPlannerServer，只是在行为规划端接入了 MPDM 策略。

（2）test_ssc_with_mpdm.cc与test_ssc_with_eudm.cc

虽然你没贴出它们的完整代码，但从文件名推测：

  • test_ssc_with_mpdm.cc：SSC（Spatial Safety Checking）+ MPDM 行为规划测试
  • test_ssc_with_eudm.cc：SSC + EUDM（另一种决策策略）测试 它们很可能也会创建TrajPlannerServer实例，因为都需要轨迹规划模块与行为规划集成。

调用模式总结

文件场景是否创建TrajPlannerServer说明
park.cc停车场景✅主入口，正式停车规划
test_traj_with_mpdm.ccMPDM 行为测试✅与park.cc同源，测试用
test_ssc_with_mpdm.ccSSC+MPDM 测试likely ✅需轨迹规划做运动生成
test_ssc_with_eudm.ccSSC+EUDM 测试likely ✅同上

在项目架构中的位置

Sim/app/planning_integrated/src/ ├── park.cc ← 正式停车入口（调用 TrajPlannerServer） ├── test_traj_with_mpdm.cc ← 测试入口（调用 TrajPlannerServer） ├── test_ssc_with_mpdm.cc ← 可能调用 ├── test_ssc_with_eudm.cc ← 可能调用 Plan/traj_planner/src/ └── traj_server_ros.cpp ← TrajPlannerServer 实现
  • TrajPlannerServer属于Plan 模块（Plan/traj_planner/）。
  • 它被Sim 模块的集成入口调用，形成Sim 调用 Plan的跨模块协作。
http://www.jsqmd.com/news/453838/

相关文章：

  • Linux:网络编程-基于HTTP协议的天气预报查询系统开发详解
  • Kafka自动提交把消息吃了：一次“已提交未处理”+重平衡导致丢数和爆堆积
  • 把 AI助手搬进飞书！OpenClaw接入完整指南
  • 2026广州GEO优化公司排名TOP5｜本地实力派盘点，亚森SEO稳居榜首！
  • 周红伟：2026年OpenClaw最佳实践：一键部署+免费API配置+集成8大股票分析Skills及避坑指南
  • matlab麻雀搜索算法(SSA)优化BP神经网络，权值和阈值，一个压缩包共三个文件，包括有数...
  • 深度学习在财务报表舞弊识别中的应用：构建一个智能审计助手
  • Rokid UXR 的手势追踪虚拟中更真实的手实战开发【含 工程源码 和 最终完成APK】
  • 开发者的临时文件自动化工具：提升效率与系统整洁度的关键方案
  • 别只当它是管家，RT-Thread 会自己生长
  • 权威解读：企业合作政策如何让非科班生通过国内AI认证实现“弯道超车”？
  • 2026年房山及燕山地区装修套餐全解析：五大优质服务商深度推荐 - 品牌2026
  • openclaw gateway status报错且gate无法正常运行解决办法
  • 无数绘画测试！Nano Banana 2 vs GPT Image 1.5，谁才是最厉害的模型
  • LeetCode-35.搜索插入位置
  • 基于javaweb的作业智能推荐系统的设计与实现
  • 2026超纯水机厂家推荐：进口与国产品牌实力对比 - 品牌推荐大师
  • 光学神经网络：进展与挑战(Optical Neural Networks: Progress and Challenges)
  • 如何本地部署大模型（以PaddleOCR-VL-1.5为例）
  • 2026年房山环保家装公司怎么选？五家实力装企深度解析 - 品牌2026
  • Gemini 3.1 Flash Image Preview (Nano Banana 2) 深度技术评测与极速接入指南
  • 2026最新25万级SUV智驾领先双能源车型推荐！权威榜单发布 - 十大品牌榜
  • ESP-IDF Chip revision问题解决方案
  • 2026年 信捷电气厂家推荐排行榜：江苏/安徽/上海/苏州/常州/合肥/芜湖/南京/镇江/南通/徐州/无锡，专业自动化解决方案与技术创新实力深度解析 - 品牌企业推荐师（官方）
  • 留学申请服务费用多少，考虑性价比，澳洲、马来西亚有啥好选择 - mypinpai
  • ARM Cortex-R52 内核详解（三）——异常处理机制
  • 【hello-agent】Plan-and-Solve
  • 2026 电池放电仪、电池内阻仪厂家：技术与口碑的双重认证 - 深度智识库
  • 计算机毕业设计源码：基于Python的智能推荐电商平台 Django Vue3 Scrapy爬虫 协同过滤推荐算法 大模型 购物 多模态 deepseek agent（建议收藏）✅
  • 2026最新25万级SUV/25万级SUV智驾/25万级SUV轿跑/25万级SUV双能源车型推荐：智驾领跑，实力重塑出行标杆 - 十大品牌榜