Check for collisions in motion primitives #889

bpwilcox · 2019-06-25T01:09:20Z

This PR requires #737.

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#533
Primary OS tested on	Ubuntu 18.04
Robotic platform tested on	Turtlebot3

Description of contribution in a few bullet points

Adds collision checking into spin and backup actions
changed default robot radius to 220 mm to reflect turtlebot3 radius for waffle model

Future work that may be required in bullet points

Add robust spin and backup tests

bpwilcox · 2019-06-26T22:30:13Z

Changing the robot radius default to match the turtlebot3 documentation significantly improved the performance both during normal operation and using motion primitives. The footprint is centered off the base link, which is seemingly the between the wheels on the front, so it is not symmetrical around the robot's center. We may want to define the actual pointwise footprint later.

orduno · 2019-06-27T18:02:14Z

The footprint is centered off the base link, which is seemingly the between the wheels on the front, so it is not symmetrical around the robot's center. We may want to define the actual pointwise footprint later.

One alternative would be to define a static transform between the base link and the robot center. Seems like the costmaps should reference the center of the robot rather than the location of the drive system. We can open an issue and address this later.

mjeronimo · 2019-06-27T18:18:09Z

nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp

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


Why the line break for these three methods?

nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp

mjeronimo · 2019-06-27T18:20:52Z

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp

