Skip to content

feat(humble): Migrated most of the source and include codes from ROS1 to ROS2 #505

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
diogovazguerra wants to merge 1 commit into
base: humble
Choose a base branch
from
Open

feat(humble): Migrated most of the source and include codes from ROS1 to ROS2 #505

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
55 changes: 23 additions & 32 deletions pcl_ros/include/pcl_ros/filters/extract_indices.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,62 +38,53 @@
#ifndef PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_
#define PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_

// PCL includes
#include <pcl/filters/extract_indices.h>
#include <rclcpp/rclcpp.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>

#include "pcl_ros/filters/filter.hpp"
#include "pcl_ros/ExtractIndicesConfig.hpp"

namespace pcl_ros
{
/** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud.
/** \brief @b ExtractIndices extracts the given set of indices from an input point cloud dataset.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class ExtractIndices : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>> srv_;
/** \brief The PCL filter implementation used. */
pcl::ExtractIndices<pcl::PCLPointCloud2> impl_;

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

/** \brief Call the actual filter.
protected:
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output)
{
boost::mutex::scoped_lock lock(mutex_);
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*(input), *(pcl_input));
impl_.setInputCloud(pcl_input);
impl_.setIndices(indices);
pcl::PCLPointCloud2 pcl_output;
impl_.filter(pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
void filter(
const PointCloud2ConstPtr & input,
const IndicesPtr & indices,
PointCloud2 & output) override;

/** \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);
bool child_init(bool has_service = false) override;

/** \brief Dynamic reconfigure service callback. */
void
config_callback(pcl_ros::ExtractIndicesConfig & config, uint32_t level);
/** \brief Parameter callback
* \param parameters the changed parameters
*/
rcl_interfaces::msg::SetParametersResult config_callback(
const std::vector<rclcpp::Parameter> & parameters) override;

private:
/** \brief The PCL filter implementation used. */
pcl::ExtractIndices<pcl::PCLPointCloud2> impl_;

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/** \brief Parameter for negative extraction */
bool negative_;
};
} // namespace pcl_ros

#endif // PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_
#endif // PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_
152 changes: 75 additions & 77 deletions pcl_ros/include/pcl_ros/filters/filter.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,129 +38,127 @@
#ifndef PCL_ROS__FILTERS__FILTER_HPP_
#define PCL_ROS__FILTERS__FILTER_HPP_

#include <pcl/filters/filter.h>
#include <dynamic_reconfigure/server.h>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl_msgs/msg/point_indices.hpp>

// TF
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

// Message filters
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <memory>
#include <string>
#include <vector>

#include "pcl_ros/pcl_nodelet.hpp"
#include "pcl_ros/FilterConfig.hpp"

namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;

/** \brief @b Filter represents the base filter class. Some generic 3D operations that are
* applicable to all filters are defined here as static methods.
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b Filter represents the base filter class. All filters must inherit from this class.
* \author Radu Bogdan Rusu
*/
class Filter : public PCLNodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef sensor_msgs::msg::PointCloud2 PointCloud2;
typedef PointCloud2::SharedPtr PointCloud2Ptr;
typedef PointCloud2::ConstSharedPtr PointCloud2ConstPtr;

typedef pcl::IndicesPtr IndicesPtr;
typedef pcl::IndicesConstPtr IndicesConstPtr;
typedef pcl_msgs::msg::PointIndices PointIndices;
typedef PointIndices::SharedPtr PointIndicesPtr;
typedef PointIndices::ConstSharedPtr PointIndicesConstPtr;

Filter() {}
/** \brief Empty constructor. */
Filter() : PCLNodelet("filter_node"), tf_input_frame_(""), tf_output_frame_("") {}

protected:
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \brief Compute the actual filtering and publish the result.
* \param input the input point cloud dataset.
* \param indices the input set of indices to use from \a input
*/
void computePublish(const PointCloud2ConstPtr & input, const IndicesPtr & indices);

message_filters::Subscriber<PointCloud2> sub_input_filter_;
protected:
/** \brief The input PointCloud2 subscriber. */
rclcpp::Subscription<PointCloud2>::SharedPtr sub_input_;

