Fix polygon padding on filter load

[geotsam]

The issue:
Setting the polygon filter parameters from a .yaml results in correct parameters loaded in the parameter server (and can be seen in rqt_reconfigure). However, the filter itself does not incorporate the padding, unless the reconfigureCb() is called at least once. This can also be seen in rViz by inspecting the Polygon messages.

The solution:
Calling padPolygon() once in configure() in order to correctly pad the polygon inside the node.

[jonbinney]

This looks reasonable. Before I merge I'm going to dig into the initialization code of DynamicReconfigure to make sure I understand in exactly what sequence things happen. One question so that I can reproduce your exact use case: When you say you're setting the parameters from a yaml file, are you loading the yaml file onto the parameter server in your roslaunch file? Is it the same roslaunch file you're using to launch the laser filters node?

[geotsam]

Yes @jonbinney, the parameters are loaded from a launch file like this:

<node pkg="laser_filters" type="scan_to_scan_filter_chain" output="screen" name="laser_filter_rear">
    <remap from="scan" to="raw_scan_rear" />
    <remap from="scan_filtered" to="scan_rear" />
    <rosparam command="load" file="$(find my_robot_bringup)/config/laser_filters_rear.yaml" />
</node>

I'll also provide you with the yaml:

scan_filter_chain:
- name: footprint_filter
  type: laser_filters/LaserScanPolygonFilter
  params:
    polygon_frame: base_link
    polygon: [[0.16, 0.27], [0.16, -0.27], [-0.45, -0.27], [-0.45, 0.27]] 
    invert: false
    polygon_padding: 0.30

Setting a large polygon_padding (like 0.3) will make the issue quickly apparent 😅

[jonbinney]

The problem here seems to come from dynamic_reconfigure not supporting variable size arrays. The initial value for polygon that is defined in the YAML and loaded onto the parameter is an array, which is supported by XmlRpc (which the is how ROS1 represents parameters). Since dynamic reconfigure can't handle arrays, it expects polygon to be a string. To workaround this, LaserScanPolygonFilterBase::configure() loads the array representation from the polygon parameter, converts it to a string, and then overwrites the parameter with that string. This is really ugly.... but mostly works.

As @geotsam points out, padPolygon() doesn't get called until dynamic reconfigure calls LaserScanPolygonFilterBase::reconfigureCb(). That actually happens when dyn_server_->setCallback(f); is called earlier in configure(), but at that point the polygon parameter hasn't yet been converted from an array to a string. I think that waiting to call dyn_server_->setCallback(f); until later in the code will be a cleaner fix; i'll add a review comment to that effect.

[geotsam]

Some auto-formatting slipped in the previous commit and modified the whole file 😖; I pushed a new one that only changes the lines we are discussing.

[jonbinney]

One more nitpick - could you add back the end line at the end of the file? Looks like that was left in from the autoformat.

[geotsam]

It should be fine now! I added the end line back.

[jonbinney]

Thank you @geotsam!

[geotsam]

Hi @jonbinney! Do you know if the apt noetic package is going to be updated to include this fix?

[jonbinney]

@geotsam I'll do a release this week. Sorry it has taken a while!

[jonbinney]

Tracking release process in this issue: #200