ros-pkg: Navigation: fixed local costmap point cloud observation source, independent of Z value. Added data_player node to replay stuff saved in RTAB-Map databases (like a rosbag but for rtabmap.db).

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1945 f169173b-cf89-36c8-b27e-44dbe73f0c83

CMakeLists.txt

@@ -176,6 +176,9 @@ target_link_libraries(rtabmapviz rtabmap_ros ${QT_LIBRARIES} ${Libraries})
 add_executable(data_recorder src/DataRecorderNode.cpp)
 target_link_libraries(data_recorder rtabmap_ros ${QT_LIBRARIES} ${Libraries})
 
+add_executable(data_player src/DbPlayerNode.cpp)
+target_link_libraries(data_player rtabmap_ros ${QT_LIBRARIES} ${Libraries})
+
 #############
 ## Install ##
 #############

launch/azimut3/az3_mapping_robot_stereo_nav.launch

@@ -99,7 +99,7 @@
 		</node>
 		<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap/obstacles_detection stereo_nodelet">
     			<remap from="cloud" to="cloudXYZ"/>
-			<remap from="obstacles_2d" to="/planner_cloud"/>
+			<remap from="obstacles" to="/planner_cloud"/>
 
 			<param name="frame_id" type="string" value="base_footprint"/>
 			<param name="map_frame_id" type="string" value="map"/>

launch/config/azimut3/costmap_common_params.yaml

@@ -16,13 +16,16 @@ laser_scan_sensor: {
   marking: true, 
   clearing: true}
 
+# assuming receiving a cloud from rtabmap/obstacles_detection node
 point_cloud_sensor: {
   sensor_frame: base_footprint,
   data_type: PointCloud2, 
   topic: openni_points, 
   expected_update_rate: 0.5, 
   marking: true, 
-  clearing: true}
+  clearing: true,
+  min_obstacle_height: -99999.0,
+  max_obstacle_height: 99999.0}
 
 controller_patience: 2.0
 

src/DbPlayerNode.cpp

