migrate passthrough and project_inliers filters (#395)
* migrate passthrough and project_inliers filters Signed-off-by: Daisuke Sato <daisukes@cmu.edu> * - remove throwing runtime_error (result always true) - use get_subscription_count() Signed-off-by: Daisuke Sato <daisukes@cmu.edu> * change comparison operator Signed-off-by: Daisuke Sato <daisukes@cmu.edu> --------- Signed-off-by: Daisuke Sato <daisukes@cmu.edu>
This commit is contained in:
Daisuke Sato
GitHub
parent
0c8e7dafce
commit
de09161924
@ -84,8 +84,8 @@ ament_target_dependencies(pcl_ros_tf
|
||||
add_library(pcl_ros_filters SHARED
|
||||
src/pcl_ros/filters/extract_indices.cpp
|
||||
src/pcl_ros/filters/filter.cpp
|
||||
# src/pcl_ros/filters/passthrough.cpp
|
||||
# src/pcl_ros/filters/project_inliers.cpp
|
||||
src/pcl_ros/filters/passthrough.cpp
|
||||
src/pcl_ros/filters/project_inliers.cpp
|
||||
# src/pcl_ros/filters/radius_outlier_removal.cpp
|
||||
# src/pcl_ros/filters/statistical_outlier_removal.cpp
|
||||
# src/pcl_ros/filters/voxel_grid.cpp
|
||||
@ -97,6 +97,14 @@ rclcpp_components_register_node(pcl_ros_filters
|
||||
PLUGIN "pcl_ros::ExtractIndices"
|
||||
EXECUTABLE filter_extract_indices_node
|
||||
)
|
||||
rclcpp_components_register_node(pcl_ros_filters
|
||||
PLUGIN "pcl_ros::PassThrough"
|
||||
EXECUTABLE filter_passthrough_node
|
||||
)
|
||||
rclcpp_components_register_node(pcl_ros_filters
|
||||
PLUGIN "pcl_ros::ProjectInliers"
|
||||
EXECUTABLE filter_project_inliers_node
|
||||
)
|
||||
class_loader_hide_library_symbols(pcl_ros_filters)
|
||||
#
|
||||
### Declare the pcl_ros_segmentation library
|
||||
|
||||
@ -71,6 +71,9 @@ protected:
|
||||
void
|
||||
use_frame_params();
|
||||
|
||||
/** \brief Add common parameters */
|
||||
std::vector<std::string> add_common_params();
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
rclcpp::Subscription<PointCloud2>::SharedPtr sub_input_;
|
||||
|
||||
|
||||
@ -40,6 +40,7 @@
|
||||
|
||||
// PCL includes
|
||||
#include <pcl/filters/passthrough.h>
|
||||
#include <vector>
|
||||
#include "pcl_ros/filters/filter.hpp"
|
||||
|
||||
namespace pcl_ros
|
||||
@ -51,9 +52,6 @@ namespace pcl_ros
|
||||
class PassThrough : public Filter
|
||||
{
|
||||
protected:
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig>> srv_;
|
||||
|
||||
/** \brief Call the actual filter.
|
||||
* \param input the input point cloud dataset
|
||||
* \param indices the input set of indices to use from \a input
|
||||
@ -61,10 +59,10 @@ protected:
|
||||
*/
|
||||
inline void
|
||||
filter(
|
||||
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
|
||||
PointCloud2 & output)
|
||||
const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
|
||||
PointCloud2 & output) override
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL(*(input), *(pcl_input));
|
||||
impl_.setInputCloud(pcl_input);
|
||||
@ -74,19 +72,13 @@ protected:
|
||||
pcl_conversions::moveFromPCL(pcl_output, output);
|
||||
}
|
||||
|
||||
/** \brief Child initialization routine.
|
||||
* \param nh ROS node handle
|
||||
* \param has_service set to true if the child has a Dynamic Reconfigure service
|
||||
/** \brief Parameter callback
|
||||
* \param params parameter values to set
|
||||
*/
|
||||
bool
|
||||
child_init(ros::NodeHandle & nh, bool & has_service);
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
config_callback(const std::vector<rclcpp::Parameter> & params);
|
||||
|
||||
/** \brief Dynamic reconfigure service callback.
|
||||
* \param config the dynamic reconfigure FilterConfig object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void
|
||||
config_callback(pcl_ros::FilterConfig & config, uint32_t level);
|
||||
OnSetParametersCallbackHandle::SharedPtr callback_handle_;
|
||||
|
||||
private:
|
||||
/** \brief The PCL filter implementation used. */
|
||||
@ -94,6 +86,8 @@ private:
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
explicit PassThrough(const rclcpp::NodeOptions & options);
|
||||
};
|
||||
} // namespace pcl_ros
|
||||
|
||||
|
||||
@ -38,8 +38,10 @@
|
||||
#ifndef PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_
|
||||
#define PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_
|
||||
|
||||
// PCL includes
|
||||
#include <pcl/filters/project_inliers.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <memory>
|
||||
#include "pcl_ros/filters/filter.hpp"
|
||||
|
||||
|
||||
@ -55,8 +57,7 @@ namespace sync_policies = message_filters::sync_policies;
|
||||
class ProjectInliers : public Filter
|
||||
{
|
||||
public:
|
||||
ProjectInliers()
|
||||
: model_() {}
|
||||
explicit ProjectInliers(const rclcpp::NodeOptions & options);
|
||||
|
||||
protected:
|
||||
/** \brief Call the actual filter.
|
||||
@ -66,7 +67,7 @@ protected:
|
||||
*/
|
||||
inline void
|
||||
filter(
|
||||
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
|
||||
const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
|
||||
PointCloud2 & output)
|
||||
{
|
||||
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
|
||||
@ -89,25 +90,20 @@ private:
|
||||
message_filters::Subscriber<ModelCoefficients> sub_model_;
|
||||
|
||||
/** \brief Synchronized input, indices, and model coefficients.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
|
||||
std::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
|
||||
PointIndices, ModelCoefficients>>> sync_input_indices_model_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
|
||||
std::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
|
||||
PointIndices, ModelCoefficients>>> sync_input_indices_model_a_;
|
||||
/** \brief The PCL filter implementation used. */
|
||||
pcl::ProjectInliers<pcl::PCLPointCloud2> impl_;
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void
|
||||
onInit();
|
||||
|
||||
/** \brief NodeletLazy connection routine. */
|
||||
void subscribe();
|
||||
void unsubscribe();
|
||||
void subscribe() override;
|
||||
void unsubscribe() override;
|
||||
|
||||
/** \brief PointCloud2 + Indices + Model data callback. */
|
||||
void
|
||||
input_indices_model_callback(
|
||||
const PointCloud2::ConstPtr & cloud,
|
||||
const PointCloud2::ConstSharedPtr & cloud,
|
||||
const PointIndicesConstPtr & indices,
|
||||
const ModelCoefficientsConstPtr & model);
|
||||
|
||||
|
||||
@ -56,6 +56,8 @@ pcl_ros::ExtractIndices::ExtractIndices(const rclcpp::NodeOptions & options)
|
||||
if (!result.successful) {
|
||||
throw std::runtime_error(result.reason);
|
||||
}
|
||||
// TODO(daisukes): lazy subscription after rclcpp#2060
|
||||
subscribe();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
@ -61,7 +61,9 @@
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Filter::computePublish(const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices)
|
||||
pcl_ros::Filter::computePublish(
|
||||
const PointCloud2::ConstSharedPtr & input,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
PointCloud2 output;
|
||||
// Call the virtual method in the child
|
||||
@ -116,21 +118,30 @@ pcl_ros::Filter::subscribe()
|
||||
// If we're supposed to look for PointIndices (indices)
|
||||
if (use_indices_) {
|
||||
// Subscribe to the input using a filter
|
||||
sub_input_filter_.subscribe(this, "input", rclcpp::QoS(max_queue_size_).get_rmw_qos_profile());
|
||||
sub_indices_filter_.subscribe(this, "indices", rclcpp::QoS(max_queue_size_).get_rmw_qos_profile());
|
||||
auto sensor_qos_profile = rclcpp::QoS(
|
||||
rclcpp::KeepLast(max_queue_size_),
|
||||
rmw_qos_profile_sensor_data).get_rmw_qos_profile();
|
||||
sub_input_filter_.subscribe(this, "input", sensor_qos_profile);
|
||||
sub_indices_filter_.subscribe(this, "indices", sensor_qos_profile);
|
||||
|
||||
if (approximate_sync_) {
|
||||
sync_input_indices_a_ =
|
||||
std::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
|
||||
pcl_msgs::msg::PointIndices>>>(max_queue_size_);
|
||||
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
|
||||
sync_input_indices_a_->registerCallback(std::bind(&Filter::input_indices_callback, this, std::placeholders::_1, std::placeholders::_2));
|
||||
sync_input_indices_a_->registerCallback(
|
||||
std::bind(
|
||||
&Filter::input_indices_callback, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
} else {
|
||||
sync_input_indices_e_ =
|
||||
std::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
|
||||
pcl_msgs::msg::PointIndices>>>(max_queue_size_);
|
||||
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
|
||||
sync_input_indices_e_->registerCallback(std::bind(&Filter::input_indices_callback, this, std::placeholders::_1, std::placeholders::_2));
|
||||
sync_input_indices_e_->registerCallback(
|
||||
std::bind(
|
||||
&Filter::input_indices_callback, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
} else {
|
||||
// Workaround for a callback with custom arguments ros2/rclcpp#766
|
||||
@ -162,8 +173,6 @@ pcl_ros::Filter::Filter(std::string node_name, const rclcpp::NodeOptions & optio
|
||||
: PCLNode(node_name, options)
|
||||
{
|
||||
pub_output_ = create_publisher<PointCloud2>("output", max_queue_size_);
|
||||
// TODO(daisukes): lazy subscription after rclcpp#2060
|
||||
subscribe();
|
||||
RCLCPP_DEBUG(this->get_logger(), "Node successfully created.");
|
||||
}
|
||||
|
||||
@ -174,18 +183,25 @@ pcl_ros::Filter::use_frame_params()
|
||||
rcl_interfaces::msg::ParameterDescriptor input_frame_desc;
|
||||
input_frame_desc.name = "input_frame";
|
||||
input_frame_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
|
||||
input_frame_desc.description = "The input TF frame the data should be transformed into before processing, if input.header.frame_id is different.";
|
||||
input_frame_desc.description =
|
||||
"The input TF frame the data should be transformed into before processing, "
|
||||
"if input.header.frame_id is different.";
|
||||
declare_parameter(input_frame_desc.name, rclcpp::ParameterValue(""), input_frame_desc);
|
||||
|
||||
rcl_interfaces::msg::ParameterDescriptor output_frame_desc;
|
||||
output_frame_desc.name = "output_frame";
|
||||
output_frame_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
|
||||
output_frame_desc.description = "The output TF frame the data should be transformed into after processing, if input.header.frame_id is different.";
|
||||
output_frame_desc.description =
|
||||
"The output TF frame the data should be transformed into after processing, "
|
||||
"if input.header.frame_id is different.";
|
||||
declare_parameter(output_frame_desc.name, rclcpp::ParameterValue(""), output_frame_desc);
|
||||
|
||||
// Validate initial values using same callback
|
||||
callback_handle_ =
|
||||
add_on_set_parameters_callback(std::bind(&Filter::config_callback, this, std::placeholders::_1));
|
||||
add_on_set_parameters_callback(
|
||||
std::bind(
|
||||
&Filter::config_callback, this,
|
||||
std::placeholders::_1));
|
||||
|
||||
std::vector<std::string> param_names{input_frame_desc.name, output_frame_desc.name};
|
||||
auto result = config_callback(get_parameters(param_names));
|
||||
@ -194,6 +210,60 @@ pcl_ros::Filter::use_frame_params()
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
std::vector<std::string>
|
||||
pcl_ros::Filter::add_common_params()
|
||||
{
|
||||
rcl_interfaces::msg::ParameterDescriptor ffn_desc;
|
||||
ffn_desc.name = "filter_field_name";
|
||||
ffn_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
|
||||
ffn_desc.description = "The field name used for filtering";
|
||||
declare_parameter(ffn_desc.name, rclcpp::ParameterValue("z"), ffn_desc);
|
||||
|
||||
rcl_interfaces::msg::ParameterDescriptor flmin_desc;
|
||||
flmin_desc.name = "filter_limit_min";
|
||||
flmin_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
|
||||
flmin_desc.description = "The minimum allowed field value a point will be considered from";
|
||||
rcl_interfaces::msg::FloatingPointRange flmin_range;
|
||||
flmin_range.from_value = -100000.0;
|
||||
flmin_range.to_value = 100000.0;
|
||||
flmin_desc.floating_point_range.push_back(flmin_range);
|
||||
declare_parameter(flmin_desc.name, rclcpp::ParameterValue(0.0), flmin_desc);
|
||||
|
||||
rcl_interfaces::msg::ParameterDescriptor flmax_desc;
|
||||
flmax_desc.name = "filter_limit_max";
|
||||
flmax_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
|
||||
flmax_desc.description = "The maximum allowed field value a point will be considered from";
|
||||
rcl_interfaces::msg::FloatingPointRange flmax_range;
|
||||
flmax_range.from_value = -100000.0;
|
||||
flmax_range.to_value = 100000.0;
|
||||
flmax_desc.floating_point_range.push_back(flmax_range);
|
||||
declare_parameter(flmax_desc.name, rclcpp::ParameterValue(1.0), flmax_desc);
|
||||
|
||||
rcl_interfaces::msg::ParameterDescriptor flneg_desc;
|
||||
flneg_desc.name = "filter_limit_negative";
|
||||
flneg_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
|
||||
flneg_desc.description =
|
||||
"Set to true if we want to return the data outside [filter_limit_min; filter_limit_max].";
|
||||
declare_parameter(flneg_desc.name, rclcpp::ParameterValue(false), flneg_desc);
|
||||
|
||||
rcl_interfaces::msg::ParameterDescriptor keep_organized_desc;
|
||||
keep_organized_desc.name = "keep_organized";
|
||||
keep_organized_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
|
||||
keep_organized_desc.description =
|
||||
"Set whether the filtered points should be kept and set to NaN, "
|
||||
"or removed from the PointCloud, thus potentially breaking its organized structure.";
|
||||
declare_parameter(keep_organized_desc.name, rclcpp::ParameterValue(false), keep_organized_desc);
|
||||
|
||||
return std::vector<std::string> {
|
||||
ffn_desc.name,
|
||||
flmin_desc.name,
|
||||
flmax_desc.name,
|
||||
flneg_desc.name,
|
||||
keep_organized_desc.name
|
||||
};
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
pcl_ros::Filter::config_callback(const std::vector<rclcpp::Parameter> & params)
|
||||
|
||||
@ -36,95 +36,94 @@
|
||||
*/
|
||||
|
||||
#include "pcl_ros/filters/passthrough.hpp"
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
bool
|
||||
pcl_ros::PassThrough::child_init(ros::NodeHandle & nh, bool & has_service)
|
||||
pcl_ros::PassThrough::PassThrough(const rclcpp::NodeOptions & options)
|
||||
: Filter("PassThroughNode", options)
|
||||
{
|
||||
// Enable the dynamic reconfigure service
|
||||
has_service = true;
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::FilterConfig>>(nh);
|
||||
dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind(
|
||||
&PassThrough::config_callback, this, _1, _2);
|
||||
srv_->setCallback(f);
|
||||
use_frame_params();
|
||||
std::vector<std::string> param_names = add_common_params();
|
||||
|
||||
return true;
|
||||
callback_handle_ =
|
||||
add_on_set_parameters_callback(
|
||||
std::bind(
|
||||
&PassThrough::config_callback, this,
|
||||
std::placeholders::_1));
|
||||
|
||||
config_callback(get_parameters(param_names));
|
||||
// TODO(daisukes): lazy subscription after rclcpp#2060
|
||||
subscribe();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t level)
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
pcl_ros::PassThrough::config_callback(const std::vector<rclcpp::Parameter> & params)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
|
||||
double filter_min, filter_max;
|
||||
impl_.getFilterLimits(filter_min, filter_max);
|
||||
|
||||
// Check the current values for filter min-max
|
||||
if (filter_min != config.filter_limit_min) {
|
||||
filter_min = config.filter_limit_min;
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the minimum filtering value a point will be "
|
||||
"considered from to: %f.",
|
||||
getName().c_str(), filter_min);
|
||||
// Set the filter min-max if different
|
||||
impl_.setFilterLimits(filter_min, filter_max);
|
||||
}
|
||||
// Check the current values for filter min-max
|
||||
if (filter_max != config.filter_limit_max) {
|
||||
filter_max = config.filter_limit_max;
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the maximum filtering value a point will be "
|
||||
"considered from to: %f.",
|
||||
getName().c_str(), filter_max);
|
||||
// Set the filter min-max if different
|
||||
impl_.setFilterLimits(filter_min, filter_max);
|
||||
}
|
||||
|
||||
// Check the current value for the filter field
|
||||
// std::string filter_field = impl_.getFilterFieldName ();
|
||||
if (impl_.getFilterFieldName() != config.filter_field_name) {
|
||||
// Set the filter field if different
|
||||
impl_.setFilterFieldName(config.filter_field_name);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the filter field name to: %s.",
|
||||
getName().c_str(), config.filter_field_name.c_str());
|
||||
}
|
||||
|
||||
// Check the current value for keep_organized
|
||||
if (impl_.getKeepOrganized() != config.keep_organized) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the filter keep_organized value to: %s.",
|
||||
getName().c_str(), config.keep_organized ? "true" : "false");
|
||||
// Call the virtual method in the child
|
||||
impl_.setKeepOrganized(config.keep_organized);
|
||||
}
|
||||
|
||||
// Check the current value for the negative flag
|
||||
if (impl_.getFilterLimitsNegative() != config.filter_limit_negative) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the filter negative flag to: %s.",
|
||||
getName().c_str(), config.filter_limit_negative ? "true" : "false");
|
||||
// Call the virtual method in the child
|
||||
impl_.setFilterLimitsNegative(config.filter_limit_negative);
|
||||
}
|
||||
|
||||
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters
|
||||
// as they are inexistent in PCL
|
||||
if (tf_input_frame_ != config.input_frame) {
|
||||
tf_input_frame_ = config.input_frame;
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the input TF frame to: %s.",
|
||||
getName().c_str(), tf_input_frame_.c_str());
|
||||
}
|
||||
if (tf_output_frame_ != config.output_frame) {
|
||||
tf_output_frame_ = config.output_frame;
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the output TF frame to: %s.",
|
||||
getName().c_str(), tf_output_frame_.c_str());
|
||||
for (const rclcpp::Parameter & param : params) {
|
||||
if (param.get_name() == "filter_field_name") {
|
||||
// Check the current value for the filter field
|
||||
if (impl_.getFilterFieldName() != param.as_string()) {
|
||||
// Set the filter field if different
|
||||
impl_.setFilterFieldName(param.as_string());
|
||||
RCLCPP_DEBUG(
|
||||
get_logger(), "Setting the filter field name to: %s.",
|
||||
param.as_string().c_str());
|
||||
}
|
||||
}
|
||||
if (param.get_name() == "filter_limit_min") {
|
||||
// Check the current values for filter min-max
|
||||
if (filter_min != param.as_double()) {
|
||||
filter_min = param.as_double();
|
||||
RCLCPP_DEBUG(
|
||||
get_logger(),
|
||||
"Setting the minimum filtering value a point will be considered from to: %f.",
|
||||
filter_min);
|
||||
// Set the filter min-max if different
|
||||
impl_.setFilterLimits(filter_min, filter_max);
|
||||
}
|
||||
}
|
||||
if (param.get_name() == "filter_limit_max") {
|
||||
// Check the current values for filter min-max
|
||||
if (filter_max != param.as_double()) {
|
||||
filter_max = param.as_double();
|
||||
RCLCPP_DEBUG(
|
||||
get_logger(),
|
||||
"Setting the maximum filtering value a point will be considered from to: %f.",
|
||||
filter_max);
|
||||
// Set the filter min-max if different
|
||||
impl_.setFilterLimits(filter_min, filter_max);
|
||||
}
|
||||
}
|
||||
if (param.get_name() == "filter_limit_negative") {
|
||||
// Check the current value for the negative flag
|
||||
if (impl_.getNegative() != param.as_bool()) {
|
||||
RCLCPP_DEBUG(
|
||||
get_logger(), "Setting the filter negative flag to: %s.",
|
||||
param.as_bool() ? "true" : "false");
|
||||
// Call the virtual method in the child
|
||||
impl_.setNegative(param.as_bool());
|
||||
}
|
||||
}
|
||||
if (param.get_name() == "keep_organized") {
|
||||
// Check the current value for keep_organized
|
||||
if (impl_.getKeepOrganized() != param.as_bool()) {
|
||||
RCLCPP_DEBUG(
|
||||
get_logger(), "Setting the filter keep_organized value to: %s.",
|
||||
param.as_bool() ? "true" : "false");
|
||||
// Call the virtual method in the child
|
||||
impl_.setKeepOrganized(param.as_bool());
|
||||
}
|
||||
}
|
||||
}
|
||||
// TODO(sloretz) constraint validation
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
result.successful = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
typedef pcl_ros::PassThrough PassThrough;
|
||||
PLUGINLIB_EXPORT_CLASS(PassThrough, nodelet::Nodelet);
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::PassThrough)
|
||||
|
||||
@ -36,46 +36,39 @@
|
||||
*/
|
||||
|
||||
#include "pcl_ros/filters/project_inliers.hpp"
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <pcl/common/io.h>
|
||||
#include <vector>
|
||||
#include <pcl/io/io.h>
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ProjectInliers::onInit()
|
||||
pcl_ros::ProjectInliers::ProjectInliers(const rclcpp::NodeOptions & options)
|
||||
: Filter("ProjectInliersNode", options), model_()
|
||||
{
|
||||
PCLNodelet::onInit();
|
||||
|
||||
// ---[ Mandatory parameters
|
||||
// The type of model to use (user given parameter).
|
||||
declare_parameter("model_type", rclcpp::ParameterType::PARAMETER_INTEGER);
|
||||
int model_type;
|
||||
if (!pnh_->getParam("model_type", model_type)) {
|
||||
NODELET_ERROR(
|
||||
"[%s::onInit] Need a 'model_type' parameter to be set before continuing!",
|
||||
getName().c_str());
|
||||
if (!get_parameter("model_type", model_type)) {
|
||||
RCLCPP_ERROR(
|
||||
get_logger(),
|
||||
"[onConstruct] Need a 'model_type' parameter to be set before continuing!");
|
||||
return;
|
||||
}
|
||||
// ---[ Optional parameters
|
||||
// True if all data will be returned, false if only the projected inliers. Default: false.
|
||||
bool copy_all_data = false;
|
||||
declare_parameter("copy_all_data", rclcpp::ParameterValue(false));
|
||||
bool copy_all_data = get_parameter("copy_all_data").as_bool();
|
||||
|
||||
// True if all fields will be returned, false if only XYZ. Default: true.
|
||||
bool copy_all_fields = true;
|
||||
declare_parameter("copy_all_fields", rclcpp::ParameterValue(true));
|
||||
bool copy_all_fields = get_parameter("copy_all_fields").as_bool();
|
||||
|
||||
pnh_->getParam("copy_all_data", copy_all_data);
|
||||
pnh_->getParam("copy_all_fields", copy_all_fields);
|
||||
pub_output_ = create_publisher<PointCloud2>("output", max_queue_size_);
|
||||
|
||||
pub_output_ = advertise<PointCloud2>(*pnh_, "output", max_queue_size_);
|
||||
|
||||
// Subscribe to the input using a filter
|
||||
sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
|
||||
|
||||
NODELET_DEBUG(
|
||||
"[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - model_type : %d\n"
|
||||
" - copy_all_data : %s\n"
|
||||
" - copy_all_fields : %s",
|
||||
getName().c_str(),
|
||||
RCLCPP_DEBUG(
|
||||
this->get_logger(),
|
||||
"[onConstruct] Node successfully created with the following parameters:\n"
|
||||
" - model_type : %d\n"
|
||||
" - copy_all_data : %s\n"
|
||||
" - copy_all_fields : %s",
|
||||
model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false");
|
||||
|
||||
// Set given parameters here
|
||||
@ -83,42 +76,50 @@ pcl_ros::ProjectInliers::onInit()
|
||||
impl_.setCopyAllFields(copy_all_fields);
|
||||
impl_.setCopyAllData(copy_all_data);
|
||||
|
||||
onInitPostProcess();
|
||||
// TODO(daisukes): lazy subscription after rclcpp#2060
|
||||
subscribe();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ProjectInliers::subscribe()
|
||||
{
|
||||
RCLCPP_DEBUG(get_logger(), "subscribe");
|
||||
/*
|
||||
TODO : implement use_indices_
|
||||
if (use_indices_)
|
||||
{*/
|
||||
|
||||
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
|
||||
|
||||
sub_model_.subscribe(*pnh_, "model", max_queue_size_);
|
||||
auto qos_profile = rclcpp::QoS(
|
||||
rclcpp::KeepLast(max_queue_size_),
|
||||
rmw_qos_profile_default).get_rmw_qos_profile();
|
||||
auto sensor_qos_profile = rclcpp::QoS(
|
||||
rclcpp::KeepLast(max_queue_size_),
|
||||
rmw_qos_profile_sensor_data).get_rmw_qos_profile();
|
||||
sub_input_filter_.subscribe(this, "input", sensor_qos_profile);
|
||||
sub_indices_filter_.subscribe(this, "indices", qos_profile);
|
||||
sub_model_.subscribe(this, "model", qos_profile);
|
||||
|
||||
if (approximate_sync_) {
|
||||
sync_input_indices_model_a_ =
|
||||
boost::make_shared<message_filters::Synchronizer<
|
||||
message_filters::sync_policies::ApproximateTime<
|
||||
PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_);
|
||||
sync_input_indices_model_a_ = std::make_shared<
|
||||
message_filters::Synchronizer<
|
||||
message_filters::sync_policies::ApproximateTime<
|
||||
PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_);
|
||||
sync_input_indices_model_a_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_);
|
||||
sync_input_indices_model_a_->registerCallback(
|
||||
bind(
|
||||
&ProjectInliers::input_indices_model_callback,
|
||||
this, _1, _2, _3));
|
||||
std::bind(
|
||||
&ProjectInliers::input_indices_model_callback, this,
|
||||
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
} else {
|
||||
sync_input_indices_model_e_ =
|
||||
boost::make_shared<message_filters::Synchronizer<
|
||||
message_filters::sync_policies::ExactTime<
|
||||
PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_);
|
||||
sync_input_indices_model_e_ = std::make_shared<
|
||||
message_filters::Synchronizer<
|
||||
message_filters::sync_policies::ExactTime<
|
||||
PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_);
|
||||
sync_input_indices_model_e_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_);
|
||||
sync_input_indices_model_e_->registerCallback(
|
||||
bind(
|
||||
&ProjectInliers::input_indices_model_callback,
|
||||
this, _1, _2, _3));
|
||||
std::bind(
|
||||
&ProjectInliers::input_indices_model_callback, this,
|
||||
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
}
|
||||
|
||||
@ -130,42 +131,39 @@ pcl_ros::ProjectInliers::unsubscribe()
|
||||
TODO : implement use_indices_
|
||||
if (use_indices_)
|
||||
{*/
|
||||
|
||||
sub_input_filter_.unsubscribe();
|
||||
sub_indices_filter_.unsubscribe();
|
||||
sub_model_.unsubscribe();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ProjectInliers::input_indices_model_callback(
|
||||
const PointCloud2::ConstPtr & cloud,
|
||||
const PointCloud2::ConstSharedPtr & cloud,
|
||||
const PointIndicesConstPtr & indices,
|
||||
const ModelCoefficientsConstPtr & model)
|
||||
{
|
||||
if (pub_output_.getNumSubscribers() <= 0) {
|
||||
if (pub_output_->get_subscription_count() == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!isValid(model) || !isValid(indices) || !isValid(cloud)) {
|
||||
NODELET_ERROR("[%s::input_indices_model_callback] Invalid input!", getName().c_str());
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(), "[%s::input_indices_model_callback] Invalid input!", this->get_name());
|
||||
return;
|
||||
}
|
||||
|
||||
NODELET_DEBUG(
|
||||
RCLCPP_DEBUG(
|
||||
this->get_logger(),
|
||||
"[%s::input_indices_model_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and "
|
||||
"frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and "
|
||||
"frame %s on topic %s received.\n"
|
||||
" - ModelCoefficients with %zu values, stamp %f, and "
|
||||
"frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
|
||||
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str(),
|
||||
indices->indices.size(), indices->header.stamp.toSec(),
|
||||
indices->header.frame_id.c_str(), pnh_->resolveName("inliers").c_str(),
|
||||
model->values.size(), model->header.stamp.toSec(),
|
||||
model->header.frame_id.c_str(), pnh_->resolveName("model").c_str());
|
||||
" - PointCloud with %d data points (%s), stamp %d.%09d, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %d.%09d, and frame %s on topic %s received.\n"
|
||||
" - ModelCoefficients with %zu values, stamp %d.%09d, and frame %s on topic %s received.",
|
||||
this->get_name(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
|
||||
cloud->header.stamp.sec, cloud->header.stamp.nanosec, cloud->header.frame_id.c_str(), "input",
|
||||
indices->indices.size(), indices->header.stamp.sec, indices->header.stamp.nanosec,
|
||||
indices->header.frame_id.c_str(), "inliers", model->values.size(),
|
||||
model->header.stamp.sec, model->header.stamp.nanosec, model->header.frame_id.c_str(), "model");
|
||||
|
||||
tf_input_orig_frame_ = cloud->header.frame_id;
|
||||
|
||||
@ -178,5 +176,5 @@ pcl_ros::ProjectInliers::input_indices_model_callback(
|
||||
computePublish(cloud, vindices);
|
||||
}
|
||||
|
||||
typedef pcl_ros::ProjectInliers ProjectInliers;
|
||||
PLUGINLIB_EXPORT_CLASS(ProjectInliers, nodelet::Nodelet);
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::ProjectInliers)
|
||||
|
||||
@ -21,15 +21,44 @@ OUTPUT
|
||||
"${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/share/ament_index/resource_index/rclcpp_components/pcl_ros_tests_filters"
|
||||
CONTENT "${components}")
|
||||
|
||||
ament_add_pytest_test(test_filter_extract_indices_node_component
|
||||
# test components
|
||||
ament_add_pytest_test(test_pcl_ros::ExtractIndices
|
||||
test_filter_component.py
|
||||
ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
|
||||
FILTER_PLUGIN=pcl_ros::ExtractIndices
|
||||
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
|
||||
)
|
||||
ament_add_pytest_test(test_pcl_ros::PassThrough
|
||||
test_filter_component.py
|
||||
ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
|
||||
FILTER_PLUGIN=pcl_ros::PassThrough
|
||||
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
|
||||
)
|
||||
ament_add_pytest_test(test_pcl_ros::ProjectInliers
|
||||
test_filter_component.py
|
||||
ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
|
||||
FILTER_PLUGIN=pcl_ros::ProjectInliers
|
||||
PARAMETERS={'model_type':0}
|
||||
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
|
||||
)
|
||||
|
||||
# test executables
|
||||
ament_add_pytest_test(test_filter_extract_indices_node
|
||||
test_filter_executable.py
|
||||
ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
|
||||
FILTER_EXECUTABLE=filter_extract_indices_node
|
||||
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
|
||||
)
|
||||
ament_add_pytest_test(test_filter_passthrough_node
|
||||
test_filter_executable.py
|
||||
ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
|
||||
FILTER_EXECUTABLE=filter_passthrough_node
|
||||
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
|
||||
)
|
||||
ament_add_pytest_test(test_filter_project_inliers_node
|
||||
test_filter_executable.py
|
||||
ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
|
||||
FILTER_EXECUTABLE=filter_project_inliers_node
|
||||
PARAMETERS={'model_type':0}
|
||||
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
|
||||
)
|
||||
|
||||
@ -27,6 +27,7 @@
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
|
||||
import ast
|
||||
import os
|
||||
import unittest
|
||||
|
||||
@ -46,14 +47,8 @@ from sensor_msgs.msg import PointCloud2
|
||||
def generate_test_description():
|
||||
dummy_plugin = os.getenv('DUMMY_PLUGIN')
|
||||
filter_plugin = os.getenv('FILTER_PLUGIN')
|
||||
use_indices = os.getenv('USE_INDICES')
|
||||
approximate_sync = os.getenv('APPROXIMATE_SYNC')
|
||||
parameters = ast.literal_eval(os.getenv('PARAMETERS')) if 'PARAMETERS' in os.environ else {}
|
||||
|
||||
parameters = {}
|
||||
if use_indices:
|
||||
parameters['use_indices'] = (use_indices == 'true')
|
||||
if approximate_sync:
|
||||
parameters['approximate_sync'] = (approximate_sync == 'true')
|
||||
print(parameters)
|
||||
|
||||
return launch.LaunchDescription([
|
||||
|
||||
@ -1,3 +1,33 @@
|
||||
#
|
||||
# Copyright (c) 2022 Carnegie Mellon University
|
||||
# All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# * Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# * Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in the
|
||||
# documentation and/or other materials provided with the distribution.
|
||||
# * Neither the name of Carnegie Mellon University nor the names of its
|
||||
# contributors may be used to endorse or promote products derived from
|
||||
# this software without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
|
||||
import ast
|
||||
import os
|
||||
import unittest
|
||||
|
||||
@ -17,14 +47,11 @@ from sensor_msgs.msg import PointCloud2
|
||||
def generate_test_description():
|
||||
dummy_plugin = os.getenv('DUMMY_PLUGIN')
|
||||
filter_executable = os.getenv('FILTER_EXECUTABLE')
|
||||
use_indices = os.getenv('USE_INDICES')
|
||||
approximate_sync = os.getenv('APPROXIMATE_SYNC')
|
||||
parameters = ast.literal_eval(os.getenv('PARAMETERS')) if 'PARAMETERS' in os.environ else {}
|
||||
|
||||
ros_arguments = ["-r", "input:=point_cloud2"]
|
||||
if use_indices:
|
||||
ros_arguments.extend(["-p", "use_indices:={}".format(use_indices)])
|
||||
if approximate_sync:
|
||||
ros_arguments.extend(["-p", "approximate_sync:={}".format(approximate_sync)])
|
||||
for key in parameters.keys():
|
||||
ros_arguments.extend(["-p", "{}:={}".format(key, parameters[key])])
|
||||
|
||||
return launch.LaunchDescription([
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user