Merge pull request #13 from ipa-rmb/indigo_dev

New features, bugfixes, compatibility fixes

ipa_building_msgs/CMakeLists.txt

@@ -7,7 +7,6 @@ project(ipa_building_msgs)
 find_package(catkin REQUIRED COMPONENTS
 	actionlib
 	actionlib_msgs
-	cmake_modules
 	genmsg
 	geometry_msgs
 	message_generation

ipa_building_msgs/action/FindRoomSequenceWithCheckpoints.action

@@ -9,7 +9,7 @@ geometry_msgs/Pose robot_start_coordinate	# current robot location (used to dete
 ---
 
 # result definition
-ipa_building_msgs/RoomSequence[] checkpoints		# sequence of checkpoints, should be ordered in optimized traveling salesman sequence
+ipa_building_msgs/RoomSequence[] checkpoints			# sequence of checkpoints, should be ordered in optimized traveling salesman sequence
 														# (checkpoint = a clique of rooms close [in terms of robot driving distance] to each other,
 														#               and a central [in terms of rooms in the group] checkpoint location)
 sensor_msgs/Image sequence_map							# map that has the calculated sequence drawn in

ipa_building_msgs/action/MapSegmentation.action

@@ -8,6 +8,14 @@ geometry_msgs/Pose map_origin			# the origin of the map in [meter]
 bool return_format_in_pixel				# return room data (see below) in [pixel]
 bool return_format_in_meter				# return room data (see below) in [meter]
 float32 robot_radius					# in [meter]; if this variable is set to a value greater than 0, the room centers are chosen at locations that are reachable from neighboring rooms (i.e. not inside the legs of a table surrounded by chairs)
+int32 room_segmentation_algorithm		# optionally overrides the parameterized segmentation method (if -1 it just takes the preset)
+										#  0 = take the preset (parameterized) method
+										#  1 = MorphologicalSegmentation
+										#  2 = DistanceSegmentation
+										#  3 = VoronoiSegmentation
+										#  4 = SemanticSegmentation
+										#  5 = RandomFieldSegmentation
+										# 99 = PassThrough (just get a pre-segmented map into the right output format)
 
 ---
 

ipa_building_msgs/action/RoomExploration.action

@@ -2,13 +2,18 @@
 # sends an occupancy grid map of a room to the server, which plans a path trough it to cover or inspect the whole room by a device or camera
 
 # goal definition
-sensor_msgs/Image input_map				# the action server needs a map as a input image to segment it, IMPORTANT: The algorithm needs a black and white 8bit single-channel image (format 8UC1), which is 0 (black) for obstacles and 255 (white) for free space
+sensor_msgs/Image input_map				# the action server needs a map as a input image to segment it,
+										# IMPORTANT: The algorithm needs a black and white 8bit single-channel image (format 8UC1), which is 0 (black) for obstacles and 255 (white) for free space
+										#            todo: the image needs to be vertically mirrored compared to the map in RViz for using right coordinate systems
+										#                  OccupancyGrid map = origin lower left corner, image = origin upper left corner
+										#            todo: take the OccupanyGrid message here instead to avoid confusion and deal with map coordinates in server
 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/Pose2D starting_position	# starting pose of the robot in the room coordinate system [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), 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,11 +1,12 @@
 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
 ---
-sensor_msgs/Image coverage_map			# the map that has the covered areas drawn in, with a value of 127, also a 8bit single-channel image
+sensor_msgs/Image coverage_map			# the map that has the covered areas drawn in, with a value of 255, an 8bit single-channel image
 sensor_msgs/Image number_of_coverage_image	# the image that carries for each pixel the number of coverages when executing the path, 32bit single-channel image

ipa_building_navigation/CMakeLists.txt

@@ -10,11 +10,11 @@ set(catkin_RUN_PACKAGES
 	roscpp
 	roslib
 	sensor_msgs
+	visualization_msgs
 )
 
 set(catkin_BUILD_PACKAGES
 	${catkin_RUN_PACKAGES}
-	cmake_modules
 	dynamic_reconfigure
 )
 

ipa_building_navigation/common/include/ipa_building_navigation/A_star_pathplanner.h

@@ -11,8 +11,8 @@
 #include <cstdlib>
 #include <stdio.h>
 
-#include <opencv/cv.h>
-#include <opencv/highgui.h>
+#include <opencv2/opencv.hpp>
+#include <opencv2/highgui/highgui.hpp>
 
 #include <ipa_building_navigation/node.h>
 

ipa_building_navigation/common/include/ipa_building_navigation/concorde_TSP.h

@@ -10,8 +10,8 @@
 #include <cstdlib>
 #include <stdio.h>
 
-#include <opencv/cv.h>
-#include <opencv/highgui.h>
+#include <opencv2/opencv.hpp>
+#include <opencv2/highgui/highgui.hpp>
 
 #include <fstream>
 
@@ -50,16 +50,17 @@ class ConcordeTSPSolver
 	AStarPlanner pathplanner_;
 
 	//Function to create neccessary TSPlib file to tell concorde what the problem is.
-	void writeToFile(const cv::Mat& pathlength_matrix);
+	void writeToFile(const cv::Mat& pathlength_matrix, const std::string& tsp_lib_filename, const std::string& tsp_order_filename);
 
 	//Function to read the saved TSP order.
-	std::vector<int> readFromFile();
+	std::vector<int> readFromFile(const std::string& tsp_order_filename);
 
 	void distance_matrix_thread(DistanceMatrix& distance_matrix_computation, cv::Mat& distance_matrix,
 			const cv::Mat& original_map, const std::vector<cv::Point>& points, double downsampling_factor,
 			double robot_radius, double map_resolution, AStarPlanner& path_planner);
 
 	bool abort_computation_;
+	std::string unique_file_identifier_;
 
 public:
 	//Constructor
@@ -76,7 +77,16 @@ class ConcordeTSPSolver
 	//with given distance matrix
 	std::vector<int> solveConcordeTSP(const cv::Mat& path_length_Matrix, const int start_Node);
 
-	//compute distance matrix and maybe return it
+	// compute distance matrix and maybe return it
+	// this version does not exclude infinite paths from the TSP ordering
 	std::vector<int> solveConcordeTSP(const cv::Mat& original_map, const std::vector<cv::Point>& points, double downsampling_factor,
 			double robot_radius, double map_resolution, const int start_Node, cv::Mat* distance_matrix = 0);
+
+	// compute TSP from a cleaned distance matrix (does not contain any infinity paths) that has to be computed
+	std::vector<int> solveConcordeTSPClean(const cv::Mat& original_map, const std::vector<cv::Point>& points,
+			double downsampling_factor, double robot_radius, double map_resolution, const int start_node);
+
+	// compute TSP with pre-computed cleaned distance matrix (does not contain any infinity paths)
+	std::vector<int> solveConcordeTSPWithCleanedDistanceMatrix(const cv::Mat& distance_matrix,
+			const std::map<int,int>& cleaned_index_to_original_index_mapping, const int start_node);
 };

ipa_building_navigation/common/include/ipa_building_navigation/contains.h

@@ -1,6 +1,6 @@
 #include "ros/ros.h"
-#include <opencv/cv.h>
-#include <opencv/highgui.h>
+#include <opencv2/opencv.hpp>
+#include <opencv2/highgui/highgui.hpp>
 #include <iostream>
 #include <list>
 #include <vector>