ego-planner代码精读(1)-simple_run_launch文件

C++ ROS
项目代码学习
发布日期:   2021-10-26
文章字数:   1.9k
阅读时长:   10 分
阅读次数:  

完整代码：GitHub - ZJU-FAST-Lab/ego-planner

对应文章：EGO-Planner: An ESDF-Free Gradient-Based Local Planner for Quadrotors | IEEE Journals & Magazine | IEEE Xplore

为区分注释，注意源码中文为本人注释，英文为源码自带

simple_run.launch文件

定位源码文件

simple_run.launch基本上实现了论文中各个算法的功能，我们从该文件了解系统如何运行。

首先根据launch文件中定义的节点定位launch中用到的源码文件cpp(节选)(odom_topic里程计话题读取)

    <!-- 此文件为ego-planner基本启动文件-->
<launch>
  <!-- size of map, change the size inflate x, y, z according to your application -->
    <!-- 定义仅在launch文件中起作用的局部变量大小，即地图大小-->
  <arg name="map_size_x" value="40.0"/>
  <arg name="map_size_y" value="40.0"/>
  <arg name="map_size_z" value=" 3.0"/>

  <!-- topic of your odometry such as VIO or LIO -->
  <!-- 这里的odom_topic的value是由odom_world重映射得到，再将其value发布到/visual_slam/odom供给终端读取相应数值-->
  <arg name="odom_topic" value="/visual_slam/odom" />

  <!-- main algorithm params -->
  <!--嵌套复用advanced_param.xml，从而引入主要优化参数 -->
  <include file="$(find ego_planner)/launch/advanced_param.xml">

    <arg name="map_size_x_" value="$(arg map_size_x)"/>
    <arg name="map_size_y_" value="$(arg map_size_y)"/>
    <arg name="map_size_z_" value="$(arg map_size_z)"/>
    <arg name="odometry_topic" value="$(arg odom_topic)"/>

  
    <!-- camera pose: transform of camera frame in the world frame -->
    <!-- depth topic: depth image, 640x480 by default -->
    <!-- don't set cloud_topic if you already set these ones! -->
    <arg name="camera_pose_topic" value="/pcl_render_node/camera_pose"/>
    <arg name="depth_topic" value="/pcl_render_node/depth"/>

    <!-- topic of point cloud measurement, such as from LIDAR  -->
    <!-- don't set camera pose and depth, if you already set this one! -->
    <arg name="cloud_topic" value="/pcl_render_node/cloud"/>

    <!-- intrinsic params of the depth camera -->
    <arg name="cx" value="321.04638671875"/>
    <arg name="cy" value="243.44969177246094"/>
    <arg name="fx" value="387.229248046875"/>
    <arg name="fy" value="387.229248046875"/>

    <!-- maximum velocity and acceleration the drone will reach -->
    <arg name="max_vel" value="2.0" />
    <arg name="max_acc" value="3.0" />

    <!--always set to 1.5 times grater than sensing horizen-->
    <arg name="planning_horizon" value="7.5" /> 

    <!-- 1: use 2D Nav Goal to select goal  -->
    <!-- 2: use global waypoints below  -->
    <arg name="flight_type" value="2" />
    
    <!-- global waypoints -->
    <!-- It generates a piecewise min-snap traj passing all waypoints -->
    <arg name="point_num" value="5" />

    <arg name="point0_x" value="-15.0" />
    <arg name="point0_y" value="0.0" />
    <arg name="point0_z" value="1.0" />

    <arg name="point1_x" value="0.0" />
    <arg name="point1_y" value="15.0" />
    <arg name="point1_z" value="1.0" />

    <arg name="point2_x" value="15.0" />
    <arg name="point2_y" value="0.0" />
    <arg name="point2_z" value="1.0" />

    <arg name="point3_x" value="0.0" />
    <arg name="point3_y" value="-15.0" />
    <arg name="point3_z" value="1.0" />

    <arg name="point4_x" value="-15.0" />
    <arg name="point4_y" value="0.0" />
    <arg name="point4_z" value="1.0" /> 
