RRT global planner

avoidance/include/avoidance/common.h

@@ -327,6 +327,8 @@ double getAngularVelocity(float desired_yaw, float curr_yaw);
 void transformToTrajectory(mavros_msgs::Trajectory& obst_avoid, geometry_msgs::PoseStamped pose,
                            geometry_msgs::Twist vel);
 
+void transformToTrajectory(mavros_msgs::Trajectory& obst_avoid, geometry_msgs::PoseStamped pose);
+
 /**
 * @brief      fills MavROS trajectory messages with NAN
 * @param      point, setpoint to be filled with NAN

avoidance/src/common.cpp

@@ -278,6 +278,14 @@ double getAngularVelocity(float desired_yaw, float curr_yaw) {
   return 0.5 * static_cast<double>(vel);
 }
 
+void transformToTrajectory(mavros_msgs::Trajectory& obst_avoid, geometry_msgs::PoseStamped pose) {
+  geometry_msgs::Twist vel;
+  vel.linear.x = NAN;
+  vel.linear.y = NAN;
+  vel.linear.z = NAN;
+  transformToTrajectory(obst_avoid, pose, vel);
+}
+
 void transformToTrajectory(mavros_msgs::Trajectory& obst_avoid, geometry_msgs::PoseStamped pose,
                            geometry_msgs::Twist vel) {
   obst_avoid.header.stamp = ros::Time::now();

global_planner/CMakeLists.txt

@@ -23,6 +23,7 @@ find_package(catkin REQUIRED COMPONENTS
 )
 find_package(PCL 1.7 REQUIRED)
 find_package(octomap REQUIRED)
+find_package(ompl REQUIRED)
 
 if(DISABLE_SIMULATION)
   message(STATUS "Building avoidance without Gazebo Simulation")
@@ -145,6 +146,7 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
   ${PCL_INCLUDE_DIRS}
   ${OCTOMAP_INCLUDE_DIRS}
+  ${OMPL_INCLUDE_DIR}
   ${YAML_CPP_INCLUDE_DIR}
 )
 link_libraries(${OCTOMAP_LIBRARIES})
@@ -155,6 +157,8 @@ add_library(global_planner
   src/library/cell.cpp
   src/library/global_planner.cpp
   src/nodes/global_planner_node.cpp
+  src/library/octomap_ompl_rrt.cpp
+  src/library/octomap_rrt_planner.cpp
 )
 
 ## Add cmake target dependencies of the library
@@ -165,9 +169,13 @@ add_dependencies(global_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXP
 ## Declare a C++ executable
 add_executable(global_planner_node src/nodes/global_planner_node_main.cpp)
 
+add_executable(octomap_rrt_planner_node src/nodes/octomap_rrt_planner_node.cpp)
+target_link_libraries(octomap_rrt_planner_node global_planner ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${YAML_CPP_LIBRARIES} ${OMPL_LIBRARIES}
+)
+
 ## Specify libraries to link a library or executable target against
 target_link_libraries(global_planner_node
-  global_planner ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${YAML_CPP_LIBRARIES}
+  global_planner ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${YAML_CPP_LIBRARIES} ${OMPL_LIBRARIES}
 )
 
 #############

global_planner/cfg/RRTPlannerNode.yaml

@@ -0,0 +1,19 @@
+
+# RRT Bounds
+min_altitude: 3.0 #min altitude [meters] to plan
+max_altitude: 7.5 #max altitude [meters] to plan
+
+max_x: 5.0 #how much ahead of the goal in the x direction
+max_y: 5.0 #how much ahead of the goal in the y direction
+
+min_x: 5.0 #how much behind of the current location in the x direction
+min_y: 5.0 #how much behind of the current location in the y direction
+
+# Planner params
+goal_radius: 0.5   #Minimum allowed distance from path end to goal
+goal_altitude: 6.0 #Default goal altitude
+
+# Control params
+default_speed: 2.0    #Default speed of the drone
+control_loop_dt: 0.05 #Control loop rate
+planner_loop_dt: 0.1  #Planner loop rate

global_planner/include/global_planner/octomap_ompl_rrt.h

@@ -0,0 +1,45 @@
+//  July/2018, ETHZ, Jaeyoung Lim, [email protected]
+
+#ifndef OCTOMAP_OMPL_RRT_H
+#define OCTOMAP_OMPL_RRT_H
+
+#include <ros/ros.h>
+#include <ros/subscribe_options.h>
+#include <tf/transform_broadcaster.h>
+
+#include <stdio.h>
+#include <cstdlib>
+#include <sstream>
+#include <string>
+
+#include <octomap/octomap.h>
+#include <Eigen/Dense>
+
+#include <global_planner/ompl_setup.h>
+
+using namespace std;
+using namespace Eigen;
+
+class OctomapOmplRrt {
+ private:
+  ros::NodeHandle nh_;
+  ros::NodeHandle nh_private_;
+
+  ompl::OmplSetup problem_setup_;
+  octomap::OcTree* map_;
+
+  Eigen::Vector3d lower_bound_, upper_bound_;
+
+ public:
+  OctomapOmplRrt(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private);
+  virtual ~OctomapOmplRrt();
+
+  void setupProblem();
+  void setBounds(Eigen::Vector3d& lower_bound, Eigen::Vector3d& upper_bound);
+  void setMap(octomap::OcTree* map);
+  bool getPath(const Eigen::Vector3d& start, const Eigen::Vector3d& goal, std::vector<Eigen::Vector3d>* path);
+  void solutionPathToTrajectoryPoints(ompl::geometric::PathGeometric& path,
+                                      std::vector<Eigen::Vector3d>* trajectory_points) const;
+};
+
+#endif

global_planner/include/global_planner/octomap_rrt_planner.h

@@ -0,0 +1,137 @@
+//  June/2019, ETHZ, Jaeyoung Lim, [email protected]
+
+#ifndef OCTOMAP_RRT_PLANNER_H
+#define OCTOMAP_RRT_PLANNER_H
+
+#include <stdio.h>
+#include <cstdlib>
+#include <cmath>
+#include <mutex>
+#include <sstream>
+#include <string>
+
+#include <Eigen/Dense>
+
+#include <ompl/base/StateValidityChecker.h>
+#include <ompl/base/spaces/SE3StateSpace.h>
+#include <ompl/geometric/SimpleSetup.h>
+
+#include <nav_msgs/Path.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/transforms.h>
+#include <ros/ros.h>
+#include <ros/subscribe_options.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+
+#include <mavros_msgs/Trajectory.h>
+#include <octomap/octomap.h>
+#include <octomap_msgs/Octomap.h>
+#include <octomap_msgs/conversions.h>
+
+#include <avoidance/avoidance_node.h>
+//#include <avoidance/common.h>
+#include <global_planner/common.h>
+#include <global_planner/common_ros.h>
+#include <global_planner/octomap_ompl_rrt.h>
+
+#ifndef DISABLE_SIMULATION
+#include <avoidance/rviz_world_loader.h>
+#endif
+
+using namespace std;
+using namespace Eigen;
+namespace ob = ompl::base;
+namespace og = ompl::geometric;
+
+struct cameraData {
+  ros::Subscriber pointcloud_sub_;
+};
+
+class OctomapRrtPlanner {
+ private:
+  std::mutex mutex_;
+
+  ros::NodeHandle nh_;
+  ros::NodeHandle nh_private_;
+
+  ros::Publisher trajectory_pub_;
+  ros::Publisher global_path_pub_;
+  ros::Publisher actual_path_pub_;
+  ros::Publisher pointcloud_pub_;
+  ros::Publisher mavros_waypoint_publisher_;
+  ros::Publisher current_waypoint_publisher_;
+
+  ros::Subscriber pose_sub_;
+  ros::Subscriber octomap_full_sub_;
+  ros::Subscriber move_base_simple_sub_;
+  ros::Subscriber desiredtrajectory_sub_;
+
+  ros::Timer cmdloop_timer_;
+  ros::Timer plannerloop_timer_;
+  ros::CallbackQueue cmdloop_queue_;
+  ros::CallbackQueue plannerloop_queue_;
+  std::unique_ptr<ros::AsyncSpinner> cmdloop_spinner_;
+  std::unique_ptr<ros::AsyncSpinner> plannerloop_spinner_;
+
+  ros::Time last_wp_time_;
+  ros::Time start_time_;
+
+  double cmdloop_dt_;
+  double plannerloop_dt_;
+  bool hover_;
+  bool plan_;
+  int num_octomap_msg_;
+  std::string frame_id_;
+
+  double max_altitude_, min_altitude_;
+  double max_x_, max_y_, min_x_, min_y_;
+  double goal_radius_;
+  double speed_;
+  double goal_altitude_;
+  double yaw_;
+
+  Eigen::Vector3d local_position_, local_velocity_;
+  Eigen::Vector3d reference_pos_;
+  geometry_msgs::PoseStamped last_pos_;
+  Eigen::Quaternionf reference_att_;
+  Eigen::Vector3d goal_;
+  geometry_msgs::PoseStamped previous_goal_,current_goal_, global_goal_;
+
+  std::vector<Eigen::Vector3d> current_path_;
+  nav_msgs::Path actual_path_;
+  std::vector<cameraData> cameras_;
+  tf::TransformListener listener_;
+
+  octomap::OcTree* octree_ = NULL;
+
+  OctomapOmplRrt rrt_planner_;
+  avoidance::AvoidanceNode avoidance_node_;
+#ifndef DISABLE_SIMULATION
+  std::unique_ptr<avoidance::WorldVisualizer> world_visualizer_;
+#endif
+  void cmdLoopCallback(const ros::TimerEvent& event);
+  void plannerLoopCallback(const ros::TimerEvent& event);
+  void octomapFullCallback(const octomap_msgs::Octomap& msg);
+  void positionCallback(const geometry_msgs::PoseStamped& msg);
+  void velocityCallback(const geometry_msgs::TwistStamped& msg);
+  void DesiredTrajectoryCallback(const mavros_msgs::Trajectory& msg);
+  void moveBaseSimpleCallback(const geometry_msgs::PoseStamped& msg);
+  void depthCameraCallback(const sensor_msgs::PointCloud2& msg);
+  void publishSetpoint();
+  void initializeCameraSubscribers(std::vector<std::string>& camera_topics);
+  void publishPath();
+  geometry_msgs::PoseStamped vector3d2PoseStampedMsg(Eigen::Vector3d position);
+  void updateReference(ros::Time current_time);
+  bool isCloseToGoal();
+  double poseDistance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2);
+  void checkBounds(const Eigen::Vector3d& in_lower, const Eigen::Vector3d& in_upper, Eigen::Vector3d& out_lower, Eigen::Vector3d& out_upper );
+
+ public:
+  OctomapRrtPlanner(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private);
+  virtual ~OctomapRrtPlanner();
+  void planWithSimpleSetup();
+  double planYaw();
+};
+
+#endif

global_planner/include/global_planner/ompl_octomap.h

@@ -0,0 +1,53 @@
+#ifndef OCTOMAP_OMPL_H
+#define OCTOMAP_OMPL_H
+
+#include <octomap/octomap.h>
+#include <ompl/base/StateValidityChecker.h>
+
+namespace ompl {
+
+class OctomapValidityChecker : public base::StateValidityChecker {
+ public:
+  OctomapValidityChecker(const base::SpaceInformationPtr& space_info, octomap::OcTree* map)
+      : base::StateValidityChecker(space_info), map_(map) {}
+  virtual bool isValid(const base::State* state) const {
+    Eigen::Vector3d position(state->as<ompl::base::RealVectorStateSpace::StateType>()->values[0],
+                             state->as<ompl::base::RealVectorStateSpace::StateType>()->values[1],
+                             state->as<ompl::base::RealVectorStateSpace::StateType>()->values[2]);
+    bool collision = checkCollision(position);
+    if (collision)
+      return false;
+    else
+    {
+      return true;
+      std::cout << "IN COLLISION!\n";
+    }
+
+  }
+
+  virtual bool checkCollision(Eigen::Vector3d state) const {
+    bool collision = true;
+    double occprob = 1.0;
+    uint octree_depth = 16;
+    double logodds;
+
+    if (map_) {
+      octomap::OcTreeNode* node = map_->search(state(0), state(1), state(2), octree_depth);
+      if (node)
+        occprob = octomap::probability(logodds = node->getValue());
+      else
+        occprob = 0.5;  // Unobserved region of the map has equal chance of being occupied / unoccupied
+
+      // Assuming a optimistic planner: Unknown space is considered as unoccupied
+      if (occprob <= 0.5) collision = false;
+
+      return collision;
+    }
+  }
+
+ protected:
+  octomap::OcTree* map_;
+};
+}  // namespace ompl
+
+#endif

global_planner/include/global_planner/ompl_setup.h

@@ -0,0 +1,44 @@
+#ifndef OMPL_SETUP_H
+#define OMPL_SETUP_H
+
+#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
+
+#include <ompl/base/spaces/SE3StateSpace.h>
+#include <ompl/geometric/SimpleSetup.h>
+#include <ompl/geometric/planners/rrt/RRTstar.h>
+#include "ompl/base/SpaceInformation.h"
+
+#include <global_planner/ompl_octomap.h>
+
+namespace ompl {
+
+class OmplSetup : public geometric::SimpleSetup {
+ public:
+  OmplSetup() : geometric::SimpleSetup(base::StateSpacePtr(new base::RealVectorStateSpace(3))) {}
+
+  void setDefaultObjective() {
+    getProblemDefinition()->setOptimizationObjective(
+        ompl::base::OptimizationObjectivePtr(new ompl::base::PathLengthOptimizationObjective(getSpaceInformation())));
+  }
+
+  void setDefaultPlanner() { setRrtStar(); }
+
+  void setRrtStar() { setPlanner(ompl::base::PlannerPtr(new ompl::geometric::RRTstar(getSpaceInformation()))); }
+
+  const base::StateSpacePtr& getGeometricComponentStateSpace() const { return getStateSpace(); }
+
+  void setStateValidityCheckingResolution(double resolution) {
+    // This is a protected attribute, so need to wrap this function.
+    si_->setStateValidityCheckingResolution(resolution);
+  }
+
+  void setOctomapCollisionChecking(octomap::OcTree* map) {
+    std::shared_ptr<OctomapValidityChecker> validity_checker(new OctomapValidityChecker(getSpaceInformation(), map));
+
+    setStateValidityChecker(base::StateValidityCheckerPtr(validity_checker));
+  }
+};
+
+}  // namespace ompl
+
+#endif