data_player: added --clock argument (like rosbag). Rtabmap: Detection rate update is done using only topic stamps (added warning if stamp is null). (#276)

CMakeLists.txt

@@ -18,8 +18,8 @@ endif (POLICY CMP0042)
 find_package(catkin REQUIRED COMPONENTS 
              cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs
              image_transport tf tf_conversions tf2_ros eigen_conversions laser_geometry pcl_conversions 
-             pcl_ros nodelet dynamic_reconfigure message_filters class_loader
-             genmsg stereo_msgs move_base_msgs image_geometry
+             pcl_ros nodelet dynamic_reconfigure message_filters class_loader rosgraph_msgs
+             genmsg stereo_msgs move_base_msgs image_geometry 
 )
 
 # Optional components
@@ -143,7 +143,7 @@ catkin_package(
   LIBRARIES rtabmap_ros
   CATKIN_DEPENDS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs
                  image_transport tf tf_conversions tf2_ros eigen_conversions laser_geometry pcl_conversions 
-                 pcl_ros nodelet dynamic_reconfigure message_filters class_loader
+                 pcl_ros nodelet dynamic_reconfigure message_filters class_loader rosgraph_msgs
                  stereo_msgs move_base_msgs image_geometry ${optional_dependencies}
   DEPENDS RTABMap OpenCV
 )

include/rtabmap_ros/CoreWrapper.h

@@ -288,7 +288,6 @@ private:
 	float rate_;
 	bool createIntermediateNodes_;
 	int maxMappingNodes_;
-	ros::Time time_;
 	ros::Time previousStamp_;
 };
 

launch/demo/demo_appearance_mapping.launch

@@ -27,7 +27,9 @@
 		<param name="Mem/STMSize"                    type="string" value="15"/>     <!-- 15 locations in short-term memory -->
 		<param name="Mem/RehearsalIdUpdatedToNewOne" type="string" value="true"/>   <!-- On merging, update to new ID-->
 		<param name="Mem/BadSignaturesIgnored"       type="string" value="true"/>
+		<param name="Mem/UseOdomFeatures"            type="string" value="false"/>
 		<param name="Kp/DetectorStrategy"            type="string" value="0"/>      <!-- use SURF -->
+		<param name="SURF/HessianThreshold"          type="string" value="100"/>
 		
 		<!-- localization mode -->
 	    <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>

package.xml

@@ -22,6 +22,7 @@
   <build_depend>stereo_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>visualization_msgs</build_depend>
+  <build_depend>rosgraph_msgs</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>tf_conversions</build_depend>
@@ -53,6 +54,7 @@
   <run_depend>stereo_msgs</run_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>visualization_msgs</run_depend>
+  <run_depend>rosgraph_msgs</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>compressed_depth_image_transport</run_depend>
   <run_depend>compressed_image_transport</run_depend>

src/CoreWrapper.cpp

@@ -114,7 +114,6 @@ CoreWrapper::CoreWrapper() :
 		rate_(Parameters::defaultRtabmapDetectionRate()),
 		createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
 		maxMappingNodes_(Parameters::defaultGridGlobalMaxNodes()),
-		time_(ros::Time::now()),
 		previousStamp_(0),
 		mbClient_("move_base", true)
 {
@@ -741,14 +740,21 @@ void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
 {
 	if(!paused_)
 	{
+		ros::Time stamp = imageMsg->header.stamp;
+		if(stamp.toSec() == 0.0)
+		{
+			ROS_WARN("A null stamp has been detected in the input topic. Make sure the stamp is set.");
+			return;
+		}
+
 		if(rate_>0.0f)
 		{
-			if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_))
+			if(previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_))
 			{
 				return;
 			}
 		}
-		time_ = ros::Time::now();
+		previousStamp_ = stamp;
 
 		if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 			 imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
@@ -894,10 +900,14 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Ti
 
 		// Throttle
 		bool ignoreFrame = false;
+		if(stamp.toSec() == 0.0)
+		{
+			ROS_WARN("A null stamp has been detected in the input topics. Make sure the stamp in all input topics is set.");
+			ignoreFrame = true;
+		}
 		if(rate_>0.0f)
 		{
-			if((previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_)) ||
-			   ((previousStamp_.toSec() <= 0.0 || stamp.toSec() <= previousStamp_.toSec()) && ros::Time::now() - time_ < ros::Duration(1.0f/rate_)))
+			if(previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_))
 			{
 				ignoreFrame = true;
 			}
@@ -915,7 +925,6 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Ti
 		}
 		else if(!ignoreFrame)
 		{
-			time_ = ros::Time::now();
 			previousStamp_ = stamp;
 		}
 
