Milestone* Milestone::makeRandomMilestone(Map& map) {
for (int i = 0; i < MAX_BRANCH_CREATION_ATTEMPS; ++i) {
// TODO: normal distribution?
float speed = randFloat(-0.05, 0.3);
// float speed = randFloat(0.1, 0.3);
// float speed = randFloat(-0.2, 0.2);
float turnRateRange = 1 - fabs(speed*2);
float turnRate = randFloat(-turnRateRange, turnRateRange);
int numCycles = (rand() % 70) + 30;
Pose endPose = getEndPose();
bool failed = false;
for (int cycle = 0; cycle < numCycles; ++cycle) {
endPose = propogateDynamics(endPose, speed, turnRate);
if (map.robotAreaOccupied(endPose)) {
// this motion path wasn't possible
failed = true;
break;
}
}
if (!failed) {
return new Milestone(this, endPose, speed, turnRate, numCycles);
}
// try another set of values
}
return NULL;
}
UPoseV UManArc::getEndPoseV(UPoseV startPoseV)
{
UPoseV result;
//
result = startPoseV + getEndPose();
result.setVel(vel);
return result;
}
// Combines all consecutive turn-in-place segments
p_nav::Path replaceMultipleTurns(const p_nav::Path& path)
{
p_nav::PathSegment currseg, nextseg, newseg;
int end_idx;
geometry_msgs::PoseStamped start, end;
p_nav::Path newpath;
newpath.header = path.header;
for(unsigned int path_idx = 0; path_idx < path.segs.size(); path_idx++)
{
currseg = path.segs.at(path_idx);
// If the current seg is a turn in place
if(currseg.seg_type == p_nav::PathSegment::SPIN_IN_PLACE)
{
end_idx = path_idx;
// Look forward and find the last consecutive turn-in-place segs
while( end_idx+1 < path.segs.size() // the next seg is in range and the next seg is a spin
&& path.segs.at(end_idx+1).seg_type == p_nav::PathSegment::SPIN_IN_PLACE)
{
end_idx++;
//ROS_INFO("Combining seg %d with %d", path_idx, end_idx);
}
start = getStartPose(currseg);
end = getEndPose (path.segs.at(end_idx));
// Combine all into a single segment
newseg = makePathSegment(
start.pose.position.x, start.pose.position.y, tf::getYaw(start.pose.orientation),
end.pose.position.x , end.pose.position.y , tf::getYaw(end.pose.orientation));
newseg.header = currseg.header;
newseg.seg_number = currseg.seg_number;
newpath.segs.push_back(newseg);
// We've now taken care of all segments including end_idx
path_idx = end_idx;
if( path_idx >= path.segs.size() )
break;
}// if turn in place
else{
newpath.segs.push_back(currseg);
}
}//for
return newpath;
}
// Resamples the path's segments
p_nav::Path
smoothPath(const p_nav::Path& path)
{
// Smoothing is possible with more than 2 segments
if( path.segs.size() < 2 ) {
return path;
}
p_nav::Path smoothpath;
p_nav::PathSegment newseg, currseg, nextseg, prevseg;
geometry_msgs::Pose start, start2, end, end2;
// Preserve the path's header information
smoothpath.header = path.header;
smoothpath.segs.clear();
for( unsigned int i=0; i<path.segs.size() - 1; i++ )
{
currseg = path.segs.at(i);
// Skip over turn-in-place segments
if( currseg.seg_type != p_nav::PathSegment::ARC ) {
smoothpath.segs.push_back(currseg);
continue;
}
// First segment in the path:
if( i==0 )
{
nextseg = path.segs.at(i+1);
start = getStartPose(currseg).pose;
end = getEndPose (currseg).pose;
end2 = getStartPose(nextseg).pose;
end.orientation = averageAngles(end.orientation, end2.orientation);
}
// Last segment in the path:
else if( i == path.segs.size()-1 )
{
prevseg = path.segs.at(i-1);
start = getEndPose (prevseg).pose;
start2 = getStartPose(currseg).pose;
end = getEndPose (currseg).pose;
start.orientation = averageAngles(start.orientation, start2.orientation);
}
// Middle segment:
else
{
prevseg = path.segs.at(i-1);
nextseg = path.segs.at(i+1);
start = getEndPose (prevseg).pose;
start2 = getStartPose(currseg).pose;
end = getEndPose (currseg).pose;
end2 = getStartPose(nextseg).pose;
start.orientation = averageAngles(start.orientation, start2.orientation);
end.orientation = averageAngles(end .orientation, end2 .orientation);
}
// Create a new segment
newseg = makePathSegment(start.position.x, start.position.y, tf::getYaw(start.orientation),
end .position.x, end .position.y, tf::getYaw(end .orientation));
// Preserve some information from the current segment
newseg.header = currseg.header;
newseg.seg_number = currseg.seg_number;
// Make sure the smoothing didn't mess anything up too badly
double start_ang_diff = tf::getYaw(subtractAngles(getStartPose(currseg).pose.orientation, getStartPose(newseg).pose.orientation));
double end_ang_diff = tf::getYaw(subtractAngles(getEndPose(currseg).pose.orientation, getEndPose(newseg).pose.orientation));
double angle_threshold = 3.141/16;
double length_difference = (newseg.seg_length - currseg.seg_length)/currseg.seg_length;
double length_threshold = .10;
if( fabs(length_difference) > length_threshold
|| fabs(start_ang_diff ) > angle_threshold
|| fabs(end_ang_diff ) > angle_threshold )
{
ROS_WARN("Smoothing produced weird results at segment %lu/%lu", i+1, path.segs.size());
ROS_WARN("length_difference=%.2f, angle diffs %.2fpi, %.2fpi", length_difference, start_ang_diff/3.141, end_ang_diff/3.141);
smoothpath.segs.push_back(currseg);
}
else {
smoothpath.segs.push_back(newseg);
}
}
return smoothpath;
}
double UManArc::getDistanceXYSigned(UPosition pos, int * where,
bool posIsRight,
bool centerOnly,
UPose * pHit,
double * atT)
{
double cy; // center
double rx, ry;
double d, dist = 0.0;
double ang, aa, ea;
int w = 3;
const double halfPi = M_PI / 2.0;
const double twoPi = M_PI * 2.0;
//
// find arc center
if (angle > 0)
cy = radius;
else
cy = -radius;
ang = atan2(pos.y - cy, pos.x);
// convert to angle relative to arc
if (angle < 0)
ang = halfPi - ang;
else
ang = halfPi + ang;
// make all angles positive
if (ang < 0.0)
ang += twoPi;
aa = absd(angle);
// distance from turn center
d = sqrt(sqr(pos.x) + sqr(pos.y - cy));
// test for within arc - outside or inside
if ((ang >= 0) and (ang <= aa))
{ // positive is to the right
dist = (d - radius) * signofd(angle);
w = 0;
if (pHit != NULL)
{ // calculate hit position too.
pHit->x = radius * sin(ang);
if (angle < 0.0)
pHit->y = cy + radius * cos(ang);
else
pHit->y = cy - radius * cos(ang);
pHit->h = ang;
}
if (atT != NULL)
// return distance into manoeuvre
*atT = ang * radius;
}
else if (not centerOnly)
{ // closer to one of ends
if ((ang - twoPi) < (aa - ang))
{ // high end is closer
rx = radius * sin(aa);
if (angle < 0.0)
ry = cy + radius * cos(aa);
else
ry = cy - radius * cos(aa);
dist = sqrt(sqr(pos.x - rx) + sqr(pos.y - ry));
w = 2;
if (pHit != NULL)
*pHit = getEndPose();
// get side of exit direction
// get angle from exit position to position
ea = atan2(pos.y - ry, pos.x - rx);
if (ea > angle)
// position is to the left of exit direction
dist = -dist;
}
else
{ // distance from (0.0, 0.0)
dist = sqrt(sqr(pos.x) + sqr(pos.y));
w = 1;
if (pHit != NULL)
// hitpoint is 0,0
pHit->set(0.0, 0.0, 0.0);
if (pos.y > 0.0)
// to the left of entry direction - is negative
dist = -dist;
}
}
if (where != NULL)
*where = w;
if (not posIsRight)
dist = -dist;
return dist;
}