<!-- trajectory server -->

 <!-- 节点所在的链接包ego_planner，节点对应的可执行文件名称traj_server，

 节点运行时对应的名称为traj_server，将节点的标准输出打印到终端屏幕screen -->

 <node pkg="ego_planner" name="traj_server" type="traj_server" output="screen">

  <remap from="/position_cmd" to="planning/pos_cmd"/>



  <remap from="/odom_world" to="$(arg odom_topic)"/>

  <param name="traj_server/time_forward" value="1.0" type="double"/>

 </node>

 <!-- 节点所在的链接包waypoint_generator，节点对应的可执行文件名称waypoint_generator，

 节点运行时对应的名称为waypoint_generator，将节点的标准输出打印到终端屏幕screen -->

 <node pkg="waypoint_generator" name="waypoint_generator" type="waypoint_generator" output="screen">

  <remap from="~odom" to="$(arg odom_topic)"/>     

  <remap from="~goal" to="/move_base_simple/goal"/>

  <remap from="~traj_start_trigger" to="/traj_start_trigger" />

  <param name="waypoint_type" value="manual-lonely-waypoint"/>   

 </node>

节点所在的链接包，节点对应的可执行文件名称是找到源码文件的关键。

我们可以通过搜索节点对应的可执行文件名称定位到源码文件名称(在CMakeList.txt中)

例如搜索可执行文件名称traj_server可以得到在CMakeList.txt的如下结果(节选)

# catkin_package(CATKIN_DEPENDS message_runtime)
catkin_package(
 INCLUDE_DIRS include
 LIBRARIES ego_planner
# 此处是源码文件(cpp)生成的可执行文件对应的链接库
 CATKIN_DEPENDS plan_env path_searching bspline_opt traj_utils message_runtime
#  DEPENDS system_lib
)

