AGV/projects/controllers/AGV_controller/AGV_controller.cpp

#include <webots/Robot.hpp>
#include <webots/Motor.hpp>
#include <webots/PositionSensor.hpp>
#include <webots/GPS.hpp>
#include <webots/Gyro.hpp>
#include <webots/InertialUnit.hpp>
#include "emqx-client/mqttmsg.h"
#include "emqx-client/paho_client.h"
#include <unistd.h>
#include <chrono>
#include <random>
#include "timedlockdata.hpp"
// All the webots classes are defined in the "webots" namespace
using namespace webots;

Paho_client* client_= nullptr;
TimedLockData<NavMessage::Speed> g_speed;


void SpeedChangeCallback(std::string topic,int QOS,MqttMsg& msg,void* context)
{
  NavMessage::Speed speed;
  msg.toProtoMessage(speed);
  g_speed.reset(speed,0.3);
}

double generae_random(std::default_random_engine& generator,double mean,double sigma)
{
  std::normal_distribution<double> distribution(mean, sigma);
  return distribution(generator);
}

bool init_mqtt(std::string ip,int port,std::string nodeId,std::string subtopic)
{
  if(client_!= nullptr)
  {
    client_->disconnect();
    delete client_;
  }
  client_=new Paho_client(nodeId);
  bool ret= client_->connect(ip,port);
  if(ret)
  {
    while(!client_->isconnected()) usleep(1000);
    client_->subcribe(subtopic,1,SpeedChangeCallback, nullptr);
  }
  return ret;
}

void Rotating(double wmg,double w,double l,
        double& R1,double& R2,double& R3,double& R4,
              double& L1,double& L2,double& L3,double& L4)
{
  double theta=atan(l/w);
  R1=(-theta);
  R2=(theta);
  R3=(theta);
  R4=(-theta);

  double velocity=wmg*sqrt(w*w+l*l);
  L1=(-velocity);
  L2=(velocity);
  L3=(-velocity);
  L4=(velocity);
}

void Horizontal(double velocity,double& R1,double& R2,double& R3,double& R4,
                double& L1,double& L2,double& L3,double& L4)
{
  R1=-M_PI/2.0;
  R2=M_PI/2.0;
  R3=M_PI/2.0;
  R4=-M_PI/2.0;
  L1=-velocity;
  L2=velocity;
  L3=velocity;
  L4=-velocity;
}

void Vrtical(double velocity,double& R1,double& R2,double& R3,double& R4,
                double& L1,double& L2,double& L3,double& L4)
{
  R1=0;
  R2=0;
  R3=0;
  R4=0;
  L1=velocity;
  L2=velocity;
  L3=velocity;
  L4=velocity;
}


