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

Trying to get things working with higher resolutions, but planning and...

Trying to get things working with higher resolutions, but planning and obstacle avoidance keep getting stuck in obstacles
parent e3d63ad0
......@@ -26,7 +26,7 @@
/homer_navigation/max_turn_speed: 0.9 # rad/s max turn velocity the navigation can send
/homer_navigation/min_turn_speed: 0.2 # rad/s min turn speed for Final Turn so the Robot doesn't stop turning
/homer_navigation/max_drive_angle: 0.5 # rad threshold for driving and turning - if above that value only turn
/homer_navigation/speed_ramp: 0.1 # the maximal speed addition to the last driving speed in one calculation
/homer_navigation/speed_ramp: 0.05 # the maximal speed addition to the last driving speed in one calculation
/homer_navigation/max_move_speed: 0.9 # m/s max move speed the navigation can send
### caution factors values near 0 mean high caution values greater values mean less caution
......
......@@ -196,6 +196,9 @@ private:
/// @brief Worker instances
Explorer* m_explorer;
bool m_IsSimulation;
bool m_IsTIAGo;
/// @brief Navigation options & data
/** list of waypoints subsampled from m_PixelPath */
......
......@@ -2,7 +2,7 @@
#include <homer_ptu/HomerPtuLib.h>
#define DUMP_OBSTACLE_AVOIDANCE_IMAGE 0
#define DUMP_OBSTACLE_AVOIDANCE_IMAGE 1
#if DUMP_OBSTACLE_AVOIDANCE_IMAGE
#include <opencv2/opencv.hpp>
#endif
......@@ -150,6 +150,16 @@ void HomerNavigationNode::init()
loadParameters();
// Defaulting to not-TIAGo if ROBOT not set
m_IsSimulation = std::getenv("SIMULATION") != nullptr;
std::string robot = "[ EMPTY ]";
const auto env_robot = std::getenv("ROBOT");
if (env_robot != nullptr)
robot = std::string(env_robot);
m_IsTIAGo = robot.compare("tiago") == 0;
ROS_INFO_STREAM("- robot: " << robot);
ROS_INFO_STREAM("- simulation: " << m_IsSimulation);
m_state = IDLE;
}
......@@ -493,6 +503,7 @@ geometry_msgs::Point HomerNavigationNode::calculateMeanPoint(
bool HomerNavigationNode::obstacleOnPath()
{
ROS_INFO_STREAM("obstacleOnPath");
m_last_check_path_time = ros::Time::now();
if (m_waypoints.size() == 0)
{
......@@ -506,37 +517,31 @@ bool HomerNavigationNode::obstacleOnPath()
waypoints.push_back(m_robot_pose.position);
// Only check if path is blocked from current robot position to the next waypoints
int count = 0;
int count = 0;
const int MAX_WAYPOINTS_TO_CHECK = 1;
for (const auto& p : m_waypoints)
{
waypoints.push_back(p.pose.position);
count++;
if (count > 0)
if (MAX_WAYPOINTS_TO_CHECK > 0 && count >= MAX_WAYPOINTS_TO_CHECK)
break;
}
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++)
{
const auto& wp0 = waypoints[i];
const auto& wp1 = waypoints[i+1];
const auto generate_mask_coordinates = [&](std::vector<Eigen::Vector2i>& coordinates, const auto& wp0, const auto& wp1, const auto distance)
{
const auto p0 = Eigen::Vector2d(wp0.x, wp0.y);
const auto p1 = Eigen::Vector2d(wp1.x, wp1.y);
Eigen::Vector2d direction = p1 - p0;
const double distance = direction.norm();
direction /= distance;
direction /= direction.norm();
const auto normal = Eigen::Vector2d(direction.y(), -direction.x());
// 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;
const auto righttop = p1 + normal * m_AllowedObstacleDistance.first;
const auto leftbottom = p0 - normal * distance + 0.2 * direction;
const auto rightbottom = p0 + normal * distance + 0.2 * direction;
const auto lefttop = p1 - normal * distance;
const auto righttop = p1 + normal * distance;
const auto lb = map_tools::toMapCoords(leftbottom, m_map->info.origin, m_map->info.resolution);
const auto lt = map_tools::toMapCoords(lefttop, m_map->info.origin, m_map->info.resolution);
......@@ -570,18 +575,19 @@ bool HomerNavigationNode::obstacleOnPath()
// Also check circle arround next waypoint and current position
const float TWO_PI = 2.f * M_PI;
for(const auto& origin : {p0, p1})
// Used this value with 3cm x 3cm resolution to form full circle, scaling to other resolutions like this
const auto ANGLE_STEP = 0.1 / std::pow((0.03 * 0.03) / (m_map->info.resolution * m_map->info.resolution), 2);
for(const auto& origin : {p1})
{
for (float angle = 0.f; angle < TWO_PI; angle += 0.1)
for (float angle = 0.f; angle < TWO_PI; angle += ANGLE_STEP)
{
const Eigen::Vector2d v = m_AllowedObstacleDistance.first * Eigen::Vector2d(std::cos(angle), std::sin(angle));
const Eigen::Vector2d v = distance * Eigen::Vector2d(std::cos(angle), std::sin(angle));
Eigen::Vector2d p = origin + v;
const auto px = map_tools::toMapCoords(p, m_map->info.origin, m_map->info.resolution);
update_ranges(px);
}
}
// Fill polygon from left to right for each row
coordinates.clear();
for (const auto& row : row_ranges)
{
const auto y = row.first;
......@@ -592,11 +598,17 @@ bool HomerNavigationNode::obstacleOnPath()
coordinates.push_back(px);
}
}
};
bool path_blocked = false;
auto collision_coordinate = Eigen::Vector2i(0,0);
std::vector<Eigen::Vector2i> coordinates;
for(auto i = 0; i < waypoints.size()-1; i++)
{
generate_mask_coordinates(coordinates, waypoints[i], waypoints[i+1], m_AllowedObstacleDistance.first + 0.01);
for (const auto& c : coordinates)
{
checked_coordinates.push_back(c);
const auto index = c.x() + m_map->info.width * c.y();
if (index < 0 || index >= m_map->data.size())
continue;
......@@ -640,8 +652,9 @@ bool HomerNavigationNode::obstacleOnPath()
for(const auto& c : coordinates)
{
auto& color = img.at<cv::Vec3b>(c.y(), c.x());
color *= 0.8;
color[2] = std::min(255, int(color[2] / 0.8 * 1.2));
color[2] = 150;
// color *= 0.8;
// color[2] = std::min(255, int(color[2] / 0.8 * 1.2));
}
for(const auto& wp : m_waypoints)
......@@ -666,6 +679,7 @@ bool HomerNavigationNode::obstacleOnPath()
cv::imwrite("/tmp/obstacles.png", img);
#endif
ROS_INFO_STREAM("obstacleOnPath -- done");
return path_blocked;
}
/*
......@@ -1528,7 +1542,7 @@ void HomerNavigationNode::initExplorer()
if (m_map->info.resolution != 0)
{
float resolution = m_map->info.resolution;
m_explorer = new Explorer(m_AllowedObstacleDistance.first / resolution,
m_explorer = new Explorer((m_AllowedObstacleDistance.first + 0.01) / resolution,
m_AllowedObstacleDistance.second / resolution,
m_SafeObstacleDistance.first / resolution,
m_SafeObstacleDistance.second / resolution, m_SafePathWeight,
......
Markdown is supported
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