@@ -77,11 +78,13 @@ class Costmap2DPublisher
  {
    costmap_pub_->on_activate();
    costmap_update_pub_->on_activate();
+    costmap_raw_pub_->on_activate();


Reading the code, I'm not clear yet on what costmap_raw_pub_ is. A comment would be helpful. I suppose there is some kind of raw costmap that is being published?

I wanted to change the name of all the pubs actually. The costmap_raw_pub publishes the costmap using the native values from the costmap2D array. The original costmap_pub' is publishing a translated occupancy grid version for rviz visualization. Would it perhaps be more helpful to change the costmap_pubtocostmap_rviz_pub`?

No, I wouldn't specify who I expect to be listening. Just keep it to a topic name that anyone can listen to.

mjeronimo · 2019-06-27T18:22:21Z

nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp

+    nav2_util::LifecycleNode::SharedPtr node,
+    std::string & topic_name);
+
+  CostmapSubscriber(


Nice. Glad you included this version of the constructor. I'm thinking we should standardize on provided all three kinds of constructors: rclcpp::Node, rclcpp_lifecycle::LifecycleNode, and the interfaces version. The first two end up just using the third and are provided for convenience.

Right, I'll add the rclcpp::Node version. Also, in the class I'm only currently using say one of the node interfaces, should we always stick to pattern even if we never use the other interfaces?

If we expect it to be a class that might have multiple clients, then I would include them. If it's done specifically for just one module implementation, you can probably skip it.

mjeronimo · 2019-06-27T18:27:41Z

nav2_costmap_2d/src/collision_checker.cpp

+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_costmap_2d/collision_checker.hpp"


FYI - The convention I've been using for implementation (.cpp) files:

Include the file's header first. In this case collision_checker.hpp. Next, another section to include any standard header files. Finally, the last section includes any headers that are required by the implementation. Each of the three sections are sorted.

So, in this case, it would look like:

#include "nav2_costmap_2d/collision_checker.hpp" #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_costmap_2d/exceptions.hpp" #include "nav2_costmap_2d/footprint.hpp" #include "nav2_util/line_iterator.hpp"

I use that convention too, this was an oversight, thanks for catching!

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

nav2_costmap_2d/test/integration/test_collision_checker.cpp

+    collision_checker_ = std::make_unique<nav2_costmap_2d::CollisionChecker>(
+      costmap_sub_, footprint_sub_, get_name());
+
+    get_robot_pose_service_ = rclcpp_node_->create_service<nav2_msgs::srv::GetRobotPose>(


I think you should be able to create the service off of the main lifecycle node and therefore not require the rclcpp_node.

I had tried to do that before, but it did not work since the service was hanging because I'm not actually spinning the lifecycle node here in this test. I used the rclcpp_node because it is spinning in a thread.

mjeronimo · 2019-06-27T18:35:24Z

nav2_costmap_2d/test/integration/test_collision_checker.cpp

+  nav2_msgs::msg::Costmap
+  toCostmapMsg(nav2_costmap_2d::Costmap2D * costmap)
+  {
+    double resolution = costmap->getResolution();


A personal, stylistic thing, but I'd organize this code to get the various values first, and then populate the structure. For example,

double resolution = costmap->getResolution(); double wx, wy; costmap->mapToWorld(0, 0, wx, wy); unsigned char * data = costmap->getCharMap(); nav2_msgs::msg::Costmap costmap_msg; costmap_msg.header.frame_id = global_frame_; costmap_msg.header.stamp = now(); costmap_msg.metadata.layer = "master"; costmap_msg.metadata.resolution = resolution; costmap_msg.metadata.size_x = costmap->getSizeInCellsX(); costmap_msg.metadata.size_y = costmap->getSizeInCellsY(); costmap_msg.metadata.origin.position.x = wx - resolution / 2; costmap_msg.metadata.origin.position.y = wy - resolution / 2; costmap_msg.metadata.origin.position.z = 0.0; costmap_msg.metadata.origin.orientation.w = 1.0; costmap_msg.data.resize(costmap_msg.metadata.size_x * costmap_msg.metadata.size_y); for (unsigned int i = 0; i < costmap_msg.data.size(); i++) { costmap_msg.data[i] = data[i]; } return costmap_msg;

I think the result is a bit easier to read and to see all of the structure fields.

Sure, I like that.

mjeronimo · 2019-06-27T18:36:36Z

nav2_motion_primitives/src/spin.cpp

+  if (!collision_checker_->isCollisionFree(pose2d)) {
+    stopRobot();
+    RCLCPP_INFO(node_->get_logger(), "Collision Ahead -Exiting Spin ");
+    return Status::SUCCEEDED;


Should is case be considered successful? We weren't able to fully execute the spin.

I thought about this, so I welcome feedback. If we return failure, then I noticed that the recovery behaviors exit and the system returns navigation failed. If we have another recovery behavior queued after spin, then we'd want to attempt to execute it even if we could not fully execute the spin. Also, I don't think that being unable to execute the full spin because of an impending collision is considered a "failure". If we can only turn 90 degrees out of the full 180 because a block prevents the rest, I'd still consider that recovery a success so far as it was implemented without any system failures or missing any data dependencies. If we DID consider it a failure, then I would still want that we don't break out of the recovery tree.

I think we should return FAILED, the action server will set the goal as aborted in that case.

I don't think we should abort the goal. Even if we consider this one recovery behavior "FAILED", I don't think we should necessarily abort the goal, since the next recovery could be successful. But firstly, I would want to hear why you think we should consider it a failure if the spin doesn't complete because of an impending obstacle. To me, that's sensible behavior that you would spin as much as you can before finishing and moving onto next recovery if needed, not that the spin itself failed.

Ok with me to return FAILED for now, but we should address this ASAP.

orduno

Review in-progress...

orduno · 2019-06-27T18:11:47Z

nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp

+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_subscriber.hpp"


Minor recommendation, some of these includes could be replaced with forward declarations:

class CostmapSubscriber; class FootprintSubscriber; class Costmap2D;

orduno · 2019-06-27T18:14:35Z

nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp

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


Not sure if getRobotPose() is something we want from the CollisionChecker, maybe better to make it private.

I made it public so that I can call it in the motion primitives as opposed to creating a new service and function for the motion primitives. My assumption was that since the motion primitives base class creates a collision_checker, it would be redundant to duplicate this functionality. What do you think?

nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp

orduno · 2019-06-27T18:20:38Z

nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp

+
+  std::string name_;
+  nav2_util::GetRobotPoseClient get_robot_pose_client_{"collision_checker"};
+  std::shared_ptr<CostmapSubscriber> costmap_sub_;


Consider references instead of pointers.

orduno · 2019-06-27T18:30:10Z

nav2_costmap_2d/src/collision_checker.cpp

+}
+
+bool
+CollisionChecker::getRobotPose(geometry_msgs::msg::Pose & current_pose)


this method can be const

orduno · 2019-06-27T18:33:40Z

nav2_costmap_2d/src/collision_checker.cpp

+  auto request = std::make_shared<nav2_util::GetRobotPoseClient::GetRobotPoseRequest>();
+
+  auto result = get_robot_pose_client_.invoke(request, 1s);
+  if (!result.get()->is_pose_valid) {


!result->is_pose_valid should also work.

orduno · 2019-06-27T18:34:40Z

nav2_costmap_2d/src/collision_checker.cpp

+  if (!result.get()->is_pose_valid) {
+    return false;
+  }
+  current_pose = result.get()->pose.pose;


No need to get the pointed object.

orduno · 2019-06-27T18:37:07Z

nav2_costmap_2d/src/collision_checker.cpp

+  return line_cost;
+}
+
+double CollisionChecker::pointCost(int x, int y)


this method can be const

orduno · 2019-06-27T18:37:32Z

nav2_costmap_2d/src/collision_checker.cpp

+  return footprint_cost;
+}
+
+double CollisionChecker::lineCost(int x0, int x1, int y0, int y1)


this method can be const

orduno · 2019-06-27T18:41:32Z

nav2_costmap_2d/src/collision_checker.cpp

+
+  // now we really have to lay down the footprint in the costmap_ grid
+  unsigned int x0, x1, y0, y1;
+  double line_cost = 0.0;


line_cost could be declared inside the for loop

Why would we want to do that here?

adding test cases move collision checker to nav2_costmap_2d

revert dwb_critics cmake changes

orduno

Looks very good. Just some minor recommendations.

orduno · 2019-06-27T18:45:01Z

nav2_costmap_2d/src/collision_checker.cpp

+    }
+
+    line_cost = lineCost(x0, x1, y0, y1);
+    footprint_cost = std::max(line_cost, footprint_cost);


max_cost = std::max(lineCost(...), max_cost);

orduno · 2019-06-27T18:46:05Z

nav2_costmap_2d/src/collision_checker.cpp

+  // we need to rasterize each line in the footprint
+  for (unsigned int i = 0; i < footprint.size() - 1; ++i) {
+    // get the cell coord of the first point
+    if (!costmap_->worldToMap(footprint[i].x, footprint[i].y, x0, y0)) {


Maybe make this a method.

orduno · 2019-06-27T21:30:44Z

nav2_costmap_2d/src/costmap_2d_publisher.cpp

@@ -59,6 +59,8 @@ Costmap2DPublisher::Costmap2DPublisher(
  // TODO(bpwilcox): port onNewSubscription functionality for publisher
  costmap_pub_ = ros_node->create_publisher<nav_msgs::msg::OccupancyGrid>(topic_name,
      custom_qos);
+  costmap_raw_pub_ = ros_node->create_publisher<nav2_msgs::msg::Costmap>(topic_name + "_raw",


use node_ instead

orduno · 2019-06-27T21:32:24Z

nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp

+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+
+  void toCostmap2D();
+  void costmap_callback(const nav2_msgs::msg::Costmap::SharedPtr msg);


costmapCallback

orduno · 2019-06-27T21:34:34Z

nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp

+public:
+  explicit CollisionCheckerException(const std::string description)
+  : std::runtime_error(description) {}
+  typedef std::shared_ptr<CollisionCheckerException> Ptr;


typedef not used here

orduno · 2019-06-27T21:41:10Z

nav2_motion_primitives/src/back_up.cpp

@@ -45,8 +45,8 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)

  command_x_ = command->target.x;

-  if (!robot_->getOdometry(initial_pose_)) {
-    RCLCPP_ERROR(node_->get_logger(), "initial robot odom pose is not available.");
+  if (!collision_checker_->getRobotPose(initial_pose_)) {


Alternatively use the GetRobotPoseClient.

So we duplicate the GetRobotPoseClient here and in the collision_checker?

Based on our conversation, can we reference an issue # so we can track it later?

orduno · 2019-06-27T21:42:55Z

nav2_motion_primitives/src/back_up.cpp

+
+  if (!collision_checker_->isCollisionFree(pose2d)) {
+    stopRobot();
+    RCLCPP_INFO(node_->get_logger(), "Collision Ahead - Exiting BackUp");


Make this RCLCPP_WARN

orduno · 2019-06-27T21:44:25Z

nav2_motion_primitives/src/back_up.cpp

+  if (!collision_checker_->isCollisionFree(pose2d)) {
+    stopRobot();
+    RCLCPP_INFO(node_->get_logger(), "Collision Ahead - Exiting BackUp");
+    return Status::SUCCEEDED;


return FAILED

As discussed, let's return SUCCEEDED for now, but open an issue to change it soon.

orduno · 2019-06-27T21:44:59Z

nav2_motion_primitives/src/spin.cpp

+  if (!collision_checker_->isCollisionFree(pose2d)) {
+    stopRobot();
+    RCLCPP_INFO(node_->get_logger(), "Collision Ahead -Exiting Spin ");
+    return Status::SUCCEEDED;


I think we should return FAILED, the action server will set the goal as aborted in that case.

… for collision checker

mjeronimo · 2019-06-28T22:17:56Z

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp

@@ -77,11 +78,13 @@ class Costmap2DPublisher
  {
    costmap_pub_->on_activate();
    costmap_update_pub_->on_activate();
+    costmap_raw_pub_->on_activate();


No, I wouldn't specify who I expect to be listening. Just keep it to a topic name that anyone can listen to.

mjeronimo · 2019-06-28T22:18:54Z

nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp

+    nav2_util::LifecycleNode::SharedPtr node,
+    std::string & topic_name);
+
+  CostmapSubscriber(


If we expect it to be a class that might have multiple clients, then I would include them. If it's done specifically for just one module implementation, you can probably skip it.

mjeronimo · 2019-06-28T22:19:33Z

nav2_costmap_2d/test/integration/test_collision_checker.cpp

+    collision_checker_ = std::make_unique<nav2_costmap_2d::CollisionChecker>(
+      costmap_sub_, footprint_sub_, get_name());
+
+    get_robot_pose_service_ = rclcpp_node_->create_service<nav2_msgs::srv::GetRobotPose>(


mjeronimo · 2019-06-28T22:20:23Z

nav2_motion_primitives/src/spin.cpp

+  if (!collision_checker_->isCollisionFree(pose2d)) {
+    stopRobot();
+    RCLCPP_INFO(node_->get_logger(), "Collision Ahead -Exiting Spin ");
+    return Status::SUCCEEDED;


Ok with me to return FAILED for now, but we should address this ASAP.

bpwilcox force-pushed the motion_primitives_collision_checking branch 2 times, most recently from 53d6c02 to 8ab347d Compare June 26, 2019 22:23

bpwilcox requested review from orduno, mjeronimo, crdelsey and mkhansenbot June 26, 2019 22:30

bpwilcox changed the title ~~[WIP] Check for collisions in motion primitives~~ Check for collisions in motion primitives Jun 26, 2019

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

mjeronimo reviewed Jun 27, 2019

View reviewed changes

orduno suggested changes Jun 27, 2019

View reviewed changes

bpwilcox added 9 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

address review feedback, refactor test

a1b7b1f

use GetRobotPose service

7e8df0d

address review feedback

6a4b217

orduno suggested changes Jun 27, 2019

View reviewed changes

orduno mentioned this pull request Jun 27, 2019

Add classes for collision checking #737

Closed

address additional feedback

89504be

bpwilcox force-pushed the motion_primitives_collision_checking branch from 8ab347d to 1337c4f Compare June 28, 2019 00:17

crdelsey mentioned this pull request Jun 28, 2019

Motion Primitives should return failure if they would collide #899

Closed

orduno approved these changes Jun 28, 2019

View reviewed changes

bpwilcox added 3 commits June 28, 2019 14:35

pass reference to GetPose client in collision checker constructor

743c3c3

add collision checker classes to motion primitives

84c3031

use tf robot pose and return succeeded

7bfe5a3

bpwilcox added 7 commits June 28, 2019 14:40

pass colon test

6f334af

remove tf and replace Odom with pose for backup

7c91f06

use stopRobot and fix rebase issue

a7a3a82

update turtlebot3 waffle default radius, use global costmap/footprint…

67936d8

… for collision checker

change info to warn in spin/backup

9c633ac

pass costmap and footprint subs by reference

a2a4131

create getRobotPose client in motion_primitive

da4f12c

bpwilcox force-pushed the motion_primitives_collision_checking branch from 615776e to da4f12c Compare June 28, 2019 22:05

mjeronimo approved these changes Jun 28, 2019

View reviewed changes

bpwilcox merged commit 328983d into ros-navigation:master Jun 29, 2019

Check for collisions in motion primitives #889

Check for collisions in motion primitives #889

Conversation

bpwilcox commented Jun 25, 2019 • edited Loading

Basic Info

Description of contribution in a few bullet points

Future work that may be required in bullet points

bpwilcox commented Jun 26, 2019

orduno commented Jun 27, 2019

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

Choose a reason for hiding this comment

Choose a reason for hiding this comment

bpwilcox Jun 27, 2019 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

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

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

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

bpwilcox commented Jun 25, 2019 •

edited

Loading

bpwilcox Jun 27, 2019 •

edited

Loading