git commit -m "first commit"

navigations/nav_core_adapter/CMakeLists.txt

@@ -0,0 +1,76 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(nav_core_adapter)
+
+set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11;-Wall;-Werror")
+
+find_package(catkin REQUIRED COMPONENTS
+        costmap_2d
+        geometry_msgs
+        nav_2d_msgs
+        nav_2d_utils
+        nav_core
+        nav_core2
+        nav_grid
+        nav_msgs
+        pluginlib
+        tf
+        visualization_msgs
+)
+
+catkin_package(
+    CATKIN_DEPENDS
+        costmap_2d
+        geometry_msgs
+        nav_2d_msgs
+        nav_2d_utils
+        nav_core
+        nav_core2
+        nav_grid
+        nav_msgs
+        pluginlib
+        tf
+        visualization_msgs
+    INCLUDE_DIRS include
+    LIBRARIES local_planner_adapter global_planner_adapter costmap_adapter global_planner_adapter2
+)
+
+add_library(costmap_adapter src/costmap_adapter.cpp)
+target_link_libraries(costmap_adapter ${catkin_LIBRARIES})
+
+add_library(local_planner_adapter src/local_planner_adapter.cpp)
+add_dependencies(local_planner_adapter ${catkin_EXPORTED_TARGETS})
+target_link_libraries(local_planner_adapter costmap_adapter ${catkin_LIBRARIES})
+
+add_library(global_planner_adapter src/global_planner_adapter.cpp)
+add_dependencies(global_planner_adapter ${catkin_EXPORTED_TARGETS})
+target_link_libraries(global_planner_adapter costmap_adapter ${catkin_LIBRARIES})
+
+add_library(global_planner_adapter2 src/global_planner_adapter2.cpp)
+add_dependencies(global_planner_adapter2 ${catkin_EXPORTED_TARGETS})
+target_link_libraries(global_planner_adapter2 ${catkin_LIBRARIES})
+
+include_directories(
+    include ${catkin_INCLUDE_DIRS}
+)
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  find_package(roslint REQUIRED)
+  roslint_cpp()
+  roslint_add_test()
+
+  add_rostest_gtest(unload_test test/unload_test.launch test/unload_test.cpp)
+  target_link_libraries(unload_test local_planner_adapter ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
+endif()
+
+install(TARGETS local_planner_adapter global_planner_adapter costmap_adapter global_planner_adapter2
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+install(FILES nav_core2_plugins.xml nav_core_plugins.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

navigations/nav_core_adapter/README.md

@@ -0,0 +1,48 @@
+# nav_core_adapter
+This package contains adapters for using `nav_core` plugins as `nav_core2` plugins and vice versa (more or less). In general, the adaptation process involves
+ * Converting between 2d and 3d datatypes.
+ * Converting between returning false and throwing exceptions on failure.
+
+We also provide an adapter for using a `costmap_2d::Costmap2DROS` as a plugin for the `nav_core2::Costmap` interface.
+
+## Adapter Classes
+ * Global Planner Adapters
+   * `GlobalPlannerAdapter` is used for employing a `nav_core2` global planner interface (such as `dlux_global_planner`) as a `nav_core` plugin, like in `move_base`.
+   * `GlobalPlannerAdapter2` is used for employing a `nav_core` global planner interface (such as `navfn`)
+as a `nav_core2` plugin, like in `locomotor`.
+ * Local Planner Adapter
+   * `LocalPlannerAdapter` is used for employing a `nav_core2` local planner interface (such as `dwb_local_planner`) as a `nav_core` plugin, like in `move_base`. In addition to the standard adaptation steps listed above, the local planner adapter also uses the costmap to grab the global pose and subscribes to the odometry in order to get the current velocity.
+   * There is no `LocalPlannerAdapter2`. The `nav_core2` interfaces use additional information (like velocity) in the `local_planner` interface than its `nav_core` counterpart. This information would be ignored by a `nav_core` planner, so no adapter is provided.
+ * `CostmapAdapter` provides most of the functionality from `nav_core2::Costmap` and also provides a raw pointer to the `Costmap2DROS` object. It is not a perfect adaptation, because
+   * `Costmap2DROS` starts its own update thread and updates on its own schedule, so calling `update()` does not actually cause the costmap to update. It does update some of the metadata though.
+   * `setInfo` is not implemented.
+
+## Parameter Setup
+Let's look at a practical example of how to use `dwb_local_planner` in `move_base`.
+
+If you were using `DWA` you would probably have parameters set up like this:
+```
+base_local_planner: dwa_local_planner/DWALocalPlanner
+DWALocalPlanner:
+  acc_lim_x: 0.42
+  ...
+```
+i.e. you specify
+ * The name of the planner
+ * A bunch of additional parameters within the planner's namespace
+
+To use the adapter, you have to provide additional information.
+```
+base_local_planner: nav_core_adapter::LocalPlannerAdapter
+LocalPlannerAdapter:
+  planner_name: dwb_local_planner::DWBLocalPlanner
+DWBLocalPlanner:
+  acc_lim_x: 0.42
+  ...
+```
+i.e.
+ * The name of the planner now points at the adapter
+ * The name of the actual planner loaded into the adapter's namespace
+ * The planner's parameters still in the planner's namespace.
+
+The process for the global planners is similar.

navigations/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h

@@ -0,0 +1,85 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H
+#define NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H
+
+#include <nav_core2/common.h>
+#include <nav_core2/costmap.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <string>
+
+namespace nav_core_adapter
+{
+nav_grid::NavGridInfo infoFromCostmap(costmap_2d::Costmap2DROS* costmap_ros);
+
+class CostmapAdapter : public nav_core2::Costmap
+{
+public:
+  /**
+   * @brief Deconstructor for possibly freeing the costmap_ros_ object
+   */
+  virtual ~CostmapAdapter();
+
+  /**
+   * @brief Initialize from an existing Costmap2DROS object
+   * @param costmap_ros A Costmap2DROS object
+   * @param needs_destruction Whether to free the costmap_ros object when this class is destroyed
+   */
+  void initialize(costmap_2d::Costmap2DROS* costmap_ros, bool needs_destruction = false);
+
+  // Standard Costmap Interface
+  void initialize(const ros::NodeHandle& parent, const std::string& name, TFListenerPtr tf) override;
+  nav_core2::Costmap::mutex_t* getMutex() override;
+
+  // NavGrid Interface
+  void reset() override;
+  void update() override;
+  void setValue(const unsigned int x, const unsigned int y, const unsigned char& value) override;
+  unsigned char getValue(const unsigned int x, const unsigned int y) const override;
+  void setInfo(const nav_grid::NavGridInfo& new_info) override;
+  void updateInfo(const nav_grid::NavGridInfo& new_info) override;
+
+  // Get Costmap Pointer for Backwards Compatibility
+  costmap_2d::Costmap2DROS* getCostmap2DROS() const { return costmap_ros_; }
+
+protected:
+  costmap_2d::Costmap2DROS* costmap_ros_;
+  costmap_2d::Costmap2D* costmap_;
+  bool needs_destruction_;
+};
+
+}  // namespace nav_core_adapter
+
+#endif  // NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H

navigations/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h

@@ -0,0 +1,77 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER_H
+#define NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER_H
+
+#include <nav_core/base_global_planner.h>
+#include <nav_core2/global_planner.h>
+#include <nav_core_adapter/costmap_adapter.h>
+#include <pluginlib/class_loader.h>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace nav_core_adapter
+{
+
+/**
+ * @class GlobalPlannerAdapter
+ * @brief used for employing a `nav_core2` global planner interface as a `nav_core` plugin, like in `move_base`.
+ */
+class GlobalPlannerAdapter: public nav_core::BaseGlobalPlanner
+{
+public:
+  GlobalPlannerAdapter();
+
+  // Standard ROS Global Planner Interface
+  void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) override;
+  bool makePlan(const geometry_msgs::PoseStamped& start,
+                const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan) override;
+
+protected:
+  // Plugin handling
+  pluginlib::ClassLoader<nav_core2::GlobalPlanner> planner_loader_;
+  boost::shared_ptr<nav_core2::GlobalPlanner> planner_;
+  ros::Publisher path_pub_;
+
+  TFListenerPtr tf_;
+
+  std::shared_ptr<CostmapAdapter> costmap_adapter_;
+  costmap_2d::Costmap2DROS* costmap_ros_;
+};
+
+}  // namespace nav_core_adapter
+
+#endif  // NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER_H

navigations/nav_core_adapter/include/nav_core_adapter/global_planner_adapter2.h

@@ -0,0 +1,73 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H
+#define NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H
+
+#include <nav_core/base_global_planner.h>
+#include <nav_core2/global_planner.h>
+#include <pluginlib/class_loader.h>
+#include <string>
+#include <vector>
+
+namespace nav_core_adapter
+{
+
+/**
+ * @class GlobalPlannerAdapter2
+ * @brief used for employing a `nav_core` global planner (such as `navfn`) as a `nav_core2` plugin, like in `locomotor`.
+ */
+class GlobalPlannerAdapter2: public nav_core2::GlobalPlanner
+{
+public:
+  GlobalPlannerAdapter2();
+
+  // Nav Core 2 Global Planner Interface
+  void initialize(const ros::NodeHandle& parent, const std::string& name,
+                  TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
+  nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start,
+                               const nav_2d_msgs::Pose2DStamped& goal) override;
+
+protected:
+  // Plugin handling
+  pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> planner_loader_;
+  boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
+
+  costmap_2d::Costmap2DROS* costmap_ros_;
+  nav_core2::Costmap::Ptr costmap_;
+};
+
+}  // namespace nav_core_adapter
+
+#endif  // NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H

navigations/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h

@@ -0,0 +1,99 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H
+#define NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H
+
+#include <nav_core/base_local_planner.h>
+#include <nav_core2/local_planner.h>
+#include <nav_core_adapter/costmap_adapter.h>
+#include <nav_2d_utils/odom_subscriber.h>
+#include <pluginlib/class_loader.h>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace nav_core_adapter
+{
+
+/**
+ * @class LocalPlannerAdapter
+ * @brief used for employing a nav_core2 local planner (such as dwb) as a nav_core plugin, like in move_base.
+ */
+class LocalPlannerAdapter: public nav_core::BaseLocalPlanner
+{
+public:
+  LocalPlannerAdapter();
+
+  // Standard ROS Local Planner Interface
+  void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) override;
+  bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) override;
+  bool isGoalReached() override;
+  bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) override;
+
+protected:
+  /**
+   * @brief Get the robot pose from the costmap and store as Pose2DStamped
+   */
+  bool getRobotPose(nav_2d_msgs::Pose2DStamped& pose2d);
+
+  /**
+   * @brief See if the back of the global plan matches the most recent goal pose
+   * @return True if the plan has changed
+   */
+  bool hasGoalChanged(const nav_2d_msgs::Pose2DStamped& new_goal);
+
+  // The most recent goal pose
+  nav_2d_msgs::Pose2DStamped last_goal_;
+  bool has_active_goal_;
+
+  /**
+   * @brief helper class for subscribing to odometry
+   */
+  std::shared_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
+
+  // Plugin handling
+  pluginlib::ClassLoader<nav_core2::LocalPlanner> planner_loader_;
+  boost::shared_ptr<nav_core2::LocalPlanner> planner_;
+
+  // Pointer Copies
+  TFListenerPtr tf_;
+
+  std::shared_ptr<CostmapAdapter> costmap_adapter_;
+  costmap_2d::Costmap2DROS* costmap_ros_;
+};
+
+}  // namespace nav_core_adapter
+
+#endif  // NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H

