Skip to content

Commit

Permalink
refactoring
Browse files Browse the repository at this point in the history 
parameters moved to the parameter server
prepeared for merging
  • Loading branch information
@KushnirDmytro
KushnirDmytro committed Mar 18, 2019
1 parent 8fab7ce commit 25e9db3
Show file tree
Hide file tree
Showing 4 changed files with 25 additions and 85 deletions.
8 changes: 5 additions & 3 deletions explore_lite_revised/include/explore/frontier_search.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,6 @@ struct Frontier {
// std::vector<geometry_msgs::Point> points;
std::vector<geometry_msgs::Point> vectors_to_points;
geometry_msgs::Point reference_robot_pose;
double max_frontier_angular_size{10.0}; // TODO make a parameter on parameter server
geometry_msgs::Point toReferenceFrame(const geometry_msgs::Point &pt);
geometry_msgs::Point fromReferenceFrame(const geometry_msgs::Point &pt_in_reference_frame);
};
Expand All @@ -41,7 +40,8 @@ class FrontierSearch
* @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
Expand All @@ -54,7 +54,7 @@ class FrontierSearch
std::pair<geometry_msgs::Point, geometry_msgs::Point> approxFrontierByPlanarFarthest(Frontier &fr,
geometry_msgs::Point &reference_robot);
std::pair<geometry_msgs::Point, geometry_msgs::Point> approximateFrontierByViewAngle(Frontier &fr);

std::vector<Frontier> splitFrontier(Frontier& fr);


