Skip to content

Commit

Permalink
planner_cspace: refactor start pose building algorithm and fix finish…
Browse files Browse the repository at this point in the history 
…ing condition when start is relocated (#718)
  • Loading branch information
@nhatao
nhatao authored Sep 11, 2023
1 parent a9d1430 commit f1720e0
Showing 1 changed file with 119 additions and 98 deletions.
217 changes: 119 additions & 98 deletions planner_cspace/src/planner_3d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1623,38 +1623,59 @@ class Planner3dNode
}

protected:
bool makePlan(const geometry_msgs::Pose& gs, const geometry_msgs::Pose& ge,
nav_msgs::Path& path, bool hyst)
void publishStartAndGoalMarkers(const Astar::Vec& start_grid, Astar::Vec& end_grid)
{
Astar::Vec e;
grid_metric_converter::metric2Grid(
map_info_, e[0], e[1], e[2],
ge.position.x, ge.position.y, tf2::getYaw(ge.orientation));
e.cycleUnsigned(map_info_.angle);
geometry_msgs::PoseStamped p;
p.header = map_header_;
float x, y, yaw;
grid_metric_converter::grid2Metric(map_info_, end_grid[0], end_grid[1], end_grid[2], x, y, yaw);
p.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
p.pose.position.x = x;
p.pose.position.y = y;
pub_end_.publish(p);
grid_metric_converter::grid2Metric(map_info_, start_grid[0], start_grid[1], start_grid[2], x, y, yaw);
p.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
p.pose.position.x = x;
p.pose.position.y = y;
pub_start_.publish(p);
}

enum class StartPoseStatus
{
START_OCCUPIED,
FINISHING,
NORMAL,
};
StartPoseStatus buildStartPoses(const geometry_msgs::Pose& start_metric, const geometry_msgs::Pose& end_metric,
std::vector<Astar::VecWithCost>& result_start_poses) const
{
Astar::Vecf sf;
grid_metric_converter::metric2Grid(
map_info_, sf[0], sf[1], sf[2],
gs.position.x, gs.position.y, tf2::getYaw(gs.orientation));
Astar::Vec s(static_cast<int>(std::floor(sf[0])), static_cast<int>(std::floor(sf[1])), std::lround(sf[2]));
s.cycleUnsigned(map_info_.angle);
if (!cm_.validate(s, range_))
start_metric.position.x, start_metric.position.y, tf2::getYaw(start_metric.orientation));
Astar::Vec start_grid(static_cast<int>(std::floor(sf[0])), static_cast<int>(std::floor(sf[1])), std::lround(sf[2]));
start_grid.cycleUnsigned(map_info_.angle);

Astar::Vec end_grid;
grid_metric_converter::metric2Grid(
map_info_, end_grid[0], end_grid[1], end_grid[2],
end_metric.position.x, end_metric.position.y, tf2::getYaw(end_metric.orientation));
end_grid.cycleUnsigned(map_info_.angle);

if (!cm_.validate(start_grid, range_))
{
ROS_ERROR("You are on the edge of the world.");
status_.error = planner_cspace_msgs::PlannerStatus::IN_ROCK;
return false;
return StartPoseStatus::START_OCCUPIED;
}