navigations/nav_core_adapter/include/nav_core_adapter/shared_pointers.h

@@ -0,0 +1,67 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV_CORE_ADAPTER_SHARED_POINTERS_H
+#define NAV_CORE_ADAPTER_SHARED_POINTERS_H
+
+#include <nav_core2/common.h>
+#include <memory>
+
+namespace nav_core_adapter
+{
+
+template <typename T>
+void null_deleter(T* raw_ptr) {}
+
+/**
+ * @brief Custom Constructor for creating a shared pointer to an existing object that doesn't delete the ptr when done
+ *
+ * @note This is considered bad form, and is only done here for backwards compatibility purposes. The nav_core2
+ *       interfaces require shared pointers, but the creation of the shared pointer from a raw pointer takes ownership
+ *       of the object such that when the object containing the shared pointer is freed, the object pointed at by the
+ *       shared pointer is also freed. This presents a problem, for instance, when switching from one planner to another
+ *       if the costmap is freed by one planner.
+ *
+ * @param raw_ptr The raw pointer to an object
+ * @return A Shared pointer pointing at the raw_ptr, but when it is freed, the raw_ptr remains valid
+ */
+template <typename T>
+std::shared_ptr<T> createSharedPointerWithNoDelete(T* raw_ptr)
+{
+  return std::shared_ptr<T>(raw_ptr, null_deleter<T>);
+}
+
+}  // namespace nav_core_adapter
+
+#endif  // NAV_CORE_ADAPTER_SHARED_POINTERS_H

