From 5d32c1df7d07e471b0d9760662a03ab029a8738e Mon Sep 17 00:00:00 2001 From: brunopinto900 Date: Tue, 8 Mar 2022 13:27:00 +0000 Subject: [PATCH 01/10] Completed pull request --- .../include/global_planner/octomap_ompl_rrt.h | 45 +++++++ .../global_planner/octomap_rrt_planner.h | 126 ++++++++++++++++++ .../launch/ompl_rrt_planner_octomap.launch | 40 ++++++ .../launch/ompl_rrt_planner_stereo.launch | 31 +++++ .../src/library/octomap_ompl_rrt.cpp | 89 +++++++++++++ 5 files changed, 331 insertions(+) create mode 100644 global_planner/include/global_planner/octomap_ompl_rrt.h create mode 100644 global_planner/include/global_planner/octomap_rrt_planner.h create mode 100644 global_planner/launch/ompl_rrt_planner_octomap.launch create mode 100644 global_planner/launch/ompl_rrt_planner_stereo.launch create mode 100644 global_planner/src/library/octomap_ompl_rrt.cpp 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..1c41d0145 --- /dev/null +++ b/global_planner/include/global_planner/octomap_rrt_planner.h @@ -0,0 +1,126 @@ +// 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 + +#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 pointcloud_pub_; + ros::Publisher mavros_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_; + + 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 current_goal_, global_goal_; + + std::vector current_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(); +}; + +#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 From 2decfe28724fb66ff31beadcab136a00592069e5 Mon Sep 17 00:00:00 2001 From: brunopinto900 Date: Tue, 8 Mar 2022 13:28:07 +0000 Subject: [PATCH 02/10] Modifed collision check to account for quadrotor size --- .../include/global_planner/ompl_octomap.h | 71 +++++++++++++++++++ 1 file changed, 71 insertions(+) create mode 100644 global_planner/include/global_planner/ompl_octomap.h 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..d7830c8f6 --- /dev/null +++ b/global_planner/include/global_planner/ompl_octomap.h @@ -0,0 +1,71 @@ +#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; + + 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 = true; + + node = map_->search(state(0)+0.5, state(1)+0.5, state(2)+0.5, 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; + + node = map_->search(state(0)-0.5, state(1)-0.5, state(2)-0.5, 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; + + return collision; + } + } + + protected: + octomap::OcTree* map_; +}; +} // namespace ompl + +#endif \ No newline at end of file From 777b6a9404c80cc911813d31b4ec756137b402f7 Mon Sep 17 00:00:00 2001 From: brunopinto900 Date: Tue, 8 Mar 2022 13:28:46 +0000 Subject: [PATCH 03/10] Finished implementation of pull request --- .../include/global_planner/ompl_setup.h | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 global_planner/include/global_planner/ompl_setup.h 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 From 6ca807c3c0f9583a625c8a6427a89a42eee98d31 Mon Sep 17 00:00:00 2001 From: brunopinto900 Date: Tue, 8 Mar 2022 13:29:51 +0000 Subject: [PATCH 04/10] Added state machine. Only plans when the goal point is provided --- .../src/library/octomap_rrt_planner.cpp | 303 ++++++++++++++++++ .../src/nodes/octomap_rrt_planner_node.cpp | 14 + 2 files changed, 317 insertions(+) create mode 100644 global_planner/src/library/octomap_rrt_planner.cpp create mode 100644 global_planner/src/nodes/octomap_rrt_planner_node.cpp 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..4dcfe2d17 --- /dev/null +++ b/global_planner/src/library/octomap_rrt_planner.cpp @@ -0,0 +1,303 @@ +// 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), + cmdloop_dt_(0.05), + plan_(false), + plannerloop_dt_(0.05), + 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); + mavros_waypoint_publisher_ = nh_.advertise("/mavros/setpoint_position/local", 10); + global_path_pub_ = nh_.advertise("/global_temp_path", 10); + pointcloud_pub_ = nh_.advertise("/cloud_in", 10); + + 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); + + 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); + + 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 (num_octomap_msg_++ % 10 > 0) { + // return; // We get too many of those messages. Only process 1/10 of them + //} + + 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; +} + +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) = 3.0; + global_goal_.pose.position.x = goal_(0); + global_goal_.pose.position.y = goal_(1); + global_goal_.pose.position.z = goal_(2); + + } +} + +void OctomapRrtPlanner::moveBaseSimpleCallback(const geometry_msgs::PoseStamped& msg) { + plan_ = true; + goal_(0) = msg.pose.position.x; + goal_(1) = msg.pose.position.y; + goal_(2) = 3.0; + global_goal_.pose.position.x = goal_(0); + global_goal_.pose.position.y = goal_(1); + global_goal_.pose.position.z = goal_(2); + +} + +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() < 1.0 ? vec.length() : 1.0; // 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(); + + mavros_waypoint_publisher_.publish(setpoint); + + // Publish setpoint for vizualization + //current_waypoint_publisher_.publish(setpoint); + + + mavros_msgs::Trajectory msg; + Eigen::Vector3f reference_posf = reference_pos_.cast(); + //avoidance::transformToTrajectory(msg, avoidance::toPoseStamped(reference_posf, reference_att_)); + 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 = "local_origin"; + + std::vector path; + + + for (int i = 0; i < current_path_.size(); i++) { + path.insert(path.begin(), vector3d2PoseStampedMsg(current_path_[i])); + } + msg.poses = path; + global_path_pub_.publish(msg); + + if(!path.empty()) + current_goal_ = path[0]; + +} + + +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 = "local_origin"; + 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; } + +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), 0.0; + in_upper << goal_(0), goal_(1), 20.0; + + 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_); + if (current_path_.empty()) { + current_path_.push_back(reference_pos_); + std::cout << "Path is empty" << std::endl; + } + } + } + else + { + plan_ = false; + std::cout << "ALREADY AT GOAL\n"; + } + +} + +bool OctomapRrtPlanner::isCloseToGoal() { return poseDistance(global_goal_, last_pos_) < 0.1; } + +double OctomapRrtPlanner::poseDistance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2) { + return norm( (p2.pose.position.x - p1.pose.position.x,p2.pose.position.y - p1.pose.position.y,p2.pose.position.z - p1.pose.position.z ) ); +} + +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 From 8ff336a46874d8a4a0379bf7c107a943aaaad82a Mon Sep 17 00:00:00 2001 From: brunopinto900 Date: Thu, 17 Mar 2022 16:30:08 +0000 Subject: [PATCH 05/10] Overloaded function transformToTrajectory. Removed the velocity argument --- avoidance/include/avoidance/common.h | 2 ++ avoidance/src/common.cpp | 8 ++++++++ 2 files changed, 10 insertions(+) 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(); From b2338d23b9e3871007cae87d271370e176d45f73 Mon Sep 17 00:00:00 2001 From: brunopinto900 Date: Thu, 17 Mar 2022 16:30:40 +0000 Subject: [PATCH 06/10] Added OMPL in find package --- global_planner/CMakeLists.txt | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) 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} ) ############# From 51b44fdfd902b228ab0d06cdfe6f9d4d95505b60 Mon Sep 17 00:00:00 2001 From: brunopinto900 Date: Thu, 17 Mar 2022 16:31:26 +0000 Subject: [PATCH 07/10] Crated YAML file for RRT planner parameters --- global_planner/cfg/RRTPlannerNode.yaml | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 global_planner/cfg/RRTPlannerNode.yaml 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 From aebf619a69509a359659d850b1420afe71ebe7e4 Mon Sep 17 00:00:00 2001 From: brunopinto900 Date: Thu, 17 Mar 2022 16:32:48 +0000 Subject: [PATCH 08/10] RRT planner parameters are now loaded from YAML file instead of being hard-coded --- .../global_planner/octomap_rrt_planner.h | 9 ++ .../include/global_planner/ompl_octomap.h | 22 +--- .../src/library/octomap_rrt_planner.cpp | 119 ++++++++++++------ 3 files changed, 89 insertions(+), 61 deletions(-) diff --git a/global_planner/include/global_planner/octomap_rrt_planner.h b/global_planner/include/global_planner/octomap_rrt_planner.h index 1c41d0145..df35ea10b 100644 --- a/global_planner/include/global_planner/octomap_rrt_planner.h +++ b/global_planner/include/global_planner/octomap_rrt_planner.h @@ -56,8 +56,10 @@ class OctomapRrtPlanner { 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_; @@ -81,6 +83,12 @@ class OctomapRrtPlanner { 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_; + Eigen::Vector3d local_position_, local_velocity_; Eigen::Vector3d reference_pos_; geometry_msgs::PoseStamped last_pos_; @@ -89,6 +97,7 @@ class OctomapRrtPlanner { geometry_msgs::PoseStamped current_goal_, global_goal_; std::vector current_path_; + nav_msgs::Path actual_path_; std::vector cameras_; tf::TransformListener listener_; diff --git a/global_planner/include/global_planner/ompl_octomap.h b/global_planner/include/global_planner/ompl_octomap.h index d7830c8f6..218919d3f 100644 --- a/global_planner/include/global_planner/ompl_octomap.h +++ b/global_planner/include/global_planner/ompl_octomap.h @@ -26,7 +26,7 @@ class OctomapValidityChecker : public base::StateValidityChecker { } virtual bool checkCollision(Eigen::Vector3d state) const { - bool collision = false; + bool collision = true; double occprob = 1.0; uint octree_depth = 16; double logodds; @@ -39,25 +39,7 @@ class OctomapValidityChecker : public base::StateValidityChecker { 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; - - node = map_->search(state(0)+0.5, state(1)+0.5, state(2)+0.5, 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; - - node = map_->search(state(0)-0.5, state(1)-0.5, state(2)-0.5, 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; + if (occprob <= 0.5) collision = false; return collision; } diff --git a/global_planner/src/library/octomap_rrt_planner.cpp b/global_planner/src/library/octomap_rrt_planner.cpp index 4dcfe2d17..139c4eb9a 100644 --- a/global_planner/src/library/octomap_rrt_planner.cpp +++ b/global_planner/src/library/octomap_rrt_planner.cpp @@ -3,6 +3,16 @@ #include "global_planner/octomap_rrt_planner.h" #include "global_planner/octomap_ompl_rrt.h" +// Commit: ctrl and plan times in configs + +// Commit: initial pose config & reference pos, initial goal +// Clean code, minimize conversions between eigen and ros + +// Commit: state machine, pop goals, controller, publish path + +// Commit: solve repeatibility, checks if the planner fails, replan + +// Commit: plan yaw using namespace Eigen; using namespace std; @@ -14,9 +24,7 @@ OctomapRrtPlanner::OctomapRrtPlanner(const ros::NodeHandle& nh, const ros::NodeH : nh_(nh), nh_private_(nh_private), avoidance_node_(nh, nh_private), - cmdloop_dt_(0.05), plan_(false), - plannerloop_dt_(0.05), rrt_planner_(nh, nh_private) { #ifndef DISABLE_SIMULATION @@ -27,21 +35,35 @@ OctomapRrtPlanner::OctomapRrtPlanner(const ros::NodeHandle& nh, const ros::NodeH 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); - + 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", 3.0); + max_altitude_ = nh_.param("max_altitude", 7.5); + max_x_ = nh_.param("max_x", 5.0); + max_y_ = nh_.param("max_y", 5.0); + min_x_ = nh_.param("min_x", 5.0); + min_y_ = nh_.param("min_y", 5.0); + goal_radius_ = nh_.param("goal_radius", 0.5); + speed_ = nh_.param("default_speed",2.0); + goal_altitude_ = nh_.param("goal_altitude", 6.0); + 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"); + nh_.param("frame_id", frame_id_, "local_origin"); ros::TimerOptions cmdlooptimer_options(ros::Duration(cmdloop_dt_), boost::bind(&OctomapRrtPlanner::cmdLoopCallback, this, _1), &cmdloop_queue_); @@ -60,6 +82,7 @@ OctomapRrtPlanner::OctomapRrtPlanner(const ros::NodeHandle& nh, const ros::NodeH initializeCameraSubscribers(camera_topics); + actual_path_.header.frame_id = frame_id_; rrt_planner_.setMap(octree_); goal_ << 0.0, 1.0, 0.0; @@ -100,17 +123,13 @@ void OctomapRrtPlanner::plannerLoopCallback(const ros::TimerEvent& event) { planWithSimpleSetup(); } - - publishPath(); // For visualization - + publishPath(); // For visualization } // Check if the current path is blocked void OctomapRrtPlanner::octomapFullCallback(const octomap_msgs::Octomap& msg) { + std::lock_guard lock(mutex_); - //if (num_octomap_msg_++ % 10 > 0) { - // return; // We get too many of those messages. Only process 1/10 of them - //} if (octree_) { delete octree_; @@ -143,6 +162,11 @@ void OctomapRrtPlanner::positionCallback(const geometry_msgs::PoseStamped& 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) { @@ -153,25 +177,29 @@ void OctomapRrtPlanner::velocityCallback(const geometry_msgs::TwistStamped& msg) void OctomapRrtPlanner::DesiredTrajectoryCallback(const mavros_msgs::Trajectory& msg) { // Read waypoint from trajectory messages + ROS_INFO("GOAL RECEIVED DESIRED TRAJ"); + if (msg.point_valid[0]) { goal_(0) = msg.point_1.position.x; goal_(1) = msg.point_1.position.y; - goal_(2) = 3.0; - global_goal_.pose.position.x = goal_(0); - global_goal_.pose.position.y = goal_(1); - global_goal_.pose.position.z = goal_(2); + 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) { + ROS_INFO("GOAL RECEIVED MOVE BASE"); plan_ = true; goal_(0) = msg.pose.position.x; goal_(1) = msg.pose.position.y; - goal_(2) = 3.0; - global_goal_.pose.position.x = goal_(0); - global_goal_.pose.position.y = goal_(1); - global_goal_.pose.position.z = goal_(2); + 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_; } @@ -180,8 +208,9 @@ 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() < 1.0 ? vec.length() : 1.0; // speed_ + double new_len = vec.length() < goal_radius_ ? vec.length() : speed_; vec.normalize(); vec *= new_len; @@ -193,35 +222,33 @@ void OctomapRrtPlanner::publishSetpoint() { mavros_waypoint_publisher_.publish(setpoint); // Publish setpoint for vizualization - //current_waypoint_publisher_.publish(setpoint); + current_waypoint_publisher_.publish(setpoint); mavros_msgs::Trajectory msg; - Eigen::Vector3f reference_posf = reference_pos_.cast(); - //avoidance::transformToTrajectory(msg, avoidance::toPoseStamped(reference_posf, reference_att_)); avoidance::transformToTrajectory(msg, setpoint); msg.header.frame_id = frame_id_; msg.header.stamp = ros::Time::now(); - trajectory_pub_.publish(msg); + //trajectory_pub_.publish(msg); } void OctomapRrtPlanner::publishPath() { nav_msgs::Path msg; msg.header.stamp = ros::Time::now(); - msg.header.frame_id = "local_origin"; + 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])); } - msg.poses = path; - global_path_pub_.publish(msg); - - if(!path.empty()) - current_goal_ = path[0]; - + + if(current_path_.size() > 2) { + msg.poses = path; + global_path_pub_.publish(msg); + current_goal_ = path[2]; + } + } @@ -236,7 +263,7 @@ void OctomapRrtPlanner::initializeCameraSubscribers(std::vector& ca 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 = "local_origin"; + 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; @@ -256,8 +283,8 @@ void OctomapRrtPlanner::planWithSimpleSetup() { { if(octree_) { Eigen::Vector3d in_lower, in_upper, out_lower,out_upper; - in_lower << local_position_(0), local_position_(1), 0.0; - in_upper << goal_(0), goal_(1), 20.0; + in_lower << local_position_(0) - min_x_, local_position_(1) - min_y_, min_altitude_; + in_upper << goal_(0) + max_x_, goal_(1) + max_y_, max_altitude_; checkBounds(in_lower,in_upper,out_lower,out_upper); @@ -267,22 +294,32 @@ void OctomapRrtPlanner::planWithSimpleSetup() { rrt_planner_.getPath(local_position_, goal_, ¤t_path_); if (current_path_.empty()) { current_path_.push_back(reference_pos_); - std::cout << "Path is empty" << std::endl; + ROS_INFO("Path is empty"); } + } } else { plan_ = false; - std::cout << "ALREADY AT GOAL\n"; + ROS_INFO("ALREADY AT GOAL"); } - + } -bool OctomapRrtPlanner::isCloseToGoal() { return poseDistance(global_goal_, last_pos_) < 0.1; } +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) { - return norm( (p2.pose.position.x - p1.pose.position.x,p2.pose.position.y - p1.pose.position.y,p2.pose.position.z - p1.pose.position.z ) ); + return norm( (p2.pose.position.x - p1.pose.position.x, p2.pose.position.y - p1.pose.position.y, p2.pose.position.z - p1.pose.position.z ) ); } void OctomapRrtPlanner::checkBounds(const Eigen::Vector3d& in_lower, const Eigen::Vector3d& in_upper, Eigen::Vector3d& out_lower, Eigen::Vector3d& out_upper ) { From e59d9ad2a9d5be5980fe73f3f35cb3048c82b106 Mon Sep 17 00:00:00 2001 From: brunopinto900 Date: Fri, 18 Mar 2022 14:23:12 +0000 Subject: [PATCH 09/10] "Yaw set to the direction of the path" --- .../global_planner/octomap_rrt_planner.h | 6 +- .../src/library/octomap_rrt_planner.cpp | 59 +++++++++++-------- 2 files changed, 38 insertions(+), 27 deletions(-) diff --git a/global_planner/include/global_planner/octomap_rrt_planner.h b/global_planner/include/global_planner/octomap_rrt_planner.h index df35ea10b..a730979c0 100644 --- a/global_planner/include/global_planner/octomap_rrt_planner.h +++ b/global_planner/include/global_planner/octomap_rrt_planner.h @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -88,13 +89,14 @@ class OctomapRrtPlanner { 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 current_goal_, global_goal_; + geometry_msgs::PoseStamped previous_goal_,current_goal_, global_goal_; std::vector current_path_; nav_msgs::Path actual_path_; @@ -125,11 +127,11 @@ class OctomapRrtPlanner { 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/src/library/octomap_rrt_planner.cpp b/global_planner/src/library/octomap_rrt_planner.cpp index 139c4eb9a..cb8529927 100644 --- a/global_planner/src/library/octomap_rrt_planner.cpp +++ b/global_planner/src/library/octomap_rrt_planner.cpp @@ -3,17 +3,6 @@ #include "global_planner/octomap_rrt_planner.h" #include "global_planner/octomap_ompl_rrt.h" -// Commit: ctrl and plan times in configs - -// Commit: initial pose config & reference pos, initial goal -// Clean code, minimize conversions between eigen and ros - -// Commit: state machine, pop goals, controller, publish path - -// Commit: solve repeatibility, checks if the planner fails, replan - -// Commit: plan yaw - using namespace Eigen; using namespace std; namespace ob = ompl::base; @@ -46,14 +35,10 @@ OctomapRrtPlanner::OctomapRrtPlanner(const ros::NodeHandle& nh, const ros::NodeH // RRT planner parameters min_altitude_ = nh_.param("min_altitude", 3.0); - max_altitude_ = nh_.param("max_altitude", 7.5); - max_x_ = nh_.param("max_x", 5.0); - max_y_ = nh_.param("max_y", 5.0); - min_x_ = nh_.param("min_x", 5.0); - min_y_ = nh_.param("min_y", 5.0); - goal_radius_ = nh_.param("goal_radius", 0.5); + max_altitude_ = nh_.param("max_altitude", 4.0); + goal_radius_ = nh_.param("goal_radius", 1.0); speed_ = nh_.param("default_speed",2.0); - goal_altitude_ = nh_.param("goal_altitude", 6.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); @@ -90,6 +75,8 @@ OctomapRrtPlanner::OctomapRrtPlanner(const ros::NodeHandle& nh, const ros::NodeH 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; @@ -177,8 +164,6 @@ void OctomapRrtPlanner::velocityCallback(const geometry_msgs::TwistStamped& msg) void OctomapRrtPlanner::DesiredTrajectoryCallback(const mavros_msgs::Trajectory& msg) { // Read waypoint from trajectory messages - ROS_INFO("GOAL RECEIVED DESIRED TRAJ"); - if (msg.point_valid[0]) { goal_(0) = msg.point_1.position.x; goal_(1) = msg.point_1.position.y; @@ -191,7 +176,7 @@ void OctomapRrtPlanner::DesiredTrajectoryCallback(const mavros_msgs::Trajectory& } void OctomapRrtPlanner::moveBaseSimpleCallback(const geometry_msgs::PoseStamped& msg) { - ROS_INFO("GOAL RECEIVED MOVE BASE"); + plan_ = true; goal_(0) = msg.pose.position.x; goal_(1) = msg.pose.position.y; @@ -218,6 +203,9 @@ void OctomapRrtPlanner::publishSetpoint() { 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); @@ -277,14 +265,24 @@ geometry_msgs::PoseStamped OctomapRrtPlanner::vector3d2PoseStampedMsg(Eigen::Vec 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) - min_x_, local_position_(1) - min_y_, min_altitude_; - in_upper << goal_(0) + max_x_, goal_(1) + max_y_, max_altitude_; + in_lower << local_position_(0), local_position_(1), min_altitude_; + in_upper << goal_(0), goal_(1), max_altitude_; checkBounds(in_lower,in_upper,out_lower,out_upper); @@ -292,6 +290,8 @@ void OctomapRrtPlanner::planWithSimpleSetup() { 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"); @@ -302,7 +302,8 @@ void OctomapRrtPlanner::planWithSimpleSetup() { else { plan_ = false; - ROS_INFO("ALREADY AT GOAL"); + yaw_ = 0.0; + ROS_INFO("Already at goal"); } } @@ -319,7 +320,15 @@ bool OctomapRrtPlanner::isCloseToGoal() { } double OctomapRrtPlanner::poseDistance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2) { - return norm( (p2.pose.position.x - p1.pose.position.x, p2.pose.position.y - p1.pose.position.y, p2.pose.position.z - p1.pose.position.z ) ); + + 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 ) { From 20180883fd4d71558ba97f4d160da3eb80d12e14 Mon Sep 17 00:00:00 2001 From: brunopinto900 Date: Sat, 19 Mar 2022 16:13:18 +0000 Subject: [PATCH 10/10] Added inflation to octomap to avoid quadrotor hit on obstacles --- .../include/global_planner/ompl_octomap.h | 54 +++++++++++++++---- .../src/library/octomap_rrt_planner.cpp | 10 ++-- 2 files changed, 49 insertions(+), 15 deletions(-) diff --git a/global_planner/include/global_planner/ompl_octomap.h b/global_planner/include/global_planner/ompl_octomap.h index 218919d3f..e213ac05b 100644 --- a/global_planner/include/global_planner/ompl_octomap.h +++ b/global_planner/include/global_planner/ompl_octomap.h @@ -26,23 +26,57 @@ class OctomapValidityChecker : public base::StateValidityChecker { } virtual bool checkCollision(Eigen::Vector3d state) const { - bool collision = true; + 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_) { - 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 + 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; + } - // Assuming a optimistic planner: Unknown space is considered as unoccupied - if (occprob <= 0.5) collision = false; + 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 - return collision; + // Assuming a optimistic planner: Unknown space is considered as unoccupied + if (occprob > 0.5) + { + collision = true; + break; + } + + + } + } + + return collision; + } protected: diff --git a/global_planner/src/library/octomap_rrt_planner.cpp b/global_planner/src/library/octomap_rrt_planner.cpp index cb8529927..88bbdcf1c 100644 --- a/global_planner/src/library/octomap_rrt_planner.cpp +++ b/global_planner/src/library/octomap_rrt_planner.cpp @@ -34,8 +34,8 @@ OctomapRrtPlanner::OctomapRrtPlanner(const ros::NodeHandle& nh, const ros::NodeH pointcloud_pub_ = nh_.advertise("/cloud_in", 10); // RRT planner parameters - min_altitude_ = nh_.param("min_altitude", 3.0); - max_altitude_ = nh_.param("max_altitude", 4.0); + 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); @@ -217,7 +217,7 @@ void OctomapRrtPlanner::publishSetpoint() { avoidance::transformToTrajectory(msg, setpoint); msg.header.frame_id = frame_id_; msg.header.stamp = ros::Time::now(); - //trajectory_pub_.publish(msg); + trajectory_pub_.publish(msg); } void OctomapRrtPlanner::publishPath() { @@ -234,7 +234,7 @@ void OctomapRrtPlanner::publishPath() { if(current_path_.size() > 2) { msg.poses = path; global_path_pub_.publish(msg); - current_goal_ = path[2]; + current_goal_ = path[1]; } } @@ -282,7 +282,7 @@ void OctomapRrtPlanner::planWithSimpleSetup() { 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), max_altitude_; + in_upper << goal_(0), goal_(1), goal_(2) + max_altitude_; checkBounds(in_lower,in_upper,out_lower,out_upper);