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;//beboptakeoff_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