navigations/nav_core_adapter/nav_core2_plugins.xml

@@ -0,0 +1,12 @@
+<class_libraries>
+  <library path="lib/libglobal_planner_adapter2">
+    <class type="nav_core_adapter::GlobalPlannerAdapter2" base_class_type="nav_core2::GlobalPlanner">
+      <description></description>
+    </class>
+  </library>
+  <library path="lib/libcostmap_adapter">
+    <class type="nav_core_adapter::CostmapAdapter" base_class_type="nav_core2::Costmap">
+      <description></description>
+    </class>
+  </library>
+</class_libraries>

navigations/nav_core_adapter/nav_core_plugins.xml

@@ -0,0 +1,12 @@
+<class_libraries>
+  <library path="lib/liblocal_planner_adapter">
+    <class type="nav_core_adapter::LocalPlannerAdapter" base_class_type="nav_core::BaseLocalPlanner">
+      <description></description>
+    </class>
+  </library>
+  <library path="lib/libglobal_planner_adapter">
+    <class type="nav_core_adapter::GlobalPlannerAdapter" base_class_type="nav_core::BaseGlobalPlanner">
+      <description></description>
+    </class>
+  </library>
+</class_libraries>

navigations/nav_core_adapter/package.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>nav_core_adapter</name>
+  <version>0.3.0</version>
+  <description>
+      This package contains adapters for using `nav_core` plugins as `nav_core2` plugins and vice versa (more or less).
+      See README.md for more information.
+  </description>
+
+  <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>costmap_2d</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_2d_msgs</depend>
+  <depend>nav_2d_utils</depend>
+  <depend>nav_core</depend>
+  <depend>nav_core2</depend>
+  <depend>nav_grid</depend>
+  <depend>nav_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>tf</depend>
+  <depend>visualization_msgs</depend>
+  <test_depend>roslint</test_depend>
+  <test_depend>rostest</test_depend>
+
+  <!-- When running the unload test, it loads DWB, so these are needed -->
+  <test_depend>dwb_local_planner</test_depend>
+  <test_depend>dwb_plugins</test_depend>
+  <test_depend>dwb_critics</test_depend>
+
+  <export>
+    <nav_core plugin="${prefix}/nav_core_plugins.xml"/>
+    <nav_core2 plugin="${prefix}/nav_core2_plugins.xml"/>
+  </export>
+</package>

