Trajectory PlanningProblem::GRRTsolve()
{
this->planningResult = false;
randomTree.clear();
float start_time = currentTimeMSec();
int tryExtensionCounter = 0;
randomTree.appendNewStation(NULL, initialState);
for(int step=0; step < MAX_RRT_STEP_TRY/5; step++)
{
Station target;
float toss = uni_rand(0, 1);
if(toss < GOAL_PROB)
target.setPosition(goal.goal_point.getPosition());
else {
Station tempSt = SampleStateUniform();
target.setPosition(tempSt.getPosition());
}
if( !target.isValid() )
continue;
// throw "can not sampled!";
SpatialVertex* near_ver = randomTree.getNearestVertex(target);
if(near_ver == NULL)
continue;
int greedyCounter = 0;
while(greedyCounter < 5){
tryExtensionCounter ++;
Station extended = RRTExtend(near_ver->state, target, agent->radius() * 2);
if(!extended.isValid())
break;
randomTree.appendNewStation(near_ver, extended);
if(Station::dubinDistance(extended, target) < agent->radius() ) {
if((target.getPosition() - goal.goal_point.getPosition()).lenght2D() < agent->radius() /2)
planningResult = true;
break;
}
// if(target != goal.goal_point) break;
greedyCounter ++;
}
cout << "Step = " << step << endl;
}
if(planningResult)
{
float finish_time = currentTimeMSec();
this->planningTime = finish_time - start_time;
// cout << "Greedy RRT Planning succeed in " << planningTime << "mili seconds" << endl;
return buildTrajectoryFromRandomTree();
}
return Trajectory();
}
// *************************************************
// ************** under construction ***************
// *************************************************
Trajectory PlanningProblem::RRTConnectSolve(double arg1)
{
double start_time = currentTimeMSec();
randomTree.clear();
backward_tree.clear();
randomTree.appendNewStation(NULL, initialState);
backward_tree.appendNewStation(NULL, goal.goal_point);
for(uint i=0; i< MAX_RRT_STEP_TRY ; ++i)
{
Station randSt = SampleStateUniform();
if(!randSt.isValid())
continue;
SpatialVertex* near_ver = randomTree.getNearestVertex(randSt);
if(near_ver == NULL)
continue;
Station extended_ = RRTExtend(near_ver->state, randSt, arg1);
if(!extended_.isValid())
continue;
randomTree.appendNewStation(near_ver, extended_);
for(int j=0; j<10; j++) {
SpatialVertex* back_near = backward_tree.getNearestVertex(extended_);
Station back_extended = RRTExtend(back_near->state, extended_, arg1);
if(!back_extended.isValid())
break;
backward_tree.appendNewStation(back_near, back_extended);
if(Station::euclideanDistance(back_extended, extended_) < agent->radius()) {
while(back_near != NULL) {
Station on_back_tree_st = back_near->state;
on_back_tree_st.setPosition(Vector3D(back_near->state.getPosition().to2D(),
back_near->state.getPosition().Teta() + M_PI));
randomTree.appendNewStation(randomTree.lastAddedVertex(), on_back_tree_st);
back_near = back_near->parent;
}
planningTime = currentTimeMSec() - start_time;
return buildTrajectoryFromRandomTree();
}
}
}
planningTime = currentTimeMSec() - start_time;
return Trajectory();
}
Trajectory PlanningProblem::RRTsolve(float arg1, float max_len)
{
// if(tree.count() > MAX_TREE_SIZE * 0.75)
randomTree.clear();
this->planningResult = false;
double start_time = currentTimeMSec();
randomTree.appendNewStation(NULL, initialState);
for(uint i=0; i< MAX_RRT_STEP_TRY ; ++i)
{
if(RRTStep(arg1, max_len) == eReached)
{
this->planningResult = true;
double finish_time = currentTimeMSec();
this->planningTime = finish_time - start_time;
return buildTrajectoryFromRandomTree();
break;
}
}
return Trajectory();
}
bool PlanningProblem::solve()
{
double start_time = currentTimeMSec();
planningResult = false;
if (!CheckValidity(goal.goal_point)) {
cerr << "Warning: the Goal state is drawn in an obstacle" << endl;
solveInvalidGoalState();
}
// if(goal.minDistTo(initialState) == 0) { // if it is already in the goal region
// trajec.clear();
// planningResult = true;
// }
// else if(!CheckValidity(initialState))
// solveInvalidInitialState();
// else
{ // normal way of solvgin motion planning problem
ObstacleSet desired_ob_set = stat_obstacles;
Trajectory apft_ = APFSolve(desired_ob_set, false);
Trajectory pt_ = PruneTrajectory(apft_, desired_ob_set);
// delete apft_;
trajec.copyFrom(pt_);
// if(!planningResult) {
// GRRTsolve();
// buildTrajectoryFromTree();
// }
// if(!planningResult) {
// RRTsolve();
// }
if(planningResult) {
if(goal.minDistTo(trajec.getLastStation()) > agent->radius())
return true;
// computeCost(trajec);
// // just when current plan get invalid
// if(!checkPlanValidity(bestPlan)) {
// checkPlanValidity(bestPlan);
// trajec.printToStream(std::cout);
// bestPlan.copyFrom(trajec);
// }
// else if(trajec.getCost() < bestPlan.getCost()) {
// cout << "best plan:" << bestPlan.getCost() << endl;
// cout << "last plan:" << trajec.getCost() << endl;
// trajec.printToStream(std::cout);
// bestPlan.copyFrom(trajec);
// }
// else {
// Trajectory opt_plan = optimizePlan(bestPlan);
// opt_plan.computeCost();
// if(checkPlanValidity(opt_plan) && opt_plan.getCost() < bestPlan.getCost()) {
// bestPlan.copyFrom(opt_plan);
// }
//
// }
}
}
// buildVelocityProfile();
// trajec.printToStream(cout);
double finish_time = currentTimeMSec();
double total_time = finish_time - start_time ;
// cout << "Planning Succeed in (ms): " << total_time << endl;
return planningResult;
}
void RobotSerialConnection::sendRobotData(int robotID, RobotCommandPacket &packet)
{
const static int packet_size = 7;
unsigned char byteArray[packet_size];
//start byte
byteArray[0] = '*';
//robotID, motor spin and dribbler state
// byteArray[1] = (unsigned char) ( (robotID + (
// (packet.getWheelSpeed(1)> 0 ? 0:1) * 1 +
// (packet.getWheelSpeed(2)> 0 ? 0:1) * 2 +
// (packet.getWheelSpeed(3)> 0 ? 0:1) * 4 +
// (packet.getWheelSpeed(4)> 0 ? 0:1) * 8) * 16)
// & 0x000000FF);
//velocity bytes
// for (int i = 1; i <= 4; i++)
// {
// byteArray[i + 1] = (unsigned char)( (int)(fabs(round(packet.getWheelSpeed(i)*255))) & 0x000000FF );
// }
Vector2D new_vel(packet.getVelocity().to2D());
new_vel.rotate(M_PI_2);
//robotID, motor spin and dribbler state
byteArray[1] = (unsigned char) ( (robotID +
(
(new_vel.X()> 0 ? 0:1) * 1
+(new_vel.Y()< 0 ? 0:1) * 2
+(packet.m_desiredTheta< 0 ? 0:1) * 4
// +(packet.getWheelSpeed(4)> 0 ? 0:1) * 8
) * 16) & 0x000000FF);
byteArray[2] = (uchar)bound(abs(new_vel.X() * 255.0), 0, 255);
byteArray[3] = (uchar)bound(abs(new_vel.Y() * 255.0), 0, 255);
byteArray[4] = (uchar)bound(abs(packet.m_desiredTheta), 0, 180);
// byteArray[5] = 0;
//kick power byte
// old version
// byteArray[6] = (unsigned char) ((packet.m_isForceKick) ? 255 :
// fabs(round(packet.m_kickPower * 255))
byteArray[6] = ((packet.m_kickPower > 0)) * 85;
// printf( "(time=%.6f) Robot[%d] (m1=%4d m2=%4d m3=%4d m4=%4d) [Vx=%.4f, Vy=%.4f, Wz=%.4f] ",
// currentTimeMSec()/1000.0,
// robotID,
// ((((byteArray[1] & 0x10)!=0)*2)-1)*byteArray[2],
// ((((byteArray[1] & 0x20)!=0)*2)-1)*byteArray[3],
// ((((byteArray[1] & 0x40)!=0)*2)-1)*byteArray[4],
// ((((byteArray[1] & 0x80)!=0)*2)-1)*byteArray[5],
// packet.getVelocity().X(),
// packet.getVelocity().Y(),
// packet.getVelocity().Teta());
printf( "(time=%.6f) Robot[%d] (Vx=%4d Vy=%4d Teta=%4d P4=%4d) [Vx=%.4f, Vy=%.4f, Wz=%.4f] ",
currentTimeMSec()/1000.0,
robotID,
((((byteArray[1] & 0x10)!=0)*2)-1)*byteArray[2],
((((byteArray[1] & 0x20)!=0)*2)-1)*byteArray[3],
((((byteArray[1] & 0x40)!=0)*2)-1)*byteArray[4],
((((byteArray[1] & 0x80)!=0)*2)-1)*byteArray[5],
packet.getVelocity().X(),
packet.getVelocity().Y(),
packet.getVelocity().Teta());
// if(packet.m_kickPower > 0)
// printf("Kick Power: %.3f", packet.m_kickPower);
cout << endl;
//transmit data to serial port
#if QT_VERSION >= 0x050000
serial->write((const char *)byteArray, packet_size);
#else
serial->Write(byteArray, packet_size);
#endif
serial->flush();
}
