Commit e23d7c37 authored by Daniel Müller's avatar Daniel Müller
Browse files

Added circle region around next way point for obstacle checking, until now,...

Added circle region around next way point for obstacle checking, until now, way up until waypoint was checked, not further, although robot has some radius when having reached this waypoint
parent cdad0cf2
......@@ -2,6 +2,7 @@
#include <homer_ptu/HomerPtuLib.h>
#define DUMP_OBSTACLE_AVOIDANCE_IMAGE 0
#if DUMP_OBSTACLE_AVOIDANCE_IMAGE
#include <opencv2/opencv.hpp>
#endif
......@@ -521,6 +522,7 @@ bool HomerNavigationNode::obstacleOnPath()
}
bool path_blocked = false;
auto collision_coordinate = Eigen::Vector2i(0,0);
std::vector<Eigen::Vector2i> coordinates;
std::vector<Eigen::Vector2i> checked_coordinates;
for(auto i = 0; i < waypoints.size()-1; i++)
{
......@@ -536,7 +538,7 @@ bool HomerNavigationNode::obstacleOnPath()
const auto normal = Eigen::Vector2d(direction.y(), -direction.x());
// Oriented rectangle region to check in map coordinates
// Oriented rectangle region to check in map coordinates, region ends on next waypoint
const auto leftbottom = p0 - normal * m_AllowedObstacleDistance.first;
const auto rightbottom = p0 + normal * m_AllowedObstacleDistance.first;
const auto lefttop = p1 - normal * m_AllowedObstacleDistance.first;
......@@ -555,45 +557,47 @@ bool HomerNavigationNode::obstacleOnPath()
generateLine(top, lt, rt);
std::map<int, std::pair<int, int>> row_ranges;
for (const auto& line : {left, right, bottom, top})
const auto update_ranges = [&row_ranges](const Eigen::Vector2i& c)
{
const auto x = c.x();
const auto y = c.y();
if (row_ranges.find(y) == row_ranges.end())
row_ranges.emplace(y, std::make_pair(std::numeric_limits<int>::max(), std::numeric_limits<int>::lowest()));
auto& range = row_ranges[y];
if(x < range.first)
range.first = x;
if (x > range.second)
range.second = x;
};
for (const auto& line : {left, right, bottom, top})
for(const auto& c : line)
{
const auto x = c.x();
const auto y = c.y();
if (row_ranges.find(y) == row_ranges.end())
row_ranges.emplace(y, std::make_pair(std::numeric_limits<int>::max(), std::numeric_limits<int>::min()));
auto& range = row_ranges[y];
if(x < range.first)
range.first = x;
if (x > range.second)
range.second = x;
}
update_ranges(c);
// Also check circle arround next waypoint
const float TWO_PI = 2.f * M_PI;
for (float angle = 0.f; angle < TWO_PI; angle += 0.1)
{
const Eigen::Vector2d v = m_AllowedObstacleDistance.first * Eigen::Vector2d(std::cos(angle), std::sin(angle));
Eigen::Vector2d p = p1 + v;
const auto px = map_tools::toMapCoords(p, m_map->info.origin, m_map->info.resolution);
ROS_INFO_STREAM(p.transpose() << " | " << px.transpose());
update_ranges(px);
}
std::vector<Eigen::Vector2i> coordinates;
// Fill polygon from left to right for each row
coordinates.clear();
for (const auto& row : row_ranges)
{
const auto y = row.first;
const auto& range = row.second;
for(auto x = range.first; x <= range.second; x++)
coordinates.push_back(Eigen::Vector2i(x, y));
{
Eigen::Vector2i px(x, y);
coordinates.push_back(px);
}
}
/*
// Generate filled polygon
for(const auto& l : left)
{
const auto point = map_tools::fromMapCoords(l, m_map->info.origin, m_map->info.resolution);
const auto p0 = Eigen::Vector2d(point.x, point.y);
const auto p1 = p0 + 2.0 * m_AllowedObstacleDistance.first * normal;
const auto r = map_tools::toMapCoords(p1, m_map->info.origin, m_map->info.resolution);
generateLine(coordinates, l, r);
// DEBUG
//generateLine(coordinates, lb, lt);
//break;
}
*/
for (const auto& c : coordinates)
{
......@@ -638,7 +642,7 @@ bool HomerNavigationNode::obstacleOnPath()
}
}
// Searched area
for(const auto& c : checked_coordinates)
for(const auto& c : coordinates)
{
auto& color = img.at<cv::Vec3b>(c.y(), c.x());
color *= 0.8;
......@@ -975,6 +979,7 @@ bool HomerNavigationNode::checkForObstacles()
// targetUnreachableCallback of the drive_to_node.
if (m_stop_before_obstacle)
{
ROS_INFO_STREAM("Stopping in front of obstacle.");
sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::LASER_OBSTACLE);
if (m_state == IDLE)
return false;
......@@ -1056,10 +1061,9 @@ void HomerNavigationNode::followPath()
void HomerNavigationNode::avoidingCollision()
{
if (isTargetPositionReached())
{
return;
}
else if ((m_max_move_distance <= m_collision_distance && m_waypoints.size() > 1)
if ((m_max_move_distance <= m_collision_distance && m_waypoints.size() > 1)
|| m_max_move_distance <= m_collision_distance_near_target)
{
float min_distance_to_last_waypoint = 0.3;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment