Skip to content

Commit

Permalink
use switch case
Browse files Browse the repository at this point in the history
  • Loading branch information
@kaichie
kaichie committed Sep 25, 2023
1 parent 7f9f55a commit 1230ede
Show file tree
Hide file tree
Showing 3 changed files with 100 additions and 85 deletions.
8 changes: 3 additions & 5 deletions nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,11 +128,6 @@ class Polygon
*/
virtual bool isShapeSet();

/**
* @brief Returns true if using velocity based polygon
*/
bool isUsingVelocityPolygonSelector();

/**
* @brief Updates polygon from footprint subscriber (if any)
*/
Expand Down Expand Up @@ -262,6 +257,9 @@ class Polygon

/// @brief Polygon points (vertices) in a base_frame_id_
std::vector<Point> poly_;

/// @brief Source for setting the polygon
PolygonSource polygon_source;
}; // class Polygon

} // namespace nav2_collision_monitor
Expand Down
9 changes: 9 additions & 0 deletions nav2_collision_monitor/include/nav2_collision_monitor/types.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,15 @@ struct Action
std::string polygon_name;
};

/// @brief Source for setting the polygon
enum PolygonSource
{
POLYGON_SOURCE_UNKNOWN = 0, // Default
STATIC_POINTS = 1, // Set from points
DYNAMIC_SUB = 2, // Set from topic subscription
VELOCITY_POLYGON = 3 // Set based on current twists
};

} // namespace nav2_collision_monitor

#endif // NAV2_COLLISION_MONITOR__TYPES_HPP_
168 changes: 88 additions & 80 deletions nav2_collision_monitor/src/polygon.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,8 @@ Polygon::Polygon(
: node_(node), polygon_name_(polygon_name), action_type_(DO_NOTHING),
slowdown_ratio_(0.0), linear_limit_(0.0), angular_limit_(0.0),
footprint_sub_(nullptr), tf_buffer_(tf_buffer),
base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance)
base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance),
polygon_source(POLYGON_SOURCE_UNKNOWN)
{
RCLCPP_INFO(logger_, "[%s]: Creating Polygon", polygon_name_.c_str());
}
Expand Down Expand Up @@ -169,89 +170,95 @@ bool Polygon::isShapeSet()
return true;
}

bool Polygon::isUsingVelocityPolygonSelector()
{
return !velocity_polygons_.empty();
}

