基於ROS的18屆中航杯比賽流程實現
阿新 • • 發佈：2019-01-25
用ROS寫了個狀態機，感覺流程上還可以改進，接下來優化一下
#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> #include <serial/serial.h> #include <std_msgs/String.h> #include <std_msgs/UInt32.h>
控制器實現
#define Err_Limit_Flag 0 #define Integrate_Separation_Flag 0 #define Integrate_Limit_Flag 0 typedef struct { float Expect; float FeedBack; float Err; float Last_Err; float Err_Max; Integrate_Separation_Err; float Integrate; float Integrate_Max; float Kp; float Ki; float Kd; float Control_OutPut; float Last_Control_OutPut; float Control_OutPut_Limit; float Last_FeedBack; float Dis_Err; float Dis_Error_History[5]; }PID_Controler; float PID_Control(PID_Controler *Controler) { Controler->Last_Err = Controler->Err; Controler->Err = Controler->Expect - Controler->FeedBack; if(Err_Limit_Flag == 1) { if(Controler->Err >= Controler->Err_Max) Controler->Err = Controler->Err_Max; if(Controler->Err <= -Controler->Err_Max) Controler->Err = -Controler->Err_Max; } if(Integrate_Separation_Flag==1) { if(fabs(Controler->Err) <= Controler->Integrate_Separation_Err)//abs->int fabs->float Controler->Integrate += Controler->Ki * Controler->Err; } else { Controler->Integrate += Controler->Ki * Controler->Err; } if(Integrate_Limit_Flag==1) { if(Controler->Integrate >= Controler->Integrate_Max) Controler->Integrate = Controler->Integrate_Max; if(Controler->Integrate <= -Controler->Integrate_Max) Controler->Integrate = -Controler->Integrate_Max ; } Controler->Last_Control_OutPut = Controler->Control_OutPut; Controler->Control_OutPut = Controler->Kp * Controler->Err//P + Controler->Integrate//I + Controler->Kd*(Controler->Err-Controler->Last_Err);//D if(Controler->Control_OutPut >= Controler->Control_OutPut_Limit) Controler->Control_OutPut = Controler->Control_OutPut_Limit; if(Controler->Control_OutPut <= -Controler->Control_OutPut_Limit) Controler->Control_OutPut = -Controler->Control_OutPut_Limit; return Controler->Control_OutPut; } PID_Controler pos_pid_x; PID_Controler pos_pid_y; PID_Controler pos_pid_z;
回撥函式
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *msg;
}
geometry_msgs::PoseStamped current_local_pos;
void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
current_local_pos = *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 set_local_pos_pub = nh.advertise<geometry_msgs::PoseStamped> ("mavros/setpoint_position/local", 10); ros::Subscriber local_pos_pub = nh.subscribe<geometry_msgs::PoseStamped> ("mavros/local_position/pose", 10, local_pos_cb); 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); ros::Publisher dis_pub = nh.advertise<std_msgs::UInt32>("absorb_cmd", 1); std::string cmd_port_name("/dev/ttyACM0"); try { absorb_cmd.setPort(cmd_port_name);/* code for Try */ absorb_cmd.setBaudrate(115200); serial::Timeout to = serial::Timeout::simpleTimeout(1000); absorb_cmd.setTimeout(to); absorb_cmd.open(); } catch (serial::IOException& e) { ROS_ERROR_STREAM("Unable to open port "); return -1; /* code for Catch */ } ros::Rate rate(20.0); // wait for FCU connection while(ros::ok() && current_state.connected){ ros::spinOnce(); rate.sleep(); } /******x_pid引數(float)******/ pos_pid_x.Kp = 1.0; pos_pid_x.Kd = 0.0; pos_pid_x.Ki = 0.0; pos_pid_x.Err_Max = 50.0; pos_pid_x.Integrate_Max = 10.0; pos_pid_x.Integrate_Separation_Err = 100.0; pos_pid_x.Control_OutPut_Limit = 2; /******y_pid引數(float)******/ pos_pid_y.Kp = 1.0; pos_pid_y.Kd = 0.0; pos_pid_y.Ki = 0.0; pos_pid_y.Err_Max = 50.0; pos_pid_y.Integrate_Max = 10.0; pos_pid_y.Integrate_Separation_Err = 100.0; pos_pid_y.Control_OutPut_Limit = 2; /******z_pid引數(float)******/ pos_pid_z.Kp = 0.5; pos_pid_z.Kd = 0.0; pos_pid_z.Ki = 0.0; pos_pid_z.Err_Max = 50.0; pos_pid_z.Integrate_Max = 10.0; pos_pid_z.Integrate_Separation_Err = 100.0; pos_pid_z.Control_OutPut_Limit = 0.6; //設定起飛點，抓箱點，放箱點 geometry_msgs::PoseStamped pose; geometry_msgs::PoseStamped takeoff_pos; geometry_msgs::PoseStamped target1; geometry_msgs::PoseStamped target2; takeoff_pos.pose.position.x = 0.0; takeoff_pos.pose.position.y = 0.0; takeoff_pos.pose.position.z = 2.5; target1.pose.position.x = 10.0; target1.pose.position.y = 0.0; target1.pose.position.z = 2.5; target2.pose.position.x = 10.0; target2.pose.position.y = 10.0; target2.pose.position.z = 2.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 = 1; int cnt = 3;//搬運次數 //注：把自動解鎖自動切外部控制放在迴圈外可避免切不回手動 ros::Time last_request = ros::Time::now(); //狀態機 while(ros::ok() && state == 1) { //自動解鎖切offb /* 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"); } } */ ROS_INFO_STREAM("state = " << state); pos_pid_x.Expect = takeoff_pos.pose.position.x; pos_pid_y.Expect = takeoff_pos.pose.position.y; pos_pid_z.Expect = takeoff_pos.pose.position.z; pos_pid_x.FeedBack = current_local_pos.pose.position.x; pos_pid_y.FeedBack = current_local_pos.pose.position.y; pos_pid_z.FeedBack = current_local_pos.pose.position.z; geometry_msgs::TwistStamped velocity_tw; velocity_tw.twist.linear.x = PID_Control(&pos_pid_x); velocity_tw.twist.linear.y = PID_Control(&pos_pid_y); velocity_tw.twist.linear.z = PID_Control(&pos_pid_z); ROS_INFO("%f %f %f" ,pos_pid_x.Expect ,pos_pid_y.Expect ,pos_pid_z.Expect); ROS_INFO("%f %f %f" ,current_local_pos.pose.position.x ,current_local_pos.pose.position.y ,current_local_pos.pose.position.z); ROS_INFO("%f %f %f" ,pos_pid_x.FeedBack ,pos_pid_y.FeedBack ,pos_pid_z.FeedBack); ROS_INFO("%f %f %f" ,pos_pid_x.Err ,pos_pid_y.Err ,pos_pid_z.Err); ROS_INFO("%f %f %f" ,PID_Control(&pos_pid_x) ,PID_Control(&pos_pid_y) ,PID_Control(&pos_pid_z)); velocity_pub.publish(velocity_tw); if(pos_pid_z.Err < 0.15) { state++; } ros::spinOnce(); rate.sleep(); } while(cnt--) { //到搬運點 last_request = ros::Time::now(); while(ros::ok() && state == 2) { ROS_INFO_STREAM("state = " << state); pos_pid_x.Expect = target1.pose.position.x; pos_pid_y.Expect = target1.pose.position.y; pos_pid_z.Expect = target1.pose.position.z; pos_pid_x.FeedBack = current_local_pos.pose.position.x; pos_pid_y.FeedBack = current_local_pos.pose.position.y; pos_pid_z.FeedBack = current_local_pos.pose.position.z; geometry_msgs::TwistStamped velocity_tw; velocity_tw.twist.linear.x = PID_Control(&pos_pid_x); velocity_tw.twist.linear.y = PID_Control(&pos_pid_y); velocity_tw.twist.linear.z = PID_Control(&pos_pid_z); ROS_INFO("%f %f %f" ,pos_pid_x.Expect ,pos_pid_y.Expect ,pos_pid_z.Expect); ROS_INFO("%f %f %f" ,current_local_pos.pose.position.x ,current_local_pos.pose.position.y ,current_local_pos.pose.position.z); ROS_INFO("%f %f %f" ,pos_pid_x.FeedBack ,pos_pid_y.FeedBack ,pos_pid_z.FeedBack); ROS_INFO("%f %f %f" ,pos_pid_x.Err ,pos_pid_y.Err ,pos_pid_z.Err); ROS_INFO("%f %f %f" ,PID_Control(&pos_pid_x) ,PID_Control(&pos_pid_y) ,PID_Control(&pos_pid_z)); velocity_pub.publish(velocity_tw); if((pos_pid_x.Err < 0.15) && (pos_pid_y.Err < 0.15) && (pos_pid_z.Err < 0.15) && (ros::Time::now() - last_request > ros::Duration(5.0))) { state++; } ros::spinOnce(); rate.sleep(); } //搬運點降落 last_request = ros::Time::now(); while(ros::ok() && state == 3){ ROS_INFO_STREAM("state = " << state); pos_pid_x.Expect = target1.pose.position.x; pos_pid_y.Expect = target1.pose.position.y; pos_pid_z.Expect = 0; pos_pid_x.FeedBack = current_local_pos.pose.position.x; pos_pid_y.FeedBack = current_local_pos.pose.position.y; pos_pid_z.FeedBack = current_local_pos.pose.position.z; geometry_msgs::TwistStamped velocity_tw; velocity_tw.twist.linear.x = PID_Control(&pos_pid_x); velocity_tw.twist.linear.y = PID_Control(&pos_pid_y); velocity_tw.twist.linear.z = PID_Control(&pos_pid_z); ROS_INFO("%f %f %f" ,pos_pid_x.Expect ,pos_pid_y.Expect ,pos_pid_z.Expect); ROS_INFO("%f %f %f" ,current_local_pos.pose.position.x ,current_local_pos.pose.position.y ,current_local_pos.pose.position.z); ROS_INFO("%f %f %f" ,pos_pid_x.FeedBack ,pos_pid_y.FeedBack ,pos_pid_z.FeedBack); ROS_INFO("%f %f %f" ,pos_pid_x.Err ,pos_pid_y.Err ,pos_pid_z.Err); ROS_INFO("%f %f %f" ,PID_Control(&pos_pid_x) ,PID_Control(&pos_pid_y) ,PID_Control(&pos_pid_z)); velocity_pub.publish(velocity_tw); if(ros::Time::now() - last_request > ros::Duration(10.0)) { state++; } ros::spinOnce(); rate.sleep(); } last_request = ros::Time::now(); //搬運點起飛 while(ros::ok() && state == 4) { ROS_INFO_STREAM("state = " << state); pos_pid_x.Expect = target1.pose.position.x; pos_pid_y.Expect = target1.pose.position.y; pos_pid_z.Expect = target1.pose.position.z; pos_pid_x.FeedBack = current_local_pos.pose.position.x; pos_pid_y.FeedBack = current_local_pos.pose.position.y; pos_pid_z.FeedBack = current_local_pos.pose.position.z; geometry_msgs::TwistStamped velocity_tw; velocity_tw.twist.linear.x = PID_Control(&pos_pid_x); velocity_tw.twist.linear.y = PID_Control(&pos_pid_y); velocity_tw.twist.linear.z = PID_Control(&pos_pid_z); ROS_INFO("%f %f %f" ,pos_pid_x.Expect ,pos_pid_y.Expect ,pos_pid_z.Expect); ROS_INFO("%f %f %f" ,current_local_pos.pose.position.x ,current_local_pos.pose.position.y ,current_local_pos.pose.position.z); ROS_INFO("%f %f %f" ,pos_pid_x.FeedBack ,pos_pid_y.FeedBack ,pos_pid_z.FeedBack); ROS_INFO("%f %f %f" ,pos_pid_x.Err ,pos_pid_y.Err ,pos_pid_z.Err); ROS_INFO("%f %f %f" ,PID_Control(&pos_pid_x) ,PID_Control(&pos_pid_y) ,PID_Control(&pos_pid_z)); velocity_pub.publish(velocity_tw); if(ros::Time::now() - last_request > ros::Duration(7.0)) { state++; } ros::spinOnce(); rate.sleep(); } //到投遞點 while(ros::ok() && state == 5) { ROS_INFO_STREAM("state = " << state); pos_pid_x.Expect = target2.pose.position.x; pos_pid_y.Expect = target2.pose.position.y; pos_pid_z.Expect = target2.pose.position.z; pos_pid_x.FeedBack = current_local_pos.pose.position.x; pos_pid_y.FeedBack = current_local_pos.pose.position.y; pos_pid_z.FeedBack = current_local_pos.pose.position.z; geometry_msgs::TwistStamped velocity_tw; velocity_tw.twist.linear.x = PID_Control(&pos_pid_x); velocity_tw.twist.linear.y = PID_Control(&pos_pid_y); velocity_tw.twist.linear.z = PID_Control(&pos_pid_z); ROS_INFO("%f %f %f" ,pos_pid_x.Expect ,pos_pid_y.Expect ,pos_pid_z.Expect); ROS_INFO("%f %f %f" ,current_local_pos.pose.position.x ,current_local_pos.pose.position.y ,current_local_pos.pose.position.z); ROS_INFO("%f %f %f" ,pos_pid_x.FeedBack ,pos_pid_y.FeedBack ,pos_pid_z.FeedBack); ROS_INFO("%f %f %f" ,pos_pid_x.Err ,pos_pid_y.Err ,pos_pid_z.Err); ROS_INFO("%f %f %f" ,PID_Control(&pos_pid_x) ,PID_Control(&pos_pid_y) ,PID_Control(&pos_pid_z)); velocity_pub.publish(velocity_tw); if((pos_pid_x.Err < 0.15) && (pos_pid_y.Err < 0.15)) { state++; } ros::spinOnce(); rate.sleep(); } //投遞點降落 last_request = ros::Time::now(); /* while(ros::ok() && state == 6){ ROS_INFO_STREAM("state = " << state); pos_pid_x.Expect = target2.pose.position.x; pos_pid_y.Expect = target2.pose.position.y; pos_pid_z.Expect = 0; pos_pid_x.FeedBack = current_local_pos.pose.position.x; pos_pid_y.FeedBack = current_local_pos.pose.position.y; pos_pid_z.FeedBack = current_local_pos.pose.position.z; geometry_msgs::TwistStamped velocity_tw; velocity_tw.twist.linear.x = PID_Control(&pos_pid_x); velocity_tw.twist.linear.y = PID_Control(&pos_pid_y); velocity_tw.twist.linear.z = PID_Control(&pos_pid_z); ROS_INFO("%f %f %f" ,pos_pid_x.Expect ,pos_pid_y.Expect ,pos_pid_z.Expect); ROS_INFO("%f %f %f" ,current_local_pos.pose.position.x ,current_local_pos.pose.position.y ,current_local_pos.pose.position.z); ROS_INFO("%f %f %f" ,pos_pid_x.FeedBack ,pos_pid_y.FeedBack ,pos_pid_z.FeedBack); ROS_INFO("%f %f %f" ,pos_pid_x.Err ,pos_pid_y.Err ,pos_pid_z.Err); ROS_INFO("%f %f %f" ,PID_Control(&pos_pid_x) ,PID_Control(&pos_pid_y) ,PID_Control(&pos_pid_z)); velocity_pub.publish(velocity_tw); if(ros::Time::now() - last_request > ros::Duration(10.0)) { state++; } ros::spinOnce(); rate.sleep(); } */ last_request = ros::Time::now(); //投遞點發送投遞指令 while(ros::ok() && state == 6) { ROS_INFO_STREAM("state = " << state); std::string send_buf; send_buf[0] = 'F'; send_buf[1] = 'F'; send_buf[2] = 'A'; send_buf[3] = 'B'; absorb_cmd.write(send_buf); if(ros::Time::now() - last_request > ros::Duration(4.0)) { state=2; } ros::spinOnce(); rate.sleep(); } } return 0; }
除錯問題：
最開始起飛和降落直接用的是mavros中的模式指令，但是發現takeoff以後，不再可以做速度控制，同樣，land以後也不能在takeoff了，即便是自動解鎖切offb也不行，暫時還沒弄懂是為啥，接下來再測試一下