int main(int argc, char **argv) {
  //init mqtt
  if(false==init_mqtt("127.0.0.1",1883,"webots-AGV","monitor/speedcmd"))
  {
    printf(" Init mqtt failed...\n");
    return -1;
  }
  //随机数

  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
  std::default_random_engine generator(seed);

  // create the Robot instance.
  Robot *robot = new Robot();

  // get the time step of the current world.
  int timeStep = (int)robot->getBasicTimeStep();

  webots::Motor* RM1=robot->getMotor("R1_motor");
  webots::Motor* RM2=robot->getMotor("R2_motor");
  webots::Motor* RM3=robot->getMotor("R3_motor");
  webots::Motor* RM4=robot->getMotor("R4_motor");

  //R1->getPositionSensor()->enable(timeStep);

  webots::Motor* R1_l=robot->getMotor("L1_motor");
  webots::Motor* R2_l=robot->getMotor("L2_motor");
  webots::Motor* R3_l=robot->getMotor("L3_motor");
  webots::Motor* R4_l=robot->getMotor("L4_motor");


  RM1->setPosition(0);
  RM2->setPosition(0);
  RM3->setPosition(0);
  RM4->setPosition(0);

  R1_l->setPosition(INFINITY);
  R2_l->setPosition(INFINITY);
  R3_l->setPosition(INFINITY);
  R4_l->setPosition(INFINITY);

  /*RM1->setTorque(100);
  RM2->setTorque(100);
  RM3->setTorque(100);
  RM4->setTorque(100);*/

  R1_l->setVelocity(0.0);
  R2_l->setVelocity(0.0);
  R3_l->setVelocity(0.0);
  R4_l->setVelocity(0.0);

  webots::GPS* gps=robot->getGPS("gps");
  webots::InertialUnit* imu=robot->getInertialUnit("imu");
  webots::Gyro* gyr=robot->getGyro("gyro");
  gyr->enable(timeStep);
  gps->enable(timeStep);
  imu->enable(timeStep);

  double w=2.5;
  double l=1.3;
  double radius=0.1915;
  double K=1.0/radius;

  auto last=std::chrono::steady_clock::now();
  while (robot->step(timeStep) != -1) {

    auto now=std::chrono::steady_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(now - last);
    double time = double(duration.count()) * std::chrono::microseconds::period::num /
            std::chrono::microseconds::period::den;

    ////发布：
    //增加高斯分布
    double x=gps->getValues()[2] + generae_random(generator,0,0.02);
    double y=gps->getValues()[0] + generae_random(generator,0,0.02);
    double theta=imu->getRollPitchYaw()[2]+generae_random(generator,0,0.3*M_PI/180.0);

    //获取gps速度，判断朝向
    double vm1=R3_l->getVelocity()*radius;
    double vm2=-R4_l->getVelocity()*radius;
    double v=gps->getSpeed();
    if(vm1+vm2<0.0)
      v=-v;
    //陀螺仪角速度
    double vth=gyr->getValues()[1];

    if(time>0.2)
    {
      printf(" publish delay................  time:%f -------------------------------------\n",time);
    }
    /*printf("dt:%f x y:%f  %f,  yaw:%f   velocity:%f vth:%f  %f  %f\n",time,
            x,y,theta,
           v,gyr->getValues()[0],gyr->getValues()[1],gyr->getValues()[2]);*/
    last=now;

    if(client_)
    {
      MqttMsg msg;
      NavMessage::AGVStatu statu;
      statu.set_x(x);
      statu.set_y(y);
      statu.set_theta(theta);
      statu.set_v(v);
      statu.set_vth(vth);
      msg.fromProtoMessage(statu);
      client_->publish("monitor/statu",1,msg);
    }

    if(g_speed.timeout())
    {
      RM1->setPosition(0);
      RM2->setPosition(0);
      RM3->setPosition(0);
      RM4->setPosition(0);

      R1_l->setVelocity(0);
      R2_l->setVelocity(0);
      R3_l->setVelocity(0);
      R4_l->setVelocity(0);
      continue;
    }

    //变速----------------------------------
    double cmd_v=g_speed.Get().v();
    double cmd_vth=g_speed.Get().vth();

    //初始值
    double R1=0,R2=0,R3=0,R4=0;
    double L1=cmd_v;
    double L2=cmd_v;
    double L3=cmd_v;
    double L4=cmd_v;

    if(g_speed.Get().type()==1) //原地旋转
    {
      if (fabs(cmd_vth) > 0.0001)
        Rotating(cmd_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
    }
    else if(g_speed.Get().type()==2)    //横移
    {
      Horizontal(g_speed.Get().v(),R1, R2, R3, R4, L1, L2, L3, L4);
    }
    else if(g_speed.Get().type()==3) {    //巡线/前进

      if (fabs(cmd_vth) > 0.4) {
        cmd_vth = cmd_vth > 0 ? 0.4 : -0.4;
      }


      //------------------------------------------------
      //无角速度，直线
      if (fabs(cmd_vth) < 0.0001) {
        R1 = R2 = R3 = R4 = 0;
        L1 = L2 = L3 = L4 = cmd_v;
      } else  //有角速度
      {
        if (fabs(cmd_v) < 0.00001) {
          //原地旋转
          if (fabs(cmd_vth) > 0.0001)
            Rotating(cmd_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
        } else {
          //默认原地旋转
          Rotating(cmd_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
          double base = pow(cmd_v / cmd_vth, 2) - (l * l / 4);
          //满足运动方程
          if (base >= 0.0001) {
            double BaseR = sqrt(base) - w / 2;

            if(BaseR<0.05)
              BaseR=0.05;
            //printf("BaseR : %f \n",BaseR);

            if (BaseR > 0) {
              if (cmd_vth * cmd_v >= 0) {
                R1 = atan(l / BaseR);
                R2 = atan(l / (BaseR + w));
                L3 = BaseR * cmd_vth;
                L4 = (BaseR + w) * cmd_vth;
                R3 = 0;
                R4 = 0;
                L1 = L3 / cos(R1);
                L2 = L4 / cos(R2);

              } else {
                R1 = -atan(l / (BaseR + w));
                R2 = -atan(l / BaseR);
                L3 = -(BaseR + w) * cmd_vth;
                L4 = -BaseR * cmd_vth;
                R3 = 0;
                R4 = 0;
                L1 = L3 / cos(R1);
                L2 = L4 / cos(R2);
              }

            }
          }

        }
      }
    }else{
    L1=L2=L3=L4=0;
    }

    printf(" R1-R4:%.5f %.5f %.5f %.5f  L1-L4:%.5f %.5f %.5f %.5f  V:%.5f  Vth:%.5f\n",
            R1,R2,R3,R4,L1,L2,L3,L4,v,vth);
     
     
    RM1->setPosition(R1);
    RM2->setPosition(R2);
    RM3->setPosition(R3);
    RM4->setPosition(R4);

    R1_l->setVelocity(L1*K);
    R2_l->setVelocity(-L2*K);
    R3_l->setVelocity(L3*K);
    R4_l->setVelocity(-L4*K);

  };

  // Enter here exit cleanup code.

  delete robot;
  return 0;
}