源码学习：amr-lmpcc 仿真分析

本文最后更新于：2020年7月6日 下午

项目地址：https://github.com/tud-amr/amr-lmpcc

1. Jackal Gazebo Simulation

1
roslaunch jackal_gazebo jackal_world.launch

1.1 jackal_world.launch

jackal_world.launch

在文件的开头是调试参数和环境配置

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
<arg name="use_sim_time" default="true" />
<arg name="gui" default="true" />
<arg name="headless" default="false" />
<arg name="world_name" default="$(find jackal_gazebo)/worlds/jackal_race.world" />

<arg name="front_laser" default="false" />
<arg name="default_config" value="front_laser" if="$(arg front_laser)" />
<arg name="default_config" value="base" unless="$(arg front_laser)" /> 

<arg name="config" default="$(arg default_config)" />

<include file="$(find gazebo_ros)/launch/empty_world.launch">
	<arg name="debug" value="0" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="use_sim_time" value="$(arg use_sim_time)" />
    <arg name="headless" value="$(arg headless)" />
    <arg name="world_name" value="$(arg world_name)" />
</include>

然后是Jackal仿真机器人的描述信息，控制器和运动控制节点的加载，分别对应：

  • description.launch
  • control.launch
  • teleop.launch
1
2
3
4
5
6
7
8
<!-- Load Jackal's description, controllers, and teleop nodes. -->
<include file="$(find jackal_description)/launch/description.launch">
	<arg name="config" value="$(arg config)" />
</include>
<include file="$(find jackal_control)/launch/control.launch" />
<include file="$(find jackal_control)/launch/teleop.launch">
	<arg name="joystick" value="false"/>
</include>

Jackal仿真机器人在 gazebo 中的位置和模型信息

1
2
3
<!-- Spawn Jackal -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
	args="-urdf -model jackal -param robot_description -x 0 -y 0 -z 1.0" />

最后是map到odom的tf变换

1
2
<!-- Static transform between odom and map frame to allow for planning in the map frame similar to the real robot -->
<node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 map odom 100" />

1.2 description.launch

description.launch

文件开头是一些配置信息和Jackal移动机器人的gazebo模型加载

1
2
3
4
5
6
7
8
9
10
11
<arg name="config" default="base" />
<!-- fix for oneweek project -->
<arg name="env_runner" value="$(eval 'env_run' if not optenv('OS', 'unknown').lower().startswith('windows') else 'env_run.bat')" />
<!-- the following seems to work too when in devel space, but not in install_isolated -->
<!-- <arg name="env_runner" value="env_run" /> -->

<param name="robot_description"
	command="$(find jackal_description)/scripts/$(arg env_runner)
				$(find jackal_description)/urdf/configs/$(arg config)
				$(find xacro)/xacro $(find jackal_description)/urdf/jackal.urdf.xacro
				--inorder" />

然后启动了robot_state_publisher节点

1
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

1.3 control.launch

control.launch

文件的开头是配置文件的加载

1
2
3
4
5
<rosparam command="load" file="$(find jackal_control)/config/control.yaml" />

<group if="$(optenv JACKAL_CONTROL_EXTRAS 0)" >
	<rosparam command="load" file="$(env JACKAL_CONTROL_EXTRAS_PATH)" />
</group>

然后启动控制器管理器controller_manager的spawner工具，自动的加载，启动，停止，卸载控制器jackal_joint_publisher和jackal_velocity_controller

1
<node name="controller_spawner" pkg="controller_manager" type="spawner" args="jackal_joint_publisher jackal_velocity_controller" />

移动机器人的定位节点ekf_localization_node

1
2
3
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization">
	<rosparam command="load" file="$(find jackal_control)/config/robot_localization.yaml" />
</node>

移动机器人的速度控制节点twist_mux，用于发布速度控制命令jackal_velocity_controller/cmd_vel

1
2
3
4
<node pkg="twist_mux" type="twist_mux" name="twist_mux">
	<rosparam command="load" file="$(find jackal_control)/config/twist_mux.yaml" />
    <remap from="cmd_vel_out" to="jackal_velocity_controller/cmd_vel"/>
</node>

1.4 teleop.launch

teleop.launch

该文件用于启动手柄控制的节点

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
<launch>
  <arg name="joy_dev" default="/dev/input/js0" />
  <arg name="joystick" default="true" />

  <group ns="bluetooth_teleop" if="$(arg joystick)">

    <group unless="$(optenv JACKAL_PS3 0)" >
      <rosparam command="load" file="$(find jackal_control)/config/teleop_ps4.yaml" />
    </group>

    <group if="$(optenv JACKAL_PS3 0)" >
      <rosparam command="load" file="$(find jackal_control)/config/teleop.yaml" />
      <param name="joy_node/dev" value="$(arg joy_dev)" />
    </group>

    <node pkg="joy" type="joy_node" name="joy_node" />

    <node pkg="teleop_twist_joy" type="teleop_node" name="teleop_twist_joy"/>
  </group>

  <node pkg="interactive_marker_twist_server" type="marker_server" name="twist_marker_server"/>
</launch>

1.5 小结

综上所述，jackal_world.launch文件除了调用仿真环境的信息，还分别启用了以下节点：

  • robot_state_publisher
  • ekf_localization_node：用于定位
  • twist_mux：用于发布速度控制命令cmd_vel
  • static_transform_publisher：用于map和odom之间的tf坐标变换

和用于手柄控制的节点。

2. lmpcc_obstacle_feed

1
2
3
4
# 仿真环境
roslaunch lmpcc_obstacle_feed lmpcc_obstacle_feed.launch
# 实际环境
roslaunch lmpcc_obstacle_feed map_lmpcc_obstacle_feed.launch

