merge with rmb

ipa_building_msgs/action/RoomExploration.action

@@ -11,7 +11,8 @@ float32 map_resolution					# the resolution of the map in [meter/cell]
 geometry_msgs/Pose map_origin			# the origin of the map in [meter], NOTE: rotations are not supported for now
 float32 robot_radius					# effective robot radius, taking the enlargement of the costmap into account, in [meter]
 float32 coverage_radius					# radius that is used to plan the coverage planning for the robot and not the field of view, assuming that the part that needs to cover everything (e.g. the cleaning part) can be represented by a fitting circle (e.g. smaller than the actual part to ensure coverage), in [meter]
-geometry_msgs/Point32[] field_of_view	# the 4 points that define the field of view of the robot, relatively to the robot coordinate system (with x pointing forwards and y pointing to the left), [meter], counter-clock-wise starting from left, nearer to robot point, assumed to be 4 different points in the right halfplane of the two dimensional space
+geometry_msgs/Point32[] field_of_view	# the 4 points that define the field of view of the robot, relatively to the robot coordinate system (with x pointing forwards and y pointing to the left), in [meter]
+geometry_msgs/Point32 field_of_view_origin	# the mounting position of the camera spanning the field of view, relative to the robot center (x-axis points to robot's front side, y-axis points to robot's left side, z-axis upwards), in [meter]
 geometry_msgs/Pose2D starting_position	# starting pose of the robot in the room coordinate system [meter,meter,rad]
 int32 planning_mode						# 1 = plans a path for coverage with the robot footprint, 2 = plans a path for coverage with the robot's field of view
 

ipa_building_msgs/srv/CheckCoverage.srv

@@ -1,8 +1,9 @@
 sensor_msgs/Image input_map				# the action server need a map as a input image to segment it, IMPORTANT: The algorithm needs a black and white 8bit single-channel image, which is 0 (black) for obstacles and 255 (white) for free space
-float32 map_resolution					# resolution of the given map, [meter/cell]
-geometry_msgs/Pose map_origin			# the origin of the map in [meter]
-geometry_msgs/Pose2D[] path				# check the coverage along this path, in the world frame [meter]
-geometry_msgs/Point32[] field_of_view	# the points that define the field of view of the robot, relatively to the robot, [meter], counter-clock-wise starting from left, nearer to robot point, assumed to be 4 different points in the right halfplane of the two dimensional space
+float32 map_resolution					# resolution of the given map, in [meter/cell]
+geometry_msgs/Pose map_origin			# the origin of the map, in [meter]
+geometry_msgs/Pose2D[] path				# check the coverage along this path of the robot center, in the world frame in [meter]
+geometry_msgs/Point32[] field_of_view	# the points that define the field of view of the robot, relative to the robot center (x-axis points to robot's front side, y-axis points to robot's left side, z-axis upwards), in [meter]
+geometry_msgs/Point32 field_of_view_origin	# the mounting position of the camera spanning the field of view, relative to the robot center (x-axis points to robot's front side, y-axis points to robot's left side, z-axis upwards), in [meter]
 float32 coverage_radius					# radius that is used to plan the coverage planning for the robot and not the field of view, assuming that the part that needs to cover everything (e.g. the cleaning part) can be represented by a fitting circle (e.g. smaller than the actual part to ensure coverage), in [meter]
 bool check_for_footprint				# determine, if the coverage check should be done for the footprint or the field of view
 bool check_number_of_coverages			# if set, the server returns a map that shows how often one pixel has been covered during the path, return format: 32bit single-channel image

ipa_building_navigation/common/src/A_star_pathplanner.cpp

@@ -261,6 +261,13 @@ double AStarPlanner::planPath(const cv::Mat& map, const cv::Point& start_point,
 		return path_length;
 	}
 
+	// check for valid coordinates of start and end points
+	if (start_point.x < 0 || start_point.x >= map.cols || start_point.y < 0 || start_point.y >= map.rows ||
+			end_point.x < 0 || end_point.x >= map.cols || end_point.y < 0 || end_point.y >= map.rows)
+	{
+		return 1e100;
+	}
+
 	cv::Mat downsampled_map;
 	downsampleMap(map, downsampled_map, downsampling_factor, robot_radius, map_resolution);
 

ipa_building_navigation/ros/launch/room_sequence_planning_action_server_params.yaml

@@ -26,7 +26,7 @@ planning_method: 2
 
 # max A* path length between two rooms that are assigned to the same clique, in [m] (only applies to CheckpointBasedPlanning (2) problem_setting)
 # double