@@ -0,0 +1,361 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <nav_msgs/Odometry.h>
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <std_srvs/Empty.h>
+#include <rtabmap/MsgConversion.h>
+#include <rtabmap/utilite/ULogger.h>
+
+#include <rtabmap/core/DBReader.h>
+
+bool paused = false;
+bool pauseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	if(paused)
+	{
+		ROS_WARN("Already paused!");
+	}
+	else
+	{
+		paused = true;
+		ROS_INFO("paused!");
+	}
+	return true;
+}
+
+bool resumeCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	if(!paused)
+	{
+		ROS_WARN("Already running!");
+	}
+	else
+	{
+		paused = false;
+		ROS_INFO("resumed!");
+	}
+	return true;
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "data_player");
+
+	//ULogger::setType(ULogger::kTypeConsole);
+	//ULogger::setLevel(ULogger::kDebug);
+	//ULogger::setEventLevel(ULogger::kWarning);
+
+
+	ros::NodeHandle nh;
+	ros::NodeHandle pnh("~");
+
+	std::string frameId = "base_link";
+	std::string odomFrameId = "odom";
+	std::string cameraFrameId = "camera_optical_link";
+	double rate = 1.0f;
+	std::string databasePath = "";
+	bool publishTf = true;
+	int startId = 0;
+
+	pnh.param("frame_id", frameId, frameId);
+	pnh.param("odom_frame_id", odomFrameId, odomFrameId);
+	pnh.param("camera_frame_id", cameraFrameId, cameraFrameId);
+	pnh.param("rate", rate, rate);
+	pnh.param("database", databasePath, databasePath);
+	pnh.param("publish_tf", publishTf, publishTf);
+	pnh.param("start_id", startId, startId);
+
+	ROS_INFO("frame_id = %s", frameId.c_str());
+	ROS_INFO("odom_frame_id = %s", odomFrameId.c_str());
+	ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str());
+	ROS_INFO("database = %s", databasePath.c_str());
+	ROS_INFO("rate = %f", rate);
+	ROS_INFO("publish_tf = %s", publishTf?"true":"false");
+
+	rtabmap::DBReader reader(databasePath, rate);
+
+	if(databasePath.empty())
+	{
+		ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database).");
+		return -1;
+	}
+
+	if(!reader.init(startId))
+	{
+		ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str());
+		return -1;
+	}
+
+	ros::ServiceServer pauseSrv = pnh.advertiseService("pause", pauseCallback);
+	ros::ServiceServer resumeSrv = pnh.advertiseService("resume", resumeCallback);
+
+	image_transport::ImageTransport it(nh);
+	image_transport::Publisher imagePub;
+	image_transport::Publisher rgbPub;
+	image_transport::Publisher depthPub;
+	image_transport::Publisher leftPub;
+	image_transport::Publisher rightPub;
+	ros::Publisher rgbCamInfoPub;
+	ros::Publisher depthCamInfoPub;
+	ros::Publisher leftCamInfoPub;
+	ros::Publisher rightCamInfoPub;
+	ros::Publisher odometryPub;
+	tf::TransformBroadcaster tfBroadcaster;
+	ros::Publisher cloudPub;
+
+	cv::Mat image, depth, depth2d;
+	float fx,fy,cx,cy;
+	rtabmap::Transform localTransform, pose;
+	int seq = 0;
+
+	reader.getNextImage(image, depth, depth2d, fx, fy, cx, cy, localTransform, pose, seq);
+	while(ros::ok() && !image.empty())
+	{
+		ROS_INFO("Reading image %d...", seq);
+
+		ros::Time time = ros::Time::now();
+
+		sensor_msgs::CameraInfo camInfoA; //rgb or left
+		sensor_msgs::CameraInfo camInfoB; //depth or right
+
+		camInfoA.K.assign(0);
+		camInfoA.K[0] = camInfoA.K[4] = camInfoA.K[8] = 1;
+		camInfoA.R.assign(0);
+		camInfoA.R[0] = camInfoA.R[4] = camInfoA.R[8] = 1;
+		camInfoA.P.assign(0);
+		camInfoA.P[10] = 1;
+
+		camInfoA.header.frame_id = cameraFrameId;
+		camInfoA.header.stamp = time;
+
+		camInfoB = camInfoA;
+
+		int type = -1;
+		if(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1))
+		{
+			//depth
+			camInfoA.D.resize(5,0);
+
+			camInfoA.P[0] = fx;
+			camInfoA.K[0] = fx;
+			camInfoA.P[5] = fy;
+			camInfoA.K[4] = fy;
+			camInfoA.P[2] = cx;
+			camInfoA.K[2] = cx;
+			camInfoA.P[6] = cy;
+			camInfoA.K[5] = cy;
+
+			camInfoB = camInfoA;
+
+			type=0;
+
+			if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1);
+			if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1);
+			if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
+			if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
+		}
+		else if(!depth.empty() && depth.type() == CV_8U)
+		{
+			//stereo
+			camInfoA.D.resize(8,0);
+
+			camInfoA.P[0] = fx;
+			camInfoA.K[0] = fx;
+			camInfoA.P[5] = fx; // fx = fy
+			camInfoA.K[4] = fx; // fx = fy
+			camInfoA.P[2] = cx;
+			camInfoA.K[2] = cx;
+			camInfoA.P[6] = cy;
+			camInfoA.K[5] = cy;
+
+			camInfoB = camInfoA;
+			camInfoB.P[3] = fy*-fx; // Right_Tx = -baseline*fx
+
+			type=1;
+
+			if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
+			if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
+			if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
+			if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
+
+		}
+		else
+		{
+			if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
+		}
+
+		camInfoA.height = image.rows;
+		camInfoA.width = image.cols;
+		camInfoB.height = depth.rows;
+		camInfoB.width = depth.cols;
+
+		// publish transforms first
+		if(publishTf)
+		{
+			ros::Time tfExpiration = time + ros::Duration(1.0/rate);
+			if(!localTransform.isNull())
+			{
+				tf::Transform baseToCamera;
+				rtabmap::transformToTF(localTransform, baseToCamera);
+				tfBroadcaster.sendTransform( tf::StampedTransform (baseToCamera, tfExpiration, frameId, cameraFrameId));
+			}
+
+			if(!pose.isNull())
+			{
+				tf::Transform odomToBase;
+				rtabmap::transformToTF(pose, odomToBase);
+				tfBroadcaster.sendTransform( tf::StampedTransform (odomToBase, tfExpiration, odomFrameId, frameId));
+			}
+		}
+		if(!pose.isNull())
+		{
+			if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);
+
+			if(odometryPub.getNumSubscribers())
+			{
+				nav_msgs::Odometry odom;
+				odom.child_frame_id = frameId;
+				odom.header.frame_id = odomFrameId;
+				odom.header.stamp = time;
+				rtabmap::transformToPoseMsg(pose, odom.pose.pose);
+				odometryPub.publish(odom);
+			}
+		}
+
+		if(type >= 0)
+		{
+			if(rgbCamInfoPub.getNumSubscribers() && type == 0)
+			{
+				rgbCamInfoPub.publish(camInfoA);
+			}
+			if(leftCamInfoPub.getNumSubscribers() && type == 1)
+			{
+				leftCamInfoPub.publish(camInfoA);
+			}
+			if(depthCamInfoPub.getNumSubscribers() && type == 0)
+			{
+				depthCamInfoPub.publish(camInfoB);
+			}
+			if(rightCamInfoPub.getNumSubscribers() && type == 1)
+			{
+				rightCamInfoPub.publish(camInfoB);
+			}
+		}
+
+		if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
+		{
+			cv_bridge::CvImage img;
+			if(image.channels() == 1)
+			{
+				img.encoding = sensor_msgs::image_encodings::MONO8;
+			}
+			else
+			{
+				img.encoding = sensor_msgs::image_encodings::BGR8;
+			}
+			img.image = image;
+			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
+			imageRosMsg->header.frame_id = cameraFrameId;
+			imageRosMsg->header.stamp = time;
+
+			if(imagePub.getNumSubscribers())
+			{
+				imagePub.publish(imageRosMsg);
+			}
+			if(rgbPub.getNumSubscribers() && type == 0)
+			{
+				rgbPub.publish(imageRosMsg);
+			}
+			if(leftPub.getNumSubscribers() && type == 1)
+			{
+				leftPub.publish(imageRosMsg);
+				leftCamInfoPub.publish(camInfoA);
+			}
+		}
+
+		if(depthPub.getNumSubscribers() && !depth.empty() && type==0)
+		{
+			cv_bridge::CvImage img;
+			if(depth.type() == CV_32FC1)
+			{
+				img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
+			}
+			else
+			{
+				img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
+			}
+			img.image = depth;
+			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
+			imageRosMsg->header.frame_id = cameraFrameId;
+			imageRosMsg->header.stamp = time;
+
+			depthPub.publish(imageRosMsg);
+			depthCamInfoPub.publish(camInfoB);
+		}
+
+		if(rightPub.getNumSubscribers() && !depth.empty() && type==1)
+		{
+			cv_bridge::CvImage img;
+			img.encoding = sensor_msgs::image_encodings::MONO8;
+			img.image = depth;
+			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
+			imageRosMsg->header.frame_id = cameraFrameId;
+			imageRosMsg->header.stamp = time;
+
+			rightPub.publish(imageRosMsg);
+			rightCamInfoPub.publish(camInfoB);
+		}
+
+		ros::spinOnce();
+
+		while(ros::ok() && paused)
+		{
+			uSleep(100);
+			ros::spinOnce();
+		}
+
+		image = cv::Mat();
+		depth = cv::Mat();
+		depth2d = cv::Mat();
+		fx=fy=cx=cy=seq=0;
+		pose.setNull();
+		localTransform.setNull();
+		reader.getNextImage(image, depth, depth2d, fx, fy, cx, cy, localTransform, pose, seq);
+	}
+
+
+	return 0;
+}

