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