-max_clique_path_length: 12.0
+max_clique_path_length: 1200.0
 
 # maximal nodes in one clique for one trolley position (only applies to CheckpointBasedPlanning (2) problem_setting)
 # int

ipa_room_exploration/CMakeLists.txt

@@ -96,6 +96,7 @@ INCLUDE_DIRS
 	common/include
 	ros/include
 LIBRARIES
+	libcoverage_check_server
 CATKIN_DEPENDS
 	${catkin_RUN_PACKAGES}
 DEPENDS
@@ -156,6 +157,7 @@ target_link_libraries(room_exploration_server
 	${Cgl_LIBRARIES}
 	${Cbc-lib_LIBRARIES}
 	${GUROBI_LIBRARIES}
+	libcoverage_check_server
 )
 add_dependencies(room_exploration_server
 	${catkin_EXPORTED_TARGETS}
@@ -219,6 +221,7 @@ add_executable(room_exploration_evaluation
 target_link_libraries(room_exploration_evaluation
 	${catkin_LIBRARIES} 
 	${OpenCV_LIBS}
+	libcoverage_check_server
 )
 add_dependencies(room_exploration_evaluation 
 	${catkin_EXPORTED_TARGETS}

ipa_room_exploration/cfg/RoomExploration.cfg

@@ -75,6 +75,18 @@ gen.add("path_eps", double_t, 0, "Minimal distance between two points on the gen
 # the additional offset of the grid to obstacles, i.e. allows to displace the grid by more than the standard half_grid_size from obstacles, in [m]
 gen.add("grid_obstacle_offset", double_t, 0, "Allows to displace the grid by more than the standard half_grid_size from obstacles [m].", 0.0, 0)
 
+# maximal allowed shift off the ideal boustrophedon track to both sides for avoiding obstacles on track
+# setting max_deviation_from_track=grid_spacing is usually a good choice
+# for negative values (e.g. max_deviation_from_track: -1) max_deviation_from_track is automatically set to grid_spacing
+# in [pixel]
+gen.add("max_deviation_from_track", int_t, 0, "Maximal allowed shift off the ideal boustrophedon track for avoiding obstacles on track, in [pixel]. For negative values max_deviation_from_track is automatically set to grid_spacing.", -1, -1)
+
+# enum for cell visiting order
+cell_visiting_order_enum = gen.enum([gen.const("OptimalTSP", int_t, 1, "The optimal visiting order of the cells is determined as TSP problem."),
+			gen.const("LeftToRight", int_t, 2, "Alternative ordering from left to right (measured on y-coordinates of the cells), visits the cells in a more obvious fashion to the human observer (though it is not optimal).")],
+			"Cell visiting order")
+gen.add("cell_visiting_order", int_t, 0, "Cell visiting order method", 1, 1, 2, edit_method=cell_visiting_order_enum)
+
 
 # Neural network explorator, see room_exploration_action_server.params.yaml for further details
 # =============================================================================================

ipa_room_exploration/common/include/ipa_room_exploration/boustrophedon_explorator.h

@@ -63,6 +63,8 @@
 #include <opencv2/highgui/highgui.hpp>
 #include <iostream>
 #include <vector>
+#include <map>
+#include <set>
 #include <cmath>
 #include <string>
 
@@ -85,20 +87,26 @@
 
 // Class that is used to store cells and obstacles in a certain manner. For this the vertexes are stored as points and
 // the edges are stored as vectors in a counter-clockwise manner. The constructor becomes a set of respectively sorted
-// points and computes the vectors out of them. Additionally the visible center of the polygon gets computed, to
-// simplify the visiting order later, by using a meanshift algorithm.
+// points and computes the vectors out of them. Additionally the accessible/visible center of the polygon gets computed,
+// to simplify the visiting order later, by using a meanshift algorithm.
 class GeneralizedPolygon
 {
 protected:
 	// vertexes
 	std::vector<cv::Point> vertices_;
 
-	// center
+	// accessible center: a central point inside the polygon with maximum distance to walls
 	cv::Point center_;
 
+	// center of bounding rectangle of polygon, may be located outside the polygon, i.e. in an inaccessible area
+	cv::Point bounding_box_center_;
+
 	// min/max coordinates
 	int max_x_, min_x_, max_y_, min_y_;
 
+	// area of the polygon (cell number), in [pixel^2]
+	double area_;
+
 public:
 	// constructor
 	GeneralizedPolygon(const std::vector<cv::Point>& vertices, const double map_resolution)
@@ -123,10 +131,14 @@ class GeneralizedPolygon
 				min_y_ = vertices_[point].y;
 		}
 
+		bounding_box_center_.x = (min_x_+max_x_)/2;
+		bounding_box_center_.y = (min_y_+max_y_)/2;
+
 		// compute visible center
 		MeanShift2D ms;
 		cv::Mat room = cv::Mat::zeros(max_y_+10, max_x_+10, CV_8UC1);
 		cv::drawContours(room, std::vector<std::vector<cv::Point> >(1,vertices), -1, cv::Scalar(255), CV_FILLED);
+		area_ = cv::countNonZero(room);
 		cv::Mat distance_map; //variable for the distance-transformed map, type: CV_32FC1
 		cv::distanceTransform(room, distance_map, CV_DIST_L2, 5);
 		// find point set with largest distance to obstacles
@@ -144,14 +156,24 @@ class GeneralizedPolygon
 		center_.y = room_center[1];
 	}
 
+	std::vector<cv::Point> getVertices() const
+	{
+		return vertices_;
+	}
+
 	cv::Point getCenter() const
 	{
 		return center_;
 	}
 
-	std::vector<cv::Point> getVertices() const
+	cv::Point getBoundingBoxCenter() const
 	{
-		return vertices_;
+		return bounding_box_center_;
+	}
+
+	double getArea() const
+	{
+		return area_;
 	}
 
 	void drawPolygon(cv::Mat& image, const cv::Scalar& color) const
@@ -180,6 +202,26 @@ struct BoustrophedonHorizontalLine
 	cv::Point left_corner_, right_corner_;
 };
 
+// Structure for saving several properties of cells
+struct BoustrophedonCell
+{
+	typedef std::set<boost::shared_ptr<BoustrophedonCell> > BoustrophedonCellSet;
+	typedef std::set<boost::shared_ptr<BoustrophedonCell> >::iterator BoustrophedonCellSetIterator;
+
+	int label_;				// label id of the cell
+	double area_;			// area of the cell, in [pixel^2]
+	cv::Rect bounding_box_;		// bounding box of the cell
+	BoustrophedonCellSet neighbors_;		// pointer to neighboring cells
+
+	BoustrophedonCell(const int label, const double area, const cv::Rect& bounding_box)
+	{
+		label_ = label;
+		area_ = area;
+		bounding_box_ = bounding_box;
+	}
+
+};
+
 
 // Class that generates a room exploration path by using the morse cellular decomposition method, proposed by
 //
@@ -198,29 +240,44 @@ class BoustrophedonExplorer
 	// pathplanner to check for the next nearest locations
 	AStarPlanner path_planner_;
 
+	static const uchar BORDER_PIXEL_VALUE = 25;
+
 	// rotates the original map for a good axis alignment and divides it into Morse cells
 	// the functions tries two axis alignments with 90deg rotation difference and chooses the one with the lower number of cells
 	void findBestCellDecomposition(const cv::Mat& room_map, const float map_resolution, const double min_cell_area,
-			cv::Mat& R, cv::Rect& bbox, cv::Mat& rotated_room_map,
+			const int min_cell_width, cv::Mat& R, cv::Rect& bbox, cv::Mat& rotated_room_map,
 			std::vector<GeneralizedPolygon>& cell_polygons, std::vector<cv::Point>& polygon_centers);
 
 	// rotates the original map for a good axis alignment and divides it into Morse cells
 	// @param rotation_offset can be used to put an offset to the computed rotation for good axis alignment, in [rad]
 	void computeCellDecompositionWithRotation(const cv::Mat& room_map, const float map_resolution, const double min_cell_area,
-			const double rotation_offset, cv::Mat& R, cv::Rect& bbox, cv::Mat& rotated_room_map,
+			const int min_cell_width, const double rotation_offset, cv::Mat& R, cv::Rect& bbox, cv::Mat& rotated_room_map,
 			std::vector<GeneralizedPolygon>& cell_polygons, std::vector<cv::Point>& polygon_centers);
 
 	// divides the provided map into Morse cells
-	virtual void computeCellDecomposition(const cv::Mat& room_map, const float map_resolution, const double min_cell_area,
-			std::vector<GeneralizedPolygon>& cell_polygons, std::vector<cv::Point>& polygon_centers);
+	void computeCellDecomposition(const cv::Mat& room_map, const float map_resolution, const double min_cell_area,
+			const int min_cell_width, std::vector<GeneralizedPolygon>& cell_polygons, std::vector<cv::Point>& polygon_centers);
+
+	// merges cells after a cell decomposition according to various criteria specified in function @mergeCellsSelection
+	// returns the number of cells after merging
+	int mergeCells(cv::Mat& cell_map, cv::Mat& cell_map_labels, const double min_cell_area, const int min_cell_width);
+
+	// implements the selection criterion for cell merging, in this case: too small (area) or too thin (width or height) cells
+	// are merged with their largest neighboring cell.
+	void mergeCellsSelection(cv::Mat& cell_map, cv::Mat& cell_map_labels, std::map<int, boost::shared_ptr<BoustrophedonCell> >& cell_index_mapping,
+			const double min_cell_area, const int min_cell_width);
+
+	// executes the merger of minor cell into major cell
+	void mergeTwoCells(cv::Mat& cell_map, cv::Mat& cell_map_labels, const BoustrophedonCell& minor_cell, BoustrophedonCell& major_cell,
+			std::map<int, boost::shared_ptr<BoustrophedonCell> >& cell_index_mapping);
 
 	// this function corrects obstacles that are one pixel width at 45deg angle, i.e. a 2x2 pixel neighborhood with [0, 255, 255, 0] or [255, 0, 0, 255]
 	void correctThinWalls(cv::Mat& room_map);
 
 	// computes the Boustrophedon path pattern for a single cell
 	void computeBoustrophedonPath(const cv::Mat& room_map, const float map_resolution, const GeneralizedPolygon& cell,
 			std::vector<cv::Point2f>& fov_middlepoint_path, cv::Point& robot_pos,
-			const int grid_spacing_as_int, const int half_grid_spacing_as_int, const double path_eps, const int grid_obstacle_offset=0);
+			const int grid_spacing_as_int, const int half_grid_spacing_as_int, const double path_eps, const int max_deviation_from_track, const int grid_obstacle_offset=0);
 
 	// downsamples a given path original_path to waypoint distances of path_eps and appends the resulting path to downsampled_path
 	void downsamplePath(const std::vector<cv::Point>& original_path, std::vector<cv::Point>& downsampled_path,
@@ -231,6 +288,8 @@ class BoustrophedonExplorer
 	void downsamplePathReverse(const std::vector<cv::Point>& original_path, std::vector<cv::Point>& downsampled_path,
 			cv::Point& robot_pos, const double path_eps);
 
+	void printCells(std::map<int, boost::shared_ptr<BoustrophedonCell> >& cell_index_mapping);
+
 public:
 	// constructor
 	BoustrophedonExplorer();
@@ -239,19 +298,21 @@ class BoustrophedonExplorer
 	// with free space drawn white (255) and obstacles as black (0). It returns a series of 2D poses that show to which positions
 	// the robot should drive at.
 	void getExplorationPath(const cv::Mat& room_map, std::vector<geometry_msgs::Pose2D>& path, const float map_resolution,
-				const cv::Point starting_position, const cv::Point2d map_origin,
-				const double grid_spacing_in_pixel, const double grid_obstacle_offset, const double path_eps, const bool plan_for_footprint,
-				const Eigen::Matrix<float, 2, 1> robot_to_fov_vector, const double min_cell_area);
+				const cv::Point starting_position, const cv::Point2d map_origin, const double grid_spacing_in_pixel,
+				const double grid_obstacle_offset, const double path_eps, const int cell_visiting_order, const bool plan_for_footprint,
+				const Eigen::Matrix<float, 2, 1> robot_to_fov_vector, const double min_cell_area, const int max_deviation_from_track);
+
+	enum CellVisitingOrder {OPTIMAL_TSP=1, LEFT_TO_RIGHT=2};
 };
 
 
 class BoustrophedonVariantExplorer : public BoustrophedonExplorer
 {
 protected:
 
-	// computes a suitable cell decomposition for the given room_map
-	void computeCellDecomposition(const cv::Mat& room_map, const float map_resolution, const double min_cell_area,
-			std::vector<GeneralizedPolygon>& cell_polygons, std::vector<cv::Point>& polygon_centers);
+	// implements the selection criterion for cell merging, in this case: only large cells with different major axis are not merged.
+	void mergeCellsSelection(cv::Mat& cell_map, cv::Mat& cell_map_labels, std::map<int, boost::shared_ptr<BoustrophedonCell> >& cell_index_mapping,
+			const double min_cell_area, const int min_cell_width);
 
 public:
 	BoustrophedonVariantExplorer() {};