Skip to content

Add function to calculate the orientation at which the footprint sweeps the smallest area #45

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
corot wants to merge 2 commits into from
Closed

Add function to calculate the orientation at which the footprint sweeps the smallest area #45

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3fa85bc
Add function to calculate the orientation at which the footprint swee…
Jul 29, 2022
2fb17ce
Address PR reviews comments
Aug 1, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions costmap_2d/include/costmap_2d/costmap_math.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,4 +66,6 @@ bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float t

bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2);

double orientation(double x0, double y0, double x1, double y1);

#endif // COSTMAP_2D_COSTMAP_MATH_H_
10 changes: 10 additions & 0 deletions costmap_2d/include/costmap_2d/footprint.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,16 @@ namespace costmap_2d
void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint,
double& min_dist, double& max_dist);

/**
* @brief Calculate the orientation at which the footprint will sweep the smallest area when moving along +x direction
* @warning This function only works under two assumptions:
* * the footprint is symmetric wrt the x axis
* * the closest edge is approximately parallel to either x or y axis
* @param footprint The footprint to examine
* @return Minimum footprint sweeping area orientation
*/
double minSweepingAreaOrientation(const std::vector<geometry_msgs::Point>& footprint);

/**
* @brief Convert Point32 to Point
*/
Expand Down
7 changes: 7 additions & 0 deletions costmap_2d/src/costmap_math.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,3 +87,10 @@ bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometr
{
return intersects_helper(polygon1, polygon2) || intersects_helper(polygon2, polygon1);
}

double orientation(double x0, double y0, double x1, double y1)
{
const double dx = x1 - x0;
const double dy = y1 - y0;
return atan2(dy, dx);
}
33 changes: 33 additions & 0 deletions costmap_2d/src/footprint.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,39 @@ void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footpr
max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
}

double minSweepingAreaOrientation(const std::vector<geometry_msgs::Point>& footprint)
{
double min_dist = std::numeric_limits<double>::max();
std::array<geometry_msgs::Point, 2> closest_edge;

if (footprint.size() <= 2)
{
return NAN;
}

// check the distance from the robot center point to each footprint edged and keep the closest one
for (unsigned int i = 0; i < footprint.size() - 1; ++i)
{
double edge_dist = distanceToLine(0, 0, footprint[i].x, footprint[i].y, footprint[i + 1].x, footprint[i + 1].y);
if (edge_dist < min_dist)

@renan028 renan028 Aug 1, 2022

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Q: I don't understand how distance to the center will give you the smallest sweeping area.
A rectangle like the one below will give you 90 degrees as output (because the left side is the closest to the center). But 90 degrees would sweep the largest area
rectangle

@corot corot Aug 1, 2022

Copy link
Copy Markdown
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

if +x is the horizontal, rotating the rectangle 90 deg will minimize the sweeping area; so 90 deg is the right answer

@renan028 renan028 Aug 1, 2022

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

but for a "kite" shape it doesn't work, because it should be 90 deg too, and it is not:
image

@corot corot Aug 1, 2022

Copy link
Copy Markdown
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

the answer won't be 90, true
but it will be correct anyway: will sweep the same area as with 90

in any case that fp violates the 2nd assumption I added to the function (true, the assumptions are a bit ad-hoc,,, but are true for every polygonal robot I know

@renan028 renan028 Aug 2, 2022

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

hum, true. But then the function should check the violation. You don't need to loop all edges if you assume the closest one is parallel to the x-axis

{
min_dist = edge_dist;
closest_edge = { footprint[i], footprint[i + 1] };
}
}

// we also need to do the last vertex and the first vertex
if (distanceToLine(0, 0, footprint.back().x, footprint.back().y, footprint.front().x, footprint.front().y) < min_dist)
{
closest_edge = { footprint.back(), footprint.front() };
}

// return the orientation of the closest edge, directed from back to front (+x axis direction)
std::sort(closest_edge.begin(), closest_edge.end(),
[](const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) { return p1.x < p2.x; });
return orientation(closest_edge.front().x, closest_edge.front().y, closest_edge.back().x, closest_edge.back().y);
}
Comment on lines +96 to +100

@ayushgaud ayushgaud Aug 1, 2022

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This logic won't work. You are trying to compute the direction in which the sweep is minimum and not the edge which minimizes the sweep (the closest edge may not be the one either as @renan028 mentioned).

You will need to implement something like this https://en.wikipedia.org/wiki/Rotating_calipers and figure out the normal to the min width of the polygon.

@corot corot Aug 1, 2022 •
edited
Loading

Copy link
Copy Markdown
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

oh,,, yes, yes;
I forgot to add the assumptions I'm doing here:

  • the footprint is symmetric wrt the x axis
  • the closest edge is approximately parallel to either x or y axis

Then the logic makes sense

@corot corot Aug 1, 2022

Copy link
Copy Markdown
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

added


geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt)
{
geometry_msgs::Point32 point32;
Expand Down