Added assert on PointCloud2 format (#560). rtabmap.launch: added gdb …

…option for convenience.
introlab · Mar 28, 2021 · b9c7efd · b9c7efd
1 parent e7dd6ca
commit b9c7efd
Show file tree

Hide file tree

Showing 6 changed files with 14 additions and 5 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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)
 					{