navigations/nav_core_adapter/src/costmap_adapter.cpp

@@ -0,0 +1,117 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <nav_core_adapter/costmap_adapter.h>
+#include <nav_core2/exceptions.h>
+#include <pluginlib/class_list_macros.h>
+#include <string>
+
+PLUGINLIB_EXPORT_CLASS(nav_core_adapter::CostmapAdapter, nav_core2::Costmap)
+
+namespace nav_core_adapter
+{
+
+nav_grid::NavGridInfo infoFromCostmap(costmap_2d::Costmap2DROS* costmap_ros)
+{
+  nav_grid::NavGridInfo info;
+  costmap_2d::Costmap2D* costmap = costmap_ros->getCostmap();
+  info.width = costmap->getSizeInCellsX();
+  info.height = costmap->getSizeInCellsY();
+  info.resolution = costmap->getResolution();
+  info.frame_id = costmap_ros->getGlobalFrameID();
+  info.origin_x = costmap->getOriginX();
+  info.origin_y = costmap->getOriginY();
+  return info;
+}
+
+CostmapAdapter::~CostmapAdapter()
+{
+  if (needs_destruction_)
+  {
+    delete costmap_ros_;
+  }
+}
+
+void CostmapAdapter::initialize(costmap_2d::Costmap2DROS* costmap_ros, bool needs_destruction)
+{
+  costmap_ros_ = costmap_ros;
+  needs_destruction_ = needs_destruction;
+  info_ = infoFromCostmap(costmap_ros_);
+  costmap_ = costmap_ros_->getCostmap();
+}
+
+void CostmapAdapter::initialize(const ros::NodeHandle& parent, const std::string& name, TFListenerPtr tf)
+{
+  initialize(new costmap_2d::Costmap2DROS(name, *tf), true);
+}
+
+nav_core2::Costmap::mutex_t* CostmapAdapter::getMutex()
+{
+  return costmap_->getMutex();
+}
+
+void CostmapAdapter::reset()
+{
+  costmap_ros_->resetLayers();
+}
+
+void CostmapAdapter::update()
+{
+  info_ = infoFromCostmap(costmap_ros_);
+  if (!costmap_ros_->isCurrent())
+    throw nav_core2::CostmapDataLagException("Costmap2DROS is out of date somehow.");
+}
+
+void CostmapAdapter::setValue(const unsigned int x, const unsigned int y, const unsigned char& value)
+{
+  costmap_->setCost(x, y, value);
+}
+
+unsigned char CostmapAdapter::getValue(const unsigned int x, const unsigned int y) const
+{
+  unsigned int index = costmap_->getIndex(x, y);
+  return costmap_->getCharMap()[index];
+}
+
+void CostmapAdapter::setInfo(const nav_grid::NavGridInfo& new_info)
+{
+  throw nav_core2::CostmapException("setInfo not implemented on CostmapAdapter");
+}
+
+void CostmapAdapter::updateInfo(const nav_grid::NavGridInfo& new_info)
+{
+  costmap_->updateOrigin(new_info.origin_x, new_info.origin_y);
+}
+
+}  // namespace nav_core_adapter

