PX4+gazebo仿真的unecpected comand176报错

hazyparker

我想使用PX4+gazebo跑一个起飞，悬停，降落的程序；结果遇到了图中错误；

问题表现为：

• invalid setpoints
• CMD: Unexpected command 176，result 0
• 在程序中的"Offboard enabled"循环出不来
• PX4不断地启动和关闭失效安全模式
  
  

1. mavros_msgs::State current_state;
   
2. void state_cb(const mavros_msgs::State::ConstPtr& msg)
   
3. {
   
4.     current_state = *msg;
   
5. }
   
6. 
   
7. geometry_msgs::PoseStamped current_pose;
   
8. 
   
9. void local_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
   
10. {
    
11.     current_pose = *msg;
    
12. }
    
13. 
    
14. int main(int argc, char **argv)
    
15. {
    
16.     ros::init(argc, argv, "offb_node");
    
17.     ros::NodeHandle nh;
    
18. 
    
19.     ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
    
20.             ("mavros/state", 10, state_cb);
    
21.     ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>
    
22.             ("mavros/local_position/pose",10,local_cb);
    
23.     ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
    
24.             ("mavros/setpoint_position/local", 10);
    
25.     ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
    
26.             ("mavros/cmd/arming");
    
27.     ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
    
28.             ("mavros/set_mode");
    
29. 
    
30.     //the setpoint publishing rate MUST be faster than 2Hz
    
31.     ros::Rate rate(20.0);
    
32. 
    
33.     // wait for FCU connection
    
34.     while(ros::ok() && !current_state.connected){
    
35.         ros::spinOnce();
    
36.         rate.sleep();
    
37.     }
    
38. 
    
39.     geometry_msgs::PoseStamped pose;
    
40.     pose.pose.position.x = 0;
    
41.     pose.pose.position.y = 0;
    
42.     pose.pose.position.z = 2;
    
43. 
    
44.     //send a few setpoints before starting
    
45.     for(int i = 100; ros::ok() && i > 0; --i){
    
46.         local_pos_pub.publish(pose);
    
47.         ros::spinOnce();
    
48.         rate.sleep();
    
49.     }
    
50. 
    
51.     mavros_msgs::SetMode offb_set_mode;
    
52.     offb_set_mode.request.custom_mode = "OFFBOARD";
    
53. 
    
54.     mavros_msgs::CommandBool arm_cmd;
    
55.     arm_cmd.request.value = true;
    
56. 
    
57.     ros::Time last_request = ros::Time::now();
    
58.     ROS_INFO("Offboarding");
    
59.     while(ros::ok())
    
60.     {
    
61.         if( current_state.mode != "OFFBOARD" &&
    
62.             (ros::Time::now() - last_request > ros::Duration(5.0))){
    
63.             if( set_mode_client.call(offb_set_mode) &&
    
64.                 offb_set_mode.response.mode_sent){
    
65.                 ROS_INFO("Offboard enabled");
    
66.             }
    
67.             last_request = ros::Time::now();
    
68.         } else {
    
69.             if( !current_state.armed &&
    
70.                 (ros::Time::now() - last_request > ros::Duration(5.0))){
    
71.                 if( arming_client.call(arm_cmd) &&
    
72.                     arm_cmd.response.success){
    
73.                     ROS_INFO("Vehicle armed");
    
74.                 }
    
75.                 last_request = ros::Time::now();
    
76.             }
    
77.         }
    
78. 
    
79.         local_pos_pub.publish(pose);
    
80. 
    
81.         ros::spinOnce();
    
82. 
    
83.         geometry_msgs::Point curr,aim;
    
84.         curr = current_pose.pose.position;
    
85.         aim = pose.pose.position;
    
86.         double dist = sqrt(pow((curr.x-aim.x),2)+pow((curr.y-aim.y),2)+pow((curr.z-aim.z),2));
    
87.         if(dist < 0.08)
    
88.         {
    
89.             ROS_INFO("Reach the goal.");
    
90.             break;
    
91.         }
    
92.         rate.sleep();
    
93.     }
    
94. 
    
95.     for(int i = 100; ros::ok() && i > 0; --i)//悬停一会
    
96.     {
    
97.         local_pos_pub.publish(pose);
    
98.         ros::spinOnce();
    
99.         rate.sleep();
    
100.     }
     
101. 
     
102.     ROS_INFO("Landing");
     
103.     mavros_msgs::SetMode land_set_mode;
     
104.     land_set_mode.request.custom_mode = "AUTO.LAND";
     
105.     while (ros::ok())
     
106.     {
     
107.         if( current_state.mode != "AUTO.LAND" &&
     
108.             (ros::Time::now() - last_request > ros::Duration(5.0))){
     
109.             if( set_mode_client.call(land_set_mode) &&
     
110.                 land_set_mode.response.mode_sent){
     
111.                 ROS_INFO("Land enabled");
     
112.             }
     
113.             last_request = ros::Time::now();
     
114.         }
     
115.         if(!current_state.armed)
     
116.         {
     
117.             break;
     
118.         }
     
119.         // local_pos_pub.publish(pose);
     
120. 
     
121.         ros::spinOnce();
     
122.         rate.sleep();
     
123.     }
     
124.     return 0;
     
125. }

复制代码

不知道是什么原因，请各位同学老师指点。

hazyparker已获得悬赏 3 阿木币

最佳答案

这个程序在别的机子上能跑，而且我看能跑的也会报这个176的错，但是不影响运行；不知道是什么原因卡在了“offboard enable”出不来，导致解锁不了，没法向下进行 ...

• hazyparker
• 发表于2022-01-11
• 详细答案 >

shanghuo

字面意思 是你给的命令没对吧

hazyparker

shanghuo 发表于 2022-1-11 16:25
字面意思 是你给的命令没对吧

这个程序在别的机子上能跑，而且我看能跑的也会报这个176的错，但是不影响运行；不知道是什么原因卡在了“offboard enable”出不来，导致解锁不了，没法向下进行