Obstacle Layer Port #318

bpwilcox · 2018-11-13T19:30:44Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#192
Primary OS tested on	Ubuntu 16.04
Primary platform tested on	NUC

Description of contribution in a few bullet points

port of obstacle layer

Future work that may be required in bullet points

re-enable message filters topic subscriptions when upstream changes in ros2/geometry2/81 and ros2/message_filters/pull/13 are merged

SteveMacenski · 2018-11-13T19:54:55Z

@bpwilcox please squash my commits - the naming of at least cough the first one isn't ideal.

SteveMacenski · 2018-11-13T20:12:23Z

nav2_costmap_2d/plugins/obstacle_layer.cpp

-
-    if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan")) {
-      ROS_FATAL("Only topics that use point clouds or laser scans are currently supported");
+    node_->get_parameter_or<std::string>("topic", topic, source);


This is in the stream of inputs from configuring multiple sensor inputs (or types if some are to only clear or mark). I dont see here where node_ or source are updated to go up and down into new sensor namespaces to get the parameters. Maybe I'm missing something.

I think you are right, I overlooked the updated namespace of the parameters, however since only one node is being spun here, the parameters must exist on this node (or some other node also being spun for which we can create a client). I can add a prefix to the param names which will effectively do the same, though they will all live on the costmap node. This should be fine since yaml files support prefix namespaces on parameters. The only other solution I think would be to create (and spin) separate nodes for each source, which I don't believe we want to do.

Yeah, that would be not good. The reason to have multiple sensors in the same node is so different sensors can clear out each other as the robot moves relative to obstacles (or obstacles themselves move). It's a pretty important feature that you can configure multiple marking/clearing sources in each layer.

The parameters would still exist in this node, however there's mulitiple sources that represents (hence why there's a subscriber vector) See a simple example here of multiple sensors. I can't share our internal ones, but suffice to say that any robot with > 1 sensor uses this

Yes, I understand the use case as you describe. I plan to update to prefix a parameter namespace as the source for the sensor.

SteveMacenski · 2018-11-13T20:13:17Z

nav2_costmap_2d/plugins/obstacle_layer.cpp

        "Created an observation buffer for source %s, topic %s, global frame: %s, "
        "expected update rate: %.2f, observation persistence: %.2f",
        source.c_str(), topic.c_str(),
        global_frame_.c_str(), expected_update_rate, observation_keep_time);

+    rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default;
+    custom_qos_profile.depth = 50;


Is this generally needed at 50? Should it be parameterized?

50 was the depth in the message filter code in the original, but it could also be a parameter per source I suppose.

mjeronimo · 2018-11-14T17:26:23Z

nav2_costmap_2d/plugins/inflation_layer.cpp

-  node_->set_parameter_if_not_set("inflation_radius", 0.55);
-  node_->set_parameter_if_not_set("cost_scaling_factor", 10.0);
-  node_->set_parameter_if_not_set("inflate_unknown",false);
+  node_->set_parameter_if_not_set(name_ + "." + "enabled",true);


Why not merge the dots into the next strings? Makes it a bit easier to read (fewer elements to concatenate).

I actually found it more convenient to copy and paste the concatenation than to manually insert the dot into every parameter (goal being to leave parameter name string untouched). I'd prefer to merge the first two strings, if anything.

