From 5792216881899f483a4d422120ee6a1ed17a742a Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Thu, 29 Aug 2019 16:52:48 -0700
Subject: [PATCH] first stab at nav2_core
---
nav2_core/CMakeLists.txt | 16 ++++
.../include/nav2_core/global_planner.hpp | 95 +++++++++++++++++++
nav2_core/include/nav2_core/recovery.hpp | 93 ++++++++++++++++++
nav2_core/package.xml | 20 ++++
nav2_msgs/CMakeLists.txt | 1 +
nav2_msgs/msg/RecoveryRequest.msg | 3 +
6 files changed, 228 insertions(+)
create mode 100644 nav2_core/CMakeLists.txt
create mode 100644 nav2_core/include/nav2_core/global_planner.hpp
create mode 100644 nav2_core/include/nav2_core/recovery.hpp
create mode 100644 nav2_core/package.xml
create mode 100644 nav2_msgs/msg/RecoveryRequest.msg
diff --git a/nav2_core/CMakeLists.txt b/nav2_core/CMakeLists.txt
new file mode 100644
index 00000000..44ed1517
--- /dev/null
+++ b/nav2_core/CMakeLists.txt
@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_core)
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+ ARCHIVE DESTINATION lib
+ LIBRARY DESTINATION lib
+ RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()
diff --git a/nav2_core/include/nav2_core/global_planner.hpp b/nav2_core/include/nav2_core/global_planner.hpp
new file mode 100644
index 00000000..1660f673
--- /dev/null
+++ b/nav2_core/include/nav2_core/global_planner.hpp
@@ -0,0 +1,95 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2019, Samsung Research America
+ * 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 NAV2_CORE_GLOBAL_PLANNER_H_
+#define NAV2_CORE_GLOBAL_PLANNER_H_
+
+#include <string>
+#include "nav2_msgs/msg/path.h"
+#include "geometry_msgs/msg/pose_stamped.h"
+
+namespace nav2_core
+{
+
+/**
+ * @class GlobalPlanner
+ * @brief Abstract interface for global planners to adhere to with pluginlib
+ */
+class GlobalPlanner
+{
+public:
+ /**
+ * @brief Virtual destructor
+ */
+ virtual ~GlobalPlanner() {}
+
+ /**
+ * @param parent pointer to user's node
+ * @param name The name of this planner
+ * @param tf A pointer to a TF buffer
+ * @param costmap_ros A pointer to the costmap
+ */
+ virtual void configure(const rclcpp::Node* parent,
+ const std::string& name, tf2::Buffer * tf,
+ nav2_costmap_2d::Costmap2DROS * costmap_ros) = 0;
+
+ /**
+ * @brief Method to cleanup resources used on shutdown.
+ */
+ virtual void cleanup() = 0;
+
+ /**
+ * @brief Method to active planner and any threads involved in execution.
+ */
+ virtual void activate() = 0;
+
+ /**
+ * @brief Method to deactive planner and any threads involved in execution.
+ */
+ virtual void deactivate() = 0;
+
+ /**
+ * @brief Method create the plan from a starting and ending goal.
+ * @param start The starting pose of the robot
+ * @param goal The goal pose of the robot
+ * @return The sequence of poses to get from start to goal, if any
+ */
+ virtual nav2_msgs::Path createPlan(
+ const geometry_msgs::msg::PoseStamped & start,
+ const geometry_msgs::msg::PoseStamped & goal) = 0;
+};
+
+} // namespace nav2_core
+
+#endif // NAV2_CORE_GLOBAL_PLANNER_H_
diff --git a/nav2_core/include/nav2_core/recovery.hpp b/nav2_core/include/nav2_core/recovery.hpp
new file mode 100644
index 00000000..ce97ed77
--- /dev/null
+++ b/nav2_core/include/nav2_core/recovery.hpp
@@ -0,0 +1,93 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2019, Samsung Research America
+ * 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 NAV2_CORE_GLOBAL_PLANNER_H_
+#define NAV2_CORE_GLOBAL_PLANNER_H_
+
+#include <string>
+#include "nav2_msgs/msg/path.h"
+#include "geometry_msgs/msg/pose_stamped.h"
+
+namespace nav2_core
+{
+
+/**
+ * @class GlobalPlanner
+ * @brief Abstract interface for recoveries to adhere to with pluginlib
+ */
+class Recovery
+{
+public:
+ /**
+ * @brief Virtual destructor
+ */
+ virtual ~Recovery() {}
+
+ /**
+ * @param parent pointer to user's node
+ * @param name The name of this planner
+ * @param tf A pointer to a TF buffer
+ * @param costmap_ros A pointer to the costmap
+ */
+ virtual void configure(const rclcpp::Node* parent,
+ const std::string& name, tf2::Buffer * tf,
+ nav2_costmap_2d::Costmap2DROS * costmap_ros) = 0;
+
+ /**
+ * @brief Method to cleanup resources used on shutdown.
+ */
+ virtual void cleanup() = 0;
+
+ /**
+ * @brief Method to active recovery and any threads involved in execution.
+ */
+ virtual void activate() = 0;
+
+ /**
+ * @brief Method to deactive recovery and any threads involved in execution.
+ */
+ virtual void deactivate() = 0;
+
+ /**
+ * @brief Method Execute recovery behavior
+ * @param name The name of this planner
+ * @return true if successful, false is failed to execute fully
+ */
+ virtual bool executeRecovery(
+ const nav2_msgs::RecoveryRequest & req) = 0;
+};
+
+} // namespace nav2_core
+
+#endif // NAV2_CORE_GLOBAL_PLANNER_H_
diff --git a/nav2_core/package.xml b/nav2_core/package.xml
new file mode 100644
index 00000000..1827b600
--- /dev/null
+++ b/nav2_core/package.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>nav2_core</name>
+ <version>0.2.2</version>
+ <description>A set of headers for plugins core to the navigation2 stack</description>
+ <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
+ <license>BSD-3-Clause</license>
+
+ <buildtool_depend>ament_cmake</buildtool_depend>
+
+ <depend>nav2_costmap_2d</depend>
+ <depend>geometry_msgs</depend>
+ <depend>std_msgs</depend>
+ <depend>tf2_ros</depend>
+
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>
diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt
index 32421f69..25d8e3de 100644
--- a/nav2_msgs/CMakeLists.txt
+++ b/nav2_msgs/CMakeLists.txt
@@ -15,6 +15,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Costmap.msg"
"msg/CostmapMetaData.msg"
"msg/Path.msg"
+ "msg/RecoveryRequest.msg"
"msg/VoxelGrid.msg"
"srv/GetCostmap.srv"
"srv/ClearCostmapExceptRegion.srv"
diff --git a/nav2_msgs/msg/RecoveryRequest.msg b/nav2_msgs/msg/RecoveryRequest.msg
new file mode 100644
index 00000000..053300ee
--- /dev/null
+++ b/nav2_msgs/msg/RecoveryRequest.msg
@@ -0,0 +1,3 @@
+geometry_msgs/PoseStamped starting
+geometry_msgs/PoseStamped goal
+std_msgs/String[] arguments
--
GitLab