Add files via upload

CMakeLists.txt

@@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD)
   set(CMAKE_C_STANDARD 99)
 endif()
 
-# Default to C++14
+# Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@@ -22,6 +22,7 @@ find_package(drone_interfaces REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_components REQUIRED)
+find_package(pluginlib REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -32,13 +33,14 @@ find_package(tf2_ros REQUIRED)
 find_package(tf2_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(BehaviorTreeV3 REQUIRED)
-find_package(ufomap REQUIRED)
+find_package(octomap REQUIRED)
+find_package(octomap_msgs REQUIRED)
 
 add_library(navigation_action_server SHARED
   src/navigation_server.cpp
   src/action_wait.cpp
   src/action_spin.cpp
-  src/action_follow_waypoints.cpp
+  src/action_follow_path.cpp
   src/action_compute_path_to_pose.cpp
   src/action_read_goal.cpp
   src/control_recovery_node.cpp
@@ -67,13 +69,13 @@ ament_target_dependencies(navigation_action_server
 rclcpp_components_register_node(navigation_action_server PLUGIN "navigation_lite::NavigationServer" EXECUTABLE navigation_server)
 
 add_library(controller_action_server SHARED
-  src/controller_server.cpp
-  src/conversions.cpp)
+  src/controller_server.cpp)
 target_include_directories(controller_action_server PRIVATE
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
+target_link_libraries(controller_action_server ${OCTOMAP_LIBRARIES})   
 target_compile_definitions(controller_action_server
-  PRIVATE "NAVIGATION_LITE_BUILDING_DLL")
+  PRIVATE "NAVIGATION_LITE_BUILDING_DLL")   
 ament_target_dependencies(controller_action_server
   "rclcpp"
   "rclcpp_action"
@@ -87,22 +89,42 @@ ament_target_dependencies(controller_action_server
   "tf2_msgs"
   "tf2_geometry_msgs"
   "navigation_interfaces"
-  "drone_interfaces")
-target_link_libraries(controller_action_server
-    UFO::Map
-)
-rclcpp_components_register_node(controller_action_server PLUGIN "navigation_lite::ControllerServer" EXECUTABLE controller_server)
+  "drone_interfaces"
+  "pluginlib"
+  "octomap"
+  "octomap_msgs")
+
+rclcpp_components_register_node(controller_action_server PLUGIN "navigation_lite::ControllerActionServer" EXECUTABLE controller_server)
 
 add_library(planner_action_server SHARED
-  src/planner_server.cpp
-  src/conversions.cpp
-  src/d_star_lite.cpp)
+  src/planner_server.cpp)
 target_include_directories(planner_action_server PRIVATE
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
+target_link_libraries(planner_action_server ${OCTOMAP_LIBRARIES})   
 target_compile_definitions(planner_action_server
   PRIVATE "NAVIGATION_LITE_BUILDING_DLL")
 ament_target_dependencies(planner_action_server
+  "rclcpp"
+  "rclcpp_action"
+  "tf2"
+  "tf2_ros"
+  "navigation_interfaces"
+  "pluginlib"
+  "octomap"
+  "octomap_msgs"
+  "nav_msgs"
+  "geometry_msgs")
+rclcpp_components_register_node(planner_action_server PLUGIN "navigation_lite::PlannerActionServer" EXECUTABLE planner_server)
+
+add_library(coverage_planner_action_server SHARED
+  src/coverage_planner_server.cpp)
+target_include_directories(coverage_planner_action_server PRIVATE
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_compile_definitions(coverage_planner_action_server
+  PRIVATE "NAVIGATION_LITE_BUILDING_DLL")
+ament_target_dependencies(coverage_planner_action_server
   "rclcpp"
   "rclcpp_action"
   "rclcpp_components"
@@ -115,11 +137,9 @@ ament_target_dependencies(planner_action_server
   "tf2_msgs"
   "tf2_geometry_msgs" 
   "navigation_interfaces"
-  "drone_interfaces")
-target_link_libraries(planner_action_server
-    UFO::Map
-)   
-rclcpp_components_register_node(planner_action_server PLUGIN "navigation_lite::PlannerServer" EXECUTABLE planner_server)
+)
+rclcpp_components_register_node(coverage_planner_action_server PLUGIN "navigation_lite::PlannerServer" EXECUTABLE coverage_planner_server)
+
 
 add_library(recovery_action_server SHARED
   src/recovery_server.cpp)
@@ -141,58 +161,30 @@ ament_target_dependencies(recovery_action_server
   "tf2_ros" 
   "tf2_msgs"
   "tf2_geometry_msgs"
-  "drone_interfaces")
-
+  "drone_interfaces") 
 rclcpp_components_register_node(recovery_action_server PLUGIN "navigation_lite::RecoveryServer" EXECUTABLE recovery_server)
 
-add_library(map_publish_server SHARED
-  src/map_server.cpp
-  src/sensor.cpp
-  src/sensor_precipitator.cpp 
-  src/conversions.cpp)
-target_include_directories(map_publish_server PRIVATE
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-target_compile_definitions(map_publish_server
-  PRIVATE "NAVIGATION_LITE_BUILDING_DLL")
-ament_target_dependencies(map_publish_server
-  "rclcpp"
-  "std_msgs"
-  "sensor_msgs"
-  "geometry_msgs"
-  "navigation_interfaces"
-  "tf2"
-  "tf2_ros"
-  "tf2_msgs"
-  "tf2_geometry_msgs" )
-target_link_libraries(map_publish_server
-    UFO::Map
-) 
-target_include_directories(map_publish_server PRIVATE
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-rclcpp_components_register_node(map_publish_server PLUGIN "navigation_lite::MapServer" EXECUTABLE map_server)
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
 
-
 install(TARGETS
   navigation_action_server
-  controller_action_server
   planner_action_server
+  controller_action_server
   recovery_action_server
-  map_publish_server
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin)
-
+    
 install(DIRECTORY
   launch
   DESTINATION share/${PROJECT_NAME}
 )
 
-install(DIRECTORY
-  config
-  DESTINATION share/${PROJECT_NAME}
+ament_export_include_directories(
+  include
 )
 
-
 ament_package()

README.md

@@ -1,33 +1,32 @@
 # Navigation Lite
-A lightweight Navigation stack for the drone, with full 3D navigation.  Built on the architecture of the Navigation2 stack.  I developed this one to learn how to. Much of this code can be used to build plugins for the Navigation2 stack.  The big deviation comes in with the incorporation of the Map Server in the stack. Launch the navigation stack like so:
+A lightweight Navigation stack for the drone, with full 3D navigation.  Built on the architecture of the Navigation2 stack.  I developed this one to learn how to. Much of this code can be used to build plugins for the Navigation2 stack.  The big deviation from Nav2 is that this stack works in three dimentional navigation. Launch the navigation stack like so:
 
 ```
 ros2 launch navigation_lite navigation_lite.launch.py
 ```
 
 ## Navigation Server
-Main interface waiting for messages nav_lite/navigate_to_pose to fly from point A to point B.  This uses a Behviour Tree to achieve the mission.  It is assumed that all the coordinates is in the map frame, NEU orientation, with angles in radians.
+Main interface waiting for messages nav_lite/navigate_to_pose to fly from point A to point B.  This uses a Behviour Tree to achieve the mission.  It is assumed that all the coordinates is in the map frame, NEU orientation, with angles in radians.  Coordinates are provided in the "map" frame.
 
 ## Controller Server
-Will move the drone along a collection of waypoints (typically the output of the planner server), using obstacle avoidance and a local planner. Obstacle avoidance is achieved by testing the path to the next waypoint against the latest published map.  The past has to be free space.  If the path is not free space, it implies that an obstacle has moved accros the path (and was detected by a sensor).  The acton server should then stop motion, and return unsuccessfull (A number of waypoints were not reached).
+Will move the drone along a collection of waypoints (typically the output of the planner server), using obstacle avoidance and a local planner. Obstacle avoidance is achieved by testing the path to the next waypoint against the latest published map.  
+
+This server uses plugins. It is important that you install a suitable plugin package.  See [Controller Plugins] (https://github.com/slaghuis/controller_plugins).  
 
 ## Recovery Server
 Executes recovery actions.  Recivery action implimented are wait and spin.  Spin will rotate the drone at the current altitude a set arc.  Hopefully this gives the sensors time to inform the map server of any obstacles.  A wait recovery is also available.  This will maintain altitude and position for a set duration of time.  This might allow an obstacle (the dog for instance) to move along, and the sensors to detect a clear path again. Future recovery actions could inclue to change the altitude x meters.  
 
 ## Planner Server
-Reads a UFO Octree Map from the Map Server and calculates a global flight plan.  Returns a sequence of waypoints for the Controller Server to follow. Uses D* Lite path planning.  Still needs to impliment replanning and services to clear the cost map.  Calculating a plan over 4 meters takes 2 (two) seconds on a Raspebrry Pi 4.  This slow performance is due to the fact that every node (one cubic meter) can have 26 (twenty six) neighbors that have to be expanded (each to their 26 neigbours.  Longer paths become exponentially slower.  A cool improvement would be a more optimistic path planning algorithm that will assuma clear path, and then execute an avoidance once an obstacle has been detected.
+Reads a Octree Map from the Map Server (external server using the [ROS2 Octomap Server](https://github.com/OctoMap/octomap_mapping) )and calculates a global flight plan.  Returns a sequence of waypoints for the Controller Server to follow. 
 
-## Map server
-Reads a list of sensors and their transforms from the parameter file and populates an octo map data structure. (UFO Map package).  The node publishes this map to the rest of the naviagation stack in a propriotary message type.
+This server uses plugins. It is important that you install a suitable plugin package.  See [Planner Plugins] (https://github.com/slaghuis/planner_plugins) for a well coded Theta Star algoritm.  Other plugins are also available. 
+
+## Dependancies
+This package needs a Octomap server [ROS2 Octomap Server](https://github.com/OctoMap/octomap_mapping).  The Octomap server needs sensor_msgs::msg::PointCloud publisher to build the map. (One could also load a static map, bat that is noo fun).  To asist, one could run the [Sensor PointCloud](https://github.com/slaghuis/sensor_pointcloud) package to read one or more range sensors, and publish the data in a point cloud.  One day someone will sponsor me a Intel Realsense camera for my drone.
+
+The planner server also requires a plugin to funciton.  See [Planner Plugins](https://github.com/slaghuis/planner_plugins)
+
+This package depend on the [Navigation Lite Interfaces](https://github.com/slaghuis/navigation_interfaces) for message definitions.
 
-# Transforms
-The Map Server reads transforms for the range sensors from the configuration file. This is the base_link->sensors transform.  The Map Server further uses a map in the map frame. The drone_mavsdk node publishes a odom->base_link transdorm. What is needed is a static transform to publish a map->odom transform.  
-```
-ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 map odom
-```
-This then forms the complete tf2 tree to produce a map->odom->base_link->sensor transform tree.  Test it with
-```
-ros2 run tf2_tools view_frames.py
-```
 # Code Status
-This code has flown on a drone!  The drone was controlled by a Pixhawk mini 4.0 controlled via a Raspberry Pi 4 4Gb companion computer via UART.  This Navigation stack used the [drone_mavsdk](https://github.com/slaghuis/drone_mavsdk) node to effect the movement.  The mission was controlled via the [flight_control](https://github.com/slaghuis/flight_control) node.
+This code has not flown on a drone yet.  If you have to fly today, please use the main branch of this package! 

behavior_trees/navigate.xml

@@ -6,9 +6,9 @@
                 <ReadGoal pose="{target_pose}" />
                 <PipelineSequence>
                     <Wait   seconds="2" />
-                    <ComputePathToPose pose="{target_pose}" path="{planned_path}" />
+                    <ComputePathToPose pose="{target_pose}" planner="ThetaStar" path="{planned_path}" />
                 </PipelineSequence>    
-                <FollowWaypoints path="{planned_path}" />
+                <FollowPath path="{planned_path}" controller="VhfPlusController" />
             </Sequence>
         </ReactiveSequence>    
      </BehaviorTree>

include/navigation_lite/action_compute_path_to_pose.h

@@ -51,6 +51,7 @@ class NavLiteComputePathToPoseAction : public BT::AsyncActionNode
     static BT::PortsList providedPorts()
     {
         return{ BT::InputPort<Pose3D>("pose"), 
+                BT::InputPort<std::string>("planner"),
                 BT::InputPort<Pose3D>("start"), 
                 BT::OutputPort<std::vector<Pose3D>>("path")};
 

include/navigation_lite/action_follow_path.h

@@ -0,0 +1,82 @@
+#ifndef ACTION_FollowPath_H
+#define ACTION_FollowPath_H
+
+#include <functional>
+#include <future>
+#include <memory>
+#include <string>
+#include <vector>  //Dealing with a vector of poses to define waypoints
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+#include <tf2/LinearMath/Quaternion.h>
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "navigation_interfaces/action/follow_path.hpp"
+
+#include "navigation_lite/navigation_server.h"
+#include "navigation_lite/pose_3D.h"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace NavigationNodes
+{
+
+class NavLiteFollowPathAction : public BT::AsyncActionNode
+{
+  public:
+    using FollowPath = navigation_interfaces::action::FollowPath;
+    using GoalHandleFollowPath = rclcpp_action::ClientGoalHandle<FollowPath>;
+
+    NavLiteFollowPathAction(const std::string& name, const BT::NodeConfiguration& config)
+      : BT::AsyncActionNode(name, config)
+    { }
+
+    void init(rclcpp::Node::SharedPtr node) {
+      node_ = node;
+
+       this->client_ptr_ = rclcpp_action::create_client<FollowPath>(
+       node_,
+      "nav_lite/follow_path");
+    }
+
+    static BT::PortsList providedPorts()
+    {
+        return{ BT::InputPort<std::vector<Pose3D>>("path"),
+                BT::InputPort<std::string>("controller") };
+    }
+
+    BT::NodeStatus tick() override;
+    void halt() override;
+    void cleanup();
+
+  private:
+    std::atomic_bool _halt_requested;
+    NavigationNodes::ActionStatus action_status;
+
+    // Pointer to the ROS node
+    rclcpp::Node::SharedPtr node_;
+    rclcpp::CallbackGroup::SharedPtr callback_group_;
+    rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
+
+    rclcpp_action::Client<FollowPath>::SharedPtr client_ptr_;
+
+    std::shared_ptr<std::shared_future<GoalHandleFollowPath::SharedPtr>> future_goal_handle_;
+    GoalHandleFollowPath::SharedPtr goal_handle_;
+
+    void goal_response_callback(std::shared_future<GoalHandleFollowPath::SharedPtr> future);
+    void feedback_callback( 
+      GoalHandleFollowPath::SharedPtr,
+      const std::shared_ptr<const FollowPath::Feedback> feedback);
+    void result_callback(const GoalHandleFollowPath::WrappedResult & result);
+
+
+};
+
+} // Namespace
+
+#endif // ACTION_FollowPath_H
+

include/navigation_lite/controller.hpp

@@ -0,0 +1,40 @@
+#ifndef NAVIGATION_LITE_CONTROLLER_HPP
+#define NAVIGATION_LITE_CONTROLLER_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_ros/buffer.h>
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+
+#include <octomap/octomap.h>
+#include <octomap/OcTree.h>
+
+namespace navigation_lite
+{
+  class Controller
+  {
+    public:
+      virtual void configure(const rclcpp::Node::SharedPtr parent, 
+                             std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
+                             std::shared_ptr<octomap::OcTree> costmap ) = 0;
+
+      virtual void setPath(const nav_msgs::msg::Path & path) = 0;
+
+      virtual geometry_msgs::msg::TwistStamped computeVelocityCommands(
+        const geometry_msgs::msg::PoseStamped & pose,
+        const geometry_msgs::msg::Twist & speed) = 0;
+//        nav2_core::GoalChecker * goal_checker) = 0;
+
+      virtual ~Controller() {}
+
+    protected:
+      Controller(){}
+  };
+} // namespace navigation_lite
+
+#endif // NAVIGATION_LITE_CONTROLLER_HPP