apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

migrate abstract filter node (#388)

Browse Source 
* migrate abstract filter node

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* remove use_frame_params from constructor and make a function for the logic

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>
This commit is contained in:
Daisuke Sato
2022-12-19 14:13:06 -05:00
committed by GitHub
parent 5c5382eb5c
commit 387f5b158b
3 changed files with 143 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pcl_ros/include/pcl_ros/filters/filter.hpp
+29 -34
View File
@@ -39,10 +39,10 @@
#define PCL_ROS__FILTERS__FILTER_HPP_
#include <pcl/filters/filter.h>
#include <dynamic_reconfigure/server.h>
#include <memory>
#include <string>
#include "pcl_ros/pcl_nodelet.hpp"
#include "pcl_ros/FilterConfig.hpp"
#include <vector>
#include "pcl_ros/pcl_node.hpp"
namespace pcl_ros
{
@@ -52,19 +52,27 @@ namespace sync_policies = message_filters::sync_policies;
* applicable to all filters are defined here as static methods.
* \author Radu Bogdan Rusu
*/
class Filter : public PCLNodelet
class Filter : public PCLNode<sensor_msgs::msg::PointCloud2>
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef sensor_msgs::msg::PointCloud2 PointCloud2;
typedef pcl::IndicesPtr IndicesPtr;
typedef pcl::IndicesConstPtr IndicesConstPtr;
Filter() {}
/** \brief Filter constructor
* \param node_name node name
* \param options node options
*/
Filter(std::string node_name, const rclcpp::NodeOptions & options);
protected:
/** \brief declare and subscribe to param callback for input_frame and output_frame params */
void
use_frame_params();
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
rclcpp::Subscription<PointCloud2>::SharedPtr sub_input_;
message_filters::Subscriber<PointCloud2> sub_input_filter_;
@@ -96,18 +104,7 @@ protected:
std::string tf_output_frame_;
/** \brief Internal mutex. */
boost::mutex mutex_;
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
virtual bool
child_init(ros::NodeHandle & nh, bool & has_service)
{
has_service = false;
return true;
}
std::mutex mutex_;
/** \brief Virtual abstract filter method. To be implemented by every child.
* \param input the input point cloud dataset.
@@ -116,7 +113,7 @@ protected:
*/
virtual void
filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
PointCloud2 & output) = 0;
/** \brief Lazy transport subscribe routine. */
@@ -127,36 +124,34 @@ protected:
virtual void
unsubscribe();
/** \brief Nodelet initialization routine. */
virtual void
onInit();
/** \brief Call the child filter () method, optionally transform the result, and publish it.
* \param input the input point cloud dataset.
* \param indices a pointer to the vector of point indices to use.
*/
void
computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices);
computePublish(const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices);
private:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig>> srv_;
/** \brief Pointer to parameters callback handle. */
OnSetParametersCallbackHandle::SharedPtr callback_handle_;
/** \brief Synchronized input, and indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
std::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
PointIndices>>> sync_input_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
std::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
PointIndices>>> sync_input_indices_a_;
/** \brief Dynamic reconfigure service callback. */
virtual void
config_callback(pcl_ros::FilterConfig & config, uint32_t level);
/** \brief Parameter callback
* \param params parameter values to set
*/
rcl_interfaces::msg::SetParametersResult
config_callback(const std::vector<rclcpp::Parameter> & params);
/** \brief PointCloud2 + Indices data callback. */
void
input_indices_callback(
const PointCloud2::ConstPtr & cloud,
const PointIndicesConstPtr & indices);
const PointCloud2::ConstSharedPtr & cloud,
const PointIndices::ConstSharedPtr & indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW