ros+coppeliasim驱动四旋翼无人机

ROS+coppeliasim实现无人机控制

参考源码：FlightController

简介: 基于ROS，C ++和coppeliaSim的四旋翼无人机控制仿真模型 采用动力学模型重构的控制器，计算升力f以及力矩M，并通过控制分配矩阵控制各电机转速，实现无人机控制

系统与配置
ubuntu系统：ubuntu18.04
coppliasim：V4.2.0
场景文件：源码文件中的~/FlightController/scenes/quadcopter.ttt

运行程序

创建工作空间：

mkdir -p tracker_ws/src  

将源文件代码重命名为tracker，将tracker_ws/src/tracker/src/main.cpp更改为tracker.cpp,并将/home/caodong/tracker_ws/src/tracker/CMakeLists.txt的代码中的代码中的main全部更改为tracker。
同时由于如下问题
catkin_make产生的可执行文件到build目录，导致rosrun找不到节点
需要将CMakeLists.txt更改为如下代码：

cmake_minimum_required(VERSION 2.8.3)
project(tracker)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11 -03)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") #c++11
#set(CMAKE_CXX_FLAGS_RELEASE "-O3 -g -march=native --fast-math -ffast-math -mtune=native -funroll-loops -DNDEBUG -DBOOST_DISABLE_ASSERTS" CACHE STRING COMPILE_FLAGS FORCE)
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -march=native -mtune=native -funroll-loops -DNDEBUG -DBOOST_DISABLE_ASSERTS" CACHE STRING COMPILE_FLAGS FORCE)

find_package(catkin REQUIRED COMPONENTS
        roscpp
        std_msgs
        image_transport
        cv_bridge
        geometry_msgs
        nav_msgs
        )

find_package(OpenCV REQUIRED)
find_package(Eigen3 REQUIRED)


catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES control
#  CATKIN_DEPENDS geometry_msgs message_generation roscpp rospy std_msgs
#  DEPENDS system_lib
)

include_directories(
         #include
        ${catkin_INCLUDE_DIRS}
        ${CMAKE_CURRENT_SOURCE_DIR}/include
        /usr/include/eigen3
)

add_library(FlightController
        src/flightController.cpp)
target_link_libraries(FlightController
        ${catkin_LIBRARIES}
        )

add_executable(tracker
        src/tracker.cpp
        )
target_link_libraries(tracker
        ${catkin_LIBRARIES}
        ${OpenCV_LIBS}
        FlightController
        )

运行如下命令行

roscore
rosrun tracker tracker

利用copeliasim打开~/FlightController/scenes/quadcopter.ttt场景文件

场景文件解释

无人机组成

四旋翼无人机主体为coppeliasim自带的无人机，其携带的传感器为imu传感器(由coppeliasim自带加速度传感器和陀螺仪(角速度)传感器构成)和三个摄像头（本项目中未使用，不做说明）。

coppeliasim中的无人机lua脚本

如下为无人机的lua脚本，主要功能是实现无人机的基本控制以及与ros通信，其中无人机控制算法相比于与ROS中的控制算法更加简单，不稳定，性能差。脚本中的中文注释为本人添加。

function __getObjectPosition__(a,b)
    -- compatibility routine, wrong results could be returned in some situations, in CoppeliaSim <4.0.1
    if b==sim.handle_parent then
        b=sim.getObjectParent(a)
    end
    if (b~=-1) and (sim.getObjectType(b)==sim.object_joint_type) and (sim.getInt32Param(sim.intparam_program_version)>=40001) then
        a=a+sim.handleflag_reljointbaseframe
    end
    return sim.getObjectPosition(a,b)
end
function __getObjectOrientation__(a,b)
    -- compatibility routine, wrong results could be returned in some situations, in CoppeliaSim <4.0.1
    if b==sim.handle_parent then
        b=sim.getObjectParent(a)
        
    end
    if (b~=-1) and (sim.getObjectType(b)==sim.object_joint_type) and (sim.getInt32Param(sim.intparam_program_version)>=40001) then
        a=a+sim.handleflag_reljointbaseframe
    end
    return sim.getObjectOrientation(a,b)
