z2pro极空间docker 极空间z4 固件

PX4编译

文章目录

• PX4编译

• 疑难杂症

• bug1
• bug2
• catkin_build_isolated

• 官方脚本

• Step1
• Step2 安装常用依赖
• Step3 创建并运行脚本
• Step4 补全代码子模块
• Step5 验证仿真

• 官方offboard 例程

• offboard控制
• 启动

• Step1：启动gazebo模型
• Step2：启动Mavros
• Step3：启动offboard示例程序
• 节点

• 利用launch文件同时启动Mavros和gazebo
• 更换模型

• 速度控制

• Topic1
• Topic2

• Apriltag 识别定位

• launch文件修改
• 位姿信息/tf或/tag_detections

疑难杂症

bug1

[gazebo-3] process has died [pid 19047, exit code 255]

gazebo崩溃，解决方式 ：killall gzserver

bug2

OpenCV库没有添加链接

在CMakelists.txt末尾添加OpenCV库

catkin_build_isolated

单独编译各个包，如果遇到一个包有错，可以正常编译剩余的九个包。

source ./devel_isolated/setup.zsh

使用里面的包时，记得在新的终端进入catkin_ws要source环境变量：

source ./devel_isolated/setup.zsh

官方脚本

https://dev.px4.io/v1.9.0/en/setup/dev_env_linux_ubuntu.html

Step1

打开一个终端执行下面的命令，让当前用户从属dialout用户组，防止以后的权限问题。

sudo usermod -a -G dialout $USER

执行完这一之后，注销（关机），再重新登入（开机）

Step2 安装常用依赖

如果是一台没有安装ROS的新ubuntu18.04系统，可以直接使用脚本

这个脚本会帮你安装ROS,Gazebo, Mavros等工具

如果已经安装了desktop-full版本的ROS，则采用脚本

该脚本内容如下

#!/bin/bash

## Bash script for setting up a PX4 development environment on Ubuntu LTS (16.04).
## It can be used for installing simulators (only) or for installing the preconditions for Snapdragon Flight or Raspberry Pi.
##
## Installs:
## - Common dependencies and tools for all targets (including: Ninja build system, Qt Creator, pyulog)
## - FastRTPS and FastCDR
## - jMAVSim simulator dependencies
## - PX4/Firmware source (to ~/src/Firmware/)

# Preventing sudo timeout https://serverfault.com/a/833888
trap "exit" INT TERM; trap "kill 0" EXIT; sudo -v || exit $?; sleep 1; while true; do sleep 60; sudo -nv; done 2>/dev/null &

# Ubuntu Config
# 先卸载掉干扰串口和USB的一个Ubuntu自带的包，一般这个包用不上，以后如果要用可以再下载。
echo "We must first remove modemmanager"
sudo apt-get remove modemmanager -y


# Common dependencies
# 公共依赖下载
echo "Installing common dependencies"
# 更新源
sudo apt-get update -y
sudo apt-get install git zip qtcreator cmake build-essential genromfs ninja-build exiftool astyle -y
# make sure xxd is installed, dedicated xxd package since Ubuntu 18.04 but was squashed into vim-common before
which xxd || sudo apt install xxd -y || sudo apt-get install vim-common --no-install-recommends -y
# Required python packages
sudo apt-get install python-argparse python-empy python-toml python-numpy python-dev python-pip -y
sudo -H pip install --upgrade pip
sudo -H pip install pandas jinja2 pyserial pyyaml
# optional python tools
sudo -H pip install pyulog

# Install FastRTPS 1.7.1 and FastCDR-1.0.8
# 简单来说Fast RTPS就是ROS 2中用来订阅发布消息的组件
fastrtps_dir=$HOME/eProsima_FastRTPS-1.7.1-Linux
echo "Installing FastRTPS to: $fastrtps_dir"
if [ -d "$fastrtps_dir" ]
then
    echo " FastRTPS already installed."
else
    pushd .
    cd ~
    wget https://www.eprosima.com/index.php/component/ars/repository/eprosima-fast-rtps/eprosima-fast-rtps-1-7-1/eprosima_fastrtps-1-7-1-linux-tar-gz -O eprosima_fastrtps-1-7-1-linux.tar.gz
    tar -xzf eprosima_fastrtps-1-7-1-linux.tar.gz eProsima_FastRTPS-1.7.1-Linux/
    tar -xzf eprosima_fastrtps-1-7-1-linux.tar.gz requiredcomponents
    tar -xzf requiredcomponents/eProsima_FastCDR-1.0.8-Linux.tar.gz
    cpucores=$(( $(lscpu | grep Core.*per.*socket | awk -F: '{print $2}') * $(lscpu | grep Socket\(s\) | awk -F: '{print $2}') ))
    (cd eProsima_FastCDR-1.0.8-Linux && ./configure --libdir=/usr/lib && make -j$cpucores && sudo make install)
    (cd eProsima_FastRTPS-1.7.1-Linux && ./configure --libdir=/usr/lib && make -j$cpucores && sudo make install)
    rm -rf requiredcomponents eprosima_fastrtps-1-7-1-linux.tar.gz
    popd
fi

# jMAVSim simulator dependencies
# jMAVSim仿真依赖安装
echo "Installing jMAVSim simulator dependencies"
sudo apt-get install ant openjdk-8-jdk openjdk-8-jre -y

# Clone PX4/Firmware
# 下载PX4源码到 src/Firmware
clone_dir=~/src
echo "Cloning PX4 to: $clone_dir."
if [ -d "$clone_dir" ]
then
    echo " Firmware already cloned."
else
    mkdir -p $clone_dir
    cd $clone_dir
    git clone https://github.com/PX4/Firmware.git
fi

