Index: /trunk/demos/CMakeLists.txt
===================================================================
--- /trunk/demos/CMakeLists.txt (revision 389)
+++ /trunk/demos/CMakeLists.txt (revision 390)
@@ -11,4 +11,5 @@
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/PidStandalone/)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/SimpleFleet/)
+add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/TwoWheelRobotSimpleFleet/)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/Sinus/)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/Skeletons/)
Index: /trunk/demos/TwoWheelRobotSimpleFleet/CMakeLists.txt
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/CMakeLists.txt (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/CMakeLists.txt (revision 390)
@@ -0,0 +1,15 @@
+PROJECT(TwoWheelRobotSimpleFleet)
+include($ENV{FLAIR_ROOT}/flair-src/cmake-modules/GlobalCmakeFlair.cmake)
+
+if("${CMAKE_SYSTEM_PROCESSOR_DEFAULTTUNE}" MATCHES "core2-64")
+
+add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/simulator)
+add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/ugv)
+
+else()
+ if(DEFINED CMAKE_SYSTEM_PROCESSOR_DEFAULTTUNE)
+ warn("${PROJECT_NAME} will not be built for ${CMAKE_SYSTEM_PROCESSOR_DEFAULTTUNE} architecture")
+ else()
+ warn("${PROJECT_NAME} will not be built for ${CMAKE_SYSTEM_PROCESSOR} architecture")
+ endif()
+endif()
Index: /trunk/demos/TwoWheelRobotSimpleFleet/simulator/CMakeLists.txt
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/simulator/CMakeLists.txt (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/simulator/CMakeLists.txt (revision 390)
@@ -0,0 +1,16 @@
+PROJECT(TwoWheelRobotSimpleFleet_simulator)
+cmake_minimum_required(VERSION 2.8)
+
+SET(FLAIR_USE_SIMULATOR TRUE)
+SET(FLAIR_USE_SIMULATOR_GL TRUE)
+
+include($ENV{FLAIR_ROOT}/flair-src/cmake-modules/GlobalCmakeFlair.cmake)
+
+SET(SRC_FILES
+ ./src/main.cpp
+)
+
+FLAIR_DEMO_HOST_CXX(${PROJECT_NAME} "${SRC_FILES}"
+ DEST_DIR TwoWheelRobotSimpleFleet
+ COPY_RESOURCES
+)
Index: /trunk/demos/TwoWheelRobotSimpleFleet/simulator/resources/core2-64/simulator.sh
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/simulator/resources/core2-64/simulator.sh (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/simulator/resources/core2-64/simulator.sh (revision 390)
@@ -0,0 +1,10 @@
+#! /bin/bash
+if [ -f /proc/xenomai/version ];then
+ EXEC=./TwoWheelRobotSimpleFleet_simulator_rt
+else
+ EXEC=./TwoWheelRobotSimpleFleet_simulator_nrt
+fi
+
+. $FLAIR_ROOT/flair-src/scripts/distribution_specific_hack.sh
+
+$EXEC -n ugv -p 9000 -a 127.0.0.1 -x simulator.xml -o 10 -m $FLAIR_ROOT/flair-src/models -s $FLAIR_ROOT/flair-src/models/indoor_flight_arena.xml
Index: /trunk/demos/TwoWheelRobotSimpleFleet/simulator/resources/core2-64/simulator.xml
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/simulator/resources/core2-64/simulator.xml (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/simulator/resources/core2-64/simulator.xml (revision 390)
@@ -0,0 +1,6 @@
+
+
+
+
+
+
Index: /trunk/demos/TwoWheelRobotSimpleFleet/simulator/src/main.cpp
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/simulator/src/main.cpp (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/simulator/src/main.cpp (revision 390)
@@ -0,0 +1,108 @@
+// created: 2020/11/20
+// filename: main.cpp
+//
+// author: Guillaume Sanahuja
+// Copyright Heudiasyc UMR UTC/CNRS 6599
+//
+// version: $Id: $
+//
+// purpose: main simulateur
+//
+//
+/*********************************************************************/
+
+#include
+#include
+#include
+#ifdef GL
+#include
+#endif
+
+using namespace TCLAP;
+using namespace std;
+using namespace flair::simulator;
+using namespace flair::sensor;
+
+int port;
+int opti_time;
+string xml_file;
+string media_path;
+string scene_file;
+string name;
+string address;
+
+void parseOptions(int argc, char** argv)
+{
+ try {
+ CmdLine cmd("Command description message", ' ', "0.1");
+
+ ValueArg nameArg("n", "name", "uav name, also used for vrpn", true, "x4", "string");
+ cmd.add(nameArg);
+
+ ValueArg xmlArg("x", "xml", "xml file", true, "./reglages.xml", "string");
+ cmd.add(xmlArg);
+
+ ValueArg portArg("p", "port", "ground station port", true, 9002, "int");
+ cmd.add(portArg);
+
+ ValueArg addressArg("a", "address", "ground station address", true, "127.0.0.1", "string");
+ cmd.add(addressArg);
+
+ ValueArg optiArg("o", "opti", "optitrack time ms", false, 0, "int");
+ cmd.add(optiArg);
+
+#ifdef GL
+ ValueArg mediaArg("m", "media", "path to media files", true, "./", "string");
+ cmd.add(mediaArg);
+
+ ValueArg sceneArg("s", "scene", "path to scene file", true, "./voliere.xml", "string");
+ cmd.add(sceneArg);
+#endif
+
+ cmd.parse(argc, argv);
+
+ // Get the value parsed by each arg.
+ port = portArg.getValue();
+ xml_file = xmlArg.getValue();
+ opti_time = optiArg.getValue();
+ name = nameArg.getValue();
+ address = addressArg.getValue();
+#ifdef GL
+ media_path = mediaArg.getValue();
+ scene_file = sceneArg.getValue();
+#endif
+
+ } catch(ArgException& e) {
+ cerr << "error: " << e.error() << " for arg " << e.argId() << endl;
+ exit(EXIT_FAILURE);
+ }
+}
+
+int main(int argc, char* argv[]) {
+ Simulator* simu;
+#ifdef GL
+ Parser* gui;
+#endif
+ parseOptions(argc, argv);
+
+ simu = new Simulator("simulator", opti_time, 90);
+ simu->SetupConnection(address, port);
+ simu->SetupUserInterface(xml_file);
+
+#ifdef GL
+ gui = new Parser(960, 480, 960, 480, media_path, scene_file);
+#endif
+ for(int i=0; i<2;i++) {
+ stringstream ugvName;
+ Model *robot;
+ ugvName << name.c_str() << "_" << i;
+ robot = new TwoWheelRobot(ugvName.str(), i);
+ }
+
+ simu->RunSimu();
+
+ delete simu;
+
+ return 0;
+}
+
Index: /trunk/demos/TwoWheelRobotSimpleFleet/ugv/CMakeLists.txt
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/ugv/CMakeLists.txt (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/ugv/CMakeLists.txt (revision 390)
@@ -0,0 +1,19 @@
+PROJECT(TwoWheelRobotSimpleFleet_ugv)
+cmake_minimum_required(VERSION 2.8)
+
+SET(FLAIR_USE_FILTER TRUE)
+SET(FLAIR_USE_SENSOR_ACTUATOR TRUE)
+SET(FLAIR_USE_META TRUE)
+SET(FLAIR_USE_VRPN TRUE)
+
+include($ENV{FLAIR_ROOT}/flair-src/cmake-modules/GlobalCmakeFlair.cmake)
+
+SET(SRC_FILES
+ src/main.cpp
+ src/SimpleFleet.cpp
+)
+
+FLAIR_DEMO(${PROJECT_NAME} "${SRC_FILES}"
+ DEST_DIR TwoWheelRobotSimpleFleet
+ COPY_RESOURCES
+)
Index: /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/setup_simu0.xml
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/setup_simu0.xml (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/setup_simu0.xml (revision 390)
@@ -0,0 +1,80 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/setup_simu1.xml
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/setup_simu1.xml (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/setup_simu1.xml (revision 390)
@@ -0,0 +1,80 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/setup_sumo0.xml
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/setup_sumo0.xml (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/setup_sumo0.xml (revision 390)
@@ -0,0 +1,80 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/setup_sumo1.xml
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/setup_sumo1.xml (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/setup_sumo1.xml (revision 390)
@@ -0,0 +1,80 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Index: /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/ugv_simu0.sh
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/ugv_simu0.sh (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/ugv_simu0.sh (revision 390)
@@ -0,0 +1,11 @@
+#! /bin/bash
+
+if [ -f /proc/xenomai/version ];then
+ EXEC=./TwoWheelRobotSimpleFleet_ugv_rt
+else
+ EXEC=./TwoWheelRobotSimpleFleet_ugv_nrt
+fi
+
+. $FLAIR_ROOT/flair-src/scripts/distribution_specific_hack.sh
+
+$EXEC -n ugv_0 -t ugv_simu0 -a 127.0.0.1 -p 9000 -l /tmp -x setup_simu0.xml -b 127.255.255.255:20010
Index: /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/ugv_simu1.sh
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/ugv_simu1.sh (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/ugv_simu1.sh (revision 390)
@@ -0,0 +1,11 @@
+#! /bin/bash
+
+if [ -f /proc/xenomai/version ];then
+ EXEC=./TwoWheelRobotSimpleFleet_ugv_rt
+else
+ EXEC=./TwoWheelRobotSimpleFleet_ugv_nrt
+fi
+
+. $FLAIR_ROOT/flair-src/scripts/distribution_specific_hack.sh
+
+$EXEC -n ugv_1 -t ugv_simu1 -a 127.0.0.1 -p 9000 -l /tmp -x setup_simu1.xml -b 127.255.255.255:20010 -d 21000
Index: /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/ugv_sumo0.sh
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/ugv_sumo0.sh (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/ugv_sumo0.sh (revision 390)
@@ -0,0 +1,11 @@
+#! /bin/bash
+
+if [ -f /proc/xenomai/version ];then
+ EXEC=./TwoWheelRobotSimpleFleet_ugv_rt
+else
+ EXEC=./TwoWheelRobotSimpleFleet_ugv_nrt
+fi
+
+. $FLAIR_ROOT/flair-src/scripts/distribution_specific_hack.sh
+
+$EXEC -n ugv_0 -t sumo -a 127.0.0.1 -p 9000 -l /tmp -x setup_sumo0.xml -b 172.26.223.255:20010
Index: /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/ugv_sumo1.sh
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/ugv_sumo1.sh (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/ugv/resources/core2-64/ugv_sumo1.sh (revision 390)
@@ -0,0 +1,11 @@
+#! /bin/bash
+
+if [ -f /proc/xenomai/version ];then
+ EXEC=./TwoWheelRobotSimpleFleet_ugv_rt
+else
+ EXEC=./TwoWheelRobotSimpleFleet_ugv_nrt
+fi
+
+. $FLAIR_ROOT/flair-src/scripts/distribution_specific_hack.sh
+
+$EXEC -n ugv_1 -t sumo -a 127.0.0.1 -p 9000 -l /tmp -x setup_sumo1.xml -b 172.26.223.255:20010
Index: /trunk/demos/TwoWheelRobotSimpleFleet/ugv/src/SimpleFleet.cpp
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/ugv/src/SimpleFleet.cpp (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/ugv/src/SimpleFleet.cpp (revision 390)
@@ -0,0 +1,241 @@
+// created: 2020/12/21
+// filename: SimpleFleet.cpp
+//
+// author: Guillaume Sanahuja
+// Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+// version: $Id: $
+//
+// purpose: demo simple fleet avec optitrack
+//
+//
+/*********************************************************************/
+
+#include "SimpleFleet.h"
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+using namespace std;
+using namespace flair::core;
+using namespace flair::gui;
+using namespace flair::sensor;
+using namespace flair::filter;
+using namespace flair::meta;
+using namespace flair::actuator;
+
+SimpleFleet::SimpleFleet(string name,string broadcast,TargetController *controller): Thread(getFrameworkManager(),"SimpleFleet",50), behaviourMode(BehaviourMode_t::Manual), vrpnLost(false) {
+ this->controller=controller;
+ controller->Start();
+
+ Ugv* ugv=GetUgv();
+ ugv->UseDefaultPlot();
+
+ VrpnClient* vrpnclient=new VrpnClient("vrpn", ugv->GetDefaultVrpnAddress(),80);
+ ugvVrpn = new MetaVrpnObject(name);
+
+ getFrameworkManager()->AddDeviceToLog(ugvVrpn);
+ vrpnclient->Start();
+
+ Tab *ugvTab = new Tab(getFrameworkManager()->GetTabWidget(), "ugv", 0);
+ GridLayout* buttonslayout = new GridLayout(ugvTab->NewRow(), "buttons");
+ button_kill = new PushButton(buttonslayout->NewRow(), "kill");
+ startCircle=new PushButton(buttonslayout->NewRow(),"start_circle");
+ stopCircle=new PushButton(buttonslayout->LastRowLastCol(),"stop_circle");
+
+ circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
+ ugvVrpn->xPlot()->AddCurve(circle->GetMatrix()->Element(0,0),DataPlot::Blue);
+ ugvVrpn->yPlot()->AddCurve(circle->GetMatrix()->Element(0,1),DataPlot::Blue);
+ ugvVrpn->VxPlot()->AddCurve(circle->GetMatrix()->Element(1,0),DataPlot::Blue);
+ ugvVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue);
+ ugvVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle");
+
+ xCircleCenter=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"x circle center"," m",-5,5,0.1,1,0);
+ yCircleCenter=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"y circle center"," m",-5,5,0.1,1,0);
+
+ Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
+ TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
+ Tab *setupLawTab = new Tab(tabWidget, "Setup");
+ Tab *graphLawTab = new Tab(tabWidget, "Graphes");
+ uX=new Pid(setupLawTab->At(1,0),"u_x");
+ uX->UseDefaultPlot(graphLawTab->NewRow());
+ uY=new Pid(setupLawTab->At(1,1),"u_y");
+ uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
+
+ l=new DoubleSpinBox(setupLawTab->NewRow(),"L", " m", 0, 10, 0.1, 1,1);
+
+ message=new UdpSocket(ugv,"Message",broadcast,true);
+}
+
+SimpleFleet::~SimpleFleet() {
+}
+
+void SimpleFleet::Run(void) {
+ WarnUponSwitches(true);
+ SetPeriodMS(20);
+
+ if (getFrameworkManager()->ErrorOccured() == true) {
+ SafeStop();
+ }
+
+ while (!ToBeStopped()) {
+ SecurityCheck();
+ CheckMessages();
+ CheckJoystick();
+ CheckPushButton();
+
+ if(behaviourMode==BehaviourMode_t::Manual) ComputeManualControls();
+ if(behaviourMode==BehaviourMode_t::Circle) ComputeCircleControls();
+ WaitPeriod();
+ }
+}
+
+void SimpleFleet::CheckPushButton(void) {
+ if (startCircle->Clicked() == true)
+ StartCircle();
+
+ if (stopCircle->Clicked() == true)
+ StopCircle();
+
+ if (button_kill->Clicked() == true)
+ SafeStop();
+}
+
+void SimpleFleet::CheckJoystick(void) {
+ //R1 and Circle
+ if(controller->IsButtonPressed(9) && controller->IsButtonPressed(4)) {
+ StartCircle();
+ }
+
+ //R1 and Cross
+ if(controller->IsButtonPressed(9) && controller->IsButtonPressed(5)) {
+ StopCircle();
+ }
+}
+
+void SimpleFleet::SecurityCheck(void) {
+ if ((!vrpnLost) && (behaviourMode==BehaviourMode_t::Circle)) {
+ if (!ugvVrpn->IsTracked(500)) {
+ Thread::Err("VRPN, ugv lost\n");
+ vrpnLost=true;
+ StopCircle();
+ }
+ }
+}
+
+void SimpleFleet::CheckMessages(void) {
+ char msg[64];
+ char src[64];
+ size_t src_size=sizeof(src);
+ while(message->RecvMessage(msg,sizeof(msg),TIME_NONBLOCK,src,&src_size)>0) {
+ //printf("%s %s\n",GetUav()->ObjectName().c_str(),src);
+ if(strcmp(src,GetUgv()->ObjectName().c_str())!=0) {
+ if(strcmp(msg,"StartCircle")==0) {
+ Printf("StartCircle fleet\n");
+ StartCircle();
+ }
+ if(strcmp(msg,"StopCircle")==0) {
+ Printf("StopCircle fleet\n");
+ StopCircle();
+ }
+ }
+ }
+}
+
+void SimpleFleet::ComputeManualControls(void) {
+ float speed=-controller->GetAxisValue(3);
+ float turn=controller->GetAxisValue(0);
+ GetUgv()->GetUgvControls()->SetControls(speed,turn);
+}
+
+void SimpleFleet::ComputeCircleControls(void) {
+
+ Vector3Df ugv_pos,ugv_vel; // in VRPN coordinate system
+ Vector2Df ugv_2Dpos,ugv_2Dvel,target_2Dpos; // in VRPN coordinate system
+ Vector2Df pos_error,vel_error;
+ Vector2Df circle_pos,circle_vel;
+
+ ugvVrpn->GetPosition(ugv_pos);
+ ugvVrpn->GetSpeed(ugv_vel);
+
+ ugv_pos.To2Dxy(ugv_2Dpos);
+ ugv_vel.To2Dxy(ugv_2Dvel);
+
+ target_2Dpos.x=xCircleCenter->Value();
+ target_2Dpos.y=yCircleCenter->Value();
+ circle->SetCenter(target_2Dpos);
+
+ //circle reference
+ circle->Update(GetTime());
+ circle->GetPosition(circle_pos);
+ circle->GetSpeed(circle_vel);
+
+ //error in optitrack frame
+ pos_error=ugv_2Dpos-circle_pos;
+ vel_error=ugv_2Dvel-circle_vel;
+
+ uX->SetValues(pos_error.x, vel_error.x);
+ uX->Update(GetTime());
+ uY->SetValues(pos_error.y, vel_error.y);
+ uY->Update(GetTime());
+
+ //get yaw from vrpn
+ Quaternion vrpnQuaternion;
+ ugvVrpn->GetQuaternion(vrpnQuaternion);
+ float yaw=vrpnQuaternion.ToEuler().yaw;
+ float L=1;
+ float v= cosf(yaw)*uX->Output() + sinf(yaw)*uY->Output();
+ float w = -sinf(yaw)/l->Value()*uX->Output() + cosf(yaw)/l->Value()*uY->Output();
+ GetUgv()->GetUgvControls()->SetControls(-v,-w);
+}
+
+
+void SimpleFleet::StartCircle(void) {
+ if(behaviourMode!=BehaviourMode_t::Circle) {
+ Vector3Df ugv_pos;
+ Vector2Df ugv_2Dpos,target_2Dpos;
+
+ target_2Dpos.x=xCircleCenter->Value();
+ target_2Dpos.y=yCircleCenter->Value();
+ circle->SetCenter(target_2Dpos);
+
+ ugvVrpn->GetPosition(ugv_pos);
+ ugv_pos.To2Dxy(ugv_2Dpos);
+ circle->StartTraj(ugv_2Dpos);
+
+ uX->Reset();
+ uY->Reset();
+ behaviourMode=BehaviourMode_t::Circle;
+ Thread::Info("SimpleFleet: start circle\n");
+ message->SendMessage("StartCircle");
+ }
+}
+
+void SimpleFleet::StopCircle(void) {
+ if(behaviourMode==BehaviourMode_t::Circle) {
+ circle->FinishTraj();
+ //GetJoystick()->Rumble(0x70);
+ behaviourMode=BehaviourMode_t::Manual;
+ Thread::Info("SimpleFleet: finishing circle\n");
+ message->SendMessage("StopCircle");
+ }
+}
+
Index: /trunk/demos/TwoWheelRobotSimpleFleet/ugv/src/SimpleFleet.h
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/ugv/src/SimpleFleet.h (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/ugv/src/SimpleFleet.h (revision 390)
@@ -0,0 +1,73 @@
+// created: 2020/12/21
+// filename: SimpleFleet.h
+//
+// author: Guillaume Sanahuja
+// Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+// version: $Id: $
+//
+// purpose: demo simple fleet avec optitrack
+//
+//
+/*********************************************************************/
+
+#ifndef SIMPLEFLEET_H
+#define SIMPLEFLEET_H
+
+#include
+
+namespace flair {
+ namespace core {
+ class UdpSocket;
+ }
+ namespace gui {
+ class PushButton;
+ class DoubleSpinBox;
+ }
+ namespace filter {
+ class TrajectoryGenerator2DCircle;
+ class Pid;
+ }
+ namespace meta {
+ class MetaVrpnObject;
+ }
+ namespace sensor {
+ class TargetController;
+ }
+}
+
+class SimpleFleet : public flair::core::Thread {
+ public:
+ SimpleFleet(std::string name,std::string broadcast,flair::sensor::TargetController *controller);
+ ~SimpleFleet();
+
+ private:
+
+ enum class BehaviourMode_t {
+ Manual,
+ Circle
+ };
+
+ void Run(void);
+ void StartCircle(void);
+ void StopCircle(void);
+ void ComputeManualControls(void);
+ void ComputeCircleControls(void);
+ void SecurityCheck(void);
+ void CheckMessages(void);
+ void CheckJoystick(void);
+ void CheckPushButton(void);
+
+ flair::filter::Pid *uX, *uY;
+ flair::gui::PushButton *startCircle,*stopCircle,*button_kill;
+ flair::gui::DoubleSpinBox *l;
+ flair::gui::DoubleSpinBox *xCircleCenter,*yCircleCenter;
+ flair::meta::MetaVrpnObject *ugvVrpn;
+ flair::filter::TrajectoryGenerator2DCircle *circle;
+ BehaviourMode_t behaviourMode;
+ bool vrpnLost;
+ flair::sensor::TargetController *controller;
+ flair::core::UdpSocket *message;
+};
+
+#endif // SIMPLEFLEET_H
Index: /trunk/demos/TwoWheelRobotSimpleFleet/ugv/src/main.cpp
===================================================================
--- /trunk/demos/TwoWheelRobotSimpleFleet/ugv/src/main.cpp (revision 390)
+++ /trunk/demos/TwoWheelRobotSimpleFleet/ugv/src/main.cpp (revision 390)
@@ -0,0 +1,103 @@
+// created: 2020/12/21
+// filename: main.cpp
+//
+// author: Guillaume Sanahuja
+// Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+// version: $Id: $
+//
+// purpose: exemple de code ugv
+//
+//
+/*********************************************************************/
+
+#include "SimpleFleet.h"
+#include
+#include
+#include
+#include
+#include
+
+using namespace TCLAP;
+using namespace std;
+using namespace flair::core;
+using namespace flair::meta;
+using namespace flair::sensor;
+
+string ugv_type;
+string log_path;
+int port;
+int ds3port;
+string xml_file;
+string name;
+string broadcast;
+string address;
+
+void parseOptions(int argc, char** argv);
+
+
+int main(int argc, char* argv[]) {
+ parseOptions(argc,argv);
+
+ FrameworkManager *manager;
+ manager= new FrameworkManager(name);
+ manager->SetupConnection(address,port);
+ manager->SetupUserInterface(xml_file);
+ manager->SetupLogger(log_path);
+
+ Ugv* ugv=CreateUgv(name,ugv_type);
+ TargetEthController *controller=new TargetEthController("Dualshock3",ds3port);
+ SimpleFleet* demo=new SimpleFleet(name,broadcast,controller);
+
+ demo->Start();
+ demo->Join();
+
+ delete manager;
+}
+
+void parseOptions(int argc, char** argv) {
+ try {
+
+ CmdLine cmd("Command description message", ' ', "0.1");
+
+ ValueArg nameArg("n","name","uav name, also used for vrpn",true,"x4","string");
+ cmd.add( nameArg );
+
+ ValueArg typeArg("t","type","ugv type: sumo, ugv_simu or ugv_simux (with x the number of the simulated ugv)",true,"ugv_sumo","string");
+ cmd.add( typeArg );
+
+ ValueArg xmlArg("x","xml","fichier xml",true,"./reglages.xml","string");
+ cmd.add( xmlArg );
+
+ ValueArg logsArg("l","logs","repertoire des logs",true,"/media/ram","string");
+ cmd.add( logsArg );
+
+ ValueArg portArg("p","port","port pour station sol",true,9000,"int");
+ cmd.add( portArg );
+
+ ValueArg addressArg("a","address","adresse station sol",true,"127.0.0.1","string");
+ cmd.add( addressArg );
+
+ ValueArg ds3portArg("d","ds3_port","port pour ds3",false,20000,"int");
+ cmd.add( ds3portArg );
+
+ ValueArg broadcastArg("b","broadcast","broadcast ip address and port,",true,"127.255.255.255:20001","string");
+ cmd.add( broadcastArg );
+
+ cmd.parse( argc, argv );
+
+ // Get the value parsed by each arg.
+ log_path = logsArg.getValue();
+ port=portArg.getValue();
+ ds3port=ds3portArg.getValue();
+ xml_file = xmlArg.getValue();
+ name=nameArg.getValue();
+ broadcast=broadcastArg.getValue();
+ address=addressArg.getValue();
+ ugv_type=typeArg.getValue();
+
+ } catch (ArgException &e) { // catch any exceptions
+ cerr << "error: " << e.error() << " for arg " << e.argId() << endl;
+ exit(EXIT_FAILURE);
+ }
+}