From 866459f093e337fb64875c5fffce9b3e195446b1 Mon Sep 17 00:00:00 2001
From: David-Michael Buckman <david.b@smartmachine.nz>
Date: Tue, 29 Mar 2022 09:09:20 +1300
Subject: [PATCH] Update documentation to include default values.

---
 README.md                              |  6 ++++--
 include/transform_pointcloud_nodelet.h | 10 ++++++++-
 src/transform_pointcloud_nodelet.cpp   | 29 +++++++++++++++-----------
 3 files changed, 30 insertions(+), 15 deletions(-)

diff --git a/README.md b/README.md
index 2e0da61..8c9b048 100644
--- a/README.md
+++ b/README.md
@@ -3,7 +3,7 @@ ROS nodelet that transforms pointcloud (`sensor_msgs/Pointcloud` or `sensor_msgs
 TF frame (e.g. from "camera_frame" to "base_link"). It uses `sensor_msgs/point_cloud_conversion.h` for conversion.
 It will publish both `Pointcloud` and `Pointcloud2` messages for any input type (if any node is subscribed to it)
 # Subscribed topics
-* `~input_pcl (sensor_msgs/Pointcloud)` input topic
+* `~input_pcl (sensor_msgs/Pointcloud)` input topic 
 * `~inptu_pcl2 (sensor_msgs/Pointcloud2)` input topic
 
 # Published topics
@@ -11,7 +11,9 @@ It will publish both `Pointcloud` and `Pointcloud2` messages for any input type
 * `~output_pcl2 (sensor_msgs/Pointcloud2)` output topic
 
 # Parameters
-* `to_frame (string, default:base_link)` to what TF frame to transform the output
+* `to_frame (string, default:base_link)` To what TF frame to transform the output
+* `transform_timeout (double, default: 0.5 seconds)` How long to block before failing
+* `polling_timeout (double, default: 0.1 seconds)` How often to retest if failed
 
 # TF
 * required is the transform from frame that input pointcloud is to frame set by `to_frame` parameter
diff --git a/include/transform_pointcloud_nodelet.h b/include/transform_pointcloud_nodelet.h
index 5441b7a..b42ba13 100644
--- a/include/transform_pointcloud_nodelet.h
+++ b/include/transform_pointcloud_nodelet.h
@@ -40,6 +40,7 @@ namespace transform_pointcloud
 	public:
 		transformPointcloud() = default;
         ~transformPointcloud() = default;
+
     private:
         void onInit() override;
 
@@ -50,12 +51,19 @@ namespace transform_pointcloud
         // Publishers.
 		ros::Publisher PointCloudPublisher;
 		ros::Publisher PointCloud2Publisher;
+
         // Subscribers.
 		ros::Subscriber PointCloudSubscriber;
 		ros::Subscriber PointCloud2Subscriber;
 
-		std::string ReferenceFrame = "base_link";
+        // Parameters.
+		std::string ReferenceFrame;
         ros::Duration TransformTimeout;
         ros::Duration PollingTimeout;
+
+        // Defaults.
+        const std::string DEFAULT_REFERENCE_FRAME = "base_link";
+        static constexpr double DEFAULT_TRANSFORM_TIMEOUT = 0.4;
+        static constexpr double DEFAULT_POLLING_TIMEOUT = 0.1;
     };
 }
\ No newline at end of file
diff --git a/src/transform_pointcloud_nodelet.cpp b/src/transform_pointcloud_nodelet.cpp
index 6d5616d..3473499 100644
--- a/src/transform_pointcloud_nodelet.cpp
+++ b/src/transform_pointcloud_nodelet.cpp
@@ -8,21 +8,17 @@ void transformPointcloud::onInit()
 	ros::NodeHandle nhp = getPrivateNodeHandle();
 	
 	// Get the frame ID.
-	std::string reference_frame;
-	if (!nhp.getParam("to_frame", reference_frame))
-	{
-		ROS_ERROR("You must specify 'to_frame' parameter !");
-		exit(1);
-	}
+	std::string reference_frame = DEFAULT_REFERENCE_FRAME;
+	nhp.getParam("to_frame", reference_frame);
 	ReferenceFrame = reference_frame;
 
 	// Get the transform timeout.
-	double transform_timeout = 0.02;
+	double transform_timeout = DEFAULT_TRANSFORM_TIMEOUT;
 	nhp.getParam("transform_timeout", transform_timeout);
 	TransformTimeout = ros::Duration(transform_timeout);
 
 	// Get the polling timeout.
-	double polling_timeout = 0.02;
+	double polling_timeout = DEFAULT_POLLING_TIMEOUT;
 	nhp.getParam("polling_timeout", polling_timeout);
 	PollingTimeout = ros::Duration(polling_timeout);
 
@@ -66,8 +62,7 @@ bool transformPointcloud::getTransform(
 	}
 	catch (const tf::TransformException &e)
 	{
-		ROS_ERROR_STREAM(
-			"Pointcloud transform | Error in lookupTransform of " << child_frame << " in " << reference_frame);
+		ROS_ERROR_STREAM("Pointcloud transform | Error in lookupTransform of " << child_frame << " in " << reference_frame);
 		return false;
 	}
 	return true;
@@ -83,7 +78,12 @@ void transformPointcloud::pointcloudCB(const sensor_msgs::PointCloud::ConstPtr &
 
 	// Get the transform.
 	tf::StampedTransform tf_between_frames;
-	getTransform(ReferenceFrame, cloud_in->header.frame_id, tf_between_frames);
+	const bool transform_retrieved = getTransform(ReferenceFrame, cloud_in->header.frame_id, tf_between_frames);
+    if (!transform_retrieved)
+    {
+        // Return early since there wasn't a transform for the given frames.
+        return;
+    }
 
 	// Convert it to geometry_msg type.
 	geometry_msgs::TransformStamped tf_between_frames_geo;
@@ -126,7 +126,12 @@ void transformPointcloud::pointcloud2CB(const sensor_msgs::PointCloud2::ConstPtr
 
 	// Get the transform.
 	tf::StampedTransform tf_between_frames;
-	getTransform(ReferenceFrame, cloud_in->header.frame_id, tf_between_frames);
+	const bool transform_retrieved = getTransform(ReferenceFrame, cloud_in->header.frame_id, tf_between_frames);
+    if (!transform_retrieved)
+    {
+        // Return early since there wasn't a transform for the given frames.
+        return;
+    }
 
 	// Convert it to geometry_msg type.
 	geometry_msgs::TransformStamped tf_between_frames_geo;