Add classes for collision checking #737

bpwilcox · 2019-05-18T00:32:13Z

This PR adds classes to abstract the footprint and costmap2D across the ROS2 topic interface and enable collision checking in other modules. The collision checking code is modified from the ObstacleFootprintCritic methods

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#533

Description of contribution in a few bullet points

Publishes a nav2_msgs::msg::Costmap in the CostmapPublisher
Adds a FootprintSubscriber class
Adds a CostmapSubscriber class
Adds a CollisionChecker class
Adds a test for the CollisionChecker
duplicates line_iterator.hpp in nav2_util (to be later removed from dwb_critics)

Future work

Use above classes in recovery behaviors
Refactor dwb_controller to utilize subscriber classes and collision checker

SteveMacenski · 2019-05-18T00:53:59Z

nav2_costmap_2d/src/collision_checker.cpp

+
+CollisionChecker::~CollisionChecker() {}
+
+bool CollisionChecker::isCollisionFree(


Exceptions in C++ to my understanding are pretty heavy, could we have this isCollisionFree return an enum type with the type of failure rather than a bool?

The only package in navigation that uses exceptions heavily is DWB and I dont thin kwe should add more. Printing the exception text is only useful for debug, but that doesnt help much for autonomous systems where the handler of isCollisionFree may want to know why to react appropriately.

I'll look and give some thought into returning enum types. The exceptions here are modified from those in DWB as you mentioned.

IMO exceptions should be used when the local code can't recover from the error but someone up the stack may be able to. They shouldn't be used to propagate a normal return value. For example, if isCollisionFree may commonly return true or false, a return value is more appropriate. So, both exceptions and return values are appropriate, depending on the circumstances, and should be applied the right way.

I could modify scorePose to return a boolean and replace isCollisionFree, but in this PR I was moving the code (including the exceptions) from the ObstacleFootprint critic which returns a score for the pose requested. My intention was to replace the ObstacleFootprint critic code with this class to avoid code duplication, which is why I thought to keep it as is.

nav2_costmap_2d/src/collision_checker.cpp

SteveMacenski · 2019-05-18T00:56:15Z

nav2_costmap_2d/src/collision_checker.cpp

+}
+
+bool
+CollisionChecker::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose) const


This should probably be moved to the Robot class/util for getting pose and then this should be a consumer of that so we don't have raw TF everywhere

I agree. We should move the one in Costmap2dROS as well.

SteveMacenski · 2019-05-18T00:56:53Z

nav2_costmap_2d/src/collision_checker.cpp

@@ -0,0 +1,209 @@
+// Copyright (c) 2019 Intel Corporation


I like this alot. We should remove the same code from costmap_2d to use this so we dont have repeating sections to update if bugs are found later.

nav2_costmap_2d/src/costmap_subscriber.cpp

SteveMacenski · 2019-05-18T01:00:05Z

nav2_costmap_2d/src/footprint_subscriber.cpp

@@ -0,0 +1,52 @@
+// Copyright (c) 2019 Intel Corporation


I see no issue with this, but I would believe that having this read the configuration file and it being a ROS agnostic helper to read from configuration file and getting as necessary may be more clear since somewhere else inside of costmap_2d we read the footprint and then publish it. Seems odd to have things running in the same process in the same node talking to each other over ROS to share data

I believe in Costmap2DROS, the footprint subscription is listening on a Polygon topic in order to update the footprint across all layers via setUnpaddedFootprint in its callback. Since in this class we're listening on the published PolygonStamped topic and have a default callback to convert and store the robot oriented footprint, I see them as separate use cases.

orduno

Mostly style recommendations, just one or two things that need to be fixed before merging.

orduno · 2019-05-21T20:35:45Z

nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp

