Global planner ros2 version with documentation (simulation tested)

.gitmodules

@@ -0,0 +1,15 @@
+[submodule "octomap_msgs"]
+	path = octomap_msgs
+	url = https://github.com/OctoMap/octomap_msgs
+	branch = ros2
+[submodule "octomap_server2"]
+	path = octomap_server2
+	url = https://github.com/ldg810/octomap_server2
+[submodule "perception_pcl"]
+	path = perception_pcl
+	url = https://github.com/ros-perception/perception_pcl
+	branch = foxy-devel
+[submodule "pcl_msgs"]
+	path = pcl_msgs
+	url = https://github.com/ros-perception/pcl_msgs
+	branch = ros2

avoidance/CMakeLists.txt

@@ -23,7 +23,9 @@ find_package(PythonInterp REQUIRED)
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED NO_MODULE)
 find_package(Boost REQUIRED COMPONENTS system)
-find_package(PCL REQUIRED)
+find_package(PCL 1.8 REQUIRED)
+
+link_directories(${PCL_LIBRARY_DIRS})
 
 if(DISABLE_SIMULATION)
   message(STATUS "Building avoidance without Gazebo Simulation")
@@ -64,7 +66,7 @@ endif()
 
 # Add avoidance lib
 add_library(avoidance SHARED "${AVOIDANCE_CPP_FILES}")
-ament_target_dependencies(avoidance Eigen3 px4_msgs ${${PROJECT_NAME}_EXPORTED_TARGETS})
+ament_target_dependencies(avoidance Eigen3 px4_msgs PCL ${${PROJECT_NAME}_EXPORTED_TARGETS})
 target_include_directories(avoidance PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}>
   $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
