湖北建设信息网站/营销推广渠道有哪些

参考：官网https://dev.px4.io/en/ros/mavros_offboard.html

环境：Ubuntu 16.04+ROS-Kinetic

           工作空间catkin_ws + MAVlink

1. 建立一个外部控制程序包

打开RoboWare在原来已经配置好MAVlink环境的工作空间catkin_ws下创建一个新的ROS包

offb roscpp mavros geometry_msgs

在/offb/src/下新建一个offb_node.cpp文件

/*** @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 <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>("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");//the setpoint publishing rate MUST be faster than 2Hzros::Rate rate(20.0);// wait for FCU connectionwhile(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 = 2;//send a few setpoints before startingfor(int i = 100; ros::ok() && i > 0; --i){local_pos_pub.publish(pose);ros::spinOnce();rate.sleep();}mavros_msgs::SetMode offb_set_mode;offb_set_mode.request.custom_mode = "OFFBOARD";mavros_msgs::CommandBool arm_cmd;arm_cmd.request.value = true;ros::Time last_request = ros::Time::now();while(ros::ok()){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();} else {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 armed");}last_request = ros::Time::now();}}local_pos_pub.publish(pose);ros::spinOnce();rate.sleep();}return 0;
}

保存编译

cd ~/catkin_ws
catkin build

2. 开始仿真

打开终端1，启动gazebo仿真：

cd ~/src/Firmware
make px4_sitl_default gazebo

打开终端2，运行mavros

roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

打开终端3，启用外部控制程序

rosrun offb offb_node

小四旋翼悬停在（0,0,2）坐标处，结束。