src/GridMapAssemblerNode.cpp

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UStl.h>
 #include <nav_msgs/OccupancyGrid.h>
 #include <nav_msgs/GetMap.h>
+#include <std_srvs/Empty.h>
 #include <pcl_ros/transforms.h>
 #include <pcl_conversions/pcl_conversions.h>
 
@@ -60,7 +61,10 @@ public:
 		mapDataTopic_ = nh.subscribe("mapData", 1, &GridMapAssembler::mapDataReceivedCallback, this);
 
 		gridMap_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1);
-		getMapService_ = nh.advertiseService("get_grid_map", &GridMapAssembler::getGridMapCallback, this);
+
+		//private service
+		getMapService_ = pnh.advertiseService("get_map", &GridMapAssembler::getGridMapCallback, this);
+		resetService_ = pnh.advertiseService("reset", &GridMapAssembler::reset, this);
 	}
 
 	~GridMapAssembler()
@@ -133,6 +137,14 @@ public:
 		return false;
 	}
 
+	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+	{
+		ROS_INFO("grid_map_assembler: reset!");
+		scans_.clear();
+		map_ = nav_msgs::OccupancyGrid();
+		return true;
+	}
+
 private:
 	double gridCellSize_;
 	bool gridUnknownSpaceFilled_;
@@ -144,6 +156,7 @@ private:
 	ros::Publisher gridMap_;
 
 	ros::ServiceServer getMapService_;
+	ros::ServiceServer resetService_;
 
 	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
 

src/MapAssemblerNode.cpp

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl_ros/transforms.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include <nav_msgs/OccupancyGrid.h>
+#include <std_srvs/Empty.h>
 
 using namespace rtabmap;
 
@@ -84,6 +85,9 @@ public:
 		{
 			occupancyMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_projection_map", 1);
 		}
+
+		// private service
+		resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this);
 	}
 
 	~MapAssembler()
