feat: migrate explore.cpp
Juan Galvis
parent
9f8bcd3b5c
commit
0f613a07cd
|
|
@ -0,0 +1,343 @@
|
||||||
|
/*********************************************************************
|
||||||
|
*
|
||||||
|
* Software License Agreement (BSD License)
|
||||||
|
*
|
||||||
|
* Copyright (c) 2008, Robert Bosch LLC.
|
||||||
|
* Copyright (c) 2015-2016, Jiri Horner.
|
||||||
|
* All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* * Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* * Redistributions in binary form must reproduce the above
|
||||||
|
* copyright notice, this list of conditions and the following
|
||||||
|
* disclaimer in the documentation and/or other materials provided
|
||||||
|
* with the distribution.
|
||||||
|
* * Neither the name of the Jiri Horner nor the names of its
|
||||||
|
* contributors may be used to endorse or promote products derived
|
||||||
|
* from this software without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||||
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
*********************************************************************/
|
||||||
|
|
||||||
|
#include <explore/explore.h>
|
||||||
|
|
||||||
|
#include <thread>
|
||||||
|
|
||||||
|
namespace explore
|
||||||
|
{
|
||||||
|
Explore::Explore()
|
||||||
|
: Node("explore_node")
|
||||||
|
, tf_buffer_(this->get_clock())
|
||||||
|
, tf_listener_(tf_buffer_)
|
||||||
|
, costmap_client_(*this, &tf_buffer_)
|
||||||
|
, prev_distance_(0)
|
||||||
|
, last_markers_count_(0)
|
||||||
|
{
|
||||||
|
double timeout;
|
||||||
|
double min_frontier_size;
|
||||||
|
this->declare_parameter<float>("planner_frequency", 1.0);
|
||||||
|
this->declare_parameter<float>("progress_timeout", 30.0);
|
||||||
|
this->declare_parameter<bool>("visualize", false);
|
||||||
|
this->declare_parameter<float>("potential_scale", 1e-3);
|
||||||
|
this->declare_parameter<float>("orientation_scale", 0.0);
|
||||||
|
this->declare_parameter<float>("gain_scale", 1.0);
|
||||||
|
this->declare_parameter<float>("min_frontier_size", 0.5);
|
||||||
|
|
||||||
|
this->get_parameter("planner_frequency", planner_frequency_);
|
||||||
|
this->get_parameter("progress_timeout", timeout);
|
||||||
|
this->get_parameter("visualize", visualize_);
|
||||||
|
this->get_parameter("potential_scale", potential_scale_);
|
||||||
|
this->get_parameter("orientation_scale", orientation_scale_);
|
||||||
|
this->get_parameter("gain_scale", gain_scale_);
|
||||||
|
this->get_parameter("min_frontier_size", min_frontier_size);
|
||||||
|
progress_timeout_ = timeout;
|
||||||
|
|
||||||
|
move_base_client_ =
|
||||||
|
rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(this,
|
||||||
|
"/navigat"
|
||||||
|
"e_to_"
|
||||||
|
"pose");
|
||||||
|
|
||||||
|
search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(),
|
||||||
|
potential_scale_, gain_scale_,
|
||||||
|
min_frontier_size);
|
||||||
|
|
||||||
|
if (visualize_) {
|
||||||
|
marker_array_publisher_ =
|
||||||
|
this->create_publisher<visualization_msgs::msg::MarkerArray>("frontier"
|
||||||
|
"s",
|
||||||
|
10);
|
||||||
|
}
|
||||||
|
|
||||||
|
RCLCPP_INFO(logger_, "Waiting to connect to move_base nav2 server");
|
||||||
|
move_base_client_->wait_for_action_server();
|
||||||
|
RCLCPP_INFO(logger_, "Connected to move_base nav2 server");
|
||||||
|
|
||||||
|
exploring_timer_ = this->create_wall_timer(
|
||||||
|
std::chrono::milliseconds((uint16_t)(1000.0 / planner_frequency_)),
|
||||||
|
[this]() { makePlan(); });
|
||||||
|
}
|
||||||
|
|
||||||
|
Explore::~Explore()
|
||||||
|
{
|
||||||
|
stop();
|
||||||
|
}
|
||||||
|
|
||||||
|
void Explore::visualizeFrontiers(
|
||||||
|
const std::vector<frontier_exploration::Frontier>& frontiers)
|
||||||
|
{
|
||||||
|
std_msgs::msg::ColorRGBA blue;
|
||||||
|
blue.r = 0;
|
||||||
|
blue.g = 0;
|
||||||
|
blue.b = 1.0;
|
||||||
|
blue.a = 1.0;
|
||||||
|
std_msgs::msg::ColorRGBA red;
|
||||||
|
red.r = 1.0;
|
||||||
|
red.g = 0;
|
||||||
|
red.b = 0;
|
||||||
|
red.a = 1.0;
|
||||||
|
std_msgs::msg::ColorRGBA green;
|
||||||
|
green.r = 0;
|
||||||
|
green.g = 1.0;
|
||||||
|
green.b = 0;
|
||||||
|
green.a = 1.0;
|
||||||
|
|
||||||
|
RCLCPP_DEBUG(logger_, "visualising %lu frontiers", frontiers.size());
|
||||||
|
visualization_msgs::msg::MarkerArray markers_msg;
|
||||||
|
std::vector<visualization_msgs::msg::Marker>& markers = markers_msg.markers;
|
||||||
|
visualization_msgs::msg::Marker m;
|
||||||
|
|
||||||
|
m.header.frame_id = costmap_client_.getGlobalFrameID();
|
||||||
|
m.header.stamp = this->now();
|
||||||
|
m.ns = "frontiers";
|
||||||
|
m.scale.x = 1.0;
|
||||||
|
m.scale.y = 1.0;
|
||||||
|
m.scale.z = 1.0;
|
||||||
|
m.color.r = 0;
|
||||||
|
m.color.g = 0;
|
||||||
|
m.color.b = 255;
|
||||||
|
m.color.a = 255;
|
||||||
|
// lives forever
|
||||||
|
// m.lifetime = ros::Duration(0);
|
||||||
|
m.lifetime = rclcpp::Duration(0);
|
||||||
|
m.frame_locked = true;
|
||||||
|
|
||||||
|
// weighted frontiers are always sorted
|
||||||
|
double min_cost = frontiers.empty() ? 0. : frontiers.front().cost;
|
||||||
|
|
||||||
|
m.action = visualization_msgs::msg::Marker::ADD;
|
||||||
|
size_t id = 0;
|
||||||
|
for (auto& frontier : frontiers) {
|
||||||
|
m.type = visualization_msgs::msg::Marker::POINTS;
|
||||||
|
m.id = int(id);
|
||||||
|
m.pose.position = {};
|
||||||
|
m.scale.x = 0.1;
|
||||||
|
m.scale.y = 0.1;
|
||||||
|
m.scale.z = 0.1;
|
||||||
|
m.points = frontier.points;
|
||||||
|
if (goalOnBlacklist(frontier.centroid)) {
|
||||||
|
m.color = red;
|
||||||
|
} else {
|
||||||
|
m.color = blue;
|
||||||
|
}
|
||||||
|
markers.push_back(m);
|
||||||
|
++id;
|
||||||
|
m.type = visualization_msgs::msg::Marker::SPHERE;
|
||||||
|
m.id = int(id);
|
||||||
|
m.pose.position = frontier.initial;
|
||||||
|
// scale frontier according to its cost (costier frontiers will be smaller)
|
||||||
|
double scale = std::min(std::abs(min_cost * 0.4 / frontier.cost), 0.5);
|
||||||
|
m.scale.x = scale;
|
||||||
|
m.scale.y = scale;
|
||||||
|
m.scale.z = scale;
|
||||||
|
m.points = {};
|
||||||
|
m.color = green;
|
||||||
|
markers.push_back(m);
|
||||||
|
++id;
|
||||||
|
}
|
||||||
|
size_t current_markers_count = markers.size();
|
||||||
|
|
||||||
|
// delete previous markers, which are now unused
|
||||||
|
m.action = visualization_msgs::msg::Marker::DELETE;
|
||||||
|
for (; id < last_markers_count_; ++id) {
|
||||||
|
m.id = int(id);
|
||||||
|
markers.push_back(m);
|
||||||
|
}
|
||||||
|
|
||||||
|
last_markers_count_ = current_markers_count;
|
||||||
|
marker_array_publisher_->publish(markers_msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Explore::makePlan()
|
||||||
|
{
|
||||||
|
// find frontiers
|
||||||
|
auto pose = costmap_client_.getRobotPose();
|
||||||
|
// get frontiers sorted according to cost
|
||||||
|
auto frontiers = search_.searchFrom(pose.position);
|
||||||
|
RCLCPP_DEBUG(logger_, "found %lu frontiers", frontiers.size());
|
||||||
|
for (size_t i = 0; i < frontiers.size(); ++i) {
|
||||||
|
RCLCPP_DEBUG(logger_, "frontier %zd cost: %f", i, frontiers[i].cost);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (frontiers.empty()) {
|
||||||
|
stop();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// publish frontiers as visualization markers
|
||||||
|
if (visualize_) {
|
||||||
|
visualizeFrontiers(frontiers);
|
||||||
|
}
|
||||||
|
|
||||||
|
// find non blacklisted frontier
|
||||||
|
auto frontier =
|
||||||
|
std::find_if_not(frontiers.begin(), frontiers.end(),
|
||||||
|
[this](const frontier_exploration::Frontier& f) {
|
||||||
|
return goalOnBlacklist(f.centroid);
|
||||||
|
});
|
||||||
|
if (frontier == frontiers.end()) {
|
||||||
|
stop();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
geometry_msgs::msg::Point target_position = frontier->centroid;
|
||||||
|
|
||||||
|
// time out if we are not making any progress
|
||||||
|
bool same_goal = prev_goal_ == target_position;
|
||||||
|
|
||||||
|
prev_goal_ = target_position;
|
||||||
|
if (!same_goal || prev_distance_ > frontier->min_distance) {
|
||||||
|
// we have different goal or we made some progress
|
||||||
|
last_progress_ = this->now();
|
||||||
|
prev_distance_ = frontier->min_distance;
|
||||||
|
}
|
||||||
|
// black list if we've made no progress for a long time
|
||||||
|
if (this->now() - last_progress_ >
|
||||||
|
tf2::durationFromSec(progress_timeout_)) { // TODO: is progress_timeout_
|
||||||
|
// in seconds?
|
||||||
|
frontier_blacklist_.push_back(target_position);
|
||||||
|
RCLCPP_DEBUG(logger_, "Adding current goal to black list");
|
||||||
|
makePlan();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// we don't need to do anything if we still pursuing the same goal
|
||||||
|
if (same_goal) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
RCLCPP_DEBUG(logger_, "Sending goal to move base nav2");
|
||||||
|
|
||||||
|
// send goal to move_base if we have something new to pursue
|
||||||
|
auto goal = nav2_msgs::action::NavigateToPose::Goal();
|
||||||
|
goal.pose.pose.position = target_position;
|
||||||
|
goal.pose.pose.orientation.w = 1.;
|
||||||
|
goal.pose.header.frame_id = costmap_client_.getGlobalFrameID();
|
||||||
|
goal.pose.header.stamp = this->now();
|
||||||
|
|
||||||
|
auto send_goal_options =
|
||||||
|
rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SendGoalOptions();
|
||||||
|
// send_goal_options.goal_response_callback =
|
||||||
|
// std::bind(&Explore::goal_response_callback, this, _1);
|
||||||
|
// send_goal_options.feedback_callback =
|
||||||
|
// std::bind(&Explore::feedback_callback, this, _1, _2);
|
||||||
|
send_goal_options.result_callback =
|
||||||
|
[this,
|
||||||
|
target_position](const NavigationGoalHandle::WrappedResult& result) {
|
||||||
|
reachedGoal(result, target_position);
|
||||||
|
};
|
||||||
|
move_base_client_->async_send_goal(goal, send_goal_options);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Explore::goalOnBlacklist(const geometry_msgs::msg::Point& goal)
|
||||||
|
{
|
||||||
|
constexpr static size_t tolerace = 5;
|
||||||
|
nav2_costmap_2d::Costmap2D* costmap2d = costmap_client_.getCostmap();
|
||||||
|
|
||||||
|
// check if a goal is on the blacklist for goals that we're pursuing
|
||||||
|
for (auto& frontier_goal : frontier_blacklist_) {
|
||||||
|
double x_diff = fabs(goal.x - frontier_goal.x);
|
||||||
|
double y_diff = fabs(goal.y - frontier_goal.y);
|
||||||
|
|
||||||
|
if (x_diff < tolerace * costmap2d->getResolution() &&
|
||||||
|
y_diff < tolerace * costmap2d->getResolution())
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Explore::reachedGoal(const NavigationGoalHandle::WrappedResult& result,
|
||||||
|
const geometry_msgs::msg::Point& frontier_goal)
|
||||||
|
{
|
||||||
|
switch (result.code) {
|
||||||
|
case rclcpp_action::ResultCode::SUCCEEDED:
|
||||||
|
RCLCPP_DEBUG(logger_, "Goal was successful");
|
||||||
|
break;
|
||||||
|
case rclcpp_action::ResultCode::ABORTED:
|
||||||
|
RCLCPP_DEBUG(logger_, "Goal was aborted");
|
||||||
|
frontier_blacklist_.push_back(frontier_goal);
|
||||||
|
RCLCPP_DEBUG(logger_, "Adding current goal to black list");
|
||||||
|
return;
|
||||||
|
case rclcpp_action::ResultCode::CANCELED:
|
||||||
|
RCLCPP_DEBUG(logger_, "Goal was canceled");
|
||||||
|
return;
|
||||||
|
default:
|
||||||
|
RCLCPP_WARN(logger_, "Unknown result code from move base nav2");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
// find new goal immediately regardless of planning frequency.
|
||||||
|
// execute via timer to prevent dead lock in move_base_client (this is
|
||||||
|
// callback for sendGoal, which is called in makePlan). the timer must live
|
||||||
|
// until callback is executed.
|
||||||
|
// oneshot_ = relative_nh_.createTimer(
|
||||||
|
// ros::Duration(0, 0), [this](const ros::TimerEvent&) { makePlan(); },
|
||||||
|
// true);
|
||||||
|
|
||||||
|
// TODO: Implement this with ros2 timers?
|
||||||
|
// Because of the async nature of ros2 calls I think this is not needed.
|
||||||
|
// makePlan();
|
||||||
|
}
|
||||||
|
|
||||||
|
void Explore::start()
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(logger_, "Exploration started.");
|
||||||
|
}
|
||||||
|
|
||||||
|
void Explore::stop()
|
||||||
|
{
|
||||||
|
move_base_client_->async_cancel_all_goals();
|
||||||
|
exploring_timer_->cancel();
|
||||||
|
RCLCPP_INFO(logger_, "Exploration stopped.");
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace explore
|
||||||
|
|
||||||
|
int main(int argc, char** argv)
|
||||||
|
{
|
||||||
|
rclcpp::init(argc, argv);
|
||||||
|
// ROS1 code
|
||||||
|
/*
|
||||||
|
if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
|
||||||
|
ros::console::levels::Debug)) {
|
||||||
|
ros::console::notifyLoggerLevelsChanged();
|
||||||
|
} */
|
||||||
|
rclcpp::spin(
|
||||||
|
std::make_shared<explore::Explore>()); // std::move(std::make_unique)?
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue