Planner integration #20

orduno · 2018-08-09T22:45:15Z

Created the DijkstraPlanner based on the core algorithms from Navfn ROS1 nav package.
Updated the SimpleNavigator to use the new DijkstraPlanner
Created an initial WorldModel node and Costmap mockup to test the new planner.
Fixed bug with TaskClient.
Added a few new messages and services related to Costmap. Made a few updates to the Path messages.

mjeronimo · 2018-08-10T20:25:13Z

src/costmap/CMakeLists.txt

+endif()
+
+find_package(ament_cmake REQUIRED)
+# find_package(rclcpp REQUIRED)


Please remove commented-out sections to keep things tidy.

mjeronimo · 2018-08-10T20:26:01Z

src/costmap/CMakeLists.txt

+
+include_directories(include)
+
+add_library(costmap STATIC


We may want a general utility library that can hold Costmap, etc., instead of proliferating directories for each library.

Agree, made a note to discuss this on our next meeting.

As discussed, i moved it to a util library

mjeronimo · 2018-08-10T20:26:57Z

src/costmap/include/costmap/CostValues.h

+#ifndef WORLD_MODEL__COST_VALUES_H_
+#define WORLD_MODEL__COST_VALUES_H_
+/** Provides a mapping for often used cost values */
+namespace costmap


Please run ament_cpplint and ament_uncrustify on all code and address issues. It may not be practical yet for old code (Navfn), but should do as much as possible.

mjeronimo · 2018-08-10T20:28:25Z

src/costmap/package.xml

+  <version>0.1.0</version>
+  <description>TODO</description>
+  <author email="[email protected]">Oregon Robotics Team</author>
+   <author email="[email protected]">Carlos Orduno</author>


Indentation is off. Also should discuss (again) how we're handing author and maintainer fields.

Added to list of discussion topics.

mjeronimo · 2018-08-10T20:30:41Z

src/costmap/src/Costmap.cpp

+  RCLCPP_INFO(node_->get_logger(), "Costmap::getCostmap");
+
+  // TODO(orduno): faking out a costmap for now
+  (void)specifications;


Instead of casting to void, you can also comment out the name ("specifications") in the function interface (line 27). I prefer commenting out the name as it results in fewer lines of code.

mjeronimo · 2018-08-10T20:31:27Z

src/costmap/src/Costmap.cpp

+
+  // Some arbitrary numbers
+  costmap.info.resolution = 1;  // m/cell
+  costmap.info.width = 10;  // cells


Can we change the name of the field from "width" to make it clearer (and not require the comment)? The general principle being to improve the variable names to avoid needing comments to explain them.

Agree on the principle. In this case, a better name is number_cells_x_direction, same for height. However I took the variable name as a carry over from Occupancy Grid, and as we see on other ROS message definitions, the preference is to use something short. Added to the list of things to discuss.

mjeronimo · 2018-08-10T20:32:27Z

src/costmap/src/Costmap.cpp

+Costmap::getTestData(const int width, const int height, vector<uint8_t>& data)
+{
+  // TODO(orduno): fixed size for now
+  (void)width;


Comment out names on line 68?

mjeronimo · 2018-08-10T20:33:18Z

src/costmap/src/Costmap.cpp

+
+  // TODO(orduno): instead of hardcoding, define a function
+
+  const uint8_t n = 255; // no information


Each of these special values should have a symbol. It could be from an enumeration (with explicit values) or a macro. That way, the code wouldn't need comments and would be clearer.

Changed to enum.

After further discussion, it was changed to static const within Costmap class

mjeronimo · 2018-08-10T20:34:27Z

src/costmap/src/Costmap.cpp

+#include "costmap/Costmap.hpp"
+#include <tf2/LinearMath/Quaternion.h>
+
+using std::vector;


We should discuss our team conventions on whether we're opening namespaces (std) or not. We should be consistent as a team (and as individuals).

I'd be in favor of opening with some rules: not on header files, only introducing the name being used. Added to the discussion list.

mjeronimo · 2018-08-10T20:35:35Z

src/nav2_msgs/msg/Costmap.msg

+std_msgs/Header header
+
+# MetaData for the map
+CostmapMetaData info


Perhaps the field should be called "metadata" instead of "info."

This is related to a previous comment about ROS message definitions and carryover from ROS1. Added to the discussion list.

mjeronimo · 2018-08-10T20:35:53Z

src/nav2_msgs/msg/Costmap.msg

+CostmapMetaData info
+
+# The cost data, in row-major order, starting with (0,0).
+uint8[] data


Seems like there needs to be newline here.

mjeronimo · 2018-08-10T20:36:21Z

src/nav2_msgs/msg/CostmapMetaData.msg

+
+# The origin of the costmap [m, m, rad].
+# This is the real-world pose of the cell (0,0) in the map.
+geometry_msgs/Pose origin


mjeronimo · 2018-08-10T20:36:51Z

src/nav2_msgs/msg/PathEndPoints.msg

-geometry_msgs/PoseWithCovariance goal
+
+# The start pose for the plan
+geometry_msgs/PoseStamped start


Why do we need a stamped version of the pose? We have the timestamp in the header already. Same for the goal.

Currently not used. In future we might want to add time constraints to start and reach goal.

Fixed, Mike is right, posestamp is not needed, and we don't want to overload the term. If we need to add time constraints we'll have to add a new field.

mjeronimo · 2018-08-10T20:37:00Z

src/nav2_msgs/srv/GetCostmap.srv

+# Specifications for the requested costmap
+nav2_msgs/Costmap map
+---
+nav2_msgs/Costmap map


Fixed. Also realized that the request should be nav2_msgs/CostmapMetaData -- no need for the data field.

mjeronimo · 2018-08-10T20:38:38Z

src/navigation/src/SimpleNavigator.cpp

 #include <string>
 #include <memory>
 #include <exception>
 #include <chrono>
+#include <iostream>


I don't see that iostream is required. Still using it? Should remove any #includes that aren't required.

mjeronimo · 2018-08-10T20:38:57Z

src/planning/include/planning/AStarPlanner.hpp

@@ -28,4 +28,4 @@ class AStarPlanner : public ComputePathToPoseTaskServer
  TaskStatus executeAsync(const ComputePathToPoseCommand::SharedPtr command) override;
 };

-#endif  // PLANNING__ASTARPLANNER_HPP_
+#endif  // PLANNING__ASTARPLANNER_HPP_


mjeronimo · 2018-08-10T20:39:31Z

src/planning/include/planning/DijkstraPlanner.hpp

+  bool computePotential(const geometry_msgs::msg::Point& world_point);
+
+  // Compute a plan to a goal from a potential - must call computePotential first
+  bool getPlanFromPotential(const geometry_msgs::msg::PoseStamped& goal, std::vector<geometry_msgs::msg::PoseStamped>& plan);


Please run (all files) through ament_cpplint and ament_uncrustify. Several lines are too long.

mjeronimo · 2018-08-10T20:44:06Z

src/planning/include/planning/Navfn.hpp

+   * @class NavFn
+   * @brief Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based. Origin is upper left, x is right, y is down. 
+   */
+  class NavFn


We should refactor this class. It's not very clear as-is. We can probably extract out the various ideas into different (reusable) classes. Doesn't have to be done immediately though.

Agreed. Once we have a sufficient level of functionality and have done tests across the components, we can proceed to refactor / replace some of the core algos.

mjeronimo · 2018-08-10T20:44:32Z

src/planning/include/planning/Planner.hpp

@@ -0,0 +1,32 @@
+// Copyright (c) 2018 Intel Corporation


This file is obsolete and can be removed. It shouldn't be there in the ros2_devel branch.

mjeronimo · 2018-08-10T20:45:06Z

src/planning/package.xml

@@ -5,7 +5,9 @@
  <version>0.1.0</version>
  <description>TODO</description>
  <author email="[email protected]">Oregon Robotics Team</author>
+   <author email="[email protected]">Carlos Orduno</author>


indentation off.

mjeronimo · 2018-08-10T20:45:19Z

src/planning/src/AStarPlanner.cpp

@@ -55,4 +55,4 @@ AStarPlanner::executeAsync(const ComputePathToPoseCommand::SharedPtr command)
  setResult(result);

  return TaskStatus::SUCCEEDED;
-}
+}


