PX4与仿真入门教程-dronedoc-基于GPS的自主飞行

PX4与仿真入门教程-dronedoc-基于GPS的自主飞行

说明：

• 介绍使用基于GPS的自主飞行
• 完整代码查看https://github.com/ncnynl/dronedoc

步骤：

• 首先安装相关导航需要的包

sudo apt install ros-melodic-amcl \
                 ros-melodic-base-local-planner \
                 ros-melodic-carrot-planner \
                 ros-melodic-clear-costmap-recovery \
                 ros-melodic-costmap-2d \
                 ros-melodic-dwa-local-planner \
                 ros-melodic-fake-localization \
                 ros-melodic-global-planner \
                 ros-melodic-map-server \
                 ros-melodic-move-base \
                 ros-melodic-move-base-msgs \
                 ros-melodic-move-slow-and-clear \
                 ros-melodic-nav-core \
                 ros-melodic-navfn \
                 ros-melodic-rotate-recovery \
                 ros-melodic-voxel-grid

• 导航的节点图

建立雷达和baselink的关系

• 雷达配置在models/iris_2d_lidar/model.sdf文件下，发布/laser/scan话题

<node pkg="tf" name="base2lidar" type="static_transform_publisher" args="0 0 0.1 0 0 0 base_link lidar_link 100"/>

• 我们需要新增如上的语句到mymodel_sitl_tf.launch 的末尾

• move_base 使用两个成本图，全局和局部，用于避障和路线规划。

• 在这个设置中，local cost map是指odom frame，global cost map是指map frame

• 所以我们需要从每一帧定义一个TF到base_link。

• odom框架常用作机器人的局部框架，map框架常用作全局框架。

• 同样，该约定是在 odom 框架中广播本地框架的 TF，在地图框架中广播全局框架的 TF。

• 关系图如下：

建立odom-> base_link的关系

• 可以使用 mavros 发布飞机的 TF。 但是，在使用 mavros_posix_sitl.launch 时，默认情况下不会发布 TF。
• /mavros/local_position/tf/send 参数的值必须为真才能发布 TF。
• 另外，作为以后使用move_base的设置，将/mavros/local_position/tf/frame_id和/mavros/local_position/tf/frame_id的参数值设置为odom。
• 将以下内容添加到 mymodel_sitl_tf.launch 的末尾
• 我们需要添加如下语句

<param name="/mavros/local_position/tf/send" type="bool" value="true" />
<param name="/mavros/local_position/frame_id" type="str" value="odom" />
<param name="/mavros/local_position/tf/frame_id" type="str" value="odom" />

建立map->odom的关系

• odom框架的TF是用车轮里程计等计算出来的，地图框架的TF是用GPS和LiDAR等传感器信息更新的，但是这次都用了GPS，由于基点是一样的，我们将按如下方式将 TF 从 map 广播到 odom。
• 将以下内容添加到 mymodel_sitl_tf.launch。

<node pkg="tf" name="map2odom" type="static_transform_publisher" args="0 0 0 0 0 0 map odom 100"/>

• 从map到lidar_link的TF
• 如下图所示。

• 从map到base_link的TF已经发布了，
• 所以这次我们只需要写nav_msgs::Odometry消息的发布者即可。
• python实现发布odom的odom_publisher.py

cd ~/dronedoc_ws/src/px4_sim_pkg/script/
vim odom_publisher.py

• 代码如下：

#!/usr/bin/env python

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped, TwistStamped, Quaternion
import tf

# Callback for local position
local_pos = PoseStamped()
def local_pos_cb(msg):
    # Need global declaration in order to assign global variable
    global local_pos
    local_pos = msg

# Callback for local velocity
local_vel = TwistStamped()
def local_vel_cb(msg):
    # Need global declaration in order to assign global variable
    global local_vel
    local_vel = msg

