fast-planner代码解读【kino_replan_fsm.cpp】

概述

kino_replan_fsm.cpp订阅实时定位和目标点信息,每隔0.01s执行一次状态机,进行状态切换;每隔0.05s执行一次碰撞检测,按需进行重新规划。核心为执行变量exec_state_

主要函数及作用

  1. KinoReplanFSM::init
输入:句柄
作用:launch参数读取、定时器声明、pub声明、sub声明
  1. KinoReplanFSM::waypointCallback
作用:孤立目标点回调函数:设置目标点位置、速度并进行其球形的可视化、执行状态变量更新
  1. KinoReplanFSM::odometryCallback
作用:定位回调函数:位置、航向、三轴速度、标志位置true(表明接受到定位数据)
  1. KinoReplanFSM::changeFSMExecState
作用:按照给定参数进行执行状态变量的更新,并打印输出

5.KinoReplanFSM::printFSMExecState

作用:打印当前执行状态变量的数值
  1. KinoReplanFSM::execFSMCallback

                
annant@annant-Legion-R7000P-ARH7:~/competition_ws$ source devel/setup.bash annant@annant-Legion-R7000P-ARH7:~/competition_ws$ roslaunch plan_manage kino_replan.launch ... logging to /home/annant/.ros/log/def20c68-9839-11f0-8aea-4368951a8594/roslaunch-annant-Legion-R7000P-ARH7-6284.log Checking log directory for disk usage. This may take a while. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB. started roslaunch server http://annant-Legion-R7000P-ARH7:43915/ SUMMARY ======== PARAMETERS * /fast_planner_node/bspline/limit_acc: 2.0 * /fast_planner_node/bspline/limit_ratio: 1.1 * /fast_planner_node/bspline/limit_vel: 3.0 * /fast_planner_node/fsm/flight_type: 1 * /fast_planner_node/fsm/thresh_no_replan: 2.0 * /fast_planner_node/fsm/thresh_replan: 1.5 * /fast_planner_node/fsm/waypoint0_x: 19.0 * /fast_planner_node/fsm/waypoint0_y: 0.0 * /fast_planner_node/fsm/waypoint0_z: 1.0 * /fast_planner_node/fsm/waypoint1_x: -19.0 * /fast_planner_node/fsm/waypoint1_y: 0.0 * /fast_planner_node/fsm/waypoint1_z: 1.0 * /fast_planner_node/fsm/waypoint2_x: 0.0 * /fast_planner_node/fsm/waypoint2_y: 19.0 * /fast_planner_node/fsm/waypoint2_z: 1.0 * /fast_planner_node/fsm/waypoint_num: 2 * /fast_planner_node/manager/clearance_threshold: 0.2 * /fast_planner_node/manager/control_points_distance: 0.5 * /fast_planner_node/manager/dynamic_environment: 0 * /fast_planner_node/manager/local_segment_length: 6.0 * /fast_planner_node/manager/max_acc: 2.0 * /fast_planner_node/manager/max_jerk: 4.0 * /fast_planner_node/manager/max_vel: 3.0 * /fast_planner_node/manager/use_geometric_path: False * /fast_planner_node/manager/use_kinodynamic_path: True * /fast_planner_node/manager/use_optimization: True * /fast_planner_node/manager/use_topo_path: False * /fast_planner_node/optimization/algorithm1: 15 * /fast_planner_node/optimization/algorithm2: 11 * /fast_planner_node/optimization/dist0: 0.4 * /fast_planner_node/optimization/lambda1: 10.0 * /fast_planner_node/optimization/lambda2: 5.0 * /fast_planner_node/optimization/lambda3: 1e-05 * /fast_planner_node/optimization/lambda4: 0.01 * /fast_planner_node/optimization/lambda7: 100.0 * /fast_planner_node/optimization/max_acc: 2.0 * /fast_planner_node/optimization/max_iteration_num1: 2 * /fast_planner_node/optimization/max_iteration_num2: 300 * /fast_planner_node/optimization/max_iteration_num3: 200 * /fast_planner_node/optimization/max_iteration_num4: 200 * /fast_planner_node/optimization/max_iteration_time1: 0.0001 * /fast_planner_node/optimization/max_iteration_time2: 0.005 * /fast_planner_node/optimization/max_iteration_time3: 0.003 * /fast_planner_node/optimization/max_iteration_time4: 0.003 * /fast_planner_node/optimization/max_vel: 3.0 * /fast_planner_node/optimization/order: 3 * /fast_planner_node/planner_node/planner: 1 * /fast_planner_node/sdf_map/cx: 321.04638671875 * /fast_planner_node/sdf_map/cy: 243.44969177246094 * /fast_planner_node/sdf_map/depth_filter_margin: 2 * /fast_planner_node/sdf_map/depth_filter_maxdist: 5.0 * /fast_planner_node/sdf_map/depth_filter_mindist: 0.2 * /fast_planner_node/sdf_map/depth_filter_tolerance: 0.15 * /fast_planner_node/sdf_map/esdf_slice_height: 0.3 * /fast_planner_node/sdf_map/frame_id: world * /fast_planner_node/sdf_map/fx: 387.229248046875 * /fast_planner_node/sdf_map/fy: 387.229248046875 * /fast_planner_node/sdf_map/ground_height: -1.0 * /fast_planner_node/sdf_map/k_depth_scaling_factor: 1000.0 * /fast_planner_node/sdf_map/local_bound_inflate: 0.0 * /fast_planner_node/sdf_map/local_map_margin: 50 * /fast_planner_node/sdf_map/local_update_range_x: 5.5 * /fast_planner_node/sdf_map/local_update_range_y: 5.5 * /fast_planner_node/sdf_map/local_update_range_z: 4.5 * /fast_planner_node/sdf_map/map_size_x: 40.0 * /fast_planner_node/sdf_map/map_size_y: 20.0 * /fast_planner_node/sdf_map/map_size_z: 5.0 * /fast_planner_node/sdf_map/max_ray_length: 4.5 * /fast_planner_node/sdf_map/min_ray_length: 0.5 * /fast_planner_node/sdf_map/obstacles_inflation: 0.099 * /fast_planner_node/sdf_map/p_hit: 0.65 * /fast_planner_node/sdf_map/p_max: 0.9 * /fast_planner_node/sdf_map/p_min: 0.12 * /fast_planner_node/sdf_map/p_miss: 0.35 * /fast_planner_node/sdf_map/p_occ: 0.8 * /fast_planner_node/sdf_map/pose_type: 1 * /fast_planner_node/sdf_map/resolution: 0.1 * /fast_planner_node/sdf_map/show_esdf_time: False * /fast_planner_node/sdf_map/show_occ_time: False * /fast_planner_node/sdf_map/skip_pixel: 2 * /fast_planner_node/sdf_map/use_depth_filter: True * /fast_planner_node/sdf_map/virtual_ceil_height: 2.5 * /fast_planner_node/sdf_map/visualization_truncate_height: 2.49 * /fast_planner_node/search/allocate_num: 100000 * /fast_planner_node/search/check_num: 5 * /fast_planner_node/search/horizon: 7.0 * /fast_planner_node/search/init_max_tau: 0.8 * /fast_planner_node/search/lambda_heu: 5.0 * /fast_planner_node/search/margin: 0.2 * /fast_planner_node/search/max_acc: 2.0 * /fast_planner_node/search/max_tau: 0.6 * /fast_planner_node/search/max_vel: 3.0 * /fast_planner_node/search/resolution_astar: 0.1 * /fast_planner_node/search/time_resolution: 0.8 * /fast_planner_node/search/w_time: 10.0 * /odom_visualization/color/a: 1.0 * /odom_visualization/color/b: 1.0 * /odom_visualization/color/g: 0.0 * /odom_visualization/color/r: 0.0 * /odom_visualization/covariance_scale: 100.0 * /odom_visualization/robot_scale: 1.0 * /pcl_render_node/cam_cx: 321.04638671875 * /pcl_render_node/cam_cy: 243.44969177246094 * /pcl_render_node/cam_fx: 387.229248046875 * /pcl_render_node/cam_fy: 387.229248046875 * /pcl_render_node/cam_height: 480 * /pcl_render_node/cam_width: 640 * /pcl_render_node/estimation_rate: 30.0 * /pcl_render_node/map/x_size: 40.0 * /pcl_render_node/map/y_size: 20.0 * /pcl_render_node/map/z_size: 5.0 * /pcl_render_node/sensing_horizon: 5.0 * /pcl_render_node/sensing_rate: 30.0 * /quadrotor_simulator_so3/rate/odom: 200.0 * /quadrotor_simulator_so3/simulator/init_state_x: -19.0 * /quadrotor_simulator_so3/simulator/init_state_y: -0.0 * /quadrotor_simulator_so3/simulator/init_state_z: 1.0 * /random_forest/ObstacleShape/lower_hei: 0.0 * /random_forest/ObstacleShape/lower_rad: 0.5 * /random_forest/ObstacleShape/radius_h: 0.5 * /random_forest/ObstacleShape/radius_l: 0.7 * /random_forest/ObstacleShape/seed: -1 * /random_forest/ObstacleShape/theta: 0.5 * /random_forest/ObstacleShape/upper_hei: 3.0 * /random_forest/ObstacleShape/upper_rad: 0.7 * /random_forest/ObstacleShape/z_h: 0.8 * /random_forest/ObstacleShape/z_l: 0.7 * /random_forest/init_state_x: -19.0 * /random_forest/init_state_y: -0.0 * /random_forest/map/circle_num: 10 * /random_forest/map/obs_num: 150 * /random_forest/map/resolution: 0.1 * /random_forest/map/x_size: 40.0 * /random_forest/map/y_size: 20.0 * /random_forest/map/z_size: 5.0 * /random_forest/sensing/radius: 5.0 * /random_forest/sensing/rate: 10.0 * /rosdistro: noetic * /rosversion: 1.17.4 * /so3_control/corrections/p: 0.0 * /so3_control/corrections/r: 0.0 * /so3_control/corrections/z: 0.0 * /so3_control/gains/ang/x: 0.07 * /so3_control/gains/ang/y: 0.07 * /so3_control/gains/ang/z: 0.1 * /so3_control/gains/pos/x: 2.0 * /so3_control/gains/pos/y: 2.0 * /so3_control/gains/pos/z: 3.5 * /so3_control/gains/rot/x: 1.0 * /so3_control/gains/rot/y: 1.0 * /so3_control/gains/rot/z: 1.0 * /so3_control/gains/vel/x: 1.8 * /so3_control/gains/vel/y: 1.8 * /so3_control/gains/vel/z: 2.0 * /so3_control/mass: 0.74 * /so3_control/use_angle_corrections: False * /so3_control/use_external_yaw: False * /traj_server/traj_server/time_forward: 1.5 * /waypoint_generator/waypoint_type: manual-lonely-way... NODES / fast_planner_node (plan_manage/fast_planner_node) odom_visualization (odom_visualization/odom_visualization) pcl_render_node (local_sensing_node/pcl_render_node) quadrotor_simulator_so3 (so3_quadrotor_simulator/quadrotor_simulator_so3) random_forest (map_generator/random_forest) so3_control (nodelet/nodelet) so3_disturbance_generator (so3_disturbance_generator/so3_disturbance_generator) traj_server (plan_manage/traj_server) waypoint_generator (waypoint_generator/waypoint_generator) ROS_MASTER_URI=http://localhost:11311 ERROR: cannot launch node of type [plan_manage/fast_planner_node]: Cannot locate node of type [fast_planner_node] in package [plan_manage]. Make sure file exists in package path and permission is set to executable (chmod +x) ERROR: cannot launch node of type [plan_manage/traj_server]: Cannot locate node of type [traj_server] in package [plan_manage]. Make sure file exists in package path and permission is set to executable (chmod +x) process[waypoint_generator-3]: started with pid [6328] ERROR: cannot launch node of type [map_generator/random_forest]: Cannot locate node of type [random_forest] in package [map_generator]. Make sure file exists in package path and permission is set to executable (chmod +x) process[quadrotor_simulator_so3-5]: started with pid [6329] process[so3_control-6]: started with pid [6334] process[so3_disturbance_generator-7]: started with pid [6336] type is so3_control/SO3ControlNodelet process[odom_visualization-8]: started with pid [6346] ERROR: cannot launch node of type [local_sensing_node/pcl_render_node]: Cannot locate node of type [pcl_render_node] in package [local_sensing_node]. Make sure file exists in package path and permission is set to executable (chmod +x) [WARN] [1758603844.642702072]: Shutdown request received. [WARN] [1758603844.643502636]: Shutdown request received. [WARN] [1758603844.644664574]: Reason given for shutdown: [[/quadrotor_simulator_so3] Reason: new node registered with same name] [WARN] [1758603844.644818133]: Shutdown request received. [WARN] [1758603844.644837708]: Reason given for shutdown: [[/waypoint_generator] Reason: new node registered with same name] [WARN] [1758603844.647252453]: Reason given for shutdown: [[/so3_control] Reason: new node registered with same name] [WARN] [1758603844.647250519]: Shutdown request received. [WARN] [1758603844.649314238]: Shutdown request received. [WARN] [1758603844.650018032]: Reason given for shutdown: [[/so3_disturbance_generator] Reason: new node registered with same name] [WARN] [1758603844.651132036]: Reason given for shutdown: [[/odom_visualization] Reason: new node registered with same name] Error in XmlRpcClient::writeRequest: write error (Connection refused). [quadrotor_simulator_so3-5] process has finished cleanly log file: /home/annant/.ros/log/def20c68-9839-11f0-8aea-4368951a8594/quadrotor_simulator_so3-5*.log ================================================================================REQUIRED process [so3_control-6] has died! process has finished cleanly log file: /home/annant/.ros/log/def20c68-9839-11f0-8aea-4368951a8594/so3_control-6*.log Initiating shutdown! ================================================================================ [odom_visualization-8] killing on exit [so3_disturbance_generator-7] killing on exit [so3_control-6] killing on exit [waypoint_generator-3] killing on exit shutting down processing monitor... ... shutting down processing monitor complete done
最新发布
09-24
<think> 我们正在解决ROS启动时无法定位节点类型的问题,并且节点重复注册导致关闭。问题涉及的节点包括: - plan_manage/fast_planner_node - plan_manage/traj_server - map_generator/random_forest - local_sensing_node/pcl_render_node 可能的原因和解决方案如下: ### 1. **节点未正确编译** - 确保包含这些节点的包已经正确编译。使用`catkin_make`或`catkin build`重新编译整个工作空间。 - 检查编译输出中是否有关于这些节点的错误信息。 ### 2. **环境变量未设置** - 在运行ROS节点前,需要设置环境变量。确保已经执行了`source devel/setup.bash`(或`setup.zsh`,取决于你的shell)。 - 可以将此命令添加到`~/.bashrc`(或`~/.zshrc`)中,以便每次打开终端时自动设置。 ### 3. **包路径问题** - 确保包含这些节点的包位于`catkin_ws/src`目录下。 - 检查`ROS_PACKAGE_PATH`环境变量是否包含这些包的路径。可以通过`echo $ROS_PACKAGE_PATH`查看。 ### 4. **节点可执行文件缺失或权限问题** - 在包目录下的`CMakeLists.txt`中,确保已经通过`add_executable`和`target_link_libraries`正确声明了节点可执行文件。 - 编译后,在`devel/lib/<package_name>`目录下检查是否存在节点的可执行文件(例如`fast_planner_node`)。 - 如果可执行文件不存在,检查编译日志。 - 如果存在,确保可执行文件具有执行权限(使用`chmod +x <executable>`)。 ### 5. **节点重复注册** - 节点重复注册通常是因为同一个节点被启动了多次。检查启动文件(`.launch`文件)和命令行,确保没有重复启动同一个节点。 - 另外,检查是否有多个工作空间被同时激活,导致同一个包被加载多次。确保只激活当前工作空间。 ### 6. **包名与节点名不匹配** - 在启动文件中,节点通常以`<package_name>/<executable_name>`的形式指定。确保包名和可执行文件名正确。 - 例如,如果`fast_planner_node`位于`plan_manage`包中,则节点类型应为`plan_manage/fast_planner_node`。 ### 7. **动态链接库问题** - 如果节点依赖其他库,确保这些库已正确安装并在`LD_LIBRARY_PATH`中。 ### 具体步骤: 1. **重新编译并设置环境** ```bash cd ~/catkin_ws catkin_make # 或者 catkin build source devel/setup.bash ``` 2. **检查节点可执行文件** - 例如,对于`plan_manage`包中的节点,检查: ```bash ls devel/lib/plan_manage/ ``` 应该看到`fast_planner_node`和`traj_server`等可执行文件。 3. **检查启动文件** - 打开启动文件(例如`xxx.launch`),检查节点声明部分。例如: ```xml <node pkg="plan_manage" type="fast_planner_node" name="fast_planner_node" output="screen"/> ``` 这里的`pkg`属性必须是包名,`type`必须是可执行文件名。 4. **使用命令行启动节点测试** - 尝试直接运行节点,例如: ```bash rosrun plan_manage fast_planner_node ``` 如果出现“找不到节点”的错误,说明环境变量或编译有问题。 5. **检查ROS包路径** ```bash echo $ROS_PACKAGE_PATH ``` 确保包含`plan_manage`等包的路径在其中(通常是`~/catkin_ws/src`)。 6. **解决节点重复注册** - 如果启动文件中有重复的节点,删除重复项。 - 使用`rosnode list`查看运行的节点,确保没有重复。 如果以上步骤无法解决问题,请提供更多信息,例如: - 编译的输出日志 - 启动文件内容 - 运行`rosrun`或`roslaunch`时的错误信息 这将有助于进一步诊断问题。 §§相关问题§§ 1. 如何检查ROS中节点的可执行文件是否正确生成? 2. 如何解决ROS启动文件中的节点重复启动问题? 3. 如何设置ROS环境变量以确保节点可被找到? 4. ROS中如何查看节点的完整路径和依赖关系? </think> ### ROS节点无法定位及重复注册问题解决方案 #### 1. **环境配置检查** - **确认工作空间初始化** 确保已执行 `source devel/setup.bash`(或对应Shell的setup文件) ```bash cd ~/catkin_ws && source devel/setup.bash ``` - **验证环境变量** 检查`ROS_PACKAGE_PATH`是否包含目标包路径: ```bash echo $ROS_PACKAGE_PATH | grep -E "plan_manage|map_generator|local_sensing_node" ``` 若缺失,手动添加路径: ```bash export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/your/custom/path ``` #### 2. **编译问题排查** - **重新编译并清理缓存** ```bash cd ~/catkin_ws catkin clean -y && catkin build ``` 重点检查编译日志中是否生成目标可执行文件(如`devel/lib/plan_manage/fast_planner_node`) - **检查CMake配置** 确认`CMakeLists.txt`包含完整编译指令(以`plan_manage`为例): ```cmake add_executable(fast_planner_node src/fast_planner_node.cpp) target_link_libraries(fast_planner_node ${catkin_LIBRARIES}) ``` #### 3. **节点权限问题** - **添加可执行权限** ```bash cd ~/catkin_ws chmod +x src/plan_manage/src/*.py src/map_generator/scripts/*.py # Python节点 chmod +x devel/lib/plan_manage/* # C++节点 ``` #### 4. **节点重复注册解决方案** - **检测冲突节点** ```bash rosnode list | grep -E "fast_planner|traj_server|random_forest|pcl_render" ``` - **终止残留进程** ```bash killall -9 fast_planner_node traj_server random_forest pcl_render_node ``` - **修改节点名称**(在启动文件中) ```xml <node name="unique_fast_planner" pkg="plan_manage" type="fast_planner_node"/> ``` #### 5. **包路径验证** - **检查包定位** ```bash rospack find plan_manage # 应返回正确路径 ``` - **重建包索引** ```bash rospack profile ``` #### 6. **依赖项检查** - **确认动态链接库** 使用`ldd`检查节点依赖: ```bash ldd devel/lib/plan_manage/fast_planner_node | grep "not found" ``` - **安装缺失依赖** 通过`rosdep`安装: ```bash rosdep install --from-paths src --ignore-src -y ``` ### 典型错误场景分析 | 错误现象 | 解决方案 | |---------|----------| | `ERROR: cannot launch node...` | 执行`catkin build --force-cmake`重建CMake | | `Multiple nodes with same name` | 在`.launch`文件中添加`ns="namespace"`属性 | | `ImportError: No module named` | 检查Python路径`export PYTHONPATH=$PYTHONPATH:~/catkin_ws/devel/lib/python2.7/dist-packages` | ### 调试建议 1. **单独启动测试** ```bash rosrun plan_manage fast_planner_node # 观察终端报错 ``` 2. **检查ROS日志** ```bash roscd /var/log/ros && tail -f latest/*.log ``` > 若仍存在问题,请提供: > - `CMakeLists.txt`相关片段 > - `rosnode info /节点名`输出 > - 完整启动文件内容
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值