void Polygon::updatePolygon(const Velocity & cmd_vel_in)
{
if (isUsingVelocityPolygonSelector()) {
for (auto & velocity_polygon : velocity_polygons_) {

if (velocity_polygon->isInRange(cmd_vel_in)) {
// Set the polygon that is within the speed range
poly_ = velocity_polygon->getPolygon();

// Update visualization polygon
polygon_.polygon.points.clear();
for (const Point & p : poly_) {
geometry_msgs::msg::Point32 p_s;
p_s.x = p.x;
p_s.y = p.y;
// p_s.z will remain 0.0
polygon_.polygon.points.push_back(p_s);
switch (polygon_source) {
case STATIC_POINTS:
{
if (!polygon_.header.frame_id.empty() && polygon_.header.frame_id != base_frame_id_) {
// Polygon is published in another frame: correct poly_ vertices to the latest frame state
std::size_t new_size = polygon_.polygon.points.size();

// Get the transform from PolygonStamped frame to base_frame_id_
tf2::Transform tf_transform;
if (
!nav2_util::getTransform(
polygon_.header.frame_id, base_frame_id_,
transform_tolerance_, tf_buffer_, tf_transform))
{
return;
}

// Correct main poly_ vertices
poly_.resize(new_size);
for (std::size_t i = 0; i < new_size; i++) {
// Transform point coordinates from PolygonStamped frame -> to base frame
tf2::Vector3 p_v3_s(polygon_.polygon.points[i].x, polygon_.polygon.points[i].y, 0.0);
tf2::Vector3 p_v3_b = tf_transform * p_v3_s;

// Fill poly_ array
poly_[i] = {p_v3_b.x(), p_v3_b.y()};
}
}

return;
break;
}
}

// Log for uncovered velocity
auto node = node_.lock();
if (!node) {
throw std::runtime_error{"Failed to lock node"};
}
RCLCPP_WARN_THROTTLE(
logger_,
*node->get_clock(), 2.0, "Velocity is not covered by any of the velocity polygons. x: %.3f y: %.3f tw: %.3f ",
cmd_vel_in.x, cmd_vel_in.y, cmd_vel_in.tw);
return;
}

if (footprint_sub_ != nullptr) {
// Get latest robot footprint from footprint subscriber
std::vector<geometry_msgs::msg::Point> footprint_vec;
std_msgs::msg::Header footprint_header;
footprint_sub_->getFootprintInRobotFrame(footprint_vec, footprint_header);

std::size_t new_size = footprint_vec.size();
poly_.resize(new_size);
polygon_.header.frame_id = base_frame_id_;
polygon_.polygon.points.resize(new_size);

geometry_msgs::msg::Point32 p_s;
for (std::size_t i = 0; i < new_size; i++) {
poly_[i] = {footprint_vec[i].x, footprint_vec[i].y};
p_s.x = footprint_vec[i].x;
p_s.y = footprint_vec[i].y;
polygon_.polygon.points[i] = p_s;
}
} else if (!polygon_.header.frame_id.empty() && polygon_.header.frame_id != base_frame_id_) {
// Polygon is published in another frame: correct poly_ vertices to the latest frame state
std::size_t new_size = polygon_.polygon.points.size();

// Get the transform from PolygonStamped frame to base_frame_id_
tf2::Transform tf_transform;
if (
!nav2_util::getTransform(
polygon_.header.frame_id, base_frame_id_,
transform_tolerance_, tf_buffer_, tf_transform))
{
return;
}

// Correct main poly_ vertices
poly_.resize(new_size);
for (std::size_t i = 0; i < new_size; i++) {
// Transform point coordinates from PolygonStamped frame -> to base frame
tf2::Vector3 p_v3_s(polygon_.polygon.points[i].x, polygon_.polygon.points[i].y, 0.0);
tf2::Vector3 p_v3_b = tf_transform * p_v3_s;
case DYNAMIC_SUB:
{
// Get latest robot footprint from footprint subscriber
std::vector<geometry_msgs::msg::Point> footprint_vec;
std_msgs::msg::Header footprint_header;
footprint_sub_->getFootprintInRobotFrame(footprint_vec, footprint_header);

std::size_t new_size = footprint_vec.size();
poly_.resize(new_size);
polygon_.header.frame_id = base_frame_id_;
polygon_.polygon.points.resize(new_size);

geometry_msgs::msg::Point32 p_s;
for (std::size_t i = 0; i < new_size; i++) {
poly_[i] = {footprint_vec[i].x, footprint_vec[i].y};
p_s.x = footprint_vec[i].x;
p_s.y = footprint_vec[i].y;
polygon_.polygon.points[i] = p_s;
}
break;
}
case VELOCITY_POLYGON:
{
for (auto & velocity_polygon : velocity_polygons_) {
if (velocity_polygon->isInRange(cmd_vel_in)) {
// Set the polygon that is within the speed range
poly_ = velocity_polygon->getPolygon();

// Update visualization polygon
polygon_.polygon.points.clear();
for (const Point & p : poly_) {
geometry_msgs::msg::Point32 p_s;
p_s.x = p.x;
p_s.y = p.y;
// p_s.z will remain 0.0
polygon_.polygon.points.push_back(p_s);
}
return;
}
}

// Fill poly_ array
poly_[i] = {p_v3_b.x(), p_v3_b.y()};
}
// Log for uncovered velocity
auto node = node_.lock();
if (!node) {
throw std::runtime_error{"Failed to lock node"};
}
RCLCPP_WARN_THROTTLE(
logger_,
*node->get_clock(), 2.0, "Velocity is not covered by any of the velocity polygons. x: %.3f y: %.3f tw: %.3f ",
cmd_vel_in.x, cmd_vel_in.y, cmd_vel_in.tw);
break;
}
case POLYGON_SOURCE_UNKNOWN:
default:
break;
}
return;
}

int Polygon::getPointsInside(const std::vector<Point> & points) const
Expand Down Expand Up @@ -438,6 +445,7 @@ bool Polygon::getParameters(
polygon_name_.c_str());
return false;
}
polygon_source = STATIC_POINTS;
return true;
} catch (const rclcpp::exceptions::ParameterUninitializedException &) {
RCLCPP_INFO(
Expand All @@ -462,6 +470,7 @@ bool Polygon::getParameters(
node, polygon_name_ + ".footprint_topic", rclcpp::PARAMETER_STRING);
footprint_topic =
node->get_parameter(polygon_name_ + ".footprint_topic").as_string();
polygon_source = DYNAMIC_SUB;
return true;
}
} catch (const rclcpp::exceptions::ParameterUninitializedException &) {
Expand All @@ -486,9 +495,8 @@ bool Polygon::getParameters(
if (velocity_polygon->getParameters()) {
velocity_polygons_.emplace_back(velocity_polygon);
}

}

polygon_source = VELOCITY_POLYGON;
} catch (const std::exception & ex) {
RCLCPP_ERROR(
logger_,
Expand Down

0 comments on commit 1230ede

Please sign in to comment.