end
---初始化函数，初始化参数，创建对象
function sysCall_init()
    -- Make sure we have version 2.4.13 or above (the particles are not supported otherwise)验证版本
    v = sim.getInt32Param(sim.intparam_program_version)
    if (v < 20413) then
        sim.displayDialog('Warning', 'The propeller model is only fully supported from V-REP version 2.4.13 and above.&&nThis simulation will not run as expected!', sim.dlgstyle_ok, false, '', nil, { 0.8, 0, 0, 0, 0, 0 })
    end

    -- Detatch the manipulation sphere:
    targetObj = sim.getObjectHandle('Quadricopter_target')-
    sim.setObjectParent(targetObj, -1, true)--创建无人机期望对象（球体）

    -- This control algo was quickly written and is dirty and not optimal. It just serves as a SIMPLE example飞控说明

    d = sim.getObjectHandle('Quadricopter_base')--

    particlesAreVisible = sim.getScriptSimulationParameter(sim.handle_self, 'particlesAreVisible')
    sim.setScriptSimulationParameter(sim.handle_tree, 'particlesAreVisible', tostring(particlesAreVisible))
    simulateParticles = sim.getScriptSimulationParameter(sim.handle_self, 'simulateParticles')
    sim.setScriptSimulationParameter(sim.handle_tree, 'simulateParticles', tostring(simulateParticles))

    propellerScripts = { -1, -1, -1, -1 }
    for i = 1, 4, 1 do
        propellerScripts[i] = sim.getScriptHandle('Quadricopter_propeller_respondable' .. i)--创建执行单元对象
    end
    heli = sim.getObjectHandle(sim.handle_self)--创建无人机本身对象
--初始化控制器参数
    particlesTargetVelocities = { 0, 0, 0, 0 }

    pParam = 2
    iParam = 0
    dParam = 0
    vParam = -2

    cumul = 0
    lastE = 0
    pAlphaE = 0
    pBetaE = 0
    psp2 = 0
    psp1 = 0

    prevEuler = 0

    fakeShadow = sim.getScriptSimulationParameter(sim.handle_self, 'fakeShadow')--投影参数
    if (fakeShadow) then
        shadowCont = sim.addDrawingObject(sim.drawing_discpoints + sim.drawing_cyclic + sim.drawing_25percenttransparency + sim.drawing_50percenttransparency + sim.drawing_itemsizes, 0.2, 0, -1, 1)
    end

    -- Prepare 2 floating views with the camera views:设置摄像头
    floorCam = sim.getObjectHandle('Quadricopter_floorCamera')
    frontCam = sim.getObjectHandle('Quadricopter_frontCamera')
    floorView = sim.floatingViewAdd(0.9, 0.9, 0.2, 0.2, 0)
    frontView = sim.floatingViewAdd(0.7, 0.9, 0.2, 0.2, 0)
    sim.adjustView(floorView, floorCam, 64)
    sim.adjustView(frontView, frontCam, 64)


    
    visionSensor_h = sim.getObjectHandle('Vision_sensor')--创建下端摄像头传感器
    imu_handle = sim.getObjectHandle('IMU_link')--创建IMU对象

    if simROS then
        
        revsSub = simROS.subscribe('/rotorRevs', 'std_msgs/Float64MultiArray', 'rotorRevs_cb')--订阅ros发布的电机转速话题

        

        Gyro_pub = simROS.advertise('/imu', 'sensor_msgs/Imu')
        simROS.publisherTreatUInt8ArrayAsString(Gyro_pub)--创建IMU发布对象
        target_pub = simROS.advertise('/target', 'geometry_msgs/Pose')
        simROS.publisherTreatUInt8ArrayAsString(target_pub)--创建目标发布对象
        Odom_pub = simROS.advertise('/odom','nav_msgs/Odometry')
        simROS.publisherTreatUInt8ArrayAsString(Odom_pub)--创建里程计发布对象

        Imu_data = {}--初始化IMU
        gyroCommunicationTube = sim.tubeOpen(0, 'gyroData' .. sim.getNameSuffix(nil), 1)--与 'gyroData' .. sim.getNameSuffix(nil)通讯道上与gyro陀螺仪传感器通讯获得陀螺仪数据
        accelCommunicationTube = sim.tubeOpen(0, 'accelerometerData' .. sim.getNameSuffix(nil), 1)--与'accelerometerData' .. sim.getNameSuffix(nil)通讯道上与加速度传感器通讯获得加速度数据
    end

    rotorRevs={0,0,0,0}--初始化转速

    initTime = sim.getSimulationTime()--初始化时间