2.1 map_lmpcc_obstacle_feed.launch

map_lmpcc_obstacle_feed.launch

文件开头首先启动了map_server节点

1
2
<arg name="map_file" default="$(find lmpcc_obstacle_feed)/maps/faculty.yaml"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

然后是用于处理动态障碍物的lmpcc_obstacle_feed_node节点

1
2
<rosparam command="load" file="$(find lmpcc_obstacle_feed)/config/lmpcc_obstacle_feed_config.yaml"/>
<node pkg="lmpcc_obstacle_feed" type="lmpcc_obstacle_feed_node" name="lmpcc_obstacle_feed_node" cwd="node" respawn="false" output="screen"/>

还有一段注释掉的map到odom的tf变换

1
<!-- <node pkg="tf" type="static_transform_publisher" name="map_odom" args="1.5 2.7 0 0 0 0 map odom 100"/> -->

2.2 小结

仿真环境与实际环境的区别：在于仿真环境下没有启动map_server节点。

在实际环境中启动的节点有：

  • map_server
  • lmpcc_obstacle_feed_node

3. static_collision_avoidance

3.1 static_collision_avoidance.launch

static_collision_avoidance.launch

文件的开头是一些配置信息

1
2
3
4
5
6
7
8
9
10
11
 <!-- Debug Info -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<arg name="debug_config_parameter" default="false"/>
<arg name="debug_kinematic_cal" default="false"/>
<arg name="debug_collision_detection" default="false"/>
<arg name="config" default="$(find mobile_robot_state_publisher)"/>

<param name="collision_avoidance/local_map" value="true" type="bool" />
<param name="simulation_mode" value="false" type="bool" />

然后是用于在rviz中显示信息的节点costmap_2d_markers

1
2
3
4
<!-- Publishes the voxel grid to rviz for display -->
<node pkg="costmap_2d" type="costmap_2d_markers" name="voxel_visualizer">
	<remap from="voxel_grid" to="costmap/voxel_grid"/>
</node>

代价地图节点costmap_2d_node

1
2
3
<node name="costmap_node" pkg="costmap_2d" type="costmap_2d_node" >
	<rosparam file="$(find static_collision_avoidance)/config/local_map_params.yaml" command="load" ns="costmap" />
</node>

最后是用于处理静态障碍物的节点static_collision_avoidance_node

1
2
<rosparam file="$(find static_collision_avoidance)/config/static_avoidance_config.yaml" command="load"/>
<node pkg="static_collision_avoidance" type="static_collision_avoidance_node" name="static_collision_avoidance_node" cwd="node" respawn="false" output="screen"/>

3.2 小结

启动的节点有：

  • costmap_2d_markers：用于rviz可视化工具
  • costmap_2d_node
  • static_collision_avoidance_node

4. lmpcc

1
2
3
4
# 仿真环境
roslaunch lmpcc lmpcc.launch
# 实际环境
roslaunch lmpcc robot_lmpcc.launch

4.1 robot_lmpcc.launch

robot_lmpcc.launch

文件的开头是初始化信息的配置

1
2
3
4
5
6
7
8
9
<!-- Debug Info -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<arg name="debug_config_parameter" default="false"/>
<arg name="debug_kinematic_cal" default="false"/>
<arg name="debug_collision_detection" default="false"/>
<arg name="config" default="$(find mobile_robot_state_publisher)"/>
<arg name="scan_topic" value="velodyne_scan1" />

然后是注释掉的定位节点

1
2
3
4
5
<!--Launch localization node-->
<!--    <arg name="map_file" default="$(find jackal_navigation)/maps/cyberzoo.yaml"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
<include file="$(find jackal_navigation)/launch/include/amcl.launch" />
-->

mobile_robot_state_publisher_node节点

1
2
3
4
<!--Publish the mobile robot pose -->
<node name="mobile_robot_state_publisher" pkg="mobile_robot_state_publisher" type="mobile_robot_state_publisher_node" respawn="false" output="screen">
	<rosparam command="load" file="$(arg config)/config/jackal.yaml" />
</node>

最后是核心算法节点lmpcc

1
2
3
4
5
6
# load Cartesian controller config
<rosparam command="load" file="$(find lmpcc)/config/lmpcc_config_parameter.yaml"/>

<node pkg="lmpcc" type="lmpcc_node" name="lmpcc_node" cwd="node" respawn="false" output="screen"/>
<param name="collision_avoidance/local_map" value="true" type="bool" />
<param name="simulation_mode" value="false" type="bool" />

4.1 小结

启动的节点有：

  • mobile_robot_state_publisher_node
  • lmpcc_node

除此之外还有注释掉的

  • map_server
  • amcl.launch：用于定位

5. 配置文件中的frame_id分析

5.1 lmpcc_obstacle_feed

lmpcc_obstacle_feed_config.yaml

1
2
3
4
# Define the coordinate frames
frames:
    planning_frame: map             # For predefined obstacles
    robot_frame: odom          # For actual measured obstacles

5.2 static_collision_avoidance

local_map_params.yaml

  • 此文件中有footprint
  • 以及雷达的相关配置
1
2
global_frame: map
robot_base_frame: base_link

static_avoidance_config.yaml

1
2
3
4
# Coordinate frame definitions
frames:
    robot_base_link:  base_link
    planning_frame:   map

5.3 lmpcc

jackal.yaml

1
2
3
4
#robot base frame
base_frame: base_link
#map frame
root_frame: map

lmpcc_config_parameter.yaml

1
2
3
4
# Coordinate frame definitions
frames:
    robot_base_link:  base_link
    planning_frame:   map
编程
MPC ROS 源码学习

本博客所有文章除特别声明外，均采用 CC BY-SA 3.0协议 。转载请注明出处！