机载电脑 Jeston Tx2 Xaiver NX 与 APM飞控 通信

//文件夹所在位置
/opt/ros/melodic/share/mavros/launch

	
	

	 //波特率
	
	
	
	
	
	

	 //节点文件位置
		
		

		
		
		
		
		
		
		
	

 1.ROS与飞控通信（必须）：

sudo chmod 777 /dev/ttyACM0 //端口权限

roslaunch mavros px4.launch _fcu_url:=/dev/ttyACM0:57600 //px4固件连接
roslaunch mavros apm.launch _fcu_url:=/dev/ttyACM0:57600 //apm固件连接 我们用这个

2.飞控数据回传机载电脑（解锁后可控 Offboard模式）

采用ROS_C++ Mavros库编写 接收IMU、位置、状态模式、遥控器、速度等数据。

#include 
#include
using namespace std;
#include 
#include   //发布的消息体对应的头文件，该消息体的类型为geometry_msgs：：PoseStamped
#include   //CommandBool服务的头文件，该服务的类型为mavros_msgs：：CommandBool
#include      //SetMode服务的头文件，该服务的类型为mavros_msgs：：SetMode
#include   //订阅的消息体的头文件，该消息体的类型为mavros_msgs：：State
#include 
#include  
#include
//建立一个订阅消息体类型的变量，用于存储订阅的信息
mavros_msgs::State current_state;//无人船状态
sensor_msgs::Imu imu_msg;//IMU 数据
geometry_msgs::Quaternion orientation1;//四元数数据
mavros_msgs::ManualControl manual_msg;//遥控器数据
geometry_msgs::TwistStamped velocitys;//速度
geometry_msgs::Vector3 linears;//线速度 没用
geometry_msgs::Twist twists;//消息 速度
geometry_msgs::PoseStamped local_pos;//位置信息 接收当前位置
//订阅时的回调函数，接受到该消息体的内容时执行里面的内容，这里面的内容就是赋值
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
    //飞控 状态读取
    ROS_INFO("Offboard msg %d",current_state.armed);//打开模式后打印信息
}
 
void manual_cb(const mavros_msgs::ManualControl::ConstPtr& msg)
{
    //遥控器按键值读取  无返回
    manual_msg = *msg;
    ROS_INFO("get the manual : %f",manual_msg.x);
}
// 
// sensor_msgs::Imu::orientation
void imu_cb(const sensor_msgs::Imu::ConstPtr& msg)
{
    //陀螺仪 四元数读取
    imu_msg = *msg;
    orientation1 = imu_msg.orientation;
    ROS_INFO("get the imu x : %f",orientation1.x);
    ROS_INFO("get the imu y : %f",orientation1.y);
}

// 订阅的无人机当前位置数据
void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    local_pos = *msg;
    //local_pos.pose.position.x x坐标
}

void vel_cb(const geometry_msgs::TwistStamped::ConstPtr& msg)
{
    //速度读取
    velocitys = *msg;
    twists = velocitys.twist;
    ROS_INFO("get the linears x : %f",twists.linear.x);
    ROS_INFO("get the linears y : %f",twists.linear.y);
}
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node"); //ros系统的初始化，最后一个参数为节点名称
    ros::NodeHandle nh;
 
    //订阅。<>里面为模板参数，传入的是订阅的消息体类型，（）里面传入三个参数，分别是该消息体的位置、缓存大小（通常为1000）、回调函数
    ros::Subscriber state_sub = nh.subscribe("mavros/state", 10, state_cb);
    ros::Subscriber manual_sub = nh.subscribe("mavros/manual_control/control", 10, manual_cb);
    ros::Subscriber imu_sub = nh.subscribe("mavros/imu/data", 10, imu_cb);
    ros::Subscriber vel_sub = nh.subscribe("mavros/local_position/velocity", 10, vel_cb);
   // 订阅无人机当前位置（反馈消息） 
    ros::Subscriber local_pos_sub = nh.subscribe
            ("mavros/local_position/pose", 10, local_pos_cb);
    // 发布无人机速度（控制） 
    ros::Publisher vec_pub = nh.advertise
            ("mavros/setpoint_velocity/cmd_vel_unstamped", 10);
    // 位置控制 发布之前需要公告，并获取句柄，发布的消息体的类型为：geometry_msgs::PoseStamped
    ros::Publisher local_pos_pub = nh.advertise("mavros/setpoint_position/local", 10);
 
    //启动服务1，设置客户端（Client）名称为arming_client，客户端的类型为ros::ServiceClient，
    //启动服务用的函数为nh下的serviceClient<>()函数，<>里面是该服务的类型，（）里面是该服务的路径
    ros::ServiceClient arming_client = nh.serviceClient("mavros/cmd/arming");
 
    //启动服务2，设置客户端（Client）名称为set_mode_client，客户端的类型为ros::ServiceClient，
    //启动服务用的函数为nh下的serviceClient<>()函数，<>里面是该服务的类型，（）里面是该服务的路径
    ros::ServiceClient set_mode_client = nh.serviceClient("mavros/set_mode");
 
    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);
 
    // 等待飞控连接mavros，current_state是我们订阅的mavros的状态，连接成功在跳出循环
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        ROS_INFO("Offboard disenabled");//打开模式后打印信息
        rate.sleep();
    }
 
 
    //先实例化一个geometry_msgs::PoseStamped类型的对象，并对其赋值，最后将其发布出去
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;
    //先实例化一个geometry_msgs::Twist类型的对象，并对其赋值，最后将其发布出去
    geometry_msgs::Twist vector;
    vector.linear.x = 0.0;
    vector.linear.y = 0.0;
    vector.linear.z = 0.0;
 
    //建立一个类型为SetMode的服务端offb_set_mode，并将其中的模式mode设为"OFFBOARD"，作用便是用于后面的
    //客户端与服务端之间的通信（服务）
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
 
    //建立一个类型为CommandBool的服务端arm_cmd，并将其中的是否解锁设为"true"，作用便是用于后面的
    //客户端与服务端之间的通信（服务）
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
 
    //更新时间
    ros::Time last_request = ros::Time::now();
 
    while(ros::ok())//进入大循环
    {
        //首先判断当前模式是否为offboard模式，如果不是，则客户端set_mode_client向服务端offb_set_mode发起请求call，
        //然后服务端回应response将模式返回，这就打开了offboard模式
        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();

            ROS_INFO("Vehicle Offboard ");//解锁后打印信息
            cout<<"Mode: "< ros::Duration(5.0)))
            {//给飞控解锁
                if( arming_client.call(arm_cmd) && arm_cmd.response.success)
                {
                    ROS_INFO("Vehicle not armed");//解锁后打印信息
                }
                last_request = ros::Time::now();
            }

            // ROS_INFO("Vehicle  armed");//解锁后打印信息

            else //  解锁成功
            {
                vector.linear.x = 1.0;
                vector.linear.y = 1.0;
                vector.linear.z = 1.0;
                vec_pub.publish(vector);

            }
        }
 
        //local_pos_pub.publish(pose); //发布位置信息，所以综上飞机只有先打开offboard模式然后解锁才能飞起来
         // 发布位置控制信息
        
        ros::spinOnce();
        rate.sleep();
    }
 
    return 0;
}