navigations/nav_core_adapter/src/global_planner_adapter.cpp

@@ -0,0 +1,97 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <nav_core_adapter/global_planner_adapter.h>
+#include <nav_core_adapter/costmap_adapter.h>
+#include <nav_2d_utils/conversions.h>
+#include <nav_2d_utils/tf_help.h>
+#include <nav_core2/exceptions.h>
+#include <pluginlib/class_list_macros.h>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace nav_core_adapter
+{
+
+GlobalPlannerAdapter::GlobalPlannerAdapter() :
+  planner_loader_("nav_core2", "nav_core2::GlobalPlanner")
+{
+}
+
+/**
+ * @brief Load the nav_core2 global planner and initialize it
+ */
+void GlobalPlannerAdapter::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
+{
+  costmap_ros_ = costmap_ros;
+  costmap_adapter_ = std::make_shared<CostmapAdapter>();
+  costmap_adapter_->initialize(costmap_ros);
+
+  ros::NodeHandle private_nh("~");
+  ros::NodeHandle adapter_nh("~/" + name);
+  std::string planner_name;
+  adapter_nh.param("planner_name", planner_name, std::string("dlux_global_planner::DluxGlobalPlanner"));
+  ROS_WARN("%s ,GlobalPlannerAdapter::initialize", name.c_str());
+  ROS_INFO_NAMED("GlobalPlannerAdapter", "Loading plugin %s", planner_name.c_str());
+  planner_ = planner_loader_.createInstance(planner_name);
+  planner_->initialize(private_nh, planner_loader_.getName(planner_name), tf_, costmap_adapter_);
+  path_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
+}
+
+bool GlobalPlannerAdapter::makePlan(const geometry_msgs::PoseStamped& start,
+                                    const geometry_msgs::PoseStamped& goal,
+                                    std::vector<geometry_msgs::PoseStamped>& plan)
+{
+  nav_2d_msgs::Pose2DStamped start2d = nav_2d_utils::poseStampedToPose2D(start),
+                             goal2d = nav_2d_utils::poseStampedToPose2D(goal);
+  try
+  {
+    nav_2d_msgs::Path2D path2d = planner_->makePlan(start2d, goal2d);
+    nav_msgs::Path path = nav_2d_utils::pathToPath(path2d);
+    plan = path.poses;
+    path_pub_.publish(path);
+    return true;
+  }
+  catch (nav_core2::PlannerException& e)
+  {
+    ROS_ERROR_NAMED("GlobalPlannerAdapter", "makePlan Exception: %s", e.what());
+    return false;
+  }
+}
+}  // namespace nav_core_adapter
+
+
+//  register this planner as a BaseGlobalPlanner plugin
+PLUGINLIB_EXPORT_CLASS(nav_core_adapter::GlobalPlannerAdapter, nav_core::BaseGlobalPlanner)

