From e0d5aee72e89dbd15bfea08aa778045154b0c60b Mon Sep 17 00:00:00 2001 From: Torsten Jandt Date: Fri, 5 Feb 2016 11:48:35 +0000 Subject: [PATCH] [NAV] Added global planner with orientations --- mcr_global_planner/CMakeLists.txt | 65 ++++++ mcr_global_planner/package.xml | 40 ++++ mcr_global_planner/plugins.xml | 7 + .../mcr_global_planner/global_planner_wo.h | 84 +++++++ .../mcr_global_planner/global_planner_wo.cpp | 214 ++++++++++++++++++ 5 files changed, 410 insertions(+) create mode 100644 mcr_global_planner/CMakeLists.txt create mode 100644 mcr_global_planner/package.xml create mode 100644 mcr_global_planner/plugins.xml create mode 100644 mcr_global_planner/ros/include/mcr_global_planner/global_planner_wo.h create mode 100644 mcr_global_planner/ros/src/mcr_global_planner/global_planner_wo.cpp diff --git a/mcr_global_planner/CMakeLists.txt b/mcr_global_planner/CMakeLists.txt new file mode 100644 index 0000000..07977e5 --- /dev/null +++ b/mcr_global_planner/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mcr_global_planner) + +find_package(catkin REQUIRED + COMPONENTS + costmap_2d + dynamic_reconfigure + geometry_msgs + nav_core + navfn + nav_msgs + pluginlib + roscpp + tf + global_planner +) + +catkin_package( + INCLUDE_DIRS + ros/include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + costmap_2d + dynamic_reconfigure + geometry_msgs + nav_core + navfn + nav_msgs + pluginlib + roscpp + tf + global_planner +) + +include_directories( + ros/include + ${catkin_INCLUDE_DIRS} +) + +add_library( + ${PROJECT_NAME} ros/src/${PROJECT_NAME}/global_planner_wo.cpp +) +target_link_libraries( + ${PROJECT_NAME} ${catkin_LIBRARIES} +) + +add_dependencies( + ${PROJECT_NAME} ${PROJECT_NAME}_gencfg +) + + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(FILES plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/mcr_global_planner/package.xml b/mcr_global_planner/package.xml new file mode 100644 index 0000000..1851aba --- /dev/null +++ b/mcr_global_planner/package.xml @@ -0,0 +1,40 @@ + + mcr_global_planner + 1.0.0 + + Collection of b-it-bots global planners. + + Torsten Jandt + GPLv3 + + Torsten Jandt + + + catkin + + costmap_2d + dynamic_reconfigure + geometry_msgs + nav_core + nav_msgs + navfn + pluginlib + roscpp + tf + global_planner + + costmap_2d + dynamic_reconfigure + geometry_msgs + nav_core + nav_msgs + navfn + pluginlib + roscpp + tf + global_planner + + + + + diff --git a/mcr_global_planner/plugins.xml b/mcr_global_planner/plugins.xml new file mode 100644 index 0000000..cb95813 --- /dev/null +++ b/mcr_global_planner/plugins.xml @@ -0,0 +1,7 @@ + + + + Wraps ros global planner and modifies the plan by removing poses and adding orientations. + + + diff --git a/mcr_global_planner/ros/include/mcr_global_planner/global_planner_wo.h b/mcr_global_planner/ros/include/mcr_global_planner/global_planner_wo.h new file mode 100644 index 0000000..6410da9 --- /dev/null +++ b/mcr_global_planner/ros/include/mcr_global_planner/global_planner_wo.h @@ -0,0 +1,84 @@ +#ifndef _GLOBALPLANNERWO_H +#define _GLOBALPLANNERWO_H + +/******************************************************************** + * Copyright [2015] + * Author: Torsten Jandt + * torsten.jandt@smail.inf.h-brs.de + *********************************************************************/ + +#include + +namespace mcr_global_planner +{ +class GlobalPlannerWO : public global_planner::GlobalPlanner +{ + private: + // Minimun distance between two poses. + double pose_distance_; + // Maximum amount of poses that should be driven in onmi mode only + int omni_poses_; + // Publisher used to publish the pose-array + ros::Publisher pub_plan; + bool initialized; + + /* + * Filters the plan by removing poses and adds orientations to remaining ones. + */ + void filterPlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, + std::vector< geometry_msgs::PoseStamped > &plan); + /* + * Initialization for this wrapper class. + */ + void initializeThis(std::string name, costmap_2d::Costmap2D *costmap, std::string frame_id); + + /* + * Used to publish the plan with orientations as PoseArray + */ + void publishVisualPlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, + std::vector< geometry_msgs::PoseStamped > &plan); + + /* + * Calculates the distance between two poses. + */ + double calcDistance(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2); + double calcDistance(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2); + + /* + * Calculates the rotation from pose to another + */ + double calcRotation(const geometry_msgs::Pose &from, const geometry_msgs::Pose &to); + double calcRotation(const geometry_msgs::PoseStamped &from, const geometry_msgs::PoseStamped &to); + + /* + * Calculates the rotation using the right rotation direction with precalced values. + */ + double calcOmniRotation(const double from, const double way1, const double way2, const double weight); + + /* + * Calculates the rotation using the right rotation direction. + */ + double calcOmniRotation(const double from, const double to, const double weight); + + /* + * mod like helper method + */ + double modlike(double number, double limit); + + public: + GlobalPlannerWO(); + GlobalPlannerWO(std::string name, costmap_2d::Costmap2D *costmap, std::string frame_id); + + void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros); + void initialize(std::string name, costmap_2d::Costmap2D *costmap, std::string frame_id); + + bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, + std::vector< geometry_msgs::PoseStamped > &plan); + bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, double tolerance, + std::vector< geometry_msgs::PoseStamped > &plan); +}; +} + +#endif + + diff --git a/mcr_global_planner/ros/src/mcr_global_planner/global_planner_wo.cpp b/mcr_global_planner/ros/src/mcr_global_planner/global_planner_wo.cpp new file mode 100644 index 0000000..9486992 --- /dev/null +++ b/mcr_global_planner/ros/src/mcr_global_planner/global_planner_wo.cpp @@ -0,0 +1,214 @@ + +/********************************************************************* + * Copyright·[2015]· + * Author: Torsten Jandt + * torsten.jandt@smail.inf.h-brs.de + *********************************************************************/ + +#include +#include +#include + +// register this planner as a BaseGlobalPlanner plugin +PLUGINLIB_EXPORT_CLASS(mcr_global_planner::GlobalPlannerWO, nav_core::BaseGlobalPlanner) + +namespace mcr_global_planner +{ +GlobalPlannerWO::GlobalPlannerWO() : global_planner::GlobalPlanner::GlobalPlanner(), initialized(false) +{ +} + +GlobalPlannerWO::GlobalPlannerWO(std::string name, costmap_2d::Costmap2D *costmap, std::string frame_id) + : global_planner::GlobalPlanner::GlobalPlanner(name, costmap, frame_id), initialized(false) +{ +} + +void GlobalPlannerWO::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros) +{ + GlobalPlanner::initialize(name, costmap_ros); + initializeThis(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID()); +} + +void GlobalPlannerWO::initialize(std::string name, costmap_2d::Costmap2D *costmap, std::string frame_id) +{ + GlobalPlanner::initialize(name, costmap, frame_id); + initializeThis(name, costmap, frame_id); +} + +void GlobalPlannerWO::initializeThis(std::string name, costmap_2d::Costmap2D *costmap, std::string frame_id) +{ + if (!initialized) { + initialized = true; + + ros::NodeHandle private_nh("~/" + name); + + private_nh.param("pose_distance", pose_distance_, 0.05); + private_nh.param("omni_poses", omni_poses_, 20); + pub_plan = private_nh.advertise< geometry_msgs::PoseArray >("planWO", 1); + } +} + +bool GlobalPlannerWO::makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, + std::vector< geometry_msgs::PoseStamped > &plan) +{ + bool result = GlobalPlanner::makePlan(start, goal, plan); + if (result) { + filterPlan(start, goal, plan); + } + return result; +} + +bool GlobalPlannerWO::makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, + double tolerance, std::vector< geometry_msgs::PoseStamped > &plan) +{ + bool result = GlobalPlanner::makePlan(start, goal, tolerance, plan); + if (result) { + filterPlan(start, goal, plan); + } + return result; +} + +void GlobalPlannerWO::filterPlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, + std::vector< geometry_msgs::PoseStamped > &plan) +{ + const double startRotation = tf::getYaw(start.pose.orientation) + M_PI; + const double goalRotation = tf::getYaw(goal.pose.orientation) + M_PI; + + // set start and goal rotations inside the plan + (&plan[(int)(plan.size() - 1)].pose)->orientation = goal.pose.orientation; + if (plan.size() < 2) { + return; + } + (&plan[0].pose)->orientation = start.pose.orientation; + + double length = 0.0; + // Remove poses + for (unsigned int i = 1; i < plan.size() - 1; ++i) { + double d = calcDistance(plan[i], plan[i - 1]); + if (d < pose_distance_) { + plan.erase(plan.begin() + i); + --i; + } else { + length += d; + } + } + + if (plan.size() < omni_poses_ * 2 + 2) { + // only omny drive if short distance + double traveled = 0.0; + double way1 = modlike(goalRotation - startRotation, M_PI * 2); + double way2 = modlike(startRotation - goalRotation, M_PI * 2); + for (unsigned int i = 1; i < plan.size() - 1; ++i) { + geometry_msgs::Pose *pose = &plan[i].pose; + traveled += calcDistance(plan[i], plan[i - 1]); + double weight = traveled / length; + double rotation = calcOmniRotation(startRotation, way1, way2, weight); + pose->orientation = tf::createQuaternionMsgFromYaw(rotation - M_PI); + } + } else { + double weight, rotation; + double targetRotation, fromRotation; + double way1, way2; + // ************************ + // Start omni + targetRotation = calcRotation(plan[omni_poses_], plan[omni_poses_ + 1]); + way1 = modlike(targetRotation - startRotation, M_PI * 2); + way2 = modlike(startRotation - targetRotation, M_PI * 2); + for (unsigned int i = 1; i <= omni_poses_; ++i) { + geometry_msgs::Pose *pose = &plan[i].pose; + weight = (double) i / (double) omni_poses_; + rotation = calcOmniRotation(startRotation, way1, way2, weight); + pose->orientation = tf::createQuaternionMsgFromYaw(rotation - M_PI); + } + + // ************************ + // Mid drive + for (unsigned int i = omni_poses_ + 1; i < plan.size() - omni_poses_; ++i) { + geometry_msgs::Pose *pose = &plan[i].pose; + rotation = calcRotation(plan[i], plan[i + 1]); + pose->orientation = tf::createQuaternionMsgFromYaw(rotation - M_PI); + } + + // ************************ + // end omni + fromRotation = calcRotation(plan[plan.size() - omni_poses_ - 1], plan[plan.size() - omni_poses_]); + way1 = modlike(goalRotation - fromRotation, M_PI * 2); + way2 = modlike(fromRotation - goalRotation, M_PI * 2); + for (unsigned int i = plan.size() - omni_poses_ - 1; i < plan.size() - 1; ++i) { + geometry_msgs::Pose *pose = &plan[i].pose; + weight = (double)(i - (plan.size() - omni_poses_ - 1)) / (double) omni_poses_; + rotation = calcOmniRotation(fromRotation, way1, way2, weight); + pose->orientation = tf::createQuaternionMsgFromYaw(rotation - M_PI); + } + + } + + publishVisualPlan(start, goal, plan); +} + +double GlobalPlannerWO::calcOmniRotation(const double from, const double way1, const double way2, const double weight) +{ + if (way1 < way2) { + return modlike(from + way1 * weight, M_PI * 2); + } else { + return modlike(from - way2 * weight, M_PI * 2); + } +} +double GlobalPlannerWO::calcOmniRotation(const double from, const double to, const double weight) +{ + double way1 = modlike(to - from, M_PI * 2); + double way2 = modlike(from - to, M_PI * 2); + return calcOmniRotation(from, way1, way2, weight); +} + +double GlobalPlannerWO::calcRotation(const geometry_msgs::PoseStamped &from, const geometry_msgs::PoseStamped &to) +{ + return calcRotation(from.pose, to.pose); +} + +double GlobalPlannerWO::calcRotation(const geometry_msgs::Pose &from, const geometry_msgs::Pose &to) +{ + return atan2(to.position.y - from.position.y, to.position.x - from.position.x) + M_PI; +} + +double GlobalPlannerWO::calcDistance(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2) +{ + return calcDistance(pose1.pose, pose2.pose); +} + +double GlobalPlannerWO::calcDistance(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2) +{ + double dx = pose2.position.x - pose1.position.x; + double dy = pose2.position.y - pose1.position.y; + return sqrt(dx * dx + dy * dy); +} + +double GlobalPlannerWO::modlike(double number, double limit) +{ + while (number > limit) { + number -= limit; + } + while (number < 0.0) { + number += limit; + } + return number; +} + +void GlobalPlannerWO::publishVisualPlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, + std::vector< geometry_msgs::PoseStamped > &plan) +{ + geometry_msgs::PoseArray array; + array.poses.clear(); + + if (!plan.empty()) { + array.header.frame_id = plan[0].header.frame_id; + array.header.stamp = plan[0].header.stamp; + } + for (unsigned int i = 0; i < plan.size(); i++) { + array.poses.push_back(plan[i].pose); + } + + pub_plan.publish(array); +} +} +