Improvements to costmap and path message definitions, and downstream changes to support these.

Author: orduno

src/control/src/DwaController.cpp

@@ -30,7 +30,7 @@ DwaController::~DwaController()
 }
 
 TaskStatus
-DwaController::executeAsync(const FollowPathCommand::SharedPtr command)
+DwaController::executeAsync(const FollowPathCommand::SharedPtr /*command*/)
 {
   RCLCPP_INFO(get_logger(), "DwaController::executeAsync");
 

src/nav2_msgs/msg/Costmap.msg

@@ -3,7 +3,7 @@
 std_msgs/Header header
 
 # MetaData for the map
-CostmapMetaData info
+CostmapMetaData metadata
 
 # The cost data, in row-major order, starting with (0,0).
 uint8[] data

src/nav2_msgs/msg/CostmapMetaData.msg

@@ -6,17 +6,17 @@ builtin_interfaces/Time map_load_time
 # The time of the last update to costmap
 builtin_interfaces/Time update_time
 
-# The corresponding layer
+# The corresponding layer name
 string layer
 
 # The map resolution [m/cell]
 float32 resolution
 
-# Map width [cells]
-uint32 width
+# Number of cells in the horizontal direction
+uint32 size_x
 
-# Map height [cells]
-uint32 height
+# Number of cells in the vertical direction
+uint32 size_y
 
 # The origin of the costmap [m, m, rad].
 # This is the real-world pose of the cell (0,0) in the map.

src/nav2_msgs/msg/Path.msg

@@ -1,4 +1,4 @@
 std_msgs/Header header
 
 # An array of poses that represents a Path for a robot to follow
-geometry_msgs/PoseStamped[] poses
+geometry_msgs/Pose[] poses

src/nav2_msgs/msg/PathEndPoints.msg

@@ -1,10 +1,10 @@
 std_msgs/Header header
 
 # The start pose for the plan
-geometry_msgs/PoseStamped start
+geometry_msgs/Pose start
 
 # The final pose of the goal
-geometry_msgs/PoseStamped goal
+geometry_msgs/Pose goal
 
 # If the goal is obstructed, how many meters the planner can
 # relax the constraint in x and y before failing.

src/navigation/src/SimpleNavigator.cpp

@@ -45,10 +45,10 @@ SimpleNavigator::executeAsync(const NavigateToPoseCommand::SharedPtr /*command*/
   // Compose the PathEndPoints message for Navigation
   auto endpoints = std::make_shared<ComputePathToPoseCommand>();
   // TODO(mdjeroni): get the starting pose from Localization (fake it out for now)
-  endpoints->start.pose.position.x = 1.0;
-  endpoints->start.pose.position.y = 1.0;
-  endpoints->goal.pose.position.x = 9.0;
-  endpoints->goal.pose.position.y = 9.0;
+  endpoints->start.position.x = 1.0;
+  endpoints->start.position.y = 1.0;
+  endpoints->goal.position.x = 9.0;
+  endpoints->goal.position.y = 9.0;
   endpoints->tolerance = 2.0;
 
   RCLCPP_INFO(get_logger(), "SimpleNavigator::executeAsync: getting the path from the planner");
@@ -94,9 +94,9 @@ SimpleNavigator::executeAsync(const NavigateToPoseCommand::SharedPtr /*command*/
   RCLCPP_INFO(get_logger(), "SimpleNavigator::executeAsync: got path of size %u",
     path->poses.size());
   int index;
-  for (auto poseStamped : path->poses) {
+  for (auto pose : path->poses) {
     RCLCPP_INFO(get_logger(), "SimpleNavigator::executeAsync: point %u x: %0.2f, y: %0.2f",
-      index, poseStamped.pose.position.x, poseStamped.pose.position.y);
+      index, pose.position.x, pose.position.y);
     index++;
   }
 

src/planning/CMakeLists.txt

@@ -20,7 +20,8 @@ include_directories(
   include
   ../task/include
   ../world_model/include
-  ../costmap/include
+  # ../util/costmap/include
+  ${costmap_INCLUDE_DIR}
   )
 
 link_directories(${CMAKE_BINARY_DIR}/../libs)

src/planning/include/planning/DijkstraPlanner.hpp

@@ -39,17 +39,17 @@ class DijkstraPlanner : public ComputePathToPoseTaskServer
 private:
   // Compute a plan given start and goal poses, provided in global world frame.
   bool makePlan(
-    const geometry_msgs::msg::PoseStamped & start,
-    const geometry_msgs::msg::PoseStamped & goal, double tolerance,
-    std::vector<geometry_msgs::msg::PoseStamped> & plan);
+    const geometry_msgs::msg::Pose & start,
+    const geometry_msgs::msg::Pose & goal, double tolerance,
+    nav2_msgs::msg::Path & plan);
 
   // Compute the navigation function given a seed point in the world to start from
   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);
+    const geometry_msgs::msg::Pose & goal,
+    nav2_msgs::msg::Path & plan);
 
   // Compute the potential, or navigation cost, at a given point in the world
   // - must call computePotential first
@@ -62,11 +62,11 @@ class DijkstraPlanner : public ComputePathToPoseTaskServer
 
   // Compute the squared distance between two points
   inline double squared_distance(
-    const geometry_msgs::msg::PoseStamped & p1,
-    const geometry_msgs::msg::PoseStamped & p2)
+    const geometry_msgs::msg::Pose & p1,
+    const geometry_msgs::msg::Pose & p2)
   {
-    double dx = p1.pose.position.x - p2.pose.position.x;
-    double dy = p1.pose.position.y - p2.pose.position.y;
+    double dx = p1.position.x - p2.position.x;
+    double dy = p1.position.y - p2.position.y;
     return dx * dx + dy * dy;
   }
 
@@ -87,9 +87,6 @@ class DijkstraPlanner : public ComputePathToPoseTaskServer
   // Wait for costmap server to appear
   void waitForCostmapServer(const std::chrono::seconds waitTime = std::chrono::seconds(10));
 
-  // Publish plan for visualization purposes
-  void publishPlan(const std::vector<geometry_msgs::msg::PoseStamped> & path);
-
   // Print costmap to terminal
   void printCostmap(const nav2_msgs::msg::Costmap & costmap);
 

src/planning/package.xml

@@ -5,9 +5,7 @@
   <version>0.1.0</version>
   <description>TODO</description>
   <author email="[email protected]">Oregon Robotics Team</author>
-  <author email="[email protected]">Carlos Orduno</author>
   <maintainer email="[email protected]">Oregon Robotics Team</maintainer>
-  <maintainer email="[email protected]">Carlos Orduno</maintainer>
   <license>Apache License 2.0</license>
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>rclcpp</build_depend>