bool PlanningProblem::hasCollision(const Station &st, const Obstacle &ob)
{
b2Transform st_Transform;
st_Transform.Set(st.getPosition().to2D().toB2vec2(), st.getPosition().Teta());
return b2TestOverlap(ob.shape, 0, agent->shape, 1, ob.predictedTransform(0), st_Transform);
}
Station PlanningProblem::getNextStation(const Station &st, Trajectory &path)
{
if(path.isEmpty())
return this->goal.goal_point;
if(path.length() <= 2)
return path.getLastStation();
float max_memberance = 0;
int nearest_segment_index = -1;
for(uint i=1; i<path.length(); i++) { // it doesnt use the last segment (go towards goal on last segment)
Vector2D pnt_1 = path.getStation(i-1).getPosition().to2D();
Vector2D pnt_2 = path.getStation(i).getPosition().to2D();
float dist_st_segment = (st.getPosition().to2D() - pnt_1).lenght() +
(st.getPosition().to2D() - pnt_2).lenght();
float segment_len = (pnt_1 - pnt_2).lenght();
float segment_mem = segment_len /dist_st_segment;
if(segment_mem > max_memberance) {
max_memberance = segment_mem;
nearest_segment_index = i;
}
}
return path.getStation(nearest_segment_index);
}
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();
}
bool PlanningProblem::pathHasCollision(Station &from, Station &to, const Obstacle &ob)
{
b2PolygonShape road_from_to;
Vector2D center((from.getPosition() + to.getPosition()).to2D() /2.0);
Vector2D half_diff((to.getPosition() - from.getPosition()).to2D() /2.0);
float safet_latera_bound = 0.1;
road_from_to.SetAsBox(agent->radius() * (1 + safet_latera_bound), half_diff.lenght() + agent->radius(),
center.toB2vec2(), M_PI_2 + half_diff.arctan());
// if(ob == NULL)
// continue;
return b2TestOverlap(ob.shape, 0, &road_from_to, 1, ob.transform, identity_trans);
}
Station PlanningProblem::RRTExtend(const Station &start, const Station &target, float extension_len)
{
Vector3D diff_vec = target.getPosition() - start.getPosition();
diff_vec.normalize2D();
diff_vec *= extension_len;
float start_angle = start.getPosition().Teta();
start_angle = continuousRadian(start_angle, diff_vec.to2D().arctan() - M_PI);
float rotate_angle = (diff_vec.to2D().arctan() - start_angle) / 1.5;
diff_vec.setTeta(rotate_angle);
Station temp_station;
temp_station.setPosition((diff_vec + start.getPosition()).standardizeTeta());
bool valid = CheckValidity(temp_station);
if(valid)
return temp_station;
return Station();
}
bool PlanningProblem::hasCollision(const Station &st, const ObstacleSet &ob_set)
{
b2Transform stateTransform;
stateTransform.Set(st.getPosition().to2D().toB2vec2(), st.getPosition().Teta());
for(unsigned int i=0; i<ob_set.size(); i++)
{
Obstacle* ob = ob_set[i];
if(ob == NULL)
continue;
bool result = b2TestOverlap(ob->shape, 0, agent->shape, 1, ob->transform, stateTransform);
if(result)
{
// std::cout << "collision with obstacle " << i << endl;
return true;
}
}
return false;
}
// this function returns -1 if two objects has collision
// And otherwise returns the distance
float PlanningProblem::distToObstacle(Station A, const Obstacle &ob, b2Vec2& A_point, b2Vec2& ob_point)
{
b2DistanceProxy state_proxy, ob_proxy;
state_proxy.Set(agent->shape, 0);
ob_proxy.Set(ob.shape, 1);
b2DistanceInput dist_in;
dist_in.proxyA = state_proxy;
dist_in.proxyB = ob_proxy;
dist_in.transformA = b2Transform(A.getPosition().toB2vec2(),
b2Rot(A.getPosition().Teta()));
dist_in.transformB = ob.transform;
b2SimplexCache dist_cache;
dist_cache.count = 0;
b2DistanceOutput dis_out;
b2Distance(&dis_out, &dist_cache, &dist_in);
A_point = dis_out.pointA;
ob_point = dis_out.pointB;
if(hasCollision(A, ob)) {
return -1;
}
return dis_out.distance;
}
bool PlanningProblem::pathHasCollision(const Station &from, const Station &to, const ObstacleSet &ob_set)
{
b2PolygonShape road_from_to;
Vector2D center((from.getPosition() + to.getPosition()).to2D() /2.0);
Vector2D half_diff((to.getPosition() - from.getPosition()).to2D() /2.0);
float safety_lateral_bound = -0.20;
road_from_to.SetAsBox(agent->radius() * (1 + safety_lateral_bound),
half_diff.lenght() + agent->radius() * (1+safety_lateral_bound),
center.toB2vec2(), M_PI_2 + half_diff.arctan());
for(unsigned int i=0; i<ob_set.size(); i++)
{
Obstacle* ob = ob_set[i];
if(ob == NULL)
continue;
bool result = b2TestOverlap(ob->shape, 0, &road_from_to, 1, ob->transform, identity_trans);
if(result)
{
// std::cout << "collision with obstacle " << i << endl;
return true;
}
}
return false;
}
vector<Vector2D> PlanningProblem::ObstacleForces(const Station &st, const ObstacleSet &ob_set)
{
vector<Vector2D> ob_force_list;
ob_force_list.reserve(ob_set.size());
float extension_len = agent->radius();
for(uint i=0; i<ob_set.size(); i++) {
const Obstacle* ob = ob_set[i];
b2Vec2 agent_colid_pnt, ob_collid_pnt;
float dist_to_ob = distToObstacle(st, *ob, agent_colid_pnt, ob_collid_pnt);
if(dist_to_ob > extension_len * 10) // ignore it
continue;
Vector2D ob_force;
float repulse_strenght_by_dist = min(extension_len /fabs(dist_to_ob), 1.099f);
if(dist_to_ob < 0) // in collision state
ob_force = (st.getPosition().to2D() - ob->CenterOfMass()).normalized();
else
ob_force = Vector2D(agent_colid_pnt - ob_collid_pnt).normalized();
ob_force *= ob->repulseStrenght * repulse_strenght_by_dist * 1.1;
ob_force_list.push_back(ob_force);
}
return ob_force_list;
}
Vector2D PlanningProblem::GoalAttractiveForce(const Station &st)
{
Vector2D diff_to_goal = (goal.goal_point.getPosition() - st.getPosition()).to2D();
double stren = 1.5 * tanh(diff_to_goal.lenght()/25.0) + 0.1;
return diff_to_goal.normalized() * stren;
}
Vector2D PlanningProblem::PathDirectedForce(const Station &st, Trajectory &path_)
{
Station sub_goal = getNextStation(st, path_);
return (sub_goal.getPosition().to2D() - st.getPosition().to2D()).normalized() * 1.0;
}