+
+  ~CollisionChecker();
+
+  double scorePose(


Maybe add a comment about the range and meaning of this score.

orduno · 2019-05-21T20:39:13Z

nav2_costmap_2d/src/collision_checker.cpp

+  const geometry_msgs::msg::Pose2D & pose)
+{
+  try {
+    if (scorePose(pose) < 0) {


A score < 0 means collision?

Yes, this is according to the code in ObstacleFootprintCritic

orduno · 2019-05-21T20:41:20Z

nav2_costmap_2d/src/collision_checker.cpp

+double CollisionChecker::scorePose(
+  const geometry_msgs::msg::Pose2D & pose)
+{
+  Costmap2D * costmap_;


costmap_ -> costmap

Why have a costmap pointer? Maybe move the worldToMap checks to CostmapSubscriber

Because the intention of the CostmapSubscriber was to convert the msg into the Costmap2D object to access its API calls. I think it's better not to duplicate that code in the subscriber class

orduno · 2019-05-21T20:54:36Z

nav2_costmap_2d/src/collision_checker.cpp

+    throw CollisionCheckerException(e.what());
+  }
+
+  unsigned int cell_x, cell_y;


Looks like you don't actually need cell_x, cell_y but only checking if pose is within the map.
And since it's used on multiple places, maybe make a separate function isOnMap(x,y)?
If so, maybe put costmap_ inside this function.
And ideally the whole check inside CostmapSubscriber.

To get the bool flag, no we don't need the cell_x, cell_y, though I think it is useful for debugging and it is part of the Costmap2D API. I'm opposed to doing the check inside CostmapSubscriber because that class should be general for anyone subscribing to the costmap topic, not just for collision checking

orduno · 2019-05-21T20:57:45Z

nav2_costmap_2d/src/collision_checker.cpp

+
+  Footprint footprint_spec;
+  unorientFootprint(footprint, footprint_spec);
+  transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint);


Suggestion: auto footprint = getUnorientedFootprint(pose) and move code above to this function.

orduno · 2019-05-21T21:28:43Z

nav2_costmap_2d/src/collision_checker.cpp

+
+double CollisionChecker::pointCost(int x, int y)
+{
+  Costmap2D * costmap_ = costmap_sub_->getCostmap();


costmap_ -> costmap

orduno · 2019-05-21T21:36:50Z

nav2_costmap_2d/src/costmap_subscriber.cpp

+void CostmapSubscriber::toCostmap2D()
+{
+  if (!costmap_received_) {
+    costmap_ = new Costmap2D(


Where do we delete? alternatively use std::shared_ptr

Or unique_ptr if it is not shared.

orduno · 2019-05-21T21:45:18Z

nav2_costmap_2d/src/costmap_subscriber.cpp

+      std::bind(&CostmapSubscriber::costmap_callback, this, std::placeholders::_1));
+}
+
+Costmap2D * CostmapSubscriber::getCostmap()


Instead of providing the costmap to the clients, can we define an interface that provides what the clients really need, i.e. poseWithinMap(), poseCost(), etc.

I think the costmap_subscriber would eventually become the client:

Perhaps, though I would address this as a future feature of the world model in a different PR. Also, I'm not sure how much client-specific context we'd really like at the interface level. Calling something like poseCost() is very context-specific and I'm not sure in the scope of the WorldModel. In my view, the WorldModel could be a plugin container for abstract map representations (of which costmap2D is one) of the environment from which clients can grab and use as needed on their end. We can continue this discussion

nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp

orduno · 2019-05-21T21:50:52Z

nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics


Add:
Copyright (c) 2019, Intel Corporation

This looks like mostly copied code. I wouldn't change the copyright.

mjeronimo · 2019-06-18T17:58:06Z

nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp

+{
+public:
+  CollisionChecker(
+    rclcpp::Node::SharedPtr ros_node,


Should make this work for both lifecycle nodes and non-lifecycle nodes. Typical method would be to accept any required interface pointers.

mjeronimo · 2019-06-18T17:59:06Z

nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp

+{
+public:
+  CostmapSubscriber(
+    rclcpp::Node::SharedPtr ros_node,


Same here. Should take interface pointers.

mjeronimo · 2019-06-18T17:59:55Z

nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp

+{
+public:
+  FootprintSubscriber(
+    rclcpp::Node::SharedPtr ros_node,


Should work for lifecycle and non-lifecycle nodes.

mjeronimo · 2019-06-18T18:02:59Z

nav2_costmap_2d/src/costmap_subscriber.cpp

+void CostmapSubscriber::toCostmap2D()
+{
+  if (!costmap_received_) {
+    costmap_ = new Costmap2D(


Or unique_ptr if it is not shared.

bpwilcox · 2019-06-21T02:02:32Z

I've addressed feedback, so this PR is open again for review. I'm adding the WIP to note that I am waiting on PR #793 to be merged so I can get rid of the tf_buffer to grab the robot pose and instead use the service call. Right now, I've hard-coded the global frames so that I don't have to pass in a node in the CollisionChecker just to grab params off the node.

bpwilcox · 2019-06-26T01:51:24Z

Open again for review now that #793 is merged and modifications were made.

mjeronimo · 2019-06-27T18:44:10Z

nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp

+#include "nav2_costmap_2d/footprint_subscriber.hpp"
+#include "nav2_util/get_robot_pose_client.hpp"
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"


Not gating this PR, but perhaps you can submit an issue to fix this upstream so we don't have to work around the warnings.

mjeronimo · 2019-06-27T18:44:46Z

nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp

+
+  double scorePose(
+    const geometry_msgs::msg::Pose2D & pose);
+  bool isCollisionFree(


Why the line breaks for these methods? Makes for inconsistent formatting.

mjeronimo · 2019-06-27T18:50:31Z

nav2_costmap_2d/src/collision_checker.cpp

+
+CollisionChecker::~CollisionChecker() {}
+
+bool CollisionChecker::isCollisionFree(


IMO exceptions should be used when the local code can't recover from the error but someone up the stack may be able to. They shouldn't be used to propagate a normal return value. For example, if isCollisionFree may commonly return true or false, a return value is more appropriate. So, both exceptions and return values are appropriate, depending on the circumstances, and should be applied the right way.

adding test cases move collision checker to nav2_costmap_2d

revert dwb_critics cmake changes

orduno · 2019-06-27T22:29:12Z

@bpwilcox is this PR replaced by #889?

bpwilcox · 2019-06-28T00:09:23Z

@orduno This PR was intended to be a prerequisite for #889 (stated in its description) as it is built off of this PR. Since there was feedback on the same commits from this PR, I'm just addressing all the feedback here.

bpwilcox · 2019-06-29T00:01:54Z

Closing because #889 is merged.

bpwilcox requested review from crdelsey, orduno, mjeronimo, mkhansenbot and mhpanah May 18, 2019 00:33

SteveMacenski reviewed May 18, 2019

View reviewed changes

orduno suggested changes May 21, 2019

View reviewed changes

crdelsey added this to the D Turtle Release milestone Jun 10, 2019

crdelsey added the 1 - High High Priority label Jun 10, 2019

mjeronimo reviewed Jun 18, 2019

View reviewed changes

bpwilcox mentioned this pull request Jun 20, 2019

Transform robot pose lifecycle #793

Merged

bpwilcox force-pushed the add_classes_for_collision_checking branch from 3f904d7 to 3314b10 Compare June 21, 2019 01:54

bpwilcox changed the title ~~Add classes for collision checking~~ [WIP] Add classes for collision checking Jun 21, 2019

bpwilcox mentioned this pull request Jun 25, 2019

Check for collisions in motion primitives #889

Merged

bpwilcox force-pushed the add_classes_for_collision_checking branch from 3314b10 to 9e4e8df Compare June 26, 2019 01:17

bpwilcox requested review from orduno, SteveMacenski and mjeronimo June 26, 2019 01:29

bpwilcox changed the title ~~[WIP] Add classes for collision checking~~ Add classes for collision checking Jun 26, 2019

bpwilcox force-pushed the add_classes_for_collision_checking branch from 9e4e8df to 7add0ba Compare June 26, 2019 21:25

mjeronimo approved these changes Jun 27, 2019

View reviewed changes

mjeronimo self-requested a review June 27, 2019 18:52

bpwilcox added 6 commits June 27, 2019 13:38

add footprint_subscriber

9a2bd8f

add costmap_subscriber and publish nav2 costmap msgs

48c55df

add collision_checker class

dcd2e5d

adding test for collision checker

cd0d916

adding test cases move collision checker to nav2_costmap_2d

fixed uncrustify

a547347

revert dwb_critics cmake changes

pass test on lifecycle

2ffbe48

bpwilcox added 3 commits June 27, 2019 13:38

address review feedback, refactor test

a1b7b1f

use GetRobotPose service

7e8df0d

address review feedback

6a4b217

bpwilcox force-pushed the add_classes_for_collision_checking branch from 7add0ba to 6a4b217 Compare June 27, 2019 21:38

address additional feedback

89504be

orduno approved these changes Jun 28, 2019

View reviewed changes

crdelsey mentioned this pull request Jun 28, 2019

Remove duplicate collision checker code #898

Closed

pass reference to GetPose client in collision checker constructor

743c3c3

bpwilcox closed this Jun 29, 2019


		CollisionChecker::~CollisionChecker() {}

		bool CollisionChecker::isCollisionFree(

Add classes for collision checking #737

Add classes for collision checking #737

Conversation

bpwilcox commented May 18, 2019

Basic Info

Description of contribution in a few bullet points

Future work

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

orduno left a comment • 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

orduno May 21, 2019 • 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

orduno May 21, 2019 • 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

bpwilcox commented Jun 21, 2019

bpwilcox commented Jun 26, 2019

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

orduno commented Jun 27, 2019

bpwilcox commented Jun 28, 2019

bpwilcox commented Jun 29, 2019

orduno left a comment •

edited

Loading

orduno May 21, 2019 •

edited

Loading

orduno May 21, 2019 •

edited

Loading