From 71a7249778d8e78f7f515712d901cec943ea125d Mon Sep 17 00:00:00 2001 From: "Loew, Tobi (Data61, Pullenvale)" Date: Thu, 8 Nov 2018 13:45:15 +1000 Subject: [PATCH] new edges for carlike robots --- .../g2o_types/edge_acceleration.h | 1040 ++++---- .../g2o_types/edge_kinematics.h | 332 ++- .../g2o_types/edge_obstacle.h | 313 +-- .../g2o_types/edge_velocity.h | 254 +- .../teb_local_planner/g2o_types/penalties.h | 295 ++- include/teb_local_planner/teb_config.h | 728 ++--- src/optimal_planner.cpp | 2355 ++++++++--------- src/teb_config.cpp | 606 ++--- 8 files changed, 2970 insertions(+), 2953 deletions(-) diff --git a/include/teb_local_planner/g2o_types/edge_acceleration.h b/include/teb_local_planner/g2o_types/edge_acceleration.h index 8eccf6de..39a6164c 100644 --- a/include/teb_local_planner/g2o_types/edge_acceleration.h +++ b/include/teb_local_planner/g2o_types/edge_acceleration.h @@ -32,7 +32,7 @@ * 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. - * + * * Notes: * The following class is derived from a class defined by the * g2o-framework. g2o is licensed under the terms of the BSD License. @@ -44,112 +44,108 @@ #ifndef EDGE_ACCELERATION_H_ #define EDGE_ACCELERATION_H_ +#include +#include #include #include -#include #include -#include #include - - namespace teb_local_planner { - -/** - * @class EdgeAcceleration - * @brief Edge defining the cost function for limiting the translational and rotational acceleration. - * - * The edge depends on five vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2}, \Delta T_i, \Delta T_{ip1} \f$ and minimizes: - * \f$ \min \textrm{penaltyInterval}( [a, omegadot } ]^T ) \cdot weight \f$. \n - * \e a is calculated using the difference quotient (twice) and the position parts of all three poses \n - * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n - * \e weight can be set using setInformation() \n - * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n - * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and - * the second one the rotational acceleration. - * @see TebOptimalPlanner::AddEdgesAcceleration - * @see EdgeAccelerationStart - * @see EdgeAccelerationGoal - * @remarks Do not forget to call setTebConfig() - * @remarks Refer to EdgeAccelerationStart() and EdgeAccelerationGoal() for defining boundary values! - */ -class EdgeAcceleration : public BaseTebMultiEdge<2, double> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeAcceleration() - { - this->resize(5); - } - - /** - * @brief Actual cost function - */ - void computeError() - { - ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()"); - const VertexPose* pose1 = static_cast(_vertices[0]); - const VertexPose* pose2 = static_cast(_vertices[1]); - const VertexPose* pose3 = static_cast(_vertices[2]); - const VertexTimeDiff* dt1 = static_cast(_vertices[3]); - const VertexTimeDiff* dt2 = static_cast(_vertices[4]); - - // VELOCITY & ACCELERATION - const Eigen::Vector2d diff1 = pose2->position() - pose1->position(); - const Eigen::Vector2d diff2 = pose3->position() - pose2->position(); - - double dist1 = diff1.norm(); - double dist2 = diff2.norm(); - const double angle_diff1 = g2o::normalize_theta(pose2->theta() - pose1->theta()); - const double angle_diff2 = g2o::normalize_theta(pose3->theta() - pose2->theta()); - - if (cfg_->trajectory.exact_arc_length) // use exact arc length instead of Euclidean approximation + /** + * @class EdgeAcceleration + * @brief Edge defining the cost function for limiting the translational and rotational acceleration. + * + * The edge depends on five vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2}, \Delta T_i, \Delta T_{ip1} \f$ and minimizes: + * \f$ \min \textrm{penaltyInterval}( [a, omegadot } ]^T ) \cdot weight \f$. \n + * \e a is calculated using the difference quotient (twice) and the position parts of all three poses \n + * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n + * \e weight can be set using setInformation() \n + * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n + * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and + * the second one the rotational acceleration. + * @see TebOptimalPlanner::AddEdgesAcceleration + * @see EdgeAccelerationStart + * @see EdgeAccelerationGoal + * @remarks Do not forget to call setTebConfig() + * @remarks Refer to EdgeAccelerationStart() and EdgeAccelerationGoal() for defining boundary values! + */ + class EdgeAcceleration : public BaseTebMultiEdge<3, double> { - if (angle_diff1 != 0) - { - const double radius = dist1/(2*sin(angle_diff1/2)); - dist1 = fabs( angle_diff1 * radius ); // actual arg length! - } - if (angle_diff2 != 0) + public: + /** + * @brief Construct edge. + */ + EdgeAcceleration() { this->resize(5); } + + /** + * @brief Actual cost function + */ + void computeError() { - const double radius = dist2/(2*sin(angle_diff2/2)); - dist2 = fabs( angle_diff2 * radius ); // actual arg length! + ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()"); + const VertexPose *pose1 = static_cast(_vertices[0]); + const VertexPose *pose2 = static_cast(_vertices[1]); + const VertexPose *pose3 = static_cast(_vertices[2]); + const VertexTimeDiff *dt1 = static_cast(_vertices[3]); + const VertexTimeDiff *dt2 = static_cast(_vertices[4]); + + // VELOCITY & ACCELERATION + const Eigen::Vector2d diff1 = pose2->position() - pose1->position(); + const Eigen::Vector2d diff2 = pose3->position() - pose2->position(); + + double dist1 = diff1.norm(); + double dist2 = diff2.norm(); + const double angle_diff1 = g2o::normalize_theta(pose2->theta() - pose1->theta()); + const double angle_diff2 = g2o::normalize_theta(pose3->theta() - pose2->theta()); + + if (cfg_->trajectory.exact_arc_length) // use exact arc length instead of Euclidean approximation + { + if (angle_diff1 != 0) + { + const double radius = dist1 / (2 * sin(angle_diff1 / 2)); + dist1 = fabs(angle_diff1 * radius); // actual arg length! + } + if (angle_diff2 != 0) + { + const double radius = dist2 / (2 * sin(angle_diff2 / 2)); + dist2 = fabs(angle_diff2 * radius); // actual arg length! + } + } + + double vel1 = dist1 / dt1->dt(); + double vel2 = dist2 / dt2->dt(); + + // consider directions + // vel1 *= g2o::sign(diff1[0]*cos(pose1->theta()) + diff1[1]*sin(pose1->theta())); + // vel2 *= g2o::sign(diff2[0]*cos(pose2->theta()) + diff2[1]*sin(pose2->theta())); + vel1 *= fast_sigmoid(100 * (diff1.x() * cos(pose1->theta()) + diff1.y() * sin(pose1->theta()))); + vel2 *= fast_sigmoid(100 * (diff2.x() * cos(pose2->theta()) + diff2.y() * sin(pose2->theta()))); + + const double acc_lin = (vel2 - vel1) * 2 / (dt1->dt() + dt2->dt()); + + _error[0] = penaltyBoundToInterval(acc_lin, cfg_->robot.acc_lim_x, cfg_->optim.penalty_epsilon); + + // ANGULAR ACCELERATION + const double omega1 = angle_diff1 / dt1->dt(); + const double omega2 = angle_diff2 / dt2->dt(); + const double acc_rot = (omega2 - omega1) * 2 / (dt1->dt() + dt2->dt()); + + _error[1] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon); + + // STEERING RATE + const double steering_angle1 = std::atan(cfg_->robot.wheelbase / (vel1 / omega1)); + const double steering_angle2 = std::atan(cfg_->robot.wheelbase / (vel2 / omega2)); + const double steering_rate = (steering_angle2 - steering_angle1) * 2 / (dt1->dt() + dt2->dt()); + + _error[2] = penaltyBoundToInterval(steering_rate, cfg_->robot.max_steering_rate, cfg_->optim.penalty_epsilon); + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n", _error[0]); + ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() rotational: _error[1]=%f\n", _error[1]); + ROS_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationStart::computeError() rotational: _error[2]=%f\n", _error[2]); } - } - - double vel1 = dist1 / dt1->dt(); - double vel2 = dist2 / dt2->dt(); - - - // consider directions -// vel1 *= g2o::sign(diff1[0]*cos(pose1->theta()) + diff1[1]*sin(pose1->theta())); -// vel2 *= g2o::sign(diff2[0]*cos(pose2->theta()) + diff2[1]*sin(pose2->theta())); - vel1 *= fast_sigmoid( 100*(diff1.x()*cos(pose1->theta()) + diff1.y()*sin(pose1->theta())) ); - vel2 *= fast_sigmoid( 100*(diff2.x()*cos(pose2->theta()) + diff2.y()*sin(pose2->theta())) ); - - const double acc_lin = (vel2 - vel1)*2 / ( dt1->dt() + dt2->dt() ); - - - _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon); - - // ANGULAR ACCELERATION - const double omega1 = angle_diff1 / dt1->dt(); - const double omega2 = angle_diff2 / dt2->dt(); - const double acc_rot = (omega2 - omega1)*2 / ( dt1->dt() + dt2->dt() ); - - _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon); - - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]); - ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() rotational: _error[1]=%f\n",_error[1]); - } - - #ifdef USE_ANALYTIC_JACOBI #if 0 @@ -262,473 +258,445 @@ class EdgeAcceleration : public BaseTebMultiEdge<2, double> #endif #endif - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - -/** - * @class EdgeAccelerationStart - * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory. - * - * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setInitialVelocity() - * and minimizes: \n - * \f$ \min \textrm{penaltyInterval}( [a, omegadot ]^T ) \cdot weight \f$. \n - * \e a is calculated using the difference quotient (twice) and the position parts of the poses. \n - * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n - * \e weight can be set using setInformation(). \n - * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n - * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and - * the second one the rotational acceleration. - * @see TebOptimalPlanner::AddEdgesAcceleration - * @see EdgeAcceleration - * @see EdgeAccelerationGoal - * @remarks Do not forget to call setTebConfig() - * @remarks Refer to EdgeAccelerationGoal() for defining boundary values at the end of the trajectory! - */ -class EdgeAccelerationStart : public BaseTebMultiEdge<2, const geometry_msgs::Twist*> -{ -public: + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** + * @class EdgeAccelerationStart + * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory. + * + * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setInitialVelocity() + * and minimizes: \n + * \f$ \min \textrm{penaltyInterval}( [a, omegadot ]^T ) \cdot weight \f$. \n + * \e a is calculated using the difference quotient (twice) and the position parts of the poses. \n + * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n + * \e weight can be set using setInformation(). \n + * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n + * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and + * the second one the rotational acceleration. + * @see TebOptimalPlanner::AddEdgesAcceleration + * @see EdgeAcceleration + * @see EdgeAccelerationGoal + * @remarks Do not forget to call setTebConfig() + * @remarks Refer to EdgeAccelerationGoal() for defining boundary values at the end of the trajectory! + */ + class EdgeAccelerationStart : public BaseTebMultiEdge<3, const geometry_msgs::Twist *> + { + public: + /** + * @brief Construct edge. + */ + EdgeAccelerationStart() + { + _measurement = NULL; + this->resize(3); + } - /** - * @brief Construct edge. - */ - EdgeAccelerationStart() - { - _measurement = NULL; - this->resize(3); - } - - - /** - * @brief Actual cost function - */ - void computeError() - { - ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setStartVelocity() on EdgeAccelerationStart()"); - const VertexPose* pose1 = static_cast(_vertices[0]); - const VertexPose* pose2 = static_cast(_vertices[1]); - const VertexTimeDiff* dt = static_cast(_vertices[2]); - - // VELOCITY & ACCELERATION - const Eigen::Vector2d diff = pose2->position() - pose1->position(); - double dist = diff.norm(); - const double angle_diff = g2o::normalize_theta(pose2->theta() - pose1->theta()); - if (cfg_->trajectory.exact_arc_length && angle_diff != 0) + /** + * @brief Actual cost function + */ + void computeError() + { + ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setStartVelocity() on EdgeAccelerationStart()"); + const VertexPose *pose1 = static_cast(_vertices[0]); + const VertexPose *pose2 = static_cast(_vertices[1]); + const VertexTimeDiff *dt = static_cast(_vertices[2]); + + // VELOCITY & ACCELERATION + const Eigen::Vector2d diff = pose2->position() - pose1->position(); + double dist = diff.norm(); + const double angle_diff = g2o::normalize_theta(pose2->theta() - pose1->theta()); + if (cfg_->trajectory.exact_arc_length && angle_diff != 0) + { + const double radius = dist / (2 * sin(angle_diff / 2)); + dist = fabs(angle_diff * radius); // actual arg length! + } + + const double vel1 = _measurement->linear.x; + double vel2 = dist / dt->dt(); + + // consider directions + // vel2 *= g2o::sign(diff[0]*cos(pose1->theta()) + diff[1]*sin(pose1->theta())); + vel2 *= fast_sigmoid(100 * (diff.x() * cos(pose1->theta()) + diff.y() * sin(pose1->theta()))); + + const double acc_lin = (vel2 - vel1) / dt->dt(); + + _error[0] = penaltyBoundToInterval(acc_lin, cfg_->robot.acc_lim_x, cfg_->optim.penalty_epsilon); + + // ANGULAR ACCELERATION + const double omega1 = _measurement->angular.z; + const double omega2 = angle_diff / dt->dt(); + const double acc_rot = (omega2 - omega1) / dt->dt(); + + _error[1] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon); + + const double steering_angle1 = _measurement->angular.z; + const double steering_angle2 = std::atan(cfg_->robot.wheelbase / (vel2 / omega2)); + const double steering_rate = (steering_angle2 - steering_angle1) / dt->dt(); + + _error[2] = penaltyBoundToInterval(steering_rate, cfg_->robot.acc_lim_theta / 2.0, cfg_->optim.penalty_epsilon); + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n", _error[0]); + ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() rotational: _error[1]=%f\n", _error[1]); + ROS_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationStart::computeError() rotational: _error[2]=%f\n", _error[2]); + } + + /** + * @brief Set the initial velocity that is taken into account for calculating the acceleration + * @param vel_start twist message containing the translational and rotational velocity + */ + void setInitialVelocity(const geometry_msgs::Twist &vel_start) { _measurement = &vel_start; } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** + * @class EdgeAccelerationGoal + * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory. + * + * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setGoalVelocity() + * and minimizes: \n + * \f$ \min \textrm{penaltyInterval}( [a, omegadot ]^T ) \cdot weight \f$. \n + * \e a is calculated using the difference quotient (twice) and the position parts of the poses \n + * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n + * \e weight can be set using setInformation() \n + * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n + * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and + * the second one the rotational acceleration. + * @see TebOptimalPlanner::AddEdgesAcceleration + * @see EdgeAcceleration + * @see EdgeAccelerationStart + * @remarks Do not forget to call setTebConfig() + * @remarks Refer to EdgeAccelerationStart() for defining boundary (initial) values at the end of the trajectory + */ + class EdgeAccelerationGoal : public BaseTebMultiEdge<3, const geometry_msgs::Twist *> { - const double radius = dist/(2*sin(angle_diff/2)); - dist = fabs( angle_diff * radius ); // actual arg length! - } - - const double vel1 = _measurement->linear.x; - double vel2 = dist / dt->dt(); + public: + /** + * @brief Construct edge. + */ + EdgeAccelerationGoal() + { + _measurement = NULL; + this->resize(3); + } - // consider directions - //vel2 *= g2o::sign(diff[0]*cos(pose1->theta()) + diff[1]*sin(pose1->theta())); - vel2 *= fast_sigmoid( 100*(diff.x()*cos(pose1->theta()) + diff.y()*sin(pose1->theta())) ); - - const double acc_lin = (vel2 - vel1) / dt->dt(); - - _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon); - - // ANGULAR ACCELERATION - const double omega1 = _measurement->angular.z; - const double omega2 = angle_diff / dt->dt(); - const double acc_rot = (omega2 - omega1) / dt->dt(); - - _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon); - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]); - ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() rotational: _error[1]=%f\n",_error[1]); - } - - /** - * @brief Set the initial velocity that is taken into account for calculating the acceleration - * @param vel_start twist message containing the translational and rotational velocity - */ - void setInitialVelocity(const geometry_msgs::Twist& vel_start) - { - _measurement = &vel_start; - } - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - - + /** + * @brief Actual cost function + */ + void computeError() + { + ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setGoalVelocity() on EdgeAccelerationGoal()"); + const VertexPose *pose_pre_goal = static_cast(_vertices[0]); + const VertexPose *pose_goal = static_cast(_vertices[1]); + const VertexTimeDiff *dt = static_cast(_vertices[2]); -/** - * @class EdgeAccelerationGoal - * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory. - * - * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setGoalVelocity() - * and minimizes: \n - * \f$ \min \textrm{penaltyInterval}( [a, omegadot ]^T ) \cdot weight \f$. \n - * \e a is calculated using the difference quotient (twice) and the position parts of the poses \n - * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n - * \e weight can be set using setInformation() \n - * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n - * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and - * the second one the rotational acceleration. - * @see TebOptimalPlanner::AddEdgesAcceleration - * @see EdgeAcceleration - * @see EdgeAccelerationStart - * @remarks Do not forget to call setTebConfig() - * @remarks Refer to EdgeAccelerationStart() for defining boundary (initial) values at the end of the trajectory - */ -class EdgeAccelerationGoal : public BaseTebMultiEdge<2, const geometry_msgs::Twist*> -{ -public: + // VELOCITY & ACCELERATION - /** - * @brief Construct edge. - */ - EdgeAccelerationGoal() - { - _measurement = NULL; - this->resize(3); - } - + const Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position(); + double dist = diff.norm(); + const double angle_diff = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta()); + if (cfg_->trajectory.exact_arc_length && angle_diff != 0) + { + double radius = dist / (2 * sin(angle_diff / 2)); + dist = fabs(angle_diff * radius); // actual arg length! + } - /** - * @brief Actual cost function - */ - void computeError() - { - ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setGoalVelocity() on EdgeAccelerationGoal()"); - const VertexPose* pose_pre_goal = static_cast(_vertices[0]); - const VertexPose* pose_goal = static_cast(_vertices[1]); - const VertexTimeDiff* dt = static_cast(_vertices[2]); + double vel1 = dist / dt->dt(); + const double vel2 = _measurement->linear.x; + + // consider directions + // vel1 *= g2o::sign(diff[0]*cos(pose_pre_goal->theta()) + diff[1]*sin(pose_pre_goal->theta())); + vel1 *= fast_sigmoid(100 * (diff.x() * cos(pose_pre_goal->theta()) + diff.y() * sin(pose_pre_goal->theta()))); - // VELOCITY & ACCELERATION + const double acc_lin = (vel2 - vel1) / dt->dt(); - const Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position(); - double dist = diff.norm(); - const double angle_diff = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta()); - if (cfg_->trajectory.exact_arc_length && angle_diff != 0) + _error[0] = penaltyBoundToInterval(acc_lin, cfg_->robot.acc_lim_x, cfg_->optim.penalty_epsilon); + + // ANGULAR ACCELERATION + const double omega1 = angle_diff / dt->dt(); + const double omega2 = _measurement->angular.z; + const double acc_rot = (omega2 - omega1) / dt->dt(); + + _error[1] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon); + + const double steering_angle1 = std::atan(cfg_->robot.wheelbase / (vel1 / omega1)); + const double steering_angle2 = _measurement->angular.z; + const double steering_rate = (steering_angle2 - steering_angle1) / dt->dt(); + _error[2] = penaltyBoundToInterval(steering_rate, cfg_->robot.acc_lim_theta / 2.0, cfg_->optim.penalty_epsilon); + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n", _error[0]); + ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() rotational: _error[1]=%f\n", _error[1]); + ROS_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationStart::computeError() rotational: _error[2]=%f\n", _error[2]); + } + + /** + * @brief Set the goal / final velocity that is taken into account for calculating the acceleration + * @param vel_goal twist message containing the translational and rotational velocity + */ + void setGoalVelocity(const geometry_msgs::Twist &vel_goal) { _measurement = &vel_goal; } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** + * @class EdgeAccelerationHolonomic + * @brief Edge defining the cost function for limiting the translational and rotational acceleration. + * + * The edge depends on five vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2}, \Delta T_i, \Delta T_{ip1} \f$ and minimizes: + * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot } ]^T ) \cdot weight \f$. \n + * \e ax is calculated using the difference quotient (twice) and the x position parts of all three poses \n + * \e ay is calculated using the difference quotient (twice) and the y position parts of all three poses \n + * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n + * \e weight can be set using setInformation() \n + * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n + * The dimension of the error / cost vector is 3: the first component represents the translational acceleration (x-dir), + * the second one the strafing acceleration and the third one the rotational acceleration. + * @see TebOptimalPlanner::AddEdgesAcceleration + * @see EdgeAccelerationHolonomicStart + * @see EdgeAccelerationHolonomicGoal + * @remarks Do not forget to call setTebConfig() + * @remarks Refer to EdgeAccelerationHolonomicStart() and EdgeAccelerationHolonomicGoal() for defining boundary values! + */ + class EdgeAccelerationHolonomic : public BaseTebMultiEdge<3, double> { - double radius = dist/(2*sin(angle_diff/2)); - dist = fabs( angle_diff * radius ); // actual arg length! - } - - double vel1 = dist / dt->dt(); - const double vel2 = _measurement->linear.x; - - // consider directions - //vel1 *= g2o::sign(diff[0]*cos(pose_pre_goal->theta()) + diff[1]*sin(pose_pre_goal->theta())); - vel1 *= fast_sigmoid( 100*(diff.x()*cos(pose_pre_goal->theta()) + diff.y()*sin(pose_pre_goal->theta())) ); - - const double acc_lin = (vel2 - vel1) / dt->dt(); + public: + /** + * @brief Construct edge. + */ + EdgeAccelerationHolonomic() { this->resize(5); } + + /** + * @brief Actual cost function + */ + void computeError() + { + ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()"); + const VertexPose *pose1 = static_cast(_vertices[0]); + const VertexPose *pose2 = static_cast(_vertices[1]); + const VertexPose *pose3 = static_cast(_vertices[2]); + const VertexTimeDiff *dt1 = static_cast(_vertices[3]); + const VertexTimeDiff *dt2 = static_cast(_vertices[4]); + + // VELOCITY & ACCELERATION + Eigen::Vector2d diff1 = pose2->position() - pose1->position(); + Eigen::Vector2d diff2 = pose3->position() - pose2->position(); + + double cos_theta1 = std::cos(pose1->theta()); + double sin_theta1 = std::sin(pose1->theta()); + double cos_theta2 = std::cos(pose2->theta()); + double sin_theta2 = std::sin(pose2->theta()); + + // transform pose2 into robot frame pose1 (inverse 2d rotation matrix) + double p1_dx = cos_theta1 * diff1.x() + sin_theta1 * diff1.y(); + double p1_dy = -sin_theta1 * diff1.x() + cos_theta1 * diff1.y(); + // transform pose3 into robot frame pose2 (inverse 2d rotation matrix) + double p2_dx = cos_theta2 * diff2.x() + sin_theta2 * diff2.y(); + double p2_dy = -sin_theta2 * diff2.x() + cos_theta2 * diff2.y(); + + double vel1_x = p1_dx / dt1->dt(); + double vel1_y = p1_dy / dt1->dt(); + double vel2_x = p2_dx / dt2->dt(); + double vel2_y = p2_dy / dt2->dt(); + + double dt12 = dt1->dt() + dt2->dt(); + + double acc_x = (vel2_x - vel1_x) * 2 / dt12; + double acc_y = (vel2_y - vel1_y) * 2 / dt12; + + _error[0] = penaltyBoundToInterval(acc_x, cfg_->robot.acc_lim_x, cfg_->optim.penalty_epsilon); + _error[1] = penaltyBoundToInterval(acc_y, cfg_->robot.acc_lim_y, cfg_->optim.penalty_epsilon); + + // ANGULAR ACCELERATION + double omega1 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt1->dt(); + double omega2 = g2o::normalize_theta(pose3->theta() - pose2->theta()) / dt2->dt(); + double acc_rot = (omega2 - omega1) * 2 / dt12; + + _error[2] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon); + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n", _error[0]); + ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() strafing: _error[1]=%f\n", _error[1]); + ROS_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAcceleration::computeError() rotational: _error[2]=%f\n", _error[2]); + } - _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon); - - // ANGULAR ACCELERATION - const double omega1 = angle_diff / dt->dt(); - const double omega2 = _measurement->angular.z; - const double acc_rot = (omega2 - omega1) / dt->dt(); - - _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon); - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]); - ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() rotational: _error[1]=%f\n",_error[1]); - } - - /** - * @brief Set the goal / final velocity that is taken into account for calculating the acceleration - * @param vel_goal twist message containing the translational and rotational velocity - */ - void setGoalVelocity(const geometry_msgs::Twist& vel_goal) - { - _measurement = &vel_goal; - } - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** + * @class EdgeAccelerationHolonomicStart + * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory. + * + * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setInitialVelocity() + * and minimizes: \n + * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot ]^T ) \cdot weight \f$. \n + * \e ax is calculated using the difference quotient (twice) and the x-position parts of the poses. \n + * \e ay is calculated using the difference quotient (twice) and the y-position parts of the poses. \n + * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n + * \e weight can be set using setInformation(). \n + * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n + * The dimension of the error / cost vector is 3: the first component represents the translational acceleration, + * the second one the strafing acceleration and the third one the rotational acceleration. + * @see TebOptimalPlanner::AddEdgesAcceleration + * @see EdgeAccelerationHolonomic + * @see EdgeAccelerationHolonomicGoal + * @remarks Do not forget to call setTebConfig() + * @remarks Refer to EdgeAccelerationHolonomicGoal() for defining boundary values at the end of the trajectory! + */ + class EdgeAccelerationHolonomicStart : public BaseTebMultiEdge<3, const geometry_msgs::Twist *> + { + public: + /** + * @brief Construct edge. + */ + EdgeAccelerationHolonomicStart() + { + this->resize(3); + _measurement = NULL; + } + /** + * @brief Actual cost function + */ + void computeError() + { + ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setStartVelocity() on EdgeAccelerationStart()"); + const VertexPose *pose1 = static_cast(_vertices[0]); + const VertexPose *pose2 = static_cast(_vertices[1]); + const VertexTimeDiff *dt = static_cast(_vertices[2]); + // VELOCITY & ACCELERATION + Eigen::Vector2d diff = pose2->position() - pose1->position(); -/** - * @class EdgeAccelerationHolonomic - * @brief Edge defining the cost function for limiting the translational and rotational acceleration. - * - * The edge depends on five vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2}, \Delta T_i, \Delta T_{ip1} \f$ and minimizes: - * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot } ]^T ) \cdot weight \f$. \n - * \e ax is calculated using the difference quotient (twice) and the x position parts of all three poses \n - * \e ay is calculated using the difference quotient (twice) and the y position parts of all three poses \n - * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n - * \e weight can be set using setInformation() \n - * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n - * The dimension of the error / cost vector is 3: the first component represents the translational acceleration (x-dir), - * the second one the strafing acceleration and the third one the rotational acceleration. - * @see TebOptimalPlanner::AddEdgesAcceleration - * @see EdgeAccelerationHolonomicStart - * @see EdgeAccelerationHolonomicGoal - * @remarks Do not forget to call setTebConfig() - * @remarks Refer to EdgeAccelerationHolonomicStart() and EdgeAccelerationHolonomicGoal() for defining boundary values! - */ -class EdgeAccelerationHolonomic : public BaseTebMultiEdge<3, double> -{ -public: + double cos_theta1 = std::cos(pose1->theta()); + double sin_theta1 = std::sin(pose1->theta()); - /** - * @brief Construct edge. - */ - EdgeAccelerationHolonomic() - { - this->resize(5); - } - - /** - * @brief Actual cost function - */ - void computeError() - { - ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()"); - const VertexPose* pose1 = static_cast(_vertices[0]); - const VertexPose* pose2 = static_cast(_vertices[1]); - const VertexPose* pose3 = static_cast(_vertices[2]); - const VertexTimeDiff* dt1 = static_cast(_vertices[3]); - const VertexTimeDiff* dt2 = static_cast(_vertices[4]); - - // VELOCITY & ACCELERATION - Eigen::Vector2d diff1 = pose2->position() - pose1->position(); - Eigen::Vector2d diff2 = pose3->position() - pose2->position(); - - double cos_theta1 = std::cos(pose1->theta()); - double sin_theta1 = std::sin(pose1->theta()); - double cos_theta2 = std::cos(pose2->theta()); - double sin_theta2 = std::sin(pose2->theta()); - - // transform pose2 into robot frame pose1 (inverse 2d rotation matrix) - double p1_dx = cos_theta1*diff1.x() + sin_theta1*diff1.y(); - double p1_dy = -sin_theta1*diff1.x() + cos_theta1*diff1.y(); - // transform pose3 into robot frame pose2 (inverse 2d rotation matrix) - double p2_dx = cos_theta2*diff2.x() + sin_theta2*diff2.y(); - double p2_dy = -sin_theta2*diff2.x() + cos_theta2*diff2.y(); - - double vel1_x = p1_dx / dt1->dt(); - double vel1_y = p1_dy / dt1->dt(); - double vel2_x = p2_dx / dt2->dt(); - double vel2_y = p2_dy / dt2->dt(); - - double dt12 = dt1->dt() + dt2->dt(); - - double acc_x = (vel2_x - vel1_x)*2 / dt12; - double acc_y = (vel2_y - vel1_y)*2 / dt12; - - _error[0] = penaltyBoundToInterval(acc_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon); - _error[1] = penaltyBoundToInterval(acc_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon); - - // ANGULAR ACCELERATION - double omega1 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt1->dt(); - double omega2 = g2o::normalize_theta(pose3->theta() - pose2->theta()) / dt2->dt(); - double acc_rot = (omega2 - omega1)*2 / dt12; - - _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon); + // transform pose2 into robot frame pose1 (inverse 2d rotation matrix) + double p1_dx = cos_theta1 * diff.x() + sin_theta1 * diff.y(); + double p1_dy = -sin_theta1 * diff.x() + cos_theta1 * diff.y(); - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]); - ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() strafing: _error[1]=%f\n",_error[1]); - ROS_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAcceleration::computeError() rotational: _error[2]=%f\n",_error[2]); - } + double vel1_x = _measurement->linear.x; + double vel1_y = _measurement->linear.y; + double vel2_x = p1_dx / dt->dt(); + double vel2_y = p1_dy / dt->dt(); -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; + double acc_lin_x = (vel2_x - vel1_x) / dt->dt(); + double acc_lin_y = (vel2_y - vel1_y) / dt->dt(); + _error[0] = penaltyBoundToInterval(acc_lin_x, cfg_->robot.acc_lim_x, cfg_->optim.penalty_epsilon); + _error[1] = penaltyBoundToInterval(acc_lin_y, cfg_->robot.acc_lim_y, cfg_->optim.penalty_epsilon); -/** - * @class EdgeAccelerationHolonomicStart - * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory. - * - * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setInitialVelocity() - * and minimizes: \n - * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot ]^T ) \cdot weight \f$. \n - * \e ax is calculated using the difference quotient (twice) and the x-position parts of the poses. \n - * \e ay is calculated using the difference quotient (twice) and the y-position parts of the poses. \n - * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n - * \e weight can be set using setInformation(). \n - * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n - * The dimension of the error / cost vector is 3: the first component represents the translational acceleration, - * the second one the strafing acceleration and the third one the rotational acceleration. - * @see TebOptimalPlanner::AddEdgesAcceleration - * @see EdgeAccelerationHolonomic - * @see EdgeAccelerationHolonomicGoal - * @remarks Do not forget to call setTebConfig() - * @remarks Refer to EdgeAccelerationHolonomicGoal() for defining boundary values at the end of the trajectory! - */ -class EdgeAccelerationHolonomicStart : public BaseTebMultiEdge<3, const geometry_msgs::Twist*> -{ -public: + // ANGULAR ACCELERATION + double omega1 = _measurement->angular.z; + double omega2 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt->dt(); + double acc_rot = (omega2 - omega1) / dt->dt(); - /** - * @brief Construct edge. - */ - EdgeAccelerationHolonomicStart() - { - this->resize(3); - _measurement = NULL; - } - - /** - * @brief Actual cost function - */ - void computeError() - { - ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setStartVelocity() on EdgeAccelerationStart()"); - const VertexPose* pose1 = static_cast(_vertices[0]); - const VertexPose* pose2 = static_cast(_vertices[1]); - const VertexTimeDiff* dt = static_cast(_vertices[2]); - - // VELOCITY & ACCELERATION - Eigen::Vector2d diff = pose2->position() - pose1->position(); - - double cos_theta1 = std::cos(pose1->theta()); - double sin_theta1 = std::sin(pose1->theta()); - - // transform pose2 into robot frame pose1 (inverse 2d rotation matrix) - double p1_dx = cos_theta1*diff.x() + sin_theta1*diff.y(); - double p1_dy = -sin_theta1*diff.x() + cos_theta1*diff.y(); - - double vel1_x = _measurement->linear.x; - double vel1_y = _measurement->linear.y; - double vel2_x = p1_dx / dt->dt(); - double vel2_y = p1_dy / dt->dt(); + _error[2] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon); - double acc_lin_x = (vel2_x - vel1_x) / dt->dt(); - double acc_lin_y = (vel2_y - vel1_y) / dt->dt(); - - _error[0] = penaltyBoundToInterval(acc_lin_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon); - _error[1] = penaltyBoundToInterval(acc_lin_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon); - - // ANGULAR ACCELERATION - double omega1 = _measurement->angular.z; - double omega2 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt->dt(); - double acc_rot = (omega2 - omega1) / dt->dt(); - - _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon); - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]); - ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() strafing: _error[1]=%f\n",_error[1]); - ROS_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationStart::computeError() rotational: _error[2]=%f\n",_error[2]); - } - - /** - * @brief Set the initial velocity that is taken into account for calculating the acceleration - * @param vel_start twist message containing the translational and rotational velocity - */ - void setInitialVelocity(const geometry_msgs::Twist& vel_start) - { - _measurement = &vel_start; - } - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - - -/** - * @class EdgeAccelerationHolonomicGoal - * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory. - * - * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setGoalVelocity() - * and minimizes: \n - * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot ]^T ) \cdot weight \f$. \n - * \e ax is calculated using the difference quotient (twice) and the x-position parts of the poses \n - * \e ay is calculated using the difference quotient (twice) and the y-position parts of the poses \n - * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n - * \e weight can be set using setInformation() \n - * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n - * The dimension of the error / cost vector is 3: the first component represents the translational acceleration, - * the second one is the strafing velocity and the third one the rotational acceleration. - * @see TebOptimalPlanner::AddEdgesAcceleration - * @see EdgeAccelerationHolonomic - * @see EdgeAccelerationHolonomicStart - * @remarks Do not forget to call setTebConfig() - * @remarks Refer to EdgeAccelerationHolonomicStart() for defining boundary (initial) values at the end of the trajectory - */ -class EdgeAccelerationHolonomicGoal : public BaseTebMultiEdge<3, const geometry_msgs::Twist*> -{ -public: + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n", _error[0]); + ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() strafing: _error[1]=%f\n", _error[1]); + ROS_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationStart::computeError() rotational: _error[2]=%f\n", _error[2]); + } - /** - * @brief Construct edge. - */ - EdgeAccelerationHolonomicGoal() - { - _measurement = NULL; - this->resize(3); - } - - /** - * @brief Actual cost function - */ - void computeError() - { - ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setGoalVelocity() on EdgeAccelerationGoal()"); - const VertexPose* pose_pre_goal = static_cast(_vertices[0]); - const VertexPose* pose_goal = static_cast(_vertices[1]); - const VertexTimeDiff* dt = static_cast(_vertices[2]); + /** + * @brief Set the initial velocity that is taken into account for calculating the acceleration + * @param vel_start twist message containing the translational and rotational velocity + */ + void setInitialVelocity(const geometry_msgs::Twist &vel_start) { _measurement = &vel_start; } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** + * @class EdgeAccelerationHolonomicGoal + * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory. + * + * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setGoalVelocity() + * and minimizes: \n + * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot ]^T ) \cdot weight \f$. \n + * \e ax is calculated using the difference quotient (twice) and the x-position parts of the poses \n + * \e ay is calculated using the difference quotient (twice) and the y-position parts of the poses \n + * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n + * \e weight can be set using setInformation() \n + * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n + * The dimension of the error / cost vector is 3: the first component represents the translational acceleration, + * the second one is the strafing velocity and the third one the rotational acceleration. + * @see TebOptimalPlanner::AddEdgesAcceleration + * @see EdgeAccelerationHolonomic + * @see EdgeAccelerationHolonomicStart + * @remarks Do not forget to call setTebConfig() + * @remarks Refer to EdgeAccelerationHolonomicStart() for defining boundary (initial) values at the end of the trajectory + */ + class EdgeAccelerationHolonomicGoal : public BaseTebMultiEdge<3, const geometry_msgs::Twist *> + { + public: + /** + * @brief Construct edge. + */ + EdgeAccelerationHolonomicGoal() + { + _measurement = NULL; + this->resize(3); + } - // VELOCITY & ACCELERATION + /** + * @brief Actual cost function + */ + void computeError() + { + ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setGoalVelocity() on EdgeAccelerationGoal()"); + const VertexPose *pose_pre_goal = static_cast(_vertices[0]); + const VertexPose *pose_goal = static_cast(_vertices[1]); + const VertexTimeDiff *dt = static_cast(_vertices[2]); - Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position(); - - double cos_theta1 = std::cos(pose_pre_goal->theta()); - double sin_theta1 = std::sin(pose_pre_goal->theta()); - - // transform pose2 into robot frame pose1 (inverse 2d rotation matrix) - double p1_dx = cos_theta1*diff.x() + sin_theta1*diff.y(); - double p1_dy = -sin_theta1*diff.x() + cos_theta1*diff.y(); - - double vel1_x = p1_dx / dt->dt(); - double vel1_y = p1_dy / dt->dt(); - double vel2_x = _measurement->linear.x; - double vel2_y = _measurement->linear.y; - - double acc_lin_x = (vel2_x - vel1_x) / dt->dt(); - double acc_lin_y = (vel2_y - vel1_y) / dt->dt(); + // VELOCITY & ACCELERATION - _error[0] = penaltyBoundToInterval(acc_lin_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon); - _error[1] = penaltyBoundToInterval(acc_lin_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon); - - // ANGULAR ACCELERATION - double omega1 = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta()) / dt->dt(); - double omega2 = _measurement->angular.z; - double acc_rot = (omega2 - omega1) / dt->dt(); - - _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon); - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]); - ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() strafing: _error[1]=%f\n",_error[1]); - ROS_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationGoal::computeError() rotational: _error[2]=%f\n",_error[2]); - } - - - /** - * @brief Set the goal / final velocity that is taken into account for calculating the acceleration - * @param vel_goal twist message containing the translational and rotational velocity - */ - void setGoalVelocity(const geometry_msgs::Twist& vel_goal) - { - _measurement = &vel_goal; - } - + Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position(); -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - + double cos_theta1 = std::cos(pose_pre_goal->theta()); + double sin_theta1 = std::sin(pose_pre_goal->theta()); + + // transform pose2 into robot frame pose1 (inverse 2d rotation matrix) + double p1_dx = cos_theta1 * diff.x() + sin_theta1 * diff.y(); + double p1_dy = -sin_theta1 * diff.x() + cos_theta1 * diff.y(); + + double vel1_x = p1_dx / dt->dt(); + double vel1_y = p1_dy / dt->dt(); + double vel2_x = _measurement->linear.x; + double vel2_y = _measurement->linear.y; + + double acc_lin_x = (vel2_x - vel1_x) / dt->dt(); + double acc_lin_y = (vel2_y - vel1_y) / dt->dt(); + + _error[0] = penaltyBoundToInterval(acc_lin_x, cfg_->robot.acc_lim_x, cfg_->optim.penalty_epsilon); + _error[1] = penaltyBoundToInterval(acc_lin_y, cfg_->robot.acc_lim_y, cfg_->optim.penalty_epsilon); + + // ANGULAR ACCELERATION + double omega1 = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta()) / dt->dt(); + double omega2 = _measurement->angular.z; + double acc_rot = (omega2 - omega1) / dt->dt(); + + _error[2] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon); + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n", _error[0]); + ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() strafing: _error[1]=%f\n", _error[1]); + ROS_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationGoal::computeError() rotational: _error[2]=%f\n", _error[2]); + } + /** + * @brief Set the goal / final velocity that is taken into account for calculating the acceleration + * @param vel_goal twist message containing the translational and rotational velocity + */ + void setGoalVelocity(const geometry_msgs::Twist &vel_goal) { _measurement = &vel_goal; } + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; -}; // end namespace +}; // namespace teb_local_planner #endif /* EDGE_ACCELERATION_H_ */ diff --git a/include/teb_local_planner/g2o_types/edge_kinematics.h b/include/teb_local_planner/g2o_types/edge_kinematics.h index 694e602a..b8dd76c5 100755 --- a/include/teb_local_planner/g2o_types/edge_kinematics.h +++ b/include/teb_local_planner/g2o_types/edge_kinematics.h @@ -32,7 +32,7 @@ * 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. - * + * * Notes: * The following class is derived from a class defined by the * g2o-framework. g2o is licensed under the terms of the BSD License. @@ -44,187 +44,177 @@ #ifndef _EDGE_KINEMATICS_H #define _EDGE_KINEMATICS_H -#include -#include #include +#include +#include #include #include namespace teb_local_planner { - -/** - * @class EdgeKinematicsDiffDrive - * @brief Edge defining the cost function for satisfying the non-holonomic kinematics of a differential drive mobile robot. - * - * The edge depends on two vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1} \f$ and minimizes a geometric interpretation - * of the non-holonomic constraint: - * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012. - * - * The \e weight can be set using setInformation(): Matrix element 1,1: (Choose a very high value: ~1000). \n - * A second equation is implemented to penalize backward motions (second element of the error /cost vector). \n - * The \e weight can be set using setInformation(): Matrix element 2,2: (A value ~1 allows backward driving, but penalizes it slighly). \n - * The dimension of the error / cost vector is 2: the first component represents the nonholonomic constraint cost, - * the second one backward-drive cost. - * @see TebOptimalPlanner::AddEdgesKinematics, EdgeKinematicsCarlike - * @remarks Do not forget to call setTebConfig() - */ -class EdgeKinematicsDiffDrive : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeKinematicsDiffDrive() - { - this->setMeasurement(0.); - } - - /** - * @brief Actual cost function - */ - void computeError() - { - ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - - Eigen::Vector2d deltaS = conf2->position() - conf1->position(); - - // non holonomic constraint - _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] ); - - // positive-drive-direction constraint - Eigen::Vector2d angle_vec ( cos(conf1->theta()), sin(conf1->theta()) ); - _error[1] = penaltyBoundFromBelow(deltaS.dot(angle_vec), 0,0); - // epsilon=0, otherwise it pushes the first bandpoints away from start - - ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsDiffDrive::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]); - } + /** + * @class EdgeKinematicsDiffDrive + * @brief Edge defining the cost function for satisfying the non-holonomic kinematics of a differential drive mobile robot. + * + * The edge depends on two vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1} \f$ and minimizes a geometric interpretation + * of the non-holonomic constraint: + * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012. + * + * The \e weight can be set using setInformation(): Matrix element 1,1: (Choose a very high value: ~1000). \n + * A second equation is implemented to penalize backward motions (second element of the error /cost vector). \n + * The \e weight can be set using setInformation(): Matrix element 2,2: (A value ~1 allows backward driving, but penalizes it slighly). \n + * The dimension of the error / cost vector is 2: the first component represents the nonholonomic constraint cost, + * the second one backward-drive cost. + * @see TebOptimalPlanner::AddEdgesKinematics, EdgeKinematicsCarlike + * @remarks Do not forget to call setTebConfig() + */ + class EdgeKinematicsDiffDrive : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose> + { + public: + /** + * @brief Construct edge. + */ + EdgeKinematicsDiffDrive() { this->setMeasurement(0.); } + + /** + * @brief Actual cost function + */ + void computeError() + { + ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()"); + const VertexPose *conf1 = static_cast(_vertices[0]); + const VertexPose *conf2 = static_cast(_vertices[1]); + + Eigen::Vector2d deltaS = conf2->position() - conf1->position(); + + // non holonomic constraint + _error[0] = fabs((cos(conf1->theta()) + cos(conf2->theta())) * deltaS[1] - (sin(conf1->theta()) + sin(conf2->theta())) * deltaS[0]); + + // positive-drive-direction constraint + Eigen::Vector2d angle_vec(cos(conf1->theta()), sin(conf1->theta())); + _error[1] = penaltyBoundFromBelow(deltaS.dot(angle_vec), 0, 0); + // epsilon=0, otherwise it pushes the first bandpoints away from start + + ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), + "EdgeKinematicsDiffDrive::computeError() _error[0]=%f _error[1]=%f\n", _error[0], _error[1]); + } #ifdef USE_ANALYTIC_JACOBI #if 1 - /** - * @brief Jacobi matrix of the cost function specified in computeError(). - */ - void linearizeOplus() - { - ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - - Eigen::Vector2d deltaS = conf2->position() - conf1->position(); - - double cos1 = cos(conf1->theta()); - double cos2 = cos(conf2->theta()); - double sin1 = sin(conf1->theta()); - double sin2 = sin(conf2->theta()); - double aux1 = sin1 + sin2; - double aux2 = cos1 + cos2; - - double dd_error_1 = deltaS[0]*cos1; - double dd_error_2 = deltaS[1]*sin1; - double dd_dev = penaltyBoundFromBelowDerivative(dd_error_1+dd_error_2, 0,0); - - double dev_nh_abs = g2o::sign( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] ); - - // conf1 - _jacobianOplusXi(0,0) = aux1 * dev_nh_abs; // nh x1 - _jacobianOplusXi(0,1) = -aux2 * dev_nh_abs; // nh y1 - _jacobianOplusXi(1,0) = -cos1 * dd_dev; // drive-dir x1 - _jacobianOplusXi(1,1) = -sin1 * dd_dev; // drive-dir y1 - _jacobianOplusXi(0,2) = (-dd_error_2 - dd_error_1) * dev_nh_abs; // nh angle - _jacobianOplusXi(1,2) = ( -sin1*deltaS[0] + cos1*deltaS[1] ) * dd_dev; // drive-dir angle1 - - // conf2 - _jacobianOplusXj(0,0) = -aux1 * dev_nh_abs; // nh x2 - _jacobianOplusXj(0,1) = aux2 * dev_nh_abs; // nh y2 - _jacobianOplusXj(1,0) = cos1 * dd_dev; // drive-dir x2 - _jacobianOplusXj(1,1) = sin1 * dd_dev; // drive-dir y2 - _jacobianOplusXj(0,2) = (-sin2*deltaS[1] - cos2*deltaS[0]) * dev_nh_abs; // nh angle - _jacobianOplusXj(1,2) = 0; // drive-dir angle1 - } + /** + * @brief Jacobi matrix of the cost function specified in computeError(). + */ + void linearizeOplus() + { + ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()"); + const VertexPose *conf1 = static_cast(_vertices[0]); + const VertexPose *conf2 = static_cast(_vertices[1]); + + Eigen::Vector2d deltaS = conf2->position() - conf1->position(); + + double cos1 = cos(conf1->theta()); + double cos2 = cos(conf2->theta()); + double sin1 = sin(conf1->theta()); + double sin2 = sin(conf2->theta()); + double aux1 = sin1 + sin2; + double aux2 = cos1 + cos2; + + double dd_error_1 = deltaS[0] * cos1; + double dd_error_2 = deltaS[1] * sin1; + double dd_dev = penaltyBoundFromBelowDerivative(dd_error_1 + dd_error_2, 0, 0); + + double dev_nh_abs = + g2o::sign((cos(conf1->theta()) + cos(conf2->theta())) * deltaS[1] - (sin(conf1->theta()) + sin(conf2->theta())) * deltaS[0]); + + // conf1 + _jacobianOplusXi(0, 0) = aux1 * dev_nh_abs; // nh x1 + _jacobianOplusXi(0, 1) = -aux2 * dev_nh_abs; // nh y1 + _jacobianOplusXi(1, 0) = -cos1 * dd_dev; // drive-dir x1 + _jacobianOplusXi(1, 1) = -sin1 * dd_dev; // drive-dir y1 + _jacobianOplusXi(0, 2) = (-dd_error_2 - dd_error_1) * dev_nh_abs; // nh angle + _jacobianOplusXi(1, 2) = (-sin1 * deltaS[0] + cos1 * deltaS[1]) * dd_dev; // drive-dir angle1 + + // conf2 + _jacobianOplusXj(0, 0) = -aux1 * dev_nh_abs; // nh x2 + _jacobianOplusXj(0, 1) = aux2 * dev_nh_abs; // nh y2 + _jacobianOplusXj(1, 0) = cos1 * dd_dev; // drive-dir x2 + _jacobianOplusXj(1, 1) = sin1 * dd_dev; // drive-dir y2 + _jacobianOplusXj(0, 2) = (-sin2 * deltaS[1] - cos2 * deltaS[0]) * dev_nh_abs; // nh angle + _jacobianOplusXj(1, 2) = 0; // drive-dir angle1 + } #endif #endif - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - - - -/** - * @class EdgeKinematicsCarlike - * @brief Edge defining the cost function for satisfying the non-holonomic kinematics of a carlike mobile robot. - * - * The edge depends on two vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1} \f$ and minimizes a geometric interpretation - * of the non-holonomic constraint: - * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012. - * - * The definition is identically to the one of the differential drive robot. - * Additionally, this edge incorporates a minimum turning radius that is required by carlike robots. - * The turning radius is defined by \f$ r=v/omega \f$. - * - * The \e weight can be set using setInformation(): Matrix element 1,1: (Choose a very high value: ~1000). \n - * The second equation enforces a minimum turning radius. - * The \e weight can be set using setInformation(): Matrix element 2,2. \n - * The dimension of the error / cost vector is 3: the first component represents the nonholonomic constraint cost, - * the second one backward-drive cost and the third one the minimum turning radius - * @see TebOptimalPlanner::AddEdgesKinematics, EdgeKinematicsDiffDrive - * @remarks Bounding the turning radius from below is not affected by the penalty_epsilon parameter, - * the user might add an extra margin to the min_turning_radius param. - * @remarks Do not forget to call setTebConfig() - */ -class EdgeKinematicsCarlike : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeKinematicsCarlike() - { - this->setMeasurement(0.); - } - - /** - * @brief Actual cost function - */ - void computeError() - { - ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsCarlike()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - - Eigen::Vector2d deltaS = conf2->position() - conf1->position(); - - // non holonomic constraint - _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] ); - - // limit minimum turning radius - double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); - if (angle_diff == 0) - _error[1] = 0; // straight line motion - else if (cfg_->trajectory.exact_arc_length) // use exact computation of the radius - _error[1] = penaltyBoundFromBelow(fabs(deltaS.norm()/(2*sin(angle_diff/2))), cfg_->robot.min_turning_radius, 0.0); - else - _error[1] = penaltyBoundFromBelow(deltaS.norm() / fabs(angle_diff), cfg_->robot.min_turning_radius, 0.0); - // This edge is not affected by the epsilon parameter, the user might add an exra margin to the min_turning_radius parameter. - - ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsCarlike::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]); - } - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - - - -} // end namespace + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** + * @class EdgeKinematicsCarlike + * @brief Edge defining the cost function for satisfying the non-holonomic kinematics of a carlike mobile robot. + * + * The edge depends on two vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1} \f$ and minimizes a geometric interpretation + * of the non-holonomic constraint: + * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012. + * + * The definition is identically to the one of the differential drive robot. + * Additionally, this edge incorporates a minimum turning radius that is required by carlike robots. + * The turning radius is defined by \f$ r=v/omega \f$. + * + * The \e weight can be set using setInformation(): Matrix element 1,1: (Choose a very high value: ~1000). \n + * The second equation enforces a minimum turning radius. + * The \e weight can be set using setInformation(): Matrix element 2,2. \n + * The dimension of the error / cost vector is 3: the first component represents the nonholonomic constraint cost, + * the second one backward-drive cost and the third one the minimum turning radius + * @see TebOptimalPlanner::AddEdgesKinematics, EdgeKinematicsDiffDrive + * @remarks Bounding the turning radius from below is not affected by the penalty_epsilon parameter, + * the user might add an extra margin to the min_turning_radius param. + * @remarks Do not forget to call setTebConfig() + */ + class EdgeKinematicsCarlike : public BaseTebBinaryEdge<3, double, VertexPose, VertexPose> + { + public: + /** + * @brief Construct edge. + */ + EdgeKinematicsCarlike() { this->setMeasurement(0.); } + + /** + * @brief Actual cost function + */ + void computeError() + { + ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsCarlike()"); + const VertexPose *conf1 = static_cast(_vertices[0]); + const VertexPose *conf2 = static_cast(_vertices[1]); + + Eigen::Vector2d deltaS = conf2->position() - conf1->position(); + + // non holonomic constraint + _error[0] = fabs((cos(conf1->theta()) + cos(conf2->theta())) * deltaS[1] - (sin(conf1->theta()) + sin(conf2->theta())) * deltaS[0]); + + // limit minimum turning radius + double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta()); + if (angle_diff == 0) + _error[1] = 0; // straight line motion + else if (cfg_->trajectory.exact_arc_length) // use exact computation of the radius + _error[1] = penaltyBoundFromBelow(fabs(deltaS.norm() / (2 * sin(angle_diff / 2))), cfg_->robot.min_turning_radius, 0.0); + else + _error[1] = penaltyBoundFromBelow(deltaS.norm() / fabs(angle_diff), cfg_->robot.min_turning_radius, 0.0); + // This edge is not affected by the epsilon parameter, the user might add an exra margin to the min_turning_radius parameter. + + double steering_angle = std::atan(cfg_->robot.wheelbase / deltaS.norm() * 2.0 * std::sin(angle_diff / 2.0)); + _error[2] = penaltyBoundToInterval(steering_angle, cfg_->robot.max_steering_angle, 0.0); + + ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsCarlike::computeError() _error[0]=%f _error[1]=%f\n", + _error[0], _error[1]); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + +} // namespace teb_local_planner #endif diff --git a/include/teb_local_planner/g2o_types/edge_obstacle.h b/include/teb_local_planner/g2o_types/edge_obstacle.h index 33f7ebcf..cf1c15bd 100755 --- a/include/teb_local_planner/g2o_types/edge_obstacle.h +++ b/include/teb_local_planner/g2o_types/edge_obstacle.h @@ -32,7 +32,7 @@ * 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. - * + * * Notes: * The following class is derived from a class defined by the * g2o-framework. g2o is licensed under the terms of the BSD License. @@ -43,56 +43,49 @@ #ifndef EDGE_OBSTACLE_H_ #define EDGE_OBSTACLE_H_ -#include -#include -#include #include #include +#include +#include +#include #include - - namespace teb_local_planner { + /** + * @class EdgeObstacle + * @brief Edge defining the cost function for keeping a minimum distance from obstacles. + * + * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n + * \f$ \min \textrm{penaltyBelow}( dist2point ) \cdot weight \f$. \n + * \e dist2point denotes the minimum distance to the point obstacle. \n + * \e weight can be set using setInformation(). \n + * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n + * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeInflatedObstacle + * @remarks Do not forget to call setTebConfig() and setObstacle() + */ + class EdgeObstacle : public BaseTebUnaryEdge<1, const Obstacle *, VertexPose> + { + public: + /** + * @brief Construct edge. + */ + EdgeObstacle() { _measurement = NULL; } -/** - * @class EdgeObstacle - * @brief Edge defining the cost function for keeping a minimum distance from obstacles. - * - * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n - * \f$ \min \textrm{penaltyBelow}( dist2point ) \cdot weight \f$. \n - * \e dist2point denotes the minimum distance to the point obstacle. \n - * \e weight can be set using setInformation(). \n - * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n - * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeInflatedObstacle - * @remarks Do not forget to call setTebConfig() and setObstacle() - */ -class EdgeObstacle : public BaseTebUnaryEdge<1, const Obstacle*, VertexPose> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeObstacle() - { - _measurement = NULL; - } - - /** - * @brief Actual cost function - */ - void computeError() - { - ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeObstacle()"); - const VertexPose* bandpt = static_cast(_vertices[0]); + /** + * @brief Actual cost function + */ + void computeError() + { + ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeObstacle()"); + const VertexPose *bandpt = static_cast(_vertices[0]); - double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement); + double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement); - _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); + _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeObstacle::computeError() _error[0]=%f\n",_error[0]); - } + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeObstacle::computeError() _error[0]=%f\n", _error[0]); + } #ifdef USE_ANALYTIC_JACOBI #if 0 @@ -138,133 +131,117 @@ class EdgeObstacle : public BaseTebUnaryEdge<1, const Obstacle*, VertexPose> } #endif #endif - - /** - * @brief Set pointer to associated obstacle for the underlying cost function - * @param obstacle 2D position vector containing the position of the obstacle - */ - void setObstacle(const Obstacle* obstacle) - { - _measurement = obstacle; - } - - /** - * @brief Set pointer to the robot model - * @param robot_model Robot model required for distance calculation - */ - void setRobotModel(const BaseRobotFootprintModel* robot_model) - { - robot_model_ = robot_model; - } - - /** - * @brief Set all parameters at once - * @param cfg TebConfig class - * @param robot_model Robot model required for distance calculation - * @param obstacle 2D position vector containing the position of the obstacle - */ - void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle) - { - cfg_ = &cfg; - robot_model_ = robot_model; - _measurement = obstacle; - } - -protected: - - const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - -/** - * @class EdgeInflatedObstacle - * @brief Edge defining the cost function for keeping a minimum distance from inflated obstacles. - * - * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n - * \f$ \min \textrm{penaltyBelow}( dist2point, min_obstacle_dist ) \cdot weight_inflation \f$. \n - * Additional, a second penalty is provided with \n - * \f$ \min \textrm{penaltyBelow}( dist2point, inflation_dist ) \cdot weight_inflation \f$. - * It is assumed that inflation_dist > min_obstacle_dist and weight_inflation << weight_inflation. - * \e dist2point denotes the minimum distance to the point obstacle. \n - * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n - * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeObstacle - * @remarks Do not forget to call setTebConfig() and setObstacle() - */ -class EdgeInflatedObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPose> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeInflatedObstacle() - { - _measurement = NULL; - } - - /** - * @brief Actual cost function - */ - void computeError() - { - ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeObstacle()"); - const VertexPose* bandpt = static_cast(_vertices[0]); - - double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement); - - _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); - _error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.inflation_dist, 0.0); - - - ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeObstacle::computeError() _error[0]=%f, _error[1]=%f\n",_error[0], _error[1]); - } - - /** - * @brief Set pointer to associated obstacle for the underlying cost function - * @param obstacle 2D position vector containing the position of the obstacle - */ - void setObstacle(const Obstacle* obstacle) - { - _measurement = obstacle; - } - - /** - * @brief Set pointer to the robot model - * @param robot_model Robot model required for distance calculation - */ - void setRobotModel(const BaseRobotFootprintModel* robot_model) - { - robot_model_ = robot_model; - } - - /** - * @brief Set all parameters at once - * @param cfg TebConfig class - * @param robot_model Robot model required for distance calculation - * @param obstacle 2D position vector containing the position of the obstacle - */ - void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle) - { - cfg_ = &cfg; - robot_model_ = robot_model; - _measurement = obstacle; - } - -protected: - - const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - -} // end namespace + /** + * @brief Set pointer to associated obstacle for the underlying cost function + * @param obstacle 2D position vector containing the position of the obstacle + */ + void setObstacle(const Obstacle *obstacle) { _measurement = obstacle; } + + /** + * @brief Set pointer to the robot model + * @param robot_model Robot model required for distance calculation + */ + void setRobotModel(const BaseRobotFootprintModel *robot_model) { robot_model_ = robot_model; } + + /** + * @brief Set all parameters at once + * @param cfg TebConfig class + * @param robot_model Robot model required for distance calculation + * @param obstacle 2D position vector containing the position of the obstacle + */ + void setParameters(const TebConfig &cfg, const BaseRobotFootprintModel *robot_model, const Obstacle *obstacle) + { + cfg_ = &cfg; + robot_model_ = robot_model; + _measurement = obstacle; + } + + protected: + const BaseRobotFootprintModel *robot_model_; //!< Store pointer to robot_model + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** + * @class EdgeInflatedObstacle + * @brief Edge defining the cost function for keeping a minimum distance from inflated obstacles. + * + * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n + * \f$ \min \textrm{penaltyBelow}( dist2point, min_obstacle_dist ) \cdot weight_inflation \f$. \n + * Additional, a second penalty is provided with \n + * \f$ \min \textrm{penaltyBelow}( dist2point, inflation_dist ) \cdot weight_inflation \f$. + * It is assumed that inflation_dist > min_obstacle_dist and weight_inflation << weight_inflation. + * \e dist2point denotes the minimum distance to the point obstacle. \n + * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n + * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeObstacle + * @remarks Do not forget to call setTebConfig() and setObstacle() + */ + class EdgeInflatedObstacle : public BaseTebUnaryEdge<2, const Obstacle *, VertexPose> + { + public: + /** + * @brief Construct edge. + */ + EdgeInflatedObstacle() { _measurement = NULL; } + + /** + * @brief Actual cost function + */ + void computeError() + { + ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeObstacle()"); + const VertexPose *bandpt = static_cast(_vertices[0]); + + double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement); + + double squared_inflation_error_limit = (cfg_->obstacles.inflation_dist - cfg_->obstacles.min_obstacle_dist) * + (cfg_->obstacles.inflation_dist - cfg_->obstacles.min_obstacle_dist) / 2; + if (dist < cfg_->obstacles.min_obstacle_dist) + _error[0] = + squared_inflation_error_limit; // + penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); + else + _error[0] = 0; + double squared_inflation_error = (cfg_->obstacles.inflation_dist - dist) * (cfg_->obstacles.inflation_dist - dist) / 2; + _error[1] = penaltyBoundFromAbove(squared_inflation_error, squared_inflation_error_limit, 0.0); + + ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeObstacle::computeError() _error[0]=%f, _error[1]=%f\n", + _error[0], _error[1]); + } + + /** + * @brief Set pointer to associated obstacle for the underlying cost function + * @param obstacle 2D position vector containing the position of the obstacle + */ + void setObstacle(const Obstacle *obstacle) { _measurement = obstacle; } + + /** + * @brief Set pointer to the robot model + * @param robot_model Robot model required for distance calculation + */ + void setRobotModel(const BaseRobotFootprintModel *robot_model) { robot_model_ = robot_model; } + + /** + * @brief Set all parameters at once + * @param cfg TebConfig class + * @param robot_model Robot model required for distance calculation + * @param obstacle 2D position vector containing the position of the obstacle + */ + void setParameters(const TebConfig &cfg, const BaseRobotFootprintModel *robot_model, const Obstacle *obstacle) + { + cfg_ = &cfg; + robot_model_ = robot_model; + _measurement = obstacle; + } + + protected: + const BaseRobotFootprintModel *robot_model_; //!< Store pointer to robot_model + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + +} // namespace teb_local_planner #endif diff --git a/include/teb_local_planner/g2o_types/edge_velocity.h b/include/teb_local_planner/g2o_types/edge_velocity.h index 47efcece..e7d87cbb 100755 --- a/include/teb_local_planner/g2o_types/edge_velocity.h +++ b/include/teb_local_planner/g2o_types/edge_velocity.h @@ -32,7 +32,7 @@ * 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. - * + * * Notes: * The following class is derived from a class defined by the * g2o-framework. g2o is licensed under the terms of the BSD License. @@ -44,81 +44,79 @@ #ifndef EDGE_VELOCITY_H #define EDGE_VELOCITY_H -#include -#include #include #include +#include +#include #include - #include namespace teb_local_planner { - - -/** - * @class EdgeVelocity - * @brief Edge defining the cost function for limiting the translational and rotational velocity. - * - * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$ and minimizes: \n - * \f$ \min \textrm{penaltyInterval}( [v,omega]^T ) \cdot weight \f$. \n - * \e v is calculated using the difference quotient and the position parts of both poses. \n - * \e omega is calculated using the difference quotient of both yaw angles followed by a normalization to [-pi, pi]. \n - * \e weight can be set using setInformation(). \n - * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n - * The dimension of the error / cost vector is 2: the first component represents the translational velocity and - * the second one the rotational velocity. - * @see TebOptimalPlanner::AddEdgesVelocity - * @remarks Do not forget to call setTebConfig() - */ -class EdgeVelocity : public BaseTebMultiEdge<2, double> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeVelocity() - { - this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices - } - - /** - * @brief Actual cost function - */ - void computeError() - { - ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - const VertexTimeDiff* deltaT = static_cast(_vertices[2]); - - const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position(); - - double dist = deltaS.norm(); - const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta()); - if (cfg_->trajectory.exact_arc_length && angle_diff != 0) + /** + * @class EdgeVelocity + * @brief Edge defining the cost function for limiting the translational and rotational velocity. + * + * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$ and minimizes: \n + * \f$ \min \textrm{penaltyInterval}( [v,omega]^T ) \cdot weight \f$. \n + * \e v is calculated using the difference quotient and the position parts of both poses. \n + * \e omega is calculated using the difference quotient of both yaw angles followed by a normalization to [-pi, pi]. \n + * \e weight can be set using setInformation(). \n + * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n + * The dimension of the error / cost vector is 2: the first component represents the translational velocity and + * the second one the rotational velocity. + * @see TebOptimalPlanner::AddEdgesVelocity + * @remarks Do not forget to call setTebConfig() + */ + class EdgeVelocity : public BaseTebMultiEdge<2, double> { - double radius = dist/(2*sin(angle_diff/2)); - dist = fabs( angle_diff * radius ); // actual arg length! - } - double vel = dist / deltaT->estimate(); - -// vel *= g2o::sign(deltaS[0]*cos(conf1->theta()) + deltaS[1]*sin(conf1->theta())); // consider direction - vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction - - const double omega = angle_diff / deltaT->estimate(); - - _error[0] = penaltyBoundToInterval(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon); - _error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon); + public: + /** + * @brief Construct edge. + */ + EdgeVelocity() + { + this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices + } - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]); - } + /** + * @brief Actual cost function + */ + void computeError() + { + ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()"); + const VertexPose *conf1 = static_cast(_vertices[0]); + const VertexPose *conf2 = static_cast(_vertices[1]); + const VertexTimeDiff *deltaT = static_cast(_vertices[2]); + + const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position(); + + double dist = deltaS.norm(); + const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta()); + if (cfg_->trajectory.exact_arc_length && angle_diff != 0) + { + double radius = dist / (2 * sin(angle_diff / 2)); + dist = fabs(angle_diff * radius); // actual arg length! + } + double vel = dist / deltaT->estimate(); + + // vel *= g2o::sign(deltaS[0]*cos(conf1->theta()) + deltaS[1]*sin(conf1->theta())); // consider direction + vel *= fast_sigmoid(100 * (deltaS.x() * cos(conf1->theta()) + deltaS.y() * sin(conf1->theta()))); // consider direction + + const double omega = angle_diff / deltaT->estimate(); + + _error[0] = penaltyBoundToInterval(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x, cfg_->optim.penalty_epsilon); + _error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta, cfg_->optim.penalty_epsilon) + + // penalize omega, if it would result in a smaller turning radius than possible + penaltyBoundFromAbove(fabs(omega), fabs(vel / cfg_->robot.min_turning_radius), 0.0); + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\n", _error[0], _error[1]); + } #ifdef USE_ANALYTIC_JACOBI -#if 0 //TODO the hardcoded jacobian does not include the changing direction (just the absolute value) - // Change accordingly... +#if 0 // TODO the hardcoded jacobian does not include the changing direction (just the absolute value) + // Change accordingly... /** * @brief Jacobi matrix of the cost function specified in computeError(). @@ -189,85 +187,73 @@ class EdgeVelocity : public BaseTebMultiEdge<2, double> } #endif #endif - - -public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + /** + * @class EdgeVelocityHolonomic + * @brief Edge defining the cost function for limiting the translational and rotational velocity according to x,y and theta. + * + * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$ and minimizes: \n + * \f$ \min \textrm{penaltyInterval}( [vx,vy,omega]^T ) \cdot weight \f$. \n + * \e vx denotes the translational velocity w.r.t. x-axis (computed using finite differneces). \n + * \e vy denotes the translational velocity w.r.t. y-axis (computed using finite differneces). \n + * \e omega is calculated using the difference quotient of both yaw angles followed by a normalization to [-pi, pi]. \n + * \e weight can be set using setInformation(). \n + * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n + * The dimension of the error / cost vector is 3: the first component represents the translational velocity w.r.t. x-axis, + * the second one w.r.t. the y-axis and the third one the rotational velocity. + * @see TebOptimalPlanner::AddEdgesVelocity + * @remarks Do not forget to call setTebConfig() + */ + class EdgeVelocityHolonomic : public BaseTebMultiEdge<3, double> + { + public: + /** + * @brief Construct edge. + */ + EdgeVelocityHolonomic() + { + this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices + } + /** + * @brief Actual cost function + */ + void computeError() + { + ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocityHolonomic()"); + const VertexPose *conf1 = static_cast(_vertices[0]); + const VertexPose *conf2 = static_cast(_vertices[1]); + const VertexTimeDiff *deltaT = static_cast(_vertices[2]); + Eigen::Vector2d deltaS = conf2->position() - conf1->position(); + double cos_theta1 = std::cos(conf1->theta()); + double sin_theta1 = std::sin(conf1->theta()); + // transform conf2 into current robot frame conf1 (inverse 2d rotation matrix) + double r_dx = cos_theta1 * deltaS.x() + sin_theta1 * deltaS.y(); + double r_dy = -sin_theta1 * deltaS.x() + cos_theta1 * deltaS.y(); + double vx = r_dx / deltaT->estimate(); + double vy = r_dy / deltaT->estimate(); + double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) / deltaT->estimate(); -/** - * @class EdgeVelocityHolonomic - * @brief Edge defining the cost function for limiting the translational and rotational velocity according to x,y and theta. - * - * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$ and minimizes: \n - * \f$ \min \textrm{penaltyInterval}( [vx,vy,omega]^T ) \cdot weight \f$. \n - * \e vx denotes the translational velocity w.r.t. x-axis (computed using finite differneces). \n - * \e vy denotes the translational velocity w.r.t. y-axis (computed using finite differneces). \n - * \e omega is calculated using the difference quotient of both yaw angles followed by a normalization to [-pi, pi]. \n - * \e weight can be set using setInformation(). \n - * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n - * The dimension of the error / cost vector is 3: the first component represents the translational velocity w.r.t. x-axis, - * the second one w.r.t. the y-axis and the third one the rotational velocity. - * @see TebOptimalPlanner::AddEdgesVelocity - * @remarks Do not forget to call setTebConfig() - */ -class EdgeVelocityHolonomic : public BaseTebMultiEdge<3, double> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeVelocityHolonomic() - { - this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices - } - - /** - * @brief Actual cost function - */ - void computeError() - { - ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocityHolonomic()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - const VertexTimeDiff* deltaT = static_cast(_vertices[2]); - Eigen::Vector2d deltaS = conf2->position() - conf1->position(); - - double cos_theta1 = std::cos(conf1->theta()); - double sin_theta1 = std::sin(conf1->theta()); - - // transform conf2 into current robot frame conf1 (inverse 2d rotation matrix) - double r_dx = cos_theta1*deltaS.x() + sin_theta1*deltaS.y(); - double r_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y(); - - double vx = r_dx / deltaT->estimate(); - double vy = r_dy / deltaT->estimate(); - double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) / deltaT->estimate(); - - _error[0] = penaltyBoundToInterval(vx, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x, cfg_->optim.penalty_epsilon); - _error[1] = penaltyBoundToInterval(vy, cfg_->robot.max_vel_y, 0.0); // we do not apply the penalty epsilon here, since the velocity could be close to zero - _error[2] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon); - - ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]) && std::isfinite(_error[2]), - "EdgeVelocityHolonomic::computeError() _error[0]=%f _error[1]=%f _error[2]=%f\n",_error[0],_error[1],_error[2]); - } - - -public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + _error[0] = penaltyBoundToInterval(vx, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x, cfg_->optim.penalty_epsilon); + _error[1] = penaltyBoundToInterval(vy, cfg_->robot.max_vel_y, + 0.0); // we do not apply the penalty epsilon here, since the velocity could be close to zero + _error[2] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta, cfg_->optim.penalty_epsilon); -}; + ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]) && std::isfinite(_error[2]), + "EdgeVelocityHolonomic::computeError() _error[0]=%f _error[1]=%f _error[2]=%f\n", _error[0], _error[1], _error[2]); + } + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; -} // end namespace +} // namespace teb_local_planner #endif diff --git a/include/teb_local_planner/g2o_types/penalties.h b/include/teb_local_planner/g2o_types/penalties.h index 68705584..67ef1c56 100755 --- a/include/teb_local_planner/g2o_types/penalties.h +++ b/include/teb_local_planner/g2o_types/penalties.h @@ -39,155 +39,170 @@ #ifndef PENALTIES_H #define PENALTIES_H -#include -#include #include +#include +#include namespace teb_local_planner { + /** + * @brief Linear penalty function for bounding \c var to the interval \f$ -a < var < a \f$ + * @param var The scalar that should be bounded + * @param a lower and upper absolute bound + * @param epsilon safty margin (move bound to the interior of the interval) + * @see penaltyBoundToIntervalDerivative + * @return Penalty / cost value that is nonzero if the constraint is not satisfied + */ + inline double penaltyBoundToInterval(const double &var, const double &a, const double &epsilon) + { + if (var < -a + epsilon) + { + return (-var - (a - epsilon)); + } + if (var <= a - epsilon) + { + return 0.; + } + else + { + return (var - (a - epsilon)); + } + } -/** - * @brief Linear penalty function for bounding \c var to the interval \f$ -a < var < a \f$ - * @param var The scalar that should be bounded - * @param a lower and upper absolute bound - * @param epsilon safty margin (move bound to the interior of the interval) - * @see penaltyBoundToIntervalDerivative - * @return Penalty / cost value that is nonzero if the constraint is not satisfied - */ -inline double penaltyBoundToInterval(const double& var,const double& a,const double& epsilon) -{ - if (var < -a+epsilon) - { - return (-var - (a - epsilon)); - } - if (var <= a-epsilon) - { - return 0.; - } - else - { - return (var - (a - epsilon)); - } -} - -/** - * @brief Linear penalty function for bounding \c var to the interval \f$ a < var < b \f$ - * @param var The scalar that should be bounded - * @param a lower bound - * @param b upper bound - * @param epsilon safty margin (move bound to the interior of the interval) - * @see penaltyBoundToIntervalDerivative - * @return Penalty / cost value that is nonzero if the constraint is not satisfied - */ -inline double penaltyBoundToInterval(const double& var,const double& a, const double& b, const double& epsilon) -{ - if (var < a+epsilon) - { - return (-var + (a + epsilon)); - } - if (var <= b-epsilon) - { - return 0.; - } - else - { - return (var - (b - epsilon)); - } -} - + /** + * @brief Linear penalty function for bounding \c var to the interval \f$ a < var < b \f$ + * @param var The scalar that should be bounded + * @param a lower bound + * @param b upper bound + * @param epsilon safty margin (move bound to the interior of the interval) + * @see penaltyBoundToIntervalDerivative + * @return Penalty / cost value that is nonzero if the constraint is not satisfied + */ + inline double penaltyBoundToInterval(const double &var, const double &a, const double &b, const double &epsilon) + { + if (var < a + epsilon) + { + return (-var + (a + epsilon)); + } + if (var <= b - epsilon) + { + return 0.; + } + else + { + return (var - (b - epsilon)); + } + } -/** - * @brief Linear penalty function for bounding \c var from below: \f$ a < var \f$ - * @param var The scalar that should be bounded - * @param a lower bound - * @param epsilon safty margin (move bound to the interior of the interval) - * @see penaltyBoundFromBelowDerivative - * @return Penalty / cost value that is nonzero if the constraint is not satisfied - */ -inline double penaltyBoundFromBelow(const double& var, const double& a,const double& epsilon) -{ - if (var >= a+epsilon) - { - return 0.; - } - else - { - return (-var + (a+epsilon)); - } -} + /** + * @brief Linear penalty function for bounding \c var from below: \f$ a < var \f$ + * @param var The scalar that should be bounded + * @param a lower bound + * @param epsilon safty margin (move bound to the interior of the interval) + * @see penaltyBoundFromBelowDerivative + * @return Penalty / cost value that is nonzero if the constraint is not satisfied + */ + inline double penaltyBoundFromBelow(const double &var, const double &a, const double &epsilon) + { + if (var >= a + epsilon) + { + return 0.; + } + else + { + return (-var + (a + epsilon)); + } + } -/** - * @brief Derivative of the linear penalty function for bounding \c var to the interval \f$ -a < var < a \f$ - * @param var The scalar that should be bounded - * @param a lower and upper absolute bound - * @param epsilon safty margin (move bound to the interior of the interval) - * @see penaltyBoundToInterval - * @return Derivative of the penalty function w.r.t. \c var - */ -inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& epsilon) -{ - if (var < -a+epsilon) - { - return -1; - } - if (var <= a-epsilon) - { - return 0.; - } - else - { - return 1; - } -} + /** + * @brief Linear penalty function for bounding \c var from above: \f$ a < var \f$ + * @param var The scalar that should be bounded + * @param a lower bound + * @param epsilon safty margin (move bound to the interior of the interval) + * @see penaltyBoundFromBelowDerivative + * @return Penalty / cost value that is nonzero if the constraint is not satisfied + */ + inline double penaltyBoundFromAbove(const double &var, const double &a, const double &epsilon) + { + if (var <= a + epsilon) + { + return 0.; + } + else + { + return (var - (a + epsilon)); + } + } -/** - * @brief Derivative of the linear penalty function for bounding \c var to the interval \f$ a < var < b \f$ - * @param var The scalar that should be bounded - * @param a lower bound - * @param b upper bound - * @param epsilon safty margin (move bound to the interior of the interval) - * @see penaltyBoundToInterval - * @return Derivative of the penalty function w.r.t. \c var - */ -inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& b, const double& epsilon) -{ - if (var < a+epsilon) - { - return -1; - } - if (var <= b-epsilon) - { - return 0.; - } - else - { - return 1; - } -} - - -/** - * @brief Derivative of the linear penalty function for bounding \c var from below: \f$ a < var \f$ - * @param var The scalar that should be bounded - * @param a lower bound - * @param epsilon safty margin (move bound to the interior of the interval) - * @see penaltyBoundFromBelow - * @return Derivative of the penalty function w.r.t. \c var - */ -inline double penaltyBoundFromBelowDerivative(const double& var, const double& a,const double& epsilon) -{ - if (var >= a+epsilon) - { - return 0.; - } - else - { - return -1; - } -} + /** + * @brief Derivative of the linear penalty function for bounding \c var to the interval \f$ -a < var < a \f$ + * @param var The scalar that should be bounded + * @param a lower and upper absolute bound + * @param epsilon safty margin (move bound to the interior of the interval) + * @see penaltyBoundToInterval + * @return Derivative of the penalty function w.r.t. \c var + */ + inline double penaltyBoundToIntervalDerivative(const double &var, const double &a, const double &epsilon) + { + if (var < -a + epsilon) + { + return -1; + } + if (var <= a - epsilon) + { + return 0.; + } + else + { + return 1; + } + } + /** + * @brief Derivative of the linear penalty function for bounding \c var to the interval \f$ a < var < b \f$ + * @param var The scalar that should be bounded + * @param a lower bound + * @param b upper bound + * @param epsilon safty margin (move bound to the interior of the interval) + * @see penaltyBoundToInterval + * @return Derivative of the penalty function w.r.t. \c var + */ + inline double penaltyBoundToIntervalDerivative(const double &var, const double &a, const double &b, const double &epsilon) + { + if (var < a + epsilon) + { + return -1; + } + if (var <= b - epsilon) + { + return 0.; + } + else + { + return 1; + } + } -} // namespace teb_local_planner + /** + * @brief Derivative of the linear penalty function for bounding \c var from below: \f$ a < var \f$ + * @param var The scalar that should be bounded + * @param a lower bound + * @param epsilon safty margin (move bound to the interior of the interval) + * @see penaltyBoundFromBelow + * @return Derivative of the penalty function w.r.t. \c var + */ + inline double penaltyBoundFromBelowDerivative(const double &var, const double &a, const double &epsilon) + { + if (var >= a + epsilon) + { + return 0.; + } + else + { + return -1; + } + } +} // namespace teb_local_planner -#endif // PENALTIES_H +#endif // PENALTIES_H diff --git a/include/teb_local_planner/teb_config.h b/include/teb_local_planner/teb_config.h index 98643956..46811dbc 100644 --- a/include/teb_local_planner/teb_config.h +++ b/include/teb_local_planner/teb_config.h @@ -46,340 +46,408 @@ #include - // Definitions -#define USE_ANALYTIC_JACOBI // if available for a specific edge, use analytic jacobi - +#define USE_ANALYTIC_JACOBI // if available for a specific edge, use analytic jacobi namespace teb_local_planner { + /** + * @class TebConfig + * @brief Config class for the teb_local_planner and its components. + */ + class TebConfig + { + public: + std::string odom_topic; //!< Topic name of the odometry message, provided by the robot driver or simulator + std::string map_frame; //!< Global planning frame -/** - * @class TebConfig - * @brief Config class for the teb_local_planner and its components. - */ -class TebConfig -{ -public: - - std::string odom_topic; //!< Topic name of the odometry message, provided by the robot driver or simulator - std::string map_frame; //!< Global planning frame - - //! Trajectory related parameters - struct Trajectory - { - double teb_autosize; //!< Enable automatic resizing of the trajectory w.r.t to the temporal resolution (recommended) - double dt_ref; //!< Desired temporal resolution of the trajectory (should be in the magniture of the underlying control rate) - double dt_hysteresis; //!< Hysteresis for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref - int min_samples; //!< Minimum number of samples (should be always greater than 2) - int max_samples; //!< Maximum number of samples; Warning: if too small the discretization/resolution might not be sufficient for the given robot model or obstacle avoidance does not work anymore. - bool global_plan_overwrite_orientation; //!< Overwrite orientation of local subgoals provided by the global planner - bool allow_init_with_backwards_motion; //!< If true, the underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors) - double global_plan_viapoint_sep; //!< Min. separation between each two consecutive via-points extracted from the global plan (if negative: disabled) - bool via_points_ordered; //!< If true, the planner adheres to the order of via-points in the storage container - double max_global_plan_lookahead_dist; //!< Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if <=0: disabled; the length is also bounded by the local costmap size!] - bool exact_arc_length; //!< If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used. - double force_reinit_new_goal_dist; //!< Reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting) - int feasibility_check_no_poses; //!< Specify up to which pose on the predicted plan the feasibility should be checked each sampling interval. - bool publish_feedback; //!< Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes) - } trajectory; //!< Trajectory related parameters - - //! Robot related parameters - struct Robot - { - double max_vel_x; //!< Maximum translational velocity of the robot - double max_vel_x_backwards; //!< Maximum translational velocity of the robot for driving backwards - double max_vel_y; //!< Maximum strafing velocity of the robot (should be zero for non-holonomic robots!) - double max_vel_theta; //!< Maximum angular velocity of the robot - double acc_lim_x; //!< Maximum translational acceleration of the robot - double acc_lim_y; //!< Maximum strafing acceleration of the robot - double acc_lim_theta; //!< Maximum angular acceleration of the robot - double min_turning_radius; //!< Minimum turning radius of a carlike robot (diff-drive robot: zero); - double wheelbase; //!< The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots! - bool cmd_angle_instead_rotvel; //!< Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance') - bool is_footprint_dynamic; // currently only activated if an oscillation is detected, see 'oscillation_recovery' - - double weight_adapt_factor; //!< Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new = weight_old*factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem. - } optim; //!< Optimization related parameters - - - struct HomotopyClasses - { - bool enable_homotopy_class_planning; //!< Activate homotopy class planning (Requires much more resources that simple planning, since multiple trajectories are optimized at once). - bool enable_multithreading; //!< Activate multiple threading for planning multiple trajectories in parallel. - bool simple_exploration; //!< If true, distinctive trajectories are explored using a simple left-right approach (pass each obstacle on the left or right side) for path generation, otherwise sample possible roadmaps randomly in a specified region between start and goal. - int max_number_classes; //!< Specify the maximum number of allowed alternative homotopy classes (limits computational effort) - double selection_cost_hysteresis; //!< Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor). - double selection_prefer_initial_plan; //!< Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan. - double selection_obst_cost_scale; //!< Extra scaling of obstacle cost terms just for selecting the 'best' candidate. - double selection_viapoint_cost_scale; //!< Extra scaling of via-point cost terms just for selecting the 'best' candidate. - bool selection_alternative_time_cost; //!< If true, time cost is replaced by the total transition time. - double switching_blocking_period; //!< Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed - - int roadmap_graph_no_samples; //! < Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off. - double roadmap_graph_area_width; //!< Random keypoints/waypoints are sampled in a rectangular region between start and goal. Specify the width of that region in meters. - double roadmap_graph_area_length_scale; //!< The length of the rectangular region is determined by the distance between start and goal. This parameter further scales the distance such that the geometric center remains equal! - double h_signature_prescaler; //!< Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2TebConfig Constructor defaults << dynamic_reconfigure defaults << rosparam server defaults - */ - TebConfig() - { - - odom_topic = "odom"; - map_frame = "odom"; - - // Trajectory - - trajectory.teb_autosize = true; - trajectory.dt_ref = 0.3; - trajectory.dt_hysteresis = 0.1; - trajectory.min_samples = 3; - trajectory.max_samples = 500; - trajectory.global_plan_overwrite_orientation = true; - trajectory.allow_init_with_backwards_motion = false; - trajectory.global_plan_viapoint_sep = -1; - trajectory.via_points_ordered = false; - trajectory.max_global_plan_lookahead_dist = 1; - trajectory.exact_arc_length = false; - trajectory.force_reinit_new_goal_dist = 1; - trajectory.feasibility_check_no_poses = 5; - trajectory.publish_feedback = false; - - // Robot - - robot.max_vel_x = 0.4; - robot.max_vel_x_backwards = 0.2; - robot.max_vel_y = 0.0; - robot.max_vel_theta = 0.3; - robot.acc_lim_x = 0.5; - robot.acc_lim_y = 0.5; - robot.acc_lim_theta = 0.5; - robot.min_turning_radius = 0; - robot.wheelbase = 1.0; - robot.cmd_angle_instead_rotvel = false; - robot.is_footprint_dynamic = false; - - // GoalTolerance - - goal_tolerance.xy_goal_tolerance = 0.2; - goal_tolerance.yaw_goal_tolerance = 0.2; - goal_tolerance.free_goal_vel = false; - goal_tolerance.complete_global_plan = true; - - // Obstacles - - obstacles.min_obstacle_dist = 0.5; - obstacles.inflation_dist = 0.6; - obstacles.dynamic_obstacle_inflation_dist = 0.6; - obstacles.include_dynamic_obstacles = true; - obstacles.include_costmap_obstacles = true; - obstacles.costmap_obstacles_behind_robot_dist = 1.5; - obstacles.obstacle_poses_affected = 25; - obstacles.legacy_obstacle_association = false; - obstacles.obstacle_association_force_inclusion_factor = 1.5; - obstacles.obstacle_association_cutoff_factor = 5; - obstacles.costmap_converter_plugin = ""; - obstacles.costmap_converter_spin_thread = true; - obstacles.costmap_converter_rate = 5; - - // Optimization - - optim.no_inner_iterations = 5; - optim.no_outer_iterations = 4; - optim.optimization_activate = true; - optim.optimization_verbose = false; - optim.penalty_epsilon = 0.1; - optim.weight_max_vel_x = 2; //1 - optim.weight_max_vel_y = 2; - optim.weight_max_vel_theta = 1; - optim.weight_acc_lim_x = 1; - optim.weight_acc_lim_y = 1; - optim.weight_acc_lim_theta = 1; - optim.weight_kinematics_nh = 1000; - optim.weight_kinematics_forward_drive = 1; - optim.weight_kinematics_turning_radius = 1; - optim.weight_optimaltime = 1; - optim.weight_obstacle = 50; - optim.weight_inflation = 0.1; - optim.weight_dynamic_obstacle = 50; - optim.weight_dynamic_obstacle_inflation = 0.1; - optim.weight_viapoint = 1; - optim.weight_prefer_rotdir = 50; - - optim.weight_adapt_factor = 2.0; - - // Homotopy Class Planner - - hcp.enable_homotopy_class_planning = true; - hcp.enable_multithreading = true; - hcp.simple_exploration = false; - hcp.max_number_classes = 5; - hcp.selection_cost_hysteresis = 1.0; - hcp.selection_prefer_initial_plan = 0.95; - hcp.selection_obst_cost_scale = 100.0; - hcp.selection_viapoint_cost_scale = 1.0; - hcp.selection_alternative_time_cost = false; - - hcp.obstacle_keypoint_offset = 0.1; - hcp.obstacle_heading_threshold = 0.45; - hcp.roadmap_graph_no_samples = 15; - hcp.roadmap_graph_area_width = 6; // [m] - hcp.roadmap_graph_area_length_scale = 1.0; - hcp.h_signature_prescaler = 1; - hcp.h_signature_threshold = 0.1; - hcp.switching_blocking_period = 0.0; - - hcp.viapoints_all_candidates = true; - - hcp.visualize_hc_graph = false; - hcp.visualize_with_time_as_z_axis_scale = 0.0; - - // Recovery - - recovery.shrink_horizon_backup = true; - recovery.shrink_horizon_min_duration = 10; - recovery.oscillation_recovery = true; - recovery.oscillation_v_eps = 0.1; - recovery.oscillation_omega_eps = 0.1; - recovery.oscillation_recovery_min_duration = 10; - recovery.oscillation_filter_duration = 10; - - - } - - /** - * @brief Load parmeters from the ros param server. - * @param nh const reference to the local ros::NodeHandle - */ - void loadRosParamFromNodeHandle(const ros::NodeHandle& nh); - - /** - * @brief Reconfigure parameters from the dynamic_reconfigure config. - * Change parameters dynamically (e.g. with rosrun rqt_reconfigure rqt_reconfigure). - * A reconfigure server needs to be instantiated that calls this method in it's callback. - * In case of the plugin \e teb_local_planner default values are defined - * in \e PROJECT_SRC/cfg/TebLocalPlannerReconfigure.cfg. - * @param cfg Config class autogenerated by dynamic_reconfigure according to the cfg-file mentioned above. - */ - void reconfigure(TebLocalPlannerReconfigureConfig& cfg); - - /** - * @brief Check parameters and print warnings in case of discrepancies - * - * Call this method whenever parameters are changed using public interfaces to inform the user - * about some improper uses. - */ - void checkParameters() const; - - /** - * @brief Check if some deprecated parameters are found and print warnings - * @param nh const reference to the local ros::NodeHandle - */ - void checkDeprecated(const ros::NodeHandle& nh) const; - - /** - * @brief Return the internal config mutex - */ - boost::mutex& configMutex() {return config_mutex_;} - -private: - boost::mutex config_mutex_; //!< Mutex for config accesses and changes - -}; - - -} // namespace teb_local_planner + //! Trajectory related parameters + struct Trajectory + { + double teb_autosize; //!< Enable automatic resizing of the trajectory w.r.t to the temporal resolution (recommended) + double dt_ref; //!< Desired temporal resolution of the trajectory (should be in the magniture of the underlying + //!< control rate) + double dt_hysteresis; //!< Hysteresis for automatic resizing depending on the current temporal resolution (dt): + //!< usually 10% of dt_ref + int min_samples; //!< Minimum number of samples (should be always greater than 2) + int max_samples; //!< Maximum number of samples; Warning: if too small the discretization/resolution might not be + //!< sufficient for the given robot model or obstacle avoidance does not work anymore. + bool global_plan_overwrite_orientation; //!< Overwrite orientation of local subgoals provided by the global planner + bool allow_init_with_backwards_motion; //!< If true, the underlying trajectories might be initialized with backwards + //!< motions in case the goal is behind the start within the local costmap + //!< (this is only recommended if the robot is equipped with rear sensors) + double global_plan_viapoint_sep; //!< Min. separation between each two consecutive via-points extracted from the + //!< global plan (if negative: disabled) + bool via_points_ordered; //!< If true, the planner adheres to the order of via-points in the storage container + double max_global_plan_lookahead_dist; //!< Specify maximum length (cumulative Euclidean distances) of the subset + //!< of the global plan taken into account for optimization [if <=0: + //!< disabled; the length is also bounded by the local costmap size!] + bool exact_arc_length; //!< If true, the planner uses the exact arc length in velocity, acceleration and turning + //!< rate computations [-> increased cpu time], otherwise the euclidean approximation is + //!< used. + double force_reinit_new_goal_dist; //!< Reinitialize the trajectory if a previous goal is updated with a + //!< seperation of more than the specified value in meters (skip hot-starting) + int feasibility_check_no_poses; //!< Specify up to which pose on the predicted plan the feasibility should be + //!< checked each sampling interval. + bool publish_feedback; //!< Publish planner feedback containing the full trajectory and a list of active + //!< obstacles (should be enabled only for evaluation or debugging purposes) + } trajectory; //!< Trajectory related parameters + + //! Robot related parameters + struct Robot + { + double max_vel_x; //!< Maximum translational velocity of the robot + double max_vel_x_backwards; //!< Maximum translational velocity of the robot for driving backwards + double max_vel_y; //!< Maximum strafing velocity of the robot (should be zero for non-holonomic robots!) + double max_vel_theta; //!< Maximum angular velocity of the robot + double acc_lim_x; //!< Maximum translational acceleration of the robot + double acc_lim_y; //!< Maximum strafing acceleration of the robot + double acc_lim_theta; //!< Maximum angular acceleration of the robot + double min_turning_radius; //!< Minimum turning radius of a carlike robot (diff-drive robot: zero); + double wheelbase; //!< The distance between the drive shaft and steering axle (only required for a carlike robot with + //!< 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots! + double max_steering_angle; //!< Maximum steering angle of the robot + double max_steering_rate; //!< Maximum steering rate of the robot + bool cmd_angle_instead_rotvel; //!< Substitute the rotational velocity in the commanded velocity message by the + //!< corresponding steering angle (check 'axles_distance') + bool is_footprint_dynamic; //!< If true, updated the footprint before checking trajectory feasibility + } robot; //!< Robot related parameters + + //! Goal tolerance related parameters + struct GoalTolerance + { + double yaw_goal_tolerance; //!< Allowed final orientation error + double xy_goal_tolerance; //!< Allowed final euclidean distance to the goal position + bool free_goal_vel; //!< Allow the robot's velocity to be nonzero (usally max_vel) for planning purposes + bool complete_global_plan; // true prevents the robot from ending the path early when it cross the end goal + } goal_tolerance; //!< Goal tolerance related parameters + + //! Obstacle related parameters + struct Obstacles + { + double min_obstacle_dist; //!< Minimum desired separation from obstacles + double inflation_dist; //!< buffer zone around obstacles with non-zero penalty costs (should be larger than + //!< min_obstacle_dist in order to take effect) + double dynamic_obstacle_inflation_dist; //!< Buffer zone around predicted locations of dynamic obstacles with + //!< non-zero penalty costs (should be larger than min_obstacle_dist in + //!< order to take effect) + bool include_dynamic_obstacles; //!< Specify whether the movement of dynamic obstacles should be predicted by a + //!< constant velocity model (this also effects homotopy class planning); If + //!< false, all obstacles are considered to be static. + bool include_costmap_obstacles; //!< Specify whether the obstacles in the costmap should be taken into account + //!< directly + double costmap_obstacles_behind_robot_dist; //!< Limit the occupied local costmap obstacles taken into account + //!< for planning behind the robot (specify distance in meters) + int obstacle_poses_affected; //!< The obstacle position is attached to the closest pose on the trajectory to reduce + //!< computational effort, but take a number of neighbors into account as well + bool legacy_obstacle_association; //!< If true, the old association strategy is used (for each obstacle, find the + //!< nearest TEB pose), otherwise the new one (for each teb pose, find only + //!< "relevant" obstacles). + double obstacle_association_force_inclusion_factor; //!< The non-legacy obstacle association technique tries to + //!< connect only relevant obstacles with the discretized + //!< trajectory during optimization, all obstacles within a + //!< specifed distance are forced to be included (as a multiple of + //!< min_obstacle_dist), e.g. choose 2.0 in order to consider + //!< obstacles within a radius of 2.0*min_obstacle_dist. + double obstacle_association_cutoff_factor; //!< See obstacle_association_force_inclusion_factor, but beyond a + //!< multiple of [value]*min_obstacle_dist all obstacles are ignored + //!< during optimization. obstacle_association_force_inclusion_factor + //!< is processed first. + std::string costmap_converter_plugin; //!< Define a plugin name of the costmap_converter package (costmap cells + //!< are converted to points/lines/polygons) + bool costmap_converter_spin_thread; //!< If \c true, the costmap converter invokes its callback queue in a + //!< different thread + int costmap_converter_rate; //!< The rate that defines how often the costmap_converter plugin processes the current + //!< costmap (the value should not be much higher than the costmap update rate) + } obstacles; //!< Obstacle related parameters + + //! Optimization related parameters + struct Optimization + { + int no_inner_iterations; //!< Number of solver iterations called in each outerloop iteration + int no_outer_iterations; //!< Each outerloop iteration automatically resizes the trajectory and invokes the + //!< internal optimizer with no_inner_iterations + + bool optimization_activate; //!< Activate the optimization + bool optimization_verbose; //!< Print verbose information + + double penalty_epsilon; //!< Add a small safety margin to penalty functions for hard-constraint approximations + + double weight_max_vel_x; //!< Optimization weight for satisfying the maximum allowed translational velocity + double weight_max_vel_y; //!< Optimization weight for satisfying the maximum allowed strafing velocity (in use + //!< only for holonomic robots) + double weight_max_vel_theta; //!< Optimization weight for satisfying the maximum allowed angular velocity + double weight_acc_lim_x; //!< Optimization weight for satisfying the maximum allowed translational acceleration + double weight_acc_lim_y; //!< Optimization weight for satisfying the maximum allowed strafing acceleration (in + //!< use only for holonomic robots) + double weight_acc_lim_theta; //!< Optimization weight for satisfying the maximum allowed angular acceleration + double weight_kinematics_nh; //!< Optimization weight for satisfying the non-holonomic kinematics + double weight_kinematics_forward_drive; //!< Optimization weight for forcing the robot to choose only forward + //!< directions (positive transl. velocities, only diffdrive robot) + double weight_kinematics_turning_radius; //!< Optimization weight for enforcing a minimum turning radius (carlike + //!< robots) + double weight_max_steering_angle; //!< Optimization weight for enforcing a maximum steering angle (carlike robots) + double weight_max_steering_rate; //!< Optimization weight for enforcing a maximum steering rate (carlike robots) + double weight_optimaltime; //!< Optimization weight for contracting the trajectory w.r.t transition time + double weight_obstacle; //!< Optimization weight for satisfying a minimum separation from obstacles + double weight_inflation; //!< Optimization weight for the inflation penalty (should be small) + double weight_dynamic_obstacle; //!< Optimization weight for satisfying a minimum separation from dynamic obstacles + double weight_dynamic_obstacle_inflation; //!< Optimization weight for the inflation penalty of dynamic obstacles + //!< (should be small) + double weight_viapoint; //!< Optimization weight for minimizing the distance to via-points + double weight_prefer_rotdir; //!< Optimization weight for preferring a specific turning direction (-> currently + //!< only activated if an oscillation is detected, see 'oscillation_recovery' + + double weight_adapt_factor; //!< Some special weights (currently 'weight_obstacle') are repeatedly scaled by this + //!< factor in each outer TEB iteration (weight_new = weight_old*factor); Increasing + //!< weights iteratively instead of setting a huge value a-priori leads to better + //!< numerical conditions of the underlying optimization problem. + } optim; //!< Optimization related parameters + + struct HomotopyClasses + { + bool enable_homotopy_class_planning; //!< Activate homotopy class planning (Requires much more resources that + //!< simple planning, since multiple trajectories are optimized at once). + bool enable_multithreading; //!< Activate multiple threading for planning multiple trajectories in parallel. + bool simple_exploration; //!< If true, distinctive trajectories are explored using a simple left-right approach + //!< (pass each obstacle on the left or right side) for path generation, otherwise + //!< sample possible roadmaps randomly in a specified region between start and goal. + int max_number_classes; //!< Specify the maximum number of allowed alternative homotopy classes (limits + //!< computational effort) + double selection_cost_hysteresis; //!< Specify how much trajectory cost must a new candidate have w.r.t. a + //!< previously selected trajectory in order to be selected (selection if + //!< new_cost < old_cost*factor). + double selection_prefer_initial_plan; //!< Specify a cost reduction in the interval (0,1) for the trajectory in + //!< the equivalence class of the initial plan. + double selection_obst_cost_scale; //!< Extra scaling of obstacle cost terms just for selecting the 'best' candidate. + double selection_viapoint_cost_scale; //!< Extra scaling of via-point cost terms just for selecting the 'best' + //!< candidate. + bool selection_alternative_time_cost; //!< If true, time cost is replaced by the total transition time. + double switching_blocking_period; //!< Specify a time duration in seconds that needs to be expired before a + //!< switch to new equivalence class is allowed + + int roadmap_graph_no_samples; //! < Specify the number of samples generated for creating the roadmap graph, if + //! simple_exploration is turend off. + double roadmap_graph_area_width; //!< Random keypoints/waypoints are sampled in a rectangular region between + //!< start and goal. Specify the width of that region in meters. + double roadmap_graph_area_length_scale; //!< The length of the rectangular region is determined by the distance + //!< between start and goal. This parameter further scales the distance + //!< such that the geometric center remains equal! + double h_signature_prescaler; //!< Scale number of obstacle value in order to allow huge number of obstacles. Do + //!< not choose it extremly low, otherwise obstacles cannot be distinguished from + //!< each other (0.2TebConfig Constructor defaults << + *dynamic_reconfigure defaults << rosparam server defaults + */ + TebConfig() + { + odom_topic = "odom"; + map_frame = "odom"; + + // Trajectory + + trajectory.teb_autosize = true; + trajectory.dt_ref = 0.3; + trajectory.dt_hysteresis = 0.1; + trajectory.min_samples = 3; + trajectory.max_samples = 500; + trajectory.global_plan_overwrite_orientation = true; + trajectory.allow_init_with_backwards_motion = false; + trajectory.global_plan_viapoint_sep = -1; + trajectory.via_points_ordered = false; + trajectory.max_global_plan_lookahead_dist = 1; + trajectory.exact_arc_length = false; + trajectory.force_reinit_new_goal_dist = 1; + trajectory.feasibility_check_no_poses = 5; + trajectory.publish_feedback = false; + + // Robot + + robot.max_vel_x = 0.4; + robot.max_vel_x_backwards = 0.2; + robot.max_vel_y = 0.0; + robot.max_vel_theta = 0.3; + robot.acc_lim_x = 0.5; + robot.acc_lim_y = 0.5; + robot.acc_lim_theta = 0.5; + robot.min_turning_radius = 0; + robot.wheelbase = 1.0; + robot.cmd_angle_instead_rotvel = false; + robot.is_footprint_dynamic = false; + + // GoalTolerance + + goal_tolerance.xy_goal_tolerance = 0.2; + goal_tolerance.yaw_goal_tolerance = 0.2; + goal_tolerance.free_goal_vel = false; + goal_tolerance.complete_global_plan = true; + + // Obstacles + + obstacles.min_obstacle_dist = 0.5; + obstacles.inflation_dist = 0.6; + obstacles.dynamic_obstacle_inflation_dist = 0.6; + obstacles.include_dynamic_obstacles = true; + obstacles.include_costmap_obstacles = true; + obstacles.costmap_obstacles_behind_robot_dist = 1.5; + obstacles.obstacle_poses_affected = 25; + obstacles.legacy_obstacle_association = false; + obstacles.obstacle_association_force_inclusion_factor = 1.5; + obstacles.obstacle_association_cutoff_factor = 5; + obstacles.costmap_converter_plugin = ""; + obstacles.costmap_converter_spin_thread = true; + obstacles.costmap_converter_rate = 5; + + // Optimization + + optim.no_inner_iterations = 5; + optim.no_outer_iterations = 4; + optim.optimization_activate = true; + optim.optimization_verbose = false; + optim.penalty_epsilon = 0.1; + optim.weight_max_vel_x = 2; // 1 + optim.weight_max_vel_y = 2; + optim.weight_max_vel_theta = 1; + optim.weight_acc_lim_x = 1; + optim.weight_acc_lim_y = 1; + optim.weight_acc_lim_theta = 1; + optim.weight_kinematics_nh = 1000; + optim.weight_kinematics_forward_drive = 1; + optim.weight_kinematics_turning_radius = 1; + optim.weight_optimaltime = 1; + optim.weight_obstacle = 50; + optim.weight_inflation = 0.1; + optim.weight_dynamic_obstacle = 50; + optim.weight_dynamic_obstacle_inflation = 0.1; + optim.weight_viapoint = 1; + optim.weight_prefer_rotdir = 50; + + optim.weight_adapt_factor = 2.0; + + // Homotopy Class Planner + + hcp.enable_homotopy_class_planning = true; + hcp.enable_multithreading = true; + hcp.simple_exploration = false; + hcp.max_number_classes = 5; + hcp.selection_cost_hysteresis = 1.0; + hcp.selection_prefer_initial_plan = 0.95; + hcp.selection_obst_cost_scale = 100.0; + hcp.selection_viapoint_cost_scale = 1.0; + hcp.selection_alternative_time_cost = false; + + hcp.obstacle_keypoint_offset = 0.1; + hcp.obstacle_heading_threshold = 0.45; + hcp.roadmap_graph_no_samples = 15; + hcp.roadmap_graph_area_width = 6; // [m] + hcp.roadmap_graph_area_length_scale = 1.0; + hcp.h_signature_prescaler = 1; + hcp.h_signature_threshold = 0.1; + hcp.switching_blocking_period = 0.0; + + hcp.viapoints_all_candidates = true; + + hcp.visualize_hc_graph = false; + hcp.visualize_with_time_as_z_axis_scale = 0.0; + + // Recovery + + recovery.shrink_horizon_backup = true; + recovery.shrink_horizon_min_duration = 10; + recovery.oscillation_recovery = true; + recovery.oscillation_v_eps = 0.1; + recovery.oscillation_omega_eps = 0.1; + recovery.oscillation_recovery_min_duration = 10; + recovery.oscillation_filter_duration = 10; + } + + /** + * @brief Load parmeters from the ros param server. + * @param nh const reference to the local ros::NodeHandle + */ + void loadRosParamFromNodeHandle(const ros::NodeHandle &nh); + + /** + * @brief Reconfigure parameters from the dynamic_reconfigure config. + * Change parameters dynamically (e.g. with rosrun rqt_reconfigure rqt_reconfigure). + * A reconfigure server needs to be instantiated that calls this method in it's callback. + * In case of the plugin \e teb_local_planner default values are defined + * in \e PROJECT_SRC/cfg/TebLocalPlannerReconfigure.cfg. + * @param cfg Config class autogenerated by dynamic_reconfigure according to the cfg-file mentioned above. + */ + void reconfigure(TebLocalPlannerReconfigureConfig &cfg); + + /** + * @brief Check parameters and print warnings in case of discrepancies + * + * Call this method whenever parameters are changed using public interfaces to inform the user + * about some improper uses. + */ + void checkParameters() const; + + /** + * @brief Check if some deprecated parameters are found and print warnings + * @param nh const reference to the local ros::NodeHandle + */ + void checkDeprecated(const ros::NodeHandle &nh) const; + + /** + * @brief Return the internal config mutex + */ + boost::mutex &configMutex() { return config_mutex_; } + + private: + boost::mutex config_mutex_; //!< Mutex for config accesses and changes + }; + +} // namespace teb_local_planner #endif diff --git a/src/optimal_planner.cpp b/src/optimal_planner.cpp index bbc7e517..5abb540c 100644 --- a/src/optimal_planner.cpp +++ b/src/optimal_planner.cpp @@ -37,1281 +37,1278 @@ *********************************************************************/ #include -#include #include - +#include namespace teb_local_planner { + // ============== Implementation =================== -// ============== Implementation =================== + TebOptimalPlanner::TebOptimalPlanner() + : cfg_(NULL), obstacles_(NULL), via_points_(NULL), cost_(HUGE_VAL), prefer_rotdir_(RotType::none), robot_model_(new PointRobotFootprint()), + initialized_(false), optimized_(false) + {} -TebOptimalPlanner::TebOptimalPlanner() : cfg_(NULL), obstacles_(NULL), via_points_(NULL), cost_(HUGE_VAL), prefer_rotdir_(RotType::none), - robot_model_(new PointRobotFootprint()), initialized_(false), optimized_(false) -{ -} - -TebOptimalPlanner::TebOptimalPlanner(const TebConfig& cfg, ObstContainer* obstacles, RobotFootprintModelPtr robot_model, TebVisualizationPtr visual, const ViaPointContainer* via_points) -{ - initialize(cfg, obstacles, robot_model, visual, via_points); -} + TebOptimalPlanner::TebOptimalPlanner(const TebConfig &cfg, ObstContainer *obstacles, RobotFootprintModelPtr robot_model, + TebVisualizationPtr visual, const ViaPointContainer *via_points) + { + initialize(cfg, obstacles, robot_model, visual, via_points); + } -TebOptimalPlanner::~TebOptimalPlanner() -{ - clearGraph(); - // free dynamically allocated memory - //if (optimizer_) - // g2o::Factory::destroy(); - //g2o::OptimizationAlgorithmFactory::destroy(); - //g2o::HyperGraphActionLibrary::destroy(); -} - -void TebOptimalPlanner::initialize(const TebConfig& cfg, ObstContainer* obstacles, RobotFootprintModelPtr robot_model, TebVisualizationPtr visual, const ViaPointContainer* via_points) -{ - // init optimizer (set solver and block ordering settings) - optimizer_ = initOptimizer(); - - cfg_ = &cfg; - obstacles_ = obstacles; - robot_model_ = robot_model; - via_points_ = via_points; - cost_ = HUGE_VAL; - prefer_rotdir_ = RotType::none; - setVisualization(visual); - - vel_start_.first = true; - vel_start_.second.linear.x = 0; - vel_start_.second.linear.y = 0; - vel_start_.second.angular.z = 0; - - vel_goal_.first = true; - vel_goal_.second.linear.x = 0; - vel_goal_.second.linear.y = 0; - vel_goal_.second.angular.z = 0; - initialized_ = true; -} - - -void TebOptimalPlanner::setVisualization(TebVisualizationPtr visualization) -{ - visualization_ = visualization; -} + TebOptimalPlanner::~TebOptimalPlanner() + { + clearGraph(); + // free dynamically allocated memory + // if (optimizer_) + // g2o::Factory::destroy(); + // g2o::OptimizationAlgorithmFactory::destroy(); + // g2o::HyperGraphActionLibrary::destroy(); + } -void TebOptimalPlanner::visualize() -{ - if (!visualization_) - return; - - visualization_->publishLocalPlanAndPoses(teb_); - - if (teb_.sizePoses() > 0) - visualization_->publishRobotFootprintModel(teb_.Pose(0), *robot_model_); - - if (cfg_->trajectory.publish_feedback) - visualization_->publishFeedbackMessage(*this, *obstacles_); - -} - - -/* - * registers custom vertices and edges in g2o framework - */ -void TebOptimalPlanner::registerG2OTypes() -{ - g2o::Factory* factory = g2o::Factory::instance(); - factory->registerType("VERTEX_POSE", new g2o::HyperGraphElementCreator); - factory->registerType("VERTEX_TIMEDIFF", new g2o::HyperGraphElementCreator); - - factory->registerType("EDGE_TIME_OPTIMAL", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_VELOCITY", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_VELOCITY_HOLONOMIC", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_ACCELERATION", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_ACCELERATION_START", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_ACCELERATION_GOAL", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_ACCELERATION_HOLONOMIC", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_ACCELERATION_HOLONOMIC_START", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_ACCELERATION_HOLONOMIC_GOAL", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_KINEMATICS_DIFF_DRIVE", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_KINEMATICS_CARLIKE", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_OBSTACLE", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_INFLATED_OBSTACLE", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_DYNAMIC_OBSTACLE", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_VIA_POINT", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_PREFER_ROTDIR", new g2o::HyperGraphElementCreator); - return; -} - -/* - * initialize g2o optimizer. Set solver settings here. - * Return: pointer to new SparseOptimizer Object. - */ -boost::shared_ptr TebOptimalPlanner::initOptimizer() -{ - // Call register_g2o_types once, even for multiple TebOptimalPlanner instances (thread-safe) - static boost::once_flag flag = BOOST_ONCE_INIT; - boost::call_once(®isterG2OTypes, flag); - - // allocating the optimizer - boost::shared_ptr optimizer = boost::make_shared(); - TEBLinearSolver* linearSolver = new TEBLinearSolver(); // see typedef in optimization.h - linearSolver->setBlockOrdering(true); - TEBBlockSolver* blockSolver = new TEBBlockSolver(linearSolver); - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(blockSolver); - - optimizer->setAlgorithm(solver); - - optimizer->initMultiThreading(); // required for >Eigen 3.1 - - return optimizer; -} - - -bool TebOptimalPlanner::optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards, - double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost) -{ - if (cfg_->optim.optimization_activate==false) - return false; - - bool success = false; - optimized_ = false; - - double weight_multiplier = 1.0; - - // TODO(roesmann): we introduced the non-fast mode with the support of dynamic obstacles - // (which leads to better results in terms of x-y-t homotopy planning). - // however, we have not tested this mode intensively yet, so we keep - // the legacy fast mode as default until we finish our tests. - bool fast_mode = !cfg_->obstacles.include_dynamic_obstacles; - - for(int i=0; itrajectory.teb_autosize) + void TebOptimalPlanner::initialize(const TebConfig &cfg, ObstContainer *obstacles, RobotFootprintModelPtr robot_model, TebVisualizationPtr visual, + const ViaPointContainer *via_points) { - //teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples); - teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples, fast_mode); + // init optimizer (set solver and block ordering settings) + optimizer_ = initOptimizer(); + + cfg_ = &cfg; + obstacles_ = obstacles; + robot_model_ = robot_model; + via_points_ = via_points; + cost_ = HUGE_VAL; + prefer_rotdir_ = RotType::none; + setVisualization(visual); + + vel_start_.first = true; + vel_start_.second.linear.x = 0; + vel_start_.second.linear.y = 0; + vel_start_.second.angular.z = 0; + + vel_goal_.first = true; + vel_goal_.second.linear.x = 0; + vel_goal_.second.linear.y = 0; + vel_goal_.second.angular.z = 0; + initialized_ = true; + } + + void TebOptimalPlanner::setVisualization(TebVisualizationPtr visualization) { visualization_ = visualization; } + + void TebOptimalPlanner::visualize() + { + if (!visualization_) + return; + visualization_->publishLocalPlanAndPoses(teb_); + + if (teb_.sizePoses() > 0) + visualization_->publishRobotFootprintModel(teb_.Pose(0), *robot_model_); + + if (cfg_->trajectory.publish_feedback) + visualization_->publishFeedbackMessage(*this, *obstacles_); } - success = buildGraph(weight_multiplier); - if (!success) + /* + * registers custom vertices and edges in g2o framework + */ + void TebOptimalPlanner::registerG2OTypes() { - clearGraph(); - return false; + g2o::Factory *factory = g2o::Factory::instance(); + factory->registerType("VERTEX_POSE", new g2o::HyperGraphElementCreator); + factory->registerType("VERTEX_TIMEDIFF", new g2o::HyperGraphElementCreator); + + factory->registerType("EDGE_TIME_OPTIMAL", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_VELOCITY", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_VELOCITY_HOLONOMIC", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_ACCELERATION", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_ACCELERATION_START", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_ACCELERATION_GOAL", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_ACCELERATION_HOLONOMIC", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_ACCELERATION_HOLONOMIC_START", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_ACCELERATION_HOLONOMIC_GOAL", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_KINEMATICS_DIFF_DRIVE", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_KINEMATICS_CARLIKE", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_OBSTACLE", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_INFLATED_OBSTACLE", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_DYNAMIC_OBSTACLE", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_VIA_POINT", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_PREFER_ROTDIR", new g2o::HyperGraphElementCreator); + return; } - success = optimizeGraph(iterations_innerloop, false); - if (!success) + + /* + * initialize g2o optimizer. Set solver settings here. + * Return: pointer to new SparseOptimizer Object. + */ + boost::shared_ptr TebOptimalPlanner::initOptimizer() { - clearGraph(); - return false; + // Call register_g2o_types once, even for multiple TebOptimalPlanner instances (thread-safe) + static boost::once_flag flag = BOOST_ONCE_INIT; + boost::call_once(®isterG2OTypes, flag); + + // allocating the optimizer + boost::shared_ptr optimizer = boost::make_shared(); + TEBLinearSolver *linearSolver = new TEBLinearSolver(); // see typedef in optimization.h + linearSolver->setBlockOrdering(true); + TEBBlockSolver *blockSolver = new TEBBlockSolver(linearSolver); + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(blockSolver); + + optimizer->setAlgorithm(solver); + + optimizer->initMultiThreading(); // required for >Eigen 3.1 + + return optimizer; } - optimized_ = true; - - if (compute_cost_afterwards && i==iterations_outerloop-1) // compute cost vec only in the last iteration - computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost); - - clearGraph(); - - weight_multiplier *= cfg_->optim.weight_adapt_factor; - } - - return true; -} - -void TebOptimalPlanner::setVelocityStart(const geometry_msgs::Twist& vel_start) -{ - vel_start_.first = true; - vel_start_.second.linear.x = vel_start.linear.x; - vel_start_.second.linear.y = vel_start.linear.y; - vel_start_.second.angular.z = vel_start.angular.z; -} -void TebOptimalPlanner::setVelocityGoal(const geometry_msgs::Twist& vel_goal) -{ - vel_goal_.first = true; - vel_goal_.second = vel_goal; -} - -bool TebOptimalPlanner::plan(const std::vector& initial_plan, const geometry_msgs::Twist* start_vel, bool free_goal_vel) -{ - ROS_ASSERT_MSG(initialized_, "Call initialize() first."); - if (!teb_.isInit()) - { - // init trajectory - teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->trajectory.global_plan_overwrite_orientation, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); - } - else // warm start - { - PoseSE2 start_(initial_plan.front().pose); - PoseSE2 goal_(initial_plan.back().pose); - if (teb_.sizePoses()>0 && (goal_.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist) // actual warm start! - teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEB - else // goal too far away -> reinit + bool TebOptimalPlanner::optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards, double obst_cost_scale, + double viapoint_cost_scale, bool alternative_time_cost) { - ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories."); - teb_.clearTimedElasticBand(); - teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, true, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); + if (cfg_->optim.optimization_activate == false) + return false; + + bool success = false; + optimized_ = false; + + double weight_multiplier = 1.0; + + // TODO(roesmann): we introduced the non-fast mode with the support of dynamic obstacles + // (which leads to better results in terms of x-y-t homotopy planning). + // however, we have not tested this mode intensively yet, so we keep + // the legacy fast mode as default until we finish our tests. + bool fast_mode = !cfg_->obstacles.include_dynamic_obstacles; + + for (int i = 0; i < iterations_outerloop; ++i) + { + if (cfg_->trajectory.teb_autosize) + { + // teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, + // cfg_->trajectory.max_samples); + teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples, + fast_mode); + } + + success = buildGraph(weight_multiplier); + if (!success) + { + clearGraph(); + return false; + } + success = optimizeGraph(iterations_innerloop, false); + if (!success) + { + clearGraph(); + return false; + } + optimized_ = true; + + if (compute_cost_afterwards && i == iterations_outerloop - 1) // compute cost vec only in the last iteration + computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost); + + clearGraph(); + + weight_multiplier *= cfg_->optim.weight_adapt_factor; + } + + return true; } - } - if (start_vel) - setVelocityStart(*start_vel); - if (free_goal_vel) - setVelocityGoalFree(); - else - vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified) - - // now optimize - return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations); -} - - -bool TebOptimalPlanner::plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::Twist* start_vel, bool free_goal_vel) -{ - PoseSE2 start_(start); - PoseSE2 goal_(goal); - return plan(start_, goal_, start_vel); -} - -bool TebOptimalPlanner::plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist* start_vel, bool free_goal_vel) -{ - ROS_ASSERT_MSG(initialized_, "Call initialize() first."); - if (!teb_.isInit()) - { - // init trajectory - teb_.initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); // 0 intermediate samples, but dt=1 -> autoResize will add more samples before calling first optimization - } - else // warm start - { - if (teb_.sizePoses()>0 && (goal.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist) // actual warm start! - teb_.updateAndPruneTEB(start, goal, cfg_->trajectory.min_samples); - else // goal too far away -> reinit + + void TebOptimalPlanner::setVelocityStart(const geometry_msgs::Twist &vel_start) { - ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories."); - teb_.clearTimedElasticBand(); - teb_.initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); + vel_start_.first = true; + vel_start_.second.linear.x = vel_start.linear.x; + vel_start_.second.linear.y = vel_start.linear.y; + vel_start_.second.angular.z = vel_start.angular.z; + } + + void TebOptimalPlanner::setVelocityGoal(const geometry_msgs::Twist &vel_goal) + { + vel_goal_.first = true; + vel_goal_.second = vel_goal; } - } - if (start_vel) - setVelocityStart(*start_vel); - if (free_goal_vel) - setVelocityGoalFree(); - else - vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified) - - // now optimize - return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations); -} - - -bool TebOptimalPlanner::buildGraph(double weight_multiplier) -{ - if (!optimizer_->edges().empty() || !optimizer_->vertices().empty()) - { - ROS_WARN("Cannot build graph, because it is not empty. Call graphClear()!"); - return false; - } - - // add TEB vertices - AddTEBVertices(); - - // add Edges (local cost functions) - if (cfg_->obstacles.legacy_obstacle_association) - AddEdgesObstaclesLegacy(weight_multiplier); - else - AddEdgesObstacles(weight_multiplier); - - if (cfg_->obstacles.include_dynamic_obstacles) - AddEdgesDynamicObstacles(); - - AddEdgesViaPoints(); - - AddEdgesVelocity(); - - AddEdgesAcceleration(); - - AddEdgesTimeOptimal(); - - if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0) - AddEdgesKinematicsDiffDrive(); // we have a differential drive robot - else - AddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded from below. - - - AddEdgesPreferRotDir(); - - return true; -} - -bool TebOptimalPlanner::optimizeGraph(int no_iterations,bool clear_after) -{ - if (cfg_->robot.max_vel_x<0.01) - { - ROS_WARN("optimizeGraph(): Robot Max Velocity is smaller than 0.01m/s. Optimizing aborted..."); - if (clear_after) clearGraph(); - return false; - } - - if (!teb_.isInit() || teb_.sizePoses() < cfg_->trajectory.min_samples) - { - ROS_WARN("optimizeGraph(): TEB is empty or has too less elements. Skipping optimization."); - if (clear_after) clearGraph(); - return false; - } - - optimizer_->setVerbose(cfg_->optim.optimization_verbose); - optimizer_->initializeOptimization(); - - int iter = optimizer_->optimize(no_iterations); - - // Save Hessian for visualization - // g2o::OptimizationAlgorithmLevenberg* lm = dynamic_cast (optimizer_->solver()); - // lm->solver()->saveHessian("~/MasterThesis/Matlab/Hessian.txt"); - - if(!iter) - { - ROS_ERROR("optimizeGraph(): Optimization failed! iter=%i", iter); - return false; - } - - if (clear_after) clearGraph(); - - return true; -} - -void TebOptimalPlanner::clearGraph() -{ - // clear optimizer states - //optimizer.edges().clear(); // optimizer.clear deletes edges!!! Therefore do not run optimizer.edges().clear() - optimizer_->vertices().clear(); // neccessary, because optimizer->clear deletes pointer-targets (therefore it deletes TEB states!) - optimizer_->clear(); -} + bool TebOptimalPlanner::plan(const std::vector &initial_plan, const geometry_msgs::Twist *start_vel, + bool free_goal_vel) + { + ROS_ASSERT_MSG(initialized_, "Call initialize() first."); + if (!teb_.isInit()) + { + // init trajectory + teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->trajectory.global_plan_overwrite_orientation, + cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); + } + else // warm start + { + PoseSE2 start_(initial_plan.front().pose); + PoseSE2 goal_(initial_plan.back().pose); + if (teb_.sizePoses() > 0 && + (goal_.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist) // actual warm start! + teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEB + else // goal too far away -> reinit + { + ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories."); + teb_.clearTimedElasticBand(); + teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, true, cfg_->trajectory.min_samples, + cfg_->trajectory.allow_init_with_backwards_motion); + } + } + if (start_vel) + setVelocityStart(*start_vel); + if (free_goal_vel) + setVelocityGoalFree(); + else + vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified) + + // now optimize + return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations); + } + bool TebOptimalPlanner::plan(const tf::Pose &start, const tf::Pose &goal, const geometry_msgs::Twist *start_vel, bool free_goal_vel) + { + PoseSE2 start_(start); + PoseSE2 goal_(goal); + return plan(start_, goal_, start_vel); + } -void TebOptimalPlanner::AddTEBVertices() -{ - // add vertices to graph - ROS_DEBUG_COND(cfg_->optim.optimization_verbose, "Adding TEB vertices ..."); - unsigned int id_counter = 0; // used for vertices ids - for (int i=0; isetId(id_counter++); - optimizer_->addVertex(teb_.PoseVertex(i)); - if (teb_.sizeTimeDiffs()!=0 && isetId(id_counter++); - optimizer_->addVertex(teb_.TimeDiffVertex(i)); + ROS_ASSERT_MSG(initialized_, "Call initialize() first."); + if (!teb_.isInit()) + { + // init trajectory + teb_.initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, + cfg_->trajectory.allow_init_with_backwards_motion); // 0 intermediate samples, but dt=1 -> autoResize will add + // more samples before calling first optimization + } + else // warm start + { + if (teb_.sizePoses() > 0 && + (goal.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist) // actual warm start! + teb_.updateAndPruneTEB(start, goal, cfg_->trajectory.min_samples); + else // goal too far away -> reinit + { + ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories."); + teb_.clearTimedElasticBand(); + teb_.initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, + cfg_->trajectory.allow_init_with_backwards_motion); + } + } + if (start_vel) + setVelocityStart(*start_vel); + if (free_goal_vel) + setVelocityGoalFree(); + else + vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified) + + // now optimize + return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations); } - } -} + bool TebOptimalPlanner::buildGraph(double weight_multiplier) + { + if (!optimizer_->edges().empty() || !optimizer_->vertices().empty()) + { + ROS_WARN("Cannot build graph, because it is not empty. Call graphClear()!"); + return false; + } -void TebOptimalPlanner::AddEdgesObstacles(double weight_multiplier) -{ - if (cfg_->optim.weight_obstacle==0 || weight_multiplier==0 || obstacles_==nullptr ) - return; // if weight equals zero skip adding edges! - - - bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist; - - Eigen::Matrix information; - information.fill(cfg_->optim.weight_obstacle * weight_multiplier); - - Eigen::Matrix information_inflated; - information_inflated(0,0) = cfg_->optim.weight_obstacle * weight_multiplier; - information_inflated(1,1) = cfg_->optim.weight_inflation; - information_inflated(0,1) = information_inflated(1,0) = 0; - - // iterate all teb points (skip first and last) - for (int i=1; i < teb_.sizePoses()-1; ++i) - { - double left_min_dist = std::numeric_limits::max(); - double right_min_dist = std::numeric_limits::max(); - Obstacle* left_obstacle = nullptr; - Obstacle* right_obstacle = nullptr; - - std::vector relevant_obstacles; - - const Eigen::Vector2d pose_orient = teb_.Pose(i).orientationUnitVec(); - - // iterate obstacles - for (const ObstaclePtr& obst : *obstacles_) - { - // we handle dynamic obstacles differently below - if(cfg_->obstacles.include_dynamic_obstacles && obst->isDynamic()) - continue; - - // calculate distance to current pose - // TODO we ignore the robot footprint here in the association stage - double dist = obst->getMinimumDistance(teb_.Pose(i).position()); - - // force considering obstacle if really close to the current pose - if (dist < cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_force_inclusion_factor) - { - relevant_obstacles.push_back(obst.get()); - continue; - } - // cut-off distance - if (dist > cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_cutoff_factor) - continue; - - // determine side (left or right) and assign obstacle if closer than the previous one - if (cross2d(pose_orient, obst->getCentroid()) > 0) // left - { - if (dist < left_min_dist) - { - left_min_dist = dist; - left_obstacle = obst.get(); - } - } - else - { - if (dist < right_min_dist) - { - right_min_dist = dist; - right_obstacle = obst.get(); - } - } - } - - // create obstacle edges - if (left_obstacle) - { - if (inflated) + // add TEB vertices + AddTEBVertices(); + + // add Edges (local cost functions) + if (cfg_->obstacles.legacy_obstacle_association) + AddEdgesObstaclesLegacy(weight_multiplier); + else + AddEdgesObstacles(weight_multiplier); + + if (cfg_->obstacles.include_dynamic_obstacles) + AddEdgesDynamicObstacles(); + + AddEdgesViaPoints(); + + AddEdgesVelocity(); + + AddEdgesAcceleration(); + + AddEdgesTimeOptimal(); + + if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0) + AddEdgesKinematicsDiffDrive(); // we have a differential drive robot + else + AddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded from below. + + AddEdgesPreferRotDir(); + + return true; + } + + bool TebOptimalPlanner::optimizeGraph(int no_iterations, bool clear_after) + { + if (cfg_->robot.max_vel_x < 0.01) + { + ROS_WARN("optimizeGraph(): Robot Max Velocity is smaller than 0.01m/s. Optimizing aborted..."); + if (clear_after) + clearGraph(); + return false; + } + + if (!teb_.isInit() || teb_.sizePoses() < cfg_->trajectory.min_samples) + { + ROS_WARN("optimizeGraph(): TEB is empty or has too less elements. Skipping optimization."); + if (clear_after) + clearGraph(); + return false; + } + + optimizer_->setVerbose(cfg_->optim.optimization_verbose); + optimizer_->initializeOptimization(); + + int iter = optimizer_->optimize(no_iterations); + + // Save Hessian for visualization + // g2o::OptimizationAlgorithmLevenberg* lm = dynamic_cast + // (optimizer_->solver()); lm->solver()->saveHessian("~/MasterThesis/Matlab/Hessian.txt"); + + if (!iter) + { + ROS_ERROR("optimizeGraph(): Optimization failed! iter=%i", iter); + return false; + } + + if (clear_after) + clearGraph(); + + return true; + } + + void TebOptimalPlanner::clearGraph() + { + // clear optimizer states + // optimizer.edges().clear(); // optimizer.clear deletes edges!!! Therefore do not run optimizer.edges().clear() + optimizer_->vertices().clear(); // neccessary, because optimizer->clear deletes pointer-targets (therefore it deletes TEB states!) + optimizer_->clear(); + } + + void TebOptimalPlanner::AddTEBVertices() + { + // add vertices to graph + ROS_DEBUG_COND(cfg_->optim.optimization_verbose, "Adding TEB vertices ..."); + unsigned int id_counter = 0; // used for vertices ids + for (int i = 0; i < teb_.sizePoses(); ++i) + { + teb_.PoseVertex(i)->setId(id_counter++); + optimizer_->addVertex(teb_.PoseVertex(i)); + if (teb_.sizeTimeDiffs() != 0 && i < teb_.sizeTimeDiffs()) { - EdgeInflatedObstacle* dist_bandpt_obst = new EdgeInflatedObstacle; - dist_bandpt_obst->setVertex(0,teb_.PoseVertex(i)); - dist_bandpt_obst->setInformation(information_inflated); - dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), left_obstacle); - optimizer_->addEdge(dist_bandpt_obst); + teb_.TimeDiffVertex(i)->setId(id_counter++); + optimizer_->addVertex(teb_.TimeDiffVertex(i)); } - else + } + } + + void TebOptimalPlanner::AddEdgesObstacles(double weight_multiplier) + { + if (cfg_->optim.weight_obstacle == 0 || weight_multiplier == 0 || obstacles_ == nullptr) + return; // if weight equals zero skip adding edges! + + bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist; + + Eigen::Matrix information; + information.fill(cfg_->optim.weight_obstacle * weight_multiplier); + + Eigen::Matrix information_inflated; + information_inflated(0, 0) = cfg_->optim.weight_obstacle * weight_multiplier; + information_inflated(1, 1) = cfg_->optim.weight_inflation; + information_inflated(0, 1) = information_inflated(1, 0) = 0; + + // iterate all teb points (skip first and last) + for (int i = 1; i < teb_.sizePoses() - 1; ++i) + { + double left_min_dist = std::numeric_limits::max(); + double right_min_dist = std::numeric_limits::max(); + Obstacle *left_obstacle = nullptr; + Obstacle *right_obstacle = nullptr; + + std::vector relevant_obstacles; + + const Eigen::Vector2d pose_orient = teb_.Pose(i).orientationUnitVec(); + + // iterate obstacles + for (const ObstaclePtr &obst : *obstacles_) { - EdgeObstacle* dist_bandpt_obst = new EdgeObstacle; - dist_bandpt_obst->setVertex(0,teb_.PoseVertex(i)); - dist_bandpt_obst->setInformation(information); - dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), left_obstacle); - optimizer_->addEdge(dist_bandpt_obst); + // we handle dynamic obstacles differently below + if (cfg_->obstacles.include_dynamic_obstacles && obst->isDynamic()) + continue; + + // calculate distance to current pose + // TODO we ignore the robot footprint here in the association stage + double dist = obst->getMinimumDistance(teb_.Pose(i).position()); + + // force considering obstacle if really close to the current pose + if (dist < cfg_->obstacles.min_obstacle_dist * cfg_->obstacles.obstacle_association_force_inclusion_factor) + { + relevant_obstacles.push_back(obst.get()); + continue; + } + // cut-off distance + if (dist > cfg_->obstacles.min_obstacle_dist * cfg_->obstacles.obstacle_association_cutoff_factor) + continue; + + // determine side (left or right) and assign obstacle if closer than the previous one + if (cross2d(pose_orient, obst->getCentroid()) > 0) // left + { + if (dist < left_min_dist) + { + left_min_dist = dist; + left_obstacle = obst.get(); + } + } + else + { + if (dist < right_min_dist) + { + right_min_dist = dist; + right_obstacle = obst.get(); + } + } } - } - - if (right_obstacle) - { - if (inflated) + + // create obstacle edges + if (left_obstacle) { - EdgeInflatedObstacle* dist_bandpt_obst = new EdgeInflatedObstacle; - dist_bandpt_obst->setVertex(0,teb_.PoseVertex(i)); - dist_bandpt_obst->setInformation(information_inflated); - dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), right_obstacle); - optimizer_->addEdge(dist_bandpt_obst); + if (inflated) + { + EdgeInflatedObstacle *dist_bandpt_obst = new EdgeInflatedObstacle; + dist_bandpt_obst->setVertex(0, teb_.PoseVertex(i)); + dist_bandpt_obst->setInformation(information_inflated); + dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), left_obstacle); + optimizer_->addEdge(dist_bandpt_obst); + } + else + { + EdgeObstacle *dist_bandpt_obst = new EdgeObstacle; + dist_bandpt_obst->setVertex(0, teb_.PoseVertex(i)); + dist_bandpt_obst->setInformation(information); + dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), left_obstacle); + optimizer_->addEdge(dist_bandpt_obst); + } } - else + + if (right_obstacle) { - EdgeObstacle* dist_bandpt_obst = new EdgeObstacle; - dist_bandpt_obst->setVertex(0,teb_.PoseVertex(i)); - dist_bandpt_obst->setInformation(information); - dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), right_obstacle); - optimizer_->addEdge(dist_bandpt_obst); - } - } - - for (const Obstacle* obst : relevant_obstacles) - { + if (inflated) + { + EdgeInflatedObstacle *dist_bandpt_obst = new EdgeInflatedObstacle; + dist_bandpt_obst->setVertex(0, teb_.PoseVertex(i)); + dist_bandpt_obst->setInformation(information_inflated); + dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), right_obstacle); + optimizer_->addEdge(dist_bandpt_obst); + } + else + { + EdgeObstacle *dist_bandpt_obst = new EdgeObstacle; + dist_bandpt_obst->setVertex(0, teb_.PoseVertex(i)); + dist_bandpt_obst->setInformation(information); + dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), right_obstacle); + optimizer_->addEdge(dist_bandpt_obst); + } + } + + for (const Obstacle *obst : relevant_obstacles) + { + if (inflated) + { + EdgeInflatedObstacle *dist_bandpt_obst = new EdgeInflatedObstacle; + dist_bandpt_obst->setVertex(0, teb_.PoseVertex(i)); + dist_bandpt_obst->setInformation(information_inflated); + dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obst); + optimizer_->addEdge(dist_bandpt_obst); + } + else + { + EdgeObstacle *dist_bandpt_obst = new EdgeObstacle; + dist_bandpt_obst->setVertex(0, teb_.PoseVertex(i)); + dist_bandpt_obst->setInformation(information); + dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obst); + optimizer_->addEdge(dist_bandpt_obst); + } + } + } + } + + void TebOptimalPlanner::AddEdgesObstaclesLegacy(double weight_multiplier) + { + if (cfg_->optim.weight_obstacle == 0 || weight_multiplier == 0 || obstacles_ == nullptr) + return; // if weight equals zero skip adding edges! + + Eigen::Matrix information; + information.fill(cfg_->optim.weight_obstacle * weight_multiplier); + + Eigen::Matrix information_inflated; + information_inflated(0, 0) = cfg_->optim.weight_obstacle * weight_multiplier; + information_inflated(1, 1) = cfg_->optim.weight_inflation; + information_inflated(0, 1) = information_inflated(1, 0) = 0; + + bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist; + + for (ObstContainer::const_iterator obst = obstacles_->begin(); obst != obstacles_->end(); ++obst) + { + if (cfg_->obstacles.include_dynamic_obstacles && (*obst)->isDynamic()) // we handle dynamic obstacles differently below + continue; + + int index; + + if (cfg_->obstacles.obstacle_poses_affected >= teb_.sizePoses()) + index = teb_.sizePoses() / 2; + else + index = teb_.findClosestTrajectoryPose(*(obst->get())); + + // check if obstacle is outside index-range between start and goal + if ((index <= 1) || (index > teb_.sizePoses() - 2)) // start and goal are fixed and findNearestBandpoint finds first or last conf + // if intersection point is outside the range + continue; + if (inflated) { - EdgeInflatedObstacle* dist_bandpt_obst = new EdgeInflatedObstacle; - dist_bandpt_obst->setVertex(0,teb_.PoseVertex(i)); + EdgeInflatedObstacle *dist_bandpt_obst = new EdgeInflatedObstacle; + dist_bandpt_obst->setVertex(0, teb_.PoseVertex(index)); dist_bandpt_obst->setInformation(information_inflated); - dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obst); + dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obst->get()); optimizer_->addEdge(dist_bandpt_obst); } else { - EdgeObstacle* dist_bandpt_obst = new EdgeObstacle; - dist_bandpt_obst->setVertex(0,teb_.PoseVertex(i)); + EdgeObstacle *dist_bandpt_obst = new EdgeObstacle; + dist_bandpt_obst->setVertex(0, teb_.PoseVertex(index)); dist_bandpt_obst->setInformation(information); - dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obst); + dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obst->get()); optimizer_->addEdge(dist_bandpt_obst); - } - } - } - -} + } + for (int neighbourIdx = 0; neighbourIdx < floor(cfg_->obstacles.obstacle_poses_affected / 2); neighbourIdx++) + { + if (index + neighbourIdx < teb_.sizePoses()) + { + if (inflated) + { + EdgeInflatedObstacle *dist_bandpt_obst_n_r = new EdgeInflatedObstacle; + dist_bandpt_obst_n_r->setVertex(0, teb_.PoseVertex(index + neighbourIdx)); + dist_bandpt_obst_n_r->setInformation(information_inflated); + dist_bandpt_obst_n_r->setParameters(*cfg_, robot_model_.get(), obst->get()); + optimizer_->addEdge(dist_bandpt_obst_n_r); + } + else + { + EdgeObstacle *dist_bandpt_obst_n_r = new EdgeObstacle; + dist_bandpt_obst_n_r->setVertex(0, teb_.PoseVertex(index + neighbourIdx)); + dist_bandpt_obst_n_r->setInformation(information); + dist_bandpt_obst_n_r->setParameters(*cfg_, robot_model_.get(), obst->get()); + optimizer_->addEdge(dist_bandpt_obst_n_r); + } + } + if (index - neighbourIdx >= 0) // needs to be casted to int to allow negative values + { + if (inflated) + { + EdgeInflatedObstacle *dist_bandpt_obst_n_l = new EdgeInflatedObstacle; + dist_bandpt_obst_n_l->setVertex(0, teb_.PoseVertex(index - neighbourIdx)); + dist_bandpt_obst_n_l->setInformation(information_inflated); + dist_bandpt_obst_n_l->setParameters(*cfg_, robot_model_.get(), obst->get()); + optimizer_->addEdge(dist_bandpt_obst_n_l); + } + else + { + EdgeObstacle *dist_bandpt_obst_n_l = new EdgeObstacle; + dist_bandpt_obst_n_l->setVertex(0, teb_.PoseVertex(index - neighbourIdx)); + dist_bandpt_obst_n_l->setInformation(information); + dist_bandpt_obst_n_l->setParameters(*cfg_, robot_model_.get(), obst->get()); + optimizer_->addEdge(dist_bandpt_obst_n_l); + } + } + } + } + } -void TebOptimalPlanner::AddEdgesObstaclesLegacy(double weight_multiplier) -{ - if (cfg_->optim.weight_obstacle==0 || weight_multiplier==0 || obstacles_==nullptr) - return; // if weight equals zero skip adding edges! - - Eigen::Matrix information; - information.fill(cfg_->optim.weight_obstacle * weight_multiplier); - - Eigen::Matrix information_inflated; - information_inflated(0,0) = cfg_->optim.weight_obstacle * weight_multiplier; - information_inflated(1,1) = cfg_->optim.weight_inflation; - information_inflated(0,1) = information_inflated(1,0) = 0; - - bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist; - - for (ObstContainer::const_iterator obst = obstacles_->begin(); obst != obstacles_->end(); ++obst) - { - if (cfg_->obstacles.include_dynamic_obstacles && (*obst)->isDynamic()) // we handle dynamic obstacles differently below - continue; - - int index; - - if (cfg_->obstacles.obstacle_poses_affected >= teb_.sizePoses()) - index = teb_.sizePoses() / 2; - else - index = teb_.findClosestTrajectoryPose(*(obst->get())); - - - // check if obstacle is outside index-range between start and goal - if ( (index <= 1) || (index > teb_.sizePoses()-2) ) // start and goal are fixed and findNearestBandpoint finds first or last conf if intersection point is outside the range - continue; - - if (inflated) + void TebOptimalPlanner::AddEdgesDynamicObstacles(double weight_multiplier) { - EdgeInflatedObstacle* dist_bandpt_obst = new EdgeInflatedObstacle; - dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index)); - dist_bandpt_obst->setInformation(information_inflated); - dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obst->get()); - optimizer_->addEdge(dist_bandpt_obst); + if (cfg_->optim.weight_obstacle == 0 || weight_multiplier == 0 || obstacles_ == NULL) + return; // if weight equals zero skip adding edges! + + Eigen::Matrix information; + information(0, 0) = cfg_->optim.weight_dynamic_obstacle * weight_multiplier; + information(1, 1) = cfg_->optim.weight_dynamic_obstacle_inflation; + information(0, 1) = information(1, 0) = 0; + + for (ObstContainer::const_iterator obst = obstacles_->begin(); obst != obstacles_->end(); ++obst) + { + if (!(*obst)->isDynamic()) + continue; + + // Skip first and last pose, as they are fixed + double time = teb_.TimeDiff(0); + for (int i = 1; i < teb_.sizePoses() - 1; ++i) + { + EdgeDynamicObstacle *dynobst_edge = new EdgeDynamicObstacle(time); + dynobst_edge->setVertex(0, teb_.PoseVertex(i)); + dynobst_edge->setInformation(information); + dynobst_edge->setParameters(*cfg_, robot_model_.get(), obst->get()); + optimizer_->addEdge(dynobst_edge); + time += teb_.TimeDiff(i); // we do not need to check the time diff bounds, since we iterate to "< sizePoses()-1". + } + } } - else + + void TebOptimalPlanner::AddEdgesViaPoints() { - EdgeObstacle* dist_bandpt_obst = new EdgeObstacle; - dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index)); - dist_bandpt_obst->setInformation(information); - dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obst->get()); - optimizer_->addEdge(dist_bandpt_obst); + if (cfg_->optim.weight_viapoint == 0 || via_points_ == NULL || via_points_->empty()) + return; // if weight equals zero skip adding edges! + + int start_pose_idx = 0; + + int n = teb_.sizePoses(); + if (n < 3) // we do not have any degrees of freedom for reaching via-points + return; + + for (ViaPointContainer::const_iterator vp_it = via_points_->begin(); vp_it != via_points_->end(); ++vp_it) + { + int index = teb_.findClosestTrajectoryPose(*vp_it, NULL, start_pose_idx); + if (cfg_->trajectory.via_points_ordered) + start_pose_idx = index + 2; // skip a point to have a DOF inbetween for further via-points + + // check if point conicides with goal or is located behind it + if (index > n - 2) + index = n - 2; // set to a pose before the goal, since we can move it away! + // check if point coincides with start or is located before it + if (index < 1) + { + if (cfg_->trajectory.via_points_ordered) + { + index = 1; // try to connect the via point with the second (and non-fixed) pose. It is likely that autoresize + // adds new poses inbetween later. + } + else + { + ROS_DEBUG("TebOptimalPlanner::AddEdgesViaPoints(): skipping a via-point that is close or behind the current " + "robot pose."); + continue; // skip via points really close or behind the current robot pose + } + } + Eigen::Matrix information; + information.fill(cfg_->optim.weight_viapoint); + + EdgeViaPoint *edge_viapoint = new EdgeViaPoint; + edge_viapoint->setVertex(0, teb_.PoseVertex(index)); + edge_viapoint->setInformation(information); + edge_viapoint->setParameters(*cfg_, &(*vp_it)); + optimizer_->addEdge(edge_viapoint); + } } - for (int neighbourIdx=0; neighbourIdx < floor(cfg_->obstacles.obstacle_poses_affected/2); neighbourIdx++) + void TebOptimalPlanner::AddEdgesVelocity() { - if (index+neighbourIdx < teb_.sizePoses()) - { - if (inflated) + if (cfg_->robot.max_vel_y == 0) // non-holonomic robot + { + if (cfg_->optim.weight_max_vel_x == 0 && cfg_->optim.weight_max_vel_theta == 0) + return; // if weight equals zero skip adding edges! + + int n = teb_.sizePoses(); + Eigen::Matrix information; + information(0, 0) = cfg_->optim.weight_max_vel_x; + information(1, 1) = cfg_->optim.weight_max_vel_theta; + information(0, 1) = 0.0; + information(1, 0) = 0.0; + + for (int i = 0; i < n - 1; ++i) { - EdgeInflatedObstacle* dist_bandpt_obst_n_r = new EdgeInflatedObstacle; - dist_bandpt_obst_n_r->setVertex(0,teb_.PoseVertex(index+neighbourIdx)); - dist_bandpt_obst_n_r->setInformation(information_inflated); - dist_bandpt_obst_n_r->setParameters(*cfg_, robot_model_.get(), obst->get()); - optimizer_->addEdge(dist_bandpt_obst_n_r); + EdgeVelocity *velocity_edge = new EdgeVelocity; + velocity_edge->setVertex(0, teb_.PoseVertex(i)); + velocity_edge->setVertex(1, teb_.PoseVertex(i + 1)); + velocity_edge->setVertex(2, teb_.TimeDiffVertex(i)); + velocity_edge->setInformation(information); + velocity_edge->setTebConfig(*cfg_); + optimizer_->addEdge(velocity_edge); } - else + } + else // holonomic-robot + { + if (cfg_->optim.weight_max_vel_x == 0 && cfg_->optim.weight_max_vel_y == 0 && cfg_->optim.weight_max_vel_theta == 0) + return; // if weight equals zero skip adding edges! + + int n = teb_.sizePoses(); + Eigen::Matrix information; + information.fill(0); + information(0, 0) = cfg_->optim.weight_max_vel_x; + information(1, 1) = cfg_->optim.weight_max_vel_y; + information(2, 2) = cfg_->optim.weight_max_vel_theta; + + for (int i = 0; i < n - 1; ++i) { - EdgeObstacle* dist_bandpt_obst_n_r = new EdgeObstacle; - dist_bandpt_obst_n_r->setVertex(0,teb_.PoseVertex(index+neighbourIdx)); - dist_bandpt_obst_n_r->setInformation(information); - dist_bandpt_obst_n_r->setParameters(*cfg_, robot_model_.get(), obst->get()); - optimizer_->addEdge(dist_bandpt_obst_n_r); + EdgeVelocityHolonomic *velocity_edge = new EdgeVelocityHolonomic; + velocity_edge->setVertex(0, teb_.PoseVertex(i)); + velocity_edge->setVertex(1, teb_.PoseVertex(i + 1)); + velocity_edge->setVertex(2, teb_.TimeDiffVertex(i)); + velocity_edge->setInformation(information); + velocity_edge->setTebConfig(*cfg_); + optimizer_->addEdge(velocity_edge); } - } - if ( index - neighbourIdx >= 0) // needs to be casted to int to allow negative values - { - if (inflated) + } + } + + void TebOptimalPlanner::AddEdgesAcceleration() + { + if (cfg_->optim.weight_acc_lim_x == 0 && cfg_->optim.weight_acc_lim_theta == 0) + return; // if weight equals zero skip adding edges! + + int n = teb_.sizePoses(); + + if (cfg_->robot.max_vel_y == 0 || cfg_->robot.acc_lim_y == 0) // non-holonomic robot + { + Eigen::Matrix information; + information.fill(0); + information(0, 0) = cfg_->optim.weight_acc_lim_x; + information(1, 1) = cfg_->optim.weight_acc_lim_theta; + information(2, 2) = cfg_->optim.weight_max_steering_rate; + + // check if an initial velocity should be taken into accound + if (vel_start_.first) { - EdgeInflatedObstacle* dist_bandpt_obst_n_l = new EdgeInflatedObstacle; - dist_bandpt_obst_n_l->setVertex(0,teb_.PoseVertex(index-neighbourIdx)); - dist_bandpt_obst_n_l->setInformation(information_inflated); - dist_bandpt_obst_n_l->setParameters(*cfg_, robot_model_.get(), obst->get()); - optimizer_->addEdge(dist_bandpt_obst_n_l); + EdgeAccelerationStart *acceleration_edge = new EdgeAccelerationStart; + acceleration_edge->setVertex(0, teb_.PoseVertex(0)); + acceleration_edge->setVertex(1, teb_.PoseVertex(1)); + acceleration_edge->setVertex(2, teb_.TimeDiffVertex(0)); + acceleration_edge->setInitialVelocity(vel_start_.second); + acceleration_edge->setInformation(information); + acceleration_edge->setTebConfig(*cfg_); + optimizer_->addEdge(acceleration_edge); } - else + + // now add the usual acceleration edge for each tuple of three teb poses + for (int i = 0; i < n - 2; ++i) { - EdgeObstacle* dist_bandpt_obst_n_l = new EdgeObstacle; - dist_bandpt_obst_n_l->setVertex(0,teb_.PoseVertex(index-neighbourIdx)); - dist_bandpt_obst_n_l->setInformation(information); - dist_bandpt_obst_n_l->setParameters(*cfg_, robot_model_.get(), obst->get()); - optimizer_->addEdge(dist_bandpt_obst_n_l); + EdgeAcceleration *acceleration_edge = new EdgeAcceleration; + acceleration_edge->setVertex(0, teb_.PoseVertex(i)); + acceleration_edge->setVertex(1, teb_.PoseVertex(i + 1)); + acceleration_edge->setVertex(2, teb_.PoseVertex(i + 2)); + acceleration_edge->setVertex(3, teb_.TimeDiffVertex(i)); + acceleration_edge->setVertex(4, teb_.TimeDiffVertex(i + 1)); + acceleration_edge->setInformation(information); + acceleration_edge->setTebConfig(*cfg_); + optimizer_->addEdge(acceleration_edge); } - } - } - - } -} + // check if a goal velocity should be taken into accound + if (vel_goal_.first) + { + EdgeAccelerationGoal *acceleration_edge = new EdgeAccelerationGoal; + acceleration_edge->setVertex(0, teb_.PoseVertex(n - 2)); + acceleration_edge->setVertex(1, teb_.PoseVertex(n - 1)); + acceleration_edge->setVertex(2, teb_.TimeDiffVertex(teb_.sizeTimeDiffs() - 1)); + acceleration_edge->setGoalVelocity(vel_goal_.second); + acceleration_edge->setInformation(information); + acceleration_edge->setTebConfig(*cfg_); + optimizer_->addEdge(acceleration_edge); + } + } + else // holonomic robot + { + Eigen::Matrix information; + information.fill(0); + information(0, 0) = cfg_->optim.weight_acc_lim_x; + information(1, 1) = cfg_->optim.weight_acc_lim_y; + information(2, 2) = cfg_->optim.weight_acc_lim_theta; + + // check if an initial velocity should be taken into accound + if (vel_start_.first) + { + EdgeAccelerationHolonomicStart *acceleration_edge = new EdgeAccelerationHolonomicStart; + acceleration_edge->setVertex(0, teb_.PoseVertex(0)); + acceleration_edge->setVertex(1, teb_.PoseVertex(1)); + acceleration_edge->setVertex(2, teb_.TimeDiffVertex(0)); + acceleration_edge->setInitialVelocity(vel_start_.second); + acceleration_edge->setInformation(information); + acceleration_edge->setTebConfig(*cfg_); + optimizer_->addEdge(acceleration_edge); + } -void TebOptimalPlanner::AddEdgesDynamicObstacles(double weight_multiplier) -{ - if (cfg_->optim.weight_obstacle==0 || weight_multiplier==0 || obstacles_==NULL ) - return; // if weight equals zero skip adding edges! - - Eigen::Matrix information; - information(0,0) = cfg_->optim.weight_dynamic_obstacle * weight_multiplier; - information(1,1) = cfg_->optim.weight_dynamic_obstacle_inflation; - information(0,1) = information(1,0) = 0; - - for (ObstContainer::const_iterator obst = obstacles_->begin(); obst != obstacles_->end(); ++obst) - { - if (!(*obst)->isDynamic()) - continue; - - // Skip first and last pose, as they are fixed - double time = teb_.TimeDiff(0); - for (int i=1; i < teb_.sizePoses() - 1; ++i) - { - EdgeDynamicObstacle* dynobst_edge = new EdgeDynamicObstacle(time); - dynobst_edge->setVertex(0,teb_.PoseVertex(i)); - dynobst_edge->setInformation(information); - dynobst_edge->setParameters(*cfg_, robot_model_.get(), obst->get()); - optimizer_->addEdge(dynobst_edge); - time += teb_.TimeDiff(i); // we do not need to check the time diff bounds, since we iterate to "< sizePoses()-1". - } - } -} + // now add the usual acceleration edge for each tuple of three teb poses + for (int i = 0; i < n - 2; ++i) + { + EdgeAccelerationHolonomic *acceleration_edge = new EdgeAccelerationHolonomic; + acceleration_edge->setVertex(0, teb_.PoseVertex(i)); + acceleration_edge->setVertex(1, teb_.PoseVertex(i + 1)); + acceleration_edge->setVertex(2, teb_.PoseVertex(i + 2)); + acceleration_edge->setVertex(3, teb_.TimeDiffVertex(i)); + acceleration_edge->setVertex(4, teb_.TimeDiffVertex(i + 1)); + acceleration_edge->setInformation(information); + acceleration_edge->setTebConfig(*cfg_); + optimizer_->addEdge(acceleration_edge); + } -void TebOptimalPlanner::AddEdgesViaPoints() -{ - if (cfg_->optim.weight_viapoint==0 || via_points_==NULL || via_points_->empty() ) - return; // if weight equals zero skip adding edges! - - int start_pose_idx = 0; - - int n = teb_.sizePoses(); - if (n<3) // we do not have any degrees of freedom for reaching via-points - return; - - for (ViaPointContainer::const_iterator vp_it = via_points_->begin(); vp_it != via_points_->end(); ++vp_it) - { - - int index = teb_.findClosestTrajectoryPose(*vp_it, NULL, start_pose_idx); - if (cfg_->trajectory.via_points_ordered) - start_pose_idx = index+2; // skip a point to have a DOF inbetween for further via-points - - // check if point conicides with goal or is located behind it - if ( index > n-2 ) - index = n-2; // set to a pose before the goal, since we can move it away! - // check if point coincides with start or is located before it - if ( index < 1) - { - if (cfg_->trajectory.via_points_ordered) - { - index = 1; // try to connect the via point with the second (and non-fixed) pose. It is likely that autoresize adds new poses inbetween later. - } - else - { - ROS_DEBUG("TebOptimalPlanner::AddEdgesViaPoints(): skipping a via-point that is close or behind the current robot pose."); - continue; // skip via points really close or behind the current robot pose - } - } - Eigen::Matrix information; - information.fill(cfg_->optim.weight_viapoint); - - EdgeViaPoint* edge_viapoint = new EdgeViaPoint; - edge_viapoint->setVertex(0,teb_.PoseVertex(index)); - edge_viapoint->setInformation(information); - edge_viapoint->setParameters(*cfg_, &(*vp_it)); - optimizer_->addEdge(edge_viapoint); - } -} - -void TebOptimalPlanner::AddEdgesVelocity() -{ - if (cfg_->robot.max_vel_y == 0) // non-holonomic robot - { - if ( cfg_->optim.weight_max_vel_x==0 && cfg_->optim.weight_max_vel_theta==0) - return; // if weight equals zero skip adding edges! - - int n = teb_.sizePoses(); - Eigen::Matrix information; - information(0,0) = cfg_->optim.weight_max_vel_x; - information(1,1) = cfg_->optim.weight_max_vel_theta; - information(0,1) = 0.0; - information(1,0) = 0.0; - - for (int i=0; i < n - 1; ++i) - { - EdgeVelocity* velocity_edge = new EdgeVelocity; - velocity_edge->setVertex(0,teb_.PoseVertex(i)); - velocity_edge->setVertex(1,teb_.PoseVertex(i+1)); - velocity_edge->setVertex(2,teb_.TimeDiffVertex(i)); - velocity_edge->setInformation(information); - velocity_edge->setTebConfig(*cfg_); - optimizer_->addEdge(velocity_edge); + // check if a goal velocity should be taken into accound + if (vel_goal_.first) + { + EdgeAccelerationHolonomicGoal *acceleration_edge = new EdgeAccelerationHolonomicGoal; + acceleration_edge->setVertex(0, teb_.PoseVertex(n - 2)); + acceleration_edge->setVertex(1, teb_.PoseVertex(n - 1)); + acceleration_edge->setVertex(2, teb_.TimeDiffVertex(teb_.sizeTimeDiffs() - 1)); + acceleration_edge->setGoalVelocity(vel_goal_.second); + acceleration_edge->setInformation(information); + acceleration_edge->setTebConfig(*cfg_); + optimizer_->addEdge(acceleration_edge); + } + } } - } - else // holonomic-robot - { - if ( cfg_->optim.weight_max_vel_x==0 && cfg_->optim.weight_max_vel_y==0 && cfg_->optim.weight_max_vel_theta==0) - return; // if weight equals zero skip adding edges! - - int n = teb_.sizePoses(); - Eigen::Matrix information; - information.fill(0); - information(0,0) = cfg_->optim.weight_max_vel_x; - information(1,1) = cfg_->optim.weight_max_vel_y; - information(2,2) = cfg_->optim.weight_max_vel_theta; - - for (int i=0; i < n - 1; ++i) - { - EdgeVelocityHolonomic* velocity_edge = new EdgeVelocityHolonomic; - velocity_edge->setVertex(0,teb_.PoseVertex(i)); - velocity_edge->setVertex(1,teb_.PoseVertex(i+1)); - velocity_edge->setVertex(2,teb_.TimeDiffVertex(i)); - velocity_edge->setInformation(information); - velocity_edge->setTebConfig(*cfg_); - optimizer_->addEdge(velocity_edge); - } - - } -} - -void TebOptimalPlanner::AddEdgesAcceleration() -{ - if (cfg_->optim.weight_acc_lim_x==0 && cfg_->optim.weight_acc_lim_theta==0) - return; // if weight equals zero skip adding edges! - - int n = teb_.sizePoses(); - - if (cfg_->robot.max_vel_y == 0 || cfg_->robot.acc_lim_y == 0) // non-holonomic robot - { - Eigen::Matrix information; - information.fill(0); - information(0,0) = cfg_->optim.weight_acc_lim_x; - information(1,1) = cfg_->optim.weight_acc_lim_theta; - - // check if an initial velocity should be taken into accound - if (vel_start_.first) + + void TebOptimalPlanner::AddEdgesTimeOptimal() { - EdgeAccelerationStart* acceleration_edge = new EdgeAccelerationStart; - acceleration_edge->setVertex(0,teb_.PoseVertex(0)); - acceleration_edge->setVertex(1,teb_.PoseVertex(1)); - acceleration_edge->setVertex(2,teb_.TimeDiffVertex(0)); - acceleration_edge->setInitialVelocity(vel_start_.second); - acceleration_edge->setInformation(information); - acceleration_edge->setTebConfig(*cfg_); - optimizer_->addEdge(acceleration_edge); + if (cfg_->optim.weight_optimaltime == 0) + return; // if weight equals zero skip adding edges! + + Eigen::Matrix information; + information.fill(cfg_->optim.weight_optimaltime); + + for (int i = 0; i < teb_.sizeTimeDiffs(); ++i) + { + EdgeTimeOptimal *timeoptimal_edge = new EdgeTimeOptimal; + timeoptimal_edge->setVertex(0, teb_.TimeDiffVertex(i)); + timeoptimal_edge->setInformation(information); + timeoptimal_edge->setTebConfig(*cfg_); + optimizer_->addEdge(timeoptimal_edge); + } } - // now add the usual acceleration edge for each tuple of three teb poses - for (int i=0; i < n - 2; ++i) + void TebOptimalPlanner::AddEdgesKinematicsDiffDrive() { - EdgeAcceleration* acceleration_edge = new EdgeAcceleration; - acceleration_edge->setVertex(0,teb_.PoseVertex(i)); - acceleration_edge->setVertex(1,teb_.PoseVertex(i+1)); - acceleration_edge->setVertex(2,teb_.PoseVertex(i+2)); - acceleration_edge->setVertex(3,teb_.TimeDiffVertex(i)); - acceleration_edge->setVertex(4,teb_.TimeDiffVertex(i+1)); - acceleration_edge->setInformation(information); - acceleration_edge->setTebConfig(*cfg_); - optimizer_->addEdge(acceleration_edge); + if (cfg_->optim.weight_kinematics_nh == 0 && cfg_->optim.weight_kinematics_forward_drive == 0) + return; // if weight equals zero skip adding edges! + + // create edge for satisfiying kinematic constraints + Eigen::Matrix information_kinematics; + information_kinematics.fill(0.0); + information_kinematics(0, 0) = cfg_->optim.weight_kinematics_nh; + information_kinematics(1, 1) = cfg_->optim.weight_kinematics_forward_drive; + + for (int i = 0; i < teb_.sizePoses() - 1; i++) // ignore twiced start only + { + EdgeKinematicsDiffDrive *kinematics_edge = new EdgeKinematicsDiffDrive; + kinematics_edge->setVertex(0, teb_.PoseVertex(i)); + kinematics_edge->setVertex(1, teb_.PoseVertex(i + 1)); + kinematics_edge->setInformation(information_kinematics); + kinematics_edge->setTebConfig(*cfg_); + optimizer_->addEdge(kinematics_edge); + } } - - // check if a goal velocity should be taken into accound - if (vel_goal_.first) - { - EdgeAccelerationGoal* acceleration_edge = new EdgeAccelerationGoal; - acceleration_edge->setVertex(0,teb_.PoseVertex(n-2)); - acceleration_edge->setVertex(1,teb_.PoseVertex(n-1)); - acceleration_edge->setVertex(2,teb_.TimeDiffVertex( teb_.sizeTimeDiffs()-1 )); - acceleration_edge->setGoalVelocity(vel_goal_.second); - acceleration_edge->setInformation(information); - acceleration_edge->setTebConfig(*cfg_); - optimizer_->addEdge(acceleration_edge); - } - } - else // holonomic robot - { - Eigen::Matrix information; - information.fill(0); - information(0,0) = cfg_->optim.weight_acc_lim_x; - information(1,1) = cfg_->optim.weight_acc_lim_y; - information(2,2) = cfg_->optim.weight_acc_lim_theta; - - // check if an initial velocity should be taken into accound - if (vel_start_.first) + + void TebOptimalPlanner::AddEdgesKinematicsCarlike() { - EdgeAccelerationHolonomicStart* acceleration_edge = new EdgeAccelerationHolonomicStart; - acceleration_edge->setVertex(0,teb_.PoseVertex(0)); - acceleration_edge->setVertex(1,teb_.PoseVertex(1)); - acceleration_edge->setVertex(2,teb_.TimeDiffVertex(0)); - acceleration_edge->setInitialVelocity(vel_start_.second); - acceleration_edge->setInformation(information); - acceleration_edge->setTebConfig(*cfg_); - optimizer_->addEdge(acceleration_edge); + if (cfg_->optim.weight_kinematics_nh == 0 && cfg_->optim.weight_kinematics_turning_radius == 0) + return; // if weight equals zero skip adding edges! + + // create edge for satisfiying kinematic constraints + Eigen::Matrix information_kinematics; + information_kinematics.fill(0.0); + information_kinematics(0, 0) = cfg_->optim.weight_kinematics_nh; + information_kinematics(1, 1) = cfg_->optim.weight_kinematics_turning_radius; + information_kinematics(2, 2) = cfg_->optim.weight_max_steering_angle; + + for (int i = 0; i < teb_.sizePoses() - 1; i++) // ignore twiced start only + { + EdgeKinematicsCarlike *kinematics_edge = new EdgeKinematicsCarlike; + kinematics_edge->setVertex(0, teb_.PoseVertex(i)); + kinematics_edge->setVertex(1, teb_.PoseVertex(i + 1)); + kinematics_edge->setInformation(information_kinematics); + kinematics_edge->setTebConfig(*cfg_); + optimizer_->addEdge(kinematics_edge); + } } - // now add the usual acceleration edge for each tuple of three teb poses - for (int i=0; i < n - 2; ++i) + void TebOptimalPlanner::AddEdgesPreferRotDir() { - EdgeAccelerationHolonomic* acceleration_edge = new EdgeAccelerationHolonomic; - acceleration_edge->setVertex(0,teb_.PoseVertex(i)); - acceleration_edge->setVertex(1,teb_.PoseVertex(i+1)); - acceleration_edge->setVertex(2,teb_.PoseVertex(i+2)); - acceleration_edge->setVertex(3,teb_.TimeDiffVertex(i)); - acceleration_edge->setVertex(4,teb_.TimeDiffVertex(i+1)); - acceleration_edge->setInformation(information); - acceleration_edge->setTebConfig(*cfg_); - optimizer_->addEdge(acceleration_edge); + // TODO(roesmann): Note, these edges can result in odd predictions, in particular + // we can observe a substantional mismatch between open- and closed-loop planning + // leading to a poor control performance. + // At the moment, we keep these functionality for oscillation recovery: + // Activating the edge for a short time period might not be crucial and + // could move the robot to a new oscillation-free state. + // This needs to be analyzed in more detail! + if (prefer_rotdir_ == RotType::none || cfg_->optim.weight_prefer_rotdir == 0) + return; // if weight equals zero skip adding edges! + + if (prefer_rotdir_ != RotType::right && prefer_rotdir_ != RotType::left) + { + ROS_WARN("TebOptimalPlanner::AddEdgesPreferRotDir(): unsupported RotType selected. Skipping edge creation."); + return; + } + + // create edge for satisfiying kinematic constraints + Eigen::Matrix information_rotdir; + information_rotdir.fill(cfg_->optim.weight_prefer_rotdir); + + for (int i = 0; i < teb_.sizePoses() - 1 && i < 3; ++i) // currently: apply to first 3 rotations + { + EdgePreferRotDir *rotdir_edge = new EdgePreferRotDir; + rotdir_edge->setVertex(0, teb_.PoseVertex(i)); + rotdir_edge->setVertex(1, teb_.PoseVertex(i + 1)); + rotdir_edge->setInformation(information_rotdir); + + if (prefer_rotdir_ == RotType::left) + rotdir_edge->preferLeft(); + else if (prefer_rotdir_ == RotType::right) + rotdir_edge->preferRight(); + + optimizer_->addEdge(rotdir_edge); + } } - - // check if a goal velocity should be taken into accound - if (vel_goal_.first) + + void TebOptimalPlanner::computeCurrentCost(double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost) { - EdgeAccelerationHolonomicGoal* acceleration_edge = new EdgeAccelerationHolonomicGoal; - acceleration_edge->setVertex(0,teb_.PoseVertex(n-2)); - acceleration_edge->setVertex(1,teb_.PoseVertex(n-1)); - acceleration_edge->setVertex(2,teb_.TimeDiffVertex( teb_.sizeTimeDiffs()-1 )); - acceleration_edge->setGoalVelocity(vel_goal_.second); - acceleration_edge->setInformation(information); - acceleration_edge->setTebConfig(*cfg_); - optimizer_->addEdge(acceleration_edge); - } - } -} + // check if graph is empty/exist -> important if function is called between buildGraph and optimizeGraph/clearGraph + bool graph_exist_flag(false); + if (optimizer_->edges().empty() && optimizer_->vertices().empty()) + { + // here the graph is build again, for time efficiency make sure to call this function + // between buildGraph and Optimize (deleted), but it depends on the application + buildGraph(); + optimizer_->initializeOptimization(); + } + else + { + graph_exist_flag = true; + } + + optimizer_->computeInitialGuess(); + + cost_ = 0; + + if (alternative_time_cost) + { + cost_ += teb_.getSumOfAllTimeDiffs(); + // TEST we use SumOfAllTimeDiffs() here, because edge cost depends on number of samples, which is not always the + // same for similar TEBs, since we are using an AutoResize Function with hysteresis. + } + + // now we need pointers to all edges -> calculate error for each edge-type + // since we aren't storing edge pointers, we need to check every edge + for (std::vector::const_iterator it = optimizer_->activeEdges().begin(); it != optimizer_->activeEdges().end(); + it++) + { + EdgeTimeOptimal *edge_time_optimal = dynamic_cast(*it); + if (edge_time_optimal != NULL && !alternative_time_cost) + { + cost_ += edge_time_optimal->getError().squaredNorm(); + continue; + } + EdgeKinematicsDiffDrive *edge_kinematics_dd = dynamic_cast(*it); + if (edge_kinematics_dd != NULL) + { + cost_ += edge_kinematics_dd->getError().squaredNorm(); + continue; + } + EdgeKinematicsCarlike *edge_kinematics_cl = dynamic_cast(*it); + if (edge_kinematics_cl != NULL) + { + cost_ += edge_kinematics_cl->getError().squaredNorm(); + continue; + } -void TebOptimalPlanner::AddEdgesTimeOptimal() -{ - if (cfg_->optim.weight_optimaltime==0) - return; // if weight equals zero skip adding edges! + EdgeVelocity *edge_velocity = dynamic_cast(*it); + if (edge_velocity != NULL) + { + cost_ += edge_velocity->getError().squaredNorm(); + continue; + } - Eigen::Matrix information; - information.fill(cfg_->optim.weight_optimaltime); + EdgeAcceleration *edge_acceleration = dynamic_cast(*it); + if (edge_acceleration != NULL) + { + cost_ += edge_acceleration->getError().squaredNorm(); + continue; + } - for (int i=0; i < teb_.sizeTimeDiffs(); ++i) - { - EdgeTimeOptimal* timeoptimal_edge = new EdgeTimeOptimal; - timeoptimal_edge->setVertex(0,teb_.TimeDiffVertex(i)); - timeoptimal_edge->setInformation(information); - timeoptimal_edge->setTebConfig(*cfg_); - optimizer_->addEdge(timeoptimal_edge); - } -} + EdgeObstacle *edge_obstacle = dynamic_cast(*it); + if (edge_obstacle != NULL) + { + cost_ += edge_obstacle->getError().squaredNorm() * obst_cost_scale; + continue; + } + EdgeInflatedObstacle *edge_inflated_obstacle = dynamic_cast(*it); + if (edge_inflated_obstacle != NULL) + { + cost_ += std::sqrt(std::pow(edge_inflated_obstacle->getError()[0], 2) * obst_cost_scale + + std::pow(edge_inflated_obstacle->getError()[1], 2)); + continue; + } + EdgeDynamicObstacle *edge_dyn_obstacle = dynamic_cast(*it); + if (edge_dyn_obstacle != NULL) + { + cost_ += edge_dyn_obstacle->getError().squaredNorm() * obst_cost_scale; + continue; + } -void TebOptimalPlanner::AddEdgesKinematicsDiffDrive() -{ - if (cfg_->optim.weight_kinematics_nh==0 && cfg_->optim.weight_kinematics_forward_drive==0) - return; // if weight equals zero skip adding edges! - - // create edge for satisfiying kinematic constraints - Eigen::Matrix information_kinematics; - information_kinematics.fill(0.0); - information_kinematics(0, 0) = cfg_->optim.weight_kinematics_nh; - information_kinematics(1, 1) = cfg_->optim.weight_kinematics_forward_drive; - - for (int i=0; i < teb_.sizePoses()-1; i++) // ignore twiced start only - { - EdgeKinematicsDiffDrive* kinematics_edge = new EdgeKinematicsDiffDrive; - kinematics_edge->setVertex(0,teb_.PoseVertex(i)); - kinematics_edge->setVertex(1,teb_.PoseVertex(i+1)); - kinematics_edge->setInformation(information_kinematics); - kinematics_edge->setTebConfig(*cfg_); - optimizer_->addEdge(kinematics_edge); - } -} - -void TebOptimalPlanner::AddEdgesKinematicsCarlike() -{ - if (cfg_->optim.weight_kinematics_nh==0 && cfg_->optim.weight_kinematics_turning_radius==0) - return; // if weight equals zero skip adding edges! - - // create edge for satisfiying kinematic constraints - Eigen::Matrix information_kinematics; - information_kinematics.fill(0.0); - information_kinematics(0, 0) = cfg_->optim.weight_kinematics_nh; - information_kinematics(1, 1) = cfg_->optim.weight_kinematics_turning_radius; - - for (int i=0; i < teb_.sizePoses()-1; i++) // ignore twiced start only - { - EdgeKinematicsCarlike* kinematics_edge = new EdgeKinematicsCarlike; - kinematics_edge->setVertex(0,teb_.PoseVertex(i)); - kinematics_edge->setVertex(1,teb_.PoseVertex(i+1)); - kinematics_edge->setInformation(information_kinematics); - kinematics_edge->setTebConfig(*cfg_); - optimizer_->addEdge(kinematics_edge); - } -} - - -void TebOptimalPlanner::AddEdgesPreferRotDir() -{ - //TODO(roesmann): Note, these edges can result in odd predictions, in particular - // we can observe a substantional mismatch between open- and closed-loop planning - // leading to a poor control performance. - // At the moment, we keep these functionality for oscillation recovery: - // Activating the edge for a short time period might not be crucial and - // could move the robot to a new oscillation-free state. - // This needs to be analyzed in more detail! - if (prefer_rotdir_ == RotType::none || cfg_->optim.weight_prefer_rotdir==0) - return; // if weight equals zero skip adding edges! - - if (prefer_rotdir_ != RotType::right && prefer_rotdir_ != RotType::left) - { - ROS_WARN("TebOptimalPlanner::AddEdgesPreferRotDir(): unsupported RotType selected. Skipping edge creation."); - return; - } - - // create edge for satisfiying kinematic constraints - Eigen::Matrix information_rotdir; - information_rotdir.fill(cfg_->optim.weight_prefer_rotdir); - - for (int i=0; i < teb_.sizePoses()-1 && i < 3; ++i) // currently: apply to first 3 rotations - { - EdgePreferRotDir* rotdir_edge = new EdgePreferRotDir; - rotdir_edge->setVertex(0,teb_.PoseVertex(i)); - rotdir_edge->setVertex(1,teb_.PoseVertex(i+1)); - rotdir_edge->setInformation(information_rotdir); - - if (prefer_rotdir_ == RotType::left) - rotdir_edge->preferLeft(); - else if (prefer_rotdir_ == RotType::right) - rotdir_edge->preferRight(); - - optimizer_->addEdge(rotdir_edge); - } -} - -void TebOptimalPlanner::computeCurrentCost(double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost) -{ - // check if graph is empty/exist -> important if function is called between buildGraph and optimizeGraph/clearGraph - bool graph_exist_flag(false); - if (optimizer_->edges().empty() && optimizer_->vertices().empty()) - { - // here the graph is build again, for time efficiency make sure to call this function - // between buildGraph and Optimize (deleted), but it depends on the application - buildGraph(); - optimizer_->initializeOptimization(); - } - else - { - graph_exist_flag = true; - } - - optimizer_->computeInitialGuess(); - - cost_ = 0; - - if (alternative_time_cost) - { - cost_ += teb_.getSumOfAllTimeDiffs(); - // TEST we use SumOfAllTimeDiffs() here, because edge cost depends on number of samples, which is not always the same for similar TEBs, - // since we are using an AutoResize Function with hysteresis. - } - - // now we need pointers to all edges -> calculate error for each edge-type - // since we aren't storing edge pointers, we need to check every edge - for (std::vector::const_iterator it = optimizer_->activeEdges().begin(); it!= optimizer_->activeEdges().end(); it++) - { - EdgeTimeOptimal* edge_time_optimal = dynamic_cast(*it); - if (edge_time_optimal!=NULL && !alternative_time_cost) - { - cost_ += edge_time_optimal->getError().squaredNorm(); - continue; - } + EdgeViaPoint *edge_viapoint = dynamic_cast(*it); + if (edge_viapoint != NULL) + { + cost_ += edge_viapoint->getError().squaredNorm() * viapoint_cost_scale; + continue; + } + } - EdgeKinematicsDiffDrive* edge_kinematics_dd = dynamic_cast(*it); - if (edge_kinematics_dd!=NULL) - { - cost_ += edge_kinematics_dd->getError().squaredNorm(); - continue; - } - - EdgeKinematicsCarlike* edge_kinematics_cl = dynamic_cast(*it); - if (edge_kinematics_cl!=NULL) - { - cost_ += edge_kinematics_cl->getError().squaredNorm(); - continue; - } - - EdgeVelocity* edge_velocity = dynamic_cast(*it); - if (edge_velocity!=NULL) - { - cost_ += edge_velocity->getError().squaredNorm(); - continue; - } - - EdgeAcceleration* edge_acceleration = dynamic_cast(*it); - if (edge_acceleration!=NULL) - { - cost_ += edge_acceleration->getError().squaredNorm(); - continue; + // delete temporary created graph + if (!graph_exist_flag) + clearGraph(); } - - EdgeObstacle* edge_obstacle = dynamic_cast(*it); - if (edge_obstacle!=NULL) + + void TebOptimalPlanner::extractVelocity(const PoseSE2 &pose1, const PoseSE2 &pose2, double dt, double &vx, double &vy, double &omega) const { - cost_ += edge_obstacle->getError().squaredNorm() * obst_cost_scale; - continue; + if (dt == 0) + { + vx = 0; + vy = 0; + omega = 0; + return; + } + + Eigen::Vector2d deltaS = pose2.position() - pose1.position(); + + if (cfg_->robot.max_vel_y == 0) // nonholonomic robot + { + Eigen::Vector2d conf1dir(cos(pose1.theta()), sin(pose1.theta())); + // translational velocity + double dir = deltaS.dot(conf1dir); + vx = (double)g2o::sign(dir) * deltaS.norm() / dt; + vy = 0; + } + else // holonomic robot + { + // transform pose 2 into the current robot frame (pose1) + // for velocities only the rotation of the direction vector is necessary. + // (map->pose1-frame: inverse 2d rotation matrix) + double cos_theta1 = std::cos(pose1.theta()); + double sin_theta1 = std::sin(pose1.theta()); + double p1_dx = cos_theta1 * deltaS.x() + sin_theta1 * deltaS.y(); + double p1_dy = -sin_theta1 * deltaS.x() + cos_theta1 * deltaS.y(); + vx = p1_dx / dt; + vy = p1_dy / dt; + } + + // rotational velocity + double orientdiff = g2o::normalize_theta(pose2.theta() - pose1.theta()); + omega = orientdiff / dt; } - - EdgeInflatedObstacle* edge_inflated_obstacle = dynamic_cast(*it); - if (edge_inflated_obstacle!=NULL) + + bool TebOptimalPlanner::getVelocityCommand(double &vx, double &vy, double &omega) const { - cost_ += std::sqrt(std::pow(edge_inflated_obstacle->getError()[0],2) * obst_cost_scale - + std::pow(edge_inflated_obstacle->getError()[1],2)); - continue; + if (teb_.sizePoses() < 2) + { + ROS_ERROR("TebOptimalPlanner::getVelocityCommand(): The trajectory contains less than 2 poses. Make sure to init " + "and optimize/plan the trajectory fist."); + vx = 0; + vy = 0; + omega = 0; + return false; + } + + double dt = teb_.TimeDiff(0); + if (dt <= 0) + { + ROS_ERROR("TebOptimalPlanner::getVelocityCommand() - timediff<=0 is invalid!"); + vx = 0; + vy = 0; + omega = 0; + return false; + } + + // Get velocity from the first two configurations + extractVelocity(teb_.Pose(0), teb_.Pose(1), dt, vx, vy, omega); + return true; } - - EdgeDynamicObstacle* edge_dyn_obstacle = dynamic_cast(*it); - if (edge_dyn_obstacle!=NULL) + + void TebOptimalPlanner::getVelocityProfile(std::vector &velocity_profile) const { - cost_ += edge_dyn_obstacle->getError().squaredNorm() * obst_cost_scale; - continue; + int n = teb_.sizePoses(); + velocity_profile.resize(n + 1); + + // start velocity + velocity_profile.front().linear.z = 0; + velocity_profile.front().angular.x = velocity_profile.front().angular.y = 0; + velocity_profile.front().linear.x = vel_start_.second.linear.x; + velocity_profile.front().linear.y = vel_start_.second.linear.y; + velocity_profile.front().angular.z = vel_start_.second.angular.z; + + for (int i = 1; i < n; ++i) + { + velocity_profile[i].linear.z = 0; + velocity_profile[i].angular.x = velocity_profile[i].angular.y = 0; + extractVelocity(teb_.Pose(i - 1), teb_.Pose(i), teb_.TimeDiff(i - 1), velocity_profile[i].linear.x, velocity_profile[i].linear.y, + velocity_profile[i].angular.z); + } + + // goal velocity + velocity_profile.back().linear.z = 0; + velocity_profile.back().angular.x = velocity_profile.back().angular.y = 0; + velocity_profile.back().linear.x = vel_goal_.second.linear.x; + velocity_profile.back().linear.y = vel_goal_.second.linear.y; + velocity_profile.back().angular.z = vel_goal_.second.angular.z; } - - EdgeViaPoint* edge_viapoint = dynamic_cast(*it); - if (edge_viapoint!=NULL) + + void TebOptimalPlanner::getFullTrajectory(std::vector &trajectory) const { - cost_ += edge_viapoint->getError().squaredNorm() * viapoint_cost_scale; - continue; + int n = teb_.sizePoses(); + + trajectory.resize(n); + + if (n == 0) + return; + + double curr_time = 0; + + // start + TrajectoryPointMsg &start = trajectory.front(); + teb_.Pose(0).toPoseMsg(start.pose); + start.velocity.linear.z = 0; + start.velocity.angular.x = start.velocity.angular.y = 0; + start.velocity.linear.x = vel_start_.second.linear.x; + start.velocity.linear.y = vel_start_.second.linear.y; + start.velocity.angular.z = vel_start_.second.angular.z; + start.time_from_start.fromSec(curr_time); + + curr_time += teb_.TimeDiff(0); + + // intermediate points + for (int i = 1; i < n - 1; ++i) + { + TrajectoryPointMsg &point = trajectory[i]; + teb_.Pose(i).toPoseMsg(point.pose); + point.velocity.linear.z = 0; + point.velocity.angular.x = point.velocity.angular.y = 0; + double vel1_x, vel1_y, vel2_x, vel2_y, omega1, omega2; + extractVelocity(teb_.Pose(i - 1), teb_.Pose(i), teb_.TimeDiff(i - 1), vel1_x, vel1_y, omega1); + extractVelocity(teb_.Pose(i), teb_.Pose(i + 1), teb_.TimeDiff(i), vel2_x, vel2_y, omega2); + point.velocity.linear.x = 0.5 * (vel1_x + vel2_x); + point.velocity.linear.y = 0.5 * (vel1_y + vel2_y); + point.velocity.angular.z = 0.5 * (omega1 + omega2); + point.time_from_start.fromSec(curr_time); + + curr_time += teb_.TimeDiff(i); + } + + // goal + TrajectoryPointMsg &goal = trajectory.back(); + teb_.BackPose().toPoseMsg(goal.pose); + goal.velocity.linear.z = 0; + goal.velocity.angular.x = goal.velocity.angular.y = 0; + goal.velocity.linear.x = vel_goal_.second.linear.x; + goal.velocity.linear.y = vel_goal_.second.linear.y; + goal.velocity.angular.z = vel_goal_.second.angular.z; + goal.time_from_start.fromSec(curr_time); } - } - - // delete temporary created graph - if (!graph_exist_flag) - clearGraph(); -} - -void TebOptimalPlanner::extractVelocity(const PoseSE2& pose1, const PoseSE2& pose2, double dt, double& vx, double& vy, double& omega) const -{ - if (dt == 0) - { - vx = 0; - vy = 0; - omega = 0; - return; - } - - Eigen::Vector2d deltaS = pose2.position() - pose1.position(); - - if (cfg_->robot.max_vel_y == 0) // nonholonomic robot - { - Eigen::Vector2d conf1dir( cos(pose1.theta()), sin(pose1.theta()) ); - // translational velocity - double dir = deltaS.dot(conf1dir); - vx = (double) g2o::sign(dir) * deltaS.norm()/dt; - vy = 0; - } - else // holonomic robot - { - // transform pose 2 into the current robot frame (pose1) - // for velocities only the rotation of the direction vector is necessary. - // (map->pose1-frame: inverse 2d rotation matrix) - double cos_theta1 = std::cos(pose1.theta()); - double sin_theta1 = std::sin(pose1.theta()); - double p1_dx = cos_theta1*deltaS.x() + sin_theta1*deltaS.y(); - double p1_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y(); - vx = p1_dx / dt; - vy = p1_dy / dt; - } - - // rotational velocity - double orientdiff = g2o::normalize_theta(pose2.theta() - pose1.theta()); - omega = orientdiff/dt; -} - -bool TebOptimalPlanner::getVelocityCommand(double& vx, double& vy, double& omega) const -{ - if (teb_.sizePoses()<2) - { - ROS_ERROR("TebOptimalPlanner::getVelocityCommand(): The trajectory contains less than 2 poses. Make sure to init and optimize/plan the trajectory fist."); - vx = 0; - vy = 0; - omega = 0; - return false; - } - - double dt = teb_.TimeDiff(0); - if (dt<=0) - { - ROS_ERROR("TebOptimalPlanner::getVelocityCommand() - timediff<=0 is invalid!"); - vx = 0; - vy = 0; - omega = 0; - return false; - } - - // Get velocity from the first two configurations - extractVelocity(teb_.Pose(0), teb_.Pose(1), dt, vx, vy, omega); - return true; -} - -void TebOptimalPlanner::getVelocityProfile(std::vector& velocity_profile) const -{ - int n = teb_.sizePoses(); - velocity_profile.resize( n+1 ); - - // start velocity - velocity_profile.front().linear.z = 0; - velocity_profile.front().angular.x = velocity_profile.front().angular.y = 0; - velocity_profile.front().linear.x = vel_start_.second.linear.x; - velocity_profile.front().linear.y = vel_start_.second.linear.y; - velocity_profile.front().angular.z = vel_start_.second.angular.z; - - for (int i=1; i& trajectory) const -{ - int n = teb_.sizePoses(); - - trajectory.resize(n); - - if (n == 0) - return; - - double curr_time = 0; - - // start - TrajectoryPointMsg& start = trajectory.front(); - teb_.Pose(0).toPoseMsg(start.pose); - start.velocity.linear.z = 0; - start.velocity.angular.x = start.velocity.angular.y = 0; - start.velocity.linear.x = vel_start_.second.linear.x; - start.velocity.linear.y = vel_start_.second.linear.y; - start.velocity.angular.z = vel_start_.second.angular.z; - start.time_from_start.fromSec(curr_time); - - curr_time += teb_.TimeDiff(0); - - // intermediate points - for (int i=1; i < n-1; ++i) - { - TrajectoryPointMsg& point = trajectory[i]; - teb_.Pose(i).toPoseMsg(point.pose); - point.velocity.linear.z = 0; - point.velocity.angular.x = point.velocity.angular.y = 0; - double vel1_x, vel1_y, vel2_x, vel2_y, omega1, omega2; - extractVelocity(teb_.Pose(i-1), teb_.Pose(i), teb_.TimeDiff(i-1), vel1_x, vel1_y, omega1); - extractVelocity(teb_.Pose(i), teb_.Pose(i+1), teb_.TimeDiff(i), vel2_x, vel2_y, omega2); - point.velocity.linear.x = 0.5*(vel1_x+vel2_x); - point.velocity.linear.y = 0.5*(vel1_y+vel2_y); - point.velocity.angular.z = 0.5*(omega1+omega2); - point.time_from_start.fromSec(curr_time); - - curr_time += teb_.TimeDiff(i); - } - - // goal - TrajectoryPointMsg& goal = trajectory.back(); - teb_.BackPose().toPoseMsg(goal.pose); - goal.velocity.linear.z = 0; - goal.velocity.angular.x = goal.velocity.angular.y = 0; - goal.velocity.linear.x = vel_goal_.second.linear.x; - goal.velocity.linear.y = vel_goal_.second.linear.y; - goal.velocity.angular.z = vel_goal_.second.angular.z; - goal.time_from_start.fromSec(curr_time); -} - - -bool TebOptimalPlanner::isTrajectoryFeasible(base_local_planner::CostmapModel* costmap_model, const std::vector& footprint_spec, - double inscribed_radius, double circumscribed_radius, int look_ahead_idx) -{ - if (look_ahead_idx < 0 || look_ahead_idx >= teb().sizePoses()) - look_ahead_idx = teb().sizePoses() - 1; - - for (int i=0; i <= look_ahead_idx; ++i) - { - if ( costmap_model->footprintCost(teb().Pose(i).x(), teb().Pose(i).y(), teb().Pose(i).theta(), footprint_spec, inscribed_radius, circumscribed_radius) < 0 ) - return false; - - // check if distance between two poses is higher than the robot radius and interpolate in that case - // (if obstacles are pushing two consecutive poses away, the center between two consecutive poses might coincide with the obstacle ;-)! - if (i &footprint_spec, double inscribed_radius, + double circumscribed_radius, int look_ahead_idx) { - if ( (teb().Pose(i+1).position()-teb().Pose(i).position()).norm() > inscribed_radius) - { - // check one more time - PoseSE2 center = PoseSE2::average(teb().Pose(i), teb().Pose(i+1)); - if ( costmap_model->footprintCost(center.x(), center.y(), center.theta(), footprint_spec, inscribed_radius, circumscribed_radius) < 0 ) - return false; - } - + if (look_ahead_idx < 0 || look_ahead_idx >= teb().sizePoses()) + look_ahead_idx = teb().sizePoses() - 1; + + for (int i = 0; i <= look_ahead_idx; ++i) + { + if (costmap_model->footprintCost(teb().Pose(i).x(), teb().Pose(i).y(), teb().Pose(i).theta(), footprint_spec, inscribed_radius, + circumscribed_radius) < 0) + return false; + + // check if distance between two poses is higher than the robot radius and interpolate in that case + // (if obstacles are pushing two consecutive poses away, the center between two consecutive poses might coincide + // with the obstacle ;-)! + if (i < look_ahead_idx) + { + if ((teb().Pose(i + 1).position() - teb().Pose(i).position()).norm() > inscribed_radius) + { + // check one more time + PoseSE2 center = PoseSE2::average(teb().Pose(i), teb().Pose(i + 1)); + if (costmap_model->footprintCost(center.x(), center.y(), center.theta(), footprint_spec, inscribed_radius, circumscribed_radius) < + 0) + return false; + } + } + } + return true; } - } - return true; -} - -bool TebOptimalPlanner::isHorizonReductionAppropriate(const std::vector& initial_plan) const -{ - if (teb_.sizePoses() < int( 1.5*double(cfg_->trajectory.min_samples) ) ) // trajectory is short already - return false; - - // check if distance is at least 2m long // hardcoded for now - double dist = 0; - for (int i=1; i < teb_.sizePoses(); ++i) - { - dist += ( teb_.Pose(i).position() - teb_.Pose(i-1).position() ).norm(); - if (dist > 2) - break; - } - if (dist <= 2) - return false; - - // check if goal orientation is differing with more than 90° and the horizon is still long enough to exclude parking maneuvers. - // use case: Sometimes the robot accomplish the following navigation task: - // 1. wall following 2. 180° curve 3. following along the other side of the wall. - // If the trajectory is too long, the trajectory might intersect with the obstace and the optimizer does - // push the trajectory to the correct side. - if ( std::abs( g2o::normalize_theta( teb_.Pose(0).theta() - teb_.BackPose().theta() ) ) > M_PI/2) - { - ROS_DEBUG("TebOptimalPlanner::isHorizonReductionAppropriate(): Goal orientation - start orientation > 90° "); - return true; - } - - // check if goal heading deviates more than 90° w.r.t. start orienation - if (teb_.Pose(0).orientationUnitVec().dot(teb_.BackPose().position() - teb_.Pose(0).position()) < 0) - { - ROS_DEBUG("TebOptimalPlanner::isHorizonReductionAppropriate(): Goal heading - start orientation > 90° "); - return true; - } - - // check ratio: distance along the inital plan and distance of the trajectory (maybe too much is cut off) - int idx=0; // first get point close to the robot (should be fast if the global path is already pruned!) - for (; idx < (int)initial_plan.size(); ++idx) - { - if ( std::sqrt(std::pow(initial_plan[idx].pose.position.x-teb_.Pose(0).x(), 2) + std::pow(initial_plan[idx].pose.position.y-teb_.Pose(0).y(), 2)) ) - break; - } - // now calculate length - double ref_path_length = 0; - for (; idx < int(initial_plan.size())-1; ++idx) - { - ref_path_length += std::sqrt(std::pow(initial_plan[idx+1].pose.position.x-initial_plan[idx].pose.position.x, 2) - + std::pow(initial_plan[idx+1].pose.position.y-initial_plan[idx].pose.position.y, 2) ); - } - - // check distances along the teb trajectory (by the way, we also check if the distance between two poses is > obst_dist) - double teb_length = 0; - for (int i = 1; i < teb_.sizePoses(); ++i ) - { - double dist = (teb_.Pose(i).position() - teb_.Pose(i-1).position()).norm(); - if (dist > 0.95*cfg_->obstacles.min_obstacle_dist) + bool TebOptimalPlanner::isHorizonReductionAppropriate(const std::vector &initial_plan) const { - ROS_DEBUG("TebOptimalPlanner::isHorizonReductionAppropriate(): Distance between consecutive poses > 0.9*min_obstacle_dist"); - return true; + if (teb_.sizePoses() < int(1.5 * double(cfg_->trajectory.min_samples))) // trajectory is short already + return false; + + // check if distance is at least 2m long // hardcoded for now + double dist = 0; + for (int i = 1; i < teb_.sizePoses(); ++i) + { + dist += (teb_.Pose(i).position() - teb_.Pose(i - 1).position()).norm(); + if (dist > 2) + break; + } + if (dist <= 2) + return false; + + // check if goal orientation is differing with more than 90° and the horizon is still long enough to exclude parking + // maneuvers. use case: Sometimes the robot accomplish the following navigation task: + // 1. wall following 2. 180° curve 3. following along the other side of the wall. + // If the trajectory is too long, the trajectory might intersect with the obstace and the optimizer does + // push the trajectory to the correct side. + if (std::abs(g2o::normalize_theta(teb_.Pose(0).theta() - teb_.BackPose().theta())) > M_PI / 2) + { + ROS_DEBUG("TebOptimalPlanner::isHorizonReductionAppropriate(): Goal orientation - start orientation > 90° "); + return true; + } + + // check if goal heading deviates more than 90° w.r.t. start orienation + if (teb_.Pose(0).orientationUnitVec().dot(teb_.BackPose().position() - teb_.Pose(0).position()) < 0) + { + ROS_DEBUG("TebOptimalPlanner::isHorizonReductionAppropriate(): Goal heading - start orientation > 90° "); + return true; + } + + // check ratio: distance along the inital plan and distance of the trajectory (maybe too much is cut off) + int idx = 0; // first get point close to the robot (should be fast if the global path is already pruned!) + for (; idx < (int)initial_plan.size(); ++idx) + { + if (std::sqrt(std::pow(initial_plan[idx].pose.position.x - teb_.Pose(0).x(), 2) + + std::pow(initial_plan[idx].pose.position.y - teb_.Pose(0).y(), 2))) + break; + } + // now calculate length + double ref_path_length = 0; + for (; idx < int(initial_plan.size()) - 1; ++idx) + { + ref_path_length += std::sqrt(std::pow(initial_plan[idx + 1].pose.position.x - initial_plan[idx].pose.position.x, 2) + + std::pow(initial_plan[idx + 1].pose.position.y - initial_plan[idx].pose.position.y, 2)); + } + + // check distances along the teb trajectory (by the way, we also check if the distance between two poses is > + // obst_dist) + double teb_length = 0; + for (int i = 1; i < teb_.sizePoses(); ++i) + { + double dist = (teb_.Pose(i).position() - teb_.Pose(i - 1).position()).norm(); + if (dist > 0.95 * cfg_->obstacles.min_obstacle_dist) + { + ROS_DEBUG("TebOptimalPlanner::isHorizonReductionAppropriate(): Distance between consecutive poses > " + "0.9*min_obstacle_dist"); + return true; + } + ref_path_length += dist; + } + if (ref_path_length > 0 && teb_length / ref_path_length < 0.7) // now check ratio + { + ROS_DEBUG("TebOptimalPlanner::isHorizonReductionAppropriate(): Planned trajectory is at least 30° shorter than " + "the initial plan"); + return true; + } + + // otherwise we do not suggest shrinking the horizon: + return false; } - ref_path_length += dist; - } - if (ref_path_length>0 && teb_length/ref_path_length < 0.7) // now check ratio - { - ROS_DEBUG("TebOptimalPlanner::isHorizonReductionAppropriate(): Planned trajectory is at least 30° shorter than the initial plan"); - return true; - } - - - // otherwise we do not suggest shrinking the horizon: - return false; -} - -} // namespace teb_local_planner + +} // namespace teb_local_planner diff --git a/src/teb_config.cpp b/src/teb_config.cpp index d9408ecf..1794d855 100644 --- a/src/teb_config.cpp +++ b/src/teb_config.cpp @@ -40,298 +40,314 @@ namespace teb_local_planner { - -void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh) -{ - - nh.param("odom_topic", odom_topic, odom_topic); - nh.param("map_frame", map_frame, map_frame); - - // Trajectory - nh.param("teb_autosize", trajectory.teb_autosize, trajectory.teb_autosize); - nh.param("dt_ref", trajectory.dt_ref, trajectory.dt_ref); - nh.param("dt_hysteresis", trajectory.dt_hysteresis, trajectory.dt_hysteresis); - nh.param("min_samples", trajectory.min_samples, trajectory.min_samples); - nh.param("max_samples", trajectory.max_samples, trajectory.max_samples); - nh.param("global_plan_overwrite_orientation", trajectory.global_plan_overwrite_orientation, trajectory.global_plan_overwrite_orientation); - nh.param("allow_init_with_backwards_motion", trajectory.allow_init_with_backwards_motion, trajectory.allow_init_with_backwards_motion); - nh.getParam("global_plan_via_point_sep", trajectory.global_plan_viapoint_sep); // deprecated, see checkDeprecated() - if (!nh.param("global_plan_viapoint_sep", trajectory.global_plan_viapoint_sep, trajectory.global_plan_viapoint_sep)) - nh.setParam("global_plan_viapoint_sep", trajectory.global_plan_viapoint_sep); // write deprecated value to param server - nh.param("via_points_ordered", trajectory.via_points_ordered, trajectory.via_points_ordered); - nh.param("max_global_plan_lookahead_dist", trajectory.max_global_plan_lookahead_dist, trajectory.max_global_plan_lookahead_dist); - nh.param("exact_arc_length", trajectory.exact_arc_length, trajectory.exact_arc_length); - nh.param("force_reinit_new_goal_dist", trajectory.force_reinit_new_goal_dist, trajectory.force_reinit_new_goal_dist); - nh.param("feasibility_check_no_poses", trajectory.feasibility_check_no_poses, trajectory.feasibility_check_no_poses); - nh.param("publish_feedback", trajectory.publish_feedback, trajectory.publish_feedback); - - // Robot - nh.param("max_vel_x", robot.max_vel_x, robot.max_vel_x); - nh.param("max_vel_x_backwards", robot.max_vel_x_backwards, robot.max_vel_x_backwards); - nh.param("max_vel_y", robot.max_vel_y, robot.max_vel_y); - nh.param("max_vel_theta", robot.max_vel_theta, robot.max_vel_theta); - nh.param("acc_lim_x", robot.acc_lim_x, robot.acc_lim_x); - nh.param("acc_lim_y", robot.acc_lim_y, robot.acc_lim_y); - nh.param("acc_lim_theta", robot.acc_lim_theta, robot.acc_lim_theta); - nh.param("min_turning_radius", robot.min_turning_radius, robot.min_turning_radius); - nh.param("wheelbase", robot.wheelbase, robot.wheelbase); - nh.param("cmd_angle_instead_rotvel", robot.cmd_angle_instead_rotvel, robot.cmd_angle_instead_rotvel); - nh.param("is_footprint_dynamic", robot.is_footprint_dynamic, robot.is_footprint_dynamic); - - // GoalTolerance - nh.param("xy_goal_tolerance", goal_tolerance.xy_goal_tolerance, goal_tolerance.xy_goal_tolerance); - nh.param("yaw_goal_tolerance", goal_tolerance.yaw_goal_tolerance, goal_tolerance.yaw_goal_tolerance); - nh.param("free_goal_vel", goal_tolerance.free_goal_vel, goal_tolerance.free_goal_vel); - nh.param("complete_global_plan", goal_tolerance.complete_global_plan, goal_tolerance.complete_global_plan); - - // Obstacles - nh.param("min_obstacle_dist", obstacles.min_obstacle_dist, obstacles.min_obstacle_dist); - nh.param("inflation_dist", obstacles.inflation_dist, obstacles.inflation_dist); - nh.param("dynamic_obstacle_inflation_dist", obstacles.dynamic_obstacle_inflation_dist, obstacles.dynamic_obstacle_inflation_dist); - nh.param("include_dynamic_obstacles", obstacles.include_dynamic_obstacles, obstacles.include_dynamic_obstacles); - nh.param("include_costmap_obstacles", obstacles.include_costmap_obstacles, obstacles.include_costmap_obstacles); - nh.param("costmap_obstacles_behind_robot_dist", obstacles.costmap_obstacles_behind_robot_dist, obstacles.costmap_obstacles_behind_robot_dist); - nh.param("obstacle_poses_affected", obstacles.obstacle_poses_affected, obstacles.obstacle_poses_affected); - nh.param("legacy_obstacle_association", obstacles.legacy_obstacle_association, obstacles.legacy_obstacle_association); - nh.param("obstacle_association_force_inclusion_factor", obstacles.obstacle_association_force_inclusion_factor, obstacles.obstacle_association_force_inclusion_factor); - nh.param("obstacle_association_cutoff_factor", obstacles.obstacle_association_cutoff_factor, obstacles.obstacle_association_cutoff_factor); - nh.param("costmap_converter_plugin", obstacles.costmap_converter_plugin, obstacles.costmap_converter_plugin); - nh.param("costmap_converter_spin_thread", obstacles.costmap_converter_spin_thread, obstacles.costmap_converter_spin_thread); - - // Optimization - nh.param("no_inner_iterations", optim.no_inner_iterations, optim.no_inner_iterations); - nh.param("no_outer_iterations", optim.no_outer_iterations, optim.no_outer_iterations); - nh.param("optimization_activate", optim.optimization_activate, optim.optimization_activate); - nh.param("optimization_verbose", optim.optimization_verbose, optim.optimization_verbose); - nh.param("penalty_epsilon", optim.penalty_epsilon, optim.penalty_epsilon); - nh.param("weight_max_vel_x", optim.weight_max_vel_x, optim.weight_max_vel_x); - nh.param("weight_max_vel_y", optim.weight_max_vel_y, optim.weight_max_vel_y); - nh.param("weight_max_vel_theta", optim.weight_max_vel_theta, optim.weight_max_vel_theta); - nh.param("weight_acc_lim_x", optim.weight_acc_lim_x, optim.weight_acc_lim_x); - nh.param("weight_acc_lim_y", optim.weight_acc_lim_y, optim.weight_acc_lim_y); - nh.param("weight_acc_lim_theta", optim.weight_acc_lim_theta, optim.weight_acc_lim_theta); - nh.param("weight_kinematics_nh", optim.weight_kinematics_nh, optim.weight_kinematics_nh); - nh.param("weight_kinematics_forward_drive", optim.weight_kinematics_forward_drive, optim.weight_kinematics_forward_drive); - nh.param("weight_kinematics_turning_radius", optim.weight_kinematics_turning_radius, optim.weight_kinematics_turning_radius); - nh.param("weight_optimaltime", optim.weight_optimaltime, optim.weight_optimaltime); - nh.param("weight_obstacle", optim.weight_obstacle, optim.weight_obstacle); - nh.param("weight_inflation", optim.weight_inflation, optim.weight_inflation); - nh.param("weight_dynamic_obstacle", optim.weight_dynamic_obstacle, optim.weight_dynamic_obstacle); - nh.param("weight_dynamic_obstacle_inflation", optim.weight_dynamic_obstacle_inflation, optim.weight_dynamic_obstacle_inflation); - nh.param("weight_viapoint", optim.weight_viapoint, optim.weight_viapoint); - nh.param("weight_prefer_rotdir", optim.weight_prefer_rotdir, optim.weight_prefer_rotdir); - nh.param("weight_adapt_factor", optim.weight_adapt_factor, optim.weight_adapt_factor); - - // Homotopy Class Planner - nh.param("enable_homotopy_class_planning", hcp.enable_homotopy_class_planning, hcp.enable_homotopy_class_planning); - nh.param("enable_multithreading", hcp.enable_multithreading, hcp.enable_multithreading); - nh.param("simple_exploration", hcp.simple_exploration, hcp.simple_exploration); - nh.param("max_number_classes", hcp.max_number_classes, hcp.max_number_classes); - nh.param("selection_obst_cost_scale", hcp.selection_obst_cost_scale, hcp.selection_obst_cost_scale); - nh.param("selection_prefer_initial_plan", hcp.selection_prefer_initial_plan, hcp.selection_prefer_initial_plan); - nh.param("selection_viapoint_cost_scale", hcp.selection_viapoint_cost_scale, hcp.selection_viapoint_cost_scale); - nh.param("selection_cost_hysteresis", hcp.selection_cost_hysteresis, hcp.selection_cost_hysteresis); - nh.param("selection_alternative_time_cost", hcp.selection_alternative_time_cost, hcp.selection_alternative_time_cost); - nh.param("switching_blocking_period", hcp.switching_blocking_period, hcp.switching_blocking_period); - nh.param("roadmap_graph_samples", hcp.roadmap_graph_no_samples, hcp.roadmap_graph_no_samples); - nh.param("roadmap_graph_area_width", hcp.roadmap_graph_area_width, hcp.roadmap_graph_area_width); - nh.param("roadmap_graph_area_length_scale", hcp.roadmap_graph_area_length_scale, hcp.roadmap_graph_area_length_scale); - nh.param("h_signature_prescaler", hcp.h_signature_prescaler, hcp.h_signature_prescaler); - nh.param("h_signature_threshold", hcp.h_signature_threshold, hcp.h_signature_threshold); - nh.param("obstacle_keypoint_offset", hcp.obstacle_keypoint_offset, hcp.obstacle_keypoint_offset); - nh.param("obstacle_heading_threshold", hcp.obstacle_heading_threshold, hcp.obstacle_heading_threshold); - nh.param("viapoints_all_candidates", hcp.viapoints_all_candidates, hcp.viapoints_all_candidates); - nh.param("visualize_hc_graph", hcp.visualize_hc_graph, hcp.visualize_hc_graph); - nh.param("visualize_with_time_as_z_axis_scale", hcp.visualize_with_time_as_z_axis_scale, hcp.visualize_with_time_as_z_axis_scale); - - // Recovery - - nh.param("shrink_horizon_backup", recovery.shrink_horizon_backup, recovery.shrink_horizon_backup); - nh.param("shrink_horizon_min_duration", recovery.shrink_horizon_min_duration, recovery.shrink_horizon_min_duration); - nh.param("oscillation_recovery", recovery.oscillation_recovery, recovery.oscillation_recovery); - nh.param("oscillation_v_eps", recovery.oscillation_v_eps, recovery.oscillation_v_eps); - nh.param("oscillation_omega_eps", recovery.oscillation_omega_eps, recovery.oscillation_omega_eps); - nh.param("oscillation_recovery_min_duration", recovery.oscillation_recovery_min_duration, recovery.oscillation_recovery_min_duration); - nh.param("oscillation_filter_duration", recovery.oscillation_filter_duration, recovery.oscillation_filter_duration); - - checkParameters(); - checkDeprecated(nh); -} - -void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg) -{ - boost::mutex::scoped_lock l(config_mutex_); - - // Trajectory - trajectory.teb_autosize = cfg.teb_autosize; - trajectory.dt_ref = cfg.dt_ref; - trajectory.dt_hysteresis = cfg.dt_hysteresis; - trajectory.global_plan_overwrite_orientation = cfg.global_plan_overwrite_orientation; - trajectory.allow_init_with_backwards_motion = cfg.allow_init_with_backwards_motion; - trajectory.global_plan_viapoint_sep = cfg.global_plan_viapoint_sep; - trajectory.via_points_ordered = cfg.via_points_ordered; - trajectory.max_global_plan_lookahead_dist = cfg.max_global_plan_lookahead_dist; - trajectory.exact_arc_length = cfg.exact_arc_length; - trajectory.force_reinit_new_goal_dist = cfg.force_reinit_new_goal_dist; - trajectory.feasibility_check_no_poses = cfg.feasibility_check_no_poses; - trajectory.publish_feedback = cfg.publish_feedback; - - // Robot - robot.max_vel_x = cfg.max_vel_x; - robot.max_vel_x_backwards = cfg.max_vel_x_backwards; - robot.max_vel_y = cfg.max_vel_y; - robot.max_vel_theta = cfg.max_vel_theta; - robot.acc_lim_x = cfg.acc_lim_x; - robot.acc_lim_y = cfg.acc_lim_y; - robot.acc_lim_theta = cfg.acc_lim_theta; - robot.min_turning_radius = cfg.min_turning_radius; - robot.wheelbase = cfg.wheelbase; - robot.cmd_angle_instead_rotvel = cfg.cmd_angle_instead_rotvel; - - // GoalTolerance - goal_tolerance.xy_goal_tolerance = cfg.xy_goal_tolerance; - goal_tolerance.yaw_goal_tolerance = cfg.yaw_goal_tolerance; - goal_tolerance.free_goal_vel = cfg.free_goal_vel; - - // Obstacles - obstacles.min_obstacle_dist = cfg.min_obstacle_dist; - obstacles.inflation_dist = cfg.inflation_dist; - obstacles.dynamic_obstacle_inflation_dist = cfg.dynamic_obstacle_inflation_dist; - obstacles.include_dynamic_obstacles = cfg.include_dynamic_obstacles; - obstacles.include_costmap_obstacles = cfg.include_costmap_obstacles; - obstacles.legacy_obstacle_association = cfg.legacy_obstacle_association; - obstacles.obstacle_association_force_inclusion_factor = cfg.obstacle_association_force_inclusion_factor; - obstacles.obstacle_association_cutoff_factor = cfg.obstacle_association_cutoff_factor; - obstacles.costmap_obstacles_behind_robot_dist = cfg.costmap_obstacles_behind_robot_dist; - obstacles.obstacle_poses_affected = cfg.obstacle_poses_affected; - - - // Optimization - optim.no_inner_iterations = cfg.no_inner_iterations; - optim.no_outer_iterations = cfg.no_outer_iterations; - optim.optimization_activate = cfg.optimization_activate; - optim.optimization_verbose = cfg.optimization_verbose; - optim.penalty_epsilon = cfg.penalty_epsilon; - optim.weight_max_vel_x = cfg.weight_max_vel_x; - optim.weight_max_vel_y = cfg.weight_max_vel_y; - optim.weight_max_vel_theta = cfg.weight_max_vel_theta; - optim.weight_acc_lim_x = cfg.weight_acc_lim_x; - optim.weight_acc_lim_y = cfg.weight_acc_lim_y; - optim.weight_acc_lim_theta = cfg.weight_acc_lim_theta; - optim.weight_kinematics_nh = cfg.weight_kinematics_nh; - optim.weight_kinematics_forward_drive = cfg.weight_kinematics_forward_drive; - optim.weight_kinematics_turning_radius = cfg.weight_kinematics_turning_radius; - optim.weight_optimaltime = cfg.weight_optimaltime; - optim.weight_obstacle = cfg.weight_obstacle; - optim.weight_inflation = cfg.weight_inflation; - optim.weight_dynamic_obstacle = cfg.weight_dynamic_obstacle; - optim.weight_dynamic_obstacle_inflation = cfg.weight_dynamic_obstacle_inflation; - optim.weight_viapoint = cfg.weight_viapoint; - optim.weight_adapt_factor = cfg.weight_adapt_factor; - - // Homotopy Class Planner - hcp.enable_multithreading = cfg.enable_multithreading; - hcp.max_number_classes = cfg.max_number_classes; - hcp.selection_cost_hysteresis = cfg.selection_cost_hysteresis; - hcp.selection_prefer_initial_plan = cfg.selection_prefer_initial_plan; - hcp.selection_obst_cost_scale = cfg.selection_obst_cost_scale; - hcp.selection_viapoint_cost_scale = cfg.selection_viapoint_cost_scale; - hcp.selection_alternative_time_cost = cfg.selection_alternative_time_cost; - hcp.switching_blocking_period = cfg.switching_blocking_period; - - hcp.obstacle_heading_threshold = cfg.obstacle_heading_threshold; - hcp.roadmap_graph_no_samples = cfg.roadmap_graph_no_samples; - hcp.roadmap_graph_area_width = cfg.roadmap_graph_area_width; - hcp.roadmap_graph_area_length_scale = cfg.roadmap_graph_area_length_scale; - hcp.h_signature_prescaler = cfg.h_signature_prescaler; - hcp.h_signature_threshold = cfg.h_signature_threshold; - hcp.viapoints_all_candidates = cfg.viapoints_all_candidates; - hcp.visualize_hc_graph = cfg.visualize_hc_graph; - hcp.visualize_with_time_as_z_axis_scale = cfg.visualize_with_time_as_z_axis_scale; - - // Recovery - - recovery.shrink_horizon_backup = cfg.shrink_horizon_backup; - recovery.oscillation_recovery = cfg.oscillation_recovery; - - checkParameters(); -} - - -void TebConfig::checkParameters() const -{ - // positive backward velocity? - if (robot.max_vel_x_backwards <= 0) - ROS_WARN("TebLocalPlannerROS() Param Warning: Do not choose max_vel_x_backwards to be <=0. Disable backwards driving by increasing the optimization weight for penalyzing backwards driving."); - - // bounds smaller than penalty epsilon - if (robot.max_vel_x <= optim.penalty_epsilon) - ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!"); - - if (robot.max_vel_x_backwards <= optim.penalty_epsilon) - ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_x_backwards <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!"); - - if (robot.max_vel_theta <= optim.penalty_epsilon) - ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!"); - - if (robot.acc_lim_x <= optim.penalty_epsilon) - ROS_WARN("TebLocalPlannerROS() Param Warning: acc_lim_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!"); - - if (robot.acc_lim_theta <= optim.penalty_epsilon) - ROS_WARN("TebLocalPlannerROS() Param Warning: acc_lim_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!"); - - // dt_ref and dt_hyst - if (trajectory.dt_ref <= trajectory.dt_hysteresis) - ROS_WARN("TebLocalPlannerROS() Param Warning: dt_ref <= dt_hysteresis. The hysteresis is not allowed to be greater or equal!. Undefined behavior... Change at least one of them!"); - - // min number of samples - if (trajectory.min_samples <3) - ROS_WARN("TebLocalPlannerROS() Param Warning: parameter min_samples is smaller than 3! Sorry, I haven't enough degrees of freedom to plan a trajectory for you. Please increase ..."); - - // costmap obstacle behind robot - if (obstacles.costmap_obstacles_behind_robot_dist < 0) - ROS_WARN("TebLocalPlannerROS() Param Warning: parameter 'costmap_obstacles_behind_robot_dist' should be positive or zero."); - - // hcp: obstacle heading threshold - if (hcp.obstacle_keypoint_offset>=1 || hcp.obstacle_keypoint_offset<=0) - ROS_WARN("TebLocalPlannerROS() Param Warning: parameter obstacle_heading_threshold must be in the interval ]0,1[. 0=0deg opening angle, 1=90deg opening angle."); - - // carlike - if (robot.cmd_angle_instead_rotvel && robot.wheelbase==0) - ROS_WARN("TebLocalPlannerROS() Param Warning: parameter cmd_angle_instead_rotvel is non-zero but wheelbase is set to zero: undesired behavior."); - - if (robot.cmd_angle_instead_rotvel && robot.min_turning_radius==0) - ROS_WARN("TebLocalPlannerROS() Param Warning: parameter cmd_angle_instead_rotvel is non-zero but min_turning_radius is set to zero: undesired behavior. You are mixing a carlike and a diffdrive robot"); - - // positive weight_adapt_factor - if (optim.weight_adapt_factor < 1.0) - ROS_WARN("TebLocalPlannerROS() Param Warning: parameter weight_adapt_factor shoud be >= 1.0"); - - if (recovery.oscillation_filter_duration < 0) - ROS_WARN("TebLocalPlannerROS() Param Warning: parameter oscillation_filter_duration must be >= 0"); - -} - -void TebConfig::checkDeprecated(const ros::NodeHandle& nh) const -{ - if (nh.hasParam("line_obstacle_poses_affected") || nh.hasParam("polygon_obstacle_poses_affected")) - ROS_WARN("TebLocalPlannerROS() Param Warning: 'line_obstacle_poses_affected' and 'polygon_obstacle_poses_affected' are deprecated. They share now the common parameter 'obstacle_poses_affected'."); - - if (nh.hasParam("weight_point_obstacle") || nh.hasParam("weight_line_obstacle") || nh.hasParam("weight_poly_obstacle")) - ROS_WARN("TebLocalPlannerROS() Param Warning: 'weight_point_obstacle', 'weight_line_obstacle' and 'weight_poly_obstacle' are deprecated. They are replaced by the single param 'weight_obstacle'."); - - if (nh.hasParam("costmap_obstacles_front_only")) - ROS_WARN("TebLocalPlannerROS() Param Warning: 'costmap_obstacles_front_only' is deprecated. It is replaced by 'costmap_obstacles_behind_robot_dist' to define the actual area taken into account."); - - if (nh.hasParam("costmap_emergency_stop_dist")) - ROS_WARN("TebLocalPlannerROS() Param Warning: 'costmap_emergency_stop_dist' is deprecated. You can safely remove it from your parameter config."); - - if (nh.hasParam("alternative_time_cost")) - ROS_WARN("TebLocalPlannerROS() Param Warning: 'alternative_time_cost' is deprecated. It has been replaced by 'selection_alternative_time_cost'."); - - if (nh.hasParam("global_plan_via_point_sep")) - ROS_WARN("TebLocalPlannerROS() Param Warning: 'global_plan_via_point_sep' is deprecated. It has been replaced by 'global_plan_viapoint_sep' due to consistency reasons."); -} - - -} // namespace teb_local_planner + void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle &nh) + { + nh.param("odom_topic", odom_topic, odom_topic); + nh.param("map_frame", map_frame, map_frame); + + // Trajectory + nh.param("teb_autosize", trajectory.teb_autosize, trajectory.teb_autosize); + nh.param("dt_ref", trajectory.dt_ref, trajectory.dt_ref); + nh.param("dt_hysteresis", trajectory.dt_hysteresis, trajectory.dt_hysteresis); + nh.param("min_samples", trajectory.min_samples, trajectory.min_samples); + nh.param("max_samples", trajectory.max_samples, trajectory.max_samples); + nh.param("global_plan_overwrite_orientation", trajectory.global_plan_overwrite_orientation, trajectory.global_plan_overwrite_orientation); + nh.param("allow_init_with_backwards_motion", trajectory.allow_init_with_backwards_motion, trajectory.allow_init_with_backwards_motion); + nh.getParam("global_plan_via_point_sep", trajectory.global_plan_viapoint_sep); // deprecated, see checkDeprecated() + if (!nh.param("global_plan_viapoint_sep", trajectory.global_plan_viapoint_sep, trajectory.global_plan_viapoint_sep)) + nh.setParam("global_plan_viapoint_sep", trajectory.global_plan_viapoint_sep); // write deprecated value to param server + nh.param("via_points_ordered", trajectory.via_points_ordered, trajectory.via_points_ordered); + nh.param("max_global_plan_lookahead_dist", trajectory.max_global_plan_lookahead_dist, trajectory.max_global_plan_lookahead_dist); + nh.param("exact_arc_length", trajectory.exact_arc_length, trajectory.exact_arc_length); + nh.param("force_reinit_new_goal_dist", trajectory.force_reinit_new_goal_dist, trajectory.force_reinit_new_goal_dist); + nh.param("feasibility_check_no_poses", trajectory.feasibility_check_no_poses, trajectory.feasibility_check_no_poses); + nh.param("publish_feedback", trajectory.publish_feedback, trajectory.publish_feedback); + + // Robot + nh.param("max_vel_x", robot.max_vel_x, robot.max_vel_x); + nh.param("max_vel_x_backwards", robot.max_vel_x_backwards, robot.max_vel_x_backwards); + nh.param("max_vel_y", robot.max_vel_y, robot.max_vel_y); + nh.param("max_vel_theta", robot.max_vel_theta, robot.max_vel_theta); + nh.param("acc_lim_x", robot.acc_lim_x, robot.acc_lim_x); + nh.param("acc_lim_y", robot.acc_lim_y, robot.acc_lim_y); + nh.param("acc_lim_theta", robot.acc_lim_theta, robot.acc_lim_theta); + nh.param("min_turning_radius", robot.min_turning_radius, robot.min_turning_radius); + nh.param("max_steering_angle", robot.max_steering_angle, robot.max_steering_angle); + nh.param("max_steering_rate", robot.max_steering_rate, robot.max_steering_rate); + nh.param("wheelbase", robot.wheelbase, robot.wheelbase); + nh.param("cmd_angle_instead_rotvel", robot.cmd_angle_instead_rotvel, robot.cmd_angle_instead_rotvel); + nh.param("is_footprint_dynamic", robot.is_footprint_dynamic, robot.is_footprint_dynamic); + + // GoalTolerance + nh.param("xy_goal_tolerance", goal_tolerance.xy_goal_tolerance, goal_tolerance.xy_goal_tolerance); + nh.param("yaw_goal_tolerance", goal_tolerance.yaw_goal_tolerance, goal_tolerance.yaw_goal_tolerance); + nh.param("free_goal_vel", goal_tolerance.free_goal_vel, goal_tolerance.free_goal_vel); + nh.param("complete_global_plan", goal_tolerance.complete_global_plan, goal_tolerance.complete_global_plan); + + // Obstacles + nh.param("min_obstacle_dist", obstacles.min_obstacle_dist, obstacles.min_obstacle_dist); + nh.param("inflation_dist", obstacles.inflation_dist, obstacles.inflation_dist); + nh.param("dynamic_obstacle_inflation_dist", obstacles.dynamic_obstacle_inflation_dist, obstacles.dynamic_obstacle_inflation_dist); + nh.param("include_dynamic_obstacles", obstacles.include_dynamic_obstacles, obstacles.include_dynamic_obstacles); + nh.param("include_costmap_obstacles", obstacles.include_costmap_obstacles, obstacles.include_costmap_obstacles); + nh.param("costmap_obstacles_behind_robot_dist", obstacles.costmap_obstacles_behind_robot_dist, obstacles.costmap_obstacles_behind_robot_dist); + nh.param("obstacle_poses_affected", obstacles.obstacle_poses_affected, obstacles.obstacle_poses_affected); + nh.param("legacy_obstacle_association", obstacles.legacy_obstacle_association, obstacles.legacy_obstacle_association); + nh.param("obstacle_association_force_inclusion_factor", obstacles.obstacle_association_force_inclusion_factor, + obstacles.obstacle_association_force_inclusion_factor); + nh.param("obstacle_association_cutoff_factor", obstacles.obstacle_association_cutoff_factor, obstacles.obstacle_association_cutoff_factor); + nh.param("costmap_converter_plugin", obstacles.costmap_converter_plugin, obstacles.costmap_converter_plugin); + nh.param("costmap_converter_spin_thread", obstacles.costmap_converter_spin_thread, obstacles.costmap_converter_spin_thread); + + // Optimization + nh.param("no_inner_iterations", optim.no_inner_iterations, optim.no_inner_iterations); + nh.param("no_outer_iterations", optim.no_outer_iterations, optim.no_outer_iterations); + nh.param("optimization_activate", optim.optimization_activate, optim.optimization_activate); + nh.param("optimization_verbose", optim.optimization_verbose, optim.optimization_verbose); + nh.param("penalty_epsilon", optim.penalty_epsilon, optim.penalty_epsilon); + nh.param("weight_max_vel_x", optim.weight_max_vel_x, optim.weight_max_vel_x); + nh.param("weight_max_vel_y", optim.weight_max_vel_y, optim.weight_max_vel_y); + nh.param("weight_max_vel_theta", optim.weight_max_vel_theta, optim.weight_max_vel_theta); + nh.param("weight_acc_lim_x", optim.weight_acc_lim_x, optim.weight_acc_lim_x); + nh.param("weight_acc_lim_y", optim.weight_acc_lim_y, optim.weight_acc_lim_y); + nh.param("weight_acc_lim_theta", optim.weight_acc_lim_theta, optim.weight_acc_lim_theta); + nh.param("weight_kinematics_nh", optim.weight_kinematics_nh, optim.weight_kinematics_nh); + nh.param("weight_kinematics_forward_drive", optim.weight_kinematics_forward_drive, optim.weight_kinematics_forward_drive); + nh.param("weight_kinematics_turning_radius", optim.weight_kinematics_turning_radius, optim.weight_kinematics_turning_radius); + nh.param("weight_max_steering_angle", optim.weight_max_steering_angle, optim.weight_max_steering_angle); + nh.param("weight_max_steering_rate", optim.weight_max_steering_rate, optim.weight_max_steering_angle); + nh.param("weight_optimaltime", optim.weight_optimaltime, optim.weight_optimaltime); + nh.param("weight_obstacle", optim.weight_obstacle, optim.weight_obstacle); + nh.param("weight_inflation", optim.weight_inflation, optim.weight_inflation); + nh.param("weight_dynamic_obstacle", optim.weight_dynamic_obstacle, optim.weight_dynamic_obstacle); + nh.param("weight_dynamic_obstacle_inflation", optim.weight_dynamic_obstacle_inflation, optim.weight_dynamic_obstacle_inflation); + nh.param("weight_viapoint", optim.weight_viapoint, optim.weight_viapoint); + nh.param("weight_prefer_rotdir", optim.weight_prefer_rotdir, optim.weight_prefer_rotdir); + nh.param("weight_adapt_factor", optim.weight_adapt_factor, optim.weight_adapt_factor); + + // Homotopy Class Planner + nh.param("enable_homotopy_class_planning", hcp.enable_homotopy_class_planning, hcp.enable_homotopy_class_planning); + nh.param("enable_multithreading", hcp.enable_multithreading, hcp.enable_multithreading); + nh.param("simple_exploration", hcp.simple_exploration, hcp.simple_exploration); + nh.param("max_number_classes", hcp.max_number_classes, hcp.max_number_classes); + nh.param("selection_obst_cost_scale", hcp.selection_obst_cost_scale, hcp.selection_obst_cost_scale); + nh.param("selection_prefer_initial_plan", hcp.selection_prefer_initial_plan, hcp.selection_prefer_initial_plan); + nh.param("selection_viapoint_cost_scale", hcp.selection_viapoint_cost_scale, hcp.selection_viapoint_cost_scale); + nh.param("selection_cost_hysteresis", hcp.selection_cost_hysteresis, hcp.selection_cost_hysteresis); + nh.param("selection_alternative_time_cost", hcp.selection_alternative_time_cost, hcp.selection_alternative_time_cost); + nh.param("switching_blocking_period", hcp.switching_blocking_period, hcp.switching_blocking_period); + nh.param("roadmap_graph_samples", hcp.roadmap_graph_no_samples, hcp.roadmap_graph_no_samples); + nh.param("roadmap_graph_area_width", hcp.roadmap_graph_area_width, hcp.roadmap_graph_area_width); + nh.param("roadmap_graph_area_length_scale", hcp.roadmap_graph_area_length_scale, hcp.roadmap_graph_area_length_scale); + nh.param("h_signature_prescaler", hcp.h_signature_prescaler, hcp.h_signature_prescaler); + nh.param("h_signature_threshold", hcp.h_signature_threshold, hcp.h_signature_threshold); + nh.param("obstacle_keypoint_offset", hcp.obstacle_keypoint_offset, hcp.obstacle_keypoint_offset); + nh.param("obstacle_heading_threshold", hcp.obstacle_heading_threshold, hcp.obstacle_heading_threshold); + nh.param("viapoints_all_candidates", hcp.viapoints_all_candidates, hcp.viapoints_all_candidates); + nh.param("visualize_hc_graph", hcp.visualize_hc_graph, hcp.visualize_hc_graph); + nh.param("visualize_with_time_as_z_axis_scale", hcp.visualize_with_time_as_z_axis_scale, hcp.visualize_with_time_as_z_axis_scale); + + // Recovery + + nh.param("shrink_horizon_backup", recovery.shrink_horizon_backup, recovery.shrink_horizon_backup); + nh.param("shrink_horizon_min_duration", recovery.shrink_horizon_min_duration, recovery.shrink_horizon_min_duration); + nh.param("oscillation_recovery", recovery.oscillation_recovery, recovery.oscillation_recovery); + nh.param("oscillation_v_eps", recovery.oscillation_v_eps, recovery.oscillation_v_eps); + nh.param("oscillation_omega_eps", recovery.oscillation_omega_eps, recovery.oscillation_omega_eps); + nh.param("oscillation_recovery_min_duration", recovery.oscillation_recovery_min_duration, recovery.oscillation_recovery_min_duration); + nh.param("oscillation_filter_duration", recovery.oscillation_filter_duration, recovery.oscillation_filter_duration); + + checkParameters(); + checkDeprecated(nh); + } + + void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig &cfg) + { + boost::mutex::scoped_lock l(config_mutex_); + + // Trajectory + trajectory.teb_autosize = cfg.teb_autosize; + trajectory.dt_ref = cfg.dt_ref; + trajectory.dt_hysteresis = cfg.dt_hysteresis; + trajectory.global_plan_overwrite_orientation = cfg.global_plan_overwrite_orientation; + trajectory.allow_init_with_backwards_motion = cfg.allow_init_with_backwards_motion; + trajectory.global_plan_viapoint_sep = cfg.global_plan_viapoint_sep; + trajectory.via_points_ordered = cfg.via_points_ordered; + trajectory.max_global_plan_lookahead_dist = cfg.max_global_plan_lookahead_dist; + trajectory.exact_arc_length = cfg.exact_arc_length; + trajectory.force_reinit_new_goal_dist = cfg.force_reinit_new_goal_dist; + trajectory.feasibility_check_no_poses = cfg.feasibility_check_no_poses; + trajectory.publish_feedback = cfg.publish_feedback; + + // Robot + robot.max_vel_x = cfg.max_vel_x; + robot.max_vel_x_backwards = cfg.max_vel_x_backwards; + robot.max_vel_y = cfg.max_vel_y; + robot.max_vel_theta = cfg.max_vel_theta; + robot.acc_lim_x = cfg.acc_lim_x; + robot.acc_lim_y = cfg.acc_lim_y; + robot.acc_lim_theta = cfg.acc_lim_theta; + robot.min_turning_radius = cfg.min_turning_radius; + robot.wheelbase = cfg.wheelbase; + robot.cmd_angle_instead_rotvel = cfg.cmd_angle_instead_rotvel; + + // GoalTolerance + goal_tolerance.xy_goal_tolerance = cfg.xy_goal_tolerance; + goal_tolerance.yaw_goal_tolerance = cfg.yaw_goal_tolerance; + goal_tolerance.free_goal_vel = cfg.free_goal_vel; + + // Obstacles + obstacles.min_obstacle_dist = cfg.min_obstacle_dist; + obstacles.inflation_dist = cfg.inflation_dist; + obstacles.dynamic_obstacle_inflation_dist = cfg.dynamic_obstacle_inflation_dist; + obstacles.include_dynamic_obstacles = cfg.include_dynamic_obstacles; + obstacles.include_costmap_obstacles = cfg.include_costmap_obstacles; + obstacles.legacy_obstacle_association = cfg.legacy_obstacle_association; + obstacles.obstacle_association_force_inclusion_factor = cfg.obstacle_association_force_inclusion_factor; + obstacles.obstacle_association_cutoff_factor = cfg.obstacle_association_cutoff_factor; + obstacles.costmap_obstacles_behind_robot_dist = cfg.costmap_obstacles_behind_robot_dist; + obstacles.obstacle_poses_affected = cfg.obstacle_poses_affected; + + // Optimization + optim.no_inner_iterations = cfg.no_inner_iterations; + optim.no_outer_iterations = cfg.no_outer_iterations; + optim.optimization_activate = cfg.optimization_activate; + optim.optimization_verbose = cfg.optimization_verbose; + optim.penalty_epsilon = cfg.penalty_epsilon; + optim.weight_max_vel_x = cfg.weight_max_vel_x; + optim.weight_max_vel_y = cfg.weight_max_vel_y; + optim.weight_max_vel_theta = cfg.weight_max_vel_theta; + optim.weight_acc_lim_x = cfg.weight_acc_lim_x; + optim.weight_acc_lim_y = cfg.weight_acc_lim_y; + optim.weight_acc_lim_theta = cfg.weight_acc_lim_theta; + optim.weight_kinematics_nh = cfg.weight_kinematics_nh; + optim.weight_kinematics_forward_drive = cfg.weight_kinematics_forward_drive; + optim.weight_kinematics_turning_radius = cfg.weight_kinematics_turning_radius; + optim.weight_optimaltime = cfg.weight_optimaltime; + optim.weight_obstacle = cfg.weight_obstacle; + optim.weight_inflation = cfg.weight_inflation; + optim.weight_dynamic_obstacle = cfg.weight_dynamic_obstacle; + optim.weight_dynamic_obstacle_inflation = cfg.weight_dynamic_obstacle_inflation; + optim.weight_viapoint = cfg.weight_viapoint; + optim.weight_adapt_factor = cfg.weight_adapt_factor; + + // Homotopy Class Planner + hcp.enable_multithreading = cfg.enable_multithreading; + hcp.max_number_classes = cfg.max_number_classes; + hcp.selection_cost_hysteresis = cfg.selection_cost_hysteresis; + hcp.selection_prefer_initial_plan = cfg.selection_prefer_initial_plan; + hcp.selection_obst_cost_scale = cfg.selection_obst_cost_scale; + hcp.selection_viapoint_cost_scale = cfg.selection_viapoint_cost_scale; + hcp.selection_alternative_time_cost = cfg.selection_alternative_time_cost; + hcp.switching_blocking_period = cfg.switching_blocking_period; + + hcp.obstacle_heading_threshold = cfg.obstacle_heading_threshold; + hcp.roadmap_graph_no_samples = cfg.roadmap_graph_no_samples; + hcp.roadmap_graph_area_width = cfg.roadmap_graph_area_width; + hcp.roadmap_graph_area_length_scale = cfg.roadmap_graph_area_length_scale; + hcp.h_signature_prescaler = cfg.h_signature_prescaler; + hcp.h_signature_threshold = cfg.h_signature_threshold; + hcp.viapoints_all_candidates = cfg.viapoints_all_candidates; + hcp.visualize_hc_graph = cfg.visualize_hc_graph; + hcp.visualize_with_time_as_z_axis_scale = cfg.visualize_with_time_as_z_axis_scale; + + // Recovery + + recovery.shrink_horizon_backup = cfg.shrink_horizon_backup; + recovery.oscillation_recovery = cfg.oscillation_recovery; + + checkParameters(); + } + + void TebConfig::checkParameters() const + { + // positive backward velocity? + if (robot.max_vel_x_backwards <= 0) + ROS_WARN("TebLocalPlannerROS() Param Warning: Do not choose max_vel_x_backwards to be <=0. Disable backwards driving by increasing the " + "optimization weight for penalyzing backwards driving."); + + // bounds smaller than penalty epsilon + if (robot.max_vel_x <= optim.penalty_epsilon) + ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... " + "Change at least one of them!"); + + if (robot.max_vel_x_backwards <= optim.penalty_epsilon) + ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_x_backwards <= penalty_epsilon. The resulting bound is negative. Undefined " + "behavior... Change at least one of them!"); + + if (robot.max_vel_theta <= optim.penalty_epsilon) + ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... " + "Change at least one of them!"); + + if (robot.acc_lim_x <= optim.penalty_epsilon) + ROS_WARN("TebLocalPlannerROS() Param Warning: acc_lim_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... " + "Change at least one of them!"); + + if (robot.acc_lim_theta <= optim.penalty_epsilon) + ROS_WARN("TebLocalPlannerROS() Param Warning: acc_lim_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... " + "Change at least one of them!"); + + // dt_ref and dt_hyst + if (trajectory.dt_ref <= trajectory.dt_hysteresis) + ROS_WARN("TebLocalPlannerROS() Param Warning: dt_ref <= dt_hysteresis. The hysteresis is not allowed to be greater or equal!. Undefined " + "behavior... Change at least one of them!"); + + // min number of samples + if (trajectory.min_samples < 3) + ROS_WARN("TebLocalPlannerROS() Param Warning: parameter min_samples is smaller than 3! Sorry, I haven't enough degrees of freedom to " + "plan a trajectory for you. Please increase ..."); + + // costmap obstacle behind robot + if (obstacles.costmap_obstacles_behind_robot_dist < 0) + ROS_WARN("TebLocalPlannerROS() Param Warning: parameter 'costmap_obstacles_behind_robot_dist' should be positive or zero."); + + // hcp: obstacle heading threshold + if (hcp.obstacle_keypoint_offset >= 1 || hcp.obstacle_keypoint_offset <= 0) + ROS_WARN("TebLocalPlannerROS() Param Warning: parameter obstacle_heading_threshold must be in the interval ]0,1[. 0=0deg opening angle, " + "1=90deg opening angle."); + + // carlike + if (robot.cmd_angle_instead_rotvel && robot.wheelbase == 0) + ROS_WARN( + "TebLocalPlannerROS() Param Warning: parameter cmd_angle_instead_rotvel is non-zero but wheelbase is set to zero: undesired behavior."); + + if (robot.cmd_angle_instead_rotvel && robot.min_turning_radius == 0) + ROS_WARN("TebLocalPlannerROS() Param Warning: parameter cmd_angle_instead_rotvel is non-zero but min_turning_radius is set to zero: " + "undesired behavior. You are mixing a carlike and a diffdrive robot"); + + // positive weight_adapt_factor + if (optim.weight_adapt_factor < 1.0) + ROS_WARN("TebLocalPlannerROS() Param Warning: parameter weight_adapt_factor shoud be >= 1.0"); + + if (recovery.oscillation_filter_duration < 0) + ROS_WARN("TebLocalPlannerROS() Param Warning: parameter oscillation_filter_duration must be >= 0"); + } + + void TebConfig::checkDeprecated(const ros::NodeHandle &nh) const + { + if (nh.hasParam("line_obstacle_poses_affected") || nh.hasParam("polygon_obstacle_poses_affected")) + ROS_WARN("TebLocalPlannerROS() Param Warning: 'line_obstacle_poses_affected' and 'polygon_obstacle_poses_affected' are deprecated. They " + "share now the common parameter 'obstacle_poses_affected'."); + + if (nh.hasParam("weight_point_obstacle") || nh.hasParam("weight_line_obstacle") || nh.hasParam("weight_poly_obstacle")) + ROS_WARN("TebLocalPlannerROS() Param Warning: 'weight_point_obstacle', 'weight_line_obstacle' and 'weight_poly_obstacle' are deprecated. " + "They are replaced by the single param 'weight_obstacle'."); + + if (nh.hasParam("costmap_obstacles_front_only")) + ROS_WARN("TebLocalPlannerROS() Param Warning: 'costmap_obstacles_front_only' is deprecated. It is replaced by " + "'costmap_obstacles_behind_robot_dist' to define the actual area taken into account."); + + if (nh.hasParam("costmap_emergency_stop_dist")) + ROS_WARN("TebLocalPlannerROS() Param Warning: 'costmap_emergency_stop_dist' is deprecated. You can safely remove it from your parameter " + "config."); + + if (nh.hasParam("alternative_time_cost")) + ROS_WARN("TebLocalPlannerROS() Param Warning: 'alternative_time_cost' is deprecated. It has been replaced by " + "'selection_alternative_time_cost'."); + + if (nh.hasParam("global_plan_via_point_sep")) + ROS_WARN("TebLocalPlannerROS() Param Warning: 'global_plan_via_point_sep' is deprecated. It has been replaced by " + "'global_plan_viapoint_sep' due to consistency reasons."); + } + +} // namespace teb_local_planner