i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

MsgConversion/laser: check if intensity field has the supported datatype before converting. PointCloud2Depth: added {cloud}_transformed output topic (with same stamp and frame_id than the depth image).

Browse Source
This commit is contained in:
matlabbe
2020-03-24 17:40:06 -04:00
parent aec4fe6bcc
commit aced8a8c93
2 changed files with 40 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+27 -2
View File
@@ -1788,10 +1788,21 @@ bool convertScanMsg(
bool containIntensity = false;
for(unsigned int i=0; i<scanOut.fields.size(); ++i)
{
if(scanOut.fields[i].name.compare("intensity") == 0)
if(scanOut.fields[i].datatype == sensor_msgs::PointField::FLOAT32)
{
containIntensity = true;
}
else
{
static bool warningShown = false;
if(!warningShown)
{
ROS_WARN("The input scan cloud has an \"intensity\" field "
"but the datatype (%d) is not supported. Intensity will be ignored. "
"This message is only shown once.", scanOut.fields[i].datatype);
warningShown = true;
}
}
}
rtabmap::LaserScan::Format format;
@@ -1860,7 +1871,21 @@ bool convertScan3dMsg(
}
if(scan3dMsg->fields[i].name.compare("intensity") == 0)
{
containIntensity = true;
if(scan3dMsg->fields[i].datatype == sensor_msgs::PointField::FLOAT32)
{
containIntensity = true;
}
else
{
static bool warningShown = false;
if(!warningShown)
{
ROS_WARN("The input scan cloud has an \"intensity\" field "
"but the datatype (%d) is not supported. Intensity will be ignored. "
"This message is only shown once.", scan3dMsg->fields[i].datatype);
warningShown = true;
}
}
}
}
src/nodelets/pointcloud_to_depthimage.cpp
+13 -2
View File
@@ -46,6 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
@@ -121,6 +122,7 @@ private:
image_transport::ImageTransport it(nh);
depthImage16Pub_ = it.advertise("image_raw", 1); // 16 bits unsigned in mm
depthImage32Pub_ = it.advertise("image", 1); // 32 bits float in meters
pointCloudTransformedPub_ = nh.advertise<sensor_msgs::PointCloud2>(nh.resolveName("cloud")+"_transformed", 1);
if(approx)
{
@@ -154,8 +156,8 @@ private:
cloudDisplacement = rtabmap_ros::getTransform(
pointCloud2Msg->header.frame_id,
fixedFrameId_,
pointCloud2Msg->header.stamp,
cameraInfoMsg->header.stamp,
pointCloud2Msg->header.stamp,
*listener_,
waitForTransform_);
}
@@ -177,7 +179,7 @@ private:
return;
}
rtabmap::Transform localTransform = cloudDisplacement.inverse()*cloudToCamera;
rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera;
rtabmap::CameraModel model = rtabmap_ros::cameraModelFromROS(*cameraInfoMsg, localTransform);
@@ -217,6 +219,14 @@ private:
depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
}
}
if(pointCloudTransformedPub_.getNumSubscribers()>0)
{
sensor_msgs::PointCloud2 pointCloud2Out;
pcl_ros::transformPointCloud(model.localTransform().inverse().toEigen4f(), *pointCloud2Msg, pointCloud2Out);
pointCloud2Out.header = cameraInfoMsg->header;
pointCloudTransformedPub_.publish(pointCloud2Out);
}
}
depthImage.header = cameraInfoMsg->header;
@@ -250,6 +260,7 @@ private:
private:
image_transport::Publisher depthImage16Pub_;
image_transport::Publisher depthImage32Pub_;
ros::Publisher pointCloudTransformedPub_;
message_filters::Subscriber<sensor_msgs::PointCloud2> pointCloudSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
std::string fixedFrameId_;