pointcloud_to_depthimage: added more error logs, removed output camer…

…a_info published twice and match ros1 namespaces
introlab · Nov 21, 2024 · 9ef8009 · 9ef8009
1 parent cb796a0
commit 9ef8009
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 7 deletions.
diff --git a/rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp b/rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp
@@ -57,8 +57,8 @@ class PointCloudToDepthImage : public rclcpp::Node
 			const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg);
 
 private:
-	image_transport::CameraPublisher depthImage16Pub_;
-	image_transport::CameraPublisher depthImage32Pub_;
+	image_transport::Publisher depthImage16Pub_;
+	image_transport::Publisher depthImage32Pub_;
 	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr cameraInfo16Pub_;
 	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr cameraInfo32Pub_;
 	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointCloudTransformedPub_;

diff --git a/rtabmap_util/src/PointCloudAssemblerNode.cpp b/rtabmap_util/src/PointCloudAssemblerNode.cpp
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 int main(int argc, char **argv)
 {
   ULogger::setType(ULogger::kTypeConsole);
+  ULogger::setLevel(ULogger::kWarning);
 	rclcpp::init(argc, argv);
 	rclcpp::spin(std::make_shared<rtabmap_util::PointCloudAssembler>(rclcpp::NodeOptions()));
 	rclcpp::shutdown();

diff --git a/rtabmap_util/src/PointCloudToDepthImageNode.cpp b/rtabmap_util/src/PointCloudToDepthImageNode.cpp
@@ -25,10 +25,13 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap/utilite/ULogger.h>
 #include "rtabmap_util/pointcloud_to_depthimage.hpp"
 
 int main(int argc, char **argv)
 {
+  ULogger::setType(ULogger::kTypeConsole);
+  ULogger::setLevel(ULogger::kWarning);
 	rclcpp::init(argc, argv);
 	rclcpp::spin(std::make_shared<rtabmap_util::PointCloudToDepthImage>(rclcpp::NodeOptions()));
 	rclcpp::shutdown();

diff --git a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp
@@ -60,7 +60,7 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
 	//tfBuffer_->setCreateTimerInterface(timer_interface);
 	tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
 
-	int topicQueueSize = 1;
+	int topicQueueSize = 10;
 	int syncQueueSize = 10;
 	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	bool approx = true;
@@ -107,8 +107,8 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
 	RCLCPP_INFO(this->get_logger(), "  decimation=%d", decimation_);
 	RCLCPP_INFO(this->get_logger(), "  upscale=%s (upscale_depth_error_ratio=%f)", upscale_?"true":"false", upscaleDepthErrorRatio_);
 
-	depthImage16Pub_ = image_transport::create_camera_publisher(this, "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm
-	depthImage32Pub_ = image_transport::create_camera_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters
+	depthImage16Pub_ = image_transport::create_publisher(this, "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm
+	depthImage32Pub_ = image_transport::create_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters
 	pointCloudTransformedPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud_transformed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
 	cameraInfo16Pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(depthImage16Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo));
 	cameraInfo32Pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(depthImage32Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo));
@@ -159,6 +159,10 @@ void PointCloudToDepthImage::callback(
 
 		if(cloudDisplacement.isNull())
 		{
+			RCLCPP_ERROR(this->get_logger(), "Could not find transform between %s and %s, accordingly to %s, aborting!",
+				pointCloud2Msg->header.frame_id.c_str(), 
+				cameraInfoMsg->header.frame_id.c_str(),
+				fixedFrameId_.c_str());
 			return;
 		}
 
@@ -171,6 +175,9 @@ void PointCloudToDepthImage::callback(
 
 		if(cloudToCamera.isNull())
 		{
+			RCLCPP_ERROR(this->get_logger(), "Could not find transform between %s and %s, aborting!",
+				pointCloud2Msg->header.frame_id.c_str(), 
+				cameraInfoMsg->header.frame_id.c_str());
 			return;
 		}
 		rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera;
@@ -239,7 +246,7 @@ void PointCloudToDepthImage::callback(
 		if(depthImage32Pub_.getNumSubscribers())
 		{
 			depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
-			depthImage32Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg);
+			depthImage32Pub_.publish(depthImage.toImageMsg());
 			if(cameraInfo32Pub_->get_subscription_count())
 			{
 				cameraInfo32Pub_->publish(cameraInfoMsgOut);
@@ -250,7 +257,7 @@ void PointCloudToDepthImage::callback(
 		{
 			depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
 			depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image);
-			depthImage16Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg);
+			depthImage16Pub_.publish(depthImage.toImageMsg());
 			if(cameraInfo16Pub_->get_subscription_count())
 			{
 				cameraInfo16Pub_->publish(cameraInfoMsgOut);