/**
Expand Down Expand Up @@ -95,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
53 changes: 7 additions & 46 deletions explore_lite_revised/src/explore.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,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);
Expand All @@ -69,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_ =
Expand Down Expand Up @@ -127,9 +130,7 @@ void Explore::visualizeFrontiers(
markers.push_back(m);
++m.id;
m.action = visualization_msgs::Marker::ADD;
// for (; m.id < last_markers_count_; ++m.id) {
// markers.push_back(m);
// }

// TODO when the work on visualisation will stabilize, make a separate functuion or even subpackage for it
for (auto& frontier : frontiers) {
m.header.frame_id = costmap_client_.getGlobalFrameID();
Expand Down Expand Up @@ -200,13 +201,7 @@ void Explore::visualizeFrontiers(
m.scale.x = 0.1;
m.scale.y = 0.1;
m.scale.z = 0.1;
// ROS_ERROR_STREAM(frontier.points_in_reference_coords.size());
// ROS_ERROR_STREAM("ref: " << frontier.points[0] << frontier.points[1] << frontier.points[2] );
// ROS_ERROR_STREAM("map_points: " << frontier.vectors_to_points[0] << frontier.vectors_to_points[1] << frontier.vectors_to_points[2] );
// ROS_ERROR_STREAM(frontier.points_in_reference_coords.size());
// ROS_ERROR_STREAM(frontier.points.size());
std::vector<geometry_msgs::Point> translated_vector;
translated_vector.insert(translated_vector.begin(), frontier.vectors_to_points.begin(), frontier.vectors_to_points.end());
std::vector<geometry_msgs::Point> 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;
Expand All @@ -216,37 +211,10 @@ void Explore::visualizeFrontiers(
m.header.stamp = ros::Time::now();
markers.push_back(m);


// for (size_t p_ind = 0; p_ind < frontier.vectors_to_points.size(); ++p_ind){
// m.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
//// m.header.frame_id = costmap_client_.getBaseFrameID();
// ++m.id;
// m.pose.position = frontier.vectors_to_points[p_ind];
// m.text = std::to_string(p_ind);
// m.scale.x = 0.3;
// m.scale.y = 0.3;
// m.scale.z = 0.3;
//// ROS_ERROR_STREAM(frontier.points_in_reference_coords.size());
// ROS_ERROR_STREAM("ref: " << frontier.points[0] << frontier.points[1] << frontier.points[2] );
// ROS_ERROR_STREAM("map_points: " << frontier.vectors_to_points[0] << frontier.vectors_to_points[1] << frontier.vectors_to_points[2] );
//// ROS_ERROR_STREAM(frontier.points_in_reference_coords.size());
//// ROS_ERROR_STREAM(frontier.points.size());
// m.points = frontier.vectors_to_points;
// m.color = *red;
// m.header.stamp = ros::Time::now();
// markers.push_back(m);
// }

}

size_t current_markers_count = markers.size();

// delete previous markers, which are now unused
// m.action = visualization_msgs::Marker::DELETE;
// for (; m.id < last_markers_count_; ++m.id) {
// markers.push_back(m);
// }

last_markers_count_ = current_markers_count;
marker_array_publisher_.publish(markers_msg);
}
Expand Down Expand Up @@ -396,13 +364,6 @@ void initialize_variables(){

default_msg_template = new visualization_msgs::Marker();
default_msg_template->ns = "frontiers";
// default_msg_template->scale.x = 1.0;
// default_msg_template->scale.y = 1.0;
// default_msg_template->scale.z = 1.0;
// default_msg_template->color.r = 0;
// default_msg_template->color.g = 0;
// default_msg_template->color.b = 255;
// default_msg_template->color.a = 255;
// lives forever
default_msg_template->lifetime = ros::Duration(0);
default_msg_template->frame_locked = false;
Expand Down
47 changes: 11 additions & 36 deletions explore_lite_revised/src/frontier_search.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,11 +53,15 @@ geometry_msgs::Point Frontier::fromReferenceFrame(const geometry_msgs::Point &p

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<geometry_msgs::Point, geometry_msgs::Point> FrontierSearch::approxFrontierByPlanarFarthest(Frontier &fr,
Expand Down Expand Up @@ -99,7 +103,7 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap,
return {p1, p2};
}

std::vector<Frontier> splitFrontier(Frontier& fr){
std::vector<Frontier> FrontierSearch::splitFrontier(Frontier& fr){
Frontier fr1, fr2;

std::vector<Frontier> frontiers{fr1, fr2}, vector_buf, res;
Expand All @@ -124,7 +128,7 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap,

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_fr.max_frontier_angular_size
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");
Expand Down Expand Up @@ -190,8 +194,6 @@ std::vector<Frontier> FrontierSearch::searchFrom(geometry_msgs::Point position)
frontier_flag[nbr] = true;
std::vector<Frontier> new_frontiers = buildNewFrontier(nbr, pos, frontier_flag);
for (auto &new_frontier: new_frontiers){
/*KD : TODO after moving sparsicng factor to the parameter server rescale those allso*/
ROS_WARN_STREAM("MIN_FRONTIER" << min_frontier_size_);
if (new_frontier.vectors_to_points.size() * costmap_->getResolution() >= min_frontier_size_) {
frontier_list.push_back(new_frontier);
}
Expand Down Expand Up @@ -241,8 +243,7 @@ std::vector<Frontier> FrontierSearch::buildNewFrontier(unsigned int initial_cell
output.reference_robot_pose.x = reference_x;
output.reference_robot_pose.y = reference_y;

// TODO place it on a parameter server
size_t choose_each{1}, cntr{0};
size_t cntr{0};

while (!bfs.empty()) {
unsigned int idx = bfs.front();
Expand All @@ -259,7 +260,7 @@ size_t choose_each{1}, cntr{0};
costmap_->mapToWorld(mx, my, wx, wy);

// leaving only each n-th point
if (cntr++ % choose_each == 0){
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;
Expand All @@ -268,38 +269,12 @@ size_t choose_each{1}, cntr{0};
output.centroid.y += wy;
}


// geometry_msgs::Point point;
// point.x = wx;
// point.y = wy;
// output.points.push_back(point);

// update frontier size
// output.size++; // FIXME R U SRIOS!?

// 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 = std::hypot(reference_x - wx, reference_y - wy);
// if (distance < output.min_distance) {
// output.min_distance = distance;
// output.closest_point.x = wx;
// output.closest_point.y = wy;
// }

// add to queue for breadth first search
bfs.push(nbr);
}
}
}

// output.min_distance = std::hypot(reference_x - output.closest_point.x, reference_y - output.closest_point.y);
// average out frontier centroid


output.centroid.x /= output.vectors_to_points.size();
output.centroid.y /= output.vectors_to_points.size();
/*KD*/
Expand All @@ -315,8 +290,8 @@ size_t choose_each{1}, cntr{0};
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 > output.max_frontier_angular_size){
ROS_WARN_STREAM("Detected too wide frontier [" << degrees_distance <<"]" << "PTS [" <<output.vectors_to_points.size()<< "] splitting on two");
if (degrees_distance > this->max_frontier_angular_size_){
ROS_INFO_STREAM("Detected wide frontier [" << degrees_distance <<"]" << "PTS [" <<output.vectors_to_points.size()<< "] SPLITTING...");
splitted_frontiers = splitFrontier(output);
}
}
Expand Down
2 changes: 2 additions & 0 deletions explore_stack/param/explore_lite_revised.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

0 comments on commit 25e9db3

Please sign in to comment.