74 connect_cb, connect_cb);
110 ROS_DEBUG(
"No valid frames have been passed into the self filter. Using a callback that will just forward scans on.");
115 ROS_DEBUG(
"Valid frames were passed in. We'll filter them.");
134 ROS_DEBUG(
"Self filter publishing unfiltered frame");
140 std::vector<int> mask;
144 sensor_msgs::PointCloud2 out2;
149 typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
151 pcl::PointCloud<pcl::PointXYZRGB> out;
154 out2.header.stamp = cloud2->header.stamp;
155 input_size = cloud->points.size();
156 output_size = out.points.size();
160 typename pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
162 pcl::PointCloud<pcl::PointXYZ> out;
165 out2.header.stamp = cloud2->header.stamp;
166 input_size = cloud->points.size();
167 output_size = out.points.size();
172 ROS_DEBUG(
"Self filter: reduced %d points to %d points in %f seconds", input_size, output_size, sec);
196 int main(
int argc,
char **argv)
void getLinkNames(std::vector< std::string > &frames) const
Get the set of link names that have been instantiated for self filtering.
filters::SelfFilter< pcl::PointXYZ > * self_filter_
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
std::vector< std::string > frames_
ros::NodeHandle root_handle_
ROSCPP_DECL void spin(Spinner &spinner)
tf::TransformListener tf_
filters::SelfFilter< pcl::PointXYZRGB > * self_filter_rgb_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void noFilterCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
robot_self_filter::SelfMask< PointT > * getSelfMask()
uint32_t getNumSubscribers() const
int main(int argc, char **argv)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
std::string sensor_frame_
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
boost::shared_ptr< tf::MessageFilter< sensor_msgs::PointCloud2 > > mn_
ros::Publisher pointCloudPublisher_
ros::Subscriber no_filter_sub_
bool updateWithSensorFrame(const PointCloud &data_in, PointCloud &data_out, const std::string &sensor_frame)
void connectionCallback(const ros::SingleSubscriberPublisher &pub)
void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud2)