end

--油门指令转转速
function rotorRevs_cb(msg)
     
    rotorRevs[1] = msg.data[1]*3.3
    rotorRevs[2] = msg.data[2]*3.3
    rotorRevs[3] = msg.data[3]*3.3
    rotorRevs[4] = msg.data[4]*3.3   
    
end

--恢复函数：
function sysCall_cleanup()
    sim.removeDrawingObject(shadowCont)
    sim.floatingViewRemove(floorView)
    sim.floatingViewRemove(frontView)
end

--驱动函数：
function sysCall_actuation()
    s = sim.getObjectSizeFactor(d)

    pos = sim.getObjectPosition(d,-1)
    if (fakeShadow) then
        itemData = { pos[1], pos[2], 0.002, 0, 0, 1, 0.2 * s }
        sim.addDrawingObjectItem(shadowCont, itemData)
    end


    -- Vertical control:高度通道环
    targetPos = sim.getObjectPosition(targetObj,-1)
    pos = sim.getObjectPosition(d,-1)
    l = sim.getVelocity(heli)
    e = (targetPos[3] - pos[3])
    cumul = cumul + e
    pv = pParam * e
    --5.335=kmg->k=5.335/1/9.8=0.544387755
    thrust = 5.335 + pv + iParam * cumul + dParam * (e - lastE) + l[3] * vParam --- 0.544387755*1*errorZ
    lastE = e

    -- Horizontal control:水平通道速度环
    sp = __getObjectPosition__(targetObj, d)
    m = sim.getObjectMatrix(d, -1)
    vx = { 1, 0, 0 }
    vx = sim.multiplyVector(m, vx)
    vy = { 0, 1, 0 }
    vy = sim.multiplyVector(m, vy)
    --m[12]->z height
    alphaE = (vy[3] - m[12])
    alphaCorr = 0.25 * alphaE + 2.1 * (alphaE - pAlphaE)
    betaE = (vx[3] - m[12])
    betaCorr = -0.25 * betaE - 2.1 * (betaE - pBetaE)
    pAlphaE = alphaE
    pBetaE = betaE
    
    alphaCorr = alphaCorr + sp[2] * 0.005 + 1 * (sp[2] - psp2) -- errorY/30
    betaCorr = betaCorr - sp[1] * 0.005 - 1 * (sp[1] - psp1) -- errorX/30
    psp2 = sp[2]
    psp1 = sp[1]

    -- Rotational control:姿态环
    euler = __getObjectOrientation__(d, targetObj)
    rotCorr = euler[3] * 0.1 + 2 * (euler[3] - prevEuler)
    prevEuler = euler[3]

    -- Decide of the motor velocities:分配电机转速
    --print(rotorRevs)打印ROS发布速度
    if (rotorRevs[1] == 0 or (sim.getSimulationTime()-initTime)<2) then--仿真内部分配电机转速
        particlesTargetVelocities[1] = thrust * (1 - alphaCorr + betaCorr + rotCorr)--+error2
        particlesTargetVelocities[2] = thrust * (1 - alphaCorr - betaCorr - rotCorr)--+error1
        particlesTargetVelocities[3] = thrust * (1 + alphaCorr - betaCorr + rotCorr)--+error4
        particlesTargetVelocities[4] = thrust * (1 + alphaCorr + betaCorr - rotCorr)--+error3
    else--ros分配电机转速
        particlesTargetVelocities[1] = rotorRevs[1]
        particlesTargetVelocities[2] = rotorRevs[2]
        particlesTargetVelocities[3] = rotorRevs[3]
        particlesTargetVelocities[4] = rotorRevs[4]
    end
    --print(particlesTargetVelocities)--打印实际速度
    -- Send the desired motor velocities to the 4 rotors:将期望电机转速输入转子
    for i = 1, 4, 1 do
        sim.setScriptSimulationParameter(propellerScripts[i], 'particleVelocity', particlesTargetVelocities[i])
    end

