Planner integration

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/mission_execution/package.xml

@@ -18,7 +18,7 @@
 
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>nav2_msgs</exec_depend>
-  
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/nav2_msgs/CMakeLists.txt

@@ -22,6 +22,9 @@ rosidl_generate_interfaces(nav2_msgs
   "msg/MissionPlan.msg"
   "msg/PathEndPoints.msg"
   "msg/Path.msg"
+  "msg/Costmap.msg"
+  "msg/CostmapMetaData.msg"
+  "srv/GetCostmap.srv"
   DEPENDENCIES builtin_interfaces geometry_msgs std_msgs
 )
 

src/nav2_msgs/msg/Costmap.msg

@@ -0,0 +1,9 @@
+# This represents a 2-D grid map, in which each cell has an associated cost
+
+std_msgs/Header header
+
+# MetaData for the map
+CostmapMetaData metadata
+
+# The cost data, in row-major order, starting with (0,0).
+uint8[] data

src/nav2_msgs/msg/CostmapMetaData.msg

@@ -0,0 +1,23 @@
+# This hold basic information about the characterists of the Costmap
+
+# The time at which the static map was loaded
+builtin_interfaces/Time map_load_time
+
+# The time of the last update to costmap
+builtin_interfaces/Time update_time
+
+# The corresponding layer name
+string layer
+
+# The map resolution [m/cell]
+float32 resolution
+
+# Number of cells in the horizontal direction
+uint32 size_x
+
+# 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.
+geometry_msgs/Pose origin

src/nav2_msgs/msg/Path.msg

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

src/nav2_msgs/msg/PathEndPoints.msg

@@ -1,3 +1,11 @@
 std_msgs/Header header
+
+# The start pose for the plan
 geometry_msgs/Pose start
-geometry_msgs/PoseWithCovariance goal
+
+# The final pose of the 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.
+float32 tolerance

src/nav2_msgs/package.xml

@@ -21,7 +21,7 @@
   <exec_depend>std_msgs</exec_depend>
 
   <member_of_group>rosidl_interface_packages</member_of_group>
-  
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/nav2_msgs/srv/GetCostmap.srv

@@ -0,0 +1,6 @@
+# Get the costmap
+
+# Specifications for the requested costmap
+nav2_msgs/CostmapMetaData specs
+---
+nav2_msgs/Costmap map

src/navigation/include/navigation/NavigateToPoseTaskMessages.hpp

@@ -15,10 +15,10 @@
 #ifndef NAVIGATION__NAVIGATETOPOSETASKMESSAGES_HPP_
 #define NAVIGATION__NAVIGATETOPOSETASKMESSAGES_HPP_
 
-#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 #include "std_msgs/msg/empty.hpp"
 
-using NavigateToPoseCommand = geometry_msgs::msg::PoseWithCovarianceStamped;
+using NavigateToPoseCommand = geometry_msgs::msg::PoseStamped;
 using NavigateToPoseResult = std_msgs::msg::Empty;
 
 #endif  // NAVIGATION__NAVIGATETOPOSETASKMESSAGES_HPP_

src/navigation/package.xml

@@ -20,7 +20,7 @@
 
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>nav2_msgs</exec_depend>
-  
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/navigation/src/SimpleNavigator.cpp

@@ -12,6 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+
+// Navigation Strategy based on:
+// Brock, O. and Oussama K. (1999). High-Speed Navigation Using
+// the Global Dynamic Window Approach. IEEE.
+// https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf
+
 #include <string>
 #include <memory>
 #include <exception>
@@ -22,7 +28,7 @@ SimpleNavigator::SimpleNavigator(const std::string & name)
 : NavigateToPoseTaskServer(name)
 {
   RCLCPP_INFO(get_logger(), "SimpleNavigator::SimpleNavigator");
-  planner_ = std::make_unique<ComputePathToPoseTaskClient>("AStarPlanner", this);
+  planner_ = std::make_unique<ComputePathToPoseTaskClient>("DijkstraPlanner", this);
   controller_ = std::make_unique<FollowPathTaskClient>("DwaController", this);
 }
 
@@ -32,20 +38,25 @@ SimpleNavigator::~SimpleNavigator()
 }
 
 TaskStatus
