Optitrack视觉定位下基于ROS及PX4搭建四旋翼多机飞行平台
2024-06-30 03:38:15
Optitrack视觉定位下基于ROS及PX4搭建四旋翼多机飞行平台搭建
- 1 单机平台
- 1.1 四旋翼硬件组装
- a)注意
- 1.2 机载板环境配置
- 1.3 飞控参数配置
- a)注意
- 1.4 实飞全流程
- 2 多机通信
- 2.1 多机ip地址存储
- 2.2 ROS主机地址设置
- 2.3 实飞验证
1 单机平台
首先是单机平台的搭建，简单来说分两步走
- 组装飞机，实现遥控器手飞
- 加入机载板，实现板载控制飞机自主飞行
1.1 四旋翼硬件组装
四旋翼硬件组成：
- 机架
- 桨叶
- 电池，电机，电调
- 飞控，选的pixhawkV4
- 飞控配件：安全开关，电流计等
- 接收机
- 遥控器
a)注意
- 电调有适配的电池，要根据电调参数选择合适的电池大小
- 搭建完成后要注意飞控绑结实了，否则手控飞不稳不排除飞控本身没粘牢抖动的可能
1.2 机载板环境配置
我用的树莓派4b，Ubuntu系统。还需要安装ROS操作系统，以及mavros功能包
ROS的安装教程：2021-06-01 Ubuntu 1604安装ROS+ ROS初始化失败的解决方案
mavros是ROS基于mavlink协议开发的功能包，将具体的mavlink的相关消息打包成话题方便用户使用。mavlink协议是飞控PX4采用的通信方式。具体的安装步骤：
sudo apt-get install ros-kinetic-mavros ros-kinetic-mavros-extras
wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
./install_geographiclib_datasets.sh
后两部没有实现问题也不大，实测能用
1.3 飞控参数配置
为了能够实现视觉定位，需要配置飞控参数为视觉定位
- EKF2_HGT_MODE改为vision模式- EKF2_AID_MASK勾选vision_position_fusion和vision_yaw_fusion两项，勾选结果应为24，如果用的是新版QGC可能勾选后结果会错误为7，建议用老版地面站对这一项设置
此外，为了实现飞控与树莓派之间的通信，还需要配置通信参数，要制定具体的telem通信端口，并且确定起效工作模式及波特率
飞控1.9以上版本，一般选择Telem2口作通信口，设置记载模式下接受通信，波特率921600实测够用，最高用到过3000000。
MAV_1_CONFIG = TELEM 2 (MAV_1_CONFIG is often used to map the TELEM 2 port)
MAV_1_MODE = Onboard
SER_TEL2_BAUD = 921600 (921600 or higher recommended for applications like log streaming or FastRTPS)飞控1.9以下版本0 to disable MAVLink output on TELEM2 (default)921600 to enable MAVLink output at 921600 baud, 8N1 (recommended)57600 to enable MAVLink output at 57600 baud, 8N1157600 to enable MAVLink in OSD mode at 57600 baud257600 to enable MAVLink in listen-only mode at 57600 baud
以上操作可在PX4开发者文档找到
a)注意
- 飞控1.8版本的ekf2融合有问题，飞控的自身定位数据经常会丢失重置，不建议烧
- 选择MAV_X_CONFIG都可以，匹配就行
1.4 实飞全流程
步骤：
1、开optirack，建刚体，发数据
2、树莓派起vrpn
roslaunch vrpn_cient_ros sample.launch server:=192.168.1.50//改成你自己motive发布的ip
上两步具体操作见Optitrack使用说明-基于ROS&vrpn实现的室内定位
3、树莓派起mavros，与飞控建立通信，
- 硬件：我采用的串口线连接，这里方法任选，但要确保你知道你选用的树莓派什么端口
- 软件：树莓派下起mavros：
roslaunch mavros px4.launch fcu_url:=/dev/ttyUSB0:921600
/dev/ttyUSB0为我选用的树莓派端口
921600为我在飞控里约定好的波特率
4、树莓派起坐标转换节点，接受vrpn解算出位置信息并转到mavros坐标系下
我用的阿木实验室代码改的，这里贴他们源码
5、至此单机平台通信链路搭建完，这里贴一个我的悬停代码。最后起这个控制节点，然后用遥控器切arm和offboard，能够实现定点悬停
//悬停
#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>("/uav0/mavros/state", 10, state_cb);ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("/uav0/mavros/setpoint_position/local", 10);//the setpoint publishing rate MUST be faster than 2Hzros::Rate rate(20.0);/确认和飞控连上了///while(ros::ok() && !current_state.connected){ros::spinOnce();//处理回调函数rate.sleep();//std::cout << "connected" << std::endl;//if(!current_state.connected){std::cout << "zhixing" << std::endl;}}std::cout << "connected" << std::endl;//给期望点，发geometry_msgs::PoseStamped pose;pose.pose.position.x = 0;pose.pose.position.y = 0;pose.pose.position.z = 1;//确定数据连接？？for(int i = 100; ros::ok() && i > 0; --i){local_pos_pub.publish(pose);ros::spinOnce();rate.sleep();}std::cout << "first" << std::endl;while(ros::ok()){if( current_state.mode != "OFFBOARD"){//local_pos_pub.publish(pose);std::cout << current_state.mode << std::endl;std::cout << "unchanged" << std::endl;}else std::cout << current_state.mode << std::endl;std::cout << "fei!" << std::endl;local_pos_pub.publish(pose);ros::spinOnce();rate.sleep();}return 0;
}2 多机通信
相比于单机，多机核心需求是信息的共享，我是基于树莓派采用ROS提供的主机网络实现
2.1 多机ip地址存储
先把所有飞机上的树莓派以及主机(可以是某个上的树莓派也可以是你的笔记本)简称为飞机，这一步的意义是让飞机A和飞机B存储互相的ip地址，否则当A想要调取B发布的话题时，虽然他们在共用ROS网络里能查到这个话题，但因为找不到对应的ip入口，无法读取数据。(个人总结，具体原理⑧懂)
查看每架飞机的ip名
hostname
这个名字后面要存到其他飞机里，如果不太好打可修改
sudo gedit /etc/hostname
改完需要更新网络起效
在每架飞机中存储其他飞机的ip
sudo gedit /etc/hosts
往里面填，例（中间的空格用tap）
192.168.1.21 uav1
192.168.1.22 uav2
...
2.2 ROS主机地址设置
然后为了所有飞机接入同一个网络，我们需要指定一台主机，比如以笔记本为主机，其hostname为legion，则其他从机里需做
sudo gedit ~/.bashrc
然后修改ROS主机地址
export ROS_MASTER_URI=http://legion:11311
完事source
2.3 实飞验证
1、主机起ROS网络，起vrpn，将所有飞机的位置数据发布出去
2、每架飞机起mavros和坐标转换节点，px4.launch我是以group的方式起，即加上前缀，否则一个网络下节点会冲突，坐标转换节点也是这个问题，我的launch文件：
<?xml version="1.0"?>
<launch><!-- UAV0 --><arg name="m_ID" default="fly1"/><arg name="ID_qiu" default="1"/><group ns="$(arg m_ID)"><!-- MAVROS and vehicle configs --><arg name="ID" value="0"/><arg name="fcu_url" default="/dev/ttyAMA0:921600"/><!-- MAVROS --><include file="$(find mavros)/launch/px4.launch"><arg name="fcu_url" value="$(arg fcu_url)"/><arg name="gcs_url" value=""/><arg name="tgt_system" value="$(eval 1 + arg('ID'))"/><arg name="tgt_component" value="1"/><arg name="log_output" default="screen" /><arg name="fcu_protocol" default="v2.0" /><arg name="respawn_mavros" default="false" /></include></group><!-- 这边给每个节点取名不同，且给坐标转换节点传入名字参数便于区分话题--><node pkg="m2puav_pkg" type="m2puav" name="$(arg m_ID)" ><param name="ID" type="int" value="$(arg ID_qiu)"/></node>
</launch>3、通信链路搭建完成，贴一个师弟写的虚拟点法实现的代码。和上文一样，主机起控制节点，遥控器切arm和offboard
//多机头文件
/*** the control part of multi uavs*/
#ifndef UAV_H
#define UAV_H#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <iostream>
#include <string>
#include <math.h>
#include <vector>using namespace std;double angel_bet(double x1, double y1, double x2, double y2);class UAV
{
public://Output & input pos is in the world coordination//ID_NUM of the UAVint UAV_ID;//Desired position in the formation coordinationfloat x_f;float y_f;float z_f;//dangerbool danger = false;//ROS node & subscriber & publisherros::NodeHandle nh;ros::Subscriber Position;ros::Subscriber Velocity;ros::Subscriber state_sub;ros::Publisher local_pos_pub;ros::ServiceClient arming_client;ros::ServiceClient set_mode_client;//UAV state & position & velocitymavros_msgs::State current_state;geometry_msgs::PoseStamped pos;geometry_msgs::TwistStamped ve;//ConstructorUAV() {}UAV(int _UAV_ID, float _x_f, float _y_f, int _z_f){Set_UAV(_UAV_ID, _x_f, _y_f, _z_f);}//initiate UAVvoid Set_UAV(int _UAV_ID, float _x_f, float _y_f, int _z_f){UAV_ID = _UAV_ID;x_f = _x_f;y_f = _y_f;z_f = _z_f;//Create string of uav_path e.g. "/uav0"// string uav_num = "/qiu";// string UAV_num = "/QIU";string uav_num = "/fly";string UAV_num = "/FLY";uav_num = uav_num + to_string(UAV_ID);UAV_num = UAV_num + to_string(UAV_ID);//ROS definitionPosition = nh.subscribe<geometry_msgs::PoseStamped>("/vrpn_client_node" + UAV_num + "/pose", 10, boost::bind(&UAV::UAV_INFO_P, this, _1));Velocity = nh.subscribe<geometry_msgs::TwistStamped>(uav_num + "/mavros/local_position/velocity_local", 10, boost::bind(&UAV::UAV_INFO_V, this, _1));state_sub = nh.subscribe<mavros_msgs::State>(uav_num + "/mavros/state", 10, boost::bind(&UAV::state_cb, this, _1));local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>(uav_num + "/mavros/setpoint_position/local", 10);arming_client = nh.serviceClient<mavros_msgs::CommandBool>(uav_num + "/mavros/cmd/arming");set_mode_client = nh.serviceClient<mavros_msgs::SetMode>(uav_num + "/mavros/set_mode");}//Output desired position, consider the offsetvoid send_pos(geometry_msgs::PoseStamped &pos_d){local_pos_pub.publish(pos_d);};//State callback fcnvoid state_cb(const mavros_msgs::State::ConstPtr &msg){current_state = *msg;}//Position callback fcnvoid UAV_INFO_P(const geometry_msgs::PoseStamped::ConstPtr &msg){pos = *msg;}//Velocity callback fcnvoid UAV_INFO_V(const geometry_msgs::TwistStamped::ConstPtr &msg){ve = *msg;}}; //class UAV endclass VP_desired
{
public:float x;float y;float z;float psi;float vx;float vy;float vz = 0.3;float circle_rad = 0.2;ros::NodeHandle nh;ros::Subscriber Target;geometry_msgs::PoseStamped position;//constructorVP_desired(float _psi, float _x, float _y, float _z, float _vx, float _vy){set_vp_desired(_psi, _x, _y, _z, _vx, _vy);}void set_vp_desired(float _psi, float _x, float _y, float _z, float _vx, float _vy){x = _x;y = _y;z = _z;psi = _psi;vx = _vx;vy = _vy;Target = nh.subscribe<geometry_msgs::PoseStamped>("/vrpn_client_node/TARGET/pose", 10, boost::bind(&VP_desired::target_cb,this,_1));}void target_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){position = *msg;}void reverse_position(UAV &uav){x = uav.pos.pose.position.x - uav.x_f * cos(psi) + uav.y_f * sin(psi);y = uav.pos.pose.position.y - uav.y_f * cos(psi) - uav.x_f * sin(psi);z = uav.pos.pose.position.z;cout << "x:" << x << " y:" << y << " z:" << z <<"\r\n";}void VP_desir_pose_update(int state, float hz){float point_angel=0;switch (state){case 2: //起飞z += vz / hz;if (z >= 1)z = 1 ;break;case 3: //直线vx = 0.6;x += vx / hz;break;case 4: //圆形point_angel = angel_bet(x - 0.0, y - 0.0, 0.0, -1.0); //角速度0.2rad/s，半径3m,circle_rad / hzvx = circle_rad * sqrt((0.0 - x) * (0.0 - x) + (0.0 - y) * (0.0 - y)) * cos(point_angel);vy = circle_rad * sqrt((0.0 - x) * (0.0 - x) + (0.0 - y) * (0.0 - y)) * sin(point_angel);x += vx / hz;y += vy / hz;z = 1.5;break;case 5: //降落z -= vz / hz;if (z <= 0)z = 0;break;case 6://悬停break;case 7://追人x=position.pose.position.x;y=position.pose.position.y;break;case 10:break;default:ROS_INFO("Waiting State! ");break;}cout << "期望点"<<" x:" << x << " y:" << y << " z:" << z <<"\r\n";}}; //class VP_desired endclass VirtualPoint
{
public:float psi;float x;float y;float z;float vx = 0;float vy = 0;ros::NodeHandle nh;ros::Publisher pos_vp ;geometry_msgs::PoseStamped position;//ConstructorVirtualPoint(float _psi, float _x, float _y, float _z){psi = _psi;x = _x;y = _y;z = _z;pos_vp=nh.advertise<geometry_msgs::PoseStamped>("/vp/pose",10);}//Virtual point controllervoid VP_Controller(UAV &uav, geometry_msgs::PoseStamped &pos_d){pos_d.pose.position.x = uav.x_f * cos(psi) - uav.y_f * sin(psi) + x; maybe wrongpos_d.pose.position.y = uav.y_f * cos(psi) + uav.x_f * sin(psi) + y;if (uav.danger)pos_d.pose.position.z = 0;elsepos_d.pose.position.z = z + uav.z_f;position.pose.position.x=x;position.pose.position.y=y;position.pose.position.z=z;pos_vp.publish(position);}
}; //class VirtualPoint endclass ArtificialField
{
public:float a0;float k0;float k1;float k2;float r0;float r_0;//ConstructorArtificialField(float (&af_para)[5]){a0 = af_para[0];k0 = af_para[1];k1 = af_para[2];k2 = af_para[3];r0 = af_para[4];r_0 = r0 + k0 * a0;}//saturation fcnvoid sat(geometry_msgs::PoseStamped &pos){float x = pos.pose.position.x;float y = pos.pose.position.y;float norm = sqrt(x * x + y * y);if (norm > a0){pos.pose.position.x *= a0 / norm;pos.pose.position.y *= a0 / norm;}}void sat(float &x, float &y){float norm = sqrt(x * x + y * y);if (norm > a0){x *= a0 / norm;y *= a0 / norm;}}
}; //class ArtificialField endclass AF_VP : public ArtificialField
{
public:float obs_x;float obs_y;//constructorAF_VP(float _obs_x, float _obs_y, float (&af_para)[5]) : ArtificialField(af_para){obs_x = _obs_x;obs_y = _obs_y;}//controllervoid AF_controller(VirtualPoint &vp, VP_desired &vp_d){float ksi_wp_x = vp_d.x + k0 * vp_d.vx;float ksi_wp_y = vp_d.y + k0 * vp_d.vy;float ksi_vp_x = vp.x + k0 * 0; big problem!!!! no velocity feedbackfloat ksi_vp_y = vp.y + k0 * 0;float ksi_obs_x = obs_x;float ksi_obs_y = obs_y;float _ksi_wp_x = ksi_vp_x - ksi_wp_x;float _ksi_wp_y = ksi_vp_y - ksi_wp_y;float _ksi_obs_x = ksi_vp_x - ksi_obs_x;float _ksi_obs_y = ksi_vp_y - ksi_obs_y;float a = k1;float norm_ksi_obs = sqrt(_ksi_obs_x * _ksi_obs_x + _ksi_obs_y * _ksi_obs_y);float offset = 0.000001;float b = k2 / ((norm_ksi_obs - r_0) * (norm_ksi_obs - r_0) + offset) / (norm_ksi_obs + offset);float tmp_x = -a * _ksi_wp_x ;//+ b * _ksi_obs_x;float tmp_y = -a * _ksi_wp_y ;//+ b * _ksi_obs_y;sat(tmp_x, tmp_y);vp.x += tmp_x/10;vp.y += tmp_y/10;vp.z = vp_d.z;vp.psi = vp_d.psi;cout <<"跟随点"<<" x:" << vp.x << " y:" << vp.y << " z:" << vp.z <<"\r\n";}
}; //class AF_VP endclass AF_formation : public ArtificialField
{
public://constructorAF_formation(float (&af_para)[5]) : ArtificialField(af_para){}//controllervoid AF_Controller(UAV &uav, vector<UAV> &uav_avoid, geometry_msgs::PoseStamped &pos_d){float ksi_x = uav.pos.pose.position.x + k0 * uav.ve.twist.linear.x;float ksi_y = uav.pos.pose.position.y + k0 * uav.ve.twist.linear.y;pos_d.pose.position.x = 0;pos_d.pose.position.y = 0;pos_d.pose.position.z = uav.z_f;for (auto ptr = uav_avoid.begin(); ptr != uav_avoid.end(); ptr++){float ksi_tmp_x = ptr->pos.pose.position.x + k0 * ptr->ve.twist.linear.x;float ksi_tmp_y = ptr->pos.pose.position.x + k0 * ptr->ve.twist.linear.x;ksi_tmp_x = ksi_x - ksi_tmp_x;ksi_tmp_y = ksi_y - ksi_tmp_y;float norm_ksi = sqrt(ksi_tmp_x * ksi_tmp_x + ksi_tmp_y * ksi_tmp_y);float offset = 0.000001;float b = k2 / ((norm_ksi - r_0) * (norm_ksi - r_0) + offset) / (norm_ksi + offset);pos_d.pose.position.x += b * ksi_tmp_x;pos_d.pose.position.y += b * ksi_tmp_y;}sat(pos_d);pos_d.pose.position.x += uav.pos.pose.position.x;pos_d.pose.position.y += uav.pos.pose.position.y;}
}; //class AF_formation endclass FormationDistance
{
public:vector<UAV> formation;vector<vector<float>> distance;int num;float safe_distance;//ConstructorFormationDistance(vector<UAV> _formation, float _safe_distance){int len = _formation.size();num = len;formation = _formation;distance = vector<vector<float>>(len, vector<float>(len, -1));safe_distance = _safe_distance;}//Calculate distancevoid GetDistance(){for (int i = 0; i < num; i++){for (int j = i + 1; j < num; j++){distance[i][j] = sqrt((formation[i].pos.pose.position.x - formation[j].pos.pose.position.x) *(formation[i].pos.pose.position.x - formation[j].pos.pose.position.x) +(formation[i].pos.pose.position.y - formation[j].pos.pose.position.y) *(formation[i].pos.pose.position.y - formation[j].pos.pose.position.y));}}}//Find if there are distance bigger than safe_distancebool HasAF(int id){bool has_af = false;for (int i = 0; i < num; i++){if (distance[i][id] < safe_distance || distance[id][i] < safe_distance){has_af = true;break;}}return has_af;}//Find UAVs whose distance with uav(id) are bigger than safe_distancevoid IsAF(int id, vector<UAV> &af_uav){for (int i = 0; i < num; i++){if (distance[i][id] < safe_distance || distance[id][i] < safe_distance){af_uav.push_back(formation[i]);}}}
}; //class FormationDistance endbool IsConnected(vector<UAV> &uav_formation)
{for (auto ptr = uav_formation.begin(); ptr != uav_formation.end(); ptr++){if (ptr->current_state.connected == false)return false;}return true;
}bool IsOffboard(vector<UAV> &uav_formation)
{for (auto ptr = uav_formation.begin(); ptr != uav_formation.end(); ptr++){if (ptr->current_state.mode != "OFFBOARD")return false;}return true;
}bool IsLand(vector<UAV> &uav_formation)
{for (auto ptr = uav_formation.begin(); ptr != uav_formation.end(); ptr++){if (ptr->current_state.mode != "AUTO.LAND")return false;}return true;
}bool IsArming(vector<UAV> &uav_formation)
{for (auto ptr = uav_formation.begin(); ptr != uav_formation.end(); ptr++){if (ptr->current_state.armed == false)return false;}return true;
}bool IsHieghtSafe(vector<UAV> &uav_formation)
{float safe_high = 3;for (auto ptr = uav_formation.begin(); ptr != uav_formation.end(); ptr++){if (ptr->pos.pose.position.z >= safe_high){return false;}}return true;
}double angel_bet(double x1, double y1, double x2, double y2)
{double multi = x1 * x2 + y1 * y2;double fenmu1 = sqrt(x1 * x1 + y1 * y1);double fenmu2 = sqrt(x2 * x2 + y2 * y2);if (x1 > 0){return acos(multi / (fenmu1 * fenmu2));}else{return 2 * M_PI - acos(multi / (fenmu1 * fenmu2));}
}#endif//主机控制节点
/*** @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 "cen_fly/uav.h"using namespace std;#define TTY_PATH "/dev/tty"
#define STTY_US "stty raw -echo -F "
#define STTY_DEF "stty -raw echo -F "//read from keyboard
int get_char()
{fd_set rfds;struct timeval tv;int ch = -1;FD_ZERO(&rfds);FD_SET(0, &rfds);tv.tv_sec = 0;tv.tv_usec = 0; //设置等待超时时间//检测键盘是否有输入if (select(1, &rfds, NULL, NULL, &tv) > 0){ch = getchar();}return ch;
}int main(int argc, char **argv)
{ros::init(argc, argv, "test");//one_test// int num_of_uav = 1;// int id[num_of_uav] = {6};// float x_f[num_of_uav] = {1};// float y_f[num_of_uav] = {0};// float z[num_of_uav] = {0};//initiate uav para// int num_of_uav = 6;// int id[num_of_uav] = {1, 2, 3, 4, 5, 6};// float x_f[num_of_uav] = {2, 2, 0, 0, -2, -2};// float y_f[num_of_uav] = {1.5, -1.5, 1.5, -1.5, 1.5, -1.5};// float z[num_of_uav] = {0, 0, 0, 0, 0, 0};// float x_f_new[num_of_uav] = {2, 2, 0, 0, -2, -2};// float y_f_new[num_of_uav] = {1, -1, 2, -2, 1, -1};//float z[num_of_uav] = {0, 0, 0, 0, 0, 0};int num_of_uav = 5;int id[num_of_uav] = {1, 3, 4, 5, 6};float x_f[num_of_uav] = {0, 0, 0, -2, -2};float y_f[num_of_uav] = {0, 1.5, -1.5, 1.5, -1.5};float z[num_of_uav] = {0, 0, 0, 0, 0, 0};float x_f_new[num_of_uav] = {2, 0, 0, -2, -2};float y_f_new[num_of_uav] = {0, 1, -1, 2, -2};initiate uav formationvector<UAV> uav_formation(num_of_uav);for (int i = 0; i < num_of_uav; i++){uav_formation[i].Set_UAV(id[i], x_f[i], y_f[i], z[i]);}VirtualPoint initiateVirtualPoint vp(0.0, 0, 3, 0);VP_desired vp_d(0.0, 0, 3, 0, 0, 0);// VirtualPoint vp(0.0, 2, -1, 0);// VP_desired vp_d(0.0, 2, -1, 0, 0, 0);float vp_af_para[5] = {0.3, 5.5, 100, 1.0, 1};AF_VP vp_af(200, 200, vp_af_para);set ros rate 20hzros::Rate rate(20.0);cout << "uav formation begin!" << endl;send a few set_points before startinggeometry_msgs::PoseStamped mypos;//读取指令int state = 1; //initial state is waitingconst int waiting = 1;const int takeoff = 2;const int line = 3;const int circle = 4;const int land = 5;const int hover = 6;const int chase = 7;const int change=8;bool emergency_hover = false;while (ros::ok() && !IsConnected(uav_formation)){ros::spinOnce();rate.sleep();}cout << "connected !" << endl;cout << "mission begin!" << endl;while (ros::ok()){get_char initiateint ch = 0;system(STTY_US TTY_PATH);//最好能打印状态float distance_tmp[num_of_uav];keyboard hover & height protectch = get_char();//cout<<ch<<"\r\n";if (ch == 'w'){state = 1;}if (ch == 'u'){state = 2;}if (ch == 'l'){state = 3;}if (ch == 'y'){state = 4;}if (ch == 'd'){state = 5;emergency_hover = false;}if (ch == 'h'){state = 6;emergency_hover = true;}if (ch == 'c'){state = 7;}if (ch == 'b'){state = 8;}if (ch == 'k'){system(STTY_DEF TTY_PATH);cout << "program end"<< "\r\n";return 0;}if (ch == 's' || !IsHieghtSafe(uav_formation)){state=10;emergency_hover = true;cout << "Stopped!"<< "\r\n";}if (emergency_hover == true){for (int i = 0; i < num_of_uav; i++){vp.VP_Controller(uav_formation[i], mypos);uav_formation[i].send_pos(mypos);}goto ROS_UPDATE;}switch (state){case waiting:cout << "Waiting..."<< "\r\n";break;case takeoff:cout << "Takeoff"<< "\r\n";break;case line:cout << "Line"<< "\r\n";break;case circle:cout << "Circle"<< "\r\n";break;case land:cout << "Landing"<< "\r\n";break;case hover:cout << "Hovering"<< "\r\n";break;case chase:cout << "Chasing"<< "\r\n";break;default:cout << "no such command!"<< "\r\n";break;}for (int i = 0; i < num_of_uav; i++){cout << "| uav" << i << " : " << uav_formation[i].current_state.mode << " | ";}cout << "\r\n";if (state == 1){vp_d.reverse_position(uav_formation[0]);vp_d.VP_desir_pose_update(state, 20);vp_af.AF_controller(vp, vp_d);for (int i = 0; i < num_of_uav; i++){vp.VP_Controller(uav_formation[i], mypos);uav_formation[i].send_pos(mypos);}ros::spinOnce();rate.sleep();}else if (state == 6){vp_d.reverse_position(uav_formation[0]);vp_d.VP_desir_pose_update(state, 20);vp_af.AF_controller(vp, vp_d);for (int i = 0; i < num_of_uav; i++){vp.VP_Controller(uav_formation[i], mypos);uav_formation[i].send_pos(mypos);}}else if (state == 8){vp_d.VP_desir_pose_update(state, 20);vp_af.AF_controller(vp, vp_d);for (int i = 0; i < num_of_uav; i++){uav_formation[i].Set_UAV(id[i], x_f_new[i], y_f_new[i], z[i]);vp.VP_Controller(uav_formation[i], mypos);uav_formation[i].send_pos(mypos);}}else{vp_d.VP_desir_pose_update(state, 20);vp_af.AF_controller(vp, vp_d);for (int i = 0; i < num_of_uav; i++){vp.VP_Controller(uav_formation[i], mypos);uav_formation[i].send_pos(mypos);}}ROS_UPDATE:ros::spinOnce();rate.sleep();}return 0;
}Optitrack视觉定位下基于ROS及PX4搭建四旋翼多机飞行平台相关推荐
- Ubuntu18.04下基于ROS和PX4的无人机仿真平台的基础配置搭建（XTDrone的）
摘自:https://www.ngui.cc/51cto/show-23557.html Ubuntu18.04下基于ROS和PX4的无人机仿真平台的基础配置搭建 编程学习 · 2020/7/12 1 ...
- Ubuntu18.04基于ROS和PX4的仿真平台配置
1.前言 作者只是一名双非本科院校飞控专业的大二学生,想以此记录一下自学飞控的经历,也希望能给刚刚入门的同学一些微薄的帮助. 这个环境的安装可以说是西天取经一般,但安装完后发现如果有领路的人,其实花费 ...
- 基于NVIDIA Xavier NX(ubuntu20.04)的Optitrack视觉定位 PX4+ros noetic(实物运行记录)
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档 文章目录 前言 一:硬件准备 两种界面化显示的方式 无线连接 有线连接 二:软件准备 1:远程登录软件 NoMachine 2:安装r ...
- windows平台下基于QT和OpenCV搭建图像处理平台
在之前的博客中,已经分别比较详细地阐述了"windows平台下基于VS和OpenCV"以及"Linux平台下基于QT和OpenCV"搭建图像处理框架,并 ...
- 【无人机】基于蒙特卡洛和控制算法实现四旋翼无人器拾物路径规划附matlab代码
1 内容介绍 四旋翼无人机飞行器(Unmanned Aerial Vehicle, UAV)是一种旋翼式直升机,它具有四个控制输入和六个控制输出,因此四旋翼无人机是一个欠驱动的旋翼直升机.四旋翼无人机 ...
- 论文速递：一种用于视觉定位的基于NLP思路的直线特征匹配算法
标题:Line as a Visual Sentence:Context-aware Line Descriptor for Visual Localization 作者:Sungho Yoon1 a ...
- ROS实验笔记之——自主搭建四旋翼无人机
最近搭建了一台小的四旋翼无人机,本博文记录一下搭建的过程以及一些问题. 请问我博客就记录了自己做实验的搭建的飞机有什么问题??? 目录 组装 飞行前准备 试飞 组装 首先是一系列的散装原件. 到最后搭 ...
- 先进非线性控制方法 INDI 快速部署到PX4用于四旋翼控制（part1）
关于INDI的前言: 增量型非线性控制方法是一种2010年以后才流行起来的非线性控制方法,目前主要用于设计先进飞行控制律.这种方法的典型代表是增量型动态逆(incremental nonlinear ...
- 基于ROS的PX4+Gazebo仿真——PX4一键起飞及飞行控制
一键起飞 参考及引用 1. CSDN博主「战争果子」的原创文章,遵循CC 4.0 BY-SA版权协议. 原文:https://blog.csdn.net/EnthusiasmZing/article/ ...
最新文章
- C# WPF动画——小游戏
- 【半译】在ASP.NET Core中创建内部使用作用域服务的Quartz.NET宿主服务
- hbase 和 zookeeper建立连接_我对Hbase的一些理解——HMaster与Zookeeper之间的交互机制...
- 最全的TCP面试知识点
- Ubuntu GitHub操作——使用仓库
- 关于渗透测试以及网络安全法
- 【双轨】加权分红+三级分销+见点奖+级差源码系统 演示网站介绍
- 凸优化1——仿射集、凸集、锥
- JavaScript随机方块
- CAD快速打印（批量打印）PDF/DWF/JPG/PNG\EPS/PLT：任意尺寸、纯命令
- 最好的60个国外壁纸网站
- Android百度地图之定位图层
- Kong 优雅实现微服务网关鉴权，登录场景落地实战篇
- 线程池、volatile、原子性、并发工具类
- 二元光学透镜的分光成像、消色差原理
- 论文选读：Maximizing the Spread of Cascades Using Network Design
- Java Clob 类型转 String
- 编译Android下可执行命令的FFmpeg
- 对新入职员工五个问题的简答
- 【最全面的】71张图详解IP 地址、IP 路由、分片和重组、三层转发、ARP、ICMP