修复横移防撞bug

MPC/loaded_mpc.cpp

@@ -113,9 +113,9 @@ class FG_eval_half_agv {
       }
 
 
-      if(m_statu.size()==2+12){
+      if(m_statu.size()==2+16){
         //与障碍物的距离
-        for(int i=0;i<6;++i) {
+        for(int i=0;i<8;++i) {
           double ox=m_statu[2+i*2];
           double oy=m_statu[2+i*2+1];
           for (int t = 0; t < N; ++t) {
@@ -242,7 +242,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd s
     //printf(" mpc find obs ,w=%f,h=%f\n",obs_w_,obs_h_);
     find_obs=true;
   }
-  if(find_obs) n_constraints = N * (5+6);
+  if(find_obs) n_constraints = N * (5+8);
 
   Dvector constraints_lowerbound(n_constraints);
   Dvector constraints_upperbound(n_constraints);
@@ -266,7 +266,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd s
 
   // 与障碍物保持距离的约束
   if(find_obs) {
-    for (int i = nobs; i < nobs + 6*N; ++i) {
+    for (int i = nobs; i < nobs + 8*N; ++i) {
       constraints_lowerbound[i] = 1.0;
       constraints_upperbound[i] = 1e19;
     }
@@ -294,7 +294,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd s
 
   Eigen::VectorXd statu_velocity(2);
   if(find_obs){
-    statu_velocity=Eigen::VectorXd(2+12);
+    statu_velocity=Eigen::VectorXd(2+16);
 
     std::vector<Pose2d> vertexs=Pose2d::generate_rectangle_vertexs(obs_relative_pose_,obs_w_*2,obs_h_*2);
     for(int i=0;i<vertexs.size();++i){
@@ -357,10 +357,12 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd s
   printf("input : %.4f  %.5f(%.5f)   output : %.4f  %.5f(%.5f)  ref : %.3f obs relative:(%f,%f) time:%.3f\n",
          line_velocity,wmg,angular,solve_velocity,solve_angular,correct_angular,
          ref_velocity,obs_relative_pose_.x(),obs_relative_pose_.y(),time);
-  if(solve_velocity>=0 && solve_velocity<min_velocity_)
+
+  /*if(solve_velocity>=0 && solve_velocity<min_velocity_)
     solve_velocity=min_velocity_;
   if(solve_velocity<0 && solve_velocity>-min_velocity_)
-    solve_velocity=-min_velocity_;
+    solve_velocity=-min_velocity_;*/
+
   out.push_back(solve_velocity);
   out.push_back(correct_angular);
 

MPC/navigation.cpp

@@ -702,8 +702,8 @@ bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& o
   Trajectory optimize_trajectory;
   Trajectory selected_trajectory;
   NavParameter::MPC_parameter mpc_parameter=parameter_.x_mpc_parameter();
-  double obs_w=0.8;
-  double obs_h=1.5;
+  double obs_w=0.85;
+  double obs_h=1.45;
 
   if (directY == true) {
     //将车身朝向旋转90°，车身朝向在(-pi,pi]
@@ -711,8 +711,8 @@ bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& o
     if (statu[2] > M_PI)
       statu[2] -= 2 * M_PI;
     mpc_parameter=parameter_.y_mpc_parameter();
-    obs_w=1.5;
-    obs_h=0.8;
+    obs_w=1.45;
+    obs_h=0.85;
   }
 
   Pose2d brother = timedBrotherPose_.Get();
@@ -922,6 +922,17 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
       Stop();
       return false;
     }
+    /*
+     * AGV 在终点附近低速巡航时，设置最小速度0.05
+     */
+    Pose2d target_in_agv=Pose2d::relativePose(target,timedPose_.Get());
+    if(Pose2d::abs(target_in_agv)<Pose2d(0.2,0.2,M_PI*2)) {
+      if (out[0] >= 0 && out[0] < limit_v.min)
+        out[0] = limit_v.min;
+      if (out[0] < 0 && out[0] > -limit_v.min)
+        out[0] = -limit_v.min;
+    }
+    //下发速度
     if(directY==false)
       SendMoveCmd(move_mode_, Monitor_emqx::eMPC, out[0], out[1]);
     else

MPC/pose2d.h

@@ -123,6 +123,8 @@ class Pose2d
       poses.push_back(pose*Pose2d(-w/2,-h/2,pose.theta()));
       poses.push_back(pose*Pose2d(-w/2,0,pose.theta()));
       poses.push_back(pose*Pose2d(w/2,0,pose.theta()));
+      poses.push_back(pose*Pose2d(0,-h/2.0,pose.theta()));
+      poses.push_back(pose*Pose2d(0,h/2.0,pose.theta()));
       return poses;
     }
 

projects/controllers/AGV_controller/AGV_controller.cpp

@@ -15,11 +15,11 @@
 using namespace webots;
 
 Paho_client* client_= nullptr;
-TimedLockData<NavMessage::Speed> g_speed;
+TimedLockData<NavMessage::ToAgvCmd> g_speed;
 
 void SpeedChangeCallback(std::string topic,int QOS,MqttMsg& msg,void* context)
 {
-  NavMessage::Speed speed;
+  NavMessage::ToAgvCmd speed;
   msg.toProtoMessage(speed);
   g_speed.reset(speed,0.3);
 }
@@ -64,36 +64,83 @@ void Rotating(double wmg,double w,double l,
   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 wmg,double w,double l,double& R1,double& R2,double& R3,double& R4,
+             double& L1,double& L2,double& L3,double& L4) {
+  if (fabs(wmg) < 0.0001) {
+    R1 = R2 = R3 = R4 = 0;
+    L1 = L2 = L3 = L4 = velocity;
+    printf("virtical111 R1-R4:%.5f %.5f %.5f %.5f  L1-L4:%.5f %.5f %.5f %.5f \n",
+           R1,R2,R3,R4,L1,L2,L3,L4);
+  } else {//有角速度
+    double base = pow(velocity / wmg, 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 (wmg * velocity >= 0) {
+          R1 = atan(l / BaseR);
+          R2 = atan(l / (BaseR + w));
+          L3 = BaseR * wmg;
+          L4 = (BaseR + w) * wmg;
+          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) * wmg;
+          L4 = -BaseR * wmg;
+          R3 = 0;
+          R4 = 0;
+          L1 = L3 / cos(R1);
+          L2 = L4 / cos(R2);
+        }
+        printf("virtical222 R1-R4:%.5f %.5f %.5f %.5f  L1-L4:%.5f %.5f %.5f %.5f \n",
+               R1,R2,R3,R4,L1,L2,L3,L4);
+
+      }
+    }
+  }
 }
 
-void Vrtical(double velocity,double& R1,double& R2,double& R3,double& R4,
+void Horizontal(double velocity,double wmg,double w,double l,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;
+  Vrtical(velocity,wmg,w,l,R3,R1,R4,R2,L3,L1,L4,L2);
+
+  if (fabs(wmg) < 0.0001) {
+    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;
+  }else{
+    R1-=M_PI/2.0;
+    R2+=M_PI/2.0;
+    R3+=M_PI/2.0;
+    R4-=M_PI/2.0;
+    L1=-L1;
+    L4=-L4;
+  }
+
 }
 
 
+
+
+
 int main(int argc, char **argv) {
   //init mqtt
-  if(false==init_mqtt("127.0.0.1",1883,"webots","monitor/speedcmd"))
+  if(false==init_mqtt("127.0.0.1",1883,"webots_main","MainPLC/speedcmd"))
   {
     printf(" Init mqtt failed...\n");
     return -1;
@@ -164,13 +211,13 @@ int main(int argc, char **argv) {
 
     ////发布：
     //增加高斯分布
-    double x=gps->getValues()[2]+ generae_random(generator,0,0.007);
-    double y=gps->getValues()[0] + generae_random(generator,0,0.007);
+    double x=gps->getValues()[2]+ generae_random(generator,0,0.005);
+    double y=gps->getValues()[0] + generae_random(generator,0,0.005);
     double theta=imu->getRollPitchYaw()[2]+generae_random(generator,0,0.1*M_PI/180.0);
 
     //获取gps速度，判断朝向
     double vm1=R3_l->getVelocity()*radius;
-    double vm2=-R4_l->getVelocity()*radius;
+    double vm2=-R2_l->getVelocity()*radius;
     double v=gps->getSpeed();
     if(vm1+vm2<0.0)
       v=-v;
@@ -189,20 +236,22 @@ int main(int argc, char **argv) {
     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);
-
-      NavMessage::AGVSpeed speed;
+      NavMessage::LidarOdomStatu odom;
+      odom.set_x(x);
+      odom.set_y(y);
+      odom.set_theta(theta);
+      odom.set_v(v);
+      odom.set_vth(vth);
+      msg.fromProtoMessage(odom);
+      client_->publish("lidar_odom/main",1,msg);
+
+      NavMessage::AgvStatu speed;
       speed.set_v(v);
       speed.set_w(vth);
+      speed.set_clamp(0);
+      speed.set_clamp_other(0);
       msg.fromProtoMessage(speed);
-      client_->publish("monitor/speed",1,msg);
+      client_->publish("MainPLC/speed",1,msg);
     }
 
     if(g_speed.timeout())
@@ -232,74 +281,29 @@ int main(int argc, char **argv) {
 
     if(g_speed.Get().t()==1) //原地旋转
     {
+      printf("旋转:");
       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().t()==2)    //横移
     {
-      Horizontal(g_speed.Get().v(),R1, R2, R3, R4, L1, L2, L3, L4);
+      printf("横移:");
+      if (fabs(cmd_vth) > 0.4) {
+        cmd_vth = cmd_vth > 0 ? 0.4 : -0.4;
+      }
+      Horizontal(cmd_v,cmd_vth,l,w,R1, R2, R3, R4, L1, L2, L3, L4);
     }
-    else {    //巡线/前进
-
+    else if(g_speed.Get().t()==3){    //巡线/前进
+      printf("前进:");
       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);
-              }
-
-            }
-          }
-
-        }
-      }
+      Vrtical(cmd_v,cmd_vth,w,l,R1,R2,R3,R4,L1,L2,L3,L4);
     }
 
     printf("Child 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);
+            R1,R2,R3,R4,L1,L2,L3,L4,cmd_v,cmd_vth);
      
     RM1->setPosition(R1);
     RM2->setPosition(R2);

projects/controllers/agv_child/AGV_controller.cpp

@@ -15,11 +15,11 @@
 using namespace webots;
 
 Paho_client* client_= nullptr;
-TimedLockData<NavMessage::Speed> g_speed;
+TimedLockData<NavMessage::ToAgvCmd> g_speed;
 
 void SpeedChangeCallback(std::string topic,int QOS,MqttMsg& msg,void* context)
 {
-  NavMessage::Speed speed;
+  NavMessage::ToAgvCmd speed;
   msg.toProtoMessage(speed);
   g_speed.reset(speed,0.3);
 }
@@ -64,36 +64,82 @@ void Rotating(double wmg,double w,double l,
   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 wmg,double w,double l,double& R1,double& R2,double& R3,double& R4,
+             double& L1,double& L2,double& L3,double& L4) {
+  if (fabs(wmg) < 0.0001) {
+    R1 = R2 = R3 = R4 = 0;
+    L1 = L2 = L3 = L4 = velocity;
+    printf("virtical111 R1-R4:%.5f %.5f %.5f %.5f  L1-L4:%.5f %.5f %.5f %.5f \n",
+           R1,R2,R3,R4,L1,L2,L3,L4);
+  } else {//有角速度
+    double base = pow(velocity / wmg, 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 (wmg * velocity >= 0) {
+          R1 = atan(l / BaseR);
+          R2 = atan(l / (BaseR + w));
+          L3 = BaseR * wmg;
+          L4 = (BaseR + w) * wmg;
+          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) * wmg;
+          L4 = -BaseR * wmg;
+          R3 = 0;
+          R4 = 0;
+          L1 = L3 / cos(R1);
+          L2 = L4 / cos(R2);
+        }
+        printf("virtical222 R1-R4:%.5f %.5f %.5f %.5f  L1-L4:%.5f %.5f %.5f %.5f \n",
+           R1,R2,R3,R4,L1,L2,L3,L4);
+
+      }
+    }
+  }
 }
 
-void Vrtical(double velocity,double& R1,double& R2,double& R3,double& R4,
+void Horizontal(double velocity,double wmg,double w,double l,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;
+  Vrtical(velocity,wmg,w,l,R3,R1,R4,R2,L3,L1,L4,L2);
+  if (fabs(wmg) < 0.0001) {
+    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;
+  }else{
+    R1-=M_PI/2.0;
+    R2+=M_PI/2.0;
+    R3+=M_PI/2.0;
+    R4-=M_PI/2.0;
+    L1=-L1;
+    L4=-L4;
+  }
+
 }
 
 
+
+
+
 int main(int argc, char **argv) {
   //init mqtt
-  if(false==init_mqtt("127.0.0.1",1883,"webots-AGV-child","monitor_child/speedcmd"))
+  if(false==init_mqtt("127.0.0.1",1883,"webots-child","ChildPLC/speedcmd"))
   {
     printf(" Init mqtt failed...\n");
     return -1;
@@ -170,7 +216,7 @@ int main(int argc, char **argv) {
 
     //获取gps速度，判断朝向
     double vm1=R3_l->getVelocity()*radius;
-    double vm2=-R4_l->getVelocity()*radius;
+    double vm2=-R2_l->getVelocity()*radius;
     double v=gps->getSpeed();
     if(vm1+vm2<0.0)
       v=-v;
@@ -189,20 +235,22 @@ int main(int argc, char **argv) {
     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_child/statu",1,msg);
-
-      NavMessage::AGVSpeed speed;
+      NavMessage::LidarOdomStatu odom;
+      odom.set_x(x);
+      odom.set_y(y);
+      odom.set_theta(theta);
+      odom.set_v(v);
+      odom.set_vth(vth);
+      msg.fromProtoMessage(odom);
+      client_->publish("lidar_odom/child",1,msg);
+
+      NavMessage::AgvStatu speed;
       speed.set_v(v);
       speed.set_w(vth);
+      speed.set_clamp(0);
+      speed.set_clamp_other(0);
       msg.fromProtoMessage(speed);
-      client_->publish("monitor_child/speed",1,msg);
+      client_->publish("ChildPLC/speed",1,msg);
     }
 
     if(g_speed.timeout())
@@ -230,76 +278,32 @@ int main(int argc, char **argv) {
     double L3=cmd_v;
     double L4=cmd_v;
 
-    if(g_speed.Get().t()==1) //原地旋转
+   if(g_speed.Get().t()==1) //原地旋转
     {
+      printf("旋转:");
       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().t()==2)    //横移
     {
-      Horizontal(g_speed.Get().v(),R1, R2, R3, R4, L1, L2, L3, L4);
+      printf("横移:");
+      if (fabs(cmd_vth) > 0.4) {
+        cmd_vth = cmd_vth > 0 ? 0.4 : -0.4;
+      }
+      Horizontal(cmd_v,cmd_vth,l,w,R1, R2, R3, R4, L1, L2, L3, L4);
     }
-    else {    //巡线/前进
-
+    else if(g_speed.Get().t()==3){    //巡线/前进
+      printf("前进:");
       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);
-              }
-
-            }
-          }
-
-        }
-      }
+      Vrtical(cmd_v,cmd_vth,w,l,R1,R2,R3,R4,L1,L2,L3,L4);
     }
 
+
     printf("Child 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);
+            R1,R2,R3,R4,L1,L2,L3,L4,cmd_v,cmd_vth);
      
     RM1->setPosition(R1);
     RM2->setPosition(R2);
@@ -318,3 +322,4 @@ int main(int argc, char **argv) {
   delete robot;
   return 0;
 }
+

projects/webots_project/four_wheels_m.wbt

@@ -1,394 +0,0 @@
-#VRML_SIM R2021a utf8
-WorldInfo {
-  basicTimeStep 16
-  coordinateSystem "NUE"
-}
-Viewpoint {
-  orientation -0.5826005034436419 -0.5689314550412352 -0.5804254067939117 2.1038509586384233
-  position -2.9312330504807185 91.87390389427381 78.54012493312264
-}
-TexturedBackground {
-}
-TexturedBackgroundLight {
-}
-RectangleArena {
-  translation 0 -0.05 50
-  rotation 1 0 0 0
-  floorSize 40 110
-  floorTileSize 20 20
-  wallThickness 0.5
-  wallHeight 2
-  wallAppearance BrushedAluminium {
-    IBLStrength 15
-  }
-}
-Robot {
-  translation -13.01216924529826 0.14903723920929007 99.93855697807129
-  rotation -6.182296082223161e-06 -0.9999999906060143 -0.00013692972821978858 -0.0023252775819534214
-  children [
-    InertialUnit {
-      translation 0 0.2 0.5
-      children [
-        Shape {
-          appearance Appearance {
-            material Material {
-              diffuseColor 0.14902 0.45098 1
-            }
-          }
-          geometry Sphere {
-            radius 0.1
-          }
-        }
-      ]
-      name "imu"
-    }
-    GPS {
-      translation 0 0.2 0
-      children [
-        Solid {
-          rotation 0 0 1 0
-          children [
-            Shape {
-              appearance Appearance {
-                material Material {
-                  diffuseColor 0.929412 0.831373 0
-                }
-              }
-              geometry Cone {
-                bottomRadius 0.3
-                height 0.4
-              }
-            }
-          ]
-        }
-      ]
-    }
-    DEF BODY Shape {
-      appearance PBRAppearance {
-        baseColor 0.917647 0.145098 0.145098
-        roughness 1
-        metalness 0
-      }
-      geometry Box {
-        size 1.9 0.2 1.5
-      }
-    }
-    DEF WHEEL1 HingeJoint {
-      jointParameters HingeJointParameters {
-        position 3204.299167224161
-        anchor 1 0 0.5
-      }
-      device [
-        RotationalMotor {
-          name "wheel1"
-          maxVelocity 100
-        }
-      ]
-      endPoint Solid {
-        translation 1 0 0.5
-        rotation -0.2652016981846079 0.2649898331356455 0.9270644247379699 1.6456546469574749
-        children [
-          DEF WHEEL Shape {
-            appearance PBRAppearance {
-              baseColor 0.305882 0.898039 0.25098
-              roughness 1
-              metalness 0
-            }
-            geometry Cylinder {
-              height 0.1
-              radius 0.2
-              subdivision 24
-            }
-          }
-        ]
-        boundingObject USE WHEEL
-        physics Physics {
-          density 1
-          mass 1
-        }
-        linearVelocity 3.73385483755766e-12 1.1181072565517522e-15 -1.6571927848411335e-11
-        angularVelocity -1.7802546558163615e-10 7.552989963818089e-12 4.099932308338187e-13
-      }
-    }
-    DEF WHEEL2 HingeJoint {
-      jointParameters HingeJointParameters {
-        position 4132.026885342275
-        anchor -1 0 0.5
-      }
-      device [
-        RotationalMotor {
-          name "wheel2"
-          maxVelocity 100
-        }
-      ]
-      endPoint Solid {
-        translation -1 0 0.5
-        rotation 0.26493925666909174 -0.2649545168758917 0.9271495533422275 1.6463574026937842
-        children [
-          USE WHEEL
-        ]
-        name "solid(1)"
-        boundingObject USE WHEEL
-        physics Physics {
-          density 1
-          mass 1
-        }
-        linearVelocity 3.7478026304332435e-12 4.908641046033565e-16 -2.24580791292885e-12
-        angularVelocity -8.874715231745456e-13 7.554383793851529e-12 4.4311065738747374e-15
-      }
-    }
-    DEF WHEEL3 HingeJoint {
-      jointParameters HingeJointParameters {
-        position 3209.9289865389387
-        anchor 1 0 -0.5
-      }
-      device [
-        RotationalMotor {
-          name "wheel3"
-          maxVelocity 100
-        }
-      ]
-      endPoint Solid {
-        translation 1 0 -0.5
-        rotation 0.6530883534797477 -0.6530877448269917 0.38334318841725423 2.409475869772187
-        children [
-          USE WHEEL
-        ]
-        name "solid(2)"
-        boundingObject USE WHEEL
-        physics Physics {
-          density 1
-          mass 1
-        }
-        linearVelocity -3.8317841996533795e-12 -4.719133131317983e-16 -1.643376735377655e-11
-        angularVelocity -6.062885574014809e-12 7.553152218081688e-12 1.8138928304137572e-14
-      }
-    }
-    DEF WHEEL4 HingeJoint {
-      jointParameters HingeJointParameters {
-        position 4130.300712701317
-        anchor -1 0 -0.5
-      }
-      device [
-        RotationalMotor {
-          name "wheel4"
-          maxVelocity 100
-        }
-      ]
-      endPoint Solid {
-        translation -1 0 -0.5
-        rotation -0.4828161187218597 0.4828343281979345 0.7305885346869387 1.8796771949779125
-        children [
-          USE WHEEL
-        ]
-        name "solid(3)"
-        boundingObject USE WHEEL
-        physics Physics {
-          density 1
-          mass 1
-        }
-        linearVelocity -3.736471420626607e-12 -7.103275163254803e-16 -1.8432971633423352e-12
-        angularVelocity -5.769639445896213e-13 7.555853023268881e-12 1.6416134434231503e-15
-      }
-    }
-  ]
-  name "robot1"
-  boundingObject USE BODY
-  physics Physics {
-    density 10
-  }
-  controller "four_controller"
-  linearVelocity -3.2722624715515054e-14 5.784067182327832e-16 -9.30981743446901e-12
-  angularVelocity -1.603332527255836e-15 7.554712457776498e-12 1.1607222508886257e-16
-}
-StraightRoadSegment {
-  translation 0 0.001 0
-  width 6
-  lines [
-    RoadLine {
-      width 0.2
-    }
-  ]
-  roadBorderHeight 0.3
-  startingRoadBorderWidth 0.5
-  endingRoadBorderWidth 0.5
-  length 17
-  appearance Asphalt {
-    IBLStrength 10
-  }
-}
-StraightRoadSegment {
-  translation 0 0.001 23
-  name "road(1)"
-  width 6
-  lines [
-    RoadLine {
-      width 0.2
-    }
-  ]
-  roadBorderHeight 0.3
-  startingRoadBorderWidth 0.5
-  endingRoadBorderWidth 0.5
-  length 33
-  appearance Asphalt {
-    IBLStrength 10
-  }
-}
-StraightRoadSegment {
-  translation -2 0.001 60
-  rotation 0 1 0 -1.5707996938995747
-  name "road(2)"
-  width 6
-  lines [
-    RoadLine {
-      width 0.2
-    }
-  ]
-  roadBorderHeight 0.3
-  startingRoadBorderWidth 0.5
-  endingRoadBorderWidth 0.5
-  length 7
-  appearance Asphalt {
-    IBLStrength 10
-  }
-}
-StraightRoadSegment {
-  translation -13 0.001 63
-  name "road(4)"
-  width 6
-  lines [
-    RoadLine {
-      width 0.2
-    }
-  ]
-  roadBorderHeight 0.3
-  startingRoadBorderWidth 0.5
-  endingRoadBorderWidth 0.5
-  length 34
-  appearance Asphalt {
-    IBLStrength 10
-  }
-}
-StraightRoadSegment {
-  translation 0 0.001 63
-  name "road(5)"
-  width 6
-  lines [
-    RoadLine {
-      width 0.2
-    }
-  ]
-  roadBorderHeight 0.3
-  startingRoadBorderWidth 0.5
-  endingRoadBorderWidth 0.5
-  length 34
-  appearance Asphalt {
-    IBLStrength 10
-  }
-}
-StraightRoadSegment {
-  translation -2 0.001 100
-  rotation 0 1 0 -1.5707996938995747
-  name "road(3)"
-  width 6
-  lines [
-    RoadLine {
-      width 0.2
-    }
-  ]
-  roadBorderHeight 0.3
-  startingRoadBorderWidth 0.5
-  endingRoadBorderWidth 0.5
-  length 7
-  appearance Asphalt {
-    IBLStrength 10
-  }
-}
-StraightRoadSegment {
-  translation 16 0.001 20
-  rotation 0 1 0 -1.5707996938995747
-  name "road(6)"
-  width 6
-  lines [
-    RoadLine {
-      width 0.2
-    }
-  ]
-  roadBorderHeight 0.3
-  startingRoadBorderWidth 0.5
-  endingRoadBorderWidth 0.3
-  length 12
-  appearance Asphalt {
-    IBLStrength 10
-  }
-}
-RoadIntersection {
-  translation 0 0.001 20
-  rotation 0 1 0 0.7853996938995746
-  roadsWidth 6
-  startRoadsLength [
-    3
-  ]
-  startRoadBorderHeight 0.3
-  startRoadBorderWidth 0.5
-}
-RoadIntersection {
-  translation 0 0.001 60
-  rotation 0 1 0 0.7853996938995746
-  name "road intersection(1)"
-  roadsWidth 6
-  startRoadsLength [
-    3
-  ]
-  startRoadBorderHeight 0.3
-  startRoadBorderWidth 0.5
-}
-RoadIntersection {
-  translation 0 0.001 100
-  rotation 0 1 0 0.7853996938995746
-  name "road intersection(2)"
-  roadsWidth 6
-  startRoadsLength [
-    3
-  ]
-  startRoadBorderHeight 0.3
-  startRoadBorderWidth 0.5
-}
-RoadIntersection {
-  translation -13 0.001 100
-  rotation 0 1 0 0.7853996938995746
-  name "road intersection(3)"
-  roadsWidth 6
-  startRoadsLength [
-    3
-  ]
-  startRoadBorderHeight 0.3
-  startRoadBorderWidth 0.5
-}
-RoadIntersection {
-  translation -13 0.001 60
-  rotation 0 1 0 0.7853996938995746
-  name "road intersection(4)"
-  roadsWidth 6
-  startRoadsLength [
-    3
-  ]
-  startRoadBorderHeight 0.3
-  startRoadBorderWidth 0.5
-}
-RoadIntersection {
-  translation 18 0.001 20
-  rotation 0 1 0 0.7853996938995746
-  name "road intersection(5)"
-  roadsWidth 6
-  startRoadsLength [
-    1
-  ]
-  startRoadsLine [
-    RoadLine {
-      width 0.3
-    }
-  ]
-  startRoadBorderHeight 0.3
-  startRoadBorderWidth 0.5
-}