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

Also added circle check for current position; Print result interpretation when...

Also added circle check for current position; Print result interpretation when calling drive_to_POI_blocked
parent 0ecf7852
......@@ -103,7 +103,19 @@ class NavigationInterface:
result_delivered = client.wait_for_result(timeout)
if not result_delivered:
return None
return client.get_result()
res = client.get_result()
if res.result == 0:
rospy.logwarn("DriveToAction responded with FAILED_TARGET_UNREACHABLE.")
if res.result == 2:
rospy.loginfo("Reached target.")
if res.result == 3:
rospy.logwarn("DriveToAction responded with SMALL_OBSTACLE.")
if res.result == 4:
rospy.logwarn("DriveToAction responded with BIG_OBSTACLE.")
if res.result == 6:
rospy.logwarn("DriveToAction responded with NO_OBSTACLE.")
return res
#---------------------------------------------------------------------------------------------------
@staticmethod
......
......@@ -574,17 +574,18 @@ bool HomerNavigationNode::obstacleOnPath()
for(const auto& c : line)
update_ranges(c);
// Also check circle arround next waypoint
// Also check circle arround next waypoint and current position
const float TWO_PI = 2.f * M_PI;
for (float angle = 0.f; angle < TWO_PI; angle += 0.1)
for(const auto& origin : {p0, p1})
{
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);
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 = 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)
......
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