思岚激光雷达rplidar从ROS 1到ROS 2的移植 订阅rviz2的导航目标位置消息“/goal_pose” 为Navigation 2创建自定义behavior tree plugin 打断behavior tree的异步动作节点，并执行其他节点动作
相比于ROS Navigation stack，ROS 2 Navigation 2 stack引入了Behavior Tree作为机器人的逻辑流程控制，并对整个navigation stack做出了适应性的修改，同时更强调plugin的组合架构。ROS 2 Navigation 2 stack的组织框架为下图：
ROS 2 Navigation 2相比于ROS Navigation的变化如下，详细内容参看官网链接： https://navigation.ros.org/about/ros1_comparison.html#ros1-comparison
移植的packages： amcl: 移植为 nav2_amcl map_server: 移植为 nav2_map_server nav2_planner: 用 N planner plugins，即插件代替global_planner nav2_controller: 用 N controller plugins，即插件代替local_planner Navfn: 移植为 nav2_navfn_planner DWB: 用 nav2_dwb_controller 代替 DWA nav_core: 移植为 nav2_core costmap_2d: 移植为 nav2_costmap_2d
新增的packages： nav2_bt_navigator: 代替 move_base 的状态机机制 nav2_lifecycle_manager: 管理服务程序的生命周期 nav2_waypoint_follower: 用于多点导航功能 nav2_system_tests: 教程相关的测试案例 nav2_rviz_plugins: rviz中于navigation相关控制的插件 nav2_experimental: 试验代码（未正式发布），包括了深度学习相关的控制器 navigation2_behavior_trees: ROS action servers使用Behavior tree的接口
ubuntu 20.04 ROS foxy 苇航智能NaviBOT A0 package
在终端输入命令：
sudo apt install ros-foxy-navigation2 sudo apt install ros-foxy-nav2-bringup sudo apt install ~nros-foxy-turtlebot3* sudo apt install ~nros-foxy-gazebo-*NOTE：如果ubuntu及ROS版本很新，会出现turtlebot3的相应deb还未发布，可以使用下载源码编译安装的方式，参考官网链接： https://emanual.robotis.com/docs/en/platform/turtlebot3/ros2_setup/
turtlebot3是关于navigation最好的示例，通过查看turtlebot3的navigation运行过程，以此指导navigation 2 stack的构建，以及自主开发导航机器人驱动package
turtlebot3的navigation simulation需要gazebo的配合，gazebo的初次使用需要加入GAZEBO_MODEL_PATH，打开新终端，输入命令：
echo 'export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:~/turtlebot3_ws/src/turtlebot3/turtlebot3_simulations/turtlebot3_gazebo/models' >> ~/.bashrc source ~/.bashrc导出turtlebot3的型号，苇航智能使用“burger”，在终端继续输入命令：
export TURTLEBOT3_MODEL=burger运行gazebo加载导航的环境，包括地图以及机器人信息，在终端继续输入命令：
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.pyNOTE：第一次运行时，可能会需要下载环境模型，但国内由于墙的原因下载会失败，如果有条件可以使用proxy，比如proxychains，如下图： 运行后如下图： 然后运行导航，打开一个新终端，再次导出turtlebot3的型号，输入命令：
export TURTLEBOT3_MODEL=burger ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=True map:=/home/whi/turtlebot3_ws/install/turtlebot3_navigation2/share/turtlebot3_navigation2/map/map.yaml运行后将启动rviz，并显示上述命令所指定的地图，如下图： 此时，导航处于未开启状态，需要通过给定initial position来激活导航，点击“2D Pose Estimate”在地图上选择一个initial position，此时，导航将开启，如下图：
TIP：关于turtlebot3 simulation的更多使用，可以参考官网链接： https://emanual.robotis.com/docs/en/platform/turtlebot3/ros2_simulation/#ros-2-simulation
msgs： 在终端输入命令：
ros2 run rqt_graph rqt_graph打开后同时勾选“tf”，如下图：
tf frames： 在终端输入命令：
ros2 run tf2_tools view_frames.py运行完毕提示“Generating graph…”，并在所运行命令的目录生成“frames.pdf”文件，如下图：
上述过程生成的是导航模式下turtlebot3的tf tree，可以看到tr tree完整的结构为“map->odom->base_footprint->base_link->xxx”，如下图：
由此可以推断：在非导航模式，即只运行turtlebot3，则应该生成的tf tree为“odom->base_footprint->base_link->xxx”，该tf tree也是自主开发机器人驱动时需要满足的tf tree结构
和ROS 1一样，在ROS 2中也有robot_state_publisher，该node的作用是发布机器人的描述，包括关节信息、关节间的tf，同时订阅joint_state以接收动态关节的信息，关于robot_state_publisher的详细信息可以参考官网链接： http://wiki.ros.org/robot_state_publisher https://index.ros.org/r/robot_state_publisher/
turtlebot3 bringup中有“turtlebot3_state_publisher.launch.py”launch文件，用于运行robot_state_publisher。可以运行该文件查看turtlebot3的机器人信息，tf tree的关系
打开一个新终端，输入命令：
export TURTLEBOT3_MODEL=burger ros2 launch turtlebot3_bringup turtlebot3_state_publisher.launch.py运行后将显示发布机器人信息，如下图：
使用rviz2来可视化机器人的信息
打开一个新终端，输入命令：
ros2 run rviz2 rviz2在运行的rviz2中，加入“RobotModel”，如下图：
然后在“RobotModel”中展开“Description Topic”，并选择“/robot_description”，如下图：
此时将显示turtlebot3的model，但为白色没有任何渲染，如下图：
这是因为“Fixed Frame”当前为“map”的原因，因为目前机器人为静态状态，所以没有map->odom->base_footprint->base_link->xxx的tf tree，所以需要调整fixed frame，比如选择为base_footprint，此时将完整显示turtlebot3模型的完整渲染，如下图：
通过选择不同的Fixed Frame，能够帮助查看各个静态frame或者link之间的static tf关系，如下图中为分别选择了base_link和base_scan后的显示：
该关系通过机器人的urdf模型文件设置，即该文件中指定parent和child的片段，如下图：
该设置表明从base_link到base_scan需要在x方向移动-0.032m，在z方向移动0.172m，这个静态transfer关系还能通过查看消息/tf_static确认
打开一个新终端，输入命令：
ros2 topic list可以看到，当前robot_state_publiser发布了如下消息，其中就包括静态link之间的tf_static消息，如下图：
在终端继续输入命令：
ros2 topic echo /tf_static查看消息内容，对应base_link和base_scan部分的translation数据即为上述urdf中的设置数据，如下图：
无论是在ROS 1还是ROS 2，turtlebot3的navigation都是一个独立的package存在，这是因为在ROS架构下的message的发布和订阅机制，得益于该机制，导航过程可以是完全运行于机器人本身，也可以运行于远程的机器，特别是当机器人本身处理能力有局限的时候。该机制在ROS 1和ROS 2的区别就是ROS 1使用了TCP协议，而在ROS 2下，因为引入了DDS中间层，协议变为了UDP
下面例子为raspberry PI（IP：192.168.10.116）接收来自PC（IP：192.168.10.111）发布的tf_static消息，如下图：
充分利用ROS分布式架构带来的优势，对navigation单独创建一个package，这样既能满足在机器人本地运行导航，也能通过远程的方式运行导航
首先，创建package。在终端输入命令：
cd ~/dev_ws/src ros2 pkg create --build-type ament_cmake motion_ctrl_nav创建package后，修改其package.xml中的版本、维护者、版权等信息
其次，建立rviz目录，用于存储运行rviz导航时的configuration文件；建立launch目录，用于存储运行导航时的launch文件；建立param目录，用于存储导航控制的配置参数如下图：
最后，编辑motion_ctrl_nav的CMakeLists文件，加入rivz、launch、param的安装目录，便于后续launch的调用。打开其CMakeLists.txt增加如下语句：
install(DIRECTORY launch param rviz DESTINATION share/${PROJECT_NAME} )如下图：
首先，创建param_NaviBOT_A0.yaml的空文件，用于存储导航控制参数，如下图：
NOTE：从复用性考虑，这里文件加入了苇航智能机器人硬件相关的型号
然后，复制如下内容至该文件：
amcl: ros__parameters: use_sim_time: False alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 alpha4: 0.2 alpha5: 0.2 base_frame_id: "base_footprint" beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 do_beamskip: false global_frame_id: "map" lambda_short: 0.1 laser_likelihood_max_dist: 2.0 laser_max_range: 100.0 laser_min_range: -1.0 laser_model_type: "likelihood_field" max_beams: 60 max_particles: 2000 min_particles: 500 odom_frame_id: "odom" pf_err: 0.05 pf_z: 0.99 recovery_alpha_fast: 0.0 recovery_alpha_slow: 0.0 resample_interval: 1 robot_model_type: "differential" save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: true transform_tolerance: 1.0 update_min_a: 0.2 update_min_d: 0.25 z_hit: 0.5 z_max: 0.05 z_rand: 0.5 z_short: 0.05 amcl_map_client: ros__parameters: use_sim_time: False amcl_rclcpp_node: ros__parameters: use_sim_time: False bt_navigator: ros__parameters: use_sim_time: False global_frame: map robot_base_frame: base_link odom_topic: /odom default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node - nav2_follow_path_action_bt_node - nav2_back_up_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node - nav2_clear_costmap_service_bt_node - nav2_is_stuck_condition_bt_node - nav2_goal_reached_condition_bt_node - nav2_goal_updated_condition_bt_node - nav2_initial_pose_received_condition_bt_node - nav2_reinitialize_global_localization_service_bt_node - nav2_rate_controller_bt_node - nav2_distance_controller_bt_node - nav2_speed_controller_bt_node - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_round_robin_node_bt_node - nav2_transform_available_condition_bt_node - nav2_time_expired_condition_bt_node - nav2_distance_traveled_condition_bt_node bt_navigator_rclcpp_node: ros__parameters: use_sim_time: False controller_server: ros__parameters: use_sim_time: False controller_frequency: 10.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 controller_plugins: ["FollowPath"] # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: True min_vel_x: 0.0 min_vel_y: 0.0 max_vel_x: 0.22 max_vel_y: 0.0 max_vel_theta: 1.0 min_speed_xy: 0.0 max_speed_xy: 0.22 min_speed_theta: 0.0 # Add high threshold velocity for turtlebot 3 issue. # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 acc_lim_x: 2.5 acc_lim_y: 0.0 acc_lim_theta: 3.2 decel_lim_x: -2.5 decel_lim_y: 0.0 decel_lim_theta: -3.2 vx_samples: 20 vy_samples: 5 vtheta_samples: 20 sim_time: 1.7 linear_granularity: 0.05 angular_granularity: 0.025 transform_tolerance: 0.2 xy_goal_tolerance: 0.25 trans_stopped_velocity: 0.25 short_circuit_trajectory_evaluation: True stateful: True critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] BaseObstacle.scale: 0.02 PathAlign.scale: 32.0 PathAlign.forward_point_distance: 0.1 GoalAlign.scale: 24.0 GoalAlign.forward_point_distance: 0.1 PathDist.scale: 32.0 GoalDist.scale: 24.0 RotateToGoal.scale: 32.0 RotateToGoal.slowing_factor: 5.0 RotateToGoal.lookahead_time: -1.0 controller_server_rclcpp_node: ros__parameters: use_sim_time: False local_costmap: local_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link use_sim_time: False rolling_window: true width: 3 height: 3 resolution: 0.05 robot_radius: 0.105 plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True observation_sources: scan scan: topic: /scan max_obstacle_height: 2.0 clearing: True marking: True data_type: "LaserScan" voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True publish_voxel_map: True origin_z: 0.0 z_resolution: 0.05 z_voxels: 16 max_obstacle_height: 2.0 mark_threshold: 0 observation_sources: pointcloud pointcloud: topic: /intel_realsense_r200_depth/points max_obstacle_height: 2.0 clearing: True marking: True data_type: "PointCloud2" static_layer: map_subscribe_transient_local: True always_send_full_costmap: True local_costmap_client: ros__parameters: use_sim_time: False local_costmap_rclcpp_node: ros__parameters: use_sim_time: False global_costmap: global_costmap: ros__parameters: update_frequency: 1.0 publish_frequency: 1.0 global_frame: map robot_base_frame: base_link use_sim_time: False robot_radius: 0.105 resolution: 0.05 plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True observation_sources: scan scan: topic: /scan max_obstacle_height: 2.0 clearing: True marking: True data_type: "LaserScan" voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True publish_voxel_map: True origin_z: 0.0 z_resolution: 0.05 z_voxels: 16 max_obstacle_height: 2.0 mark_threshold: 0 observation_sources: pointcloud pointcloud: topic: /intel_realsense_r200_depth/points max_obstacle_height: 2.0 clearing: True marking: True data_type: "PointCloud2" static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" always_send_full_costmap: True global_costmap_client: ros__parameters: use_sim_time: False global_costmap_rclcpp_node: ros__parameters: use_sim_time: False map_server: ros__parameters: use_sim_time: False yaml_filename: "turtlebot3_world.yaml" map_saver: ros__parameters: use_sim_time: False save_map_timeout: 5000 free_thresh_default: 0.25 occupied_thresh_default: 0.65 planner_server: ros__parameters: expected_planner_frequency: 20.0 use_sim_time: False planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" tolerance: 0.5 use_astar: false allow_unknown: true planner_server_rclcpp_node: ros__parameters: use_sim_time: False recoveries_server: ros__parameters: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 recovery_plugins: ["spin", "backup", "wait"] spin: plugin: "nav2_recoveries/Spin" backup: plugin: "nav2_recoveries/BackUp" wait: plugin: "nav2_recoveries/Wait" global_frame: odom robot_base_frame: base_link transform_timeout: 0.1 use_sim_time: False simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 robot_state_publisher: ros__parameters: use_sim_time: False仔细观察上述文件，并对比ROS 1中navigation的构建，会发现ROS 2把导航参数都集中到一个文件管理，更加直观，维护也更方便
首先，创建“navigation_local_NaviBOT_A0.launch.py”和“navigation_remote_NaviBOT_A0.launch.py”两个空文件，用于存储导航控制参数。其中第一个用于本地导航运行，第二个用于远程导航运行，如下图：
NOTE：从复用性考虑，这里文件加入了苇航机器人硬件相关的型号；同时建立的为运行在机器人本地的导航方式，添加了local属性
然后，复制如下内容至文件“navigation_local_NaviBOT_A0.launch.py”：
# Copyright 2019 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # # Author: Xinjue Zou import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time', default='false') map_dir = LaunchConfiguration( 'map', default='/home/whi/dev_ws/map.yaml') params_dir = '/home/whi/dev_ws/src/motion_ctrl_nav/param/param_NaviBOT_A0.yaml' nav2_launch_file_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch') rviz_config_dir = os.path.join( get_package_share_directory('motion_ctrl_nav'), 'rviz', 'nav_view.rviz') motion_ctrl_pkg_dir = LaunchConfiguration('motion_ctrl_pkg_dir', default=os.path.join(get_package_share_directory('motion_ctrl'), 'launch')) return LaunchDescription([ DeclareLaunchArgument( 'map', default_value='/home/whi/dev_ws/map.yaml', description='Full path to map file to load'), DeclareLaunchArgument( 'params_file', default_value=params_dir, description='Full path to param file to load'), DeclareLaunchArgument( 'use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true'), IncludeLaunchDescription( PythonLaunchDescriptionSource([motion_ctrl_pkg_dir, '/NaviBOT_A0.launch.py']) ), IncludeLaunchDescription( PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch.py']), launch_arguments={ 'map': map_dir, 'use_sim_time': use_sim_time, 'params_file': params_dir}.items(), ), Node( package='rviz2', executable='rviz2', name='rviz2', arguments=['-d', rviz_config_dir], parameters=[{'use_sim_time': use_sim_time}], output='screen'), ])NOTE：上述内容中参数目录params_dir直接使用了motion_ctrl_nav package中param目录的绝对地址，这是为了方便导航参数的调整，也可以使用“get_package_share_directory”，但这样就需要每次修改导航参数后，再次编译motion_ctrl_nav package
然后，复制如下内容至文件“navigation_remote_NaviBOT_A0.launch.py”：
# Copyright 2019 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # # Author: Xinjue Zou import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time', default='false') map_dir = LaunchConfiguration( 'map', default='/home/whi/dev_ws/map.yaml') params_dir = '/home/whi/dev_ws/src/motion_ctrl_nav/param/param_NaviBOT_A0.yaml' nav2_launch_file_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch') rviz_config_dir = os.path.join( get_package_share_directory('motion_ctrl_nav'), 'rviz', 'nav_view.rviz') return LaunchDescription([ DeclareLaunchArgument( 'map', default_value='/home/whi/dev_ws/map.yaml', description='Full path to map file to load'), DeclareLaunchArgument( 'params_file', default_value=params_dir, description='Full path to param file to load'), DeclareLaunchArgument( 'use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true'), IncludeLaunchDescription( PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch.py']), launch_arguments={ 'map': map_dir, 'use_sim_time': use_sim_time, 'params_file': params_dir}.items(), ), Node( package='rviz2', executable='rviz2', name='rviz2', arguments=['-d', rviz_config_dir], parameters=[{'use_sim_time': use_sim_time}], output='screen'), ])NOTE：remote的launch文件和local的相比，少了嵌套调用NaviBOT_A0.launch.py的过程
NOTE：上述两个文件都有和苇航智能机器人驱动相关的代码，请根据实际情况做适当的修改
在终端输入命令：
colcon build --packages-select motion_ctrl_nav编译motion_ctrl_nav，编译完成后将在workspace的install目录下生成motion_ctrl_nav目录以及其下share\launch以及share\param，如下图：
苇航智能的机器人驱动没有用代码的方式实现base_footprint到base_link之间的translation,通过ros2 run命令直接运行苇航智能的机器人驱动后的tr tree如下图，可以看到只有odom到base_footprint的tf tree：
苇航智能采用了static transform的方式实现base_footprint与base_link之间的关联。和ROS 1一样，在ROS 2中static transform可以在launch文件中实现，比如思岚激光雷达做逆时针旋转180度的静态translation：
Node( package='tf2_ros', node_executable='static_transform_publisher', node_name='base_to_laser', arguments=['0', '0', '0.125', ‘3.14’, '0', '0', 'base_link', 'laser_frame], output='screen'),但该方式不直观，不便于维护时的理解，以及模块的调整，因此，苇航智能使用了robot_state_publisher发布机器人信息的方式实现base_footprint到base_link之间的关联。robot_state_publisher的详细信息可以参考官网链接： http://wiki.ros.org/robot_state_publisher https://index.ros.org/r/robot_state_publisher/
苇航智能规定base_footprint坐标基准为地面、base_link坐标基准为驱动轮的轴心，详细坐标关系如下图：
在苇航智能机器人驱动motion_ctrl package的根目录下，建立“urdf”文件夹，同时建立空白文件“NaviBOT_A0.urdf”，如下图：
复制如下内容至该文件：
<?xml version="1.0" ?> <robot name="NaviBOT A0" xmlns:xacro="http://ros.org/wiki/xacro"> <!-- <xacro:include filename="$(find turtlebot3_description)/urdf/common_properties.urdf"/> --> <!-- Init colour --> <material name="black"> <color rgba="0.0 0.0 0.0 1.0"/> </material> <material name="dark"> <color rgba="0.3 0.3 0.3 1.0"/> </material> <material name="light_black"> <color rgba="0.4 0.4 0.4 1.0"/> </material> <material name="blue"> <color rgba="0.0 0.0 0.8 1.0"/> </material> <material name="green"> <color rgba="0.0 0.8 0.0 1.0"/> </material> <material name="grey"> <color rgba="0.5 0.5 0.5 1.0"/> </material> <material name="orange"> <color rgba="1.0 0.4235 0.0392 1.0"/> </material> <material name="brown"> <color rgba="0.8706 0.8118 0.7647 1.0"/> </material> <material name="red"> <color rgba="0.8 0.0 0.0 1.0"/> </material> <material name="white"> <color rgba="1.0 1.0 1.0 1.0"/> </material> <link name="base_footprint"/> <joint name="base_joint" type="fixed"> <parent link="base_footprint"/> <child link="base_link"/> <origin xyz="0.0 0.0 0.0325" rpy="0 0 0"/> </joint> <link name="base_link"> <visual> <origin xyz="-0.0365 0 0.0525" rpy="0 0 0"/> <geometry> <box size="0.205 0.190 0.065"/> </geometry> <material name="grey"/> </visual> <collision> <origin xyz="-0.0365 0.0 0.0525" rpy="0 0 0"/> <geometry> <box size="0.205 0.190 0.065"/> </geometry> </collision> <inertial> <origin xyz="0 0 0" rpy="0 0 0"/> <mass value="8.2573504e-01"/> <inertia ixx="2.2124416e-03" ixy="-1.2294101e-05" ixz="3.4938785e-05" iyy="2.1193702e-03" iyz="-5.0120904e-06" izz="2.0064271e-03" /> </inertial> </link> <joint name="wheel_left_joint" type="continuous"> <parent link="base_link"/> <child link="wheel_left_link"/> <origin xyz="0.0 0.0865 0.0" rpy="1.57 0.0 0.0"/> <axis xyz="0 0 1"/> </joint> <link name="wheel_left_link"> <visual> <origin xyz="0.0 0.0865 0.0" rpy="1.57 0.0 0.0"/> <geometry> <cylinder length="0.018" radius="0.033"/> </geometry> <material name="dark"/> </visual> <collision> <origin xyz="0.0 0.0865 0.0" rpy="1.57 0.0 0.0"/> <geometry> <cylinder length="0.018" radius="0.033"/> </geometry> </collision> <inertial> <origin xyz="0 0 0" /> <mass value="2.8498940e-02" /> <inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09" iyy="1.1192413e-05" iyz="-1.4400107e-11" izz="2.0712558e-05" /> </inertial> </link> <joint name="wheel_right_joint" type="continuous"> <parent link="base_link"/> <child link="wheel_right_link"/> <origin xyz="0.0 -0.0865 0.0" rpy="-1.57 0.0 0.0"/> <axis xyz="0 0 1"/> </joint> <link name="wheel_right_link"> <visual> <origin xyz="0.0 -0.0865 0.0" rpy="-1.57 0.0 0.0"/> <geometry> <cylinder length="0.018" radius="0.033"/> </geometry> <material name="dark"/> </visual> <collision> <origin xyz="0.0 -0.0865 0.0" rpy="-1.57 0.0 0.0"/> <geometry> <cylinder length="0.018" radius="0.033"/> </geometry> </collision> <inertial> <origin xyz="0 0 0" /> <mass value="2.8498940e-02" /> <inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09" iyy="1.1192413e-05" iyz="-1.4400107e-11" izz="2.0712558e-05" /> </inertial> </link> <joint name="imu_joint" type="fixed"> <parent link="base_link"/> <child link="imu_link"/> <origin xyz="0.0 0 0.085" rpy="0 0 0"/> </joint> <link name="imu_link"/> <joint name="scan_joint" type="fixed"> <parent link="base_link"/> <child link="base_scan"/> <origin xyz="0.0 0.0 0.124" rpy="0.0 0.0 3.14"/> </joint> <link name="base_scan"> <visual> <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/> <geometry> <cylinder length="0.0245" radius="0.035"/> </geometry> <material name="red"/> </visual> <collision> <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/> <geometry> <cylinder length="0.0245" radius="0.035"/> </geometry> </collision> <inertial> <mass value="0.114" /> <origin xyz="0 0 0" /> <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" /> </inertial> </link> </robot>机器人信息文件urdf建立后，需要建立一个launch来运行robot_state_publisher，也为后续导航包调用提供嵌套的launch。在motion_ctrl package的launch目录下建立空白文件“NaviBOT_A0_state_publisher.launch.py”如下图：
复制如下内容至该文件：
#!/usr/bin/env python3 # # Copyright 2020 Wheel Hub Intelligent CO., LTD. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # # Authors: Xinjue Zou import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time', default='false') urdf_file_name = 'NaviBOT_A0.urdf' print("urdf_file_name : {}".format(urdf_file_name)) urdf = LaunchConfiguration('urdf', default=os.path.join(get_package_share_directory('motion_ctrl'), 'urdf', urdf_file_name)) return LaunchDescription([ DeclareLaunchArgument( 'use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true'), Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='screen', parameters=[{'use_sim_time': use_sim_time}], arguments=[urdf]), ])编译package motion_ctrl，编译完成后将在workspace的“install/<package>/share/”目录下生成urdf目录及文件NaviBOT_A0.urdf
NOTE：motion_ctrl的CMakeLists文件需要添加对应的install内容，才能在install//share目录下生成上述的urdf及其文件，可以参照章节“创建navigation package”中install的内容
编译后，通过运行launch文件“NaviBOT_A0_state_publisher.launch.py”，并使用rviz来可视化机器人模型
在终端输入命令：
ros2 launch motion_ctrl NaviBOT_A0_state_publisher.launch.py运行机器人的state publisher，发布模型信息，以及static transform。打开新终端，输入命令：
ros2 run rviz2 rviz2运行rviz，启动后添加“RobotModel”，并选择“Description Topic”为“/robot_description”，“Fixed Frame”为“base_link”，将显示机器人的模型，如下图：
TIP：可以尝试改变Fixed Frame查看各个frames之间的基准关系
最后，使用robot_state_publisher最终目的为获得base_footprint到base_link的tf tree，因此，在新终端中，输入命令：
ros2 topic list查看是否有/tf_static主题的消息，如下图：
再通过命令：
ros2 topic echo /tf_static查看该消息发布的，在urdf中各个frame间的translation，可以看到base_footprint到base_link之间的translation，如下图：
NOTE： 本章节内容提到了苇航智能的机器人驱动package，但超出本说明范围，不展开说明
前面提到navigation需要有完整结构为“map->odom->base_footprint->base_link->xxx”的tf tree，在开始运行导航之前，首先检查是否满足该要求
在终端中输入命令：
ros2 launch motion_ctrl NaviBOT_A0.launch.py以苇航智能机器人驱动的launch文件为例，打开新终端，输入命令：
ros2 run tf2_tools view_frames.py等待运行完毕后，将在运行命令的目录下生成名为“frames.pdf”文件，查看是否有完备tr tree，如下图：
NOTE：上述命令“ros2 launch motion_ctrl NaviBOT_A0.launch.py”中的py文件为苇航智能机器人驱动package的launch文件
完成上述步骤后，navigation已经构建完毕，可以尝试运行，查看是否遗留其他问题
打开新终端，输入命令：
sudo su . install/setup.bash ros2 launch motion_ctrl_nav navigation_NaviBOT_A0.launch.py map:=/home/whi/dev_ws/whiMap_h.yamlNOTE：因为苇航智能的机器人驱动package需要root权限，因此进入了su用户，同时需要给定导航地图的绝对路径
一切顺利的话将开启rivz，如下图：
因为这里使用了navigation2默认的rviz配置，所以同样需要首先指定initial position来激活导航，点击“2D Pose Estimate”给定机器人initial position后将激活导航，如下图：
得益于ROS的消息机制，ROS 1和ROS 2都能远程运行导航，而让机器人本身只运行运动驱动、lidar雷达、以及发送必要的odom和tf tree，这样可以借助远程高性能的机器提高导航的运算频率
打开新终端，输入命令：
cd ~/dev_ws sudo su . install/setup.bash ros2 launch motion_ctrl NaviBOT_A0.launch.py运行机器人驱动、lidar、发布odom和tf tree，如下图：
首先，需要将导航需要的地图拷贝到PC的workspace下，比如dev_ws，同时将motion_ctrl_nav package拷贝到PC workspace的src目录下，分别如下图：
然后，打开新终端，输入命令：
cd ~/dev_ws colcon build编译motion_ctrl_nav package，编译完成后，继续在终端输入命令：
ros2 launch motion_ctrl_nav navigation_remote_NaviBOT_A0.launch.py map:=/home/whi/dev_ws/whiMap_h.yaml远程PC将打开rviz并显示导航地图，如下图：
和运行local一样，给定机器人的initial position以激活导航，指定导航目标点，机器人将被远程PC导航控制的开始运行，其中机器人IP：192.168.10.116，远程PC IP：192.168.10.111，如下图：
至此，ROS 2的navigation 2 stack已经构建完毕，本说明着重关注navigation stack本身，其中涉及到部分苇航智能机器人驱动package的内容，但不影响navigation构建的过程和思路
如果本说明有错误和不明确的地方，欢迎指正，共同交流，xinjue.zou.whi@gmail.com
