diff --git a/avoidance/include/avoidance/common.h b/avoidance/include/avoidance/common.h index 5ab3eda2f..1d81df4c4 100644 --- a/avoidance/include/avoidance/common.h +++ b/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 diff --git a/avoidance/src/common.cpp b/avoidance/src/common.cpp index 9a8e21a7b..256316a70 100644 --- a/avoidance/src/common.cpp +++ b/avoidance/src/common.cpp @@ -278,6 +278,14 @@ double getAngularVelocity(float desired_yaw, float curr_yaw) { return 0.5 * static_cast(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(); diff --git a/global_planner/CMakeLists.txt b/global_planner/CMakeLists.txt index afe8e280f..a17a309be 100644 --- a/global_planner/CMakeLists.txt +++ b/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} ) ############# diff --git a/global_planner/cfg/RRTPlannerNode.yaml b/global_planner/cfg/RRTPlannerNode.yaml new file mode 100644 index 000000000..7736d6c03 --- /dev/null +++ b/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 diff --git a/global_planner/include/global_planner/octomap_ompl_rrt.h b/global_planner/include/global_planner/octomap_ompl_rrt.h new file mode 100644 index 000000000..65628491c --- /dev/null +++ b/global_planner/include/global_planner/octomap_ompl_rrt.h @@ -0,0 +1,45 @@ +// July/2018, ETHZ, Jaeyoung Lim, jalim@student.ethz.ch + +#ifndef OCTOMAP_OMPL_RRT_H +#define OCTOMAP_OMPL_RRT_H + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include + +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* path); + void solutionPathToTrajectoryPoints(ompl::geometric::PathGeometric& path, + std::vector* trajectory_points) const; +}; + +#endif \ No newline at end of file diff --git a/global_planner/include/global_planner/octomap_rrt_planner.h b/global_planner/include/global_planner/octomap_rrt_planner.h new file mode 100644 index 000000000..a730979c0 --- /dev/null +++ b/global_planner/include/global_planner/octomap_rrt_planner.h @@ -0,0 +1,137 @@ +// June/2019, ETHZ, Jaeyoung Lim, jalim@student.ethz.ch + +#ifndef OCTOMAP_RRT_PLANNER_H +#define OCTOMAP_RRT_PLANNER_H + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +//#include +#include +#include +#include + +#ifndef DISABLE_SIMULATION +#include +#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 cmdloop_spinner_; + std::unique_ptr 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 current_path_; + nav_msgs::Path actual_path_; + std::vector cameras_; + tf::TransformListener listener_; + + octomap::OcTree* octree_ = NULL; + + OctomapOmplRrt rrt_planner_; + avoidance::AvoidanceNode avoidance_node_; +#ifndef DISABLE_SIMULATION + std::unique_ptr 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& 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 \ No newline at end of file diff --git a/global_planner/include/global_planner/ompl_octomap.h b/global_planner/include/global_planner/ompl_octomap.h new file mode 100644 index 000000000..e213ac05b --- /dev/null +++ b/global_planner/include/global_planner/ompl_octomap.h @@ -0,0 +1,87 @@ +#ifndef OCTOMAP_OMPL_H +#define OCTOMAP_OMPL_H + +#include +#include + +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()->values[0], + state->as()->values[1], + state->as()->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 = false; + double occprob = 1.0; + uint octree_depth = 16; + double logodds; + octomap::OcTreeNode* node; + double resolution = 0.1; + double radius = 0.6; + int search_iters = radius / resolution; + double increment = 0; + + if (map_) + { + + for(int i = 0; i <= search_iters; i++) + { + + increment = i*resolution; + + node = map_->search(state(0) + increment, state(1) + increment, state(2) + increment, 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 = true; + break; + } + + node = map_->search(state(0) - increment, state(1) - increment, state(2) - increment, 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 = true; + break; + } + + + } + + } + + return collision; + + } + + protected: + octomap::OcTree* map_; +}; +} // namespace ompl + +#endif \ No newline at end of file diff --git a/global_planner/include/global_planner/ompl_setup.h b/global_planner/include/global_planner/ompl_setup.h new file mode 100644 index 000000000..1249780d7 --- /dev/null +++ b/global_planner/include/global_planner/ompl_setup.h @@ -0,0 +1,44 @@ +#ifndef OMPL_SETUP_H +#define OMPL_SETUP_H + +#include + +#include +#include +#include +#include "ompl/base/SpaceInformation.h" + +#include + +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 validity_checker(new OctomapValidityChecker(getSpaceInformation(), map)); + + setStateValidityChecker(base::StateValidityCheckerPtr(validity_checker)); + } +}; + +} // namespace ompl + +#endif \ No newline at end of file diff --git a/global_planner/launch/ompl_rrt_planner_octomap.launch b/global_planner/launch/ompl_rrt_planner_octomap.launch new file mode 100644 index 000000000..5289c3ac6 --- /dev/null +++ b/global_planner/launch/ompl_rrt_planner_octomap.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + $(arg pointcloud_topics) + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/global_planner/launch/ompl_rrt_planner_stereo.launch b/global_planner/launch/ompl_rrt_planner_stereo.launch new file mode 100644 index 000000000..5a21f47b9 --- /dev/null +++ b/global_planner/launch/ompl_rrt_planner_stereo.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/global_planner/src/library/octomap_ompl_rrt.cpp b/global_planner/src/library/octomap_ompl_rrt.cpp new file mode 100644 index 000000000..4d7cfac29 --- /dev/null +++ b/global_planner/src/library/octomap_ompl_rrt.cpp @@ -0,0 +1,89 @@ +// June/2019, ETHZ, Jaeyoung Lim, jalim@student.ethz.ch + +#include "global_planner/octomap_ompl_rrt.h" + +using namespace Eigen; +using namespace std; +// Constructor +OctomapOmplRrt::OctomapOmplRrt(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private) + : nh_(nh), nh_private_(nh_private) {} +OctomapOmplRrt::~OctomapOmplRrt() { + // Destructor +} + +void OctomapOmplRrt::setupProblem() { + problem_setup_.setDefaultPlanner(); + problem_setup_.setDefaultObjective(); + problem_setup_.setOctomapCollisionChecking(map_); + ompl::base::RealVectorBounds bounds(3); + bounds.setLow(0, lower_bound_.x()); + bounds.setLow(1, lower_bound_.y()); + bounds.setLow(2, lower_bound_.z()); + + bounds.setHigh(0, upper_bound_.x()); + bounds.setHigh(1, upper_bound_.y()); + bounds.setHigh(2, upper_bound_.z()); + + // Define start and goal positions. + problem_setup_.getGeometricComponentStateSpace()->as()->setBounds(bounds); + + problem_setup_.setStateValidityCheckingResolution(0.001); +} + +void OctomapOmplRrt::setBounds(Eigen::Vector3d& lower_bound, Eigen::Vector3d& upper_bound) { + lower_bound_ = lower_bound; + upper_bound_ = upper_bound; +} + +bool OctomapOmplRrt::getPath(const Eigen::Vector3d& start, const Eigen::Vector3d& goal, + std::vector* path) { + problem_setup_.clear(); + path->clear(); + + ompl::base::ScopedState start_ompl(problem_setup_.getSpaceInformation()); + ompl::base::ScopedState goal_ompl(problem_setup_.getSpaceInformation()); + + start_ompl->values[0] = start(0); + start_ompl->values[1] = start(1); + start_ompl->values[2] = start(2); + + goal_ompl->values[0] = goal(0); + goal_ompl->values[1] = goal(1); + goal_ompl->values[2] = goal(2); + problem_setup_.setStartAndGoalStates(start_ompl, goal_ompl); + problem_setup_.setup(); + + if (problem_setup_.solve(1.0)) { + std::cout << "Found solution:" << std::endl; + // problem_setup_.getSolutionPath().print(std::cout); + // problem_setup_.simplifySolution(); + problem_setup_.getSolutionPath().print(std::cout); + + } else { + std::cout << "Solution Not found" << std::endl; + } + + if (problem_setup_.haveSolutionPath()) { + solutionPathToTrajectoryPoints(problem_setup_.getSolutionPath(), path); + return true; + } + return false; +} + +void OctomapOmplRrt::setMap(octomap::OcTree* map) { map_ = map; } + +void OctomapOmplRrt::solutionPathToTrajectoryPoints(ompl::geometric::PathGeometric& path, + std::vector* trajectory_points) const { + // trajectory_points->clear(); + // trajectory_points->reserve(path.getStateCount()); + + std::vector& state_vector = path.getStates(); + + for (ompl::base::State* state_ptr : state_vector) { + Eigen::Vector3d position(state_ptr->as()->values[0], + state_ptr->as()->values[1], + state_ptr->as()->values[2]); + + trajectory_points->emplace_back(position); + } +} \ No newline at end of file diff --git a/global_planner/src/library/octomap_rrt_planner.cpp b/global_planner/src/library/octomap_rrt_planner.cpp new file mode 100644 index 000000000..88bbdcf1c --- /dev/null +++ b/global_planner/src/library/octomap_rrt_planner.cpp @@ -0,0 +1,349 @@ +// June/2019, ETHZ, Jaeyoung Lim, jalim@student.ethz.ch + +#include "global_planner/octomap_rrt_planner.h" +#include "global_planner/octomap_ompl_rrt.h" + +using namespace Eigen; +using namespace std; +namespace ob = ompl::base; +namespace og = ompl::geometric; + +// Constructor +OctomapRrtPlanner::OctomapRrtPlanner(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private) + : nh_(nh), + nh_private_(nh_private), + avoidance_node_(nh, nh_private), + plan_(false), + rrt_planner_(nh, nh_private) { + +#ifndef DISABLE_SIMULATION + world_visualizer_.reset(new avoidance::WorldVisualizer(nh_, ros::this_node::getName())); +#endif + + avoidance_node_.init(); + + pose_sub_ = nh_.subscribe("/mavros/local_position/pose", 1, &OctomapRrtPlanner::positionCallback, this); + move_base_simple_sub_ = nh_.subscribe("/move_base_simple/goal", 1, &OctomapRrtPlanner::moveBaseSimpleCallback, this); + desiredtrajectory_sub_ = nh_.subscribe("/mavros/trajectory/desired", 1, &OctomapRrtPlanner::DesiredTrajectoryCallback, this); + octomap_full_sub_ = nh_.subscribe("/octomap_full", 1, &OctomapRrtPlanner::octomapFullCallback, this); + trajectory_pub_ = nh_.advertise("/mavros/trajectory/generated", 10); + actual_path_pub_ = nh_.advertise("/actual_path", 10); + mavros_waypoint_publisher_ = nh_.advertise("/mavros/setpoint_position/local", 10); + current_waypoint_publisher_ = nh_.advertise("/current_setpoint", 10); + global_path_pub_ = nh_.advertise("/global_temp_path", 10); + pointcloud_pub_ = nh_.advertise("/cloud_in", 10); + + // RRT planner parameters + min_altitude_ = nh_.param("min_altitude", 1.0); + max_altitude_ = nh_.param("max_altitude", 5.0); + goal_radius_ = nh_.param("goal_radius", 1.0); + speed_ = nh_.param("default_speed",2.0); + goal_altitude_ = nh_.param("goal_altitude", 3.5); + cmdloop_dt_ = nh_.param("control_loop_dt", 0.05); + plannerloop_dt_ = nh_.param("planner_loop_dt", 0.1); + + + listener_.waitForTransform("/fcu", "/world", ros::Time(0), ros::Duration(3.0)); + listener_.waitForTransform("/local_origin", "/world", ros::Time(0), ros::Duration(3.0)); + + std::vector camera_topics; + nh_.getParam("pointcloud_topics", camera_topics); + nh_.param("frame_id", frame_id_, "local_origin"); + + ros::TimerOptions cmdlooptimer_options(ros::Duration(cmdloop_dt_), + boost::bind(&OctomapRrtPlanner::cmdLoopCallback, this, _1), &cmdloop_queue_); + cmdloop_timer_ = nh_.createTimer(cmdlooptimer_options); + + cmdloop_spinner_.reset(new ros::AsyncSpinner(1, &cmdloop_queue_)); + cmdloop_spinner_->start(); + + ros::TimerOptions plannerlooptimer_options(ros::Duration(plannerloop_dt_), + boost::bind(&OctomapRrtPlanner::plannerLoopCallback, this, _1), + &plannerloop_queue_); + plannerloop_timer_ = nh_.createTimer(plannerlooptimer_options); + + plannerloop_spinner_.reset(new ros::AsyncSpinner(1, &plannerloop_queue_)); + plannerloop_spinner_->start(); + + initializeCameraSubscribers(camera_topics); + + actual_path_.header.frame_id = frame_id_; + rrt_planner_.setMap(octree_); + + goal_ << 0.0, 1.0, 0.0; + + current_goal_.header.frame_id = frame_id_; + current_goal_.pose.position.x = 0; current_goal_.pose.position.y = 0; current_goal_.pose.position.z = 3.5; + current_goal_.pose.orientation = tf::createQuaternionMsgFromYaw(0.0); + + previous_goal_ = current_goal_; + + reference_att_.x() = 0.0; + reference_att_.y() = 0.0; + reference_att_.z() = 0.0; + reference_att_.w() = 1.0; +} +OctomapRrtPlanner::~OctomapRrtPlanner() { + // Destructor +} + +void OctomapRrtPlanner::cmdLoopCallback(const ros::TimerEvent& event) { + hover_ = false; + + // Check if all information was received + ros::Time now = ros::Time::now(); + last_wp_time_ = ros::Time::now(); + + ros::Duration since_last_cloud = now - last_wp_time_; + ros::Duration since_start = now - start_time_; + + avoidance_node_.checkFailsafe(since_last_cloud, since_start, hover_); + updateReference(now); + publishSetpoint(); +} + +void OctomapRrtPlanner::plannerLoopCallback(const ros::TimerEvent& event) { + std::lock_guard lock(mutex_); + + if(plan_) + { + planWithSimpleSetup(); + + } + publishPath(); // For visualization +} + +// Check if the current path is blocked +void OctomapRrtPlanner::octomapFullCallback(const octomap_msgs::Octomap& msg) { + + std::lock_guard lock(mutex_); + + if (octree_) { + delete octree_; + } + octree_ = dynamic_cast(octomap_msgs::msgToMap(msg)); +} + +// Go through obstacle points and store them +void OctomapRrtPlanner::depthCameraCallback(const sensor_msgs::PointCloud2& msg) { + try { + // Transform msg from camera frame to world frame + ros::Time now = ros::Time::now(); + listener_.waitForTransform("/world", "/camera_link", now, ros::Duration(5.0)); + tf::StampedTransform transform; + listener_.lookupTransform("/world", "/camera_link", now, transform); + sensor_msgs::PointCloud2 transformed_msg; + pcl_ros::transformPointCloud("/world", transform, msg, transformed_msg); + pcl::PointCloud cloud; // Easier to loop through pcl::PointCloud + pcl::fromROSMsg(transformed_msg, cloud); + + pointcloud_pub_.publish(msg); + } catch (tf::TransformException const& ex) { + ROS_DEBUG("%s", ex.what()); + ROS_WARN("Transformation not available (/world to /camera_link"); + } +} + +void OctomapRrtPlanner::positionCallback(const geometry_msgs::PoseStamped& msg) { + last_pos_ = msg; + local_position_(0) = msg.pose.position.x; + local_position_(1) = msg.pose.position.y; + local_position_(2) = msg.pose.position.z; + + last_pos_.header.frame_id = frame_id_; + actual_path_.poses.push_back(last_pos_); + actual_path_pub_.publish(actual_path_); + +} + +void OctomapRrtPlanner::velocityCallback(const geometry_msgs::TwistStamped& msg) { + local_velocity_(0) = msg.twist.linear.x; + local_velocity_(1) = msg.twist.linear.y; + local_velocity_(2) = msg.twist.linear.z; +} + +void OctomapRrtPlanner::DesiredTrajectoryCallback(const mavros_msgs::Trajectory& msg) { + // Read waypoint from trajectory messages + if (msg.point_valid[0]) { + goal_(0) = msg.point_1.position.x; + goal_(1) = msg.point_1.position.y; + goal_(2) = goal_altitude_; + global_goal_.pose.position.x = msg.point_1.position.x; + global_goal_.pose.position.y = msg.point_1.position.y; + global_goal_.pose.position.z = goal_altitude_; + + } +} + +void OctomapRrtPlanner::moveBaseSimpleCallback(const geometry_msgs::PoseStamped& msg) { + + plan_ = true; + goal_(0) = msg.pose.position.x; + goal_(1) = msg.pose.position.y; + goal_(2) = goal_altitude_; + + global_goal_.pose.position.x = msg.pose.position.x; + global_goal_.pose.position.y = msg.pose.position.y; + global_goal_.pose.position.z = goal_altitude_; + +} + +void OctomapRrtPlanner::publishSetpoint() { + + // Vector pointing from current position to the current goal + tf::Vector3 vec = global_planner::toTfVector3(global_planner::subtractPoints(current_goal_.pose.position, last_pos_.pose.position)); + + + // If we are less than 1.0 away, then we should stop at the goal + double new_len = vec.length() < goal_radius_ ? vec.length() : speed_; + vec.normalize(); + vec *= new_len; + + auto setpoint = current_goal_; // The intermediate position sent to Mavros + setpoint.pose.position.x = last_pos_.pose.position.x + vec.getX(); + setpoint.pose.position.y = last_pos_.pose.position.y + vec.getY(); + setpoint.pose.position.z = last_pos_.pose.position.z + vec.getZ(); + setpoint.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_); + + previous_goal_ = current_goal_; + + mavros_waypoint_publisher_.publish(setpoint); + + // Publish setpoint for vizualization + current_waypoint_publisher_.publish(setpoint); + + + mavros_msgs::Trajectory msg; + avoidance::transformToTrajectory(msg, setpoint); + msg.header.frame_id = frame_id_; + msg.header.stamp = ros::Time::now(); + trajectory_pub_.publish(msg); +} + +void OctomapRrtPlanner::publishPath() { + nav_msgs::Path msg; + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = frame_id_; + + std::vector path; + + for (int i = 0; i < current_path_.size(); i++) { + path.insert(path.begin(), vector3d2PoseStampedMsg(current_path_[i])); + } + + if(current_path_.size() > 2) { + msg.poses = path; + global_path_pub_.publish(msg); + current_goal_ = path[1]; + } + +} + + +void OctomapRrtPlanner::initializeCameraSubscribers(std::vector& camera_topics) { + cameras_.resize(camera_topics.size()); + + for (size_t i = 0; i < camera_topics.size(); i++) { + cameras_[i].pointcloud_sub_ = nh_.subscribe(camera_topics[i], 1, &OctomapRrtPlanner::depthCameraCallback, this); + } +} + +geometry_msgs::PoseStamped OctomapRrtPlanner::vector3d2PoseStampedMsg(Eigen::Vector3d position) { + geometry_msgs::PoseStamped encode_msg; + encode_msg.header.stamp = ros::Time::now(); + encode_msg.header.frame_id = frame_id_; + encode_msg.pose.orientation.w = 1.0; + encode_msg.pose.orientation.x = 0.0; + encode_msg.pose.orientation.y = 0.0; + encode_msg.pose.orientation.z = 0.0; + encode_msg.pose.position.x = position(0); + encode_msg.pose.position.y = position(1); + encode_msg.pose.position.z = position(2); + + return encode_msg; +} + +void OctomapRrtPlanner::updateReference(ros::Time current_time) { reference_pos_ << 0.0, 0.0, 3.0; } + +double OctomapRrtPlanner::planYaw() +{ + + double dx = current_goal_.pose.position.x - last_pos_.pose.position.x; + double dy = current_goal_.pose.position.y - last_pos_.pose.position.y; + + return atan2(dy,dx); + +} + +void OctomapRrtPlanner::planWithSimpleSetup() { + + if(!isCloseToGoal()) + { + if(octree_) { + Eigen::Vector3d in_lower, in_upper, out_lower,out_upper; + in_lower << local_position_(0), local_position_(1), min_altitude_; + in_upper << goal_(0), goal_(1), goal_(2) + max_altitude_; + + checkBounds(in_lower,in_upper,out_lower,out_upper); + + rrt_planner_.setMap(octree_); + rrt_planner_.setBounds(out_lower, out_upper); + rrt_planner_.setupProblem(); + rrt_planner_.getPath(local_position_, goal_, ¤t_path_); + yaw_ = planYaw(); + + if (current_path_.empty()) { + current_path_.push_back(reference_pos_); + ROS_INFO("Path is empty"); + } + + } + } + else + { + plan_ = false; + yaw_ = 0.0; + ROS_INFO("Already at goal"); + } + +} + +bool OctomapRrtPlanner::isCloseToGoal() { + + double dist = poseDistance(global_goal_, last_pos_); + + if( dist < goal_radius_) + return true; + else + return false; + +} + +double OctomapRrtPlanner::poseDistance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2) { + + double dx = p2.pose.position.x - p1.pose.position.x; + double dy = p2.pose.position.y - p1.pose.position.y; + double dz = p2.pose.position.z - p1.pose.position.z; + + Vector3d vec(dx,dy,dz); + + return vec.norm(); + +} + +void OctomapRrtPlanner::checkBounds(const Eigen::Vector3d& in_lower, const Eigen::Vector3d& in_upper, Eigen::Vector3d& out_lower, Eigen::Vector3d& out_upper ) { + for(int i = 0; i < 3; i++) + { + if(in_lower(i) < in_upper(i)) + { + out_lower(i) = in_lower(i); + out_upper(i) = in_upper(i); + } + else + { + out_lower(i) = in_upper(i); + out_upper(i) = in_lower(i); + } + } + +} \ No newline at end of file diff --git a/global_planner/src/nodes/octomap_rrt_planner_node.cpp b/global_planner/src/nodes/octomap_rrt_planner_node.cpp new file mode 100644 index 000000000..b5094c404 --- /dev/null +++ b/global_planner/src/nodes/octomap_rrt_planner_node.cpp @@ -0,0 +1,14 @@ +// June/2019, ETHZ, Jaeyoung Lim, jalim@student.ethz.ch + +#include "global_planner/octomap_rrt_planner.h" + +int main(int argc, char** argv) { + ros::init(argc, argv, "octomap_rrt_planner"); + ros::NodeHandle nh("~"); + ros::NodeHandle nh_private(""); + + OctomapRrtPlanner* rrt_planner = new OctomapRrtPlanner(nh, nh_private); + + ros::spin(); + return 0; +} \ No newline at end of file