def odom_publisher():
    rospy.init_node("odom_publisher")

    odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)

    local_pos_sub = rospy.Subscriber("mavros/local_position/pose",
                                      PoseStamped,
                                      callback=local_pos_cb)
    local_vel_sub = rospy.Subscriber("mavros/local_position/velocity",
                                      TwistStamped,
                                      callback=local_vel_cb)

    rate = rospy.Rate(20)

    while not rospy.is_shutdown():

        pose = local_pos.pose
        twist = local_vel.twist

        # Create quaternion from euler angle of local position
        odom_quat = tf.transformations.quaternion_from_euler(pose.orientation.x,
                                                             pose.orientation.y,
                                                             pose.orientation.z)

        odom = Odometry()
        odom.header.stamp = rospy.Time.now()
        # Global frame is "odom"
        odom.header.frame_id = "odom"

        odom.pose.pose.position.x = pose.position.x
        odom.pose.pose.position.y = pose.position.y
        odom.pose.pose.position.z = 0
        odom.pose.pose.orientation = odom_quat

        # Set robot's base frame as odometry child frame
        odom.child_frame_id = "base_link"
        odom.twist.twist.linear.x = twist.linear.x
        odom.twist.twist.linear.y = twist.linear.y
        odom.twist.twist.angular.z = twist.angular.z

        odom_pub.publish(odom)
        rate.sleep()

# Only run if executed as top level script
if __name__ == "__main__":
    try:
        odom_publisher()
    except rospy.ROSInterruptException:
        pass

• 给python脚本要增加权限

chmod +x odom_publisher.py

• 使用c++实现发布odom的odom_publisher.cpp

cd ~/dronedoc_ws/src/px4_sim_pkg/src/
vim odom_publisher.cpp

• 代码如下：

/**
 * @file odom_publisher.cpp
 * @brief Node for publishing odom topic and TF to base_footprint from odom
 * @author Takaki Ueno
 */

// roscpp
#include <ros/ros.h>

// nav_msgs
#include <nav_msgs/Odometry.h>

// geomtery_msgs
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Quaternion.h>

// tf
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>

//! Storage for local position
geometry_msgs::PoseStamped local_pos;

/**
 * @brief Callback function for local position
 * @param msg Incaming msg
 */
void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
  local_pos = *msg;
}

//! Storage for local velocity
geometry_msgs::TwistStamped local_vel;

/**
 * @brief Callback for local velocity
 * @param msg Incoming message
 */

void local_vel_cb(const geometry_msgs::TwistStamped::ConstPtr& msg)
{
  local_vel = *msg;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "odom_publisher");
  ros::NodeHandle nh;
  ros::Rate rate(20);

  // Publisher
  ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 50);
  // Subscriber
  ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>("mavros/local_position/pose", 50, local_pos_cb);
  ros::Subscriber local_vel_sub = nh.subscribe<geometry_msgs::TwistStamped>("mavros/local_position/velocity", 50, local_vel_cb);

  while(ros::ok())
  {
    ros::spinOnce();

    tf::Quaternion tf_quat = tf::createQuaternionFromYaw(local_pos.pose.orientation.z);

    geometry_msgs::Quaternion odom_quat;
    tf::quaternionTFToMsg(tf_quat, odom_quat);

    nav_msgs::Odometry odom;
    odom.header.stamp = ros::Time::now();
    odom.header.frame_id = "odom";

    odom.pose.pose.position.x = local_pos.pose.position.x;
    odom.pose.pose.position.y = local_pos.pose.position.y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;

    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = local_vel.twist.linear.x;
    odom.twist.twist.linear.y = local_vel.twist.linear.y;
    odom.twist.twist.angular.z = local_vel.twist.angular.z;

    odom_pub.publish(odom);
    rate.sleep();
  }
}

• 将以下内容添加到 CMakeLists.txt

add_executable(odom_publisher src/odom_publisher.cpp)
target_link_libraries(odom_publisher ${catkin_LIBRARIES})

• 编译

cd ~/dronedoc_ws/
catkin_make

速度指令

• 使用 mavros，您可以通过向 /mavros/setpoint_velocity/cmd_vel_unstamped话题
• 或 /mavros/setpoint_velocity/cmd_vel 话题发送速度命令来移动您的无人机。
• 这些话题题按消息类型分隔，cmd_vel_unstamped 使用 Twist 类型消息，cmd_vel 使用 TwistStamped 类型消息。
• 由于 move_base 发布的是 Twist 类型的消息，这次我们将使用 /mavros/setpoint_velocity/cmd_vel_unstamped 话题。
• 此外，move_base 现在将速度指令发布到 /cmd_vel 主题，因此通过在 Launch 文件中重新映射将速度指令从 move_base 重新映射到无人机
  -如下所示：

<remap from="/cmd_vel" to="/mavros/setpoint_velocity/cmd_vel_unstamped" />

mav_frame参数

• 当使用 cmd_vel 主题或 cmd_vel_unstamped 主题时，速度指令使用相对于作为无人机位置参考的框架（例如 odom 或地图）的速度，而不是默认情况下无人机的基础框架。

