//文件夹所在位置
/opt/ros/melodic/share/mavros/launch
<launch>
<!-- vim: set ft=xml noet : -->
<!-- example launch script for ArduPilot based FCU's -->
<arg name="fcu_url" default="/dev/ttyACM0:57600" /> //波特率
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<arg name="fcu_protocol" default="v2.0" />
<arg name="respawn_mavros" default="false" />
<include file="$(find mavros)/launch/node.launch"> //节点文件位置
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
<arg name="log_output" value="$(arg log_output)" />
<arg name="fcu_protocol" value="$(arg fcu_protocol)" />
<arg name="respawn_mavros" value="$(arg respawn_mavros)" />
</include>
</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 <ros/ros.h>
#include<iostream>
using namespace std;
//消息类型 及其头文件
#include <geometry_msgs/TwistStamped.h>//速度
#include <geometry_msgs/PoseStamped.h> //发布的消息体对应的头文件，该消息体的类型为geometry_msgs：：PoseStamped 位置
#include <mavros_msgs/CommandBool.h> //CommandBool服务的头文件，该服务的类型为mavros_msgs：：CommandBool 命令
#include <mavros_msgs/SetMode.h> //SetMode服务的头文件，该服务的类型为mavros_msgs：：SetMode 模式设置
#include <mavros_msgs/State.h> //订阅的消息体的头文件，该消息体的类型为mavros_msgs：：State 飞控模式
#include <mavros_msgs/ManualControl.h>//遥控器 消息类型与头文件
#include <sensor_msgs/NavSatFix.h>//GPS数据
#include <sensor_msgs/Imu.h> //Imu数据
#include<geometry_msgs/Twist.h>
//建立一个订阅消息体类型的变量，用于存储订阅的信息 中间变量
mavros_msgs::State current_state;//无人船状态
sensor_msgs::Imu imu_msg;//IMU 数据
sensor_msgs::NavSatFix gps_msg;//GPS 数据
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);
}
//GPS 数据 经纬度数据
void local_gps_cb(const sensor_msgs::NavSatFix::ConstPtr& msg)
{
gps_msg = *msg;
cout<<"GPS经度 :";
ROS_INFO("%f",gps_msg.latitude);
cout<<"GPS纬度 :";
ROS_INFO("%f",gps_msg.longitude);
cout<<"GPS海拔 :";
ROS_INFO("%f",gps_msg.altitude);
}
// 订阅的无人机当前位置数据
void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
local_pos = *msg;
ROS_INFO("get the positions x : %f",local_pos.pose.position.x);
ROS_INFO("get the positions y : %f",local_pos.pose.position.y);
ROS_INFO("get the positions z : %f",local_pos.pose.position.z);
//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_msgs::State>("mavros/state", 10, state_cb);
//手柄键值
ros::Subscriber manual_sub = nh.subscribe<mavros_msgs::ManualControl>("mavros/manual_control/control", 10, manual_cb);
//IMU 数据
ros::Subscriber imu_sub = nh.subscribe<sensor_msgs::Imu>("mavros/imu/data", 10, imu_cb);
//无人船速度
ros::Subscriber vel_sub = nh.subscribe<geometry_msgs::TwistStamped>("mavros/local_position/velocity", 10, vel_cb);
// 订阅无人机当前位置（反馈消息）
ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>
("mavros/local_position/pose", 10, local_pos_cb);
//经纬度
ros::Subscriber gps_msg = nh.subscribe<sensor_msgs::NavSatFix>("mavros/global_position/global", 10, local_gps_cb);
//发布
// 发布无人船速度（控制）
ros::Publisher vec_pub = nh.advertise<geometry_msgs::Twist>
("mavros/setpoint_velocity/cmd_vel_unstamped", 10);
// 位置控制 发布之前需要公告，并获取句柄，发布的消息体的类型为：geometry_msgs::PoseStamped
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
//启动服务1，设置客户端（Client）名称为arming_client，客户端的类型为ros::ServiceClient，
//启动服务用的函数为nh下的serviceClient<>()函数，<>里面是该服务的类型，（）里面是该服务的路径
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
//启动服务2，设置客户端（Client）名称为set_mode_client，客户端的类型为ros::ServiceClient，
//启动服务用的函数为nh下的serviceClient<>()函数，<>里面是该服务的类型，（）里面是该服务的路径
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("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: "<<current_state.mode<<endl;
}
else //else指已经为offboard模式，然后进去判断是否解锁，如果没有解锁，则客户端arming_client向服务端arm_cmd发起请求call
//然后服务端回应response成功解锁，这就解锁了
{
//current_state.armed : 1解锁 0未解锁
if( !current_state.armed && (ros::Time::now() - last_request > 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;
cout<<" 解锁 "<<endl; //
vec_pub.publish(vector);
}
}
//local_pos_pub.publish(pose); //发布位置信息，所以综上飞机只有先打开offboard模式然后解锁才能飞起来
// 发布位置控制信息
ros::spinOnce();
rate.sleep(); // 影响消息发布与更新的周期
}
return 0;
}
创建功能包：
https://miracle.blog.csdn.net/article/details/96306977?spm=1001.2014.3001.5506
//古月居 ROS入门21讲运行我的功能包：
//创建工作空间
mkdir mavros_src/src
cd mavros_src/src
catkin_init_workspace
//创建功能包
catkin_create_pkg hellow std_msgs rospy roscpp
//将代码放在 hellow/src/pix4.cpp 里面
//修改 CMakeLists.txt
//139 行 (功能包名字 src/xxx.cpp)
add_executable(${PROJECT_NAME}_node src/apm4_node.cpp) 改为
add_executable(apm4 src/pix4.cpp)
//152行
target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
)
改为 apm4为功能包名字
target_link_libraries(apm4
${catkin_LIBRARIES}
)
//运行功能包
cd mavros_src
catkin_make
source ./devel/setup.bash
rosrun hellow hellow
2070
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