end
--传感器函数
function sysCall_sensing()
    if simROS then
        quaternion = sim.getObjectQuaternion(imu_handle,-1)--读取IMU的四元数
        accele_data = sim.tubeRead(accelCommunicationTube)--读取角速度传感器信号
        gyro_data = sim.tubeRead(gyroCommunicationTube)--读取陀螺仪传感器信号（角速度）
        if (accele_data and gyro_data) then
            acceleration = sim.unpackFloatTable(accele_data)--转化为浮点数
            angularVariations = sim.unpackFloatTable(gyro_data)--转化为浮点数
            Imu_data['orientation'] = { x = quaternion[1], y = quaternion[2], z = quaternion[3], w = quaternion[4] }
            Imu_data['header'] = { seq = 0, stamp = sim.getSystemTime(), frame_id = "imu_link" }
            Imu_data['linear_acceleration'] = { x = acceleration[1], y = acceleration[2], z = -acceleration[3] }
            Imu_data['angular_velocity'] = { x = angularVariations[1], y = angularVariations[2], z = angularVariations[3] }

            --simROS.sendTransform(getTransformStamped(imu_handle,'imu_link',base_handle,'base_link'))
            simROS.publish(Gyro_pub, Imu_data)--发布当前的线加速度和四元数和角速度
            
            local pos = sim.getObjectPosition(imu_handle,-1)--读取IMU的位置
            local ori = sim.getObjectQuaternion(imu_handle,-1)--读取IMU的四元数
            local vel = sim.getObjectVelocity(imu_handle, -1)--读取IMU的速度

            odom = {}
            odom.header = {seq=0,stamp=sim.getSystemTime(), frame_id="odom"}
            odom.child_frame_id = 'base_link'
            odom.pose = { pose={position={x=pos[1],y=pos[2],z=pos[3]}, orientation={x=ori[1],y=ori[2],z=ori[3],w=ori[4]} } }
            odom.twist={twist = { linear={x=vel[1], y=vel[2], z=vel[3]},angular = { x = angularVariations[1], y = angularVariations[2], z = angularVariations[3] }}}

            simROS.publish(Odom_pub, odom)--当前的位置、四元数和线速度和角速度
        end
        
        local targetPos = sim.getObjectPosition(targetObj,-1)
        local targetOri = sim.getObjectQuaternion(targetObj,-1)
        target_data={}
        target_data['position']={x=targetPos[1],y=targetPos[2],z=targetPos[3]}
        target_data['orientation']={x=targetOri[1],y=targetOri[2],z=targetOri[3],w=targetOri[4]}
        simROS.publish(target_pub, target_data)--目标的位置和四元数
    end
end

基于ROS的无人机控制

ROS的无人机控制主要他依靠c++源码如下。采用的控制算法与全权老师的《多旋翼飞行器设计与控制》底层控制基本完全一致，只有位置控制环期望推力的计算不同，全权老师书中的期望推力只考虑高度通道，而本源码考虑全部的三个通道。

flightController.h:

//
// Created by chrisliu on 2020/2/26.
//

//输入期望为位置，偏航角，同时未考虑系统时间
#ifndef FLIGHT_CONTROLLER_H
#define FLIGHT_CONTROLLER_H

#include <Eigen/Core>
#include <Eigen/Geometry>

class PayloadController
{
public:

    double massQuadcopter;//无人机质量

    Eigen::Vector3d cAngularVelocity_body;//无人机角速度
    Eigen::Matrix3d rotationMatrix_BuW;//无人机旋转矩阵

    Eigen::Vector3d positionBody;//机体位置
    Eigen::Vector3d cDesiredPositionBody;//历史期望位置
    Eigen::Vector3d pDesiredPositionBody;//期望位置

    Eigen::Quaterniond desiredQuaterniondBody;//四元数

    Eigen::Vector3d velocityBody;//速度
    Eigen::Vector3d desiredVelocityBody;//期望速度

    Eigen::Vector3d acceleration;//加速度

    void initializeParameter(double inputMassQuadcopter);

    void updatePastStates();

    void updateTargetStates(Eigen::Vector3d positionTarget,Eigen::Quaterniond quaternionTarget);

    Eigen::Vector4d getRevs();


private:

