source: flair-src/trunk/demos/TwoWheelRobotSimpleFleet/ugv/src/SimpleFleet.cpp @ 390

Last change on this file since 390 was 390, checked in by Sanahuja Guillaume, 10 months ago

add simple fleet demo for ugv

File size: 7.3 KB
Line 
1//  created:    2020/12/21
2//  filename:   SimpleFleet.cpp
3//
4//  author:     Guillaume Sanahuja
5//              Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7//  version:    $Id: $
8//
9//  purpose:    demo simple fleet avec optitrack
10//
11//
12/*********************************************************************/
13
14#include "SimpleFleet.h"
15#include <TargetController.h>
16#include <GridLayout.h>
17#include <PushButton.h>
18#include <DataPlot1D.h>
19#include <DataPlot2D.h>
20#include <FrameworkManager.h>
21#include <VrpnClient.h>
22#include <MetaVrpnObject.h>
23#include <TrajectoryGenerator2DCircle.h>
24#include <Matrix.h>
25#include <Tab.h>
26#include <TabWidget.h>
27#include <DoubleSpinBox.h>
28#include <Pid.h>
29#include <Quaternion.h>
30#include <Euler.h>
31#include <Ugv.h>
32#include <UgvControls.h>
33#include <math.h>
34#include <UdpSocket.h>
35#include <string.h>
36
37using namespace std;
38using namespace flair::core;
39using namespace flair::gui;
40using namespace flair::sensor;
41using namespace flair::filter;
42using namespace flair::meta;
43using namespace flair::actuator;
44
45SimpleFleet::SimpleFleet(string name,string broadcast,TargetController *controller): Thread(getFrameworkManager(),"SimpleFleet",50), behaviourMode(BehaviourMode_t::Manual), vrpnLost(false) {
46    this->controller=controller;
47    controller->Start();
48   
49    Ugv* ugv=GetUgv();
50    ugv->UseDefaultPlot();
51   
52    VrpnClient* vrpnclient=new VrpnClient("vrpn", ugv->GetDefaultVrpnAddress(),80);
53    ugvVrpn = new MetaVrpnObject(name);
54       
55    getFrameworkManager()->AddDeviceToLog(ugvVrpn);
56    vrpnclient->Start();
57       
58    Tab *ugvTab = new Tab(getFrameworkManager()->GetTabWidget(), "ugv", 0);
59    GridLayout* buttonslayout = new GridLayout(ugvTab->NewRow(), "buttons");
60    button_kill = new PushButton(buttonslayout->NewRow(), "kill");
61    startCircle=new PushButton(buttonslayout->NewRow(),"start_circle");
62    stopCircle=new PushButton(buttonslayout->LastRowLastCol(),"stop_circle");
63   
64    circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
65    ugvVrpn->xPlot()->AddCurve(circle->GetMatrix()->Element(0,0),DataPlot::Blue);
66    ugvVrpn->yPlot()->AddCurve(circle->GetMatrix()->Element(0,1),DataPlot::Blue);
67    ugvVrpn->VxPlot()->AddCurve(circle->GetMatrix()->Element(1,0),DataPlot::Blue);
68    ugvVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue);
69    ugvVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle");
70   
71    xCircleCenter=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"x circle center"," m",-5,5,0.1,1,0);
72    yCircleCenter=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"y circle center"," m",-5,5,0.1,1,0);
73
74    Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
75    TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
76    Tab *setupLawTab = new Tab(tabWidget, "Setup");
77    Tab *graphLawTab = new Tab(tabWidget, "Graphes");
78    uX=new Pid(setupLawTab->At(1,0),"u_x");
79    uX->UseDefaultPlot(graphLawTab->NewRow());
80    uY=new Pid(setupLawTab->At(1,1),"u_y");
81    uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
82   
83    l=new DoubleSpinBox(setupLawTab->NewRow(),"L", " m", 0, 10, 0.1, 1,1);
84   
85    message=new UdpSocket(ugv,"Message",broadcast,true);
86}
87
88SimpleFleet::~SimpleFleet() {
89}
90
91void SimpleFleet::Run(void) {
92    WarnUponSwitches(true);
93    SetPeriodMS(20);
94   
95    if (getFrameworkManager()->ErrorOccured() == true) {
96        SafeStop();
97    }
98
99    while (!ToBeStopped()) {
100        SecurityCheck();
101        CheckMessages();
102        CheckJoystick();
103        CheckPushButton();
104       
105        if(behaviourMode==BehaviourMode_t::Manual) ComputeManualControls();
106        if(behaviourMode==BehaviourMode_t::Circle) ComputeCircleControls();
107        WaitPeriod();
108    }
109}
110
111void SimpleFleet::CheckPushButton(void) {
112  if (startCircle->Clicked() == true)
113      StartCircle();
114       
115  if (stopCircle->Clicked() == true)
116      StopCircle();
117
118  if (button_kill->Clicked() == true)
119      SafeStop();
120}
121
122void SimpleFleet::CheckJoystick(void) {
123  //R1 and Circle
124  if(controller->IsButtonPressed(9) && controller->IsButtonPressed(4)) {
125      StartCircle();
126  }
127
128  //R1 and Cross
129  if(controller->IsButtonPressed(9) && controller->IsButtonPressed(5)) {
130      StopCircle();
131  }
132}
133
134void SimpleFleet::SecurityCheck(void) {
135    if ((!vrpnLost) && (behaviourMode==BehaviourMode_t::Circle)) {
136        if (!ugvVrpn->IsTracked(500)) {
137            Thread::Err("VRPN, ugv lost\n");
138            vrpnLost=true;
139            StopCircle();
140        }
141    }
142}
143
144void SimpleFleet::CheckMessages(void) {
145    char msg[64];
146    char src[64];
147    size_t src_size=sizeof(src);
148    while(message->RecvMessage(msg,sizeof(msg),TIME_NONBLOCK,src,&src_size)>0) {
149        //printf("%s %s\n",GetUav()->ObjectName().c_str(),src);
150        if(strcmp(src,GetUgv()->ObjectName().c_str())!=0) {
151            if(strcmp(msg,"StartCircle")==0) {
152                Printf("StartCircle fleet\n");
153                StartCircle();
154            }
155            if(strcmp(msg,"StopCircle")==0) {
156                Printf("StopCircle fleet\n");
157                StopCircle();
158            }
159        }
160    }
161}
162
163void SimpleFleet::ComputeManualControls(void) {
164  float speed=-controller->GetAxisValue(3);
165  float turn=controller->GetAxisValue(0);
166  GetUgv()->GetUgvControls()->SetControls(speed,turn);
167}
168
169void SimpleFleet::ComputeCircleControls(void) {
170
171  Vector3Df ugv_pos,ugv_vel; // in VRPN coordinate system
172  Vector2Df ugv_2Dpos,ugv_2Dvel,target_2Dpos; // in VRPN coordinate system
173  Vector2Df pos_error,vel_error;
174  Vector2Df circle_pos,circle_vel;
175   
176  ugvVrpn->GetPosition(ugv_pos);
177  ugvVrpn->GetSpeed(ugv_vel);
178
179  ugv_pos.To2Dxy(ugv_2Dpos);
180  ugv_vel.To2Dxy(ugv_2Dvel);
181 
182  target_2Dpos.x=xCircleCenter->Value();
183  target_2Dpos.y=yCircleCenter->Value();
184  circle->SetCenter(target_2Dpos);
185
186  //circle reference
187  circle->Update(GetTime());
188  circle->GetPosition(circle_pos);
189  circle->GetSpeed(circle_vel);
190
191  //error in optitrack frame
192  pos_error=ugv_2Dpos-circle_pos;
193  vel_error=ugv_2Dvel-circle_vel;
194   
195  uX->SetValues(pos_error.x, vel_error.x);
196  uX->Update(GetTime());
197  uY->SetValues(pos_error.y, vel_error.y);
198  uY->Update(GetTime());
199 
200  //get yaw from vrpn
201  Quaternion vrpnQuaternion;
202  ugvVrpn->GetQuaternion(vrpnQuaternion);
203  float yaw=vrpnQuaternion.ToEuler().yaw;
204  float L=1;
205  float v= cosf(yaw)*uX->Output() + sinf(yaw)*uY->Output();
206  float w = -sinf(yaw)/l->Value()*uX->Output() + cosf(yaw)/l->Value()*uY->Output();
207  GetUgv()->GetUgvControls()->SetControls(-v,-w);
208}
209
210
211void SimpleFleet::StartCircle(void) {
212  if(behaviourMode!=BehaviourMode_t::Circle) {
213    Vector3Df ugv_pos;
214    Vector2Df ugv_2Dpos,target_2Dpos;
215
216    target_2Dpos.x=xCircleCenter->Value();
217    target_2Dpos.y=yCircleCenter->Value();
218    circle->SetCenter(target_2Dpos);
219
220    ugvVrpn->GetPosition(ugv_pos);
221    ugv_pos.To2Dxy(ugv_2Dpos);
222    circle->StartTraj(ugv_2Dpos);
223
224    uX->Reset();
225    uY->Reset();
226    behaviourMode=BehaviourMode_t::Circle;
227    Thread::Info("SimpleFleet: start circle\n");
228    message->SendMessage("StartCircle");
229  }
230}
231
232void SimpleFleet::StopCircle(void) {
233    if(behaviourMode==BehaviourMode_t::Circle) {
234      circle->FinishTraj();
235      //GetJoystick()->Rumble(0x70);
236      behaviourMode=BehaviourMode_t::Manual;
237      Thread::Info("SimpleFleet: finishing circle\n");
238      message->SendMessage("StopCircle");
239    }
240}
241
Note: See TracBrowser for help on using the repository browser.