PX4从放弃到精通（二十三）：仿真

#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 )/ ros//Publisher local_pos_pub1 = nh/advertise</geometry_msgs//PoseStamped>/ ( uav1/mavros/setpoint_position/local / 10)/ ros//ServiceClient arming_client1 = nh/serviceClient</mavros_msgs//CommandBool>/ ( uav1/mavros/cmd/arming )/ ros//ServiceClient set_mode_client1 = nh/serviceClient</mavros_msgs//SetMode>/ ( uav1/mavros/set_mode )/ ros//Publisher local_pos_pub2 = nh/advertise</geometry_msgs//PoseStamped>/ ( uav2/mavros/setpoint_position/local / 10)/ ros//ServiceClient arming_client2 = nh/serviceClient</mavros_msgs//CommandBool>/ ( uav2/mavros/cmd/arming )/ ros//ServiceClient set_mode_client2 = nh/serviceClient</mavros_msgs//SetMode>/ ( uav2/mavros/set_mode )/ ros//Publisher local_pos_pub3 = nh/advertise</geometry_msgs//PoseStamped>/ ( uav3/mavros/setpoint_position/local / 10)/ ros//ServiceClient arming_client3 = nh/serviceClient</mavros_msgs//CommandBool>/ ( uav3/mavros/cmd/arming )/ ros//ServiceClient set_mode_client3 = nh/serviceClient</mavros_msgs//SetMode>/ ( uav3/mavros/set_mode )/ ros//Publisher local_pos_pub4 = nh/advertise</geometry_msgs//PoseStamped>/ ( uav4/mavros/setpoint_position/local / 10)/ ros//ServiceClient arming_client4 = nh/serviceClient</mavros_msgs//CommandBool>/ ( uav4/mavros/cmd/arming )/ ros//ServiceClient set_mode_client4= nh/serviceClient</mavros_msgs//SetMode>/ ( uav4/mavros/set_mode )/ ros//Publisher local_pos_pub5 = nh/advertise</geometry_msgs//PoseStamped>/ ( uav5/mavros/setpoint_position/local / 10)/ ros//ServiceClient arming_client5 = nh/serviceClient</mavros_msgs//CommandBool>/ ( uav5/mavros/cmd/arming )/ ros//ServiceClient set_mode_client5= nh/serviceClient</mavros_msgs//SetMode>/ ( uav5/mavros/set_mode )/ ros//Publisher local_pos_pub6 = nh/advertise</geometry_msgs//PoseStamped>/ ( uav6/mavros/setpoint_position/local / 10)/ ros//ServiceClient arming_client6 = nh/serviceClient</mavros_msgs//CommandBool>/ ( uav6/mavros/cmd/arming )/ ros//ServiceClient set_mode_client6= nh/serviceClient</mavros_msgs//SetMode>/ ( uav6/mavros/set_mode )/ ros//Publisher local_pos_pub7 = nh/advertise</geometry_msgs//PoseStamped>/ ( uav7/mavros/setpoint_position/local / 10)/ ros//ServiceClient arming_client7 = nh/serviceClient</mavros_msgs//CommandBool>/ ( uav7/mavros/cmd/arming )/ ros//ServiceClient set_mode_client7= nh/serviceClient</mavros_msgs//SetMode>/ ( uav7/mavros/set_mode )/ ros//Publisher local_pos_pub8 = nh/advertise</geometry_msgs//PoseStamped>/ ( uav8/mavros/setpoint_position/local / 10)/ ros//ServiceClient arming_client8 = nh/serviceClient</mavros_msgs//CommandBool>/ ( uav8/mavros/cmd/arming )/ ros//ServiceClient set_mode_client8= nh/serviceClient</mavros_msgs//SetMode>/ ( uav8/mavros/set_mode )/ ros//Publisher local_pos_pub9 = nh/advertise</geometry_msgs//PoseStamped>/ ( uav9/mavros/setpoint_position/local / 10)/ ros//ServiceClient arming_client9 = nh/serviceClient</mavros_msgs//CommandBool>/ ( uav9/mavros/cmd/arming )/ ros//ServiceClient set_mode_client9= nh/serviceClient</mavros_msgs//SetMode>/ ( uav9/mavros/set_mode )/ //the setpoint publishing rate MUST be faster than 2Hz ros//Rate rate(20/0)/ geometry_msgs//PoseStamped pose/pose1/pose2/pose3/pose4/pose5/pose6/pose7/pose8/pose9/ pose/pose/position/x = 0/ pose/pose/position/y = 0/ pose/pose/position/z = 6/ pose1/pose/position/x = 0/ pose1/pose/position/y = 0/ pose1/pose/position/z = 6/ pose2/pose/position/x = 0/ pose2/pose/position/y = 0/ pose2/pose/position/z = 6/ pose3/pose/position/x = 0/ pose3/pose/position/y = 0/ pose3/pose/position/z = 6/ pose4/pose/position/x =0/ pose4/pose/position/y =0/ pose4/pose/position/z =6/ pose5/pose/position/x =0/ pose5/pose/position/y =0/ pose5/pose/position/z =6/ pose6/pose/position/x =0/ pose6/pose/position/y =0/ pose6/pose/position/z =6/ pose7/pose/position/x =0/ pose7/pose/position/y =0/ pose7/pose/position/z =6/ pose8/pose/position/x =0/ pose8/pose/position/y =0/ pose8/pose/position/z =6/ pose9/pose/position/x =0/ pose9/pose/position/y =0/ pose9/pose/position/z =6/ mavros_msgs//SetMode offb_set_mode/ offb_set_mode/request/custom_mode = TAKEOFF / mavros_msgs//CommandBool arm_cmd/ arm_cmd/request/value = true/ ros//Time last_request = ros//Time//now()/int flag=0/ while(ros//ok()){ if(flag>/30){ pose/pose/position/x = 0/ pose/pose/position/y = 10/ pose/pose/position/z = 6/ pose1/pose/position/x = 0/ pose1/pose/position/y = 10/ pose1/pose/position/z = 6/ pose2/pose/position/x = 0/ pose2/pose/position/y = 10/ pose2/pose/position/z = 6/ pose3/pose/position/x = 0/ pose3/pose/position/y = 10/ pose3/pose/position/z = 6/ pose4/pose/position/x =0/ pose4/pose/position/y =10/ pose4/pose/position/z =6/ pose5/pose/position/x =0/ pose5/pose/position/y =10/ pose5/pose/position/z =6/ pose6/pose/position/x =0/ pose6/pose/position/y =10/ pose6/pose/position/z =6/ pose7/pose/position/x =0/ pose7/pose/position/y =10/ pose7/pose/position/z =6/ pose8/pose/position/x =0/ pose8/pose/position/y =10/ pose8/pose/position/z =6/ pose9/pose/position/x =0/ pose9/pose/position/y =10/ pose9/pose/position/z =6/}set_mode_client/call(offb_set_mode)/arming_client/call(arm_cmd)/ local_pos_pub/publish(pose)/set_mode_client1/call(offb_set_mode)/arming_client1/call(arm_cmd)/ local_pos_pub1/publish(pose1)/set_mode_client2/call(offb_set_mode)/arming_client2/call(arm_cmd)/ local_pos_pub2/publish(pose2)/set_mode_client3/call(offb_set_mode)/arming_client3/call(arm_cmd)/ local_pos_pub3/publish(pose3)/set_mode_client4/call(offb_set_mode)/arming_client4/call(arm_cmd)/ local_pos_pub4/publish(pose4)/set_mode_client5/call(offb_set_mode)/arming_client5/call(arm_cmd)/ local_pos_pub5/publish(pose5)/set_mode_client6/call(offb_set_mode)/arming_client6/call(arm_cmd)/ local_pos_pub6/publish(pose6)/set_mode_client7/call(offb_set_mode)/arming_client7/call(arm_cmd)/ local_pos_pub7/publish(pose7)/set_mode_client8/call(offb_set_mode)/arming_client8/call(arm_cmd)/ local_pos_pub8/publish(pose8)/set_mode_client9/call(offb_set_mode)/arming_client9/call(arm_cmd)/ local_pos_pub9/publish(pose9)/flag / ros//spinOnce()/ rate/sleep()/ } return 0/}

温馨提示:
1. WEO啦仅提供《PX4从放弃到精通（二十三）：仿真》全文中的部分公开内容，版权归原著者或相关公司所有。
2. 以上信息来源于互联网免费公开的渠道，若文章所含内容侵犯了您的版权或隐私，请通知我们立即删除。
3. 当前页面地址：https://www.weo.la/doc/10204be55a1deb7e.html 复制内容请保留相关链接。