    Eigen::Vector3d getVectorD(Eigen::Vector3d cVector, Eigen::Vector3d pVector);

    Eigen::Vector3d antisymmetricMatrixToVector(Eigen::Matrix3d antisymmetricMatrix);


    Eigen::Vector4d getAllocatedRevs(double Force, Eigen::Vector3d Moment);
};


#endif //FLIGHT_CONTROLLER_H

flightController.cpp

//
// Created by chrisliu on 2020/2/26.
//

#include "flightController.h"

using namespace std;
 /*初始化参数：期望位置和无人机质量  */
void PayloadController::initializeParameter(double inputMassQuadcopter)
{
    massQuadcopter = inputMassQuadcopter;

    cDesiredPositionBody.Zero();
}
/* 更新历史期望位置 */
void PayloadController::updatePastStates()
{
    pDesiredPositionBody = cDesiredPositionBody;
}
/* 更新期望位置和姿态四元数并计算期望速度 */
void PayloadController::updateTargetStates(Eigen::Vector3d positionTarget, Eigen::Quaterniond quaternionTarget)
{
    cDesiredPositionBody = positionTarget;
    desiredVelocityBody = getVectorD(cDesiredPositionBody, pDesiredPositionBody);
    desiredQuaterniondBody = quaternionTarget;

}

/* 计算速度，返回值为速度 */
Eigen::Vector3d PayloadController::getVectorD(Eigen::Vector3d cVector, Eigen::Vector3d pVector)
{
    return cVector - pVector;
}
/* 旋转矩阵误差（角速度，姿态跟踪误差）反对称矩阵转化为旋转矩阵误差向量，返回值为向量*/
Eigen::Vector3d PayloadController::antisymmetricMatrixToVector(Eigen::Matrix3d   antisymmetricMatrix)
{
    Eigen::Vector3d vector(antisymmetricMatrix(7), antisymmetricMatrix(2), antisymmetricMatrix(3));

    return vector;
}
/*  通过控制分配模型分配电机转速，参考全权的《多旋翼飞行器设计与控制》书P245，返回值为电机转速*/
Eigen::Vector4d PayloadController::getAllocatedRevs(double Force, Eigen::Vector3d Moment)
{
    Eigen::Matrix4d Minvese;
    double sqrt2 = sqrt(2);
    Minvese << 1, -sqrt2, sqrt2, 1,
            1, -sqrt2, -sqrt2, -1,
            1, sqrt2, -sqrt2, 1,
            1, sqrt2, sqrt2, -1;
    Minvese = 0.25 * Minvese;//控制分配矩阵

    Eigen::Vector4d input(Force, Moment.x(), Moment.y(), Moment.z());
    Eigen::Vector4d revs = Minvese * input;
    if (revs.x() < 0)
    {
        revs.x() = 0;
    }
    if (revs.y() < 0)
    {
        revs.y() = 0;
    }
    if (revs.z() < 0)
    {
        revs.z() = 0;
    }
    if (revs.w() < 0)
    {
        revs.w() = 0;
    }
    revs.x() = sqrt(revs.x());
    revs.y() = sqrt(revs.y());
    revs.z() = sqrt(revs.z());
    revs.w() = sqrt(revs.w());//控制分配
    return revs;
}
/*姿态控制器、姿态解算器和位置控制器，输出为电机转速*/
Eigen::Vector4d PayloadController::getRevs()
{
    Eigen::Vector3d errorPosition = positionBody - cDesiredPositionBody;
    Eigen::Vector3d errorVelocity = velocityBody - desiredVelocityBody;

    const Eigen::Vector3d k_bx = Eigen::Vector3d(2.0, 2.0, 1.5);//p系数
    const Eigen::Vector3d k_bv = Eigen::Vector3d(2.5, 2.5, 2.5);//d系数

    Eigen::Vector3d e3(0, 0, -1);//定义z轴方向

    Eigen::Vector3d F_n = -errorPosition.cwiseProduct(k_bx) - errorVelocity.cwiseProduct(k_bv);//pd控制输出拉力，位置控制器

    F_n += (massQuadcopter) * (-e3 * 9.8);//加上z轴重力

    Eigen::Vector3d F = F_n;

    //保方向饱和函数
    if (F.norm() > 15)
    {
        F = 15 * F / F.norm();
    }//位置控制器p231

    //2,1,0->ZYX，即RPY
    Eigen::Vector3d eulerAngle = desiredQuaterniondBody.matrix().eulerAngles(2, 1, 0);//四元数转欧拉角（期望）
    Eigen::Vector3d b1_des(cos(eulerAngle.x()), sin(eulerAngle.x()), 0);//小角度假设，只有偏航角被计算

    Eigen::Vector3d b3_des = F / F.norm();//期望拉力方向

    Eigen::Vector3d b2_des;
    b2_des = b3_des.cross(b1_des);//叉积
    b2_des /= b2_des.norm();//单位化

    Eigen::Matrix3d desiredRotationMatrix;
    desiredRotationMatrix.col(0) = b2_des.cross(b3_des);//叉积
    desiredRotationMatrix.col(1) = b2_des;
    desiredRotationMatrix.col(2) = b3_des;//姿态解算器p235

    Eigen::Vector3d errorRotation =
            0.5 * antisymmetricMatrixToVector((desiredRotationMatrix.transpose() * rotationMatrix_BuW -
                                               rotationMatrix_BuW.transpose() * desiredRotationMatrix));// 姿态跟踪误差

    Eigen::Vector3d errorAngular = cAngularVelocity_body;//角速度跟踪误差小角度假设下可忽略： - rotationMatrix_BuW.transpose() * desiredRotationMatrix * desiredAngular;

//    Eigen::Matrix3d J_q;
//    J_q << 0.1, 0, 0,
//            0, 0.1, 0,
//            0, 0, 0.1;
    const Eigen::Vector3d k_p = Eigen::Vector3d(3, 3, 1);
    const Eigen::Vector3d k_d = Eigen::Vector3d(0.8, 0.8, 1.5);
    Eigen::Vector3d moment =
            -errorRotation.cwiseProduct(k_p) +
            errorAngular.cwiseProduct(k_d);// + cAngularVelocity_body.cross(J_q * cAngularVelocity_body);//pd控制输出力矩，姿态控制器

//            保方向饱和函数
    if (moment.norm() > 1)
    {
        moment = 1 * moment / moment.norm();
    }//姿态控制器

    double liftForce = b3_des.transpose() * F;//合力标量

    Eigen::Vector4d revs = getAllocatedRevs(liftForce, moment);//输入控制分配模型进行控制

    return revs;
}

