From 92bed1f1dcc18181bcb3be10eb7f2b6abccf947b Mon Sep 17 00:00:00 2001 From: Aitor Martinez Date: Tue, 27 Jun 2017 13:51:20 +0200 Subject: [PATCH 1/2] [issue #833] Added limits to uav_viewer --- src/tools/uav_viewer/sensors/sensors.cpp | 58 +++++++++++++++++++++--- src/tools/uav_viewer/sensors/sensors.h | 7 +++ src/tools/uav_viewer/uav_viewer.cfg | 5 ++ 3 files changed, 64 insertions(+), 6 deletions(-) diff --git a/src/tools/uav_viewer/sensors/sensors.cpp b/src/tools/uav_viewer/sensors/sensors.cpp index 8f1118e13..fd339182c 100644 --- a/src/tools/uav_viewer/sensors/sensors.cpp +++ b/src/tools/uav_viewer/sensors/sensors.cpp @@ -62,9 +62,26 @@ Sensors::Sensors(Ice::CommunicatorPtr ic) try{ /*cast to CMDVelPrx*/ cmdprx = jderobot::CMDVelPrx::checkedCast(basecmd); + std::string temp; + if (0==cmdprx) throw "CMDVel -> Invalid proxy"; + temp = prop->getPropertyWithDefault("UAVViewer.CMDVel.Xmax", "1"); + this->maxLinX = std::stof(temp); + + temp = prop->getPropertyWithDefault("UAVViewer.CMDVel.Ymax", "1"); + this->maxLinY = std::stof(temp); + + temp = prop->getPropertyWithDefault("UAVViewer.CMDVel.Zmax", "1"); + this->maxLinZ = std::stof(temp); + + temp = prop->getPropertyWithDefault("UAVViewer.CMDVel.Ymax", "0.5"); + this->maxAngZ = std::stof(temp); + + this->maxAngX = 1; + this->maxAngY = 1; + cmdVelON = true; }catch(Ice::ConnectionRefusedException& e){ cmdVelON = false; @@ -126,12 +143,41 @@ void Sensors::sendVelocitiesToUAV(float vx,float vy,float vz,float roll,float pi { mutexDrone.lock(); jderobot::CMDVelDataPtr vel=new jderobot::CMDVelData(); - vel->linearX=vx; - vel->linearY=vy; - vel->linearZ=vz; - vel->angularZ=yaw; - vel->angularX=roll; - vel->angularY=pitch; + if (vx < this->maxLinX){ + vel->linearX=maxLinX; + }else{ + vel->linearX=vx; + } + + if (vy < this->maxLinY){ + vel->linearY=maxLinY; + }else{ + vel->linearY=vy; + } + + if (vz < this->maxLinZ){ + vel->linearZ=maxLinZ; + }else{ + vel->linearZ=vz; + } + + if (yaw < this->maxAngZ){ + vel->angularZ=maxAngZ; + }else{ + vel->angularZ=yaw; + } + + if (roll < this->maxAngX){ + vel->angularX=maxAngX; + }else{ + vel->angularX=roll; + } + + if (pitch < this->maxAngY){ + vel->angularY=maxAngY; + }else{ + vel->angularY=pitch; + } if(cmdVelON) cmdprx->setCMDVelData(vel); diff --git a/src/tools/uav_viewer/sensors/sensors.h b/src/tools/uav_viewer/sensors/sensors.h index d243d6011..3ee2274d0 100644 --- a/src/tools/uav_viewer/sensors/sensors.h +++ b/src/tools/uav_viewer/sensors/sensors.h @@ -70,6 +70,13 @@ class Sensors bool pose3dON; bool extraON; + float maxLinX; + float maxLinY; + float maxLinZ; + float maxAngZ; + float maxAngY; + float maxAngX; + }; #endif // SENSORS_H diff --git a/src/tools/uav_viewer/uav_viewer.cfg b/src/tools/uav_viewer/uav_viewer.cfg index 69d1fdedd..976c7307b 100644 --- a/src/tools/uav_viewer/uav_viewer.cfg +++ b/src/tools/uav_viewer/uav_viewer.cfg @@ -4,3 +4,8 @@ UAVViewer.CMDVel.Proxy=ardrone_cmdvel:default -h 0.0.0.0 -p 9995 UAVViewer.Navdata.Proxy=ardrone_navdata:default -h 0.0.0.0 -p 9996 UAVViewer.Extra.Proxy=ardrone_extra:default -h 0.0.0.0 -p 9994 +UAVViewer.Xmax=1 +UAVViewer.Ymax=1 +UAVViewer.Zmax=1 +UAVViewer.Yawmax=0.5 + From 0a84868e01ef22fdecc4945827c87e7cee7a58b1 Mon Sep 17 00:00:00 2001 From: Aitor Martinez Date: Wed, 28 Jun 2017 15:22:41 +0200 Subject: [PATCH 2/2] [issue #833] Solved problem with max values of cmdvel in uav_viewer --- src/tools/uav_viewer/gui/mainwindow.cpp | 8 +-- src/tools/uav_viewer/sensors/sensors.cpp | 55 ++----------------- src/tools/uav_viewer/sensors/sensors.h | 7 --- src/tools/uav_viewer/uav_viewer.cfg | 11 ++-- src/tools/uav_viewer/uav_viewer_simulated.cfg | 10 ++-- 5 files changed, 23 insertions(+), 68 deletions(-) diff --git a/src/tools/uav_viewer/gui/mainwindow.cpp b/src/tools/uav_viewer/gui/mainwindow.cpp index dd793ca68..3b3a38256 100644 --- a/src/tools/uav_viewer/gui/mainwindow.cpp +++ b/src/tools/uav_viewer/gui/mainwindow.cpp @@ -24,10 +24,10 @@ void MainWindow::setSpeeds(Ice::CommunicatorPtr ic){ Ice::PropertiesPtr prop = ic->getProperties(); - this->max_x = std::atof( prop->getPropertyWithDefault("UAVViewer.Xmax", "0.3").c_str() ); - this->max_y = std::atof( prop->getPropertyWithDefault("UAVViewer.Ymax", "0.3").c_str() ); - this->max_z = std::atof( prop->getPropertyWithDefault("UAVViewer.Zmax", "0.3").c_str() ); - this->max_yaw = std::atof( prop->getPropertyWithDefault("UAVViewer.Yawmax", "0.3").c_str() ); + this->max_x = std::atof( prop->getPropertyWithDefault("UAVViewer.Xmax", "3").c_str() )/10; + this->max_y = std::atof( prop->getPropertyWithDefault("UAVViewer.Ymax", "3").c_str() )/10; + this->max_z = std::atof( prop->getPropertyWithDefault("UAVViewer.Zmax", "3").c_str() )/10; + this->max_yaw = std::atof( prop->getPropertyWithDefault("UAVViewer.Yawmax", "3").c_str() )/10; } void MainWindow::initButtons(){ diff --git a/src/tools/uav_viewer/sensors/sensors.cpp b/src/tools/uav_viewer/sensors/sensors.cpp index fd339182c..c608a2604 100644 --- a/src/tools/uav_viewer/sensors/sensors.cpp +++ b/src/tools/uav_viewer/sensors/sensors.cpp @@ -67,21 +67,6 @@ Sensors::Sensors(Ice::CommunicatorPtr ic) if (0==cmdprx) throw "CMDVel -> Invalid proxy"; - temp = prop->getPropertyWithDefault("UAVViewer.CMDVel.Xmax", "1"); - this->maxLinX = std::stof(temp); - - temp = prop->getPropertyWithDefault("UAVViewer.CMDVel.Ymax", "1"); - this->maxLinY = std::stof(temp); - - temp = prop->getPropertyWithDefault("UAVViewer.CMDVel.Zmax", "1"); - this->maxLinZ = std::stof(temp); - - temp = prop->getPropertyWithDefault("UAVViewer.CMDVel.Ymax", "0.5"); - this->maxAngZ = std::stof(temp); - - this->maxAngX = 1; - this->maxAngY = 1; - cmdVelON = true; }catch(Ice::ConnectionRefusedException& e){ cmdVelON = false; @@ -143,41 +128,13 @@ void Sensors::sendVelocitiesToUAV(float vx,float vy,float vz,float roll,float pi { mutexDrone.lock(); jderobot::CMDVelDataPtr vel=new jderobot::CMDVelData(); - if (vx < this->maxLinX){ - vel->linearX=maxLinX; - }else{ - vel->linearX=vx; - } - - if (vy < this->maxLinY){ - vel->linearY=maxLinY; - }else{ - vel->linearY=vy; - } - if (vz < this->maxLinZ){ - vel->linearZ=maxLinZ; - }else{ - vel->linearZ=vz; - } - - if (yaw < this->maxAngZ){ - vel->angularZ=maxAngZ; - }else{ - vel->angularZ=yaw; - } - - if (roll < this->maxAngX){ - vel->angularX=maxAngX; - }else{ - vel->angularX=roll; - } - - if (pitch < this->maxAngY){ - vel->angularY=maxAngY; - }else{ - vel->angularY=pitch; - } + vel->linearX=vx; + vel->linearY=vy; + vel->linearZ=vz; + vel->angularZ=yaw; + vel->angularX=roll; + vel->angularY=pitch; if(cmdVelON) cmdprx->setCMDVelData(vel); diff --git a/src/tools/uav_viewer/sensors/sensors.h b/src/tools/uav_viewer/sensors/sensors.h index 3ee2274d0..d243d6011 100644 --- a/src/tools/uav_viewer/sensors/sensors.h +++ b/src/tools/uav_viewer/sensors/sensors.h @@ -70,13 +70,6 @@ class Sensors bool pose3dON; bool extraON; - float maxLinX; - float maxLinY; - float maxLinZ; - float maxAngZ; - float maxAngY; - float maxAngX; - }; #endif // SENSORS_H diff --git a/src/tools/uav_viewer/uav_viewer.cfg b/src/tools/uav_viewer/uav_viewer.cfg index 976c7307b..418349560 100644 --- a/src/tools/uav_viewer/uav_viewer.cfg +++ b/src/tools/uav_viewer/uav_viewer.cfg @@ -4,8 +4,11 @@ UAVViewer.CMDVel.Proxy=ardrone_cmdvel:default -h 0.0.0.0 -p 9995 UAVViewer.Navdata.Proxy=ardrone_navdata:default -h 0.0.0.0 -p 9996 UAVViewer.Extra.Proxy=ardrone_extra:default -h 0.0.0.0 -p 9994 -UAVViewer.Xmax=1 -UAVViewer.Ymax=1 -UAVViewer.Zmax=1 -UAVViewer.Yawmax=0.5 + + +# Following values will be divided by 10 to be used (3 --> 0.3 m/s) because atof doesn't work well (Truncates the decimal part) +UAVViewer.Xmax=3 +UAVViewer.Ymax=3 +UAVViewer.Zmax=3 +UAVViewer.Yawmax=3 diff --git a/src/tools/uav_viewer/uav_viewer_simulated.cfg b/src/tools/uav_viewer/uav_viewer_simulated.cfg index 2b3f81bce..d2d4c8b74 100644 --- a/src/tools/uav_viewer/uav_viewer_simulated.cfg +++ b/src/tools/uav_viewer/uav_viewer_simulated.cfg @@ -3,8 +3,10 @@ UAVViewer.Pose3D.Proxy=Pose3D:default -h 0.0.0.0 -p 9000 UAVViewer.CMDVel.Proxy=CMDVel:default -h 0.0.0.0 -p 9000 UAVViewer.Navdata.Proxy=Navdata:default -h 0.0.0.0 -p 9000 UAVViewer.Extra.Proxy=Extra:default -h 0.0.0.0 -p 9000 -UAVViewer.Xmax=10 -UAVViewer.Ymax=10 -UAVViewer.Zmax=10 -UAVViewer.Yawmax=10 + +# Following values will be divided by 10 to be used (3 --> 0.3 m/s) because atof doesn't work well (Truncates the decimal part) +UAVViewer.Xmax=100 +UAVViewer.Ymax=100 +UAVViewer.Zmax=100 +UAVViewer.Yawmax=100