std::vector<Astar::VecWithCost> starts;
if (antialias_start_)
{
const int x_cand[] = {0, ((sf[0] - s[0]) < 0.5 ? -1 : 1)};
const int y_cand[] = {0, ((sf[1] - s[1]) < 0.5 ? -1 : 1)};
const int x_cand[] = {0, ((sf[0] - start_grid[0]) < 0.5 ? -1 : 1)};
const int y_cand[] = {0, ((sf[1] - start_grid[1]) < 0.5 ? -1 : 1)};
for (const int x : x_cand)
{
for (const int y : y_cand)
{
const Astar::Vec p = s + Astar::Vec(x, y, 0);
const Astar::Vec p = start_grid + Astar::Vec(x, y, 0);
if (!cm_.validate(p, range_))
continue;

Expand All @@ -1664,43 +1685,74 @@ class Planner3dNode
if (cm_[p] > 99)
continue;

starts.push_back(Astar::VecWithCost(p));
const Astar::Vecf diff = Astar::Vecf(p[0] + 0.5f, p[1] + 0.5f, 0.0f) - sf;
const double cost = std::hypot(diff[0] * ec_[0], diff[1] * ec_[0]) +
cm_[p] * cc_.weight_costmap_ / 100.0;
result_start_poses.push_back(Astar::VecWithCost(p, cost));
}
}
}
else
else if (cm_[start_grid] < 100)
{
if (cm_[s] < 100)
result_start_poses.push_back(Astar::VecWithCost(start_grid));
}
if (result_start_poses.empty())
{
if (!searchAvailablePos(cm_, start_grid, tolerance_range_, tolerance_angle_))
{
starts.push_back(Astar::VecWithCost(s));
ROS_WARN("Oops! You are in Rock!");
return StartPoseStatus::START_OCCUPIED;
}
ROS_INFO("Start moved");
result_start_poses.push_back(Astar::VecWithCost(start_grid));
}
for (Astar::VecWithCost& s : starts)
int g_tolerance_lin, g_tolerance_ang;
if (act_tolerant_->isActive())
{
g_tolerance_lin = std::lround(goal_tolerant_->goal_tolerance_lin / map_info_.linear_resolution);
g_tolerance_ang = std::lround(goal_tolerant_->goal_tolerance_ang / map_info_.angular_resolution);
}
else
{
g_tolerance_lin = goal_tolerance_lin_;
g_tolerance_ang = goal_tolerance_ang_;
}
for (const Astar::VecWithCost& s : result_start_poses)
{
const Astar::Vecf diff =
Astar::Vecf(s.v_[0] + 0.5f, s.v_[1] + 0.5f, 0.0f) - sf;
s.c_ = std::hypot(diff[0] * ec_[0], diff[1] * ec_[0]);
s.c_ += cm_[s.v_] * cc_.weight_costmap_ / 100.0;

// Check if arrived to the goal
Astar::Vec remain = s.v_ - e;
Astar::Vec remain = s.v_ - end_grid;
remain.cycle(map_info_.angle);

int g_tolerance_lin, g_tolerance_ang;
if (act_tolerant_->isActive())
{
g_tolerance_lin = std::lround(goal_tolerant_->goal_tolerance_lin / map_info_.linear_resolution);
g_tolerance_ang = std::lround(goal_tolerant_->goal_tolerance_ang / map_info_.angular_resolution);
}
else
{
g_tolerance_lin = goal_tolerance_lin_;
g_tolerance_ang = goal_tolerance_ang_;
}

if (remain.sqlen() <= g_tolerance_lin * g_tolerance_lin &&
std::abs(remain[2]) <= g_tolerance_ang)
{
return StartPoseStatus::FINISHING;
}
}
return StartPoseStatus::NORMAL;
}

bool makePlan(const geometry_msgs::Pose& start_metric, const geometry_msgs::Pose& end_metric,
nav_msgs::Path& path, bool hyst)
{
Astar::Vec start_grid;
grid_metric_converter::metric2Grid(
map_info_, start_grid[0], start_grid[1], start_grid[2],
start_metric.position.x, start_metric.position.y, tf2::getYaw(start_metric.orientation));
start_grid.cycleUnsigned(map_info_.angle);
Astar::Vec end_grid;
grid_metric_converter::metric2Grid(
map_info_, end_grid[0], end_grid[1], end_grid[2],
end_metric.position.x, end_metric.position.y, tf2::getYaw(end_metric.orientation));
end_grid.cycleUnsigned(map_info_.angle);
publishStartAndGoalMarkers(start_grid, end_grid);

std::vector<Astar::VecWithCost> starts;
switch (buildStartPoses(start_metric, end_metric, starts))
{
case StartPoseStatus::START_OCCUPIED:
status_.error = planner_cspace_msgs::PlannerStatus::IN_ROCK;
return false;
case StartPoseStatus::FINISHING:
if (escaping_)
{
goal_ = goal_raw_;
Expand All @@ -1709,64 +1761,42 @@ class Planner3dNode
ROS_INFO("Escaped");
return true;
}
if (act_tolerant_->isActive() && goal_tolerant_->continuous_movement_mode)
{
ROS_INFO("Robot reached near the goal.");
act_tolerant_->setSucceeded(planner_cspace_msgs::MoveWithToleranceResult(),
"Goal reached (Continuous movement mode).");
goal_tolerant_ = nullptr;
}
else
{
if (act_tolerant_->isActive() && goal_tolerant_->continuous_movement_mode)
{
ROS_INFO("Robot reached near the goal.");
act_tolerant_->setSucceeded(planner_cspace_msgs::MoveWithToleranceResult(),
"Goal reached (Continuous movement mode).");
goal_tolerant_ = nullptr;
}
else
{
status_.status = planner_cspace_msgs::PlannerStatus::FINISHING;
publishFinishPath();
ROS_INFO("Path plan finishing");
return true;
}
status_.status = planner_cspace_msgs::PlannerStatus::FINISHING;
publishFinishPath();
ROS_INFO("Path plan finishing");
return true;
}
}
}