/** \brief The desired user filter field name. */
std::string filter_field_name_;

/** \brief The minimum allowed filter value a point will be considered from. */
/** \brief The minimum allowed filter value a point will be filtered with. */
double filter_limit_min_;

/** \brief The maximum allowed filter value a point will be considered from. */
/** \brief The maximum allowed filter value a point will be filtered with. */
double filter_limit_max_;

/** \brief Set to true if we want to return the data outside
* (\a filter_limit_min_;\a filter_limit_max_). Default: false.
*/
/** \brief Set to true if point filtering should be limited to a certain values range. */
bool filter_limit_negative_;

/** \brief The input TF frame the data should be transformed into,
* if input.header.frame_id is different.
*/
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */
std::string tf_input_frame_;

/** \brief The original data input TF frame. */
std::string tf_input_orig_frame_;

/** \brief The output TF frame the data should be transformed into,
* if input.header.frame_id is different.
*/
/** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
std::string tf_output_frame_;

/** \brief Internal mutex. */
boost::mutex mutex_;
/** \brief TF2 buffer and listener for transforms. */
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

/** \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;
}

/** \brief Virtual abstract filter method. To be implemented by every child.
* \param input the input point cloud dataset.
* \param indices a pointer to the vector of point indices to use.
* \param output the resultant filtered PointCloud2
*/
virtual void
filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output) = 0;
/** \brief The message filter subscriber for PointCloud2. */
message_filters::Subscriber<PointCloud2> sub_input_filter_;

/** \brief Lazy transport subscribe routine. */
virtual void
subscribe();
/** \brief The message filter subscriber for PointIndices. */
message_filters::Subscriber<PointIndices> sub_indices_filter_;

/** \brief Lazy transport unsubscribe routine. */
virtual void
unsubscribe();
/** \brief Synchronized input, and indices.*/
std::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointIndices>>> sync_input_indices_e_;
std::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointIndices>>> sync_input_indices_a_;

/** \brief Nodelet initialization routine. */
virtual void
onInit();
/** \brief Parameter callback handle. */
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;

/** \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.
/** \brief Child initialization routine. Internal method. */
virtual bool child_init(bool has_service = false) = 0;

/** \brief Filter a Point Cloud.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
void
computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices);
virtual void filter(const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) = 0;

private:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig>> srv_;
/** \brief Parameter callback
* \param parameters the changed parameters
*/
virtual rcl_interfaces::msg::SetParametersResult config_callback(const std::vector<rclcpp::Parameter> & parameters);

/** \brief Synchronized input, and indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
PointIndices>>> sync_input_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
PointIndices>>> sync_input_indices_a_;
/** \brief PointCloud2 + Indices data callback. */
void input_indices_callback(const PointCloud2ConstPtr & cloud, const PointIndicesConstPtr & indices);

/** \brief Dynamic reconfigure service callback. */
virtual void
config_callback(pcl_ros::FilterConfig & config, uint32_t level);
private:
/** \brief Nodelet initialization routine. */
void onInit();

/** \brief PointCloud2 + Indices data callback. */
void
input_indices_callback(
const PointCloud2::ConstPtr & cloud,
const PointIndicesConstPtr & indices);
/** \brief LazyNodelet connection routine. */
void subscribe();
void unsubscribe();

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros

#endif // PCL_ROS__FILTERS__FILTER_HPP_
#endif // PCL_ROS__FILTERS__FILTER_HPP_
12 changes: 6 additions & 6 deletions pcl_ros/include/pcl_ros/filters/passthrough.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ class PassThrough : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig>> srv_;
// boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig>> srv_;

/** \brief Call the actual filter.
* \param input the input point cloud dataset
Expand All @@ -64,7 +64,7 @@ class PassThrough : public Filter
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output)
{
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);
Expand All @@ -78,15 +78,15 @@ class PassThrough : public Filter
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
bool
child_init(ros::NodeHandle & nh, bool & has_service);
// bool
// child_init(ros::NodeHandle & nh, bool & has_service);

/** \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);
// void
// config_callback(pcl_ros::FilterConfig & config, uint32_t level);

private:
/** \brief The PCL filter implementation used. */
Expand Down
Loading