#include <trgen.h>
#include <map.h>
#include <robomath.h>
-#include <hokuyo.h>
+#include <lidar_params.h>
#include <actuators.h>
#include "PlaygroundScene.h"
#include "MiscGui.h"
playgroundScene->addItem(trailPosIndepOdo);
playgroundScene->addItem(trailOdoPos);
- hokuyoScan = new HokuyoScan();
+ hokuyoScan = new LidarScan(hokuyo_params);
hokuyoScan->setZValue(10);
playgroundScene->addItem(hokuyoScan);
+ sickScan = new LidarScan(sick_params);
+ sickScan->setZValue(10);
+ playgroundScene->addItem(sickScan);
+
}
void RobomonAtlantis::createMap()
{
if(state) {
robottype_publisher_sick_scan_create(&orte, NULL, this);
+ robottype_publisher_hokuyo_scan_create(&orte, NULL, this);
} else {
if (!simulationEnabled)
return;
robottype_publisher_sick_scan_destroy(&orte);
+ robottype_publisher_hokuyo_scan_destroy(&orte);
}
simulationEnabled = state;
}
void RobomonAtlantis::setObstacleSimulation(int state)
{
if (state) {
- /* TODO Maybe it is possible to attach only once to Shmap */
- ShmapInit(0);
- obstacleSimulationTimer = new QTimer(this);
- connect(obstacleSimulationTimer, SIGNAL(timeout()),
+ /* TODO Maybe it is possible to attach only once to Shmap */
+ ShmapInit(0);
+ obstacleSimulationTimer = new QTimer(this);
+ connect(obstacleSimulationTimer, SIGNAL(timeout()),
this, SLOT(simulateObstaclesHokuyo()));
- obstacleSimulationTimer->start(100);
- setMouseTracking(true);
+ connect(obstacleSimulationTimer, SIGNAL(timeout()),
+ this, SLOT(simulateObstaclesSick()));
+ obstacleSimulationTimer->start(100);
+ setMouseTracking(true);
hokuyoScan->setVisible(true);
+ sickScan->setVisible(true);
} else {
if (obstacleSimulationTimer)
delete obstacleSimulationTimer;
-
- hokuyoScan->setVisible(false); /* hide hokuyo scan*/
+ // Hide scans of lidars
+ hokuyoScan->setVisible(false);
+ sickScan->setVisible(false);
}
}
-void RobomonAtlantis::simulateObstaclesHokuyo()
+void RobomonAtlantis::simulateObstaclesLidar(const struct lidar_params lidar)
{
double distance, wall_distance;
unsigned int i;
- uint16_t *hokuyo = orte.hokuyo_scan.data;
-
- for (i=0; i<HOKUYO_ARRAY_SIZE; i++) {
- wall_distance = distanceToWallHokuyo(i);
+ unsigned int data_lenght = 0;
+ uint16_t *lidar_data = NULL;
+ switch (lidar.type) {
+ case HOKUYO:
+ lidar_data = orte.hokuyo_scan.data;
+ data_lenght = hokuyo_params.data_lenght;
+ break;
+ case SICK_TIM3XX:
+ lidar_data = orte.sick_scan.data;
+ data_lenght = sick_params.data_lenght;
+ break;
+ default:
+ return;
+ }
- distance = distanceToCircularObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M);
+ for (i = 0; i < data_lenght; i++) {
+ wall_distance = distanceToWallLidar(lidar, i);
+ distance = distanceToCircularObstacleLidar(lidar, i, simulatedObstacle, SIM_OBST_SIZE_M);
if (wall_distance < distance)
distance = wall_distance;
- hokuyo[i] = distance*1000;
+ lidar_data[i] = distance*1000;
+ }
+
+ switch (lidar.type) {
+ case HOKUYO:
+ orte.hokuyo_scan.data_lenght = hokuyo_params.data_lenght;
+ orte.hokuyo_scan.lidar_type = hokuyo_params.type;
+ ORTEPublicationSend(orte.publication_hokuyo_scan);
+ break;
+ case SICK_TIM3XX:
+ orte.sick_scan.data_lenght = sick_params.data_lenght;
+ orte.sick_scan.lidar_type = sick_params.type;
+ ORTEPublicationSend(orte.publication_sick_scan);
+ break;
+ default:
+ return;
}
- ORTEPublicationSend(orte.publication_hokuyo_scan);
+}
+
+void RobomonAtlantis::simulateObstaclesHokuyo()
+{
+ simulateObstaclesLidar(hokuyo_params);
+}
+void RobomonAtlantis::simulateObstaclesSick()
+{
+ simulateObstaclesLidar(sick_params);
}
void RobomonAtlantis::changeObstacle(QPointF position)
simulatedObstacle.x = position.x();
simulatedObstacle.y = position.y();
- simulateObstaclesHokuyo();
+ simulateObstaclesLidar(hokuyo_params);
+ simulateObstaclesLidar(sick_params);
}
/**********************************************************************
emit motionStatusReceivedSignal();
break;
case QEVENT(QEV_SICK_SCAN):
- hokuyoScan->newScan(&orte.sick_scan);
+ sickScan->newScan(&orte.sick_scan);
+ break;
+ case QEVENT(QEV_HOKUYO_SCAN):
+ hokuyoScan->newScan(&orte.hokuyo_scan);
break;
case QEVENT(QEV_JAWS_CMD):
robotEstPosBest->setJaws(orte.jaws_cmd.req_pos.left);
hokuyoScan->setPosition(orte.est_pos_best.x,
orte.est_pos_best.y,
orte.est_pos_best.phi);
+ sickScan->setPosition(orte.est_pos_best.x,
+ orte.est_pos_best.y,
+ orte.est_pos_best.phi);
break;
case QEVENT(QEV_POWER_VOLTAGE):
emit powerVoltageReceivedSignal();
generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_BEST));
robottype_subscriber_sick_scan_create(&orte,
generic_rcv_cb, new OrteCallbackInfo(this, QEV_SICK_SCAN));
+ robottype_subscriber_hokuyo_scan_create(&orte,
+ generic_rcv_cb, new OrteCallbackInfo(this, QEV_HOKUYO_SCAN));
robottype_subscriber_jaws_cmd_create(&orte,
generic_rcv_cb, new OrteCallbackInfo(this, QEV_JAWS_CMD));
robottype_subscriber_fsm_main_create(&orte,
sharedMemoryOpened = true;
}
-double RobomonAtlantis::distanceToWallHokuyo(int beamnum)
+double RobomonAtlantis::distanceToWallLidar(const struct lidar_params lidar, int beamnum)
{
double distance = 4.0, min_distance = 4.0;
int i, j;
// WALL
ShmapCell2Point(i, j, &wall.x, &wall.y);
- distance = distanceToObstacleHokuyo(beamnum, wall, MAP_CELL_SIZE_M);
+ distance = distanceToObstacleLidar(lidar, beamnum, wall, MAP_CELL_SIZE_M);
if (distance < min_distance)
min_distance = distance;
}
return min_distance;
}
-double RobomonAtlantis::distanceToCircularObstacleHokuyo(int beamnum, Point center, double diameter)
+double RobomonAtlantis::distanceToCircularObstacleLidar(const struct lidar_params lidar, int beamnum, Point center, double diameter)
{
struct robot_pos_type e = orte.est_pos_best;
double sensor_a;
struct sharp_pos s;
- s.x = HOKUYO_CENTER_OFFSET_M;
+ s.x = lidar.center_offset_m;
s.y = 0.0;
- s.ang = HOKUYO_INDEX_TO_RAD(beamnum);
+ s.ang = index2rad(lidar, beamnum);
Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
e.y + s.x*sin(e.phi) + s.y*cos(e.phi));
}
/**
- * Calculation for Hokuyo simulation. Calculates distance that would
- * be returned by Hokuyo sensors, if there is only one obstacle (as
+ * Calculation for Lidar simulation. Calculates distance that would
+ * be returned by Lidar sensors, if there is only one obstacle (as
* specified by parameters).
*
- * @param beamnum Hokuyo's bean number [0..HOKUYO_CLUSTER_CNT]
+ * @param beamnum Lidar's bean number [0..LIDAR_CLUSTER_CNT]
* @param obstacle Position of the obstacle (x, y in meters).
* @param obstacleSize Size (diameter) of the obstacle in meters.
*
* @return Distance measured by sensors in meters.
*/
-double RobomonAtlantis::distanceToObstacleHokuyo(int beamnum, Point obstacle, double obstacleSize)
+double RobomonAtlantis::distanceToObstacleLidar(const struct lidar_params lidar, int beamnum, Point obstacle, double obstacleSize)
{
struct robot_pos_type e = orte.est_pos_best;
double sensor_a;
struct sharp_pos s;
- s.x = HOKUYO_CENTER_OFFSET_M;
+ s.x = lidar.center_offset_m;
s.y = 0.0;
- s.ang = HOKUYO_INDEX_TO_RAD(beamnum);
+ s.ang = index2rad(lidar, beamnum);
Point sensor(e.x + s.x * cos(e.phi) - s.y * sin(e.phi),
e.y + s.x * sin(e.phi) + s.y * cos(e.phi));
projects / eurobot / public.git / blobdiff