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 @@
-
+
-
+
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 @@
-
+
@@ -46,7 +46,9 @@
-
+
+
+
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)
{