71 connect_cb, connect_cb);
107 ROS_DEBUG(
"No valid frames have been passed into the self filter. Using a callback that will just forward scans on.");
112 ROS_DEBUG(
"Valid frames were passed in. We'll filter them.");
131 ROS_DEBUG(
"Self filter publishing unfiltered frame");
137 std::vector<int> mask;
141 sensor_msgs::PointCloud2 out2;
146 typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
148 pcl::PointCloud<pcl::PointXYZRGB> out;
151 out2.header.stamp = cloud2->header.stamp;
152 input_size = cloud->points.size();
153 output_size = out.points.size();
157 typename pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
159 pcl::PointCloud<pcl::PointXYZ> out;
162 out2.header.stamp = cloud2->header.stamp;
163 input_size = cloud->points.size();
164 output_size = out.points.size();
169 ROS_DEBUG(
"Self filter: reduced %d points to %d points in %f seconds", input_size, output_size, sec);
193 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.
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
ros::Publisher pointCloudPublisher_
void publish(const boost::shared_ptr< M > &message) const
void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud2)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::string sensor_frame_
filters::SelfFilter< pcl::PointXYZ > * self_filter_
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)
ros::Subscriber no_filter_sub_
void noFilterCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud)
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_
ROSCPP_DECL void spin(Spinner &spinner)
ros::NodeHandle root_handle_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void connectionCallback(const ros::SingleSubscriberPublisher &pub)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
boost::shared_ptr< tf::MessageFilter< sensor_msgs::PointCloud2 > > mn_
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)
tf::TransformListener tf_
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
std::vector< std::string > frames_
filters::SelfFilter< pcl::PointXYZRGB > * self_filter_rgb_
bool updateWithSensorFrame(const PointCloud &data_in, PointCloud &data_out, const std::string &sensor_frame)