@@ -947,10 +956,14 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp)
 		lastPoseStamp_ = stamp;
 
 		bool ignoreFrame = false;
+		if(stamp.toSec() == 0.0)
+		{
+			ROS_WARN("A null stamp has been detected in the input topics. Make sure the stamp in all input topics is set.");
+			ignoreFrame = true;
+		}
 		if(rate_>0.0f)
 		{
-			if((previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_)) ||
-			   ((previousStamp_.toSec() <= 0.0 || stamp.toSec() <= previousStamp_.toSec()) && ros::Time::now() - time_ < ros::Duration(1.0f/rate_)))
+			if(previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_))
 			{
 				ignoreFrame = true;
 			}
@@ -968,7 +981,6 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp)
 		}
 		else if(!ignoreFrame)
 		{
-			time_ = ros::Time::now();
 			previousStamp_ = stamp;
 		}
 

src/DbPlayerNode.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/CameraInfo.h>
+#include <rosgraph_msgs/Clock.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include <nav_msgs/Odometry.h>
 #include <cv_bridge/cv_bridge.h>
@@ -86,6 +87,14 @@ int main(int argc, char** argv)
 	//ULogger::setLevel(ULogger::kDebug);
 	//ULogger::setEventLevel(ULogger::kWarning);
 
+	bool publishClock = false;
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--clock") == 0)
+		{
+			publishClock = true;
+		}
+	}
 
 	ros::NodeHandle nh;
 	ros::NodeHandle pnh("~");
@@ -98,6 +107,7 @@ int main(int argc, char** argv)
 	std::string databasePath = "";
 	bool publishTf = true;
 	int startId = 0;
+	bool useDbStamps = true;
 
 	pnh.param("frame_id", frameId, frameId);
 	pnh.param("odom_frame_id", odomFrameId, odomFrameId);
@@ -108,7 +118,7 @@ int main(int argc, char** argv)
 	pnh.param("publish_tf", publishTf, publishTf);
 	pnh.param("start_id", startId, startId);
 
-	// based on URG-04LX
+	// A general 360 lidar with 0.5 deg increment
 	double scanAngleMin, scanAngleMax, scanAngleIncrement, scanTime, scanRangeMin, scanRangeMax;
 	pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI);
 	pnh.param<double>("scan_angle_max", scanAngleMax, M_PI);
@@ -124,6 +134,7 @@ int main(int argc, char** argv)
 	ROS_INFO("rate = %f", rate);
 	ROS_INFO("publish_tf = %s", publishTf?"true":"false");
 	ROS_INFO("start_id = %d", startId);
+	ROS_INFO("Publish clock (--clock): %s", publishClock?"true":"false");
 
 	if(databasePath.empty())
 	{
@@ -159,8 +170,14 @@ int main(int argc, char** argv)
 	ros::Publisher rightCamInfoPub;
 	ros::Publisher odometryPub;
 	ros::Publisher scanPub;
+	ros::Publisher clockPub;
 	tf2_ros::TransformBroadcaster tfBroadcaster;
 
+	if(publishClock)
+	{
+		clockPub = nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
+	}
+
 	UTimer timer;
 	rtabmap::CameraInfo cameraInfo;
 	rtabmap::SensorData data = reader.takeImage(&cameraInfo);
@@ -172,7 +189,14 @@ int main(int argc, char** argv)
 	{
 		ROS_INFO("Reading sensor data %d...", odom.data().id());
 
-		ros::Time time = ros::Time::now();
+		ros::Time time(odom.data().stamp());
+
+		if(publishClock)
+		{
+			rosgraph_msgs::Clock msg;
+			msg.clock = time;
+			clockPub.publish(msg);
+		}
 
 		sensor_msgs::CameraInfo camInfoA; //rgb or left
 		sensor_msgs::CameraInfo camInfoB; //depth or right
@@ -263,7 +287,16 @@ int main(int argc, char** argv)
 
 		if(!odom.data().laserScanRaw().isEmpty())
 		{
-			if(scanPub.getTopic().empty()) scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
+			if(scanPub.getTopic().empty())
+			{
+				scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
+				ROS_INFO("Scan will be published with those parameters:");
+				ROS_INFO("  scan_angle_min=%f", scanAngleMin);
+				ROS_INFO("  scan_angle_max=%f", scanAngleMax);
+				ROS_INFO("  scan_angle_increment=%f", scanAngleIncrement);
+				ROS_INFO("  scan_range_min=%f", scanRangeMin);
+				ROS_INFO("  scan_range_max=%f", scanRangeMax);
+			}
 		}
 
 		// publish transforms first