Namespace harmonization to private node handles and debug display improvement

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]

cob_map_accessibility_analysis/ros/launch/map_accessibility_analysis.launch

@@ -1,15 +1,13 @@
 <?xml version="1.0"?>
 <launch>
 
-	<!-- send parameters to parameter server -->
-	<rosparam command="load" ns="map_accessibility_analysis" file="$(find cob_map_accessibility_analysis)/ros/launch/map_accessibility_analysis_params.yaml"/>
-
 	<!-- start map accessibility analysis -->
 	<node pkg="cob_map_accessibility_analysis" ns="map_accessibility_analysis" type="map_accessibility_analysis_server" name="map_accessibility_analysis" output="screen">	<!-- ns=namespace (arbitrary), type=name of executable, name=node name (arbitrary) -->
 	<!--launch-prefix="/usr/bin/gdb"-->
-		<remap from="map" to="/map"/>
-		<remap from="obstacles" to="/move_base/local_costmap/obstacles"/>
-		<remap from="inflated_obstacles" to="/move_base/local_costmap/inflated_obstacles"/>
+		<rosparam command="load" file="$(find cob_map_accessibility_analysis)/ros/launch/map_accessibility_analysis_params.yaml"/>
+		<remap from="~map" to="/map"/>
+		<remap from="~obstacles" to="/move_base/local_costmap/obstacles"/>
+		<remap from="~inflated_obstacles" to="/move_base/local_costmap/inflated_obstacles"/>
 	</node>
 
 </launch>

cob_map_accessibility_analysis/ros/src/map_accessibility_analysis.cpp

@@ -50,6 +50,10 @@ void MapAccessibilityAnalysis::checkPoses(const std::vector<cv::Point>& 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<cv::Point>& 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<Pose>& 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<Pose>& 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
 	}
 

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

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);