int32_t KobukiMobileController::GetPosition(OPRoS::MobilePositionData &position)
{
double x, y, radian;
getOdometry(x, y, radian);
position.x = x;
position.y = y;
position.theta = radian * 180.0 / M_PI;
return API_SUCCESS;
}
const bool RobotAction::rotation(const double& swingRange) {
MessageFreqMgr requestOdo;
sendMessageFreq(requestOdo);
Sleep(sizeof(requestOdo));
this->buzyWaitForMgr(500);
OdometryMgr curPos;
getOdometry(curPos);
this->getCurrentTime();
/* Prepare subgoal message */
SubgoalMgr goal;
goal.x = curPos.x;
goal.y = curPos.y;
goal.theta = curPos.theta + swingRange * 2.0; // Multiple 2 to speed up the rotation
/* Send subgoal message */
sendSubgoal(goal);
Sleep(sizeof(goal) + 2000);
// Turn to the another direction
goal.theta = curPos.theta + swingRange * -2.0;
sendSubgoal(goal);
Sleep(sizeof(goal) + 4000);
/* Turn back */
goal.theta = curPos.theta + swingRange * 2.0; // To speed up the turn back
sendSubgoal(goal);
Sleep(sizeof(goal) + 2000);
goal.theta = curPos.theta;
sendSubgoal(goal);
naviExecuting = true;
Sleep(sizeof(goal));
while (this->executingTime() <= timeout) {
if (naviExecuting == false)
return true;
Sleep(50);
}
// Timeout
cout << "> WARNING: Action, Rotation, timeout" << endl;
return false;
}
/**
* @brief RobotThread::RobotThread
*/
RobotThread::RobotThread()
{
// necessary to send this type over qued connections
qRegisterMetaType< QVector<double> >("QVector<double>");
qRegisterMetaType< QList<Obstacle> >("QList<Obstacle>");
qRegisterMetaType< QList< QPair<double,double> > >("QList< QPair<double,double> >"); // fuer path planning -> path realizer
qRegisterMetaType< PathPlotData >("PathPlotData");
qRegisterMetaType< FilterParams >("FilterParams");
qRegisterMetaType< CameraParams >("CameraParams");
qRegisterMetaType< PIDParams >("PIDParams");
qRegisterMetaType< PIDPlotData >("PIDPlotData");
qRegisterMetaType< cv::Mat >("cv::Mat");
qRegisterMetaType< QString >("QString"); //Log
qRegisterMetaType< LaserPlotData > ("LaserPlotData");
qRegisterMetaType< Position > ("Position");
qRegisterMetaType< TeamColor >("TeamColor");
qRegisterMetaType< CamColor >("CamColor");
qDebug() << "INIT 'RobotThread'";
//initialize Objects for Actors and Sensory
actorLowLevel = new ActorLowLevel();
actorHighLevel = new ActorHighLevel();
sensorHighLevel = new SensorHighLevel();
sensorLowLevel = new SensorLowLevel();
pathPlanner = new PathPlanning();
gameEngine = new GameEngine();
cam = new Cam();
// moving objects to different threads
sensorLowLevel->moveToThread(&threadRobotLowLevel);
sensorHighLevel->moveToThread(&threadSensorHighLevel);
actorLowLevel->moveToThread(&threadRobotLowLevel);
actorHighLevel->moveToThread(&threadActorHighLevel);
pathPlanner->moveToThread(&threadPathPlanner);
gameEngine->moveToThread(&threadGameEngine);
cam->moveToThread(&threadCam);
// Name the threads (helpful for debugging)
threadRobotLowLevel.setObjectName("Robot Low Level Thread");
threadSensorHighLevel.setObjectName("High Level Sensor Thread");
threadPathRealizer.setObjectName("Path Realizer Thread");
threadPathPlanner.setObjectName("Path Planner Thread");
threadGameEngine.setObjectName("Game Engine Thread");
threadCam.setObjectName("Cam Thread");
// ***********************************************************************
// DELETE LATER
connect(&threadRobotLowLevel, SIGNAL(finished()),
sensorLowLevel, SLOT(deleteLater()));
connect(&threadSensorHighLevel, SIGNAL(finished()),
sensorHighLevel, SLOT(deleteLater()));
connect(&threadRobotLowLevel, SIGNAL(finished()),
actorLowLevel, SLOT(deleteLater()));
connect(&threadActorHighLevel, SIGNAL(finished()),
actorHighLevel, SLOT(deleteLater()));
connect(&threadPathPlanner, SIGNAL(finished()),
pathPlanner, SLOT(deleteLater()));
connect(&threadCam, SIGNAL(finished()),
cam, SLOT(deleteLater()));
connect(&threadGameEngine, SIGNAL(finished()),
gameEngine, SLOT(deleteLater()));
// ***********************************************************************
// SIGNALS FROM GUI
// start Orientation
connect(mainWindow,SIGNAL(signalStartOrientation(bool)),
sensorHighLevel, SLOT(slotStartDetection(bool)));
// remote controle over GUI
connect(mainWindow,SIGNAL(robotRemoteControllUpdate(double,double)),
actorLowLevel,SLOT(setRobotRemoteControllParams(double,double)));
// remote Odometry update
connect(mainWindow,SIGNAL(updateRemoteOdometry(Position)),
actorLowLevel, SLOT(setOdometry(Position)));
// Cam Display
// Camera Parameters in GUI have changed, send to Cam class
connect(mainWindow, SIGNAL(signalChangeCamParams(CameraParams)),
cam, SLOT(slotSetCameraParams(CameraParams)));
// Set Median Filter
connect(mainWindow, SIGNAL(signalChangeFilterParams(FilterParams)),
sensorHighLevel, SLOT(slotSetFilterParams(FilterParams)));
// PID Tab
connect(mainWindow, SIGNAL(signalChangePIDParams(PIDParams)),
actorHighLevel, SLOT(slotChangePIDParams(PIDParams)));
// ***********************************************************************
// SIGNALS TO GUI
// Sensor Display
// Display the sensor data in GUI
connect(sensorLowLevel, SIGNAL(signalLaserPlotRaw(LaserPlotData)), // raw data
mainWindow, SLOT(slotLaserDisplay(LaserPlotData)));
connect(sensorHighLevel, SIGNAL(signalSendLaserData(LaserPlotData)), // filtered data
mainWindow, SLOT(slotLaserDisplay(LaserPlotData)));
// Path Display
// Display potential map and raw waypoints
connect(pathPlanner, SIGNAL(pathDisplay(PathPlotData)),
mainWindow, SLOT(updatePathDisplay(PathPlotData)));
// Display calculated spline
connect(actorHighLevel, SIGNAL(signalSplinePlot(PathPlotData)),
mainWindow, SLOT(updatePathDisplay(PathPlotData)));
// Display PID Plot
connect(actorHighLevel, SIGNAL(signalPIDPlot(PIDPlotData)),
mainWindow, SLOT(slotPIDPlot(PIDPlotData)));
// Cam grabbed a frame, display in GUI
connect(cam, SIGNAL(signalDisplayFrame(cv::Mat)),
mainWindow, SLOT(slotDisplayFrame(cv::Mat)));
// ***********************************************************************
// emergency detected/received
connect(sensorHighLevel,SIGNAL(signalEmergencyStopEnabled(bool)), // emergency stop durch kollisionsvermeidung
actorLowLevel,SLOT(slotEmergencyStopEnabled(bool)));
connect(gameEngine, SIGNAL(signalEmergencyStopEnabled(bool)), // stopMovement() von Referee
actorLowLevel, SLOT(slotEmergencyStopEnabled(bool)));
//send turn command from high sensor to low level actor
connect(sensorHighLevel,SIGNAL(signalSendRobotControlParams(double,double)),
actorLowLevel,SLOT(setRobotControllParams(double,double)));
// controle command from High Level Aktor
connect(actorHighLevel,SIGNAL(signalSendRobotControlParams(double,double)),
actorLowLevel,SLOT(setRobotControllParams(double,double)));
// Get waypoints from path planning into the path realizer
connect(pathPlanner, SIGNAL(sendUpdatedWaypoints(QList< QPair<double,double> >)),
actorHighLevel, SLOT(slotUpdateWaypoints(QList< QPair<double,double> >)));
// SENSOR DATA TO HIGH LEVEL SENSOR
connect(sensorLowLevel,SIGNAL(laserDataReady(QVector<double>)),
sensorHighLevel, SLOT(getLaserData(QVector<double>)));
connect(sensorLowLevel,SIGNAL(sonarDataReady(QVector<double>)),
sensorHighLevel, SLOT(getSonarData(QVector<double>)));
// start color detection from sensor
connect(sensorHighLevel, SIGNAL(signalStartColorDetection()),
cam, SLOT(slotStartColorDetection()));
// color succesfully detected
connect(cam, SIGNAL(signalColorDetected(CamColor)),
sensorHighLevel, SLOT(slotColorDetected(CamColor)));
// ***********************************************************************
// update Odometry
connect(sensorLowLevel,SIGNAL(updateOdometry()),
actorLowLevel, SLOT(getOdometry()));
connect(sensorHighLevel, SIGNAL(sendOdometryData(Position)),
actorLowLevel, SLOT(setOdometry(Position)));
// ***********************************************************************
// Signals from GameEngine
connect(gameEngine,SIGNAL(signalStartDetection(bool)),
sensorHighLevel, SLOT(slotStartDetection(bool)));
// Signals to GameEngine
connect(sensorHighLevel, SIGNAL(signalSendTeamColor(CamColor)),
gameEngine, SLOT(slotDetectionFinished(CamColor)));
// starting the eventloop of all threads
threadRobotLowLevel.start();
threadSensorHighLevel.start();
threadPathRealizer.start();
threadPathPlanner.start();
threadCam.start();
threadGameEngine.start();
// Starte Game Engine State Machine
gameEngine->start();
// Start the PathPlanning "loop"
/// \todo: Das ist erstmal nur zum Debuggen drinnen.
// Spaeter darf die Pfadplanung nur bei beginn der Spielphase aus der GameEngine heraus angestossen werden
QMetaObject::invokeMethod(pathPlanner, "planPath");
}
const bool RobotAction::movingToAroundOfHuman(const int& speed, const float& distance, const double& angle2FrontOfHuman) {
this->turnFaceToHuman();
Sleep(1000);
/* Request current robot pose */
MessageFreqMgr requestOdo;
sendMessageFreq(requestOdo);
Sleep(sizeof(requestOdo));
this->buzyWaitForMgr(250);
OdometryMgr curPos;
getOdometry(curPos);
/* Request current human pose */
PerceptionMgr legRequest;
legRequest.sensing = legDetection;
sendPerception(legRequest);
Sleep(sizeof(legDetection));
this->buzyWaitForMgr(250);
PeopleMgr targetPos;
getPeople(targetPos);
/* Request body direction */
PerceptionHAEMgr requestBody;
requestBody.sensing = bodyDirectionCont;
sendPerceptionHAE(requestBody);
Sleep(sizeof(requestBody));
this->buzyWaitForMgr(250);
HAEMgr receivedBodyDir;
getHAE(receivedBodyDir);
/* Prepare subgoal message */
// Find the possible candidate of the target, the nearest one
double possibleCandidateX = 0.0, possibleCandidateY = 0.0;
if (targetPos.count == 0) {
cout << "> WARNING: No Human Candidate!" << endl;
return 1;
} else if(targetPos.count > 1) {
/* Find the person who is nearest to the robot */
float minX = 0.0, minY = 0.0, squaredDistance = 0.0, minSquaredDistance = 999999.0;
for (int i = 0; i < targetPos.count; i++) {
squaredDistance = pow(targetPos.x[i],2) + pow(targetPos.y[i],2);
if (squaredDistance <= minSquaredDistance) {
minSquaredDistance = squaredDistance;
minX = targetPos.x[i];
minY = targetPos.y[i];
}
}
possibleCandidateX = minX / 100.0;
possibleCandidateY = minY / 100.0;
}
double dist_robot2human = sqrt(pow(possibleCandidateX / 100.0, 2) + pow(possibleCandidateY / 100.0, 2));
double theta_BD = (-1 * receivedBodyDir.body_direction_cont);
double theta_robotHeading = atan2(possibleCandidateY, possibleCandidateX) * 180 / M_PI;
/* Coordinate Transformation */
CoordTrans coordinateTrans;
// From human coordinate (B) to robot coordinate (R)
double X_g_H = distance * cos(angle2FrontOfHuman / 180 * M_PI), Y_g_H = distance * sin(angle2FrontOfHuman / 180 * M_PI), theta_g_H = 180.0 + angle2FrontOfHuman;
double X_g_R = 0.0, Y_g_R = 0.0, theta_g_R = 0.0;
coordinateTrans.trans2D(X_g_H, Y_g_H, theta_g_H,
dist_robot2human * cos(theta_BD / 180 * M_PI), dist_robot2human * sin(theta_BD / 180 * M_PI), 180.0 + theta_BD - theta_robotHeading,
X_g_R, Y_g_R, theta_g_R);
// From robot coordinate to world coordinate (W)
double X_g_W = 0.0, Y_g_W = 0.0, theta_g_W = 0.0;
coordinateTrans.trans2D(X_g_R, Y_g_R, theta_g_R,
curPos.x - 0.0, curPos.y - 0.0, -1 * curPos.theta,
X_g_W, Y_g_W, theta_g_W);
/* For visualization, used for debug */
// Transform the centor point of human
//double X_c_H = 0.0, Y_c_H = 0.0, Y_c_R, Y_c_W, X_c_R, X_c_W;
//coordinateTrans.trans2D(X_c_H, Y_c_H, theta_g_H,
// dist_robot2human * cos(theta_BD / 180 * M_PI), dist_robot2human * sin(theta_BD / 180 * M_PI), 180.0 + theta_BD - theta_robotHeading,
// X_c_R, Y_c_R, theta_g_R);
//coordinateTrans.trans2D(X_c_R, Y_c_R, theta_g_R,
// curPos.x - 0.0, curPos.y - 0.0, -1 * curPos.theta,
// X_c_W, Y_c_W, theta_g_W);
//int imgX_g = 0, imgY_g = 0, imgX_c = 0, imgY_c = 0;
//coordinateTrans.plane2Img(X_g_W, Y_g_W, imgX_g, imgY_g);
//coordinateTrans.plane2Img(X_c_W, Y_c_W, imgX_c, imgY_c);
//coordinateTrans.insertPoint(imgX_g, imgY_g);
//coordinateTrans.insertPoint(imgX_c, imgY_c);
//coordinateTrans.drawPoint();
SubgoalMgr goal;
goal.theta = theta_g_W;
goal.x = X_g_W;
goal.y = Y_g_W;
/* Print information for debug */
cout << "BodyDirection: " << receivedBodyDir.body_direction_cont << endl;
cout << "X_Goal: " << goal.x << ", Y_Goal: " << goal.y << ", Theta_Goal: " << goal.theta << endl;
/* Send subgoal message */
sendSubgoal(goal);
naviExecuting = true;
Sleep(sizeof(goal) + 500);
this->getCurrentTime();
this->turningFace(0);
Result_Navi resultNaviStatus;
getResultNavi(resultNaviStatus);
while (this->executingTime() <= timeout) {
if (naviExecuting == false || resultNaviStatus.current_status == FINISHED)
return true;
getResultNavi(resultNaviStatus);
Sleep(50);
}
// Timeout
cout << "> WARNING: Action, MovingToFrontOfHuman, timeout" << endl;
naviExecuting = false;
return false;
}
const bool RobotAction::forwardApproach(const int& speed, const double& distance) {
this->turningFace(0);
/* Request current robot pose */
MessageFreqMgr requestOdo;
sendMessageFreq(requestOdo);
Sleep(sizeof(requestOdo));
this->buzyWaitForMgr(500);
OdometryMgr curPos;
getOdometry(curPos);
/* Request current human pose */
PerceptionMgr legRequest;
legRequest.sensing = legDetection;
sendPerception(legRequest);
Sleep(sizeof(legDetection));
this->buzyWaitForMgr(500);
PeopleMgr targetPos;
getPeople(targetPos);
/* Prepare subgoal message */
if (targetPos.count == 0) {
cout << "> WARNING: No Human!" << endl;
return 1;
} else if(targetPos.count > 1) {
/* Find the person who is nearest to the robot */
float minX = 0.0, minY = 0.0, squaredDistance = 0.0, minSquaredDistance = 999999.0;
for (int i = 0; i < targetPos.count; i++) {
squaredDistance = pow(targetPos.x[i],2) + pow(targetPos.y[i],2);
if (squaredDistance <= minSquaredDistance) {
minSquaredDistance = squaredDistance;
minX = targetPos.x[i];
minY = targetPos.y[i];
}
}
targetPos.x[0] = minX;
targetPos.y[0] = minY;
}
double dX = (targetPos.x[0] / 100.0) - curPos.x;
double dY = (targetPos.y[0] / 100.0) - curPos.y;
if (sqrt(pow(dX, 2) + pow(dY, 2)) < 1.5) {
cout << "> WARNING: Too close to human!" << endl;
return false;
}
SubgoalMgr goal;
goal.theta = atan2(dY, dX) * 180 / M_PI;
goal.x = curPos.x + distance * cos(goal.theta / 180 * M_PI);
goal.y = curPos.y + distance * sin(goal.theta / 180 * M_PI);
cout << "Xr: " << curPos.x << ", Yr: " << curPos.y << endl;
cout << "Xh: " << targetPos.x[0] << ", Yh: " << targetPos.y[0] << endl;
cout << "Xg: " << goal.x << ", Yg: " << goal.y << endl;
/* Send subgoal message */
sendSubgoal(goal);
naviExecuting = true;
Sleep(sizeof(goal) + 500);
this->getCurrentTime();
Result_Navi resultNaviStatus;
getResultNavi(resultNaviStatus);
while (this->executingTime() <= timeout) {
if (naviExecuting == false || resultNaviStatus.current_status == FINISHED)
return true;
getResultNavi(resultNaviStatus);
Sleep(50);
}
// Timeout
cout << "> WARNING: Action, ForwardApproach, timeout" << endl;
naviExecuting = false;
return false;
}
