Skip to content

Commit

Permalink
optimization
Browse files Browse the repository at this point in the history 
Signed-off-by: asarazin <anaelle.sarazin@robocc.com>
  • Loading branch information
asarazin committed Jun 11, 2024
1 parent 8264866 commit 1c672fa
Show file tree
Hide file tree
Showing 5 changed files with 135 additions and 102 deletions.
11 changes: 7 additions & 4 deletions nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <string>
#include <vector>
#include <memory>
#include <unordered_map>

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
Expand Down Expand Up @@ -159,28 +160,30 @@ class CollisionMonitor : public nav2_util::LifecycleNode
/**
* @brief Processes the polygon of STOP, SLOWDOWN and LIMIT action type
* @param polygon Polygon to process
* @param collision_points Array of 2D obstacle points
* @param sources_collision_points_map Map containing source name as key and
* array of source's 2D obstacle points as value
* @param velocity Desired robot velocity
* @param robot_action Output processed robot action
* @return True if returned action is caused by current polygon, otherwise false
*/
bool processStopSlowdownLimit(
const std::shared_ptr<Polygon> polygon,
const std::vector<Point> & collision_points,
const std::unordered_map<std::string, std::vector<Point>> & sources_collision_points_map,
const Velocity & velocity,
Action & robot_action) const;

/**
* @brief Processes APPROACH action type
* @param polygon Polygon to process
* @param collision_points Array of 2D obstacle points
* @param sources_collision_points_map Map containing source name as key and
* array of source's 2D obstacle points as value
* @param velocity Desired robot velocity
* @param robot_action Output processed robot action
* @return True if returned action is caused by current polygon, otherwise false
*/
bool processApproach(
const std::shared_ptr<Polygon> polygon,
const std::vector<Point> & collision_points,
const std::unordered_map<std::string, std::vector<Point>> & sources_collision_points_map,
const Velocity & velocity,
Action & robot_action) const;

Expand Down
17 changes: 15 additions & 2 deletions nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <memory>
#include <string>
#include <vector>
#include <unordered_map>

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/polygon_stamped.hpp"
Expand Down Expand Up @@ -151,16 +152,28 @@ class Polygon
*/
virtual int getPointsInside(const std::vector<Point> & points) const;

/**
* @brief Gets number of points inside given polygon
* @param sources_collision_points_map Map containing source name as key,
* and input array of source's points to be checked as value
* @return Number of points inside polygon,
* for sources in map that are associated with current polygon.
* If there are no points, returns zero value.
*/
virtual int getPointsInside(
const std::unordered_map<std::string, std::vector<Point>> & sources_collision_points_map) const;

/**
* @brief Obtains estimated (simulated) time before a collision.
* Applicable for APPROACH model.
* @param collision_points Array of 2D obstacle points
* @param sources_collision_points_map Map containing source name as key,
* and input array of source's 2D obstacle points as value
* @param velocity Simulated robot velocity
* @return Estimated time before a collision. If there is no collision,
* return value will be negative.
*/
double getCollisionTime(
const std::vector<Point> & collision_points,
const std::unordered_map<std::string, std::vector<Point>> & sources_collision_points_map,
const Velocity & velocity) const;

