Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

planner_cspace: refactor start pose building algorithm and fix finishing condition when start is relocated #718

Merged
merged 1 commit into from
Sep 11, 2023
Merged

planner_cspace: refactor start pose building algorithm and fix finishing condition when start is relocated #718

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
217 changes: 119 additions & 98 deletions planner_cspace/src/planner_3d.cpp
View file
Open in desktop
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.");
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think logs in buildStartPoses shouldn't be shown on GetPlan service call

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

cbMakePlan() does not call this function as it uses the start pose designated in GetPlan::Request without anti-alias or relocations.

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