tracker.cpp(main.cpp)

#include "flightController.h"

#include <ros/ros.h>

#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Float64MultiArray.h>

using namespace std;

PayloadController controller;

void callbackOdometry(const nav_msgs::OdometryConstPtr &odom)
{
    Eigen::Vector3d positionBody(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z);
    Eigen::Vector3d linearVelocity(odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.linear.z);

    controller.positionBody = positionBody;
    controller.velocityBody = linearVelocity;
}//订阅当前位置和线速度

void callbackImu(const sensor_msgs::ImuConstPtr &imu)
{

    Eigen::Quaterniond quaternion(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z);
    Eigen::Vector3d linearAcceleration(imu->linear_acceleration.x, imu->linear_acceleration.y,
                                       imu->linear_acceleration.z);
    Eigen::Vector3d omega(imu->angular_velocity.x, imu->angular_velocity.y, imu->angular_velocity.z);

    controller.acceleration = linearAcceleration;
    controller.cAngularVelocity_body = omega;
    controller.rotationMatrix_BuW = quaternion.toRotationMatrix();

}//订阅当前的线加速度和四元数和角速度

void callbackTarget(const geometry_msgs::PoseConstPtr &pose)
{
    Eigen::Vector3d positionTarget(pose->position.x, pose->position.y, pose->position.z);
    Eigen::Quaterniond quaternionTarget(pose->orientation.w, pose->orientation.x, pose->orientation.y,
                                        pose->orientation.z);

    controller.updateTargetStates(positionTarget, quaternionTarget);
}//期望的位置和四元数


