基于ROS的18届中航杯比赛流程实现

用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也不行,暂时还没弄懂是为啥,接下来再测试一下

  • 2
    点赞
  • 4
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值