• 如下图所示，如果作为速度指令参考的坐标系不同，即使输入相同的速度指令值，机器人的速度（粗箭头）也会不同。

• move_base 发布的速度命令基于机器人的基本框架。
• 因此，这次必须将作为速度指令参考的坐标系设置为机器人的基础坐标系。
• 要更改 cmd_vel 主题的速度指令所基于的框架，您需要更改 /mavros/setpoint_velocity/mav_frame 参数的设置。
• 如果要基于无人机的基本框架，请将此参数的值更改为 BODY_NED。 默认值为 LOCAL_NED。
• 您可以通过将以下内容添加到 mymodel_sitl_tf.launch 来更改此参数的值：

<param name="/mavros/setpoint_velocity/mav_frame"  type="str" value="BODY_NED" />

• 其他可用的框架在 mavros / Enumerations 中列出。

导航包配置文件

• 全局和本地路线规划
• 成本地图是根据预先给出的地图信息和传感器获取的障碍物信息构建的地图，用于路线规划。 （参考）
• 全局路线规划从头到尾规划路线。全局成本地图用于全局路线规划。
• 在本地路线规划中，您可以规划路线以避开障碍物。本地成本地图用于本地路线规划。
• 全局路由规划的路由规划方法称为全局规划器，局部路由规划的方法称为局部规划器。全局规划器包括 Dijkstra 算法和 A* 方法，局部规划器包括动态窗口方法（DWA）。
• 成本地图设置
• yaml 文件中描述了成本图设置。准备三个配置文件，分别是通用设置、全局代价图设置、局部代价图设置，如下所示。
• 设置成本地图参数也会影响全局和本地规划器生成的路由。如果您对生成的全局或局部路径不满意，您可能需要考虑更改成本图的参数。
• 以下设置是最低要求，这些配置放在px4_sim_pkg/config目录下
• 新建costmap_common_params.yaml
• 内容如下：

obstacle_range: 3.0
raytrace_range: 2.0
footprint: [[0.3, 0.3], [0.3, -0.3], [-0.3, -0.3], [-0.3, 0.3]]
inflation_radius: 0.5

observation_sources: laser_scan_sensor

laser_scan_sensor: {
    sensor_frame: lidar_link,
    data_type: LaserScan,
    topic: /laser/scan,
    marking: true,
    clearing: true
  }

• 新建global_costmap_params.yaml
• 内容如下：

global_costmap:
  global_frame: map
  robot_base_frame: base_link
  publish_frequency: 1.0
  update_frequency: 5.0
  static_map: false
  rolling_window: true
  width: 20.0
  height: 20.0
  resolution: 0.1

• 新建local_costmap_params.yaml

local_costmap:
  global_frame: odom
  robot_base_frame: base_link
  update_frequency: 10.0
  publish_frequency: 5.0
  static_map: false
  rolling_window: true
  width: 6.0
  height: 6.0
  resolution: 0.05

• base_local_planner_params.yaml

TrajectoryPlannerROS:
  max_vel_x: 0.4
  min_vel_x: -0.1
  meter_scoring: true
  pdist_scale: 0.9
  xy_goal_tolerance: 1.0
  holonomic_robot: true

更改无人机参数

• 为了提高二维自位置估计和映射的精度，需要尽可能保持传感器水平。
• 设置无人机参数，使俯仰角和滚转角不超过一定水平。
• 此外，默认情况下，故障保护功能被禁用，因为当与 GCS（地面控制站）和 RC 的连接被切断以及当 OFFBOARD 控制被切断时，它会自动返回到初始位置。
• 在实际设备上取消故障安全时要非常小心。
• 更改以下参数的设置。 有关可用参数的列表，请参阅参数参考

名称	min > max (Incr.)	默认	単位
FW_P_LIM_MAX	0.0 > 60.0 (0.5)	45.0	deg
FW_P_LIM_MIN	-60.0 > 0.0 (0.5)	-45.0	deg
FW_R_LIM	35.0 > 65.0 (0.5)	50.0	deg
NAV_DLL_ACT	0 > 6	0
NAV_RCL_ACT	0 > 6	2
COM_OBL_ACT	0 > 2	0

通过命令设置

• PX4模拟器启动后，执行以下命令。
• 这一次，我将尝试设置以下值。 请根据需要更改该值。

param set FW_P_LIM_MAX 10.0
param set FW_P_LIM_MIN -10.0
param set FW_R_LIM 40.0
param set NAV_DLL_ACT 0
param set NAV_RCL_ACT 0
param set COM_OBL_ACT 0