-SimpleNavigator::executeAsync(const NavigateToPoseCommand::SharedPtr command)
+SimpleNavigator::executeAsync(const NavigateToPoseCommand::SharedPtr /*command*/)
 {
   RCLCPP_INFO(get_logger(), "SimpleNavigator::executeAsync");
 
   // 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 = command->pose.pose;
-  endpoints->goal = command->pose;
+  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");
   auto path = std::make_shared<ComputePathToPoseResult>();
   planner_->executeAsync(endpoints);
 
+  // TODO(orduno): implement continous replanning
+
   // Loop until the subtasks are completed
   for (;; ) {
     // Check to see if this task (navigation) has been canceled. If so, cancel any child
@@ -79,6 +90,16 @@ SimpleNavigator::executeAsync(const NavigateToPoseCommand::SharedPtr command)
   }
 
 here:
+
+  RCLCPP_INFO(get_logger(), "SimpleNavigator::executeAsync: got path of size %u",
+    path->poses.size());
+  int index;
+  for (auto pose : path->poses) {
+    RCLCPP_INFO(get_logger(), "SimpleNavigator::executeAsync: point %u x: %0.2f, y: %0.2f",
+      index, pose.position.x, pose.position.y);
+    index++;
+  }
+
   RCLCPP_INFO(get_logger(),
     "SimpleNavigator::executeAsync: sending the path to the controller to execute");
 
@@ -113,7 +134,7 @@ SimpleNavigator::executeAsync(const NavigateToPoseCommand::SharedPtr command)
         return TaskStatus::FAILED;
 
       case TaskStatus::RUNNING:
-        RCLCPP_INFO(get_logger(), "SimpleNavigator::executeAsync: control task still running");
+        // RCLCPP_INFO(get_logger(), "SimpleNavigator::executeAsync: control task still running");
         break;
 
       default:

src/planning/CMakeLists.txt

@@ -15,24 +15,28 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(task REQUIRED)
+find_package(costmap REQUIRED)
 
 include_directories(
   include
 )
 
-add_executable(astar_planner
-  src/AStarPlanner.cpp
+add_executable(dijkstra_planner
+  src/DijkstraPlanner.cpp
+  src/Navfn.cpp
   src/main.cpp
 )
 
-ament_target_dependencies(astar_planner
+ament_target_dependencies(dijkstra_planner
   rclcpp
   std_msgs
   nav2_msgs
   task
+  costmap
 )
 
-install(TARGETS astar_planner
+install(TARGETS
+  dijkstra_planner
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}

src/planning/include/planning/DijkstraPlanner.hpp

@@ -0,0 +1,115 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PLANNING__DIJKSTRAPLANNER_HPP_
+#define PLANNING__DIJKSTRAPLANNER_HPP_
+
+#include <string>
+#include <vector>
+#include <memory>
+#include <chrono>
+#include "planning/ComputePathToPoseTaskServer.hpp"
+#include "planning/Navfn.hpp"
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "nav2_msgs/msg/costmap.hpp"
+#include "nav2_msgs/srv/get_costmap.hpp"
+
+class DijkstraPlanner : public ComputePathToPoseTaskServer
+{
+public:
+  explicit DijkstraPlanner(const std::string & name);
+  DijkstraPlanner() = delete;
+  ~DijkstraPlanner();
+
+  TaskStatus executeAsync(const ComputePathToPoseCommand::SharedPtr command) override;
+
+private:
+  // Compute a plan given start and goal poses, provided in global world frame.
+  bool makePlan(
+    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::Pose & goal,
+    nav2_msgs::msg::Path & plan);
+
+  // Compute the potential, or navigation cost, at a given point in the world
+  // - must call computePotential first
+  double getPointPotential(const geometry_msgs::msg::Point & world_point);
+
+  // Check for a valid potential value at a given point in the world
+  // - must call computePotential first
+  bool validPointPotential(const geometry_msgs::msg::Point & world_point);
+  bool validPointPotential(const geometry_msgs::msg::Point & world_point, double tolerance);
+
+  // Compute the squared distance between two points
+  inline double squared_distance(
+    const geometry_msgs::msg::Pose & p1,
+    const geometry_msgs::msg::Pose & p2)
+  {
+    double dx = p1.position.x - p2.position.x;
+    double dy = p1.position.y - p2.position.y;
+    return dx * dx + dy * dy;
+  }
+
+  // Transform a point from world to map frame
+  bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my);
+
+  // Transform a point from map to world frame
+  void mapToWorld(double mx, double my, double & wx, double & wy);
+
+  // Set the corresponding cell cost to be free space
+  void clearRobotCell(unsigned int mx, unsigned int my);
+
+  // Request costmap from world model
+  void getCostmap(
+    nav2_msgs::msg::Costmap & costmap, const std::string layer = "master",
+    const std::chrono::milliseconds waitTime = std::chrono::milliseconds(100));
+
+  // Wait for costmap server to appear
+  void waitForCostmapServer(const std::chrono::seconds waitTime = std::chrono::seconds(10));
+
+  // Print costmap to terminal
+  void printCostmap(const nav2_msgs::msg::Costmap & costmap);
+
+  // Planner based on ROS1 NavFn algorithm
+  std::shared_ptr<NavFn> planner_;
+
+  // Client for getting the costmap
+  rclcpp::Client<nav2_msgs::srv::GetCostmap>::SharedPtr costmap_client_;
+
+  // Computed path publisher
+  rclcpp::Publisher<nav2_msgs::msg::Path>::SharedPtr plan_publisher_;
+
+  // The costmap to use
+  nav2_msgs::msg::Costmap costmap_;
+
+  // The global frame of the costmap
+  std::string global_frame_;
+
+  // Whether or not the planner should be allowed to plan through unknown space
+  bool allow_unknown_;
+
+  // Amount the planner can relax the space constraint
+  double default_tolerance_;
+};
+
+#endif  // PLANNING__DIJKSTRAPLANNER_HPP_