From b9c7efdc8ae993e2b780615d506a3ab28984cfd0 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sat, 27 Mar 2021 21:51:06 -0400
Subject: [PATCH] Added assert on PointCloud2 format (#560). rtabmap.launch:
 added gdb option for convenience.

---
 launch/config/husky_velodyne_extra.urdf.xacro | 4 ++--
 launch/rtabmap.launch                         | 6 ++++--
 src/MsgConversion.cpp                         | 2 ++
 src/OdometryROS.cpp                           | 3 ++-
 src/nodelets/icp_odometry.cpp                 | 2 ++
 src/nodelets/rgbdicp_odometry.cpp             | 2 ++
 6 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/launch/config/husky_velodyne_extra.urdf.xacro b/launch/config/husky_velodyne_extra.urdf.xacro
index 08edc973a..8c9687953 100644
--- a/launch/config/husky_velodyne_extra.urdf.xacro
+++ b/launch/config/husky_velodyne_extra.urdf.xacro
@@ -2,8 +2,8 @@
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
 
   <xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
-  <VLP-16 parent="base_laser_mount" name="velodyne" topic="velodyne_points" gpu="true">
+  <xacro:VLP-16 parent="base_laser_mount" name="velodyne" topic="velodyne_points" gpu="true" organize_cloud="true">
     <origin xyz="0.025 0 0.175" rpy="0 0 0" />
-  </VLP-16>
+  </xacro:VLP-16>
   
 </robot>
diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch
index fdeadfc70..2a1e39e0e 100644
--- a/launch/rtabmap.launch
+++ b/launch/rtabmap.launch
@@ -1,4 +1,4 @@
-
+<!-- -->
 <launch>
   <!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->
 
@@ -46,7 +46,9 @@
   <arg name="wait_for_transform"      default="0.2"/>
   <arg name="args"                    default=""/>              <!-- delete_db_on_start, udebug -->
   <arg name="rtabmap_args"            default="$(arg args)"/>   <!-- deprecated, use "args" argument -->
-  <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
+  <arg name="gdb"                     default="false"/>         <!-- Launch nodes in gdb for debugging (apt install xterm gdb) -->
+  <arg     if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/>
+  <arg unless="$(arg gdb)" name="launch_prefix" default=""/>
   <arg name="output"                  default="screen"/>        <!-- Control node output (screen or log) -->
   <arg name="publish_tf_map"          default="true"/>
 
diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp
index 9362e03aa..68dffd2d6 100644
--- a/src/MsgConversion.cpp
+++ b/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;
diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp
index d4ec56743..64f2430ba 100644
--- a/src/OdometryROS.cpp
+++ b/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;
 		}
diff --git a/src/nodelets/icp_odometry.cpp b/src/nodelets/icp_odometry.cpp
index b10da78a2..46195fdb6 100644
--- a/src/nodelets/icp_odometry.cpp
+++ b/src/nodelets/icp_odometry.cpp
@@ -449,6 +449,8 @@ class ICPOdometry : public rtabmap_ros::OdometryROS
 
 	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 "
diff --git a/src/nodelets/rgbdicp_odometry.cpp b/src/nodelets/rgbdicp_odometry.cpp
index ed78aa1c2..2fbba0ee9 100644
--- a/src/nodelets/rgbdicp_odometry.cpp
+++ b/src/nodelets/rgbdicp_odometry.cpp
@@ -343,6 +343,8 @@ class RGBDICPOdometry : public rtabmap_ros::OdometryROS
 				}
 				else if(cloudMsg.get() != 0)
 				{
+					UASSERT(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height);
+
 					bool containNormals = false;
 					if(scanVoxelSize_ == 0.0f)
 					{