From 0f613a07cd42c9ab310537accc8b4f09f6158a20 Mon Sep 17 00:00:00 2001 From: Juan Galvis Date: Thu, 5 Aug 2021 00:39:39 +0400 Subject: [PATCH] feat: migrate explore.cpp --- explore/src/explore.cpp | 343 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 343 insertions(+) create mode 100644 explore/src/explore.cpp diff --git a/explore/src/explore.cpp b/explore/src/explore.cpp new file mode 100644 index 0000000..ca6807c --- /dev/null +++ b/explore/src/explore.cpp @@ -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 + +#include + +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("planner_frequency", 1.0); + this->declare_parameter("progress_timeout", 30.0); + this->declare_parameter("visualize", false); + this->declare_parameter("potential_scale", 1e-3); + this->declare_parameter("orientation_scale", 0.0); + this->declare_parameter("gain_scale", 1.0); + this->declare_parameter("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(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("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& 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& 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::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()); // std::move(std::make_unique)? + rclcpp::shutdown(); + return 0; +}