include_directories(
  include
  SYSTEM
  ${catkin_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/include
  ${EIGEN3_INCLUDE_DIR}
  ${PCL_INCLUDE_DIRS}
)


add_executable(ego_planner_node
  src/ego_planner_node.cpp 
  src/ego_replan_fsm.cpp
  src/planner_manager.cpp
  )
  #源码文件为：src/ego_planner_node.cpp
  #           src/ego_replan_fsm.cpp
  #           src/planner_manager.cpp     
  #可执行文件为 ego_planner_node
target_link_libraries(ego_planner_node 
  ${catkin_LIBRARIES}
  )
  #可执行文件为 ego_planner_node
  #对应的链接库catkin_LIBRARIES为catkin_package中的ego_planner，即可执行文件放置在工作空间的~/ego-planner/devel/lib/ego_planner下
add_dependencies(ego_planner_node ${${PROJECT_NAME}_EXPORTED_TARGETS})
  # 添加对本package消息的依赖
add_executable(traj_server src/traj_server.cpp)
  #源码文件为：src/traj_server.cpp    
  #可执行文件为 traj_server
target_link_libraries(traj_server ${catkin_LIBRARIES})

add_dependencies(traj_server ${${PROJECT_NAME}_EXPORTED_TARGETS})
  # 添加对本package消息的依赖

可以看到CMakeList.txt中的链接库ego_planner以及可执行文件名称traj_server都与simple_run.launch文件一一对应，再定位到代码行add_executable(traj_server src/traj_server.cpp)可以得到源码文件为src下的traj_server.cpp。

确定嵌套复用文件

嵌套服用文件中也存在需要运行的节点、各类需要用到的参数信息、或者直接是另外一个launch文件。simple_run.launch嵌套复用文件如下：

advanced_param.xml：包括主要的算法参数

<!-- main algorithm params -->
<include file="$(find ego_planner)/launch/advanced_param.xml">

  <arg name="map_size_x_" value="$(arg map_size_x)"/>
  <arg name="map_size_y_" value="$(arg map_size_y)"/>
  <arg name="map_size_z_" value="$(arg map_size_z)"/>
  <arg name="odometry_topic" value="$(arg odom_topic)"/>

  <!-- camera pose: transform of camera frame in the world frame -->
  <!-- depth topic: depth image, 640x480 by default -->
  <!-- don't set cloud_topic if you already set these ones! -->
  <arg name="camera_pose_topic" value="/pcl_render_node/camera_pose"/>
  <arg name="depth_topic" value="/pcl_render_node/depth"/>

  <!-- topic of point cloud measurement, such as from LIDAR  -->
  <!-- don't set camera pose and depth, if you already set this one! -->
  <arg name="cloud_topic" value="/pcl_render_node/cloud"/>

  <!-- intrinsic params of the depth camera -->
  <arg name="cx" value="321.04638671875"/>
  <arg name="cy" value="243.44969177246094"/>
  <arg name="fx" value="387.229248046875"/>
  <arg name="fy" value="387.229248046875"/>

  <!-- maximum velocity and acceleration the drone will reach -->
  <arg name="max_vel" value="2.0" />
  <arg name="max_acc" value="3.0" />

  <!--always set to 1.5 times grater than sensing horizen-->
  <arg name="planning_horizon" value="7.5" /> 

  <!-- 1: use 2D Nav Goal to select goal  -->
  <!-- 2: use global waypoints below  -->
  <arg name="flight_type" value="2" />
  
  <!-- global waypoints -->
  <!-- It generates a piecewise min-snap traj passing all waypoints -->
  <arg name="point_num" value="5" />

  <arg name="point0_x" value="-15.0" />
  <arg name="point0_y" value="0.0" />
  <arg name="point0_z" value="1.0" />

  <arg name="point1_x" value="0.0" />
  <arg name="point1_y" value="15.0" />
  <arg name="point1_z" value="1.0" />

  <arg name="point2_x" value="15.0" />
  <arg name="point2_y" value="0.0" />
  <arg name="point2_z" value="1.0" />

  <arg name="point3_x" value="0.0" />
  <arg name="point3_y" value="-15.0" />
  <arg name="point3_z" value="1.0" />

  <arg name="point4_x" value="-15.0" />
  <arg name="point4_y" value="0.0" />
  <arg name="point4_z" value="1.0" />
  
</include>

simulator.xml:包括仿真中需要用到的节点。

<!-- use simulator -->
<include file="$(find ego_planner)/launch/simulator.xml">
  <arg name="map_size_x_" value="$(arg map_size_x)"/>
  <arg name="map_size_y_" value="$(arg map_size_y)"/>
  <arg name="map_size_z_" value="$(arg map_size_z)"/>
  <arg name="c_num" value="200"/>
  <arg name="p_num" value="200"/>
  <arg name="min_dist" value="1.2"/>

  <arg name="odometry_topic" value="$(arg odom_topic)" />
</include>

rviz.launch:启动仿真环境rviz

<include file="$(find ego_planner)/launch/rviz.launch"/>

确定可运行文件名称

通过launch文件及其嵌套复用文件我们可以确定出可运行文件名称，从而找到我们需要重点阅读的源码程序cpp。

定义的全部节点为：

<node pkg="ego_planner" name="traj_server" type="traj_server" output="screen">
<node pkg="waypoint_generator" name="waypoint_generator" type="waypoint_generator" output="screen">
<node pkg="mockamap" type="mockamap_node" name="mockamap_node" output="screen">  
<node pkg="so3_quadrotor_simulator" type="quadrotor_simulator_so3" name="quadrotor_simulator_so3" output="screen">
<node pkg="nodelet" type="nodelet" args="standalone so3_control/SO3ControlNodelet" name="so3_control" required="true" output="screen">
<node pkg="odom_visualization" name="odom_visualization" type="odom_visualization" output="screen">
<node pkg="local_sensing_node" type="pcl_render_node" name="pcl_render_node" output="screen">     
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ego_planner)/launch/default.rviz" required="true" />

对应的可执行文件(依次执行)为

traj_server

waypoint_generator

mockamap_node

quadrotor_simulator_so3

standalone so3_control/SO3ControlNodelet

odom_visualization

pcl_render_node

/launch/default.rviz
文章作者: CaoDong street
文章链接: http://example.com/2021/10/26/ego-planner-dai-ma-jing-du-simple-run-launch-wen-jian/
版权声明: 本博客所有文章除特別声明外，均采用 CC BY 4.0 许可协议。转载请注明来源 CaoDong street !
C++ ROS
  目录