[Issue #652] Modified laser Interface

src/drivers/gazeboserver/plugins/f1/laser.cc

@@ -19,6 +19,17 @@
 
 #include <jderobot/laser.h>
 
+
+class LaserD
+{
+public:
+    std::vector<float> values;
+    double minAngle = 0;
+    double maxAngle = 3.1416;
+    double minRange = 0;
+    double maxRange = 10; //10 m
+};
+
 void *mainLaser(void* v);
 
 namespace gazebo
@@ -44,6 +55,7 @@ namespace gazebo
 		// Update the controller
 		public: void OnNewLaserScans()
 		{
+
 			if(count == 0){
 				count++;
 
@@ -63,17 +75,24 @@ namespace gazebo
 
 			physics::MultiRayShapePtr laser = this->parentSensor->LaserShape();
 
-			pthread_mutex_lock (&mutex); 
+			LaserD data;
+			std::vector<float> laserValues(laser->GetSampleCount ());
+
 			laserValues.resize(laser->GetSampleCount ());
 			for (int i = 0; i< laser->GetSampleCount (); i++){
 				laserValues[i] = laser->GetRange(i);
 			}
+
+			data.values = laserValues;
+
+			pthread_mutex_lock (&mutex); 
+			laserData = data;
 			pthread_mutex_unlock (&mutex); 
 		}
 		sensors::RaySensorPtr parentSensor;
 		int count;
 		std::string nameLaser;
-		std::vector<float> laserValues;
+		LaserD laserData;
 		pthread_mutex_t mutex;
   	};
 
@@ -93,12 +112,17 @@ class LaserI: virtual public jderobot::Laser {
 		virtual jderobot::LaserDataPtr getLaserData(const Ice::Current&) {
 		    jderobot::LaserDataPtr laserData (new jderobot::LaserData());
 			pthread_mutex_lock (&laser->mutex); 
-			laserData->numLaser = laser->laserValues.size();
+			laserData->numLaser = laser->laserData.values.size();
 			laserData->distanceData.resize(sizeof(int)*laserData->numLaser);
+
+			laserData->minAngle = laser->laserData.minAngle;
+    		laserData->maxAngle = laser->laserData.maxAngle;
+    		laserData->minRange = laser->laserData.minRange;
+    		laserData->maxRange = laser->laserData.maxRange*1000;
 
 			//Update laser values
 			for(int i = 0 ; i < laserData->numLaser; i++){
-			   laserData->distanceData[i] = laser->laserValues[i]*1000;
+			   laserData->distanceData[i] = laser->laserData.values[i]*1000;
 			}
 			pthread_mutex_unlock (&laser->mutex); 
 			return laserData;

src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotsensors.hh

@@ -37,6 +37,16 @@
 
 namespace turtlebot{
 
+class LaserD
+{
+public:
+    std::vector<float> values;
+    double minAngle = 0;
+    double maxAngle = 3.1416;
+    double minRange = 0;
+    double maxRange = 10; //10 m
+};
+
 class TurtlebotSensors
 {
 public:
@@ -78,7 +88,7 @@ private:
 
 public:
     cv::Mat img[NUM_CAMS];
-    std::vector<float> laserValues;
+    LaserD laserData;
     gazebo::math::Pose pose;
 
     double position;

src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/laseri.cpp

@@ -15,12 +15,24 @@ LaserI::~LaserI ()
 LaserDataPtr
 LaserI::getLaserData ( const Ice::Current& ){
 
-    laserdata->numLaser = sensor->laserValues.size();
+    laserdata->numLaser = sensor->laserData.values.size();
     laserdata->distanceData.resize(sizeof(int)*laserdata->numLaser);
+    laserdata->minAngle = sensor->laserData.minAngle;
+    laserdata->maxAngle = sensor->laserData.maxAngle;
+    laserdata->minRange = sensor->laserData.minRange;
+    laserdata->maxRange = sensor->laserData.maxRange*1000;
+
 
     //Update laser values
     for(int i = 0 ; i < laserdata->numLaser; i++){
-       laserdata->distanceData[i] = sensor->laserValues[i]*1000;
+       laserdata->distanceData[i] = sensor->laserData.values[i]*1000;
     }
     return laserdata;
 }
+
+
+
+
+
+
+

src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotsensors.cc

@@ -118,22 +118,29 @@ TurtlebotSensors::_on_cam(int id){
 
 void
 TurtlebotSensors::_on_laser(){
-    /*assert(laser->GetRangeCount() > 0);
-    std::vector<double> ranges(laser->GetRangeCount());
-    laser->GetRanges(ranges);
-    std::sort(ranges.begin(), ranges.end());
-    //altitude = ranges[0];
-    int c = std::ceil(ranges.size()*0.20); // smooth value by take 20% of minor values
-    ranges.resize(c);*/
+    LaserD data;
+
 
     //laser values
     MultiRayShapePtr laserV = this->laser->LaserShape();
 
 
-    laserValues.resize(laserV->GetSampleCount ());
+
+
+    std::vector<float> laserValues(laserV->GetSampleCount ());
     for (int i = 0; i< laserV->GetSampleCount (); i++){
         laserValues[i] = laserV->GetRange(i);
     }
+
+    data.values = laserValues;
+    //This values don't work well
+    //data.minAngle = this->laser->AngleMin().Radian();
+    //data.maxAngle = this->laser->AngleMax().Radian();
+    //data.minRange = this->laser->RangeMin();
+    //data.maxAngle = this->laser->RangeMax();
+
+    laserData = data;
+
 }
 
 void

src/drivers/laser_server/hokuyo/hokuyomanager.cpp

@@ -32,6 +32,8 @@ std::cout<< "range_res: " << config.range_res << std::endl;
 	{
 	    data->numLaser = scan.ranges.size();
 	    data->distanceData.resize(sizeof(int)*data->numLaser);
+	    data->maxRange = scan.config.max_range*1000;
+	    data->minRange = 0;
         if (this->faceup!=0){
             for(int i = 0 ; i < data->numLaser; i++){
                 if (std::numeric_limits<float>::infinity() == scan.ranges[i]){
@@ -59,6 +61,9 @@ std::cout<< "range_res: " << config.range_res << std::endl;
 	*HokuyoManager::getLaserData()
 	{
 	    jderobot::LaserData *data = new jderobot::LaserData();
+            data->minAngle = this->min_ang;
+            data->maxAngle = this->max_ang;
+
 	    hokuyo::LaserScan  scan;
 	    int res;
 	    mutex.lock();

src/interfaces/slice/jderobot/laser.ice

@@ -31,7 +31,12 @@ module jderobot{
   class LaserData
   {
     IntSeq distanceData;
-	int numLaser;
+    int numLaser;
+    float minAngle;
+    float maxAngle;
+    float minRange;
+    float maxRange;
+    Time timeStamp;
   };
 
 

src/tools/kobukiViewer/CMakeLists.txt

@@ -13,7 +13,7 @@ if (${QT5_COMPILE})
     )
 
 
-    SET(HEADERS robot/robot.h gui/gui.h gui/widget/cameraswidget.h gui/widget/controlvw.h gui/widget/glwidget.h )
+    SET(HEADERS robot/robot.h gui/gui.h gui/widget/cameraswidget.h gui/widget/controlvw.h gui/widget/glwidget.h robot/types.h)
     SET(RESOURCES resources.qrc)
 
 	SET(SOURCE main.cpp depuratewindow.cpp 

src/tools/kobukiViewer/gui/widget/glwidget.cpp

@@ -368,10 +368,10 @@ void GLWidget::drawLaser()
 
     v3f(0, 0,  2);
 
-    for (unsigned k = 0; k < laserData.size(); k++) {
+    for (unsigned k = 0; k < laserData.values.size(); k++) {
 
-        Xp_sensor = this->laserData[k]/100 * cos(((float) k - 90.) * 3.14/180);
-        Yp_sensor = this->laserData[k]/100 * sin(((float) k - 90.) * 3.14/180);
+        Xp_sensor = this->laserData.values[k]/100 * cos(((float) k - 90.) * 3.14/180);
+        Yp_sensor = this->laserData.values[k]/100 * sin(((float) k - 90.) * 3.14/180);
 
         v3f(Xp_sensor, Yp_sensor,  2.0f);
 

src/tools/kobukiViewer/gui/widget/glwidget.h

@@ -79,7 +79,7 @@ class GLWidget : public QGLWidget
 
 private:
 
-    std::vector<float> laserData;
+    LaserD laserData;
 
     StateGUI *stategui;
     Robot* robot;

src/tools/kobukiViewer/gui/widget/laserwidget.cpp

@@ -1,6 +1,5 @@
 #include "laserwidget.h"
-#include <iostream>
-#include <math.h>
+
 LaserWidget::LaserWidget()
 {
 
@@ -12,13 +11,14 @@ LaserWidget::LaserWidget()
     setAutoFillBackground(true);
     setPalette(Pal);
 
-    setMaximumSize(500, 250);
+    setMaximumSize(500, 500);
+    setMinimumSize(500, 500);
 
 
     setWindowTitle("Laser");
 }
 
-void LaserWidget::update(std::vector<float> laserData)
+void LaserWidget::update(LaserD laserData)
 {
     mutex.lock();
 
@@ -32,6 +32,11 @@ void LaserWidget::update(std::vector<float> laserData)
 void LaserWidget::paintEvent(QPaintEvent *)
 {
     int _width = width();
+    int _height = height();
+
+    //center of laser
+    int cx = _width/2;
+    int cy = _height/2; 
 
     float x0, y0, x1, y1, d, ang;
 
@@ -41,21 +46,28 @@ void LaserWidget::paintEvent(QPaintEvent *)
     QPainter painter(this);
     painter.setPen(pen);
 
+    pen = QPen(Qt::green, width);
+    painter.setPen(pen);
+
+    painter.drawPoint(QPointF(20,480));
+
     pen = QPen(Qt::blue, width);
     painter.setPen(pen);
 
-    float PI = 3.1416;
+    float step = (this->laserData.maxAngle - this->laserData.minAngle) /this->laserData.values.size();
+
+    d = this->laserData.maxRange/(_width/2); //normalizing distances 
+
+    ang = this->laserData.minAngle;
+    x0 = cx + (this->laserData.values[0] / d) * cos(ang);
+    y0 = cy - ((this->laserData.values[0] / d) * sin(ang));
+    for (int i = 1; i < this->laserData.values.size(); i++) {
 
-    ang = 0;
-    x0 = _width/2 + (this->laserData[0] / d) * cos(ang);
-    y0 = _width/2 - ((this->laserData[0] / d) * sin(ang));
-    d = 10000/(_width/2);
-    for (int i = 1; i < this->laserData.size(); i++) {
 
 
-        ang = i*PI/this->laserData.size();
-        x1 = _width/2 + (this->laserData[i] / d) * cos(ang);
-        y1 = _width/2 - ((this->laserData[i] / d) * sin(ang));
+        ang = this->laserData.minAngle + i*step;
+        x1 = cx + (this->laserData.values[i] / d) * cos(ang);
+        y1 = cy - ((this->laserData.values[i] / d) * sin(ang));
 
         painter.drawLine(QPointF(x0,y0), QPointF(x1,y1));
 

src/tools/kobukiViewer/gui/widget/laserwidget.h

@@ -1,19 +1,23 @@
 #ifndef LASERWIDGET_H
 #define LASERWIDGET_H
 
+
+#include <iostream>
+#include <math.h>
 #include <QtWidgets>
+#include "../../robot/types.h"
 
 class LaserWidget: public QWidget
 {
 public:
     LaserWidget();
-    void update(std::vector<float> laserData);
+    void update(LaserD laserData);
 
 protected:
     void paintEvent(QPaintEvent *);
 
     QMutex mutex;
-    std::vector<float> laserData;
+    LaserD laserData;
 
 };
 

src/tools/kobukiViewer/robot/sensors.cpp

@@ -193,10 +193,21 @@ void Sensors::update()
 
 	if (laserON) {
 		ld = laserprx->getLaserData();
+		laserData.minRange = ld->minRange;
+    	laserData.maxRange = ld->maxRange;
+    	laserData.minAngle = ld->minAngle;
+    	laserData.maxAngle = ld->maxAngle;
+		/*std::cout << "--------------------------------------------"<< std::endl;
+		std::cout << "minAngle: " << laserData.minAngle << std::endl;
+		std::cout << "maxAngle: " << laserData.maxAngle << std::endl;
+		std::cout << "minRange: " << laserData.minRange << std::endl;
+		std::cout << "maxRange: " << laserData.maxRange << std::endl;
+		std::cout << "--------------------------------------------"<< std::endl;
+		*/
 		mutex.lock();
-		laserData.resize(ld->numLaser);
+		laserData.values.resize(ld->numLaser);
         for(int i = 0; i< ld->numLaser; i++ ){
-            laserData[i] = ld->distanceData[i];
+            laserData.values[i] = ld->distanceData[i];
         }
 		mutex.unlock();
 	}
@@ -244,14 +255,12 @@ float Sensors::getRobotPoseTheta()
     return theta;
 }
 
-std::vector<float> Sensors::getLaserData()
+LaserD Sensors::getLaserData()
 {
-    std::vector<float> laserDataAux;
+    LaserD laserDataAux;
     mutex.lock();
 	if (laserON)
 	    laserDataAux = laserData;
-	else
-		laserDataAux = {0};
     mutex.unlock();
     return laserDataAux;
 }

src/tools/kobukiViewer/robot/sensors.h

@@ -10,6 +10,8 @@
 #include <jderobot/camera.h>
 #include <jderobot/laser.h>
 
+#include "types.h"
+
 //Opencv
 #include <opencv2/core/core.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
@@ -25,7 +27,7 @@ class Sensors
     float getRobotPoseX();
     float getRobotPoseY();
     float getRobotPoseTheta();
-    std::vector<float> getLaserData();
+    LaserD getLaserData();
 
     cv::Mat getCamera2();
     cv::Mat getCamera1();
@@ -58,7 +60,7 @@ class Sensors
     bool boolLaser;
     jderobot::LaserPrx laserprx;
     jderobot::LaserDataPtr ld;
-    std::vector<float> laserData;
+    LaserD laserData;
 
     //CAMERADATA7