diff --git a/explore_lite_revised/include/explore/explore.h b/explore_lite_revised/include/explore/explore.h index 52cdbcf..da9e811 100644 --- a/explore_lite_revised/include/explore/explore.h +++ b/explore_lite_revised/include/explore/explore.h @@ -90,6 +90,7 @@ class Explore ros::Publisher marker_array_publisher_; tf::TransformListener tf_listener_; + Costmap2DClient costmap_client_; actionlib::SimpleActionClient move_base_client_; diff --git a/explore_lite_revised/include/explore/frontier_search.h b/explore_lite_revised/include/explore/frontier_search.h index e0e228e..e95bbdf 100644 --- a/explore_lite_revised/include/explore/frontier_search.h +++ b/explore_lite_revised/include/explore/frontier_search.h @@ -9,14 +9,21 @@ namespace frontier_exploration * @brief Represents a frontier * */ + struct Frontier { - std::uint32_t size; - double min_distance; - double cost; + //todo make a class of it, make methods static methods of class +// std::uint32_t size{0}; + double min_distance{std::numeric_limits::infinity()}; + double cost{0.0}; geometry_msgs::Point initial; geometry_msgs::Point centroid; - geometry_msgs::Point middle; - std::vector points; +// geometry_msgs::Point closest_point; // for now we ommit it as recomputing is qwestionable + std::pair interpolated_line; +// std::vector points; + std::vector vectors_to_points; + geometry_msgs::Point reference_robot_pose; + geometry_msgs::Point toReferenceFrame(const geometry_msgs::Point &pt); + geometry_msgs::Point fromReferenceFrame(const geometry_msgs::Point &pt_in_reference_frame); }; /** @@ -26,16 +33,15 @@ struct Frontier { class FrontierSearch { public: - FrontierSearch() - { - } + FrontierSearch() = default; /** * @brief Constructor for search task * @param costmap Reference to costmap data to search. */ FrontierSearch(costmap_2d::Costmap2D* costmap, double potential_scale, - double gain_scale, double min_frontier_size); + double gain_scale, double min_frontier_size, + int use_every_k_point,double max_frontier_angular_size); /** * @brief Runs search implementation, outward from the start position @@ -45,6 +51,12 @@ class FrontierSearch std::vector searchFrom(geometry_msgs::Point position); protected: + std::pair approxFrontierByPlanarFarthest(Frontier &fr, + geometry_msgs::Point &reference_robot); + std::pair approximateFrontierByViewAngle(Frontier &fr); + std::vector splitFrontier(Frontier& fr); + + /** * @brief Starting from an initial cell, build a frontier from valid adjacent * cells @@ -54,8 +66,8 @@ class FrontierSearch * as frontiers * @return new frontier */ - Frontier buildNewFrontier(unsigned int initial_cell, unsigned int reference, - std::vector& frontier_flag); + std::vector buildNewFrontier(unsigned int initial_cell, unsigned int reference, + std::vector &frontier_flag); /** * @brief isNewFrontierCell Evaluate if candidate cell is a valid candidate @@ -83,6 +95,8 @@ class FrontierSearch unsigned int size_x_, size_y_; double potential_scale_, gain_scale_; double min_frontier_size_; + int use_every_k_point_{1}; + double max_frontier_angular_size_{10.0}; // TODO make a parameter on parameter server }; } #endif diff --git a/explore_lite_revised/launch/explore.launch b/explore_lite_revised/launch/explore.launch index cc79723..8498f13 100644 --- a/explore_lite_revised/launch/explore.launch +++ b/explore_lite_revised/launch/explore.launch @@ -1,16 +1,5 @@ - - - - - - - - - - - - + \ No newline at end of file diff --git a/explore_lite_revised/launch/explore_costmap.launch b/explore_lite_revised/launch/explore_costmap.launch index 3ff1fb0..2b0acae 100644 --- a/explore_lite_revised/launch/explore_costmap.launch +++ b/explore_lite_revised/launch/explore_costmap.launch @@ -1,15 +1,5 @@ - - - - - - - - - - - + diff --git a/explore_lite_revised/src/explore.cpp b/explore_lite_revised/src/explore.cpp index 6be6cb7..9e2bfe7 100644 --- a/explore_lite_revised/src/explore.cpp +++ b/explore_lite_revised/src/explore.cpp @@ -50,6 +50,7 @@ inline static bool operator==(const geometry_msgs::Point& one, namespace explore { + Explore::Explore() : private_nh_("~") , tf_listener_(ros::Duration(10.0)) @@ -59,7 +60,8 @@ Explore::Explore() , last_markers_count_(0) { double timeout; - double min_frontier_size; + int use_each_k_point; + double min_frontier_size, max_frontier_angular_size; // parameters for FrontierSearch class private_nh_.param("planner_frequency", planner_frequency_, 1.0); private_nh_.param("progress_timeout", timeout, 30.0); progress_timeout_ = ros::Duration(timeout); @@ -68,10 +70,12 @@ Explore::Explore() private_nh_.param("orientation_scale", orientation_scale_, 0.0); private_nh_.param("gain_scale", gain_scale_, 1.0); private_nh_.param("min_frontier_size", min_frontier_size, 0.5); + private_nh_.param("use_each_k_point", use_each_k_point, 1); + private_nh_.param("max_frontier_angular_size", max_frontier_angular_size, 10.0); search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(), potential_scale_, gain_scale_, - min_frontier_size); + min_frontier_size, use_each_k_point, max_frontier_angular_size); if (visualize_) { marker_array_publisher_ = @@ -92,86 +96,125 @@ Explore::~Explore() stop(); } +std_msgs::ColorRGBA *blue; +std_msgs::ColorRGBA *red; +std_msgs::ColorRGBA *green; + + visualization_msgs::Marker *default_msg_template; + +// todo consicer cleanup +void set_default_frontier_marker(visualization_msgs::Marker& m){ + m.header.stamp = ros::Time::now(); + m.ns = "frontiers"; +} + + void Explore::visualizeFrontiers( const std::vector& frontiers) { - std_msgs::ColorRGBA blue; - blue.r = 0; - blue.g = 0; - blue.b = 1.0; - blue.a = 1.0; - std_msgs::ColorRGBA red; - red.r = 1.0; - red.g = 0; - red.b = 0; - red.a = 1.0; - std_msgs::ColorRGBA green; - green.r = 0; - green.g = 1.0; - green.b = 0; - green.a = 1.0; ROS_DEBUG("visualising %lu frontiers", frontiers.size()); visualization_msgs::MarkerArray markers_msg; std::vector& markers = markers_msg.markers; visualization_msgs::Marker m; - + m = *default_msg_template; m.header.frame_id = costmap_client_.getGlobalFrameID(); - m.header.stamp = ros::Time::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.frame_locked = true; // weighted frontiers are always sorted double min_cost = frontiers.empty() ? 0. : frontiers.front().cost; + + m.id = 0; + + m.action = visualization_msgs::Marker::DELETEALL; + markers.push_back(m); + ++m.id; m.action = visualization_msgs::Marker::ADD; - size_t id = 0; + +// TODO when the work on visualisation will stabilize, make a separate functuion or even subpackage for it for (auto& frontier : frontiers) { - m.type = visualization_msgs::Marker::POINTS; - m.id = int(id); + m.header.frame_id = costmap_client_.getGlobalFrameID(); + + m.type = visualization_msgs::Marker::SPHERE_LIST; + ++m.id; + m.pose.position = {}; + m.scale.x = 0.2; + m.scale.y = 0.2; + m.scale.z = 0.2; + m.points = {frontier.interpolated_line.first, frontier.centroid, frontier.interpolated_line.second}; + m.color = *red; + m.header.stamp = ros::Time::now(); + markers.push_back(m); + + + + m.type = visualization_msgs::Marker::LINE_STRIP; + ++m.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; - } + m.points = {frontier.interpolated_line.first, frontier.centroid, frontier.interpolated_line.second}; + m.color = *red; + m.header.stamp = ros::Time::now(); markers.push_back(m); - ++id; - m.type = visualization_msgs::Marker::ARROW; - 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.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + ++m.id; + m.text = "F"; + m.pose.position = frontier.interpolated_line.first; + m.scale.x = 0.2; + m.scale.y = 0.2; + m.scale.z = 0.2; m.points = {}; - m.color = green; + m.color = *blue; + m.header.stamp = ros::Time::now(); + markers.push_back(m); + + m.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + ++m.id; + m.text = "M"; + m.pose.position = frontier.centroid; + m.scale.x = 0.2; + m.scale.y = 0.2; + m.scale.z = 0.2; + m.points = {}; + m.color = *blue; + m.header.stamp = ros::Time::now(); markers.push_back(m); - ++id; - } - size_t current_markers_count = markers.size(); - // delete previous markers, which are now unused - m.action = visualization_msgs::Marker::DELETE; - for (; id < last_markers_count_; ++id) { - m.id = int(id); + m.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + ++m.id; + m.text = "L"; + m.pose.position = frontier.interpolated_line.second; + m.scale.x = 0.2; + m.scale.y = 0.2; + m.scale.z = 0.2; + m.points = {}; + m.color = *blue; + m.header.stamp = ros::Time::now(); + markers.push_back(m); + + m.type = visualization_msgs::Marker::POINTS; + ++m.id; + m.pose.position = {}; + m.scale.x = 0.1; + m.scale.y = 0.1; + m.scale.z = 0.1; + std::vector translated_vector{ frontier.vectors_to_points.begin(), frontier.vectors_to_points.end()}; + for (auto &i : translated_vector) { + i.x += frontier.reference_robot_pose.x; + i.y += frontier.reference_robot_pose.y; + } + m.points = translated_vector; + m.color = goalOnBlacklist(frontier.centroid) ? *red : *blue; + m.header.stamp = ros::Time::now(); markers.push_back(m); + } + size_t current_markers_count = markers.size(); + last_markers_count_ = current_markers_count; marker_array_publisher_.publish(markers_msg); } @@ -188,6 +231,11 @@ void Explore::makePlan() } if (frontiers.empty()) { + visualization_msgs::MarkerArray empty_marker_msg; +// empty_marker_msg.markers + marker_array_publisher_.publish(empty_marker_msg); + visualizeFrontiers(frontiers); + ROS_WARN_STREAM("NO FRONTIERS LEFT, PUBLISHING EMPTY LIST"); stop(); return; } @@ -287,16 +335,49 @@ void Explore::start() void Explore::stop() { +// marker_array_publisher_.publish(); move_base_client_.cancelAllGoals(); exploring_timer_.stop(); ROS_INFO("Exploration stopped."); } +void initialize_variables(){ +// TODO rewrite in better style with alocator + red = new std_msgs::ColorRGBA(); + blue = new std_msgs::ColorRGBA(); + green = new std_msgs::ColorRGBA(); + + blue->r = 0; + blue->g = 0; + blue->b = 1.0; + blue->a = 1.0; + + red->r = 1.0; + red->g = 0; + red->b = 0; + red->a = 1.0; + + green->r = 0; + green->g = 1.0; + green->b = 0; + green->a = 1.0; + + default_msg_template = new visualization_msgs::Marker(); + default_msg_template->ns = "frontiers"; + // lives forever + default_msg_template->lifetime = ros::Duration(0); + default_msg_template->frame_locked = false; +}; + } // namespace explore + + + int main(int argc, char** argv) { ros::init(argc, argv, "explore"); + explore::initialize_variables(); if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { ros::console::notifyLoggerLevelsChanged(); diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index c873a9c..b59c2c8 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -8,24 +8,142 @@ #include +#include +#include +#include + namespace frontier_exploration { using costmap_2d::LETHAL_OBSTACLE; using costmap_2d::NO_INFORMATION; using costmap_2d::FREE_SPACE; + +inline double angular_vector_distance(const double x1,const double y1,const double x2,const double y2){ + return std::abs( atan2(y1, x1) - atan2(y2, x2) ) * 180.0 / M_PI; +} +inline double angular_vector_distance(const geometry_msgs::Point &v1,const geometry_msgs::Point &v2) { + return angular_vector_distance(v1.x, v1.y, v2.x, v2.y); +} +// for the case when vectors are presented by points with single point-of-origin +inline double angular_vector_distance(const geometry_msgs::Point &p1,const geometry_msgs::Point &p2,const geometry_msgs::Point &reference_pt){ + return angular_vector_distance(p1.x - reference_pt.x, p1.y - reference_pt.y, p2.x - reference_pt.x, p2.y - reference_pt.y); +} + +// for now is not used +inline geometry_msgs::Point getClosestPointTo(const std::vector &distance_vectors, const geometry_msgs::Point &goal){ + auto min_elem_iter = std::min_element(distance_vectors.begin(), distance_vectors.end(), + [](const geometry_msgs::Point &p1,const geometry_msgs::Point &p2) + {return p1.x + p1.y < p2.x + p2.y;}); //using Manhatten distance + return *min_elem_iter; +} + +geometry_msgs::Point Frontier::toReferenceFrame(const geometry_msgs::Point &pt_in_absolute_frame){ + geometry_msgs::Point pt_in_reference_frame; + pt_in_reference_frame.x = pt_in_absolute_frame.x - reference_robot_pose.x; + pt_in_reference_frame.y = pt_in_absolute_frame.y - reference_robot_pose.y; + return pt_in_reference_frame; +} +geometry_msgs::Point Frontier::fromReferenceFrame(const geometry_msgs::Point &pt_in_reference_frame){ + geometry_msgs::Point pt_in_absolute_frame; + pt_in_absolute_frame.x = pt_in_reference_frame.x + reference_robot_pose.x; + pt_in_absolute_frame.y = pt_in_reference_frame.y + reference_robot_pose.y; + return pt_in_absolute_frame; +} + FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, double potential_scale, double gain_scale, - double min_frontier_size) + double min_frontier_size, + int use_every_k_point, + double max_frontier_angular_size) : costmap_(costmap) , potential_scale_(potential_scale) , gain_scale_(gain_scale) , min_frontier_size_(min_frontier_size) + , use_every_k_point_(use_every_k_point) + , max_frontier_angular_size_(max_frontier_angular_size) { } + std::pair FrontierSearch::approxFrontierByPlanarFarthest(Frontier &fr, + geometry_msgs::Point &reference_robot){ + double max_dist = 0.0; + double heuristics_dist {0.0}; + geometry_msgs::Point p1, p2; + for (auto &point: fr.vectors_to_points){ + heuristics_dist = + std::hypot(point.x , point.y ) + /* robot_point_dist */ + std::hypot(point.x , point.y ); /* centroid_point dist */ + if (heuristics_dist > max_dist){ + max_dist = heuristics_dist; + p1 = point; + } + } + max_dist = 0.0; + for (auto &point: fr.vectors_to_points){ + heuristics_dist = + std::hypot(point.x - p1.x , point.y - p1.y) + /* point-point dist */ + std::hypot( (/*transforming to map frame*/point.x + reference_robot.x) - fr.centroid.x , (point.y + reference_robot.y) - fr.centroid.y); /* centroid_point dist */ + if (heuristics_dist > max_dist){ + max_dist = heuristics_dist; + p2 = point; + } + } + return {p1, p2}; +} + + + std::pair FrontierSearch::approximateFrontierByViewAngle( + frontier_exploration::Frontier &fr) { + geometry_msgs::Point p1, p2; + if (!fr.vectors_to_points.empty()){ + p1 = fr.fromReferenceFrame(fr.vectors_to_points.front()); + p2 = fr.fromReferenceFrame(fr.vectors_to_points.back()); + ROS_DEBUG_STREAM( " approximated borders with view angle " << fr.vectors_to_points.front() << fr.vectors_to_points.back()); + } // else default zero values + return {p1, p2}; + } + + std::vector FrontierSearch::splitFrontier(Frontier& fr){ + Frontier fr1, fr2; + + std::vector frontiers{fr1, fr2}, vector_buf, res; + size_t splits = frontiers.size(); + size_t current_element {0}, split_size {fr.vectors_to_points.size() / splits}; + for (auto& this_fr: frontiers){ + this_fr.reference_robot_pose = fr.reference_robot_pose; + this_fr.vectors_to_points = std::vector(fr.vectors_to_points.begin() + current_element, fr.vectors_to_points.begin() + current_element + split_size); + current_element += split_size; + this_fr.interpolated_line = {this_fr.vectors_to_points.front(), this_fr.vectors_to_points.back()}; + this_fr.centroid = *(this_fr.vectors_to_points.begin() + split_size / 2); // not true centroid, but who cares... //todo maybe + + this_fr.centroid = this_fr.fromReferenceFrame(this_fr.centroid); + this_fr.interpolated_line.first = this_fr.fromReferenceFrame(this_fr.interpolated_line.first); + this_fr.interpolated_line.second = this_fr.fromReferenceFrame(this_fr.interpolated_line.second); + + if (fr.vectors_to_points.end() - fr.vectors_to_points.begin() - current_element < split_size) { + this_fr.vectors_to_points.insert(this_fr.vectors_to_points.end(), + fr.vectors_to_points.begin() + current_element, + fr.vectors_to_points.end()); // uppend last chunk in case of division leftover + } + + double fr_angular_dist = angular_vector_distance(this_fr.interpolated_line.first, this_fr.interpolated_line.second); + ROS_DEBUG_STREAM("FR has [" << fr_angular_dist << "] deg [" << frontiers[0].vectors_to_points.size() << "] PTS"); + if (fr_angular_dist > this->max_frontier_angular_size_ + && fr1.vectors_to_points.size() > 6) // to allways have two subdrontiers with middlepoints + { + ROS_DEBUG_STREAM("PERFORM FRONTIER RECURSIVE SPLITTING"); + vector_buf = splitFrontier(this_fr); + } else{ + vector_buf = {this_fr}; + } + res.insert(res.end(), vector_buf.begin(), vector_buf.end()); + } + return res; + } + std::vector FrontierSearch::searchFrom(geometry_msgs::Point position) -{ + { std::vector frontier_list; // Sanity check that robot is inside costmap bounds before searching @@ -74,10 +192,11 @@ std::vector FrontierSearch::searchFrom(geometry_msgs::Point position) // neighbour) } else if (isNewFrontierCell(nbr, frontier_flag)) { frontier_flag[nbr] = true; - Frontier new_frontier = buildNewFrontier(nbr, pos, frontier_flag); - if (new_frontier.size * costmap_->getResolution() >= - min_frontier_size_) { - frontier_list.push_back(new_frontier); + std::vector new_frontiers = buildNewFrontier(nbr, pos, frontier_flag); + for (auto &new_frontier: new_frontiers){ + if (new_frontier.vectors_to_points.size() * costmap_->getResolution() >= min_frontier_size_) { + frontier_list.push_back(new_frontier); + } } } } @@ -90,20 +209,22 @@ std::vector FrontierSearch::searchFrom(geometry_msgs::Point position) std::sort( frontier_list.begin(), frontier_list.end(), [](const Frontier& f1, const Frontier& f2) { return f1.cost < f2.cost; }); + // TODO do we need sort if only min is interesting? maybe once we have cache behaviour prtial sort will be ok... return frontier_list; } -Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, - unsigned int reference, - std::vector& frontier_flag) + + + +std::vector FrontierSearch::buildNewFrontier(unsigned int initial_cell, + unsigned int reference, + std::vector &frontier_flag) { // initialize frontier structure Frontier output; output.centroid.x = 0; output.centroid.y = 0; - output.size = 1; - output.min_distance = std::numeric_limits::infinity(); // record initial contact point for frontier unsigned int ix, iy; @@ -119,11 +240,14 @@ Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, double reference_x, reference_y; costmap_->indexToCells(reference, rx, ry); costmap_->mapToWorld(rx, ry, reference_x, reference_y); + output.reference_robot_pose.x = reference_x; + output.reference_robot_pose.y = reference_y; + +size_t cntr{0}; while (!bfs.empty()) { unsigned int idx = bfs.front(); bfs.pop(); - // try adding cells in 8-connected neighborhood to frontier for (unsigned int nbr : nhood8(idx, *costmap_)) { // check if neighbour is a potential frontier cell @@ -135,26 +259,14 @@ Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, costmap_->indexToCells(nbr, mx, my); costmap_->mapToWorld(mx, my, wx, wy); - geometry_msgs::Point point; - point.x = wx; - point.y = wy; - output.points.push_back(point); - - // update frontier size - output.size++; - - // update centroid of frontier - output.centroid.x += wx; - output.centroid.y += wy; - - // determine frontier's distance from robot, going by closest gridcell - // to robot - double distance = sqrt(pow((double(reference_x) - double(wx)), 2.0) + - pow((double(reference_y) - double(wy)), 2.0)); - if (distance < output.min_distance) { - output.min_distance = distance; - output.middle.x = wx; - output.middle.y = wy; + // leaving only each n-th point + if (cntr++ % this->use_every_k_point_ == 0){ + geometry_msgs::Point reference_scope_point; + reference_scope_point.x = wx - reference_x; + reference_scope_point.y = wy - reference_y; + output.vectors_to_points.push_back(reference_scope_point); + output.centroid.x += wx; // map frame coords + output.centroid.y += wy; } // add to queue for breadth first search @@ -162,12 +274,31 @@ Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, } } } - // average out frontier centroid - output.centroid.x /= output.size; - output.centroid.y /= output.size; - return output; + output.centroid.x /= output.vectors_to_points.size(); + output.centroid.y /= output.vectors_to_points.size(); + /*KD*/ + + std::sort(output.vectors_to_points.begin(), output.vectors_to_points.end(), + [](const geometry_msgs::Point &p1,const geometry_msgs::Point &p2) + {return atan2(p1.y, p1.x) < atan2(p2.y, p2.x);} + ); + + // fixme find out why there are occuring empty frontiers (empty raw points) + output.interpolated_line = approximateFrontierByViewAngle(output); + std::vector splitted_frontiers{output}; +if (!output.vectors_to_points.empty()){ + + double degrees_distance = angular_vector_distance(output.interpolated_line.first, output.interpolated_line.second, output.reference_robot_pose); + if (degrees_distance > this->max_frontier_angular_size_){ + ROS_INFO_STREAM("Detected wide frontier [" << degrees_distance <<"]" << "PTS [" <& frontier_flag) @@ -190,8 +321,15 @@ bool FrontierSearch::isNewFrontierCell(unsigned int idx, double FrontierSearch::frontierCost(const Frontier& frontier) { - return (potential_scale_ * frontier.min_distance * - costmap_->getResolution()) - - (gain_scale_ * frontier.size * costmap_->getResolution()); + return (potential_scale_ * frontier.min_distance + // * costmap_->getResolution() + ) + - + (gain_scale_ + /*KD*/ * frontier.vectors_to_points.size() + // * frontier.size + // * costmap_->getResolution() + ); } + } diff --git a/explore_stack/launch/combined_bringup.launch b/explore_stack/launch/combined_bringup.launch index 62545b0..f766327 100644 --- a/explore_stack/launch/combined_bringup.launch +++ b/explore_stack/launch/combined_bringup.launch @@ -16,8 +16,8 @@ - - + + diff --git a/explore_stack/param/explore_lite_revised.yaml b/explore_stack/param/explore_lite_revised.yaml index 34f83cc..6f27c31 100644 --- a/explore_stack/param/explore_lite_revised.yaml +++ b/explore_stack/param/explore_lite_revised.yaml @@ -1,5 +1,5 @@ robot_base_frame: base_link -min_frontier_size: 0.75 +min_frontier_size: 0.4 costmap_topic: map costmap_updates_topic: map_updates visualize: true @@ -9,5 +9,7 @@ potential_scale: 3.0 orientation_scale: 0.0 gain_scale": 1.0 transform_tolerance: 0.3 +use_each_k_point: 1 # other k-1 point are dropped +max_frontier_angular_size: 10.0 # TODO make finalizing publication when task is done \ No newline at end of file diff --git a/velocity_topic_watchdog/src/velocity_topic_watchdog.cpp b/velocity_topic_watchdog/src/velocity_topic_watchdog.cpp index 30e3914..cdfc8f4 100644 --- a/velocity_topic_watchdog/src/velocity_topic_watchdog.cpp +++ b/velocity_topic_watchdog/src/velocity_topic_watchdog.cpp @@ -16,9 +16,9 @@ ros::Duration *watchdog_duration; double watchdog_timeout_param (3.0); void watchdogTimerCallback(const ros::TimerEvent &timerEvent){ - ROS_WARN_STREAM("velocity_topic_watchdog activated!"); + ROS_INFO_STREAM("velocity_topic_watchdog activated!"); pub_ptr->publish(*stop_msg_ptr); - ROS_WARN_STREAM("STOP MSG PUBLISHED to [" << pub_ptr->getTopic() << "]"); + ROS_INFO_STREAM("STOP MSG PUBLISHED to [" << pub_ptr->getTopic() << "]"); global_timer->stop(); }