int main(int argc, char **argv)
{
    ros::init(argc, argv, "tracker");
    ros::NodeHandle nh;
    ros::Rate loop_rate(100);

    ros::Subscriber subIMU = nh.subscribe("/imu", 1, callbackImu);////订阅当前的线加速度和四元数和角速度（IMU）
    ros::Subscriber subOdometry = nh.subscribe("/odom", 1, callbackOdometry);//订阅当前位置和线速度(里程计)
    ros::Subscriber subTarget = nh.subscribe("/target", 1, callbackTarget);//期望的位置和四元数（期望位置）


    //多线程订阅
    ros::AsyncSpinner spinner(4); // Use 4 threads

    ros::Publisher pubRotorRevs = nh.advertise<std_msgs::Float64MultiArray>("/rotorRevs", 1);

    double massQuadcopter = 1;//无人机质量确定
    controller.initializeParameter(massQuadcopter);//初始化参数

    while (ros::ok())
    {
        {

            Eigen::Vector4d revs = controller.getRevs();

            std_msgs::Float64MultiArray revsArray;
            revsArray.data = {revs.x(), revs.y(), revs.z(), revs.w()};

            pubRotorRevs.publish(revsArray);
        }

        controller.updatePastStates();

        spinner.start();
    }

    ros::waitForShutdown();
    return 0;
}

增加可视化

coppeliasim 4.2.0中修改了可视化graph的功能，需要修改coppeliasim中usrset.txt中的showOldDlgs = false可恢复老版本的gui创建修改graph的功能。本文创建了可视化无人机xy平面位置graph，实现了无人机实际位置与期望位置的对比。

首先在coppeliasim中的add-graph创建新graph，然后修改graph脚本：

---
--- Created by caodong.
--- DateTime: 2022/3/4 
---
-- This script creates a comparison graph of the target position and the actual position in the x-y plane
graph=require('graph_customization')--no delete!
function sysCall_init()
    graphHandle=sim.getObjectHandle(sim.handle_self)--create graph handle
    targetObj = sim.getObjectHandle('Quadricopter_target')
    imu_handle = sim.getObjectHandle('IMU_link')--create IMU handle to get position
    streamx=sim.addGraphStream(graphHandle,'p_x','m',0,{1,0,0})--add streamx as p_x to graph,
    --0 for line, 2 for invisible, 4 for circle
    --{1,0,0} for color
    streamy=sim.addGraphStream(graphHandle,'p_y','m',0,{0,1,0})--add streamy as p_y to graph
    streamz=sim.addGraphStream(graphHandle,'p_z','m',0,{0,0,1})--add streamz as p_z to graph
    streamtx=sim.addGraphStream(graphHandle,'p_tx','m',0,{0.5,0.5,0})--add streamtx as p_tx to graph
    streamty=sim.addGraphStream(graphHandle,'p_ty','m',0,{0,0.5,0.5})--add streamty as p_ty to graph
    streamtz=sim.addGraphStream(graphHandle,'p_tz','m',0,{0,0.5,0.5})--add streamtz as p_tz to graph
    xyCurve=sim.addGraphCurve(graphHandle,'position output',2,{streamx,streamy},{0,0},'',4,{1,1,0})
    --create 2D graph based p_x and p_y
    txyCurve=sim.addGraphCurve(graphHandle,'tposition output',2,{streamtx,streamty},{0,0},'',4,{0,1,1})
    --create 2D graph based p_tx and p_ty
    graph.init()--no delete!
end

function sysCall_sensing()
    post = sim.getObjectPosition(targetObj,-1)-- get tposition
    pos = sim.getObjectPosition(imu_handle,-1)-- get position
    sim.setGraphStreamValue(graphHandle,streamx,pos[1])--set streamtx
    sim.setGraphStreamValue(graphHandle,streamy,pos[2])--set streamty
    sim.setGraphStreamValue(graphHandle,streamz,pos[3])--set streamtz
    sim.setGraphStreamValue(graphHandle,streamtx,post[1])--set streamx
    sim.setGraphStreamValue(graphHandle,streamty,post[2])--set streamy
    sim.setGraphStreamValue(graphHandle,streamtz,post[3])--set streamz
    graph.handle(0)--no delete!
    graph.updateCurves()--update  Curves no delete!
end

最后注意点击脚本编辑界面右上角的resstart按钮，重新编辑可视化graph。即可进行仿真