Added, not sure how i removed those...

mjeronimo · 2018-08-10T20:46:04Z

src/planning/src/DijkstraPlanner.cpp

+  }
+  catch (...) {
+    RCLCPP_ERROR(this->get_logger(), "DijkstraPlanner::makePlan: failed to obtain costmap from server");
+    throw;


I recommend throwing a specific kind of exception. See the standard exception types.

The intention here is to re-throw the current exception.

mjeronimo · 2018-08-10T20:47:35Z

src/planning/src/DijkstraPlanner.cpp

+  planner_ = std::make_shared<NavFn>(costmap_.info.width, costmap_.info.height);
+
+  // Plan publisher for visualization purposes
+  plan_publisher_ = this->create_publisher<nav2_msgs::msg::Path>("plan", 1);


Did you intend to include the second parameter (qos_history_depth)? Or, is this a left-over from porting?

left-over from porting

mjeronimo · 2018-08-10T20:48:47Z

src/planning/src/Logger.h

@@ -0,0 +1,17 @@
+#pragma once


Should port the code to not require this header (and remove it).

Yes, also needs to be moved to a util folder

mjeronimo · 2018-08-10T20:50:12Z

src/world_model/package.xml

+  <version>0.1.0</version>
+  <description>TODO</description>
+  <author email="[email protected]">Oregon Robotics Team</author>
+   <author email="[email protected]">Carlos Orduno</author>


indentation

mjeronimo · 2018-08-10T20:51:20Z

src/world_model/src/WorldModel.cpp

+{
+  costmap_ = std::make_unique<Costmap>(this);
+
+  // Create a (lambda) callback function for when a costmap service is received.


Why a lambda function here versus a class method? Is there an advantage? In my opinion, it would be clearer to have a separate method and use it here.

I'm having problems defining a ROS2 service that takes in a class method for the callback.

mjeronimo · 2018-08-10T20:51:33Z

src/world_model/src/main.cpp

+  rclcpp::shutdown();
+
+  return 0;
+}


mjeronimo

A few items to address.

ghost · 2018-08-16T21:57:05Z

src/costmap/CMakeLists.txt

+  costmap
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib)
+


