Skip to content

Commit

Permalink
Update documentation to include default values.
Browse files Browse the repository at this point in the history
  • Loading branch information
dmbuck32 committed Mar 28, 2022
1 parent 1b219ca commit 866459f
Show file tree
Hide file tree
Showing 3 changed files with 30 additions and 15 deletions.
6 changes: 4 additions & 2 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -3,15 +3,17 @@ 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
* `~output_pcl (sensor_msgs/Pointcloud)` output topic
* `~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
Expand Down
10 changes: 9 additions & 1 deletion include/transform_pointcloud_nodelet.h
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@ namespace transform_pointcloud
public:
transformPointcloud() = default;
~transformPointcloud() = default;

private:
void onInit() override;

Expand All @@ -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;
};
}
29 changes: 17 additions & 12 deletions src/transform_pointcloud_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 866459f

Please sign in to comment.