static I impl(I begin, I end, C pred, P proj, D len, Pair const p, concepts::ForwardIterator *fi)
{
// *begin is known to be false
// len >= 1
if(len == 1)
return begin;
if(len == 2)
{
I tmp = begin;
if(invoke(pred, invoke(proj, *++tmp)))
{
ranges::iter_swap(begin, tmp);
return tmp;
}
return begin;
}
if(len <= p.second)
{ // The buffer is big enough to use
// Move the falses into the temporary buffer, and the trues to the front of the line
// Update begin to always point to the end of the trues
auto tmpbuf = make_raw_buffer(p.first);
auto buf = tmpbuf.begin();
*buf = iter_move(begin);
++buf;
auto res = partition_copy(make_move_iterator(next(begin)),
make_move_sentinel(end), begin, buf, std::ref(pred), std::ref(proj));
// All trues now at start of range, all falses in buffer
// Move falses back into range, but don't mess up begin which points to first false
ranges::move(p.first, res.out2().base().base(), res.out1());
// h destructs moved-from values out of the temp buffer, but doesn't deallocate buffer
return res.out1();
}
// Else not enough buffer, do in place
// len >= 3
D half = len / 2; // half >= 2
I middle = next(begin, half);
// recurse on [begin, middle), *begin know to be false
// F?????????????????
// f m l
I begin_false = stable_partition_fn::impl(begin, middle, pred, proj, half, p, fi);
// TTTFFFFF??????????
// f ff m l
// recurse on [middle, end], except increase middle until *(middle) is false, *end know to be true
I m1 = middle;
D len_half = len - half;
while(invoke(pred, invoke(proj, *m1)))
{
if(++m1 == end)
return ranges::rotate(begin_false, middle, end).begin();
--len_half;
}
// TTTFFFFFTTTF??????
// f ff m m1 l
I end_false = stable_partition_fn::impl(m1, end, pred, proj, len_half, p, fi);
// TTTFFFFFTTTTTFFFFF
// f ff m sf l
return ranges::rotate(begin_false, middle, end_false).begin();
// TTTTTTTTFFFFFFFFFF
// |
}
void String::reallocate()
{
auto newcapacity = size() ? size()*2 : 1;
// auto new_data = alloc.allocate(newcapacity);
// auto dest = new_data;
// auto elem = elements;
// for(size_t i=0; i !=size(); i++){
// alloc.construct(dest++, std::move(*elem++));
// }
auto first = alloc.allocate(newcapacity);
auto last = uninitialized_copy(make_move_iterator(begin()), make_move_iterator(end()), first);
free();
// elements = new_data;
// first_free = dest;
elements = first;
first_free = last;
cap = elements + newcapacity;
}
static void merge_sort_loop(I1 begin, I1 end, I2 result, D step_size, C &pred, P &proj)
{
D two_step = 2 * step_size;
while(end - begin >= two_step)
{
result = merge(make_move_iterator(begin),
make_move_iterator(begin + step_size),
make_move_iterator(begin + step_size),
make_move_iterator(begin + two_step), result,
std::ref(pred), std::ref(proj), std::ref(proj)).out();
begin += two_step;
}
step_size = ranges::min(D(end - begin), step_size);
merge(make_move_iterator(begin), make_move_iterator(begin + step_size),
make_move_iterator(begin + step_size), make_move_iterator(end), result,
std::ref(pred), std::ref(proj), std::ref(proj));
}
static I impl(I begin, I end, C pred, P proj, D len, Pair p,
detail::bidirectional_iterator_tag_ bi)
{
// *begin is known to be false
// *end is known to be true
// len >= 2
if(len == 2)
{
ranges::iter_swap(begin, end);
return end;
}
if(len == 3)
{
I tmp = begin;
if(invoke(pred, invoke(proj, *++tmp)))
{
ranges::iter_swap(begin, tmp);
ranges::iter_swap(tmp, end);
return end;
}
ranges::iter_swap(tmp, end);
ranges::iter_swap(begin, tmp);
return tmp;
}
if(len <= p.second)
{ // The buffer is big enough to use
// Move the falses into the temporary buffer, and the trues to the front
// of the line Update begin to always point to the end of the trues
auto tmpbuf = ranges::make_raw_buffer(p.first);
auto buf = tmpbuf.begin();
*buf = iter_move(begin);
++buf;
auto res = partition_copy(make_move_iterator(next(begin)),
make_move_sentinel(end),
begin,
buf,
std::ref(pred),
std::ref(proj));
begin = res.out1;
// move *end, known to be true
*begin = iter_move(res.in);
++begin;
// All trues now at start of range, all falses in buffer
// Move falses back into range, but don't mess up begin which points to
// first false
ranges::move(p.first, res.out2.base().base(), begin);
// h destructs moved-from values out of the temp buffer, but doesn't
// deallocate buffer
return begin;
}
// Else not enough buffer, do in place
// len >= 4
I middle = begin;
D half = len / 2; // half >= 2
advance(middle, half);
// recurse on [begin, middle-1], except reduce middle-1 until *(middle-1) is
// true, *begin know to be false F????????????????T f m l
I m1 = middle;
I begin_false = begin;
D len_half = half;
while(!invoke(pred, invoke(proj, *--m1)))
{
if(m1 == begin)
goto first_half_done;
--len_half;
}
// F???TFFF?????????T
// f m1 m l
begin_false =
stable_partition_fn::impl(begin, m1, pred, proj, len_half, p, bi);
first_half_done:
// TTTFFFFF?????????T
// f ff m l
// recurse on [middle, end], except increase middle until *(middle) is false,
// *end know to be true
m1 = middle;
len_half = len - half;
while(invoke(pred, invoke(proj, *m1)))
{
if(++m1 == end)
return ranges::rotate(begin_false, middle, ++end).begin();
--len_half;
}
// TTTFFFFFTTTF?????T
// f ff m m1 l
I end_false = stable_partition_fn::impl(m1, end, pred, proj, len_half, p, bi);
// TTTFFFFFTTTTTFFFFF
// f ff m sf l
return ranges::rotate(begin_false, middle, end_false).begin();
// TTTTTTTTFFFFFFFFFF
// |
}
bool PFLocalPlanner::select_goal(geometry_msgs::PoseStamped *_goal)
{
if (! this->local_reached)
{
*_goal = this->current_local_goal;
return true;
}
if (this->global_plan.size() == 0)
{
//reached = true;
return false;
}
geometry_msgs::PoseStamped pose;
if (!this->my_pose(&pose))
{
ROS_ERROR("Cannot get pose on the goal frame: %s", this->goal_frame_id.c_str());
return false;
}
tf::StampedTransform goalToLocal;
try
{
this->tf->lookupTransform(this->cmd_frame_id, this->goal_frame_id, ros::Time(0), goalToLocal);
}
catch (tf::TransformException ex)
{
ROS_ERROR("Goal To Local: %s", ex.what());
return false;
}
std::vector<geometry_msgs::PoseStamped>::iterator it;
double d, dx, dy;
bool has_goal = false;
tf::Vector3 candidate;
tf::Quaternion orientation;
int i=0;
for (it = this->global_plan.begin(); it != this->global_plan.end(); it++)
{
candidate.setX(it->pose.position.x);
candidate.setY(it->pose.position.y);
orientation = tf::Quaternion(it->pose.orientation.x,it->pose.orientation.y,it->pose.orientation.z,it->pose.orientation.w);
candidate = goalToLocal * candidate;
geometry_msgs::Point tmp;
tmp.x = candidate.x();
tmp.y = candidate.y();
// if(it->pose.position.z == 1.0) continue;
d = this->dist(pose.pose.position,tmp);
if (d > max_local_goal_dist)
break;
i++;
}
//if(it != global_plan.end())
// it->pose.position.z = 1.0
orientation = goalToLocal*orientation;
_goal->pose.position.x = candidate.x();
_goal->pose.position.y = candidate.y();
_goal->pose.orientation.x = orientation.x();
_goal->pose.orientation.y = orientation.y();
_goal->pose.orientation.z = orientation.z();
_goal->pose.orientation.w = orientation.w();
_goal->header.frame_id = this->cmd_frame_id;
global_plan = vector<geometry_msgs::PoseStamped>(
make_move_iterator(global_plan.begin() + i),
make_move_iterator(global_plan.end()));
local_goal_pub.publish(*_goal);
this->current_local_goal = *_goal;
return true;
}
