From 890b96e76a506244218eaf215044b4fe2193a63f Mon Sep 17 00:00:00 2001
From: Lisa <robbie@uni-koblenz.de>
Date: Thu, 8 Dec 2016 21:46:58 +0000
Subject: [PATCH] added support for not quadratic maps
---
homer_nav_libs/CMakeLists.txt | 2 ++
homer_nav_libs/src/Explorer/Explorer.cpp | 8 ++++----
homer_navigation/src/homer_navigation_node.cpp | 12 ++++++------
3 files changed, 12 insertions(+), 10 deletions(-)
diff --git a/homer_nav_libs/CMakeLists.txt b/homer_nav_libs/CMakeLists.txt
index 4bdc2491..9d4962b2 100644
--- a/homer_nav_libs/CMakeLists.txt
+++ b/homer_nav_libs/CMakeLists.txt
@@ -1,6 +1,8 @@
cmake_minimum_required(VERSION 2.8.3)
project(homer_nav_libs)
+set(CMAKE_BUILD_TYPE Release)
+
find_package(catkin REQUIRED COMPONENTS
roscpp
geometry_msgs
diff --git a/homer_nav_libs/src/Explorer/Explorer.cpp b/homer_nav_libs/src/Explorer/Explorer.cpp
index 509964e4..e57d7226 100644
--- a/homer_nav_libs/src/Explorer/Explorer.cpp
+++ b/homer_nav_libs/src/Explorer/Explorer.cpp
@@ -173,9 +173,9 @@ Eigen::Vector2i Explorer::getNearestAccessibleTarget(Eigen::Vector2i target )
ROS_INFO_STREAM("target cell in drivingdistancetransform: " << m_DrivingDistanceTransform->getValue ( target.x(), target.y() ));
ROS_INFO_STREAM("target " << target << " is not approachable. Correcting target...");
int minSqrDist=INT_MAX;
- for ( int x = 0; x < m_ObstacleTransform->height(); x++ )
+ for ( int x = 0; x < m_ObstacleTransform->width(); x++ )
{
- for ( int y = 0; y < m_ObstacleTransform->width(); y++ )
+ for ( int y = 0; y < m_ObstacleTransform->height(); y++ )
{
bool isSafe = m_ObstacleTransform->getValue ( x, y ) > m_FrontierSafenessFactor * m_MinAllowedObstacleDistance;
if ( isApproachable ( x,y ) && isWalkable( x , y) && isSafe )
@@ -219,9 +219,9 @@ Eigen::Vector2i Explorer::getNearestWalkablePoint( Eigen::Vector2i target )
if ( !isWalkable( target.x(), target.y() ) )
{
int minSqrDist=INT_MAX;
- for ( int x = 0; x < m_ObstacleTransform->height(); x++ )
+ for ( int x = 0; x < m_ObstacleTransform->width(); x++ )
{
- for ( int y = 0; y < m_ObstacleTransform->width(); y++ )
+ for ( int y = 0; y < m_ObstacleTransform->height(); y++ )
{
if ( isWalkable ( x,y ) )
{
diff --git a/homer_navigation/src/homer_navigation_node.cpp b/homer_navigation/src/homer_navigation_node.cpp
index 11c740c4..976bf007 100644
--- a/homer_navigation/src/homer_navigation_node.cpp
+++ b/homer_navigation/src/homer_navigation_node.cpp
@@ -208,7 +208,7 @@ void HomerNavigationNode::calculatePath() {
m_path_reaches_target = true;
return;
}
- m_explorer->setOccupancyMap(m_width, m_width, m_origin,
+ m_explorer->setOccupancyMap(m_width, m_height, m_origin,
&(*m_last_map_data)[0]);
m_explorer->setStart(
map_tools::toMapCoords(m_robot_pose.position, m_origin, m_resolution));
@@ -271,7 +271,7 @@ void HomerNavigationNode::startNavigation() {
maskMap();
}
- m_explorer->setOccupancyMap(m_width, m_width, m_origin,
+ m_explorer->setOccupancyMap(m_width, m_height, m_origin,
&(*m_last_map_data)[0]);
// check if there still exists a path to the original target
@@ -896,10 +896,10 @@ void HomerNavigationNode::refreshParamsCallback(
void HomerNavigationNode::mapCallback(
const nav_msgs::OccupancyGrid::ConstPtr& msg) {
- if (msg->info.height != msg->info.width) {
- ROS_ERROR_STREAM("Incoming Map not quadratic. No map update!");
- return;
- }
+ //if (msg->info.height != msg->info.width) {
+ //ROS_ERROR_STREAM("Incoming Map not quadratic. No map update!");
+ //return;
+ //}
if (m_last_map_data) {
delete m_last_map_data;
}
--
GitLab