@@ -75,15 +77,16 @@ target_include_directories(avoidance PUBLIC
 target_link_libraries(
   avoidance
   Eigen3::Eigen
+  ${PCL_LIBRARIES}
   ${YAML_CPP_LIBRARIES}
 )
 
 # Add node dependencies
-ament_target_dependencies(avoidance px4_msgs rclcpp Eigen3::Eigen)
+ament_target_dependencies(avoidance px4_msgs rclcpp Eigen3)
 
 # Export information to downstream packages
 ament_export_dependencies(ament_cmake rclcpp rosidl_default_runtime eigen3_cmake_module Eigen3 px4_msgs geometry_msgs sensor_msgs std_msgs)
-ament_export_interfaces(export_avoidance HAS_LIBRARY_TARGET)
+ament_export_targets(export_avoidance HAS_LIBRARY_TARGET)
 ament_export_include_directories(include)
 ament_export_libraries(avoidance)
 
@@ -113,6 +116,11 @@ install(DIRECTORY
         DESTINATION share/${PROJECT_NAME}
 )
 
+install(DIRECTORY
+	sim
+	DESTINATION share/${PROJECT_NAME}
+)
+
 #############
 ## Testing ##
 #############

avoidance/include/avoidance/avoidance_node.h

@@ -5,11 +5,16 @@
 
 #include "avoidance/common.h"
 #include <px4_msgs/msg/telemetry_status.hpp>
+#include <px4_msgs/msg/vehicle_odometry.hpp>
+#include <px4_msgs/msg/vehicle_local_position.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
 
 #include <chrono>
 
 namespace avoidance {
 
+using std::placeholders::_1;
+
 class AvoidanceNode {
  public:
   AvoidanceNode();
@@ -37,13 +42,20 @@ class AvoidanceNode {
  private:
   // telemetry_status Publisher
   rclcpp::Publisher<px4_msgs::msg::TelemetryStatus>::SharedPtr telemetry_status_pub_;
+  rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr odom_sub_;
+  rclcpp::Subscription<px4_msgs::msg::VehicleLocalPosition>::SharedPtr position_sub_;
 
   rclcpp::executors::MultiThreadedExecutor cmdloop_executor_;
   rclcpp::executors::MultiThreadedExecutor statusloop_executor_;
-
   rclcpp::TimerBase::SharedPtr cmdloop_timer_;
   rclcpp::TimerBase::SharedPtr statusloop_timer_;
 
+  std::thread* statusloop_thread;
+  std::thread* cmdloop_thread;
+
+  rclcpp::Node::SharedPtr avoidance_node_status;
+  rclcpp::Node::SharedPtr avoidance_node_cmd;
+
   MAV_STATE companion_state_ = MAV_STATE::MAV_STATE_STANDBY;
 
   // PX4 Firmware parameters
@@ -56,15 +68,16 @@ class AvoidanceNode {
   rclcpp::Duration timeout_critical_;
   rclcpp::Duration timeout_startup_;
 
+  int agent_number_ = 1;
   bool position_received_;
   bool should_exit_;
 
   float mission_item_speed_;
 
   void publishSystemStatus();
-
+  
   rclcpp::Logger avoidance_node_logger_ = rclcpp::get_logger("avoidance_node");
 
 };
 }
-#endif  // AVOIDANCE_AVOIDANCE_NODE_H
+#endif  // AVOIDANCE_AVOIDANCE_NODE_H

avoidance/include/avoidance/common.h

@@ -9,6 +9,7 @@
 #include <Eigen/Dense>
 
 #include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 #include <geometry_msgs/msg/vector3_stamped.hpp>
@@ -294,6 +295,19 @@ float getYawFromQuaternion(const Eigen::Quaternionf q);
 **/
 float getPitchFromQuaternion(const Eigen::Quaternionf q);
 
+
+/**
+* @brief     Tranform yaw angle to quaternion msg
+* @returns   quaternion msg (in geometry_msgs)
+**/
+geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw);
+
+/**
+* @brief     Tranform between NED PoseStamped msg and ENU PoseStamped msg
+* @returns   PoseStamped msg (in geometry_msgs)
+**/
+geometry_msgs::msg::PoseStamped transformNEDandENU(geometry_msgs::msg::PoseStamped pose);
+
 /**
 * @brief     wrappes the input angle in to plus minus PI space
 * @param[in] angle to be wrapped  [rad]

avoidance/include/avoidance/rviz_world_loader.h

@@ -13,12 +13,21 @@
 #include "rclcpp/rclcpp.hpp"
 #include <visualization_msgs/msg/marker.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
-#include <px4_msgs/msg/vehicle_odometry.hpp>
+// #include <px4_msgs/msg/vehicle_odometry.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/create_timer_ros.h>
+#include <tf2_ros/buffer.h>
+#include <tf2/convert.h>
 
 #include <sys/stat.h>
 
 namespace avoidance {
 
+using std::placeholders::_1;
+using namespace std::chrono_literals;
+
 // data types
 struct world_object {
   std::string type;
@@ -44,15 +53,22 @@ class WorldVisualizer : public rclcpp::Node
   int resolveUri(std::string& uri) const;
 
   rclcpp::TimerBase::SharedPtr loop_timer_;
+  rclcpp::TimerBase::SharedPtr visualize_drone_timer_;
 
-  rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr pose_sub_;
+  // rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr pose_sub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr world_pub_;
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr drone_pub_;
 
   std::string world_path_;
 
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;  
+  std::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster_;
+
   void loopCallback();
 
+  void tf2Callback(const std::shared_future<geometry_msgs::msg::TransformStamped>& tf);
+
 public:
   /**
    * @brief     WorldVisualizer Node Class
@@ -63,7 +79,7 @@ class WorldVisualizer : public rclcpp::Node
   /**
    * @brief      callback for subscribing mav pose topic
    **/
-  void positionCallback(const px4_msgs::msg::VehicleOdometry::SharedPtr msg) const;
+  // void positionCallback(const px4_msgs::msg::VehicleOdometry::SharedPtr msg) const;
 
   /**
    * @brief      parse the yaml file and publish world marker
@@ -75,7 +91,7 @@ class WorldVisualizer : public rclcpp::Node
    * @brief      visualize the drone mesh at the current drone position
    * @param[in]  pose, current drone pose
    **/
-  int visualizeDrone(const px4_msgs::msg::VehicleOdometry& pose) const;
+  void visualizeDrone();
 };
 }
 

avoidance/sim/worlds/simple_obstacle.yaml

@@ -1,95 +1,95 @@
 
  - type: "mesh"
    name: "pinetree1"