navigations/nav_core_adapter/src/global_planner_adapter2.cpp

@@ -0,0 +1,95 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <nav_core_adapter/global_planner_adapter2.h>
+#include <nav_core_adapter/costmap_adapter.h>
+#include <nav_2d_utils/conversions.h>
+#include <nav_2d_utils/tf_help.h>
+#include <nav_core2/exceptions.h>
+#include <pluginlib/class_list_macros.h>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace nav_core_adapter
+{
+
+GlobalPlannerAdapter2::GlobalPlannerAdapter2() :
+  planner_loader_("nav_core", "nav_core::BaseGlobalPlanner")
+{
+}
+
+/**
+ * @brief Load the nav_core global planner and initialize it
+ */
+void GlobalPlannerAdapter2::initialize(const ros::NodeHandle& parent, const std::string& name,
+                                       TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
+{
+  costmap_ = costmap;
+  std::shared_ptr<CostmapAdapter> ptr = std::static_pointer_cast<CostmapAdapter>(costmap);
+
+  if (!ptr)
+  {
+    ROS_FATAL_NAMED("GlobalPlannerAdapter2",
+                    "GlobalPlannerAdapter2 can only be used with the CostmapAdapter, not other Costmaps!");
+    exit(EXIT_FAILURE);
+  }
+  costmap_ros_ = ptr->getCostmap2DROS();
+
+  ros::NodeHandle planner_nh(parent, name);
+  std::string planner_name;
+  planner_nh.param("planner_name", planner_name, std::string("global_planner/GlobalPlanner"));
+  ROS_INFO_NAMED("GlobalPlannerAdapter2", "Loading plugin %s", planner_name.c_str());
+  planner_ = planner_loader_.createInstance(planner_name);
+  planner_->initialize(planner_loader_.getName(planner_name), costmap_ros_);
+}
+
+nav_2d_msgs::Path2D GlobalPlannerAdapter2::makePlan(const nav_2d_msgs::Pose2DStamped& start,
+                                                    const nav_2d_msgs::Pose2DStamped& goal)
+{
+  geometry_msgs::PoseStamped start3d = nav_2d_utils::pose2DToPoseStamped(start),
+                             goal3d = nav_2d_utils::pose2DToPoseStamped(goal);
+  std::vector<geometry_msgs::PoseStamped> plan;
+  bool ret = planner_->makePlan(start3d, goal3d, plan);
+  if (!ret)
+  {
+    throw nav_core2::PlannerException("Generic Global Planner Error");
+  }
+  return nav_2d_utils::posesToPath2D(plan);
+}
+}  // namespace nav_core_adapter
+
+
+//  register this planner as a GlobalPlanner plugin
+PLUGINLIB_EXPORT_CLASS(nav_core_adapter::GlobalPlannerAdapter2, nav_core2::GlobalPlanner)

navigations/nav_core_adapter/src/local_planner_adapter.cpp

@@ -0,0 +1,190 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <nav_core_adapter/local_planner_adapter.h>
+#include <nav_core_adapter/costmap_adapter.h>
+#include <nav_core_adapter/shared_pointers.h>
+#include <nav_2d_utils/conversions.h>
+#include <nav_2d_utils/tf_help.h>
+#include <nav_core2/exceptions.h>
+#include <pluginlib/class_list_macros.h>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace nav_core_adapter
+{
+
+LocalPlannerAdapter::LocalPlannerAdapter() :
+  planner_loader_("nav_core2", "nav_core2::LocalPlanner")
+{
+}
+
+/**
+ * @brief Load the nav_core2 local planner and initialize it
+ */
+void LocalPlannerAdapter::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros)
+{
+  // ROS_WARN("LocalPlannerAdapter::initialize");
+  tf_ = createSharedPointerWithNoDelete<tf2_ros::Buffer>(tf);
+  costmap_ros_ = costmap_ros;
+  costmap_adapter_ = std::make_shared<CostmapAdapter>();
+  costmap_adapter_->initialize(costmap_ros);
+
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh("~");
+  ros::NodeHandle adapter_nh("~/" + name);
+  std::string planner_name;
+  adapter_nh.param("planner_name", planner_name, std::string("dwb_local_planner::DWBLocalPlanner"));
+  ROS_INFO_NAMED("LocalPlannerAdapter", "Loading plugin %s", planner_name.c_str());
+  planner_ = planner_loader_.createInstance(planner_name);
+  // From void DWBLocalPlanner::initialize(const ros::NodeHandle& parent, const std::string& name,
+  //                               TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
+  planner_->initialize(private_nh, planner_loader_.getName(planner_name), tf_, costmap_adapter_);
+  has_active_goal_ = false;
+
+  odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(nh);
+}
+
+/**
+ * @brief Collect the additional information needed by nav_core2 and call the child computeVelocityCommands
+ */
+bool LocalPlannerAdapter::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
+{
+  if (!has_active_goal_)
+  {
+    return false;
+  }
+
+  // Get the Pose
+  nav_2d_msgs::Pose2DStamped pose2d;
+  if (!getRobotPose(pose2d))
+  {
+    return false;
+  }
+
+  // Get the Velocity
+  nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
+  nav_2d_msgs::Twist2DStamped cmd_vel_2d;
+  try
+  {                                                           // to -> DWBLocalPlanner::computeVelocityCommands()
+    cmd_vel_2d = planner_->computeVelocityCommands(pose2d, velocity);
+  }
+  catch (const nav_core2::PlannerException& e)
+  {
+    ROS_ERROR_NAMED("LocalPlannerAdapter", "computeVelocityCommands exception: %s", e.what());
+    return false;
+  }
+  cmd_vel = nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity);
+  return true;
+}
+
+/**
+ * @brief Collect the additional information needed by nav_core2 and call the child isGoalReached
+ */
+bool LocalPlannerAdapter::isGoalReached()
+{
+  // Get the Pose
+  nav_2d_msgs::Pose2DStamped pose2d;
+  if (!getRobotPose(pose2d))
+    return false;
+
+  nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
+  bool ret = planner_->isGoalReached(pose2d, velocity);
+  if (ret)
+  {
+    has_active_goal_ = false;
+  }
+  return ret;
+}
+
+/**
+ * @brief Convert from 2d to 3d and call child setPlan
+ */
+bool LocalPlannerAdapter::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)
+{
+  nav_2d_msgs::Path2D path = nav_2d_utils::posesToPath2D(orig_global_plan);
+  try
+  {
+    if (path.poses.size() > 0)
+    {
+      nav_2d_msgs::Pose2DStamped goal_pose;
+      goal_pose.header = path.header;
+      goal_pose.pose = path.poses.back();
+
+      if (!has_active_goal_ || hasGoalChanged(goal_pose))
+      {
+        last_goal_ = goal_pose;
+        has_active_goal_ = true;
+        planner_->setGoalPose(goal_pose);
+      }
+    }
+
+    planner_->setPlan(path);
+    return true;
+  }
+  catch (const nav_core2::PlannerException& e)
+  {
+    ROS_ERROR_NAMED("LocalPlannerAdapter", "setPlan Exception: %s", e.what());
+    return false;
+  }
+}
+
+bool LocalPlannerAdapter::hasGoalChanged(const nav_2d_msgs::Pose2DStamped& new_goal)
+{
+  if (last_goal_.header.frame_id != new_goal.header.frame_id)
+  {
+    return true;
+  }
+
+  return last_goal_.pose.x != new_goal.pose.x || last_goal_.pose.y != new_goal.pose.y ||
+         last_goal_.pose.theta != new_goal.pose.theta;
+}
+
+bool LocalPlannerAdapter::getRobotPose(nav_2d_msgs::Pose2DStamped& pose2d)
+{
+  geometry_msgs::PoseStamped current_pose;
+  if (!costmap_ros_->getRobotPose(current_pose))
+  {
+    ROS_ERROR_NAMED("LocalPlannerAdapter", "Could not get robot pose");
+    return false;
+  }
+  pose2d = nav_2d_utils::poseStampedToPose2D(current_pose);
+  return true;
+}
+
+}  // namespace nav_core_adapter
+
+//  register this planner as a BaseLocalPlanner plugin
+PLUGINLIB_EXPORT_CLASS(nav_core_adapter::LocalPlannerAdapter, nav_core::BaseLocalPlanner)