/**
Expand Down
105 changes: 40 additions & 65 deletions nav2_collision_monitor/src/collision_monitor_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -414,24 +414,19 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
// Points array collected from different data sources in a robot base frame
std::unordered_map<std::string, std::vector<Point>> sources_collision_points_map;

// Fill collision_points array from different data sources
for (std::shared_ptr<Source> source : sources_) {
std::vector<Point> collision_points;
source->getData(curr_time, collision_points);
sources_collision_points_map.insert({source->getSourceName(), collision_points});
}

// By default - there is no action
Action robot_action{DO_NOTHING, cmd_vel_in, ""};
// Polygon causing robot action (if any)
std::shared_ptr<Polygon> action_polygon;

// Fill collision_points array from different data sources
// Fill collision points array from different data sources
for (std::shared_ptr<Source> source : sources_) {
std::vector<Point> collision_points;
auto iter = sources_collision_points_map.insert({source->getSourceName(), collision_points});
bool is_source_valid = source->getData(curr_time, iter.first->second);

if (source->getEnabled()) {
if (!source->getData(curr_time, collision_points) &&
source->getSourceTimeout().seconds() != 0.0)
{
if (!is_source_valid && source->getSourceTimeout().seconds() != 0.0) {
action_polygon = nullptr;
robot_action.polygon_name = "invalid source";
robot_action.action_type = STOP;
Expand All @@ -441,34 +436,34 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
break;
}
}
}

if (collision_points_marker_pub_->get_subscription_count() > 0) {
// visualize collision points with markers
auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
visualization_msgs::msg::Marker marker;
marker.header.frame_id = get_parameter("base_frame_id").as_string();
marker.header.stamp = rclcpp::Time(0, 0);
marker.ns = "collision_points";
marker.id = 0;
marker.type = visualization_msgs::msg::Marker::POINTS;
marker.action = visualization_msgs::msg::Marker::ADD;
marker.scale.x = 0.02;
marker.scale.y = 0.02;
marker.color.r = 1.0;
marker.color.a = 1.0;
marker.lifetime = rclcpp::Duration(0, 0);
marker.frame_locked = true;

for (const auto & point : collision_points) {
geometry_msgs::msg::Point p;
p.x = point.x;
p.y = point.y;
p.z = 0.0;
marker.points.push_back(p);
if (collision_points_marker_pub_->get_subscription_count() > 0) {
// visualize collision points with markers
auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
visualization_msgs::msg::Marker marker;
marker.header.frame_id = get_parameter("base_frame_id").as_string();
marker.header.stamp = rclcpp::Time(0, 0);
marker.ns = "collision_points";
marker.id = 0;
marker.type = visualization_msgs::msg::Marker::POINTS;
marker.action = visualization_msgs::msg::Marker::ADD;
marker.scale.x = 0.02;
marker.scale.y = 0.02;
marker.color.r = 1.0;
marker.color.a = 1.0;
marker.lifetime = rclcpp::Duration(0, 0);
marker.frame_locked = true;

for (const auto & point : iter.first->second) {
geometry_msgs::msg::Point p;
p.x = point.x;
p.y = point.y;
p.z = 0.0;
marker.points.push_back(p);
}
marker_array->markers.push_back(marker);
collision_points_marker_pub_->publish(std::move(marker_array));
}
marker_array->markers.push_back(marker);
collision_points_marker_pub_->publish(std::move(marker_array));
}

for (std::shared_ptr<Polygon> polygon : polygons_) {
Expand All @@ -483,37 +478,17 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
// Update polygon coordinates
polygon->updatePolygon(cmd_vel_in);

// Get collision points for sources associated to polygon
std::vector<std::string> sources_names = polygon->getSourcesNames();
std::vector<Point> collision_points;
for (auto source_name : sources_names) {
try {
// Get vector for source
auto source_collision_points = sources_collision_points_map.at(source_name);
// Concatenate vectors
collision_points.insert(
collision_points.end(),
source_collision_points.begin(),
source_collision_points.end());
} catch (std::exception & e) {
RCLCPP_ERROR_STREAM_THROTTLE(
get_logger(),
*get_clock(),
1000,
"Observation source [" << source_name <<
"] configured for polygon [" << polygon->getName() << "] is not defined!");
}
}

const ActionType at = polygon->getActionType();
if (at == STOP || at == SLOWDOWN || at == LIMIT) {
// Process STOP/SLOWDOWN for the selected polygon
if (processStopSlowdownLimit(polygon, collision_points, cmd_vel_in, robot_action)) {
if (processStopSlowdownLimit(
polygon, sources_collision_points_map, cmd_vel_in, robot_action))
{
action_polygon = polygon;
}
} else if (at == APPROACH) {
// Process APPROACH for the selected polygon
if (processApproach(polygon, collision_points, cmd_vel_in, robot_action)) {
if (processApproach(polygon, sources_collision_points_map, cmd_vel_in, robot_action)) {
action_polygon = polygon;
}
}
Expand All @@ -535,15 +510,15 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:

bool CollisionMonitor::processStopSlowdownLimit(
const std::shared_ptr<Polygon> polygon,
const std::vector<Point> & collision_points,
const std::unordered_map<std::string, std::vector<Point>> & sources_collision_points_map,
const Velocity & velocity,
Action & robot_action) const
{
if (!polygon->isShapeSet()) {
return false;
}

if (polygon->getPointsInside(collision_points) >= polygon->getMinPoints()) {
if (polygon->getPointsInside(sources_collision_points_map) >= polygon->getMinPoints()) {
if (polygon->getActionType() == STOP) {
// Setting up zero velocity for STOP model
robot_action.polygon_name = polygon->getName();
Expand Down Expand Up @@ -590,7 +565,7 @@ bool CollisionMonitor::processStopSlowdownLimit(

bool CollisionMonitor::processApproach(
const std::shared_ptr<Polygon> polygon,
const std::vector<Point> & collision_points,
const std::unordered_map<std::string, std::vector<Point>> & sources_collision_points_map,
const Velocity & velocity,
Action & robot_action) const
{
Expand All @@ -599,7 +574,7 @@ bool CollisionMonitor::processApproach(
}

// Obtain time before a collision
const double collision_time = polygon->getCollisionTime(collision_points, velocity);
const double collision_time = polygon->getCollisionTime(sources_collision_points_map, velocity);
if (collision_time >= 0.0) {
// If collision will occurr, reduce robot speed
const double change_ratio = collision_time / polygon->getTimeBeforeCollision();
Expand Down
81 changes: 61 additions & 20 deletions nav2_collision_monitor/src/polygon.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -234,39 +234,80 @@ int Polygon::getPointsInside(const std::vector<Point> & points) const
return num;
}

int Polygon::getPointsInside(
const std::unordered_map<std::string,
std::vector<Point>> & sources_collision_points_map) const
{
int num = 0;
std::vector<std::string> polygon_sources_names = getSourcesNames();

for (const auto & iter : sources_collision_points_map) {
// Check the points only if the source is associated with current polygon
if (std::find(
polygon_sources_names.begin(), polygon_sources_names.end(), iter.first) ==
polygon_sources_names.end())
{
continue;
}

// Sum the number of points from all sources associated with polygon
num += getPointsInside(iter.second);
}
return num;
}

double Polygon::getCollisionTime(
const std::vector<Point> & collision_points,
const std::unordered_map<std::string, std::vector<Point>> & sources_collision_points_map,
const Velocity & velocity) const
{
// Initial robot pose is {0,0} in base_footprint coordinates
Pose pose = {0.0, 0.0, 0.0};
Velocity vel = velocity;

// Array of points transformed to the frame concerned with pose on each simulation step
std::vector<Point> points_transformed = collision_points;
double collision_time = -1.0; // initialize collision time as if there is no collision
std::vector<std::string> polygon_sources_names = getSourcesNames();

// Check static polygon
if (getPointsInside(points_transformed) >= min_points_) {
return 0.0;
}
for (const auto & iter : sources_collision_points_map) {
// Check the points only if the source is associated with current polygon
if (std::find(
polygon_sources_names.begin(), polygon_sources_names.end(), iter.first) ==
polygon_sources_names.end())
{
continue;
}

// Robot movement simulation
for (double time = 0.0; time <= time_before_collision_; time += simulation_time_step_) {
// Shift the robot pose towards to the vel during simulation_time_step_ time interval
// NOTE: vel is changing during the simulation
projectState(simulation_time_step_, pose, vel);
// Transform collision_points to the frame concerned with current robot pose
points_transformed = collision_points;
transformPoints(pose, points_transformed);
// If the collision occurred on this stage, return the actual time before a collision
// as if robot was moved with given velocity
if (getPointsInside(points_transformed) >= min_points_) {
return time;
// Array of points transformed to the frame concerned with pose on each simulation step
std::vector<Point> points_transformed = iter.second;

// Check static polygon
if (getPointsInside(iter.second) >= min_points_) {
return 0.0; // immediate collision, other sources don't need to be checked
}

// Robot movement simulation
for (double time = 0.0; time <= time_before_collision_; time += simulation_time_step_) {
// Shift the robot pose towards to the vel during simulation_time_step_ time interval
// NOTE: vel is changing during the simulation
projectState(simulation_time_step_, pose, vel);
// Transform collision_points to the frame concerned with current robot pose
points_transformed = iter.second;
transformPoints(pose, points_transformed);
// If the collision occurred on this stage, return the actual time before a collision
// as if robot was moved with given velocity
if (getPointsInside(points_transformed) >= min_points_) {
if (collision_time == -1.0) {
// If first collision detected, keep collision time value
collision_time = time;
} else if (time < collision_time) {
// Keep the smallest collision time value from all sources
collision_time = time;
}
}
}
}

// There is no collision
return -1.0;
return collision_time;
}

void Polygon::publish()
Expand Down
Loading

0 comments on commit 1c672fa

Please sign in to comment.