-   frame_id: "local_origin"
+   frame_id: "base_frame"
    mesh_resource: "model://pine_tree/meshes/pine_tree.dae"
    position: [4.0, 7.0, 0.0]
    orientation: [0.0, 0.0, 0.0, 1.0]
    scale: [1.0, 1.0, 1.0]
 
  - type: "mesh"
    name: "pinetree2"
-   frame_id: "local_origin"
+   frame_id: "base_frame"
    mesh_resource: "model://pine_tree/meshes/pine_tree.dae"
    position: [6.1, 7.6, 0.0]
    orientation: [0.0, 0.0, 0.0, 1.0]
    scale: [1.0, 1.0, 1.0]
 
  - type: "mesh"
    name: "pinetree3"
-   frame_id: "local_origin"
+   frame_id: "base_frame"
    mesh_resource: "model://pine_tree/meshes/pine_tree.dae"
    position: [19.3, -14.0, 0.0]
    orientation: [0.0, 0.0, 0.0, 1.0]
    scale: [1.0, 1.0, 1.0]
 
  - type: "mesh"
    name: "pinetree4"
-   frame_id: "local_origin"
+   frame_id: "base_frame"
    mesh_resource: "model://pine_tree/meshes/pine_tree.dae"
    position: [16.0, -11.5, 0.0]
    orientation: [0.0, 0.0, 0.0, 1.0]
    scale: [1.0, 1.0, 1.0]
 
  - type: "mesh"
    name: "oaktree1"
-   frame_id: "local_origin"
+   frame_id: "base_frame"
    mesh_resource: "model://oak_tree/meshes/oak_tree.dae"
    position: [10.0, 0.0, 0.0]
    orientation: [0.0, 0.0, 0.0, 1.0]
    scale: [1.0, 1.0, 1.0]
 
  - type: "mesh"
    name: "oaktree2"
-   frame_id: "local_origin"
+   frame_id: "base_frame"
    mesh_resource: "model://oak_tree/meshes/oak_tree.dae"
    position: [10.25, -4.25, 0.0]
    orientation: [0.0, 0.0, 0.0, 1.0]
    scale: [1.0, 1.0, 1.0]
 
  - type: "mesh"
    name: "oaktree3"
-   frame_id: "local_origin"
+   frame_id: "base_frame"
    mesh_resource: "model://oak_tree/meshes/oak_tree.dae"
    position: [20.2, 14.4, 0.0]
    orientation: [0.0, 0.0, 0.0, 1.0]
    scale: [1.0, 1.0, 1.0]
 
  - type: "mesh"
    name: "oaktree4"
-   frame_id: "local_origin"
+   frame_id: "base_frame"
    mesh_resource: "model://oak_tree/meshes/oak_tree.dae"
    position: [30.0, 8.5, 0.0]
    orientation: [0.0, 0.0, 0.0, 1.0]
    scale: [1.0, 1.0, 1.0]
 
  - type: "mesh"
    name: "oaktree5"
-   frame_id: "local_origin"
+   frame_id: "base_frame"
    mesh_resource: "model://oak_tree/meshes/oak_tree.dae"
    position: [136.2, -0.4, 0.0]
    orientation: [0.0, 0.0, 0.0, 1.0]
    scale: [1.0, 1.0, 1.0]
 
  - type: "mesh"
    name: "oaktree6"
-   frame_id: "local_origin"
+   frame_id: "base_frame"
    mesh_resource: "model://oak_tree/meshes/oak_tree.dae"
    position: [38.0, -9.8, 0.0]
    orientation: [0.0, 0.0, 0.0, 1.0]
    scale: [1.0, 1.0, 1.0]
 
  - type: "mesh"
    name: "oaktree7"
-   frame_id: "local_origin"
+   frame_id: "base_frame"
    mesh_resource: "model://oak_tree/meshes/oak_tree.dae"
    position: [38.6, -4.6, 0.0]
    orientation: [0.0, 0.0, 0.0, 1.0]
    scale: [1.0, 1.0, 1.0]
 
  - type: "mesh"
    name: "oaktree8"
-   frame_id: "local_origin"
+   frame_id: "base_frame"
    mesh_resource: "model://oak_tree/meshes/oak_tree.dae"
    position: [16.0, 30.0, 1.6]
    orientation: [0.0, 0.0, 0.0, 1.0]

avoidance/src/avoidance_node.cpp

