From 8bf65cfba0d571474fe7a354985c8ae5d27ccbb6 Mon Sep 17 00:00:00 2001 From: Mudit Jindal Date: Sat, 21 Jan 2017 17:40:31 +0530 Subject: [PATCH 1/9] Updated teb_local_planner.cpp Added a peakViaPoint( ) so that the local planner strictly follows the peak points of the global plan. --- src/teb_local_planner_ros.cpp | 38 +++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp index 9265cb33..bd492308 100644 --- a/src/teb_local_planner_ros.cpp +++ b/src/teb_local_planner_ros.cpp @@ -206,6 +206,41 @@ bool TebLocalPlannerROS::setPlan(const std::vector& return true; } +void TebLocalPlannerROS::peakViaPoint(double robot_x, double robot_y, double localGoal_x, double localGoal_y, + std::vector& transformed_plan) //@mudit +{ + double planLineSlope= (localGoal_y-robot_y)/(localGoal_x-robot_x); // slope of the line connecting robot pose and local goal + double planLine_yIntercept= robot_y-(planLineSlope*robot_x); // y-intercept of the line connecting robot pose and local goal + double max_dist=0; + int viaIdx; + for (std::size_t i=1; i < transformed_plan.size(); ++i) + {double x_transformed= transformed_plan[i].pose.position.x; //x1 + double y_transformed= transformed_plan[i].pose.position.y; //y1 + double c_perpendicular= y_transformed+(1/planLineSlope)*x_transformed; // y-intercept of the line connecting (x1,y1) and the perpendicular point + // meeting the line connecting robot pose and local goal + + double x2= (planLine_yIntercept-c_perpendicular)/((-1/planLineSlope)-planLineSlope); //x2 and y2 are the new points on the line connecting robot pose and local goal + double y2= (planLineSlope*x2)+planLine_yIntercept; + + double sqDist= (x2-x_transformed)*(x2-x_transformed)+(y2-y_transformed)*(y2-y_transformed); //Calculating distance b/w the two points + + if(max_dist0.01) //move_base crashes for unknown reason if max_dist=0.000000 + {via_points_.push_back( Eigen::Vector2d( transformed_plan[viaIdx].pose.position.x, transformed_plan[viaIdx].pose.position.y ) ); + // ROS_INFO("The Distance is %f",max_dist); + cfg_.hcp.max_number_classes=1; + } + else + + cfg_.hcp.max_number_classes=4; // ROS_INFO("Number : %d",cfg_.hcp.max_number_classes); // transformed_plan[i].pose.position.x, transformed_plan[i].pose.position.y +} bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) { @@ -311,6 +346,9 @@ bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) // update via-points container updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep); + //@mudit edit : creating a via point for including peak points of the global plan in the robot's path + peakViaPoint(robot_pose_.x(),robot_pose_.y(),robot_goal_.x(),robot_goal_.y(),transformed_plan); + // Do not allow config changes during the following optimization step boost::mutex::scoped_lock cfg_lock(cfg_.configMutex()); From 9fbe28cc641584801830ac84b05ae1508f12b17f Mon Sep 17 00:00:00 2001 From: Mudit Jindal Date: Sat, 21 Jan 2017 17:45:34 +0530 Subject: [PATCH 2/9] Update teb_local_planner_ros.h added as a requirement of teb_local_planner_ros.cpp --- include/teb_local_planner/teb_local_planner_ros.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/teb_local_planner/teb_local_planner_ros.h b/include/teb_local_planner/teb_local_planner_ros.h index 78445aef..5167df00 100644 --- a/include/teb_local_planner/teb_local_planner_ros.h +++ b/include/teb_local_planner/teb_local_planner_ros.h @@ -338,6 +338,8 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner */ void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist); + void peakViaPoint(double robot_x, double robot_y, double localGoal_x, double localGoal_y, + std::vector& transformed_plan); //Edit @mudit void configureBackupModes(std::vector& transformed_plan, int& goal_idx); From 34fd47e7c77b73241dae2c9607bb38c91a0c4909 Mon Sep 17 00:00:00 2001 From: Mudit Jindal Date: Wed, 1 Feb 2017 23:16:02 +0530 Subject: [PATCH 3/9] Update teb_local_planner_ros.cpp --- src/teb_local_planner_ros.cpp | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp index bd492308..a7a49e0a 100644 --- a/src/teb_local_planner_ros.cpp +++ b/src/teb_local_planner_ros.cpp @@ -206,40 +206,41 @@ bool TebLocalPlannerROS::setPlan(const std::vector& return true; } -void TebLocalPlannerROS::peakViaPoint(double robot_x, double robot_y, double localGoal_x, double localGoal_y, +void TebLocalPlannerROS::peakViaPoint(double robot_x, double robot_y, double local_goal_x, double local_goal_y, std::vector& transformed_plan) //@mudit { - double planLineSlope= (localGoal_y-robot_y)/(localGoal_x-robot_x); // slope of the line connecting robot pose and local goal - double planLine_yIntercept= robot_y-(planLineSlope*robot_x); // y-intercept of the line connecting robot pose and local goal + double plan_line_slope= (local_goal_y-robot_y)/(local_goal_x-robot_x); // slope of the line connecting robot pose and local goal + double plan_line_yintercept= robot_y-(plan_line_slope*robot_x); // y-intercept of the line connecting robot pose and local goal double max_dist=0; - int viaIdx; + int via_idx; for (std::size_t i=1; i < transformed_plan.size(); ++i) {double x_transformed= transformed_plan[i].pose.position.x; //x1 double y_transformed= transformed_plan[i].pose.position.y; //y1 - double c_perpendicular= y_transformed+(1/planLineSlope)*x_transformed; // y-intercept of the line connecting (x1,y1) and the perpendicular point + double c_perpendicular= y_transformed+(1/plan_line_slope)*x_transformed; // y-intercept of the line connecting (x1,y1) and the perpendicular point // meeting the line connecting robot pose and local goal - double x2= (planLine_yIntercept-c_perpendicular)/((-1/planLineSlope)-planLineSlope); //x2 and y2 are the new points on the line connecting robot pose and local goal - double y2= (planLineSlope*x2)+planLine_yIntercept; + double x2= (plan_line_yintercept-c_perpendicular)/((-1/plan_line_slope)-plan_line_slope); //x2 and y2 are the new points on the line connecting robot pose and local goal + double y2= (plan_line_slope*x2)+plan_line_yintercept; double sqDist= (x2-x_transformed)*(x2-x_transformed)+(y2-y_transformed)*(y2-y_transformed); //Calculating distance b/w the two points if(max_dist0.01) //move_base crashes for unknown reason if max_dist=0.000000 - {via_points_.push_back( Eigen::Vector2d( transformed_plan[viaIdx].pose.position.x, transformed_plan[viaIdx].pose.position.y ) ); - // ROS_INFO("The Distance is %f",max_dist); - cfg_.hcp.max_number_classes=1; + { + via_points_.push_back( Eigen::Vector2d( transformed_plan[via_idx].pose.position.x, transformed_plan[via_idx].pose.position.y ) ); + cfg_.hcp.max_number_classes=1; } else - cfg_.hcp.max_number_classes=4; // ROS_INFO("Number : %d",cfg_.hcp.max_number_classes); // transformed_plan[i].pose.position.x, transformed_plan[i].pose.position.y + cfg_.hcp.max_number_classes=4; // transformed_plan[i].pose.position.x, transformed_plan[i].pose.position.y } bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) From 09bc002ce92e7f91306ac625a59fb451475dcccb Mon Sep 17 00:00:00 2001 From: Mudit Jindal Date: Wed, 1 Feb 2017 23:31:48 +0530 Subject: [PATCH 4/9] Update teb_local_planner_ros.cpp --- src/teb_local_planner_ros.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp index a7a49e0a..a9b3d5cb 100644 --- a/src/teb_local_planner_ros.cpp +++ b/src/teb_local_planner_ros.cpp @@ -209,6 +209,7 @@ bool TebLocalPlannerROS::setPlan(const std::vector& void TebLocalPlannerROS::peakViaPoint(double robot_x, double robot_y, double local_goal_x, double local_goal_y, std::vector& transformed_plan) //@mudit { + if (local_goal_x-robot_x==0) return; double plan_line_slope= (local_goal_y-robot_y)/(local_goal_x-robot_x); // slope of the line connecting robot pose and local goal double plan_line_yintercept= robot_y-(plan_line_slope*robot_x); // y-intercept of the line connecting robot pose and local goal double max_dist=0; From ca8d658e6ba2f693211e9d2e72aa704d2ab3f325 Mon Sep 17 00:00:00 2001 From: Mudit Jindal Date: Sun, 26 Feb 2017 16:14:01 +0530 Subject: [PATCH 5/9] Update teb_local_planner_ros.cpp Used robot_circumscribed_radius so that minimum curvature of global plan on which a peak point will be present is a function of robot's radius. --- src/teb_local_planner_ros.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp index a9b3d5cb..8b930ba1 100644 --- a/src/teb_local_planner_ros.cpp +++ b/src/teb_local_planner_ros.cpp @@ -207,7 +207,7 @@ bool TebLocalPlannerROS::setPlan(const std::vector& } void TebLocalPlannerROS::peakViaPoint(double robot_x, double robot_y, double local_goal_x, double local_goal_y, - std::vector& transformed_plan) //@mudit + std::vector& transformed_plan, double robot_circumscribed_radius) //@mudit { if (local_goal_x-robot_x==0) return; double plan_line_slope= (local_goal_y-robot_y)/(local_goal_x-robot_x); // slope of the line connecting robot pose and local goal @@ -234,14 +234,16 @@ void TebLocalPlannerROS::peakViaPoint(double robot_x, double robot_y, double loc } - if(max_dist>0.01) //move_base crashes for unknown reason if max_dist=0.000000 + if(max_dist>(robot_circumscribed_radius/16)) //move_base crashes for unknown reason if max_dist=0.000000 { via_points_.push_back( Eigen::Vector2d( transformed_plan[via_idx].pose.position.x, transformed_plan[via_idx].pose.position.y ) ); - cfg_.hcp.max_number_classes=1; + cfg_.hcp.enable_multithreading=false; + cfg_.hcp.max_number_classes=false; } else - cfg_.hcp.max_number_classes=4; // transformed_plan[i].pose.position.x, transformed_plan[i].pose.position.y + cfg_.hcp.enable_multithreading=true; + cfg_.hcp.max_number_classes=true; } bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) From ff1065b6c666483405b608670905de68f4c05e7c Mon Sep 17 00:00:00 2001 From: Mudit Jindal Date: Sun, 26 Feb 2017 16:15:52 +0530 Subject: [PATCH 6/9] Update teb_local_planner_ros.h --- include/teb_local_planner/teb_local_planner_ros.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/teb_local_planner/teb_local_planner_ros.h b/include/teb_local_planner/teb_local_planner_ros.h index 5167df00..7f8f80f5 100644 --- a/include/teb_local_planner/teb_local_planner_ros.h +++ b/include/teb_local_planner/teb_local_planner_ros.h @@ -339,7 +339,7 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist); void peakViaPoint(double robot_x, double robot_y, double localGoal_x, double localGoal_y, - std::vector& transformed_plan); //Edit @mudit + std::vector& transformed_plan, double robot_radius); //Edit @mudit void configureBackupModes(std::vector& transformed_plan, int& goal_idx); From 266e56fde476097e6cee3bb7986feb7d1c2fee71 Mon Sep 17 00:00:00 2001 From: Mudit Jindal Date: Fri, 10 Mar 2017 19:59:21 +0530 Subject: [PATCH 7/9] Update teb_local_planner_ros.cpp --- src/teb_local_planner_ros.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp index 8b930ba1..70655c29 100644 --- a/src/teb_local_planner_ros.cpp +++ b/src/teb_local_planner_ros.cpp @@ -351,7 +351,7 @@ bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep); //@mudit edit : creating a via point for including peak points of the global plan in the robot's path - peakViaPoint(robot_pose_.x(),robot_pose_.y(),robot_goal_.x(),robot_goal_.y(),transformed_plan); + peakViaPoint(robot_pose_.x(),robot_pose_.y(),robot_goal_.x(),robot_goal_.y(),transformed_plan, robot_circumscribed_radius); // Do not allow config changes during the following optimization step boost::mutex::scoped_lock cfg_lock(cfg_.configMutex()); From e7e1e7fb3ec5a4fb89d97192831bf7fc4fdd0bdd Mon Sep 17 00:00:00 2001 From: Mudit Jindal Date: Sun, 25 Feb 2018 23:08:53 +0530 Subject: [PATCH 8/9] Delete teb_local_planner_ros.cpp --- src/teb_local_planner_ros.cpp | 1133 --------------------------------- 1 file changed, 1133 deletions(-) delete mode 100644 src/teb_local_planner_ros.cpp diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp deleted file mode 100644 index 70655c29..00000000 --- a/src/teb_local_planner_ros.cpp +++ /dev/null @@ -1,1133 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include - -#include - -#include - -// pluginlib macros -#include - -#include "g2o/core/sparse_optimizer.h" -#include "g2o/core/block_solver.h" -#include "g2o/core/factory.h" -#include "g2o/core/optimization_algorithm_gauss_newton.h" -#include "g2o/core/optimization_algorithm_levenberg.h" -#include "g2o/solvers/csparse/linear_solver_csparse.h" -#include "g2o/solvers/cholmod/linear_solver_cholmod.h" - - -// register this planner as a BaseLocalPlanner plugin -PLUGINLIB_DECLARE_CLASS(teb_local_planner, TebLocalPlannerROS, teb_local_planner::TebLocalPlannerROS, nav_core::BaseLocalPlanner) - -namespace teb_local_planner -{ - - -TebLocalPlannerROS::TebLocalPlannerROS() : costmap_ros_(NULL), tf_(NULL), costmap_model_(NULL), - costmap_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"), - dynamic_recfg_(NULL), goal_reached_(false), no_infeasible_plans_(0), last_preferred_rotdir_(RotType::none), - initialized_(false) -{ -} - - -TebLocalPlannerROS::~TebLocalPlannerROS() -{ -} - -void TebLocalPlannerROS::reconfigureCB(TebLocalPlannerReconfigureConfig& config, uint32_t level) -{ - cfg_.reconfigure(config); -} - -void TebLocalPlannerROS::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) -{ - // check if the plugin is already initialized - if(!initialized_) - { - // create Node Handle with name of plugin (as used in move_base for loading) - ros::NodeHandle nh("~/" + name); - - // get parameters of TebConfig via the nodehandle and override the default config - cfg_.loadRosParamFromNodeHandle(nh); - - // reserve some memory for obstacles - obstacles_.reserve(500); - - // init some variables - tf_ = tf; - costmap_ros_ = costmap_ros; - costmap_ = costmap_ros_->getCostmap(); // locking should be done in MoveBase. - - costmap_model_ = boost::make_shared(*costmap_); - - - global_frame_ = costmap_ros_->getGlobalFrameID(); - cfg_.map_frame = global_frame_; // TODO - robot_base_frame_ = costmap_ros_->getBaseFrameID(); - - // create visualization instance - visualization_ = TebVisualizationPtr(new TebVisualization(nh, cfg_.map_frame)); - - // create robot footprint/contour model for optimization - RobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh); - - // create the planner instance - if (cfg_.hcp.enable_homotopy_class_planning) - { - planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_)); - ROS_INFO("Parallel planning in distinctive topologies enabled."); - } - else - { - planner_ = PlannerInterfacePtr(new TebOptimalPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_)); - ROS_INFO("Parallel planning in distinctive topologies disabled."); - } - - //Initialize a costmap to polygon converter - if (!cfg_.obstacles.costmap_converter_plugin.empty()) - { - try - { - costmap_converter_ = costmap_converter_loader_.createInstance(cfg_.obstacles.costmap_converter_plugin); - std::string converter_name = costmap_converter_loader_.getName(cfg_.obstacles.costmap_converter_plugin); - // replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace - boost::replace_all(converter_name, "::", "/"); - costmap_converter_->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name)); - costmap_converter_->setCostmap2D(costmap_); - - costmap_converter_->startWorker(ros::Rate(cfg_.obstacles.costmap_converter_rate), costmap_, cfg_.obstacles.costmap_converter_spin_thread); - ROS_INFO_STREAM("Costmap conversion plugin " << cfg_.obstacles.costmap_converter_plugin << " loaded."); - } - catch(pluginlib::PluginlibException& ex) - { - ROS_WARN("The specified costmap converter plugin cannot be loaded. All occupied costmap cells are treaten as point obstacles. Error message: %s", ex.what()); - costmap_converter_.reset(); - } - } - else - ROS_INFO("No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles."); - - - // Get footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices. - footprint_spec_ = costmap_ros_->getRobotFootprint(); - costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius); - - // init the odom helper to receive the robot's velocity from odom messages - odom_helper_.setOdomTopic(cfg_.odom_topic); - - // setup dynamic reconfigure - dynamic_recfg_ = boost::make_shared< dynamic_reconfigure::Server >(nh); - dynamic_reconfigure::Server::CallbackType cb = boost::bind(&TebLocalPlannerROS::reconfigureCB, this, _1, _2); - dynamic_recfg_->setCallback(cb); - - // validate optimization footprint and costmap footprint - validateFootprints(robot_model->getInscribedRadius(), robot_inscribed_radius_, cfg_.obstacles.min_obstacle_dist); - - // setup callback for custom obstacles - custom_obst_sub_ = nh.subscribe("obstacles", 1, &TebLocalPlannerROS::customObstacleCB, this); - - // initialize failure detector - ros::NodeHandle nh_move_base("~"); - double controller_frequency = 5; - nh_move_base.param("controller_frequency", controller_frequency, controller_frequency); - failure_detector_.setBufferLength(std::round(cfg_.recovery.oscillation_filter_duration*controller_frequency)); - - // set initialized flag - initialized_ = true; - - ROS_DEBUG("teb_local_planner plugin initialized."); - } - else - { - ROS_WARN("teb_local_planner has already been initialized, doing nothing."); - } -} - - - -bool TebLocalPlannerROS::setPlan(const std::vector& orig_global_plan) -{ - // check if plugin is initialized - if(!initialized_) - { - ROS_ERROR("teb_local_planner has not been initialized, please call initialize() before using this planner"); - return false; - } - - // store the global plan - global_plan_.clear(); - global_plan_ = orig_global_plan; - - // we do not clear the local planner here, since setPlan is called frequently whenever the global planner updates the plan. - // the local planner checks whether it is required to reinitialize the trajectory or not within each velocity computation step. - - // reset goal_reached_ flag - goal_reached_ = false; - - return true; -} - -void TebLocalPlannerROS::peakViaPoint(double robot_x, double robot_y, double local_goal_x, double local_goal_y, - std::vector& transformed_plan, double robot_circumscribed_radius) //@mudit -{ - if (local_goal_x-robot_x==0) return; - double plan_line_slope= (local_goal_y-robot_y)/(local_goal_x-robot_x); // slope of the line connecting robot pose and local goal - double plan_line_yintercept= robot_y-(plan_line_slope*robot_x); // y-intercept of the line connecting robot pose and local goal - double max_dist=0; - int via_idx; - for (std::size_t i=1; i < transformed_plan.size(); ++i) - {double x_transformed= transformed_plan[i].pose.position.x; //x1 - double y_transformed= transformed_plan[i].pose.position.y; //y1 - double c_perpendicular= y_transformed+(1/plan_line_slope)*x_transformed; // y-intercept of the line connecting (x1,y1) and the perpendicular point - // meeting the line connecting robot pose and local goal - - double x2= (plan_line_yintercept-c_perpendicular)/((-1/plan_line_slope)-plan_line_slope); //x2 and y2 are the new points on the line connecting robot pose and local goal - double y2= (plan_line_slope*x2)+plan_line_yintercept; - - double sqDist= (x2-x_transformed)*(x2-x_transformed)+(y2-y_transformed)*(y2-y_transformed); //Calculating distance b/w the two points - - if(max_dist(robot_circumscribed_radius/16)) //move_base crashes for unknown reason if max_dist=0.000000 - { - via_points_.push_back( Eigen::Vector2d( transformed_plan[via_idx].pose.position.x, transformed_plan[via_idx].pose.position.y ) ); - cfg_.hcp.enable_multithreading=false; - cfg_.hcp.max_number_classes=false; - } - else - - cfg_.hcp.enable_multithreading=true; - cfg_.hcp.max_number_classes=true; -} - -bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("teb_local_planner has not been initialized, please call initialize() before using this planner"); - return false; - } - - cmd_vel.linear.x = 0; - cmd_vel.linear.y = 0; - cmd_vel.angular.z = 0; - goal_reached_ = false; - - // Get robot pose - tf::Stamped robot_pose; - costmap_ros_->getRobotPose(robot_pose); - robot_pose_ = PoseSE2(robot_pose); - - // Get robot velocity - tf::Stamped robot_vel_tf; - odom_helper_.getRobotVel(robot_vel_tf); - robot_vel_.linear.x = robot_vel_tf.getOrigin().getX(); - robot_vel_.linear.y = robot_vel_tf.getOrigin().getY(); - robot_vel_.angular.z = tf::getYaw(robot_vel_tf.getRotation()); - - // prune global plan to cut off parts of the past (spatially before the robot) - pruneGlobalPlan(*tf_, robot_pose, global_plan_); - - // Transform global plan to the frame of interest (w.r.t. the local costmap) - std::vector transformed_plan; - int goal_idx; - tf::StampedTransform tf_plan_to_global; - if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist, - transformed_plan, &goal_idx, &tf_plan_to_global)) - { - ROS_WARN("Could not transform the global plan to the frame of the controller"); - return false; - } - - // check if global goal is reached - tf::Stamped global_goal; - tf::poseStampedMsgToTF(global_plan_.back(), global_goal); - global_goal.setData( tf_plan_to_global * global_goal ); - double dx = global_goal.getOrigin().getX() - robot_pose_.x(); - double dy = global_goal.getOrigin().getY() - robot_pose_.y(); - double delta_orient = g2o::normalize_theta( tf::getYaw(global_goal.getRotation()) - robot_pose_.theta() ); - if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance - && fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance) - { - goal_reached_ = true; - return true; - } - - - // check if we should enter any backup mode and apply settings - configureBackupModes(transformed_plan, goal_idx); - - - // Return false if the transformed global plan is empty - if (transformed_plan.empty()) - { - ROS_WARN("Transformed plan is empty. Cannot determine a local plan."); - return false; - } - - // Get current goal point (last point of the transformed plan) - tf::Stamped goal_point; - tf::poseStampedMsgToTF(transformed_plan.back(), goal_point); - robot_goal_.x() = goal_point.getOrigin().getX(); - robot_goal_.y() = goal_point.getOrigin().getY(); - if (cfg_.trajectory.global_plan_overwrite_orientation) - { - robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, goal_point, goal_idx, tf_plan_to_global); - // overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization) - transformed_plan.back().pose.orientation = tf::createQuaternionMsgFromYaw(robot_goal_.theta()); - } - else - { - robot_goal_.theta() = tf::getYaw(goal_point.getRotation()); - } - - // overwrite/update start of the transformed plan with the actual robot position (allows using the plan as initial trajectory) - if (transformed_plan.size()==1) // plan only contains the goal - { - transformed_plan.insert(transformed_plan.begin(), geometry_msgs::PoseStamped()); // insert start (not yet initialized) - } - tf::poseTFToMsg(robot_pose, transformed_plan.front().pose); // update start; - - // clear currently existing obstacles - obstacles_.clear(); - - // Update obstacle container with costmap information or polygons provided by a costmap_converter plugin - if (costmap_converter_) - updateObstacleContainerWithCostmapConverter(); - else - updateObstacleContainerWithCostmap(); - - // also consider custom obstacles (must be called after other updates, since the container is not cleared) - updateObstacleContainerWithCustomObstacles(); - - // update via-points container - updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep); - - //@mudit edit : creating a via point for including peak points of the global plan in the robot's path - peakViaPoint(robot_pose_.x(),robot_pose_.y(),robot_goal_.x(),robot_goal_.y(),transformed_plan, robot_circumscribed_radius); - - // Do not allow config changes during the following optimization step - boost::mutex::scoped_lock cfg_lock(cfg_.configMutex()); - - // Now perform the actual planning -// bool success = planner_->plan(robot_pose_, robot_goal_, robot_vel_, cfg_.goal_tolerance.free_goal_vel); // straight line init - bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel); - if (!success) - { - planner_->clearPlanner(); // force reinitialization for next time - ROS_WARN("teb_local_planner was not able to obtain a local plan for the current setting."); - - ++no_infeasible_plans_; // increase number of infeasible solutions in a row - time_last_infeasible_plan_ = ros::Time::now(); - last_cmd_ = cmd_vel; - return false; - } - - // Check feasibility (but within the first few states only) - bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.trajectory.feasibility_check_no_poses); - if (!feasible) - { - cmd_vel.linear.x = 0; - cmd_vel.linear.y = 0; - cmd_vel.angular.z = 0; - - // now we reset everything to start again with the initialization of new trajectories. - planner_->clearPlanner(); - ROS_WARN("TebLocalPlannerROS: trajectory is not feasible. Resetting planner..."); - - ++no_infeasible_plans_; // increase number of infeasible solutions in a row - time_last_infeasible_plan_ = ros::Time::now(); - last_cmd_ = cmd_vel; - return false; - } - - // Get the velocity command for this sampling interval - if (!planner_->getVelocityCommand(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z)) - { - planner_->clearPlanner(); - ROS_WARN("TebLocalPlannerROS: velocity command invalid. Resetting planner..."); - ++no_infeasible_plans_; // increase number of infeasible solutions in a row - time_last_infeasible_plan_ = ros::Time::now(); - last_cmd_ = cmd_vel; - return false; - } - - // Saturate velocity, if the optimization results violates the constraints (could be possible due to soft constraints). - saturateVelocity(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z, cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, - cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards); - - // convert rot-vel to steering angle if desired (carlike robot). - // The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint - // and opposed to the other constraints not affected by penalty_epsilon. The user might add a safety margin to the parameter itself. - if (cfg_.robot.cmd_angle_instead_rotvel) - { - cmd_vel.angular.z = convertTransRotVelToSteeringAngle(cmd_vel.linear.x, cmd_vel.angular.z, cfg_.robot.wheelbase, 0.95*cfg_.robot.min_turning_radius); - if (!std::isfinite(cmd_vel.angular.z)) - { - cmd_vel.linear.x = cmd_vel.linear.y = cmd_vel.angular.z = 0; - last_cmd_ = cmd_vel; - planner_->clearPlanner(); - ROS_WARN("TebLocalPlannerROS: Resulting steering angle is not finite. Resetting planner..."); - ++no_infeasible_plans_; // increase number of infeasible solutions in a row - time_last_infeasible_plan_ = ros::Time::now(); - return false; - } - } - - // a feasible solution should be found, reset counter - no_infeasible_plans_ = 0; - - // store last command (for recovery analysis etc.) - last_cmd_ = cmd_vel; - - // Now visualize everything - planner_->visualize(); - visualization_->publishObstacles(obstacles_); - visualization_->publishViaPoints(via_points_); - visualization_->publishGlobalPlan(global_plan_); - return true; -} - - -bool TebLocalPlannerROS::isGoalReached() -{ - if (goal_reached_) - { - ROS_INFO("GOAL Reached!"); - planner_->clearPlanner(); - return true; - } - return false; -} - - - -void TebLocalPlannerROS::updateObstacleContainerWithCostmap() -{ - // Add costmap obstacles if desired - if (cfg_.obstacles.include_costmap_obstacles) - { - Eigen::Vector2d robot_orient = robot_pose_.orientationUnitVec(); - - for (unsigned int i=0; igetSizeInCellsX()-1; ++i) - { - for (unsigned int j=0; jgetSizeInCellsY()-1; ++j) - { - if (costmap_->getCost(i,j) == costmap_2d::LETHAL_OBSTACLE) - { - Eigen::Vector2d obs; - costmap_->mapToWorld(i,j,obs.coeffRef(0), obs.coeffRef(1)); - - // check if obstacle is interesting (e.g. not far behind the robot) - Eigen::Vector2d obs_dir = obs-robot_pose_.position(); - if ( obs_dir.dot(robot_orient) < 0 && obs_dir.norm() > cfg_.obstacles.costmap_obstacles_behind_robot_dist ) - continue; - - obstacles_.push_back(ObstaclePtr(new PointObstacle(obs))); - } - } - } - } -} - -void TebLocalPlannerROS::updateObstacleContainerWithCostmapConverter() -{ - if (!costmap_converter_) - return; - - //Get obstacles from costmap converter - costmap_converter::PolygonContainerConstPtr polygons = costmap_converter_->getPolygons(); - if (!polygons) - return; - - for (std::size_t i=0; isize(); ++i) - { - if (polygons->at(i).points.size()==1) // Point - { - obstacles_.push_back(ObstaclePtr(new PointObstacle(polygons->at(i).points[0].x, polygons->at(i).points[0].y))); - } - else if (polygons->at(i).points.size()==2) // Line - { - obstacles_.push_back(ObstaclePtr(new LineObstacle(polygons->at(i).points[0].x, polygons->at(i).points[0].y, - polygons->at(i).points[1].x, polygons->at(i).points[1].y ))); - } - else if (polygons->at(i).points.size()>2) // Real polygon - { - PolygonObstacle* polyobst = new PolygonObstacle; - for (std::size_t j=0; jat(i).points.size(); ++j) - { - polyobst->pushBackVertex(polygons->at(i).points[j].x, polygons->at(i).points[j].y); - } - polyobst->finalizePolygon(); - obstacles_.push_back(ObstaclePtr(polyobst)); - } - } -} - - -void TebLocalPlannerROS::updateObstacleContainerWithCustomObstacles() -{ - // Add custom obstacles obtained via message - boost::mutex::scoped_lock l(custom_obst_mutex_); - - if (!custom_obstacle_msg_.obstacles.empty()) - { - // We only use the global header to specify the obstacle coordinate system instead of individual ones - Eigen::Affine3d obstacle_to_map_eig; - try - { - tf::StampedTransform obstacle_to_map; - tf_->waitForTransform(global_frame_, ros::Time(0), - custom_obstacle_msg_.header.frame_id, ros::Time(0), - custom_obstacle_msg_.header.frame_id, ros::Duration(0.5)); - tf_->lookupTransform(global_frame_, ros::Time(0), - custom_obstacle_msg_.header.frame_id, ros::Time(0), - custom_obstacle_msg_.header.frame_id, obstacle_to_map); - tf::transformTFToEigen(obstacle_to_map, obstacle_to_map_eig); - } - catch (tf::TransformException ex) - { - ROS_ERROR("%s",ex.what()); - obstacle_to_map_eig.setIdentity(); - } - - for (std::vector::const_iterator obst_it = custom_obstacle_msg_.obstacles.begin(); obst_it != custom_obstacle_msg_.obstacles.end(); ++obst_it) - { - if (obst_it->polygon.points.size() == 1 ) // point - { - Eigen::Vector3d pos( obst_it->polygon.points.front().x, obst_it->polygon.points.front().y, obst_it->polygon.points.front().z ); - obstacles_.push_back(ObstaclePtr(new PointObstacle( (obstacle_to_map_eig * pos).head(2) ))); - } - else if (obst_it->polygon.points.size() == 2 ) // line - { - Eigen::Vector3d line_start( obst_it->polygon.points.front().x, obst_it->polygon.points.front().y, obst_it->polygon.points.front().z ); - Eigen::Vector3d line_end( obst_it->polygon.points.back().x, obst_it->polygon.points.back().y, obst_it->polygon.points.back().z ); - obstacles_.push_back(ObstaclePtr(new LineObstacle( (obstacle_to_map_eig * line_start).head(2), (obstacle_to_map_eig * line_end).head(2) ))); - } - else // polygon - { - PolygonObstacle* polyobst = new PolygonObstacle; - for (int i=0; i<(int)obst_it->polygon.points.size(); ++i) - { - Eigen::Vector3d pos( obst_it->polygon.points[i].x, obst_it->polygon.points[i].y, obst_it->polygon.points[i].z ); - polyobst->pushBackVertex( (obstacle_to_map_eig * pos).head(2) ); - } - polyobst->finalizePolygon(); - obstacles_.push_back(ObstaclePtr(polyobst)); - } - } - } -} - -void TebLocalPlannerROS::updateViaPointsContainer(const std::vector& transformed_plan, double min_separation) -{ - via_points_.clear(); - - if (min_separation<0) - return; - - std::size_t prev_idx = 0; - for (std::size_t i=1; i < transformed_plan.size(); ++i) // skip first one, since we do not need any point before the first min_separation [m] - { - // check separation to the previous via-point inserted - if (distance_points2d( transformed_plan[prev_idx].pose.position, transformed_plan[i].pose.position ) < min_separation) - continue; - - // add via-point - via_points_.push_back( Eigen::Vector2d( transformed_plan[i].pose.position.x, transformed_plan[i].pose.position.y ) ); - prev_idx = i; - } - -} - -Eigen::Vector2d TebLocalPlannerROS::tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel) -{ - Eigen::Vector2d vel; - vel.coeffRef(0) = std::sqrt( tf_vel.getOrigin().getX() * tf_vel.getOrigin().getX() + tf_vel.getOrigin().getY() * tf_vel.getOrigin().getY() ); - vel.coeffRef(1) = tf::getYaw(tf_vel.getRotation()); - return vel; -} - - -bool TebLocalPlannerROS::pruneGlobalPlan(const tf::TransformListener& tf, const tf::Stamped& global_pose, std::vector& global_plan, double dist_behind_robot) -{ - if (global_plan.empty()) - return true; - - try - { - // transform robot pose into the plan frame (we do not wait here, since pruning not crucial, if missed a few times) - tf::StampedTransform global_to_plan_transform; - tf.lookupTransform(global_plan.front().header.frame_id, global_pose.frame_id_, ros::Time(0), global_to_plan_transform); - tf::Stamped robot; - robot.setData( global_to_plan_transform * global_pose ); - - double dist_thresh_sq = dist_behind_robot*dist_behind_robot; - - // iterate plan until a pose close the robot is found - std::vector::iterator it = global_plan.begin(); - std::vector::iterator erase_end = it; - while (it != global_plan.end()) - { - double dx = robot.getOrigin().x() - it->pose.position.x; - double dy = robot.getOrigin().y() - it->pose.position.y; - double dist_sq = dx * dx + dy * dy; - if (dist_sq < dist_thresh_sq) - { - erase_end = it; - break; - } - ++it; - } - if (erase_end == global_plan.end()) - return false; - - if (erase_end != global_plan.begin()) - global_plan.erase(global_plan.begin(), erase_end); - } - catch (const tf::TransformException& ex) - { - ROS_DEBUG("Cannot prune path since no transform is available: %s\n", ex.what()); - return false; - } - return true; -} - - -bool TebLocalPlannerROS::transformGlobalPlan(const tf::TransformListener& tf, const std::vector& global_plan, - const tf::Stamped& global_pose, const costmap_2d::Costmap2D& costmap, const std::string& global_frame, double max_plan_length, - std::vector& transformed_plan, int* current_goal_idx, tf::StampedTransform* tf_plan_to_global) const -{ - // this method is a slightly modified version of base_local_planner/goal_functions.h - - const geometry_msgs::PoseStamped& plan_pose = global_plan[0]; - - transformed_plan.clear(); - - try - { - if (global_plan.empty()) - { - ROS_ERROR("Received plan with zero length"); - *current_goal_idx = 0; - return false; - } - - // get plan_to_global_transform from plan frame to global_frame - tf::StampedTransform plan_to_global_transform; - tf.waitForTransform(global_frame, ros::Time::now(), - plan_pose.header.frame_id, plan_pose.header.stamp, - plan_pose.header.frame_id, ros::Duration(0.5)); - tf.lookupTransform(global_frame, ros::Time(), - plan_pose.header.frame_id, plan_pose.header.stamp, - plan_pose.header.frame_id, plan_to_global_transform); - - //let's get the pose of the robot in the frame of the plan - tf::Stamped robot_pose; - tf.transformPose(plan_pose.header.frame_id, global_pose, robot_pose); - - //we'll discard points on the plan that are outside the local costmap - double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0, - costmap.getSizeInCellsY() * costmap.getResolution() / 2.0); - dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are - // located on the border of the local costmap - - - int i = 0; - double sq_dist_threshold = dist_threshold * dist_threshold; - double sq_dist = 1e10; - - //we need to loop to a point on the plan that is within a certain distance of the robot - while(i < (int)global_plan.size()) - { - double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x; - double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y; - double new_sq_dist = x_diff * x_diff + y_diff * y_diff; - if (new_sq_dist > sq_dist && sq_dist < sq_dist_threshold) // find first distance that is greater - { - sq_dist = new_sq_dist; - break; - } - sq_dist = new_sq_dist; - ++i; - } - - tf::Stamped tf_pose; - geometry_msgs::PoseStamped newer_pose; - - double plan_length = 0; // check cumulative Euclidean distance along the plan - - //now we'll transform until points are outside of our distance threshold - while(i < (int)global_plan.size() && sq_dist <= sq_dist_threshold && (max_plan_length<=0 || plan_length <= max_plan_length)) - { - const geometry_msgs::PoseStamped& pose = global_plan[i]; - tf::poseStampedMsgToTF(pose, tf_pose); - tf_pose.setData(plan_to_global_transform * tf_pose); - tf_pose.stamp_ = plan_to_global_transform.stamp_; - tf_pose.frame_id_ = global_frame; - tf::poseStampedTFToMsg(tf_pose, newer_pose); - - transformed_plan.push_back(newer_pose); - - double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x; - double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y; - sq_dist = x_diff * x_diff + y_diff * y_diff; - - // caclulate distance to previous pose - if (i>0 && max_plan_length>0) - plan_length += distance_points2d(global_plan[i-1].pose.position, global_plan[i].pose.position); - - ++i; - } - - // if we are really close to the goal (>0) - // the resulting transformed plan can be empty. In that case we explicitly inject the global goal. - if (transformed_plan.empty()) - { - tf::poseStampedMsgToTF(global_plan.back(), tf_pose); - tf_pose.setData(plan_to_global_transform * tf_pose); - tf_pose.stamp_ = plan_to_global_transform.stamp_; - tf_pose.frame_id_ = global_frame; - tf::poseStampedTFToMsg(tf_pose, newer_pose); - - transformed_plan.push_back(newer_pose); - - // Return the index of the current goal point (inside the distance threshold) - if (current_goal_idx) *current_goal_idx = int(global_plan.size())-1; - } - else - { - // Return the index of the current goal point (inside the distance threshold) - if (current_goal_idx) *current_goal_idx = i-1; // subtract 1, since i was increased once before leaving the loop - } - - // Return the transformation from the global plan to the global planning frame if desired - if (tf_plan_to_global) *tf_plan_to_global = plan_to_global_transform; - } - catch(tf::LookupException& ex) - { - ROS_ERROR("No Transform available Error: %s\n", ex.what()); - return false; - } - catch(tf::ConnectivityException& ex) - { - ROS_ERROR("Connectivity Error: %s\n", ex.what()); - return false; - } - catch(tf::ExtrapolationException& ex) - { - ROS_ERROR("Extrapolation Error: %s\n", ex.what()); - if (global_plan.size() > 0) - ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str()); - - return false; - } - - return true; -} - - - - -double TebLocalPlannerROS::estimateLocalGoalOrientation(const std::vector& global_plan, const tf::Stamped& local_goal, - int current_goal_idx, const tf::StampedTransform& tf_plan_to_global, int moving_average_length) const -{ - int n = (int)global_plan.size(); - - // check if we are near the global goal already - if (current_goal_idx > n-moving_average_length-2) - { - if (current_goal_idx >= n-1) // we've exactly reached the goal - { - return tf::getYaw(local_goal.getRotation()); - } - else - { - tf::Quaternion global_orientation; - tf::quaternionMsgToTF(global_plan.back().pose.orientation, global_orientation); - return tf::getYaw(tf_plan_to_global.getRotation() * global_orientation ); - } - } - - // reduce number of poses taken into account if the desired number of poses is not available - moving_average_length = std::min(moving_average_length, n-current_goal_idx-1 ); // maybe redundant, since we have checked the vicinity of the goal before - - std::vector candidates; - tf::Stamped tf_pose_k = local_goal; - tf::Stamped tf_pose_kp1; - - int range_end = current_goal_idx + moving_average_length; - for (int i = current_goal_idx; i < range_end; ++i) - { - // Transform pose of the global plan to the planning frame - const geometry_msgs::PoseStamped& pose = global_plan.at(i+1); - tf::poseStampedMsgToTF(pose, tf_pose_kp1); - tf_pose_kp1.setData(tf_plan_to_global * tf_pose_kp1); - - // calculate yaw angle - candidates.push_back( std::atan2(tf_pose_kp1.getOrigin().getY() - tf_pose_k.getOrigin().getY(), - tf_pose_kp1.getOrigin().getX() - tf_pose_k.getOrigin().getX() ) ); - - if (i max_vel_x) - vx = max_vel_x; - - // limit strafing velocity - if (vy > max_vel_y) - vy = max_vel_y; - else if (vy < -max_vel_y) - vy = -max_vel_y; - - // Limit angular velocity - if (omega > max_vel_theta) - omega = max_vel_theta; - else if (omega < -max_vel_theta) - omega = -max_vel_theta; - - // Limit backwards velocity - if (max_vel_x_backwards<=0) - { - ROS_WARN_ONCE("TebLocalPlannerROS(): Do not choose max_vel_x_backwards to be <=0. Disable backwards driving by increasing the optimization weight for penalyzing backwards driving."); - } - else if (vx < -max_vel_x_backwards) - vx = -max_vel_x_backwards; -} - - -double TebLocalPlannerROS::convertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius) const -{ - if (omega==0 || v==0) - return 0; - - double radius = v/omega; - - if (fabs(radius) < min_turning_radius) - radius = double(g2o::sign(radius)) * min_turning_radius; - - return std::atan(wheelbase / radius); -} - - -void TebLocalPlannerROS::validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist) -{ - ROS_WARN_COND(opt_inscribed_radius + min_obst_dist < costmap_inscribed_radius, - "The inscribed radius of the footprint specified for TEB optimization (%f) + min_obstacle_dist (%f) are smaller " - "than the inscribed radius of the robot's footprint in the costmap parameters (%f, including 'footprint_padding'). " - "Infeasible optimziation results might occur frequently!", opt_inscribed_radius, min_obst_dist, costmap_inscribed_radius); -} - - - -void TebLocalPlannerROS::configureBackupModes(std::vector& transformed_plan, int& goal_idx) -{ - ros::Time current_time = ros::Time::now(); - - // reduced horizon backup mode - if (cfg_.recovery.shrink_horizon_backup && - goal_idx < (int)transformed_plan.size()-1 && // we do not reduce if the goal is already selected (because the orientation might change -> can introduce oscillations) - (no_infeasible_plans_>0 || (current_time - time_last_infeasible_plan_).toSec() < cfg_.recovery.shrink_horizon_min_duration )) // keep short horizon for at least a few seconds - { - ROS_INFO_COND(no_infeasible_plans_==1, "Activating reduced horizon backup mode for at least %.2f sec (infeasible trajectory detected).", cfg_.recovery.shrink_horizon_min_duration); - - - // Shorten horizon if requested - // reduce to 50 percent: - int horizon_reduction = goal_idx/2; - - if (no_infeasible_plans_ > 9) - { - ROS_INFO_COND(no_infeasible_plans_==10, "Infeasible trajectory detected 10 times in a row: further reducing horizon..."); - horizon_reduction /= 2; - } - - // we have a small overhead here, since we already transformed 50% more of the trajectory. - // But that's ok for now, since we do not need to make transformGlobalPlan more complex - // and a reduced horizon should occur just rarely. - int new_goal_idx_transformed_plan = int(transformed_plan.size()) - horizon_reduction - 1; - goal_idx -= horizon_reduction; - if (new_goal_idx_transformed_plan>0 && goal_idx >= 0) - transformed_plan.erase(transformed_plan.begin()+new_goal_idx_transformed_plan, transformed_plan.end()); - else - goal_idx += horizon_reduction; // this should not happen, but safety first ;-) - } - - - // detect and resolve oscillations - if (cfg_.recovery.oscillation_recovery) - { - double max_vel_theta; - double max_vel_current = last_cmd_.linear.x >= 0 ? cfg_.robot.max_vel_x : cfg_.robot.max_vel_x_backwards; - if (cfg_.robot.min_turning_radius!=0 && max_vel_current>0) - max_vel_theta = std::max( max_vel_current/std::abs(cfg_.robot.min_turning_radius), cfg_.robot.max_vel_theta ); - else - max_vel_theta = cfg_.robot.max_vel_theta; - - failure_detector_.update(last_cmd_, cfg_.robot.max_vel_x, cfg_.robot.max_vel_x_backwards, max_vel_theta, - cfg_.recovery.oscillation_v_eps, cfg_.recovery.oscillation_omega_eps); - - bool oscillating = failure_detector_.isOscillating(); - bool recently_oscillated = (ros::Time::now()-time_last_oscillation_).toSec() < cfg_.recovery.oscillation_recovery_min_duration; // check if we have already detected an oscillation recently - - if (oscillating) - { - if (!recently_oscillated) - { - // save current turning direction - if (robot_vel_.angular.z > 0) - last_preferred_rotdir_ = RotType::left; - else - last_preferred_rotdir_ = RotType::right; - ROS_WARN("TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization)."); - } - time_last_oscillation_ = ros::Time::now(); - planner_->setPreferredTurningDir(last_preferred_rotdir_); - } - else if (!recently_oscillated && last_preferred_rotdir_ != RotType::none) // clear recovery behavior - { - last_preferred_rotdir_ = RotType::none; - planner_->setPreferredTurningDir(last_preferred_rotdir_); - ROS_INFO("TebLocalPlannerROS: oscillation recovery disabled/expired."); - } - } - -} - - -void TebLocalPlannerROS::customObstacleCB(const teb_local_planner::ObstacleMsg::ConstPtr& obst_msg) -{ - boost::mutex::scoped_lock l(custom_obst_mutex_); - custom_obstacle_msg_ = *obst_msg; -} - -RobotFootprintModelPtr TebLocalPlannerROS::getRobotFootprintFromParamServer(const ros::NodeHandle& nh) -{ - std::string model_name; - if (!nh.getParam("footprint_model/type", model_name)) - { - ROS_INFO("No robot footprint model specified for trajectory optimization. Using point-shaped model."); - return boost::make_shared(); - } - - // point - if (model_name.compare("point") == 0) - { - ROS_INFO("Footprint model 'point' loaded for trajectory optimization."); - return boost::make_shared(); - } - - // circular - if (model_name.compare("circular") == 0) - { - // get radius - double radius; - if (!nh.getParam("footprint_model/radius", radius)) - { - ROS_ERROR_STREAM("Footprint model 'circular' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() - << "/footprint_model/radius' does not exist. Using point-model instead."); - return boost::make_shared(); - } - ROS_INFO_STREAM("Footprint model 'circular' (radius: " << radius <<"m) loaded for trajectory optimization."); - return boost::make_shared(radius); - } - - // line - if (model_name.compare("line") == 0) - { - // check parameters - if (!nh.hasParam("footprint_model/line_start") || !nh.hasParam("footprint_model/line_end")) - { - ROS_ERROR_STREAM("Footprint model 'line' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() - << "/footprint_model/line_start' and/or '.../line_end' do not exist. Using point-model instead."); - return boost::make_shared(); - } - // get line coordinates - std::vector line_start, line_end; - nh.getParam("footprint_model/line_start", line_start); - nh.getParam("footprint_model/line_end", line_end); - if (line_start.size() != 2 || line_end.size() != 2) - { - ROS_ERROR_STREAM("Footprint model 'line' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() - << "/footprint_model/line_start' and/or '.../line_end' do not contain x and y coordinates (2D). Using point-model instead."); - return boost::make_shared(); - } - - ROS_INFO_STREAM("Footprint model 'line' (line_start: [" << line_start[0] << "," << line_start[1] <<"]m, line_end: [" - << line_end[0] << "," << line_end[1] << "]m) loaded for trajectory optimization."); - return boost::make_shared(Eigen::Map(line_start.data()), Eigen::Map(line_end.data())); - } - - // two circles - if (model_name.compare("two_circles") == 0) - { - // check parameters - if (!nh.hasParam("footprint_model/front_offset") || !nh.hasParam("footprint_model/front_radius") - || !nh.hasParam("footprint_model/rear_offset") || !nh.hasParam("footprint_model/rear_radius")) - { - ROS_ERROR_STREAM("Footprint model 'two_circles' cannot be loaded for trajectory optimization, since params '" << nh.getNamespace() - << "/footprint_model/front_offset', '.../front_radius', '.../rear_offset' and '.../rear_radius' do not exist. Using point-model instead."); - return boost::make_shared(); - } - double front_offset, front_radius, rear_offset, rear_radius; - nh.getParam("footprint_model/front_offset", front_offset); - nh.getParam("footprint_model/front_radius", front_radius); - nh.getParam("footprint_model/rear_offset", rear_offset); - nh.getParam("footprint_model/rear_radius", rear_radius); - ROS_INFO_STREAM("Footprint model 'two_circles' (front_offset: " << front_offset <<"m, front_radius: " << front_radius - << "m, rear_offset: " << rear_offset << "m, rear_radius: " << rear_radius << "m) loaded for trajectory optimization."); - return boost::make_shared(front_offset, front_radius, rear_offset, rear_radius); - } - - // polygon - if (model_name.compare("polygon") == 0) - { - - // check parameters - XmlRpc::XmlRpcValue footprint_xmlrpc; - if (!nh.getParam("footprint_model/vertices", footprint_xmlrpc) ) - { - ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() - << "/footprint_model/vertices' does not exist. Using point-model instead."); - return boost::make_shared(); - } - // get vertices - if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray) - { - try - { - Point2dContainer polygon = makeFootprintFromXMLRPC(footprint_xmlrpc, "/footprint_model/vertices"); - ROS_INFO_STREAM("Footprint model 'polygon' loaded for trajectory optimization."); - return boost::make_shared(polygon); - } - catch(const std::exception& ex) - { - ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization: " << ex.what() << ". Using point-model instead."); - return boost::make_shared(); - } - } - else - { - ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() - << "/footprint_model/vertices' does not define an array of coordinates. Using point-model instead."); - return boost::make_shared(); - } - - } - - // otherwise - ROS_WARN_STREAM("Unknown robot footprint model specified with parameter '" << nh.getNamespace() << "/footprint_model/type'. Using point model instead."); - return boost::make_shared(); -} - - - - -Point2dContainer TebLocalPlannerROS::makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name) -{ - // Make sure we have an array of at least 3 elements. - if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray || - footprint_xmlrpc.size() < 3) - { - ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s", - full_param_name.c_str(), std::string(footprint_xmlrpc).c_str()); - throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least " - "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]"); - } - - Point2dContainer footprint; - Eigen::Vector2d pt; - - for (int i = 0; i < footprint_xmlrpc.size(); ++i) - { - // Make sure each element of the list is an array of size 2. (x and y coordinates) - XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ]; - if (point.getType() != XmlRpc::XmlRpcValue::TypeArray || - point.size() != 2) - { - ROS_FATAL("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: " - "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.", - full_param_name.c_str()); - throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: " - "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form"); - } - - pt.x() = getNumberFromXMLRPC(point[ 0 ], full_param_name); - pt.y() = getNumberFromXMLRPC(point[ 1 ], full_param_name); - - footprint.push_back(pt); - } - return footprint; -} - -double TebLocalPlannerROS::getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name) -{ - // Make sure that the value we're looking at is either a double or an int. - if (value.getType() != XmlRpc::XmlRpcValue::TypeInt && - value.getType() != XmlRpc::XmlRpcValue::TypeDouble) - { - std::string& value_string = value; - ROS_FATAL("Values in the footprint specification (param %s) must be numbers. Found value %s.", - full_param_name.c_str(), value_string.c_str()); - throw std::runtime_error("Values in the footprint specification must be numbers"); - } - return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value); -} - -} // end namespace teb_local_planner - - From 7eb712343895e648e37f5c3d4f8c411b63a1ec24 Mon Sep 17 00:00:00 2001 From: Mudit Jindal Date: Sun, 25 Feb 2018 23:09:42 +0530 Subject: [PATCH 9/9] Add files via upload --- src/teb_local_planner_ros.cpp | 1164 +++++++++++++++++++++++++++++++++ 1 file changed, 1164 insertions(+) create mode 100644 src/teb_local_planner_ros.cpp diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp new file mode 100644 index 00000000..11e9881a --- /dev/null +++ b/src/teb_local_planner_ros.cpp @@ -0,0 +1,1164 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#include + +#include + +#include + +// pluginlib macros +#include + +#include "g2o/core/sparse_optimizer.h" +#include "g2o/core/block_solver.h" +#include "g2o/core/factory.h" +#include "g2o/core/optimization_algorithm_gauss_newton.h" +#include "g2o/core/optimization_algorithm_levenberg.h" +#include "g2o/solvers/csparse/linear_solver_csparse.h" +#include "g2o/solvers/cholmod/linear_solver_cholmod.h" + + +// register this planner as a BaseLocalPlanner plugin +PLUGINLIB_DECLARE_CLASS(teb_local_planner, TebLocalPlannerROS, teb_local_planner::TebLocalPlannerROS, nav_core::BaseLocalPlanner) + +namespace teb_local_planner +{ + + +TebLocalPlannerROS::TebLocalPlannerROS() : costmap_ros_(NULL), tf_(NULL), costmap_model_(NULL), + costmap_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"), + dynamic_recfg_(NULL), goal_reached_(false), no_infeasible_plans_(0), last_preferred_rotdir_(RotType::none), + initialized_(false) +{ +} + + +TebLocalPlannerROS::~TebLocalPlannerROS() +{ +} + +void TebLocalPlannerROS::reconfigureCB(TebLocalPlannerReconfigureConfig& config, uint32_t level) +{ + cfg_.reconfigure(config); +} + +void TebLocalPlannerROS::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) +{ + // check if the plugin is already initialized + if(!initialized_) + { + // create Node Handle with name of plugin (as used in move_base for loading) + ros::NodeHandle nh("~/" + name); + + // get parameters of TebConfig via the nodehandle and override the default config + cfg_.loadRosParamFromNodeHandle(nh); + + // reserve some memory for obstacles + obstacles_.reserve(500); + + // create visualization instance + visualization_ = TebVisualizationPtr(new TebVisualization(nh, cfg_)); + + // create robot footprint/contour model for optimization + RobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh); + + // create the planner instance + if (cfg_.hcp.enable_homotopy_class_planning) + { + planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_)); + ROS_INFO("Parallel planning in distinctive topologies enabled."); + } + else + { + planner_ = PlannerInterfacePtr(new TebOptimalPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_)); + ROS_INFO("Parallel planning in distinctive topologies disabled."); + } + + // init other variables + tf_ = tf; + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); // locking should be done in MoveBase. + + costmap_model_ = boost::make_shared(*costmap_); + + global_frame_ = costmap_ros_->getGlobalFrameID(); + cfg_.map_frame = global_frame_; // TODO + robot_base_frame_ = costmap_ros_->getBaseFrameID(); + + //Initialize a costmap to polygon converter + if (!cfg_.obstacles.costmap_converter_plugin.empty()) + { + try + { + costmap_converter_ = costmap_converter_loader_.createInstance(cfg_.obstacles.costmap_converter_plugin); + std::string converter_name = costmap_converter_loader_.getName(cfg_.obstacles.costmap_converter_plugin); + // replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace + boost::replace_all(converter_name, "::", "/"); + costmap_converter_->setOdomTopic(cfg_.odom_topic); + costmap_converter_->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name)); + costmap_converter_->setCostmap2D(costmap_); + + costmap_converter_->startWorker(ros::Rate(cfg_.obstacles.costmap_converter_rate), costmap_, cfg_.obstacles.costmap_converter_spin_thread); + ROS_INFO_STREAM("Costmap conversion plugin " << cfg_.obstacles.costmap_converter_plugin << " loaded."); + } + catch(pluginlib::PluginlibException& ex) + { + ROS_WARN("The specified costmap converter plugin cannot be loaded. All occupied costmap cells are treaten as point obstacles. Error message: %s", ex.what()); + costmap_converter_.reset(); + } + } + else + ROS_INFO("No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles."); + + + // Get footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices. + footprint_spec_ = costmap_ros_->getRobotFootprint(); + costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius); + + // init the odom helper to receive the robot's velocity from odom messages + odom_helper_.setOdomTopic(cfg_.odom_topic); + + // setup dynamic reconfigure + dynamic_recfg_ = boost::make_shared< dynamic_reconfigure::Server >(nh); + dynamic_reconfigure::Server::CallbackType cb = boost::bind(&TebLocalPlannerROS::reconfigureCB, this, _1, _2); + dynamic_recfg_->setCallback(cb); + + // validate optimization footprint and costmap footprint + validateFootprints(robot_model->getInscribedRadius(), robot_inscribed_radius_, cfg_.obstacles.min_obstacle_dist); + + // setup callback for custom obstacles + custom_obst_sub_ = nh.subscribe("obstacles", 1, &TebLocalPlannerROS::customObstacleCB, this); + + // initialize failure detector + ros::NodeHandle nh_move_base("~"); + double controller_frequency = 5; + nh_move_base.param("controller_frequency", controller_frequency, controller_frequency); + failure_detector_.setBufferLength(std::round(cfg_.recovery.oscillation_filter_duration*controller_frequency)); + + // set initialized flag + initialized_ = true; + + ROS_DEBUG("teb_local_planner plugin initialized."); + } + else + { + ROS_WARN("teb_local_planner has already been initialized, doing nothing."); + } +} + + + +bool TebLocalPlannerROS::setPlan(const std::vector& orig_global_plan) +{ + // check if plugin is initialized + if(!initialized_) + { + ROS_ERROR("teb_local_planner has not been initialized, please call initialize() before using this planner"); + return false; + } + + // store the global plan + global_plan_.clear(); + global_plan_ = orig_global_plan; + + // we do not clear the local planner here, since setPlan is called frequently whenever the global planner updates the plan. + // the local planner checks whether it is required to reinitialize the trajectory or not within each velocity computation step. + + // reset goal_reached_ flag + goal_reached_ = false; + + return true; +} + +void TebLocalPlannerROS::peakViaPoint(double localGoal_x, double localGoal_y, + std::vector& transformed_plan, double robot_circumscribed_radius) //@mudit +{ + double planLineSlope= (localGoal_y-transformed_plan[1].pose.position.y)/(localGoal_x-transformed_plan[1].pose.position.x); // slope of the line connecting trans. global plan origin and local goal + double planLine_yIntercept= transformed_plan[1].pose.position.y-(planLineSlope*transformed_plan[1].pose.position.x); // y-intercept of the line connecting robot pose and local goal + double max_dist=0; + int viaIdx; + for (std::size_t i=1; i < transformed_plan.size(); ++i) + {double x_transformed= transformed_plan[i].pose.position.x; //x1 + double y_transformed= transformed_plan[i].pose.position.y; //y1 + double c_perpendicular= y_transformed+(1/planLineSlope)*x_transformed; // y-intercept of the line connecting (x1,y1) and the perpendicular point + // meeting the line connecting robot pose and local goal + + double x2= (planLine_yIntercept-c_perpendicular)/((-1/planLineSlope)-planLineSlope); //x2 and y2 are the new points on the line connecting robot pose and local goal + double y2= (planLineSlope*x2)+planLine_yIntercept; + + double sqDist= (x2-x_transformed)*(x2-x_transformed)+(y2-y_transformed)*(y2-y_transformed); //Calculating distance b/w the two points + + if(max_dist(robot_circumscribed_radius/8)) //move_base crashes for unknown reason if max_dist=0.000000 + {via_points_.push_back( Eigen::Vector2d( transformed_plan[viaIdx].pose.position.x, transformed_plan[viaIdx].pose.position.y ) ); + // ROS_INFO("The Distance is %f",max_dist); + //cfg_.hcp.enable_multithreading=false; + // cfg_.hcp.max_number_classes=false; + } + else +{ + //cfg_.hcp.enable_multithreading=true; + // cfg_.hcp.max_number_classes=true; + }// ROS_INFO("Number : %d",cfg_.hcp.max_number_classes); // transformed_plan[i].pose.position.x, transformed_plan[i].pose.position.y +} + +bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) +{ + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("teb_local_planner has not been initialized, please call initialize() before using this planner"); + return false; + } + + cmd_vel.linear.x = 0; + cmd_vel.linear.y = 0; + cmd_vel.angular.z = 0; + goal_reached_ = false; + + // Get robot pose + tf::Stamped robot_pose; + costmap_ros_->getRobotPose(robot_pose); + robot_pose_ = PoseSE2(robot_pose); + + // Get robot velocity + tf::Stamped robot_vel_tf; + odom_helper_.getRobotVel(robot_vel_tf); + robot_vel_.linear.x = robot_vel_tf.getOrigin().getX(); + robot_vel_.linear.y = robot_vel_tf.getOrigin().getY(); + robot_vel_.angular.z = tf::getYaw(robot_vel_tf.getRotation()); + + // prune global plan to cut off parts of the past (spatially before the robot) + pruneGlobalPlan(*tf_, robot_pose, global_plan_); + + // Transform global plan to the frame of interest (w.r.t. the local costmap) + std::vector transformed_plan; + int goal_idx; + tf::StampedTransform tf_plan_to_global; + if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist, + transformed_plan, &goal_idx, &tf_plan_to_global)) + { + ROS_WARN("Could not transform the global plan to the frame of the controller"); + return false; + } + + // check if global goal is reached + tf::Stamped global_goal; + tf::poseStampedMsgToTF(global_plan_.back(), global_goal); + global_goal.setData( tf_plan_to_global * global_goal ); + double dx = global_goal.getOrigin().getX() - robot_pose_.x(); + double dy = global_goal.getOrigin().getY() - robot_pose_.y(); + double delta_orient = g2o::normalize_theta( tf::getYaw(global_goal.getRotation()) - robot_pose_.theta() ); + if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance + && fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance) + { + goal_reached_ = true; + return true; + } + + + // check if we should enter any backup mode and apply settings + configureBackupModes(transformed_plan, goal_idx); + + + // Return false if the transformed global plan is empty + if (transformed_plan.empty()) + { + ROS_WARN("Transformed plan is empty. Cannot determine a local plan."); + return false; + } + + // Get current goal point (last point of the transformed plan) + tf::Stamped goal_point; + tf::poseStampedMsgToTF(transformed_plan.back(), goal_point); + robot_goal_.x() = goal_point.getOrigin().getX(); + robot_goal_.y() = goal_point.getOrigin().getY(); + if (cfg_.trajectory.global_plan_overwrite_orientation) + { + robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, goal_point, goal_idx, tf_plan_to_global); + // overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization) + transformed_plan.back().pose.orientation = tf::createQuaternionMsgFromYaw(robot_goal_.theta()); + } + else + { + robot_goal_.theta() = tf::getYaw(goal_point.getRotation()); + } + + // overwrite/update start of the transformed plan with the actual robot position (allows using the plan as initial trajectory) + if (transformed_plan.size()==1) // plan only contains the goal + { + transformed_plan.insert(transformed_plan.begin(), geometry_msgs::PoseStamped()); // insert start (not yet initialized) + } + tf::poseTFToMsg(robot_pose, transformed_plan.front().pose); // update start; + + // clear currently existing obstacles + obstacles_.clear(); + + // Update obstacle container with costmap information or polygons provided by a costmap_converter plugin + if (costmap_converter_) + updateObstacleContainerWithCostmapConverter(); + else + updateObstacleContainerWithCostmap(); + + // also consider custom obstacles (must be called after other updates, since the container is not cleared) + updateObstacleContainerWithCustomObstacles(); + + // update via-points container + updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep); + + //@mudit edit : creating a via point for including peak points of the global plan in the robot's path + if(cfg_.trajectory.peak_via_points) + peakViaPoint(robot_goal_.x(),robot_goal_.y(),transformed_plan,robot_circumscribed_radius); + + // Do not allow config changes during the following optimization step + boost::mutex::scoped_lock cfg_lock(cfg_.configMutex()); + + // Now perform the actual planning +// bool success = planner_->plan(robot_pose_, robot_goal_, robot_vel_, cfg_.goal_tolerance.free_goal_vel); // straight line init + bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel); + if (!success) + { + planner_->clearPlanner(); // force reinitialization for next time + ROS_WARN("teb_local_planner was not able to obtain a local plan for the current setting."); + + ++no_infeasible_plans_; // increase number of infeasible solutions in a row + time_last_infeasible_plan_ = ros::Time::now(); + last_cmd_ = cmd_vel; + return false; + } + + // Check feasibility (but within the first few states only) + if(cfg_.robot.is_footprint_dynamic) + { + // Update footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices. + footprint_spec_ = costmap_ros_->getRobotFootprint(); + costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius); + } + + bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.trajectory.feasibility_check_no_poses); + if (!feasible) + { + cmd_vel.linear.x = 0; + cmd_vel.linear.y = 0; + cmd_vel.angular.z = 0; + + // now we reset everything to start again with the initialization of new trajectories. + planner_->clearPlanner(); + ROS_WARN("TebLocalPlannerROS: trajectory is not feasible. Resetting planner..."); + + ++no_infeasible_plans_; // increase number of infeasible solutions in a row + time_last_infeasible_plan_ = ros::Time::now(); + last_cmd_ = cmd_vel; + return false; + } + + // Get the velocity command for this sampling interval + if (!planner_->getVelocityCommand(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z)) + { + planner_->clearPlanner(); + ROS_WARN("TebLocalPlannerROS: velocity command invalid. Resetting planner..."); + ++no_infeasible_plans_; // increase number of infeasible solutions in a row + time_last_infeasible_plan_ = ros::Time::now(); + last_cmd_ = cmd_vel; + return false; + } + + // Saturate velocity, if the optimization results violates the constraints (could be possible due to soft constraints). + saturateVelocity(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z, cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, + cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards); + + // convert rot-vel to steering angle if desired (carlike robot). + // The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint + // and opposed to the other constraints not affected by penalty_epsilon. The user might add a safety margin to the parameter itself. + if (cfg_.robot.cmd_angle_instead_rotvel) + { + cmd_vel.angular.z = convertTransRotVelToSteeringAngle(cmd_vel.linear.x, cmd_vel.angular.z, cfg_.robot.wheelbase, 0.95*cfg_.robot.min_turning_radius); + if (!std::isfinite(cmd_vel.angular.z)) + { + cmd_vel.linear.x = cmd_vel.linear.y = cmd_vel.angular.z = 0; + last_cmd_ = cmd_vel; + planner_->clearPlanner(); + ROS_WARN("TebLocalPlannerROS: Resulting steering angle is not finite. Resetting planner..."); + ++no_infeasible_plans_; // increase number of infeasible solutions in a row + time_last_infeasible_plan_ = ros::Time::now(); + return false; + } + } + + // a feasible solution should be found, reset counter + no_infeasible_plans_ = 0; + + // store last command (for recovery analysis etc.) + last_cmd_ = cmd_vel; + + // Now visualize everything + planner_->visualize(); + visualization_->publishObstacles(obstacles_); + visualization_->publishViaPoints(via_points_); + visualization_->publishGlobalPlan(global_plan_); + return true; +} + + +bool TebLocalPlannerROS::isGoalReached() +{ + if (goal_reached_) + { + ROS_INFO("GOAL Reached!"); + planner_->clearPlanner(); + return true; + } + return false; +} + + + +void TebLocalPlannerROS::updateObstacleContainerWithCostmap() +{ + // Add costmap obstacles if desired + if (cfg_.obstacles.include_costmap_obstacles) + { + Eigen::Vector2d robot_orient = robot_pose_.orientationUnitVec(); + + for (unsigned int i=0; igetSizeInCellsX()-1; ++i) + { + for (unsigned int j=0; jgetSizeInCellsY()-1; ++j) + { + if (costmap_->getCost(i,j) == costmap_2d::LETHAL_OBSTACLE) + { + Eigen::Vector2d obs; + costmap_->mapToWorld(i,j,obs.coeffRef(0), obs.coeffRef(1)); + + // check if obstacle is interesting (e.g. not far behind the robot) + Eigen::Vector2d obs_dir = obs-robot_pose_.position(); + if ( obs_dir.dot(robot_orient) < 0 && obs_dir.norm() > cfg_.obstacles.costmap_obstacles_behind_robot_dist ) + continue; + + obstacles_.push_back(ObstaclePtr(new PointObstacle(obs))); + } + } + } + } +} + +void TebLocalPlannerROS::updateObstacleContainerWithCostmapConverter() +{ + if (!costmap_converter_) + return; + + //Get obstacles from costmap converter + costmap_converter::ObstacleArrayConstPtr obstacles = costmap_converter_->getObstacles(); + if (!obstacles) + return; + + for (std::size_t i=0; iobstacles.size(); ++i) + { + const geometry_msgs::Polygon* polygon = &obstacles->obstacles.at(i).polygon; + + if (polygon->points.size()==1) // Point + { + obstacles_.push_back(ObstaclePtr(new PointObstacle(polygon->points[0].x, polygon->points[0].y))); + } + else if (polygon->points.size()==2) // Line + { + obstacles_.push_back(ObstaclePtr(new LineObstacle(polygon->points[0].x, polygon->points[0].y, + polygon->points[1].x, polygon->points[1].y ))); + } + else if (polygon->points.size()>2) // Real polygon + { + PolygonObstacle* polyobst = new PolygonObstacle; + for (std::size_t j=0; jpoints.size(); ++j) + { + polyobst->pushBackVertex(polygon->points[j].x, polygon->points[j].y); + } + polyobst->finalizePolygon(); + obstacles_.push_back(ObstaclePtr(polyobst)); + } + + // Set velocity, if obstacle is moving + if(!obstacles_.empty()) + obstacles_.back()->setCentroidVelocity(obstacles->obstacles[i].velocities, obstacles->obstacles[i].orientation); + } +} + + +void TebLocalPlannerROS::updateObstacleContainerWithCustomObstacles() +{ + // Add custom obstacles obtained via message + boost::mutex::scoped_lock l(custom_obst_mutex_); + + if (!custom_obstacle_msg_.obstacles.empty()) + { + // We only use the global header to specify the obstacle coordinate system instead of individual ones + Eigen::Affine3d obstacle_to_map_eig; + try + { + tf::StampedTransform obstacle_to_map; + tf_->waitForTransform(global_frame_, ros::Time(0), + custom_obstacle_msg_.header.frame_id, ros::Time(0), + custom_obstacle_msg_.header.frame_id, ros::Duration(0.5)); + tf_->lookupTransform(global_frame_, ros::Time(0), + custom_obstacle_msg_.header.frame_id, ros::Time(0), + custom_obstacle_msg_.header.frame_id, obstacle_to_map); + tf::transformTFToEigen(obstacle_to_map, obstacle_to_map_eig); + } + catch (tf::TransformException ex) + { + ROS_ERROR("%s",ex.what()); + obstacle_to_map_eig.setIdentity(); + } + + for (size_t i=0; ipushBackVertex( (obstacle_to_map_eig * pos).head(2) ); + } + polyobst->finalizePolygon(); + obstacles_.push_back(ObstaclePtr(polyobst)); + } + + // Set velocity, if obstacle is moving + if(!obstacles_.empty()) + obstacles_.back()->setCentroidVelocity(custom_obstacle_msg_.obstacles[i].velocities, custom_obstacle_msg_.obstacles[i].orientation); + } + } +} + +void TebLocalPlannerROS::updateViaPointsContainer(const std::vector& transformed_plan, double min_separation) +{ + via_points_.clear(); + + if (min_separation<0) + return; + + std::size_t prev_idx = 0; + for (std::size_t i=1; i < transformed_plan.size(); ++i) // skip first one, since we do not need any point before the first min_separation [m] + { + // check separation to the previous via-point inserted + if (distance_points2d( transformed_plan[prev_idx].pose.position, transformed_plan[i].pose.position ) < min_separation) + continue; + + // add via-point + via_points_.push_back( Eigen::Vector2d( transformed_plan[i].pose.position.x, transformed_plan[i].pose.position.y ) ); + prev_idx = i; + } + +} + +Eigen::Vector2d TebLocalPlannerROS::tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel) +{ + Eigen::Vector2d vel; + vel.coeffRef(0) = std::sqrt( tf_vel.getOrigin().getX() * tf_vel.getOrigin().getX() + tf_vel.getOrigin().getY() * tf_vel.getOrigin().getY() ); + vel.coeffRef(1) = tf::getYaw(tf_vel.getRotation()); + return vel; +} + + +bool TebLocalPlannerROS::pruneGlobalPlan(const tf::TransformListener& tf, const tf::Stamped& global_pose, std::vector& global_plan, double dist_behind_robot) +{ + if (global_plan.empty()) + return true; + + try + { + // transform robot pose into the plan frame (we do not wait here, since pruning not crucial, if missed a few times) + tf::StampedTransform global_to_plan_transform; + tf.lookupTransform(global_plan.front().header.frame_id, global_pose.frame_id_, ros::Time(0), global_to_plan_transform); + tf::Stamped robot; + robot.setData( global_to_plan_transform * global_pose ); + + double dist_thresh_sq = dist_behind_robot*dist_behind_robot; + + // iterate plan until a pose close the robot is found + std::vector::iterator it = global_plan.begin(); + std::vector::iterator erase_end = it; + while (it != global_plan.end()) + { + double dx = robot.getOrigin().x() - it->pose.position.x; + double dy = robot.getOrigin().y() - it->pose.position.y; + double dist_sq = dx * dx + dy * dy; + if (dist_sq < dist_thresh_sq) + { + erase_end = it; + break; + } + ++it; + } + if (erase_end == global_plan.end()) + return false; + + if (erase_end != global_plan.begin()) + global_plan.erase(global_plan.begin(), erase_end); + } + catch (const tf::TransformException& ex) + { + ROS_DEBUG("Cannot prune path since no transform is available: %s\n", ex.what()); + return false; + } + return true; +} + + +bool TebLocalPlannerROS::transformGlobalPlan(const tf::TransformListener& tf, const std::vector& global_plan, + const tf::Stamped& global_pose, const costmap_2d::Costmap2D& costmap, const std::string& global_frame, double max_plan_length, + std::vector& transformed_plan, int* current_goal_idx, tf::StampedTransform* tf_plan_to_global) const +{ + // this method is a slightly modified version of base_local_planner/goal_functions.h + + const geometry_msgs::PoseStamped& plan_pose = global_plan[0]; + + transformed_plan.clear(); + + try + { + if (global_plan.empty()) + { + ROS_ERROR("Received plan with zero length"); + *current_goal_idx = 0; + return false; + } + + // get plan_to_global_transform from plan frame to global_frame + tf::StampedTransform plan_to_global_transform; + tf.waitForTransform(global_frame, ros::Time::now(), + plan_pose.header.frame_id, plan_pose.header.stamp, + plan_pose.header.frame_id, ros::Duration(0.5)); + tf.lookupTransform(global_frame, ros::Time(), + plan_pose.header.frame_id, plan_pose.header.stamp, + plan_pose.header.frame_id, plan_to_global_transform); + + //let's get the pose of the robot in the frame of the plan + tf::Stamped robot_pose; + tf.transformPose(plan_pose.header.frame_id, global_pose, robot_pose); + + //we'll discard points on the plan that are outside the local costmap + double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0, + costmap.getSizeInCellsY() * costmap.getResolution() / 2.0); + dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are + // located on the border of the local costmap + + + int i = 0; + double sq_dist_threshold = dist_threshold * dist_threshold; + double sq_dist = 1e10; + + //we need to loop to a point on the plan that is within a certain distance of the robot + while(i < (int)global_plan.size()) + { + double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x; + double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y; + double new_sq_dist = x_diff * x_diff + y_diff * y_diff; + if (new_sq_dist > sq_dist && sq_dist < sq_dist_threshold) // find first distance that is greater + { + sq_dist = new_sq_dist; + break; + } + sq_dist = new_sq_dist; + ++i; + } + + tf::Stamped tf_pose; + geometry_msgs::PoseStamped newer_pose; + + double plan_length = 0; // check cumulative Euclidean distance along the plan + + //now we'll transform until points are outside of our distance threshold + while(i < (int)global_plan.size() && sq_dist <= sq_dist_threshold && (max_plan_length<=0 || plan_length <= max_plan_length)) + { + const geometry_msgs::PoseStamped& pose = global_plan[i]; + tf::poseStampedMsgToTF(pose, tf_pose); + tf_pose.setData(plan_to_global_transform * tf_pose); + tf_pose.stamp_ = plan_to_global_transform.stamp_; + tf_pose.frame_id_ = global_frame; + tf::poseStampedTFToMsg(tf_pose, newer_pose); + + transformed_plan.push_back(newer_pose); + + double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x; + double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y; + sq_dist = x_diff * x_diff + y_diff * y_diff; + + // caclulate distance to previous pose + if (i>0 && max_plan_length>0) + plan_length += distance_points2d(global_plan[i-1].pose.position, global_plan[i].pose.position); + + ++i; + } + + // if we are really close to the goal (>0) + // the resulting transformed plan can be empty. In that case we explicitly inject the global goal. + if (transformed_plan.empty()) + { + tf::poseStampedMsgToTF(global_plan.back(), tf_pose); + tf_pose.setData(plan_to_global_transform * tf_pose); + tf_pose.stamp_ = plan_to_global_transform.stamp_; + tf_pose.frame_id_ = global_frame; + tf::poseStampedTFToMsg(tf_pose, newer_pose); + + transformed_plan.push_back(newer_pose); + + // Return the index of the current goal point (inside the distance threshold) + if (current_goal_idx) *current_goal_idx = int(global_plan.size())-1; + } + else + { + // Return the index of the current goal point (inside the distance threshold) + if (current_goal_idx) *current_goal_idx = i-1; // subtract 1, since i was increased once before leaving the loop + } + + // Return the transformation from the global plan to the global planning frame if desired + if (tf_plan_to_global) *tf_plan_to_global = plan_to_global_transform; + } + catch(tf::LookupException& ex) + { + ROS_ERROR("No Transform available Error: %s\n", ex.what()); + return false; + } + catch(tf::ConnectivityException& ex) + { + ROS_ERROR("Connectivity Error: %s\n", ex.what()); + return false; + } + catch(tf::ExtrapolationException& ex) + { + ROS_ERROR("Extrapolation Error: %s\n", ex.what()); + if (global_plan.size() > 0) + ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str()); + + return false; + } + + return true; +} + + + + +double TebLocalPlannerROS::estimateLocalGoalOrientation(const std::vector& global_plan, const tf::Stamped& local_goal, + int current_goal_idx, const tf::StampedTransform& tf_plan_to_global, int moving_average_length) const +{ + int n = (int)global_plan.size(); + + // check if we are near the global goal already + if (current_goal_idx > n-moving_average_length-2) + { + if (current_goal_idx >= n-1) // we've exactly reached the goal + { + return tf::getYaw(local_goal.getRotation()); + } + else + { + tf::Quaternion global_orientation; + tf::quaternionMsgToTF(global_plan.back().pose.orientation, global_orientation); + return tf::getYaw(tf_plan_to_global.getRotation() * global_orientation ); + } + } + + // reduce number of poses taken into account if the desired number of poses is not available + moving_average_length = std::min(moving_average_length, n-current_goal_idx-1 ); // maybe redundant, since we have checked the vicinity of the goal before + + std::vector candidates; + tf::Stamped tf_pose_k = local_goal; + tf::Stamped tf_pose_kp1; + + int range_end = current_goal_idx + moving_average_length; + for (int i = current_goal_idx; i < range_end; ++i) + { + // Transform pose of the global plan to the planning frame + const geometry_msgs::PoseStamped& pose = global_plan.at(i+1); + tf::poseStampedMsgToTF(pose, tf_pose_kp1); + tf_pose_kp1.setData(tf_plan_to_global * tf_pose_kp1); + + // calculate yaw angle + candidates.push_back( std::atan2(tf_pose_kp1.getOrigin().getY() - tf_pose_k.getOrigin().getY(), + tf_pose_kp1.getOrigin().getX() - tf_pose_k.getOrigin().getX() ) ); + + if (i max_vel_x) + vx = max_vel_x; + + // limit strafing velocity + if (vy > max_vel_y) + vy = max_vel_y; + else if (vy < -max_vel_y) + vy = -max_vel_y; + + // Limit angular velocity + if (omega > max_vel_theta) + omega = max_vel_theta; + else if (omega < -max_vel_theta) + omega = -max_vel_theta; + + // Limit backwards velocity + if (max_vel_x_backwards<=0) + { + ROS_WARN_ONCE("TebLocalPlannerROS(): Do not choose max_vel_x_backwards to be <=0. Disable backwards driving by increasing the optimization weight for penalyzing backwards driving."); + } + else if (vx < -max_vel_x_backwards) + vx = -max_vel_x_backwards; +} + + +double TebLocalPlannerROS::convertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius) const +{ + if (omega==0 || v==0) + return 0; + + double radius = v/omega; + + if (fabs(radius) < min_turning_radius) + radius = double(g2o::sign(radius)) * min_turning_radius; + + return std::atan(wheelbase / radius); +} + + +void TebLocalPlannerROS::validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist) +{ + ROS_WARN_COND(opt_inscribed_radius + min_obst_dist < costmap_inscribed_radius, + "The inscribed radius of the footprint specified for TEB optimization (%f) + min_obstacle_dist (%f) are smaller " + "than the inscribed radius of the robot's footprint in the costmap parameters (%f, including 'footprint_padding'). " + "Infeasible optimziation results might occur frequently!", opt_inscribed_radius, min_obst_dist, costmap_inscribed_radius); +} + + + +void TebLocalPlannerROS::configureBackupModes(std::vector& transformed_plan, int& goal_idx) +{ + ros::Time current_time = ros::Time::now(); + + // reduced horizon backup mode + if (cfg_.recovery.shrink_horizon_backup && + goal_idx < (int)transformed_plan.size()-1 && // we do not reduce if the goal is already selected (because the orientation might change -> can introduce oscillations) + (no_infeasible_plans_>0 || (current_time - time_last_infeasible_plan_).toSec() < cfg_.recovery.shrink_horizon_min_duration )) // keep short horizon for at least a few seconds + { + ROS_INFO_COND(no_infeasible_plans_==1, "Activating reduced horizon backup mode for at least %.2f sec (infeasible trajectory detected).", cfg_.recovery.shrink_horizon_min_duration); + + + // Shorten horizon if requested + // reduce to 50 percent: + int horizon_reduction = goal_idx/2; + + if (no_infeasible_plans_ > 9) + { + ROS_INFO_COND(no_infeasible_plans_==10, "Infeasible trajectory detected 10 times in a row: further reducing horizon..."); + horizon_reduction /= 2; + } + + // we have a small overhead here, since we already transformed 50% more of the trajectory. + // But that's ok for now, since we do not need to make transformGlobalPlan more complex + // and a reduced horizon should occur just rarely. + int new_goal_idx_transformed_plan = int(transformed_plan.size()) - horizon_reduction - 1; + goal_idx -= horizon_reduction; + if (new_goal_idx_transformed_plan>0 && goal_idx >= 0) + transformed_plan.erase(transformed_plan.begin()+new_goal_idx_transformed_plan, transformed_plan.end()); + else + goal_idx += horizon_reduction; // this should not happen, but safety first ;-) + } + + + // detect and resolve oscillations + if (cfg_.recovery.oscillation_recovery) + { + double max_vel_theta; + double max_vel_current = last_cmd_.linear.x >= 0 ? cfg_.robot.max_vel_x : cfg_.robot.max_vel_x_backwards; + if (cfg_.robot.min_turning_radius!=0 && max_vel_current>0) + max_vel_theta = std::max( max_vel_current/std::abs(cfg_.robot.min_turning_radius), cfg_.robot.max_vel_theta ); + else + max_vel_theta = cfg_.robot.max_vel_theta; + + failure_detector_.update(last_cmd_, cfg_.robot.max_vel_x, cfg_.robot.max_vel_x_backwards, max_vel_theta, + cfg_.recovery.oscillation_v_eps, cfg_.recovery.oscillation_omega_eps); + + bool oscillating = failure_detector_.isOscillating(); + bool recently_oscillated = (ros::Time::now()-time_last_oscillation_).toSec() < cfg_.recovery.oscillation_recovery_min_duration; // check if we have already detected an oscillation recently + + if (oscillating) + { + if (!recently_oscillated) + { + // save current turning direction + if (robot_vel_.angular.z > 0) + last_preferred_rotdir_ = RotType::left; + else + last_preferred_rotdir_ = RotType::right; + ROS_WARN("TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization)."); + } + time_last_oscillation_ = ros::Time::now(); + planner_->setPreferredTurningDir(last_preferred_rotdir_); + } + else if (!recently_oscillated && last_preferred_rotdir_ != RotType::none) // clear recovery behavior + { + last_preferred_rotdir_ = RotType::none; + planner_->setPreferredTurningDir(last_preferred_rotdir_); + ROS_INFO("TebLocalPlannerROS: oscillation recovery disabled/expired."); + } + } + +} + + +void TebLocalPlannerROS::customObstacleCB(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg) +{ + boost::mutex::scoped_lock l(custom_obst_mutex_); + custom_obstacle_msg_ = *obst_msg; +} + +RobotFootprintModelPtr TebLocalPlannerROS::getRobotFootprintFromParamServer(const ros::NodeHandle& nh) +{ + std::string model_name; + if (!nh.getParam("footprint_model/type", model_name)) + { + ROS_INFO("No robot footprint model specified for trajectory optimization. Using point-shaped model."); + return boost::make_shared(); + } + + // point + if (model_name.compare("point") == 0) + { + ROS_INFO("Footprint model 'point' loaded for trajectory optimization."); + return boost::make_shared(); + } + + // circular + if (model_name.compare("circular") == 0) + { + // get radius + double radius; + if (!nh.getParam("footprint_model/radius", radius)) + { + ROS_ERROR_STREAM("Footprint model 'circular' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() + << "/footprint_model/radius' does not exist. Using point-model instead."); + return boost::make_shared(); + } + ROS_INFO_STREAM("Footprint model 'circular' (radius: " << radius <<"m) loaded for trajectory optimization."); + return boost::make_shared(radius); + } + + // line + if (model_name.compare("line") == 0) + { + // check parameters + if (!nh.hasParam("footprint_model/line_start") || !nh.hasParam("footprint_model/line_end")) + { + ROS_ERROR_STREAM("Footprint model 'line' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() + << "/footprint_model/line_start' and/or '.../line_end' do not exist. Using point-model instead."); + return boost::make_shared(); + } + // get line coordinates + std::vector line_start, line_end; + nh.getParam("footprint_model/line_start", line_start); + nh.getParam("footprint_model/line_end", line_end); + if (line_start.size() != 2 || line_end.size() != 2) + { + ROS_ERROR_STREAM("Footprint model 'line' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() + << "/footprint_model/line_start' and/or '.../line_end' do not contain x and y coordinates (2D). Using point-model instead."); + return boost::make_shared(); + } + + ROS_INFO_STREAM("Footprint model 'line' (line_start: [" << line_start[0] << "," << line_start[1] <<"]m, line_end: [" + << line_end[0] << "," << line_end[1] << "]m) loaded for trajectory optimization."); + return boost::make_shared(Eigen::Map(line_start.data()), Eigen::Map(line_end.data())); + } + + // two circles + if (model_name.compare("two_circles") == 0) + { + // check parameters + if (!nh.hasParam("footprint_model/front_offset") || !nh.hasParam("footprint_model/front_radius") + || !nh.hasParam("footprint_model/rear_offset") || !nh.hasParam("footprint_model/rear_radius")) + { + ROS_ERROR_STREAM("Footprint model 'two_circles' cannot be loaded for trajectory optimization, since params '" << nh.getNamespace() + << "/footprint_model/front_offset', '.../front_radius', '.../rear_offset' and '.../rear_radius' do not exist. Using point-model instead."); + return boost::make_shared(); + } + double front_offset, front_radius, rear_offset, rear_radius; + nh.getParam("footprint_model/front_offset", front_offset); + nh.getParam("footprint_model/front_radius", front_radius); + nh.getParam("footprint_model/rear_offset", rear_offset); + nh.getParam("footprint_model/rear_radius", rear_radius); + ROS_INFO_STREAM("Footprint model 'two_circles' (front_offset: " << front_offset <<"m, front_radius: " << front_radius + << "m, rear_offset: " << rear_offset << "m, rear_radius: " << rear_radius << "m) loaded for trajectory optimization."); + return boost::make_shared(front_offset, front_radius, rear_offset, rear_radius); + } + + // polygon + if (model_name.compare("polygon") == 0) + { + + // check parameters + XmlRpc::XmlRpcValue footprint_xmlrpc; + if (!nh.getParam("footprint_model/vertices", footprint_xmlrpc) ) + { + ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() + << "/footprint_model/vertices' does not exist. Using point-model instead."); + return boost::make_shared(); + } + // get vertices + if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray) + { + try + { + Point2dContainer polygon = makeFootprintFromXMLRPC(footprint_xmlrpc, "/footprint_model/vertices"); + ROS_INFO_STREAM("Footprint model 'polygon' loaded for trajectory optimization."); + return boost::make_shared(polygon); + } + catch(const std::exception& ex) + { + ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization: " << ex.what() << ". Using point-model instead."); + return boost::make_shared(); + } + } + else + { + ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() + << "/footprint_model/vertices' does not define an array of coordinates. Using point-model instead."); + return boost::make_shared(); + } + + } + + // otherwise + ROS_WARN_STREAM("Unknown robot footprint model specified with parameter '" << nh.getNamespace() << "/footprint_model/type'. Using point model instead."); + return boost::make_shared(); +} + + + + +Point2dContainer TebLocalPlannerROS::makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name) +{ + // Make sure we have an array of at least 3 elements. + if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray || + footprint_xmlrpc.size() < 3) + { + ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s", + full_param_name.c_str(), std::string(footprint_xmlrpc).c_str()); + throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least " + "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]"); + } + + Point2dContainer footprint; + Eigen::Vector2d pt; + + for (int i = 0; i < footprint_xmlrpc.size(); ++i) + { + // Make sure each element of the list is an array of size 2. (x and y coordinates) + XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ]; + if (point.getType() != XmlRpc::XmlRpcValue::TypeArray || + point.size() != 2) + { + ROS_FATAL("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: " + "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.", + full_param_name.c_str()); + throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: " + "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form"); + } + + pt.x() = getNumberFromXMLRPC(point[ 0 ], full_param_name); + pt.y() = getNumberFromXMLRPC(point[ 1 ], full_param_name); + + footprint.push_back(pt); + } + return footprint; +} + +double TebLocalPlannerROS::getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name) +{ + // Make sure that the value we're looking at is either a double or an int. + if (value.getType() != XmlRpc::XmlRpcValue::TypeInt && + value.getType() != XmlRpc::XmlRpcValue::TypeDouble) + { + std::string& value_string = value; + ROS_FATAL("Values in the footprint specification (param %s) must be numbers. Found value %s.", + full_param_name.c_str(), value_string.c_str()); + throw std::runtime_error("Values in the footprint specification must be numbers"); + } + return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value); +} + +} // end namespace teb_local_planner + +