上一篇简要介绍了ROS控制PX4并使用仿真环境进行调试的框架
点击打开链接
本篇将详细介绍ROS节点的发布与订阅细节,并使用gazebo进行仿真调试。
任务:实现飞机的自动起飞,然后在两点之间循环飞行数次,最后自动降落。
1.使用RoboWare在已经建立好的工作空间中新建立一个holdheight.cpp用于写控制命令
ps:有时RoboWare会把之前已经打开的工作空间丢失
解决方法:进入工作空间目录,使用ls -a查看隐藏文件,将.vscode文件夹删除
rm -rf /.vscode
然后进入RoboWare,打开已有工作空间,以我的为例,将目录设置为catkin_ws即可,不是进入catkin_ws/src/
2.代码编写
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <geometry_msgs/TwistStamped.h>
#include <string>
#include <sstream>
#include <iostream>
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, "holdheight");
ros::NodeHandle nh;
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");
//ros::Publisher velocity_pub = nh.advertise<geometry_msgs::TwistStamped>
// ("mavros/setpoint_velocity/cmd_vel", 10);
//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
// wait for FCU connection
while(ros::ok() && current_state.connected){
ros::spinOnce();
rate.sleep();
}
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 5;
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
int state = 3;
ros::Time last_request = ros::Time::now();
while(ros::ok() ){
if( !current_state.armed ){
if( arming_client.call(arm_cmd) &&
arm_cmd.response.success){
ROS_INFO("Vehicle armed");
}
}
if( current_state.mode != "OFFBOARD"){
if( set_mode_client.call(offb_set_mode) &&
offb_set_mode.response.success){
ROS_INFO("Offboard enabled");
}
}
if( (ros::Time::now() - last_request > ros::Duration(5.0))) break;
ros::spinOnce();
rate.sleep();
}
while(state--) {
last_request = ros::Time::now();
while(ros::ok()) {
if( (ros::Time::now() - last_request > ros::Duration(5.0))) break;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
local_pos_pub.publish(pose);
//geometry_msgs::TwistStamped velocity_tw;
//velocity_pub.publish(velocity_tw);
ROS_INFO("111111");
ros::spinOnce();
rate.sleep();
}
last_request = ros::Time::now();
while(ros::ok()) {
if( (ros::Time::now() - last_request > ros::Duration(5.0))) break;
pose.pose.position.x = 5;
pose.pose.position.y = 5;
local_pos_pub.publish(pose);
//geometry_msgs::TwistStamped velocity_tw;
//velocity_pub.publish(velocity_tw);
ROS_INFO("222222");
ros::spinOnce();
rate.sleep();
}
ROS_INFO_STREAM("state="<<state);
}
offb_set_mode.request.custom_mode = "AUTO.LAND";
if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.success)
{
ROS_INFO("AUTO.LAND enabled");
last_request = ros::Time::now();
}
return 0;
}
3.代码说明:
首先根据节点要实现的功能订阅mavros中相应的节点,因为我要控制PX4的位置,所以要订阅发布位置消息的节点;因为要获取 飞机模式,所以要订阅获取模式的节点。。。
以位置节点为例,对订阅格式进行说明:
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
其中的Publisher说明我要向mavros的local_position节点发布消息
根据wiki mavros中的说明将<>和()的内容填写上即可。注意要相应的把该节点地址include一下
#include <geometry_msgs/PoseStamped.h>
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)