• 如果命令成功，您应该看到如下内容： 以下是 FW_R_LIM 的示例。

FW_R_LIM: curr: 50.0000 -> new: 40.0000

使用启动的配置文件

• PX4模拟通过启动脚本（~/.ros/etc/init.d-posix/rcS）启动，通过读取里面的设置脚本来设置飞行器的参数。

• 模拟中用到的飞机配置脚本在~/.ros/etc/init.d-posix下，从这里加载。因此，在创建配置文件后，您需要移动到此目录或放置一个符号链接。您可以手动移动文件，但您可能会在更新后忘记移动它，或者您可能会在初始设置期间忘记移动它。

• 所以，这一次，我将编辑 CMakeLists.txt 以便它在构建后自动移动。

• 在本节中，我们将创建一个配置脚本并向 CMakeLists.txt 添加描述。

• 将px4_sim_pkg / posix_airframes下内容如下的配置脚本保存为70010_iris_2d_lidar。

• 有关未解释的参数，请参阅参数列表。 添加新机身配置--PX4 开发人员指南中描述了配置脚本。

• 70010_iris_2d_lidar内容如下：

# For detail of parameters,
# see https://dev.px4.io/en/advanced/parameter_reference.html

sh /etc/init.d-posix/10016_iris

# Setting MAVLink airframe type
param set MAV_TYPE 2

param set SYS_AUTOSTART 70010
param set SYS_RESTART_TYPE 2
param set BAT_N_CELLS 3

# Takeoff altitude
param set MIS_TAKEOFF_ALT 2.0

# Setting tilt angle limit
param set FW_P_LIM_MAX 10.0
param set FW_P_LIM_MIN -10.0
param set FW_R_LIM 35.0

# Setting failsafe action
param set NAV_DLL_ACT 0
param set NAV_RCL_ACT 0
param set COM_OBL_ACT 1

• 另外，将以下内容添加到 CMakeLists.txt。
• 在下面，add_custom_target 用于创建一个名为 iris_2d_lidar 的目标，该目标创建一个符号链接。
• 如果没有安装 PX4 Firmware 会报错，请确保安装了 PX4 Firmware 并在构建前启动模拟。

set(PACK_AIRFRAME_DIR ${CMAKE_CURRENT_SOURCE_DIR}/posix_airframes)
set(LOCAL_AIRFRAME_DIR $ENV{HOME}/.ros/etc/init.d-posix)
add_custom_target(iris_2d_lidar
                  ALL ln -s -b ${PACK_AIRFRAME_DIR}/70010_iris_2d_lidar ${LOCAL_AIRFRAME_DIR}/)

• 另外，mymodel_sitl_tf.launch增加如下内容：

<arg name="vehicle" default="iris_2d_lidar"/>
<arg name="sdf" default="$(find px4_sim_pkg)/models/iris_2d_lidar/model.sdf" />

<include file="$(find px4)/launch/mavros_posix_sitl.launch" >
    <arg name="sdf" value="$(arg sdf)" />
    <arg name="vehicle" value="$(arg vehicle)" />
</include>

• 再编译一次

cd ~/dronedoc_ws/
catkin_make

编写启动文件

• 导航启动
• 这个Launch文件假设cost map等所有配置文件都保存在px4_sim_pkg/config下。
• 将以下内容保存为 px4_sim_pkg /launch 下的 navigation.launch。
• navigation.launch内容如下：

<launch>

  <node pkg="px4_sim_pkg" type="odom_publisher" name="odom_publisher"/>

  <remap from="/cmd_vel" to="/mavros/setpoint_velocity/cmd_vel_unstamped"/>

  <node pkg="move_base" type="move_base" name="move_base" respawn="false" output="screen">

    <!-- update frequency of global plan -->
    <param name="planner_frequency" value="2.0"/>

    <!-- Common params for costmap -->
    <rosparam command="load" ns="global_costmap" file="$(find px4_sim_pkg)/config/costmap_common_params.yaml"/>
    <rosparam command="load" ns="local_costmap" file="$(find px4_sim_pkg)/config/costmap_common_params.yaml"/>

    <!-- Params for global costmap -->
    <rosparam command="load" file="$(find px4_sim_pkg)/config/global_costmap_params.yaml"/>

    <!-- Params for local costmap -->
    <rosparam command="load" file="$(find px4_sim_pkg)/config/local_costmap_params.yaml"/>

    <!-- Params for local planner -->
    <rosparam command="load" file="$(find px4_sim_pkg)/config/base_local_planner_params.yaml"/>
  </node>