navigations/nav_core_adapter/test/unload_test.cpp

@@ -0,0 +1,69 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <gtest/gtest.h>
+#include <nav_core_adapter/local_planner_adapter.h>
+
+
+TEST(LocalPlannerAdapter, unload_local_planner)
+{
+  tf2_ros::Buffer tf;
+  tf.setUsingDedicatedThread(true);
+
+  // This empty transform is added to satisfy the constructor of
+  // Costmap2DROS, which waits for the transform from map to base_link
+  // to become available.
+  geometry_msgs::TransformStamped base_rel_map;
+  base_rel_map.child_frame_id = "/base_link";
+  base_rel_map.header.frame_id = "/map";
+  base_rel_map.transform.rotation.w = 1.0;
+  tf.setTransform(base_rel_map, "unload", true);
+
+  nav_core_adapter::LocalPlannerAdapter* lpa = new nav_core_adapter::LocalPlannerAdapter();
+
+  costmap_2d::Costmap2DROS costmap_ros("local_costmap", tf);
+  lpa->initialize("lpa", &tf, &costmap_ros);
+
+  delete lpa;
+
+  // Simple test to make sure costmap hasn't been deleted
+  EXPECT_EQ("map", costmap_ros.getGlobalFrameID());
+}
+
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "unload_test");
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

navigations/nav_core_adapter/test/unload_test.launch

@@ -0,0 +1,3 @@
+<launch>
+  <test time-limit="10" test-name="unload_test" pkg="nav_core_adapter" type="unload_test" />
+</launch>