Offboard control is dangerous. If you are operating on a real vehicle be sure to have a way of gaining back manual control in case something goes wrong.
mavros_msgs 功能包中包含操作 MAVROS 包中服务和主题所需的自定义消息文件。 所有服务和主题及其相应的消息类型都可以在 mavros wiki 中找到。 :::
代码
Create the offb_node.cpp file in your ROS package (by also adding it to your CMakeList.txt so it is compiled), and paste the following inside it:
/** * @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;voidstate_cb(const mavros_msgs::State::ConstPtr& msg){ current_state =*msg;}intmain(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 2Hz ros::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(); }return0;}
//the setpoint publishing rate MUST be faster than 2Hzros::Raterate(20.0);
尽管PX4飞控在NED坐标系下操控飞机,但MAVROS是在ENU系下进行指令传输的。 这也就是为什么我们设置z为+2。 This is why the publishing rate must be faster than 2 Hz to also account for possible latencies. This is also the same reason why it is recommended to enter Offboard mode from Position mode, this way if the vehicle drops out of Offboard mode it will stop in its tracks and hover.
// wait for FCU connectionwhile(ros::ok() &&!current_state.connected){ ros::spinOnce();rate.sleep();}
The rest of the code is pretty self explanatory. We attempt to switch to Offboard mode, after which we arm the quad to allow it to fly. We space out the service calls by 5 seconds so to not flood the autopilot with the requests. In the same loop, we continue sending the requested pose at the appropriate rate.
This code has been simplified to the bare minimum for illustration purposes. In larger systems, it is often useful to create a new thread which will be in charge of periodically publishing the setpoints.