</launch>

• mymodel_sitl_tf.launch内容如下：

<launch>

    <node pkg="tf" name="base2lidar" type="static_transform_publisher" args="0 0 0.1 0 0 0 base_link lidar_link 100"/>
    <node pkg="tf" name="map2odom" type="static_transform_publisher" args="0 0 0 0 0 0 map odom 100"/>

    <arg name="vehicle" default="iris_2d_lidar"/>
    <arg name="sdf" default="$(find px4_sim_pkg)/models/iris_2d_lidar/model.sdf" />

    <include file="$(find px4)/launch/mavros_posix_sitl.launch" >
        <arg name="sdf" value="$(arg sdf)" />
        <arg name="vehicle" value="$(arg vehicle)" />
    </include>

    <param name="/mavros/local_position/tf/send" type="bool" value="true" />
    <param name="/mavros/local_position/frame_id" type="str" value="odom" />
    <param name="/mavros/local_position/tf/frame_id" type="str" value="odom" />
    <param name="/mavros/setpoint_velocity/mav_frame"  type="str" value="BODY_NED" />

</launch>

测试：

• 分别终端，启动gazebo

cd ~/tools/dronedoc/
source load_environment.sh
roslaunch px4_sim_pkg mymodel_sitl_tf.launch

• 新开终端，执行导航

cd ~/tools/dronedoc/
source load_environment.sh
roslaunch px4_sim_pkg navigation.launch

• 效果如下：

• 发送目标，起飞无人机
• 在发送目标之前起飞无人机。 通过在运行模拟节点的终端中执行以下命令来起飞。

commander takeoff

• 也可以使用rosservice启动起飞

rosservice call /mavros/cmd/takeoff "{min_pitch: 0.0, yaw: 0.0, latitude: 47.3977506, longitude: 8.5456074, altitude: 5}"

• 也可以使用rosrun启动起飞

rosrun mavros mavcmd takeoffcur -a 0 0 5

• 启动rviz

rviz

• rviz里通过add按钮，增加TF话题，下图左侧所示

• 使用 Rviz 发送目标

• 您还可以通过向 move_base/goal 主题发送类型为 move_base_msgs / MoveBaseActionGoal 的消息来设置目标，但这次使用 Rviz 发送目标更容易。

• 您可以使用 Rviz 设置 move_base 的目标，如下图所示。

• 单击红框中的 2D Nav Goal 将其选中，然后单击目标位置确定它。

• 您还可以通过在单击的同时移动鼠标光标来指定目标位置的方向。

• 效果如下：

切换到Offboard模式

• 另外，如果您不是在 Offboard 模式下，您将无法使用 cmd_vel_unstamped 主题进行操作，因此将其设置为 Offboard 模式。 使用以下命令将无人机的飞行模式设置为 Offboard。
• 如果话题没有发布到/mavros/setpoint_velocity/cmd_vel_unstamped，则不会切换到Offboard模式，所以先点目标再切换到Offboard模式。

rosservice call /mavros/set_mode "base_mode: 0 custom_mode: 'OFFBOARD'"

• 或者使用 mavros 节点

rosrun mavros mavsys mode -c OFFBOARD

• 执行结果
• 以下视频显示了执行上述步骤的结果。 如果你指定一个目标，你可以看到你正在朝着目标飞行。

• 除了 TF 之外，此视频中的 Rviz 还显示了有关以下主题的消息。

/move_base/current_goal
当前目标姿势

/move_base/local_costmap/footprint
机器人外形

/move_base/local_costmap/costmap
本地成本图

/move_base/TrajectoryPlannerROS/local_plan
本地路径

也可以通过单击“添加”按钮显示的“添加消息”窗口的“按主题”选项卡添加这些消息。
全局路径

• 也可以通过单击“添加”按钮显示的“添加消息”窗口的“按主题”选项卡添加这些消息。

避障

• 如果无人机的路径中有障碍物，则会生成绕行路线
• 如图：

• 路线与障碍物的距离、代价地图的大小、分辨率等都可以通过更改配置文件中的参数来调整。
• 另外，请参阅 costmap_2d 包和 base_local_planner 包 Wiki 页面以了解其他参数。
• 通过以上，我们能够为无人机指定目标位置，执行考虑障碍物的路线计划，在路线上移动，并移动到目标位置。

参考：

• https://uenota.github.io/dronedoc/en/slam/gps_nav.html