You should add these lines here so other packages can find this one.
ament_export_include_directories(include)
ament_export_libraries(costmap)

ghost · 2018-08-16T22:08:12Z

src/costmap/include/costmap/CostValues.h

+#define COSTMAP__COSTVALUES_H_
+
+/** Provides a mapping for often used cost values */
+enum class CostValue: uint8_t


I'm not a big fan of enums to represent constants due to various portability issues in the past. I'd rather just see these as static consts defined in the Costmap class.

ghost · 2018-08-16T22:26:29Z

src/costmap/src/Costmap.cpp

+}
+
+void
+Costmap::getCostmap(const nav2_msgs::msg::CostmapMetaData & /*specifications*/,


The coding standard says you should break after the parenthesis and put the first parameter on the next line.

Also, return type is typically on the same line in the coding standard, though I don't know if it is explicitly required.

eg.

void Costmap::getCostmap( const nav2_msgs::msg::CostmapMetaData & spec, nav2_msgs::msg::Costmap & costmap)

ghost · 2018-08-16T22:33:04Z

src/costmap/src/Costmap.cpp

+
+void
+Costmap::getCostmap(const nav2_msgs::msg::CostmapMetaData & /*specifications*/,
+  nav2_msgs::msg::Costmap & costmap)


Rather than use an output parameter here, I'd just return the costmap. eg.

nav2_msgs::msg::Costmap Costmap::getCostmap(const nav2_msgs::msg::CostmapMetaData & /*specifications*/) { nav2_msgs::msg::Costmap costmap; ... return costmap; }

The return value elision optimization will remove any overhead.

ghost · 2018-08-16T22:35:56Z

src/costmap/src/Costmap.cpp

+}
+
+void
+Costmap::getTestData(const int /*width*/, const int /*height*/, vector<uint8_t> & data)


Just leave out the parameters you aren't using. You can always add them when you need them.

ghost · 2018-08-16T22:37:50Z

src/costmap/src/Costmap.cpp

+     n,o,o,o,o,o,o,o,o,n,   //8
+     n,n,n,n,n,n,n,n,n,n};  //9
+
+  data = costmapMaze2;


If you made these a vector of vectors, you could select the maze you want by index. That way you could run all mazes in one test set instead of having to recompile every time you want to run a different maze.

Will modify once I integrate the PlannerTestNode, right now is just a place holder for the actual costmap.

ghost · 2018-08-16T23:08:17Z