Added assert on PointCloud2 format (#560). rtabmap.launch: added gdb option for convenience.

src/MsgConversion.cpp

@@ -2149,6 +2149,8 @@ bool convertScan3dMsg(
 		int maxPoints,
 		float maxRange)
 {
+	UASSERT(scan3dMsg.data.size() == scan3dMsg.row_step*scan3dMsg.height);
+
 	bool hasNormals = false;
 	bool hasColors = false;
 	bool hasIntensity = false;

src/OdometryROS.cpp

@@ -507,7 +507,8 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
 	{
 		if(previousStamp_>0.0 && previousStamp_ >= header.stamp.toSec())
 		{
-			NODELET_WARN("Odometry: Detected not valid consecutive stamps (previous=%fs new=%fs). New stamp should be always greater than previous stamp. This new data is ignored. This message will appear only once.",
+			NODELET_WARN("Odometry: Detected not valid consecutive stamps (previous=%fs new=%fs). "
+					"New stamp should be always greater than previous stamp. This new data is ignored.",
 					previousStamp_, header.stamp.toSec());
 			return;
 		}

src/nodelets/icp_odometry.cpp

@@ -449,6 +449,8 @@ private:
 
 	void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg)
 	{
+		UASSERT(pointCloudMsg->data.size() == pointCloudMsg->row_step*pointCloudMsg->height);
+		
 		if(scanReceived_)
 		{
 			ROS_ERROR("%s is already receiving scans on \"%s\", but also "

src/nodelets/rgbdicp_odometry.cpp

@@ -343,6 +343,8 @@ private:
 				}
 				else if(cloudMsg.get() != 0)
 				{
+					UASSERT(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height);
+
 					bool containNormals = false;
 					if(scanVoxelSize_ == 0.0f)
 					{