How to implement the robot body filter?

edit

I haven't tested my implementation much yet but this is what you need to do to make it work.

1. Create a robot_body_filter.yaml file that will contain the configuration values required for each filter you need in your chain (could be multiple filters in the chain or only one)

   • add this file to <PACKAGE_NAME>/config/ directory.
   • refer to examples for examples of such YAML files.
   • note that params: section in your YAML file can be filled with whichever parameters needed by your filter, refer to robot_body_filter for a list of applicable parameters.

2. Launch: robot_body_filter package does not implement launchable nodes as opposed to laser_filter package, thus, you will have to implement the functionality in your code and load the previously created robot_body_filter.yaml file to your server.

3. load the robot_body_filter.yaml configuration file to your ros_parameter server by adding <rosparam command="load" file="$(find PACKAGE_NAME)/config/robot_body_filter.yaml" /> inside your <node> tag to load the robot_body_filter.yaml file into the parameter server.

   • create a filters::FilterChain object in your class that will be used to configure the filters listed in robot_body_filter.yaml file filters::FilterChain<T> filter_chain_;, where T is the template you need for the incoming msgs, i.e. sensor_msgs::PointCloud2 or sensor_msgs::LaserScan

   • construct the filter_chain_ object in your constructor's initialization list with the T you have previously selected filter_chain_("T")

   • configure the filter_chain_ object in your constructor filter_chain_.configure("NAME", pnh_);, where NAME is the name of your filter chain mentioned at the beginning of your robot_body_filter.yaml file (i.e. scan_filter_chain or cloud_filter_chain)

4. at this point, your filter_chain_ (including all the listed filters) is ready to be used by the update() function, once you have the pointcloud or laserscan msg received in your callback function, apply filter_chain_.update(*msg_in, msg_out) and the result will be saved in msg_out.

Note: you will know that everything is loaded properly by seeing INFO and WARNING log messages for the listed filter parameters in the Yaml file in the log when you launch the node.

Update:

try this

#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"
#include "filters/filter_chain.h"
#include <ros/console.h>

class robot_body_filter
{  
  filters::FilterChain<sensor_msgs::PointCloud2> filter_chain_;    
public:
  robot_body_filter() : filter_chain_("sensor_msgs::PointCloud2")
  {
    filter_chain_.configure("cloud_filter_chain");
    pub_ = n_.advertise<sensor_msgs::PointCloud2>("/camera/depth/color/points_filtered", 1);
    sub_ = n_.subscribe("/camera/depth/color/points", 1, &robot_body_filter::callback, this);
  }
  void callback(const sensor_msgs::PointCloud2ConstPtr& input)
  {    
    sensor_msgs::PointCloud2 output;
    filter_chain_.update(*input, output);
    pub_.publish(output);
  }
private:
  ros::NodeHandle n_;
  ros::Publisher pub_;
  ros::Subscriber sub_;
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "load_filter_chain");
  robot_body_filter rbf;
  ros::spin();
  return 0;
}

Hi, I know it was a bit cumbersome to actually start using the filter, so I created a helper package for it a few months ago. Have a look at http://wiki.ros.org/sensor_filters . It sets up the simplest possible node or nodelet running a single filter chain. Implementations for all sensor_msgs types are provided.

We've also added an examples folder to robot_body_filter that contains some YAML files and an example launch file. It is basically what @ahedfares89 wrote.

d435.yaml:

cloud_filter_chain:
  - name: RobotBodyFilter
    type: robot_body_filter/RobotBodyFilterPointCloud2
    params:
      frames/sensor: 'camera_color_optical_frame'
      filter/keep_clouds_organized: True
      filter/do_clipping: True
      filter/do_contains_test: True
      filter/do_shadow_test: False
      sensor/point_by_point: False
      sensor/min_distance: 0.3
      sensor/max_distance: 5.0
      body_model/inflation/scale: 1.07
      body_model/inflation/padding: 0.01
      transforms/buffer_length: 15.0
      transforms/timeout/reachable: 0.2
      transforms/timeout/unreachable: 0.2

filter.launch:

<launch>
    <node name="filter" pkg="sensor_filters" type="pointcloud2_filter_chain" output="screen">
        <rosparam command="load" file="$(dirname)/d435.yaml" />
        <remap from="~input" to="camera/depth/color/points" />
        <remap from="~output" to="camera/depth/color/points_filtered" />
    </node>
</launch>