geometry_msgs::PoseStamped p;
p.header = map_header_;
float x, y, yaw;
grid_metric_converter::grid2Metric(map_info_, e[0], e[1], e[2], x, y, yaw);
p.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
p.pose.position.x = x;
p.pose.position.y = y;
pub_end_.publish(p);

if (starts.size() == 0)
{
if (!searchAvailablePos(cm_, s, tolerance_range_, tolerance_angle_))
{
ROS_WARN("Oops! You are in Rock!");
status_.error = planner_cspace_msgs::PlannerStatus::IN_ROCK;
return false;
}
ROS_INFO("Start moved");
starts.push_back(Astar::VecWithCost(s));
break;
default:
break;
}
const Astar::Vec s_rough(s[0], s[1], 0);

const float initial_2dof_cost = cost_estim_cache_[Astar::Vec(start_grid[0], start_grid[1], 0)];
// If goal gets occupied, cost_estim_cache_ is not updated to reduce
// computational cost for clearing huge map. In this case, cm_[e] is 100.
if (cost_estim_cache_[s_rough] == std::numeric_limits<float>::max() || cm_[e] >= 100)
if (initial_2dof_cost == std::numeric_limits<float>::max() || cm_[end_grid] >= 100)
{
status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
ROS_WARN("Goal unreachable.");
if (!escaping_ && temporary_escape_)
{
e = s;
if (searchAvailablePos(cm_, e, esc_range_, esc_angle_, 50, esc_range_ / 2))
end_grid = start_grid;
if (searchAvailablePos(cm_, end_grid, esc_range_, esc_angle_, 50, esc_range_ / 2))
{
escaping_ = true;
ROS_INFO("Temporary goal (%d, %d, %d)",
e[0], e[1], e[2]);
end_grid[0], end_grid[1], end_grid[2]);
float x, y, yaw;
grid_metric_converter::grid2Metric(map_info_, e[0], e[1], e[2], x, y, yaw);
grid_metric_converter::grid2Metric(map_info_, end_grid[0], end_grid[1], end_grid[2], x, y, yaw);
goal_.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
goal_.pose.position.x = x;
goal_.pose.position.y = y;
Expand All @@ -1778,19 +1808,10 @@ class Planner3dNode
return false;
}

grid_metric_converter::grid2Metric(map_info_, s[0], s[1], s[2], x, y, yaw);
p.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
p.pose.position.x = x;
p.pose.position.y = y;
pub_start_.publish(p);

const float range_limit = cost_estim_cache_[s_rough] - (local_range_ + range_) * ec_[0];

const float range_limit = initial_2dof_cost - (local_range_ + range_) * ec_[0];
const auto ts = boost::chrono::high_resolution_clock::now();
// ROS_INFO("Planning from (%d, %d, %d) to (%d, %d, %d)",
// s[0], s[1], s[2], e[0], e[1], e[2]);

const auto cb_progress = [this, ts, s, e](const std::list<Astar::Vec>& path_grid, const SearchStats& stats) -> bool
const auto cb_progress =
[this, ts, start_grid, end_grid](const std::list<Astar::Vec>& path_grid, const SearchStats& stats) -> bool
{
const auto tnow = boost::chrono::high_resolution_clock::now();
const auto tdiff = boost::chrono::duration<float>(tnow - ts).count();
Expand All @@ -1805,7 +1826,7 @@ class Planner3dNode
"num_search_queue=%lu, "
"num_prev_updates=%lu, "
"num_total_updates=%lu, ",
s[0], s[1], s[2], e[0], e[1], e[2], tdiff,
start_grid[0], start_grid[1], start_grid[2], end_grid[0], end_grid[1], end_grid[2], tdiff,
stats.num_loop, stats.num_search_queue, stats.num_prev_updates, stats.num_total_updates);
return false;
}
Expand All @@ -1815,7 +1836,7 @@ class Planner3dNode

model_->enableHysteresis(hyst && has_hysteresis_map_);
std::list<Astar::Vec> path_grid;
if (!as_.search(starts, e, path_grid,
if (!as_.search(starts, end_grid, path_grid,
model_,
cb_progress,
range_limit,
Expand Down

0 comments on commit f1720e0

Please sign in to comment.