/**
* @brief A point of the road is visible if it is between the robot and the laser beam running through it, and if the previous point was visible
* All points in the road are updated
* @param road ...
* @param laserData ...
* @return bool
*/
bool ElasticBand::checkVisiblePoints(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData)
{
//Simplify laser polyline using Ramer-Douglas-Peucker algorithm
std::vector<Point> points, res;
QVec wd;
for (auto &ld : laserData)
{
//wd = innermodel->laserTo("world", "laser", ld.dist, ld.angle); //OPTIMIZE THIS FOR ALL CLASS METHODS
wd = innermodel->getNode<InnerModelLaser>("laser")->laserTo("world", ld.dist, ld.angle);
points.push_back(Point(wd.x(), wd.z()));
}
res = simPath.simplifyWithRDP(points, 70);
//qDebug() << __FUNCTION__ << "laser polygon after simp" << res.size();
// Create a QPolygon so we can check if robot outline falls inside
QPolygonF polygon;
for (auto &p: res)
polygon << QPointF(p.x, p.y);
// Move the robot along the road
int robot = road.getIndexOfNextPoint();
QVec memo = innermodel->transform6D("world", "robot");
for(int it = robot; it<road.size(); ++it)
{
road[it].isVisible = true;
innermodel->updateTransformValues("robot", road[it].pos.x(), road[it].pos.y(), road[it].pos.z(), 0, road[it].rot.y(), 0);
//get Robot transformation matrix
QMat m = innermodel->getTransformationMatrix("world", "robot");
// Transform all points at one to world RS
//m.print("m");
//pointsMat.print("pointsMat");
QMat newPoints = m * pointsMat;
//Check if they are inside the laser polygon
for (int i = 0; i < newPoints.nCols(); i++)
{
// qDebug() << __FUNCTION__ << "----------------------------------";
// qDebug() << __FUNCTION__ << QPointF(newPoints(0, i), newPoints(2, i));
// qDebug() << __FUNCTION__ << polygon;
if (polygon.containsPoint(QPointF(newPoints(0, i), newPoints(2, i)),Qt::OddEvenFill) == false)
{
road[it].isVisible = false;
//qFatal("fary");
break;
}
}
// if( road[it].isVisible == false)
// {
// for (int k = it; k < road.size(); ++k)
// road[k].isVisible = false;
// break;
// }
}
// Set the robot back to its original state
innermodel->updateTransformValues("robot", memo.x(), memo.y(), memo.z(), 0, memo.ry(), 0);
//road.print();
return true;
}
/**
* @brief Moves a virtual copy of the robot along the road checking for enough free space around it
*
* @param innermodel ...
* @param road ...
* @param laserData ...
* @param robotRadius ...
* @return bool
*/
bool ElasticBand::checkCollisionAlongRoad(InnerModel *innermodel, const RoboCompLaser::TLaserData& laserData, WayPoints &road, WayPoints::const_iterator robot,
WayPoints::const_iterator target, float robotRadius)
{
//Simplify laser polyline using Ramer-Douglas-Peucker algorithm
std::vector<Point> points, res;
QVec wd;
for( auto &ld : laserData)
{
wd = innermodel->laserTo("world", "laser", ld.dist, ld.angle); //OPTIMIZE THIS FOR ALL CLASS METHODS
points.push_back(Point(wd.x(), wd.z()));
}
res = simPath.simplifyWithRDP(points, 70);
qDebug() << __FUNCTION__ << "laser polygon after simp" << res.size();
// Create a QPolygon so we can check if robot outline falls inside
QPolygonF polygon;
for (auto &p: res)
polygon << QPointF(p.x, p.y);
// Move the robot along the road
QVec memo = innermodel->transform6D("world","robot");
bool free = false;
for( WayPoints::const_iterator it = robot; it != target; ++it)
{
if( it->isVisible == false)
break;
// compute orientation of the robot at the point
innermodel->updateTransformValues("robot", it->pos.x(), it->pos.y(), it->pos.z(), 0, it->rot.y(), 0);
//get Robot transformation matrix
QMat m = innermodel->getTransformationMatrix("world", "robot");
// Transform all points at one
qDebug() << __FUNCTION__ << "hello2";
m.print("m");
pointsMat.print("pointsMat");
QMat newPoints = m * pointsMat;
qDebug() << __FUNCTION__ << "hello3";
//Check if they are inside the laser polygon
for( int i=0; i<newPoints.nRows(); i++)
if( polygon.containsPoint(QPointF(pointsMat(i,0)/pointsMat(i,3), pointsMat(i,2)/pointsMat(i,3)), Qt::OddEvenFill ) == false)
{
free = false;
break;
}
free = true;
}
qDebug() << __FUNCTION__ << "hello";
// Set the robot back to its original state
innermodel->updateTransformValues("robot", memo.x(), memo.y(), memo.z(), 0, memo.ry(), 0);
return free ? true : false;
}
/**
* \brief Default constructor
*/
SpecificWorker::SpecificWorker(MapPrx& mprx) : GenericWorker(mprx)
{
inner = new InnerModel("/home/salabeta/Robotica2015/RoCKIn@home/world/apartment.xml");
//Set odometry for initial robot TargetPose
try {
differentialrobot_proxy->getBaseState ( bState );
qDebug() << __FUNCTION__<< bState.x << bState.z << bState.alpha;
try {
inner->transform ( "world",QVec::zeros ( 6 ),"initialRobotPose" );
if ( bState.x == 0 and bState.z == 0 ) { //RCIS just initiated. We change robot odometry to the initialRobotPose
QVec rpos = inner->transform ( "world", QVec::zeros ( 6 ),"robot" );
RoboCompDifferentialRobot::TBaseState bs;
bs.x=rpos.x();
bs.z=rpos.z();
bs.alpha=rpos.ry();
differentialrobot_proxy->setOdometer ( bs );
qDebug() << "Robot odometry set to" << rpos;
} else {
inner->updateTransformValues ( "initialRobotPose", 0,0,0,0,0,0 );
}
} catch ( std::exception &ex ) {
std::cout<<ex.what() <<std::endl;
};
} catch ( Ice::Exception &ex ) {
std::cout<<ex.what() <<std::endl;
};
qDebug() << __FUNCTION__<< bState.x << bState.z << bState.alpha;
graphicsView->setScene ( &scene );
graphicsView->show();
graphicsView->scale ( 3,3 );
//Innermodelviewer
osgView = new OsgView ( this );
osgGA::TrackballManipulator *tb = new osgGA::TrackballManipulator;
osg::Vec3d eye ( osg::Vec3 ( 4000.,4000.,-1000. ) );
osg::Vec3d center ( osg::Vec3 ( 0.,0.,-0. ) );
osg::Vec3d up ( osg::Vec3 ( 0.,1.,0. ) );
tb->setHomePosition ( eye, center, up, true );
tb->setByMatrix ( osg::Matrixf::lookAt ( eye,center,up ) );
osgView->setCameraManipulator ( tb );
innerViewer = new InnerModelViewer ( inner, "root", osgView->getRootGroup(), true );
show();
}
/**
* \brief Called when the robot is sent close to an object's location
*/
void SpecificWorker::action_SetObjectReach(bool newAction)
{ // Get symbols' map
std::map<std::string, AGMModelSymbol::SPtr> symbols;
try
{
symbols = worldModel->getSymbolsMap(params); //ALL THE SYMBOLS GIVEN IN THE RULE
}
catch(...)
{
printf("navigationAgent: Couldn't retrieve action's parameters\n");
printf("<<WORLD\n");
AGMModelPrinter::printWorld(worldModel);
printf("WORLD>>\n");
if (worldModel->size() > 0) { exit(-1); }
}
// Get target
int roomID, objectID, robotID;
try
{
if (symbols["room"].get() and symbols["object"].get() and symbols["robot"].get() )
{
roomID = symbols["room"]->identifier;
objectID =symbols["object"]->identifier;
robotID = symbols["robot"]->identifier;
try // If we can access the 'reach' edge for the object of the action
{ // is not really necessary. The planner is probably replanning.
worldModel->getEdgeByIdentifiers(objectID, objectID, "reach");
{
static QTime lastMsg = QTime::currentTime().addSecs(-1000);
if (lastMsg.elapsed() > 1000)
{
rDebug2(("navigationAgent ignoring action setObjectReach (object currently reached)"));
lastMsg = QTime::currentTime();
return;
}
printf("ask the platform to stop\n");
stop();
}
}
catch(...)
{
}
}
else
{
printf("parameters not in the model yet\n");
return;
}
}
catch(...)
{
printf("ERROR: SYMBOL DOESN'T EXIST \n");
exit(2);
}
// GET THE INNERMODEL NAMES OF TH SYMBOLS
QString robotIMID;
QString roomIMID;
QString objectIMID;
try
{
robotIMID = QString::fromStdString(worldModel->getSymbol(robotID)->getAttribute("imName"));
roomIMID = QString::fromStdString(worldModel->getSymbol(roomID)->getAttribute("imName"));
objectIMID = QString::fromStdString(worldModel->getSymbol(objectID)->getAttribute("imName"));
// check if object has reachPosition
AGMModelSymbol::SPtr object = worldModel->getSymbol(objectID);
for (auto edge = object->edgesBegin(worldModel); edge != object->edgesEnd(worldModel); edge++)
{
if (edge->getLabel() == "reachPosition")
{
const std::pair<int32_t, int32_t> symbolPair = edge->getSymbolPair();
objectID = symbolPair.second;
objectIMID = QString::fromStdString(worldModel->getSymbol(objectID)->getAttribute("imName"));
qDebug() << __FUNCTION__ << "Target object " << symbolPair.first<<"->"<<symbolPair.second<<" object "<<objectIMID;
}
}
}
catch(...)
{
printf("ERROR IN GET THE INNERMODEL NAMES\n");
exit(2);
}
// GET THE TARGET POSE:
RoboCompTrajectoryRobot2D::TargetPose tgt;
try
{
if (not (innerModel->getNode(roomIMID) and innerModel->getNode(objectIMID)))
{
printf("Cant find objects to reach %d\n", __LINE__);
return;
}
QVec poseInRoom = innerModel->transform6D(roomIMID, objectIMID); // FROM OBJECT TO ROOM
qDebug() << __FUNCTION__ <<" Target pose: "<< poseInRoom;
tgt.x = poseInRoom.x();
tgt.y = 0;
tgt.z = poseInRoom.z();
tgt.rx = 0;
//tgt.ry = 0;
tgt.ry = poseInRoom.ry(); //needed to reach tables
tgt.rz = 0;
tgt.doRotation = true;
}
catch (...)
{
qDebug()<< __FUNCTION__ << "InnerModel Exception. Element not found in tree";
}
// Execute target
try
{
// if (!haveTarget)
{
try
{
QVec O = innerModel->transform("shellyArm_grasp_pose", objectIMID);
//O.print(" O pose relativa");
//qDebug() << __FUNCTION__ << "O norm:" << O.norm2();
QVec graspRef = innerModel->transform("robot", "shellyArm_grasp_pose");
graspRef.print("graspRef");
float th=20;
tgt.ry = -3.1415926535/2.;
go(tgt.x, tgt.z, tgt.ry, tgt.doRotation, graspRef.x(), graspRef.z(), th);
qDebug() << __FUNCTION__ << "trajectoryrobot2d->go(" << tgt.x << ", " << tgt.z << ", " << tgt.ry << ", " << graspRef.x() << ", " << graspRef.z() << " )\n";
haveTarget = true;
}
catch(const RoboCompTrajectoryRobot2D::RoboCompException &ex)
{
std::cout << ex << " " << ex.text << std::endl;
throw;
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
}
}
string state;
try
{
state = trajectoryrobot2d_proxy->getState().state;
}
catch(const Ice::Exception &ex)
{
std::cout <<"trajectoryrobot2d->getState().state "<< ex << std::endl;
throw ex;
}
//state="IDLE";
std::cout<<state<<" haveTarget "<<haveTarget;
if (state=="IDLE" && haveTarget)
{
//std::cout<<"\ttrajectoryrobot2d_proxy->getState() "<<trajectoryrobot2d_proxy->getState().state<<"\n";
try
{
// AGMModel::SPtr newModel(new AGMModel(worldModel));
// int statusID =symbols["status"]->identifier;
// newModel->getEdgeByIdentifiers(objectID, statusID, "noReach").setLabel("reach");
// sendModificationProposal(worldModel, newModel);
haveTarget=false;
}
catch (...)
{
std::cout<<"\neeeee"<< "\n";
}
}
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
}
}
/**
* \brief Called when the robot is sent close to a person to offer the object
*/
void SpecificWorker::action_HandObject_Offer(bool newAction)
{
// Get symbols' map
std::map<std::string, AGMModelSymbol::SPtr> symbols;
try
{
symbols = worldModel->getSymbolsMap(params); //ALL THE SYMBOLS GIVEN IN THE RULE
}
catch(...)
{
printf("navigationAgent: Couldn't retrieve action's parameters\n");
printf("<<WORLD\n");
AGMModelPrinter::printWorld(worldModel);
printf("WORLD>>\n");
if (worldModel->size() > 0) { exit(-1); }
}
// Get target
int roomID, personID, robotID;
try
{
if (symbols["room"].get() and symbols["person"].get() and symbols["robot"].get() )
{
roomID = symbols["room"]->identifier;
personID =symbols["person"]->identifier;
robotID = symbols["robot"]->identifier;
try // If we can access the 'reach' edge for the object of the action
{ // is not really necessary. The planner is probably replanning.
worldModel->getEdgeByIdentifiers(personID, personID, "reach");
{
static QTime lastMsg = QTime::currentTime().addSecs(-1000);
if (lastMsg.elapsed() > 1000)
{
rDebug2(("navigationAgent ignoring action setHandObject_Offer (person currently reached)"));
lastMsg = QTime::currentTime();
return;
}
printf("ask the platform to stop\n");
stop();
}
}
catch(...)
{
}
}
else
{
printf("parameters not in the model yet\n");
return;
}
}
catch(...)
{
printf("ERROR: SYMBOL DOESN'T EXIST \n");
exit(2);
}
// GET THE INNERMODEL NAMES OF TH SYMBOLS
QString robotIMID;
QString roomIMID;
QString personIMID;
try
{
robotIMID = QString::fromStdString(worldModel->getSymbol(robotID)->getAttribute("imName"));
roomIMID = QString::fromStdString(worldModel->getSymbol(roomID)->getAttribute("imName"));
personIMID = QString::fromStdString(worldModel->getSymbol(personID)->getAttribute("imName"));
}
catch(...)
{
printf("ERROR IN GET THE INNERMODEL NAMES\n");
exit(2);
}
// GET THE TARGET POSE:
RoboCompTrajectoryRobot2D::TargetPose tgt;
try
{
if (not (innerModel->getNode(roomIMID) and innerModel->getNode(personIMID))) return;
QVec poseInRoom = innerModel->transform6D(roomIMID, personIMID); // FROM PERSON TO ROOM
qDebug() << __FUNCTION__ <<" Target pose: "<< poseInRoom;
tgt.x = poseInRoom.x();
tgt.y = 0;
tgt.z = poseInRoom.z();
tgt.rx = 0;
tgt.ry = poseInRoom.ry();
tgt.rz = 0;
tgt.doRotation = true;
}
catch (...)
{
qDebug()<< __FUNCTION__ << "InnerModel Exception. Element not found in tree";
}
// Execute target
try
{
try
{
QVec O = innerModel->transform("shellyArm_grasp_pose", personIMID);
QVec graspRef = innerModel->transform("robot", "shellyArm_grasp_pose");
go(tgt.x, tgt.z, 0, tgt.doRotation, graspRef.x(), graspRef.z(), 20);
qDebug() << __FUNCTION__ << "trajectoryrobot2d->go(" << tgt.x << ", " << tgt.z << ", " << tgt.ry << ", " << graspRef.x() << ", " << graspRef.z() << " )\n";
haveTarget = true;
}
catch(const RoboCompTrajectoryRobot2D::RoboCompException &ex)
{
std::cout << ex << " " << ex.text << std::endl;
throw;
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
}
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
}
}
void SpecificWorker::action_HandObject(bool newAction)
{
// Get symbols' map
std::map<std::string, AGMModelSymbol::SPtr> symbols;
try
{
symbols = worldModel->getSymbolsMap(params/*, "robot", "room", "object", "status"*/); //ALL THE SYMBOLS GIVEN IN THE RULE
}
catch(...)
{
printf("navigationAgent, action_HandObject: Couldn't retrieve action's parameters\n");
printf("<<WORLD\n");
AGMModelPrinter::printWorld(worldModel);
printf("WORLD>>\n");
if (worldModel->size() > 0) { exit(-1); }
}
// Get target
int roomID, personID, robotID;
try
{
if (symbols["room"].get() and symbols["person"].get() and symbols["robot"].get())
{
roomID = symbols["room"]->identifier; //7 ROOM
personID =symbols["person"]->identifier;// PERSON
robotID = symbols["robot"]->identifier; //1 ROBOT
}
else
{
printf("navigationAgent, action_HandObject: parameters not in the model yet\n");
return;
}
}
catch(...)
{
printf("navigationAgent, action_HandObject ERROR: SYMBOL DOESN'T EXIST \n");
exit(2);
}
// GET THE INNERMODEL NAMES OF TH SYMBOLS
QString robotIMID;
QString roomIMID;
QString personIMID;
try
{
robotIMID = QString::fromStdString(worldModel->getSymbol(robotID)->getAttribute("imName"));
roomIMID = QString::fromStdString(worldModel->getSymbol(roomID)->getAttribute("imName"));
//we need to obtain the imName of the torso node. TrackingId+"XN_SKEL_TORSO"
QString trackingId= QString::fromStdString(worldModel->getSymbol(personID)->getAttribute("TrackingId"));
personIMID = trackingId +"XN_SKEL_TORSO";
}
catch(...)
{
printf("navigationAgent, action_HandObject: ERROR IN GET THE INNERMODEL NAMES\n");
qDebug()<<"[robotIMID"<<robotIMID<<"roomIMID"<<roomIMID<<"personIMID"<<personIMID<<"]";
exit(2);
}
// GET THE TARGET POSE:
RoboCompTrajectoryRobot2D::TargetPose tgt;
try
{
if (not (innerModel->getNode(roomIMID) and innerModel->getNode(personIMID))) return;
QVec poseInRoom = innerModel->transform6D(roomIMID, personIMID); // FROM OBJECT TO ROOM
qDebug()<<"[robotIMID"<<robotIMID<<"roomIMID"<<roomIMID<<"personIMID"<<personIMID<<"]";
qDebug()<<" TARGET POSE: "<< poseInRoom;
tgt.x = poseInRoom.x();
tgt.y = 0;
tgt.z = poseInRoom.z();
tgt.rx = 0;
tgt.ry = poseInRoom.ry();
tgt.rz = 0;
tgt.doRotation = false;
}
catch (...)
{
qDebug()<<"navigationAgent, action_HandObject: innerModel exception";
}
try
{
// if (!haveTarget)
{
try
{
QVec graspRef = innerModel->transform("robot", "shellyArm_grasp_pose");
float th=20;
go(tgt.x, tgt.z, tgt.ry, tgt.doRotation, graspRef.x(), graspRef.z(), th);
std::cout << "trajectoryrobot2d->go(" << currentTarget.x << ", " << currentTarget.z << ", " << currentTarget.ry << ", " << currentTarget.doRotation << ", " << graspRef.x() << ", " << graspRef.z() << " )\n";
haveTarget = true;
}
catch(const Ice::Exception &ex)
{
std::cout <<"navigationAgent, action_HandObject: ERROR trajectoryrobot2d->go "<< ex << std::endl;
throw ex;
}
}
string state;
try
{
state = trajectoryrobot2d_proxy->getState().state;
}
catch(const Ice::Exception &ex)
{
std::cout <<"navigationAgent, action_HandObject: trajectoryrobot2d->getState().state "<< ex << std::endl;
throw ex;
}
//state="IDLE";
std::cout<<state<<" haveTarget "<<haveTarget;
if (state=="IDLE" && haveTarget)
{
//std::cout<<"\ttrajectoryrobot2d_proxy->getState() "<<trajectoryrobot2d_proxy->getState().state<<"\n";
try
{
// AGMModel::SPtr newModel(new AGMModel(worldModel));
// int statusID =symbols["status"]->identifier;
// newModel->getEdgeByIdentifiers(objectID, statusID, "noReach").setLabel("reach");
// sendModificationProposal(worldModel, newModel);
haveTarget=false;
}
catch (...)
{
std::cout<<"\neeeee"<< "\n";
}
}
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
}
}