@@ -9,7 +9,7 @@ namespace avoidance {
 AvoidanceNode::AvoidanceNode()
     : cmdloop_dt_(100ms),
       statusloop_dt_(200ms),
-      timeout_termination_(15000000000ns), // ns
+      timeout_termination_(15000000000000ns), // ns
       timeout_critical_(500000000ns), // ns
       timeout_startup_(5000000000ns), // ns
       position_received_(true),
@@ -29,29 +29,27 @@ AvoidanceNode::~AvoidanceNode() {}
 void AvoidanceNode::init() {
   setSystemStatus(MAV_STATE::MAV_STATE_BOOT);
 
-  // Command loop executor
-  auto cmd_loop_callback = [&]() { /* Empty ?? */ };
-  auto avoidance_node_cmd = rclcpp::Node::make_shared("avoidance_node_cmd");
-  cmdloop_timer_ = avoidance_node_cmd->create_wall_timer(cmdloop_dt_, cmd_loop_callback);
+
+  avoidance_node_cmd = rclcpp::Node::make_shared("avoidance_node_cmd");
+  cmdloop_timer_ = avoidance_node_cmd->create_wall_timer(cmdloop_dt_, [&](){});
   cmdloop_executor_.add_node(avoidance_node_cmd);
 
-  // Status loop executor
-  auto status_loop_callback = [&]() { publishSystemStatus(); };
-  auto avoidance_node_status = rclcpp::Node::make_shared("avoidance_node_status");
+  // Start cmdloop thread
+  cmdloop_thread = new std::thread([&]() {
+    cmdloop_executor_.spin();
+  });
+
+  avoidance_node_status = rclcpp::Node::make_shared("avoidance_node_status");
   // This is a passthrough that replaces the usage of Mavlink Heartbeats
   telemetry_status_pub_ =
-        avoidance_node_status->create_publisher<px4_msgs::msg::TelemetryStatus>("TelemetryStatus_PubSubTopic", 1);
-  statusloop_timer_ = avoidance_node_status->create_wall_timer(statusloop_dt_, status_loop_callback);
+        avoidance_node_status->create_publisher<px4_msgs::msg::TelemetryStatus>("/TelemetryStatus_PubSubTopic", 1);
+  statusloop_timer_ = avoidance_node_status->create_wall_timer(statusloop_dt_, [&](){ publishSystemStatus(); });
   statusloop_executor_.add_node(avoidance_node_status);
 
-  auto statusloop_spin_executor = [&]() {
+  // Start statusloop thread
+  statusloop_thread = new std::thread([&]() {
     statusloop_executor_.spin();
-  };
-
-  // Launch both executors
-  std::thread execution_thread(statusloop_spin_executor);
-  cmdloop_executor_.spin();
-  execution_thread.join();
+  });
 }
 
 void AvoidanceNode::setSystemStatus(MAV_STATE state) {
@@ -66,13 +64,14 @@ void AvoidanceNode::publishSystemStatus() {
   // Publish companion process status as telemetry_status msg
   auto status_msg = px4_msgs::msg::TelemetryStatus();
 
-  status_msg.timestamp = std::chrono::time_point_cast<std::chrono::microseconds>(std::chrono::steady_clock::now()).time_since_epoch().count();
+  // TODO : TelemetryStatus.msg is totally changed in px4_msgs. It should be reflected.
+  /*status_msg.timestamp = std::chrono::time_point_cast<std::chrono::microseconds>(std::chrono::steady_clock::now()).time_since_epoch().count();
   status_msg.heartbeat_time = status_msg.timestamp;
   status_msg.remote_system_id = 1;
   status_msg.remote_component_id = px4_msgs::msg::TelemetryStatus::COMPONENT_ID_OBSTACLE_AVOIDANCE;
   status_msg.remote_type = px4_msgs::msg::TelemetryStatus::MAV_TYPE_ONBOARD_CONTROLLER;
   status_msg.remote_system_status = (int)(this->getSystemStatus());
-  status_msg.type = px4_msgs::msg::TelemetryStatus::LINK_TYPE_WIRE;
+  status_msg.type = px4_msgs::msg::TelemetryStatus::LINK_TYPE_WIRE;*/
 
   telemetry_status_pub_->publish(status_msg);
 }