Step3 创建并运行脚本

进入Home

gedit ubuntu_sim_common_deps.sh //创建脚本，再将上述内容复制进去
source ubuntu_sim_common_deps.sh //等待

如果报错，缺少包，则缺什么补什么

Step4 补全代码子模块

接下来补全代码的子模块，防止编译的时候出一下意外的问题。

cd arc/Firmware
git submodule update --init --recursive

Step5 验证仿真

首先编译jmavsim仿真验证。

cd src/Firmware
make px4_sitl jmavsim

如果没有什么问题会显示出下面的界面

终端输入下面的命令，飞机会起飞

commander takeoff

现在可以Ctrl/Command + C关掉继续进行下面的操作
输入下面的命令然后进行gazebo仿真的编译

cd src/Firmware
make px4_sitl gazebo

如果没有问题，gazebo正常打开，出现了一个小飞机.

官方offboard 例程

offboard控制

打开官方

https://dev.px4.io/v1.9.0/zh/ros/mavros_offboard.htmlMAVROS

Offboard control example链接，然后在catkin_ws/src目录中，运行命令

catkin_create_pkg offboard_pkg roscpp std_msgs geometry_msgs mavros_msgs

然后定位到目录~/catkin_ws/src/offboard_pkg/src/，新建一个文件offboard_node.cpp。

gedit offboard_node.cpp

将代码复制进去(官方示例):

/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo SITL
 */

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
//我们创建一个简单的回调函数来保存飞控的当前状态。我们可以用它检查连接状态，解锁状态以及外部控制标志。
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }

        local_pos_pub.publish(pose);

        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}

然后打开目录~/catkin_ws/src/offboard_pkg/下的CMakeLists.txt添加下面的两行：

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

然后到目录~/catkin_ws下，运行命令

catkin_make

等待编译完成后，如果你要在gazebo中仿真，运行命令（启动PX4的gazebo仿真环境）四旋翼无人机名字：iris

启动

Step1：启动gazebo模型

下面三个命令三选一

make px4_sitl gazebo_iris//四旋翼
make px4_sitl gazebo_iris_opt_flow//带双目摄像头的无人机
make px4_sitl_default gazebo // 效果和第一条代码一样

无人机模型可以更换，模型存放于目录/home/fangcheng/src/Firmware/Tools/sitl_gazebo/models

只需将gazebo_iris换成相应的模型即可：模型列表

https://dev.px4.io/v1.9.0/zh/simulation/gazebo.html

Step2：启动Mavros

然后在终端下运行命令启动Mavros：(官网也有) 目的是启动PX4与Mavros之间的连接。/home/fangcheng/src/Firmware/launch/px4.launch

roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

Step3：启动offboard示例程序

然后运行命令，启动示例程序

rosrun offboard_pkg offboard_node

然后进入gazebo中进行观察。

节点

查看消息的具体格式

PoseStamped

Header header

Pose pose

uint32 seq

time stamp

string frame-id

Point position

Quaternion quaternion

float64 x

float64 y

float64 z

float64 x

float64 y

float64 z

float64 w

bool类型只有两个值：true or false

Nodes Only

利用launch文件同时启动Mavros和gazebo

打开一个终端依次输入

source ~/src/Firmware/Tools/setup_gazebo.bash ~/src/Firmware/ ~/src/Firmware/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/src/Firmware
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/src/Firmware/Tools/sitl_gazebo

但每打开一个新的终端都这样做很麻烦，可以将以上代码添加到.zshrc末尾

gedit .zshrc
source ~/.zshrc

以后打开新的终端直接运行下面的launch即可

再运行固件库中写好的launch文件，存放于/home/fangcheng/src/Firmware/launch/mavros_posix_sitl.launch

此命令同时启动了mavros和gazebo

roslaunch px4 mavros_posix_sitl.launch

更换模型

在~/src/Firmware/Tools/sitl_gazebo/models中存放了许多模型，默认的模型为iris，常用的还有https://dev.px4.io/v1.9.0/zh/simulation/gazebo.html

其中iris_stereo_camera为带双目摄像头的模型，以此模型为例演示如何修改启动的模型

首先确认要更换的模型存在于~/src/Firmware/Tools/sitl_gazebo/models

打开/home/fangcheng/src/Firmware/launch/mavros_posix_sitl.launch文件，将第16行更改为

<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/iris_stereo_camera/iris_stereo_camera.sdf"/>

保存后再运行

roslaunch px4 mavros_posix_sitl.launch

查看话题，可以发现多了很多双目相关的话题

更换为单目摄像机 iris_fpv_cam，方法同上。但默认的摄像头不在下方，要改为下置摄像头，则需要改模型参数

将iris_fpv_cam.sdf中的0 0 0 0 0 0改成0 0 0 0 1.57 0

to

采集的图像话题为

可以使用 rqt_image_view来查看

速度控制

Topic1

mavros/setpoint_raw/local

选择坐标系为FRAME_BODY_NED表示使用机身坐标系

当设置yaw_rate=pos.velocity.x或pos.velocity.y可以画圆。

Topic2

mavros/setpoint_velocity/cmd_vel_unstamped

相对本地gazebo坐标系，而非机身坐标系。

Apriltag 识别定位

Tutorials：

http://wiki.ros.org/apriltag_ros/Tutorials/Detection%20in%20a%20video%20stream

launch文件修改

位姿信息/tf或/tag_detections

平移向量

相机后方，z为负，前方z为正；

相机左边，x为负，右边x为正；

相机上方，y为负，下方y为正。

/tag_detections  tag相對於相機的坐標
相機與飛機的相對位姿爲（0，0，0，0，0，0）
/mavros/local_position/pose 飛機相對於世界的坐標