ros+ Optitrack控制bebop2无人机

准备工作：

Motive的安装及使用：https://blog.csdn.net/weixin_41536025/article/details/89913961

ros订阅 OptiTrack 的消息通过vrpn_client_ros包：http://wiki.ros.org/vrpn_client_ros

bebop2 开发包安装指导：https://bebop-autonomy.readthedocs.io/en/latest/installation.html

注：官方的安装步骤在Ubuntu 18.04下运行可能会报错

解决方法：

         在当前工作空间的devel/lib文件夹下新建一个链接，链接到/opt/ros/melodic/lib/parrot_arsdk文件夹，并将~/bebop_ws/devel/lib/parrot_arsdk添加到$LD_LIBRARY_PATH

$ cd ~/bebop_ws/devel/lib
$ ln -s /opt/ros/melodic/lib/parrot_arsdk parrot_arsdk
$ echo "export LD_LIBRARY_PATH="~/bebop_ws/devel/lib/parrot_arsdk:$LD_LIBRARY_PATH"" >> ~/.bashrc
$ source ~/.bashrc

          参考： https://github.com/AutonomyLab/bebop_autonomy/issues/170         

  中文版安装指导：https://www.ncnynl.com/category/ros-bebop/

（安装过程中遇到问题可百度查询）

总体思路：

1、安装Motive的主机、控制器(自己的电脑)、bebop无人机 三者连接在同一个局域网

2、Motive通过 VRPN 发布无人机位置信息

3、控制器 通过订阅VRPN 的topic “vrpn_client_node/xxx/pose” 获取无人机的具体位置

      注：xxx是你在Motive中创建的刚体的名称

4、控制器 通过向topic xxx/takeoff  发送起飞命令

                                        xxx/land  发送降落命令命令

                                        xxx/cmd_vel  发送运动控制命令

       注：xxx 代表bebop无人机的命名，在 bebop_ws/src/bebop_autonomy/bebop_driver/launch/bebop_node.launch中修改。

                                在namespace后 修改名称            用于订阅消息

                                在ip 后 修改为bebop的ip地址      用于连接bebop

Ps：如何给bebop刷固件使其连接特定的wifi，并固定其ip地址，我暂时还没找到，后面补上

---

更新：给bebop刷固件使其连接特定的wifi 参考 bebop2连接到路由器并固定ip

具体程序实现

1、定义一个类

因为要控制两架bebop,所以定义一个通用的控制类实现无人机的基本操作。

class bebopControl
{
public:
    bebopControl();

    ros::NodeHandle nh;

    geometry_msgs::Twist cmd_vel;
    
    ros::Publisher takeoff_pub;
    ros::Publisher land_pub;
    ros::Publisher cmd_pub;

    ros::Subscriber vrpn_bebop_sub;

    ros::Rate *rate;
    ros::Time start_time;

    float current_x,current_y,current_z;
    void takeoff();
    void land();
    void cmd(float x,float y,float z);
    void vrpn_bebop_cb(const geometry_msgs::PoseStamped::ConstPtr& msg);
    void start();

private:
    /* data */
};

2、bebop无人机控制

Motive中创建的刚体的名称：bebop4  ==> vrpn_client_node/bebop4/pose

bebop无人机的名称：bebop4  ==> bebop4/takeoff  bebop4/land   bebop4/cmd_vel

using namespace std;
#include <iostream>
#include <stdio.h>
#include <ros/ros.h>
//topic
#include <std_msgs/Empty.h>
#include <std_msgs/Int32.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>

#include <bebop_control/bebop_control.h>


bebopControl::bebopControl()
{
    ROS_INFO("bebop  init! ");
    rate = new ros::Rate(10.0);
    current_x = 0;
    current_y = 0;
    current_z = 0;
    //bebop
    takeoff_pub= nh.advertise<std_msgs::Empty>("bebop4/takeoff", 1);         // 发布 起飞命令
    land_pub   = nh.advertise<std_msgs::Empty>("bebop4/land", 1);            // 发布 降落命令
    cmd_pub    = nh.advertise<geometry_msgs::Twist>("bebop4/cmd_vel", 1);    // 发布 移动命令
    vrpn_bebop_sub = nh.subscribe<geometry_msgs::PoseStamped>("vrpn_client_node/bebop4/pose", 1, &bebopControl::vrpn_bebop_cb,this);
    
}
/*起飞指令*/
void bebopControl::takeoff()
{
    takeoff_pub.publish(std_msgs::Empty());
    ROS_INFO("bebop  takeoff! ");
}
/*降落指令*/
void bebopControl::land()
{
    land_pub.publish(std_msgs::Empty());
    ROS_INFO("bebop  land! ");
}
/*发布位置控制指令*/
void bebopControl::cmd(float x,float y,float z)
{
    cmd_vel.linear.x = x;
    cmd_vel.linear.y = y;
    cmd_vel.linear.z = z;
    cmd_pub.publish(cmd_vel);
    ROS_INFO("bebop  send cmd! ");
}
/*通过vrpn接受bebop位置消息*/
void bebopControl::vrpn_bebop_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    /*Motive通过 VRPN 发布的位置消息 单位是 米
      ×1000 转化为mm
    */
    current_x = msg->pose.position.x*1000;
    current_y = msg->pose.position.y*1000;
    current_z = msg->pose.position.z*1000;
    cout <<"vrpn position:  " << current_x  <<"   " << current_y <<"   " << current_z << endl;

}
void bebopControl::start()
{
    
    ros::Duration(3).sleep();//必须延时一段时间，不然飞不起来
    this->takeoff();        //起飞
    ros::Duration(3).sleep();
    ros::spinOnce();        //向上飞（此处只有一个动作，持续向上飞需要连续发布此消息）
    this->cmd(0,0,0.2);
    ros::Duration(3).sleep();
    ros::spinOnce();
    this->cmd(0,0,0.2);
    ros::Duration(3).sleep();
    ros::spinOnce();
    this->cmd(0,0,0.2);
    // this->cmd(1,0,0);       //向前飞（机头下沉1度，此处只会产生一个动作，持续向前飞需要连续发布）
    // ros::Duration(3).sleep();
    // this->cmd(0,0,-0.6);
    // ros::Duration(5).sleep();
    // this->land();
    // this->cmd(0,0,0.1);
    // ros::Duration(3).sleep();

    while(ros::ok())
    {
        /*高度小于55cm直接降落 
          注意：  Motive的y轴是垂直向上的
        */
        if(current_y<=550 && current_y>0)
        {
            this->land();
            break;
        }
        /*以0.1m/s的速度下降*/
        this->cmd(0,0,-0.1);
        ros::spinOnce();
        rate->sleep();
    }

}
int main(int argc, char **argv)
{   
    ros::init(argc, argv, "bebopLeader");
    
    bebopControl bebopLeader;
    bebopLeader.start();

    return 0;
}

这里只贴了一个无人机的控制程序，另一个无人机的控制程序与之类似，只需修改订阅的话题即可。

3、修改vrpn_client_ros/launch/sample.launch文件

ip地址修改为安装Motive的主机的ip地址

4、飞行

通过Optitrack获得刚体（bebop4,bebop5）的姿态信息，并传给ROS

$ roslaunch vrpn_client_ros sample.launch

连接 bebop

新开一个终端 连接bebop4
$ roslaunch bebop_driver bebop4_node.launch
新开一个终端 连接bebop5
$ roslaunch bebop_driver bebop5_node.launch

运行自己的节点控制无人机起飞

rosrun bebop_control bebopLeader