diff --git a/cob_map_accessibility_analysis/ros/include/cob_map_accessibility_analysis/map_accessibility_analysis.h b/cob_map_accessibility_analysis/ros/include/cob_map_accessibility_analysis/map_accessibility_analysis.h
index ff480fed..37ddc030 100644
--- a/cob_map_accessibility_analysis/ros/include/cob_map_accessibility_analysis/map_accessibility_analysis.h
+++ b/cob_map_accessibility_analysis/ros/include/cob_map_accessibility_analysis/map_accessibility_analysis.h
@@ -72,7 +72,7 @@ class MapAccessibilityAnalysis
/**
* This function checks the accessibility of points on a perimeter around a center point.
*
- * \param accessible_poses_on_radius The returned set of accessible poses on the perimeter of the given circle, in map coordinates [px,px,rad]
+ * \param accessible_poses_on_radius The returned set of accessible poses on the perimeter of the given circle, orientation x axis facing to the circle center, in map coordinates [px,px,rad]
* \param center Center of the circle whose perimeter should be checked for accessibility, in map coordinates [px,px,rad]
* \param radius Radius of the circle whose perimeter should be checked for accessibility, in [px]
* \param rotational_sampling_step Rotational sampling step width for checking points on the perimeter, in [rad]
diff --git a/cob_map_accessibility_analysis/ros/launch/map_accessibility_analysis.launch b/cob_map_accessibility_analysis/ros/launch/map_accessibility_analysis.launch
index 53cebb1e..7b28698d 100644
--- a/cob_map_accessibility_analysis/ros/launch/map_accessibility_analysis.launch
+++ b/cob_map_accessibility_analysis/ros/launch/map_accessibility_analysis.launch
@@ -1,15 +1,13 @@
-
-
-
-
-
-
+
+
+
+
diff --git a/cob_map_accessibility_analysis/ros/src/map_accessibility_analysis.cpp b/cob_map_accessibility_analysis/ros/src/map_accessibility_analysis.cpp
index 08519e3a..b36ca627 100644
--- a/cob_map_accessibility_analysis/ros/src/map_accessibility_analysis.cpp
+++ b/cob_map_accessibility_analysis/ros/src/map_accessibility_analysis.cpp
@@ -50,6 +50,10 @@ void MapAccessibilityAnalysis::checkPoses(const std::vector& points_t
cv::findContours(inflated_map_copy, area_contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
}
+#ifdef __DEBUG_DISPLAYS__
+ if (approach_path_accessibility_check == true)
+ cv::circle(display_map, robot_location, 2, cv::Scalar(127), -1);
+#endif
for (unsigned int i = 0; i < points_to_check.size(); ++i)
{
const int u = points_to_check[i].x;
@@ -65,9 +69,9 @@ void MapAccessibilityAnalysis::checkPoses(const std::vector& points_t
#ifdef __DEBUG_DISPLAYS__
if (accessibility_flags[i] == false)
- cv::circle(display_map, cv::Point(u, v), 2, cv::Scalar(32), 10);
+ cv::circle(display_map, cv::Point(u, v), 1, cv::Scalar(32), -1);
else
- cv::circle(display_map, cv::Point(u, v), 2, cv::Scalar(192), 10);
+ cv::circle(display_map, cv::Point(u, v), 1, cv::Scalar(192), -1);
#endif
}
@@ -94,6 +98,11 @@ void MapAccessibilityAnalysis::checkPerimeter(std::vector& accessible_pose
cv::findContours(inflated_map_copy, area_contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
}
+#ifdef __DEBUG_DISPLAYS__
+ cv::circle(display_map, cv::Point(center.x, center.y), 12, cv::Scalar(0), -1);
+ if (approach_path_accessibility_check == true)
+ cv::circle(display_map, robot_location, 2, cv::Scalar(127), -1);
+#endif
for (double angle = center.orientation; angle < center.orientation + 2 * CV_PI; angle += rotational_sampling_step)
{
const double x = center.x + radius * cos(angle);
@@ -122,9 +131,9 @@ void MapAccessibilityAnalysis::checkPerimeter(std::vector& accessible_pose
}
#ifdef __DEBUG_DISPLAYS__
if (found_pose == true)
- cv::circle(display_map, cv::Point(u, v), 2, cv::Scalar(192), 5);
+ cv::circle(display_map, cv::Point(u, v), 1, cv::Scalar(192), -1);
else
- cv::circle(display_map, cv::Point(u, v), 2, cv::Scalar(32), 5);
+ cv::circle(display_map, cv::Point(u, v), 1, cv::Scalar(32), -1);
#endif
}
diff --git a/cob_map_accessibility_analysis/ros/src/map_accessibility_analysis_client.cpp b/cob_map_accessibility_analysis/ros/src/map_accessibility_analysis_client.cpp
index c8e9ceb9..3d7bc639 100644
--- a/cob_map_accessibility_analysis/ros/src/map_accessibility_analysis_client.cpp
+++ b/cob_map_accessibility_analysis/ros/src/map_accessibility_analysis_client.cpp
@@ -33,9 +33,9 @@ int main(int argc, char** argv)
ros::NodeHandle n;
- std::string points_service_name = "/map_accessibility_analysis/map_points_accessibility_check";
- std::string perimeter_service_name = "/map_accessibility_analysis/map_perimeter_accessibility_check";
- std::string polygon_service_name = "/map_accessibility_analysis/map_polygon_accessibility_check";
+ std::string points_service_name = "/map_accessibility_analysis/map_accessibility_analysis/map_points_accessibility_check";
+ std::string perimeter_service_name = "/map_accessibility_analysis/map_accessibility_analysis/map_perimeter_accessibility_check";
+ std::string polygon_service_name = "/map_accessibility_analysis/map_accessibility_analysis/map_polygon_accessibility_check";
// here we wait until the service is available; please use the same service name as the one in the server; you may
// define a timeout if the service does not show up
diff --git a/cob_map_accessibility_analysis/ros/src/map_accessibility_analysis_server.cpp b/cob_map_accessibility_analysis/ros/src/map_accessibility_analysis_server.cpp
index 124c7ca5..6edda43b 100644
--- a/cob_map_accessibility_analysis/ros/src/map_accessibility_analysis_server.cpp
+++ b/cob_map_accessibility_analysis/ros/src/map_accessibility_analysis_server.cpp
@@ -566,7 +566,7 @@ int main(int argc, char** argv)
{
ros::init(argc, argv, "map_accessibility_analysis_server");
- ros::NodeHandle nh;
+ ros::NodeHandle nh("~");
MapAccessibilityAnalysisServer map_accessibility_analysis(nh);