@@ -280,6 +284,15 @@ public:
 		}
 	}
 
+	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+	{
+		ROS_INFO("map_assembler: reset!");
+		occupancyLocalMaps_.clear();
+		rgbClouds_.clear();
+		scans_.clear();
+		return true;
+	}
+
 private:
 	int cloudDecimation_;
 	double cloudMaxDepth_;
@@ -304,6 +317,8 @@ private:
 	ros::Publisher assembledMapScans_;
 	ros::Publisher occupancyMapPub_;
 
+	ros::ServiceServer resetService_;
+
 	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > rgbClouds_;
 	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
 };

src/nodelets/obstacles_detection.cpp

@@ -65,7 +65,6 @@ class ObstaclesDetection : public nodelet::Nodelet
 public:
 	ObstaclesDetection() :
 		frameId_("base_link"),
-		mapFrameId_("odom"),
 		normalEstimationRadius_(0.05),
 		groundNormalAngle_(M_PI_4),
 		minClusterSize_(20),
@@ -84,7 +83,6 @@ private:
 		int queueSize = 10;
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("frame_id", frameId_, frameId_);
-		pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
 		pnh.param("normal_estimation_radius", normalEstimationRadius_, normalEstimationRadius_);
 		pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_);
 		pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
@@ -94,16 +92,15 @@ private:
 
 		groundPub_ = nh.advertise<sensor_msgs::PointCloud2>("ground", 1);
 		obstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles", 1);
-		obstaclesToMap2dPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles_2d", 1);
 	}
 
 
 
 	void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
 	{
-		if(groundPub_.getNumSubscribers() || obstaclesPub_.getNumSubscribers() || obstaclesToMap2dPub_.getNumSubscribers())
+		if(groundPub_.getNumSubscribers() || obstaclesPub_.getNumSubscribers())
 		{
-			Transform localTransform, fixedToBaseTransform;
+			Transform localTransform;
 			try
 			{
 				tf::StampedTransform tmp;
@@ -114,17 +111,6 @@ private:
 				}
 				tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
 				localTransform = transformFromTF(tmp);
-
-				if(obstaclesToMap2dPub_.getNumSubscribers())
-				{
-					if(!tfListener_.waitForTransform(mapFrameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
-					{
-						ROS_WARN("Could not get transform from %s to %s after 1 second!", mapFrameId_.c_str(), cloudMsg->header.frame_id.c_str());
-						return;
-					}
-					tfListener_.lookupTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, tmp);
-					fixedToBaseTransform = transformFromTF(tmp);
-				}
 			}
 			catch(tf::TransformException & ex)
 			{
@@ -153,19 +139,9 @@ private:
 				pcl::copyPointCloud(*cloud, *ground, *groundCloud);
 			}
 			pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
-			pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesToMap2dCloud(new pcl::PointCloud<pcl::PointXYZ>);
-			if((obstaclesPub_.getNumSubscribers() || obstaclesToMap2dPub_.getNumSubscribers()) && obstacles->size())
+			if(obstaclesPub_.getNumSubscribers() && obstacles->size())
 			{
 				pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
-
-				if(obstaclesToMap2dPub_.getNumSubscribers())
-				{
-					//force 2d
-					float x,y,z, roll,pitch,yaw;
-					fixedToBaseTransform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-					Transform transform2d(x,y, 0, 0,0,yaw);
-					obstaclesToMap2dCloud = util3d::transformPointCloud<pcl::PointXYZ>(obstaclesCloud, transform2d);
-				}
 			}
 
 			if(groundPub_.getNumSubscribers())
@@ -189,22 +165,11 @@ private:
 				//publish the message
 				obstaclesPub_.publish(rosCloud);
 			}
-			if(obstaclesToMap2dPub_.getNumSubscribers())
-			{
-				sensor_msgs::PointCloud2 rosCloud;
-				pcl::toROSMsg(*obstaclesToMap2dCloud, rosCloud);
-				rosCloud.header.stamp = cloudMsg->header.stamp;
-				rosCloud.header.frame_id = mapFrameId_;
-
-				//publish the message
-				obstaclesToMap2dPub_.publish(rosCloud);
-			}
 		}
 	}
 
 private:
 	std::string frameId_;
-	std::string mapFrameId_;
 	double normalEstimationRadius_;
 	double groundNormalAngle_;
 	int minClusterSize_;
@@ -214,7 +179,6 @@ private:
 
 	ros::Publisher groundPub_;
 	ros::Publisher obstaclesPub_;
-	ros::Publisher obstaclesToMap2dPub_;
 
 	ros::Subscriber cloudSub_;
 };