You could've done what @mjeronimo suggested by using search and replace in your editor
eg.
search for set_parameter_if_not_set("
and replace with set_parameter_if_not_set(name_ + ".

Good point. Still, I made a comment in the voxel layer PR that I'm not a fan of altering the base parameter name for clarity as well as the cases where it is a variable.

mjeronimo · 2018-11-14T17:27:16Z

nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp

+  //std::vector<std::shared_ptr<message_filters::SubscriberBase> > observation_subscribers_;  ///< @brief Used for the observation message filters
+  //std::vector<std::shared_ptr<tf2_ros::MessageFilterBase> > observation_notifiers_;  ///< @brief Used to make sure that transforms are available for each sensor
+
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::LaserScan>::ConstSharedPtr> observation_laser_subscribers_;


A comment or two about the usage of these members.

base include in CMakaeLists and first stab port of obstacle layer continued porting obstacle layer changes resolving everything for next person except stringstream + message filters issues add back current amcl changes add back current dockerfile changes

renable dwbplugin tests

adding comment

ghost · 2018-11-14T20:34:55Z

nav2_costmap_2d/plugins/inflation_layer.cpp

-  node_->set_parameter_if_not_set("inflation_radius", 0.55);
-  node_->set_parameter_if_not_set("cost_scaling_factor", 10.0);
-  node_->set_parameter_if_not_set("inflate_unknown",false);
+  node_->set_parameter_if_not_set(name_ + "." + "enabled",true);


You could've done what @mjeronimo suggested by using search and replace in your editor
eg.
search for set_parameter_if_not_set("
and replace with set_parameter_if_not_set(name_ + ".

ghost · 2018-11-14T20:41:53Z

nav2_costmap_2d/plugins/obstacle_layer.cpp

+
+    node_->get_parameter_or<std::string>(source + "." + "topic", topic, source);
+    node_->get_parameter_or<std::string>(source + "." + "sensor_frame", sensor_frame, std::string(""));
+    node_->get_parameter_or<double>(source + "." + "observation_persistence", observation_keep_time, 0.0);


Do you really need the type specifiers here (<double>)? I thought that it was automatically deduced from the variable's type.

I can probably remove the specifier

ghost · 2018-11-14T20:51:21Z

nav2_costmap_2d/plugins/obstacle_layer.cpp

+    // TODO(bpwilcox): replace with message filters above when enabled
+    // create a callback for the topic
+    auto buffer = observation_buffers_.back();
+    if (data_type == "LaserScan") {


There's a lot of redundancy in this code block. I realize it is temporary code and should go away when message filters is enabled, but who knows how long temporary is. I'll leave it up to you whether you want to fix it or leave it alone such that your name comes up when somebody runs git blame ;-)

The message filters code is equally redundant, my format is pretty much the same. I'm ok leaving as is for now.

Ok. For future reference, I think what I'd have done here is just define a macro at the start of the function and then used that to simplify the subscription creation code.

#define CREATE_SUBSCRIBER(message_type, callback) \ node_->create_subscription<message_type>(topic, [&, buffer] (message_type::ConstSharedPtr message) \ { callback(message, buffer); }, custom_qos_profile); ... if (data_type == "LaserScan") { if (inf_is_valid) { sub = CREATE_SUBSCRIBER(sensor_msgs::msg::LaserScan, laserScanValidInfCallback); } else { sub = CREATE_SUBSCRIBER(sensor_msgs::msg::LaserScan, laserScanValidCallback); } } else { if (inf_is_valid) { RCLCPP_WARN ... } else { sub = CREATE_SUBSCRIBER(sensor_msgs::msg::PointCloud2, pointCloud2Callback); } }

bpwilcox added enhancement New feature or request costmap_2d 0 - Critical Critical to project, highest priority labels Nov 13, 2018

bpwilcox added this to the November 2018 milestone Nov 13, 2018

bpwilcox requested review from mjeronimo, mkhansenbot, SteveMacenski, a user and orduno November 13, 2018 19:30

SteveMacenski reviewed Nov 13, 2018

View reviewed changes

bpwilcox force-pushed the port_obstacle branch 5 times, most recently from 98136ca to ddfb26e Compare November 14, 2018 07:58

bpwilcox mentioned this pull request Nov 14, 2018

Voxel Layer Port #322

Merged

mjeronimo approved these changes Nov 14, 2018

View reviewed changes

bpwilcox force-pushed the port_obstacle branch from ddfb26e to 3164bba Compare November 14, 2018 20:01

SteveMacenski and others added 8 commits November 14, 2018 12:02

starting port of obstacle_layer

25b0dd0

base include in CMakaeLists and first stab port of obstacle layer continued porting obstacle layer changes resolving everything for next person except stringstream + message filters issues add back current amcl changes add back current dockerfile changes

fixing port issues with message filters and grabbing parameters off node

cc9a20c

added dynamic params callback functionality

b6418a3

renable dwbplugin tests

replace message filter subscriptions with rclcpp subscriptions

51fa986

adding obstacle layer to costmap2DROS plugin param

a602c84

clean up commented out code

2d56bd7

adding source prefix to sensor parameters

1ae2894

adding plugin name prefix to dynamic parameters

3164bba

adding comment

ghost approved these changes Nov 14, 2018

View reviewed changes

removing specifiers and unnecessary function argument

2de7ed8

bpwilcox merged commit 0215a75 into ros-navigation:master Nov 14, 2018

bpwilcox mentioned this pull request Nov 14, 2018

Port obstacle layer in nav2_costmap_2d #192

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Obstacle Layer Port #318

Obstacle Layer Port #318

bpwilcox commented Nov 13, 2018

SteveMacenski commented Nov 13, 2018

SteveMacenski Nov 13, 2018

bpwilcox Nov 13, 2018

SteveMacenski Nov 13, 2018 •

edited

Loading

bpwilcox Nov 13, 2018

SteveMacenski Nov 13, 2018

bpwilcox Nov 13, 2018

mjeronimo Nov 14, 2018

bpwilcox Nov 14, 2018

ghost Nov 14, 2018

bpwilcox Nov 14, 2018

mjeronimo Nov 14, 2018

ghost Nov 14, 2018

ghost Nov 14, 2018

bpwilcox Nov 14, 2018

ghost Nov 14, 2018

bpwilcox Nov 14, 2018

ghost Nov 14, 2018

Obstacle Layer Port #318

Obstacle Layer Port #318

Conversation

bpwilcox commented Nov 13, 2018

Basic Info

Description of contribution in a few bullet points

Future work that may be required in bullet points

SteveMacenski commented Nov 13, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

SteveMacenski Nov 13, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

SteveMacenski Nov 13, 2018 •

edited

Loading