Tharp/adding_global_planning_functionality

docs/robot/autonomy/4_global/planning.md

@@ -13,6 +13,11 @@ If a waypoint's header timestamp is empty, the local planner should assume there
 The global planner should make a trajectory that is collision-free according to the global map.
 However, avoiding fine obstacles is delegated to the local planner that operates at a faster rate.
 
+For the structure of the package, the global planner node should not include any logic to generate the path. This should be located in a seperate logic class and be seperated from ROS. This will allow more modularity in the future for testing and easy interface changes.
+
+### Actions Interface
+The global planner should provide an action server for the topic `$(arg robot_name)/global_planner/plan` of message type `nav_msgs/GetPlan`. This should be triggered from the base station and should trigger a path generation based off a fed goal position and optionally a tolerance to the final goal, otherwise a default tolerance will be used. This will then trigger the global planner to generate a path and publish it to the gloabal trajectory topic. 
+=======
 We intend the global planners to be modular. _AirStack_ implements a basic Random Walk planner as a baseline. 
 Feel free to implement your own through the following interfaces.
 
@@ -84,6 +89,8 @@ nav_msgs/Path.msg
 #### Feedback
 All other fields are optional.
 
+## Global Planners
+### Random Walk Planner
 
 More info about ROS2 actions may be found in the official [tutorial](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Actions/Understanding-ROS2-Actions.html) and [design philosophy](https://design.ros2.org/articles/actions.html) documents.
 
@@ -102,4 +109,5 @@ The global planner can do whatever it wants internally with this information.
 
 ### Random Walk planner
 
-The random walk planner replans when the robot is getting close to the goal. The random walk planner is a trivial planner that generates a plan by randomly selecting a direction to move in. The random walk planner is useful for testing the robot's ability to follow a plan.
+The random walk planner replans when the robot is getting close to the goal. The random walk planner is a trivial planner that generates a plan by randomly selecting a direction to move in. The random walk planner is useful for testing the robot's ability to follow a plan.
+

ros_ws/src/airstack_msgs/srv/PlanToWaypoint.srv

@@ -0,0 +1,5 @@
+geometry_msgs/Pose goal_pose
+
+uint8 command
+---
+bool success

ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/CMakeLists.txt

@@ -0,0 +1,77 @@
+cmake_minimum_required(VERSION 3.5)
+project(ensemble_global_planner)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(global_planner_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+###################################
+## ament specific configuration  ##
+###################################
+
+ament_package()
+
+###########
+## Build ##
+###########
+
+# Specify additional locations of header files
+
+# Declare a C++ executable
+add_executable(ensemble_global_planner src/ensemble_global_planner_node.cpp)
+
+# Link libraries
+ament_target_dependencies(ensemble_global_planner
+  rclcpp
+  rclcpp_action
+  std_msgs
+  std_srvs
+  geometry_msgs
+  nav_msgs
+  global_planner_msgs
+  visualization_msgs
+  tf2
+  tf2_ros
+)
+
+# Install executable
+install(TARGETS
+  ensemble_global_planner
+  DESTINATION lib/${PROJECT_NAME}
+)
+# install(DIRECTORY
+#   launch
+#   DESTINATION share/${PROJECT_NAME})
+
+install(DIRECTORY
+  config
+  DESTINATION share/${PROJECT_NAME})
+
+install(DIRECTORY
+  src
+  DESTINATION share/${PROJECT_NAME})
+
+
+#############
+## Testing ##
+#############
+
+# Add gtest based cpp test target and link libraries
+# ament_add_gtest(${PROJECT_NAME}-test test/test_ensemble_global_planner.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_node)
+# endif()

ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/config/ensemble_global_planner_config.yaml

@@ -0,0 +1,8 @@
+/**:
+  ros__parameters:
+    srv_random_walk_toggle_topic: "/robot_1/behavior/global_plan_toggle"
+    way_point_planners: 
+      - name: "random_walk"
+        config: 
+          frequency: 1.0 #hz 
+

ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/include/ensemble_global_planner_node.hpp

@@ -0,0 +1,58 @@
+
+#ifndef RANDOM_WALK_NODE_H
+#define RANDOM_WALK_NODE_H
+
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Quaternion.h>
+
+#include <cmath>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/transform.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <nav_msgs/srv/get_plan.hpp>
+#include <optional>
+#include <string>
+#include <tf2_msgs/msg/tf_message.hpp>
+#include <tuple>
+#include <vector>
+#include <visualization_msgs/msg/marker.hpp>
+#include <std_srvs/srv/trigger.hpp>
+
+#include "rclcpp/rclcpp.hpp"
+
+class EnsembleGlobalPlannerNode : public rclcpp::Node {
+   private:
+
+    // String constants
+    std::string srv_global_plan_toggle_topic_;
+
+    void globalPlannnerToggleCallback(const std_srvs::srv::Trigger::Request::SharedPtr request,
+                                  std_srvs::srv::Trigger::Response::SharedPtr response);
+
+    // Other functions
+    void readParameters();
+
+    bool enable_global_planner = false;
+
+   public:
+    // explicit RandomWalkNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+    EnsembleGlobalPlannerNode();
+    ~EnsembleGlobalPlannerNode() = default;
+
+    // ROS subscribers
+    rclcpp::Subscription<visualization_msgs::msg::Marker>::SharedPtr sub_map;
+    rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr sub_robot_tf;
+
+    // ROS publishers
+    // rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_global_path;
+    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub_goal_point;
+    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub_trajectory_lines;
+
+    // ROS services
+    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr srv_global_planner_toggle;
+
+    // ROS timers
+    rclcpp::TimerBase::SharedPtr timer;
+};
+
+#endif  // RANDOM_WALK_NODE_H

ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/package.xml

@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ensemble_global_planner</name>
+  <version>0.0.0</version>
+  <description>Ensemble planner to coordinate different groups of planners</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
+  <maintainer email="[email protected]">todo</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/dfs_behavior_planner</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="[email protected]">Jane Doe</author> -->
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>rclcpp</build_depend>
+  <build_depend>rclcpp_action</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
+  <build_depend>base</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>global_planner_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_ros</build_depend>
+
+  <build_export_depend>rclcpp</build_export_depend>
+  <build_export_depend>rclcpp_action</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>std_srvs</build_export_depend>
+  <build_export_depend>base</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>nav_msgs</build_export_depend>
+  <build_export_depend>visualization_msgs</build_export_depend>
+  <build_export_depend>global_planner_msgs</build_export_depend>
+  <build_export_depend>message_runtime</build_export_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rclcpp_action</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>base</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+  <exec_depend>global_planner_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>tf2</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/src/ensemble_global_planner_node.cpp

@@ -0,0 +1,41 @@
+
+#include "../include/ensemble_global_planner_node.hpp"
+
+void EnsembleGlobalPlannerNode::readParameters() {
+    this->declare_parameter<std::string>("srv_global_plan_toggle_topic");
+    if (!this->get_parameter("srv_global_plan_toggle_topic", this->srv_global_plan_toggle_topic_)) {
+        RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: srv_global_plan_toggle_topic");
+    }
+}
+
+EnsembleGlobalPlannerNode::EnsembleGlobalPlannerNode() : Node("ensemble_global_planner_node") {
+    // Initialize the Global Planner planner
+    EnsembleGlobalPlannerNode::readParameters();
+    this->srv_global_planner_toggle = this->create_service<std_srvs::srv::Trigger>(
+        srv_global_plan_toggle_topic_,
+        std::bind(&EnsembleGlobalPlannerNode::globalPlannnerToggleCallback, this, std::placeholders::_1,
+                  std::placeholders::_2));
+}
+
+void EnsembleGlobalPlannerNode::globalPlannnerToggleCallback(
+    const std_srvs::srv::Trigger::Request::SharedPtr request,
+    std_srvs::srv::Trigger::Response::SharedPtr response) {
+    if (this->enable_global_planner == false) {
+        this->enable_global_planner = true;
+        response->success = true;
+        response->message = "Global Planner enabled";
+        RCLCPP_INFO(this->get_logger(), "Global Planner enabled");
+    } else {
+        this->enable_global_planner = false;
+        response->success = true;
+        response->message = "Global Planer disabled";
+        RCLCPP_INFO(this->get_logger(), "Global Planner disabled");
+    }
+}
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<EnsembleGlobalPlannerNode>());
+    rclcpp::shutdown();
+    return 0;
+}

ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/CMakeLists.txt → ros_ws/src/robot/autonomy/4_global/b_planners/global_planner_bringup/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(trivial_planners)
+project(global_planner_bringup)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
@@ -11,15 +11,6 @@ find_package(ament_cmake REQUIRED)
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
-add_executable(random_walk_planner_node src/random_walk_planner_node.cpp)
-target_include_directories(random_walk_planner_node PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-target_compile_features(random_walk_planner_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
-
-install(TARGETS random_walk_planner_node
-  DESTINATION lib/${PROJECT_NAME})
-
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights
@@ -32,4 +23,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+# Install files.
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+
 ament_package()

ros_ws/src/robot/autonomy/4_global/b_planners/global_planner_bringup/launch/global_planner.launch.xml

@@ -0,0 +1,6 @@
+<!-- GLOBAL -->
+<launch>
+    <!-- Ensemble Planner -->
+    <!-- Waypoint Planners-->
+
+</launch>

ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/package.xml → ros_ws/src/robot/autonomy/4_global/b_planners/global_planner_bringup/package.xml

@@ -1,11 +1,11 @@
 <?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>trivial_planners</name>
+  <name>global_planner_bringup</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="[email protected]">andrew</maintainer>
-  <license>TODO: License declaration</license>
+  <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/CMakeLists.txt

@@ -9,13 +9,15 @@ endif()
 # Find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
+find_package(global_planner_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
-find_package(airstack_msgs REQUIRED)
 
 ###################################
 ## ament specific configuration  ##
@@ -35,13 +37,15 @@ add_executable(random_walk_planner src/random_walk_node.cpp src/random_walk_logi
 # Link libraries
 ament_target_dependencies(random_walk_planner
   rclcpp
+  rclcpp_action
   std_msgs
+  std_srvs
   geometry_msgs
   nav_msgs
+  global_planner_msgs
   visualization_msgs
   tf2
   tf2_ros
-  airstack_msgs
 )
 
 # Install executable