57 int main(
int argc,
char **argv) {
59 ros::init(argc, argv,
"pcd_filter_pa_node");
71 std::vector<std::string> temp_filter;
72 paramloader.
load(
"~/filters" , temp_filter);
80 paramloader.
load(
"~/laser_nan_replacement_value",
84 paramloader.
load(
"~/enabled" , enabled );
87 bool remapping =
false;
88 remapping|= paramloader.
loadTopic(
"~/topic_in_cloud" ,
90 remapping|= paramloader.
loadTopic(
"~/topic_in_cloud_old",
92 remapping|= paramloader.
loadTopic(
"~/topic_in_laser" ,
95 paramloader.
loadTopic(
"~/topic_out_cloud" ,
126 ": no input topic was remapped\n" <<
127 "this seems to be a mistake - try the following:\n" <<
128 " rosrun pcdfilter_pa pcdfilter_pa_node _topic_in_cloud:=" <<
129 "/new_in_topic _topic_out_cloud:=/new_out_topic");
141 const sensor_msgs::PointCloud2ConstPtr &msg) {
144 int count = (msg->width > 1 ? msg->width : 1) * msg->height;
145 ROS_INFO(
"%s: #### received new pointcloud (%d)",
150 ROS_INFO(
"%s: skipped (input throttle)",
161 sensor_msgs::PointCloud2Ptr result;
164 ROS_INFO(
"%s: publishing filtered pointcloud",
173 const sensor_msgs::PointCloudConstPtr &msg) {
180 const sensor_msgs::LaserScanConstPtr &msg) {
187 pcdfilter_pa::PcdFilterPaFilter::Request &req,
188 pcdfilter_pa::PcdFilterPaFilter::Response &res) {
197 sensor_msgs::PointCloud2Ptr cloud;
207 pcdfilter_pa::PcdFilterPaCloud::Request &req,
208 pcdfilter_pa::PcdFilterPaCloud::Response &res) {
212 sensor_msgs::PointCloud2ConstPtr msg(&req.in_cloud);
213 sensor_msgs::PointCloud2Ptr result;
216 msg->header.stamp)) {
221 ROS_INFO(
"%s: service call for filtering",
224 sensor_msgs::PointCloud2_<std::allocator<void> >::_data_type temp;
225 temp.swap(result->data);
226 res.out_cloud = *result;
227 res.out_cloud.data.swap(temp);
237 const std::vector<std::string> &new_filters) {
239 for (
int i = 0; i < new_filters.size(); i++) {
241 ROS_INFO(
"%s: added new filter definition \"%s\"",
244 ROS_WARN(
"%s: error in filter definition \"%s\"",
252 pcdfilter_pa::PcdFilterPaFilter::Request &req,
253 pcdfilter_pa::PcdFilterPaFilter::Response &res) {
258 ROS_INFO(
"%s: removed all filters (%d)",
265 std_srvs::Empty::Response &res) {
273 std_srvs::Empty::Response &res) {
std::string topic_in_laser_
name of the topic for subscribing a laserscan ("~in_laser")
void addFilters(const std::vector< std::string > &new_filters)
function for adding several filters and giving feedback
void disable(void)
function for deactivating this node (disabling inputs)
std::string getLast(void) const
ros::ServiceServer ser_add_filters_
service for adding additional filters
void publish(const boost::shared_ptr< M > &message) const
double laser_nan_replacement_value_
std::string topic_in_cloud_old_
name of the topic for subscribing a pointcloud ("~in_cloud_old")
cPcdFilterPaRosThrottle input_throttle_
object for input throttling - e.g. only every 5th pointcloud
ros::ServiceServer ser_filter_
service for filtering
bool enableCallbackSrv(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
callback function for enabling filter node
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int main(int argc, char **argv)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::string topic_out_cloud_
tf::TransformListener tf_listener_
for getting all necessary tfs
ros::Subscriber sub_pcd_old_
subscriber for a pointcloud (old format)
ROSCPP_DECL const std::string & getName()
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool loadTopic(const std::string name, std::string &value, const bool print_default=true) const
cPcdFilterPaRosParameter rosparams_
ros specific parameter
bool debugging_
flag for extended output
bool load(const std::string name, bool &value, const bool print_default=true) const
ROSCPP_DECL void spin(Spinner &spinner)
bool changeFiltersCallbackSrv(pcdfilter_pa::PcdFilterPaFilter::Request &req, pcdfilter_pa::PcdFilterPaFilter::Response &res)
callback function for changing filters
ros::ServiceServer ser_enable_
service for enabling node
std::string topic_in_cloud_
name of the topic for subscribing a pointcloud ("~in_cloud")
bool updateTf(const tf::TransformListener &tf_listener, const std::string base_frame, const ros::Time time=ros::Time::now())
bool addFiltersCallbackSrv(pcdfilter_pa::PcdFilterPaFilter::Request &req, pcdfilter_pa::PcdFilterPaFilter::Response &res)
callback function for adding additional filters
cPcdFilterPaNode()
default constructor
void enable(void)
function for activating this node (enabling inputs)
sensor_msgs::PointCloud2Ptr convertCloud(const sensor_msgs::PointCloudConstPtr &msg) const
converts the given pointcloud to newer format
cPcdFilterPaRosFilters filters_
object for filter handling
ros::Subscriber sub_laser_
subscriber for a laserscan
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Subscriber sub_pcd_
subscriber for a pointcloud
#define ROS_WARN_STREAM(args)
static std::string resolveRessourcename(const std::string name)
void setCloudLaserCallbackSub(const sensor_msgs::LaserScanConstPtr &msg)
callback function for receiving a laserscan
~cPcdFilterPaNode()
default destructor
bool filterCloud(const sensor_msgs::PointCloud2ConstPtr &msg, sensor_msgs::PointCloud2Ptr &result)
filters the given pointcloud
bool add(std::string filter)
ros::NodeHandle nh_
node handler for topic subscription and advertising
cPcdFilterPaNodeParameter nodeparams_
ros specific variables, which will be read from Parameterserver
bool disableCallbackSrv(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
callback function for disabling filter node
bool checkNewInput(ros::Time time=ros::Time::now())
checks if the current data is accepted
void setCloudCallbackSub(const sensor_msgs::PointCloud2ConstPtr &msg)
callback function for receiving a pointcloud
ros::Publisher pub_pcd_
publisher for filtered pointcloud
bool filterCallbackSrv(pcdfilter_pa::PcdFilterPaCloud::Request &req, pcdfilter_pa::PcdFilterPaCloud::Response &res)
callback function for filtering via service
ros::ServiceServer ser_change_filters_
service for changing filters
ros::ServiceServer ser_disable_
service for disabling node
std::vector< std::string > get(void) const
void setCloudOldCallbackSub(const sensor_msgs::PointCloudConstPtr &msg)
callback function for receiving a pointcloud (old format)