From db13aa3820d9c08acaf65108fd43cccee92b16f1 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 03:09:26 -0400 Subject: [PATCH 01/46] Add files via upload --- TebLocalPlannerReconfigure.cfg | 440 ++++++++++ base_teb_edges.h | 278 +++++++ edge_acceleration.h | 734 +++++++++++++++++ edge_dynamic_obstacle.h | 153 ++++ edge_kinematics.h | 230 ++++++ edge_obstacle.h | 295 +++++++ edge_prefer_rotdir.h | 115 +++ edge_shortest_path.h | 89 +++ edge_time_optimal.h | 116 +++ edge_velocity.h | 489 ++++++++++++ edge_velocity_obstacle_ratio.h | 166 ++++ edge_via_point.h | 120 +++ misc.h | 152 ++++ optimal_planner.cpp | 1373 ++++++++++++++++++++++++++++++++ optimal_planner.h | 718 +++++++++++++++++ penalties.h | 193 +++++ teb_config.cpp | 378 +++++++++ teb_config.h | 429 ++++++++++ vertex_pose.h | 229 ++++++ vertex_timediff.h | 145 ++++ visualize_vel_and_steering.py | 90 +++ 21 files changed, 6932 insertions(+) create mode 100644 TebLocalPlannerReconfigure.cfg create mode 100644 base_teb_edges.h create mode 100644 edge_acceleration.h create mode 100644 edge_dynamic_obstacle.h create mode 100644 edge_kinematics.h create mode 100644 edge_obstacle.h create mode 100644 edge_prefer_rotdir.h create mode 100644 edge_shortest_path.h create mode 100644 edge_time_optimal.h create mode 100644 edge_velocity.h create mode 100644 edge_velocity_obstacle_ratio.h create mode 100644 edge_via_point.h create mode 100644 misc.h create mode 100644 optimal_planner.cpp create mode 100644 optimal_planner.h create mode 100644 penalties.h create mode 100644 teb_config.cpp create mode 100644 teb_config.h create mode 100644 vertex_pose.h create mode 100644 vertex_timediff.h create mode 100644 visualize_vel_and_steering.py diff --git a/TebLocalPlannerReconfigure.cfg b/TebLocalPlannerReconfigure.cfg new file mode 100644 index 00000000..c66a4f1a --- /dev/null +++ b/TebLocalPlannerReconfigure.cfg @@ -0,0 +1,440 @@ +#!/usr/bin/env python + +from dynamic_reconfigure.parameter_generator_catkin import * +#from local_planner_limits import add_generic_localplanner_params + +gen = ParameterGenerator() + +# This unusual line allows to reuse existing parameter definitions +# that concern all localplanners +#add_generic_localplanner_params(gen) + +# For integers and doubles: +# Name Type Reconfiguration level +# Description +# Default Min Max + + +grp_trajectory = gen.add_group("Trajectory", type="tab") + +# Trajectory +grp_trajectory.add("teb_autosize", bool_t, 0, + "Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended)", + True) + +grp_trajectory.add("dt_ref", double_t, 0, + "Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)", + 0.3, 0.01, 1) + +grp_trajectory.add("dt_hysteresis", double_t, 0, + "Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref", + 0.1, 0.002, 0.5) + +grp_trajectory.add("global_plan_overwrite_orientation", bool_t, 0, + "Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically", + True) + +grp_trajectory.add("allow_init_with_backwards_motion", bool_t, 0, + "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)", + False) + +grp_trajectory.add("max_global_plan_lookahead_dist", double_t, 0, + "Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]", + 3.0, 0, 50.0) + +grp_trajectory.add("force_reinit_new_goal_dist", double_t, 0, + "Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)", + 1.0, 0.0, 10.0) + +grp_trajectory.add("force_reinit_new_goal_angular", double_t, 0, + "Force the planner to reinitialize the trajectory if a previous goal is updated with a rotational difference of more than the specified value in radians (skip hot-starting)", + 0.78, 0.0, 4.0) + +grp_trajectory.add("feasibility_check_no_poses", int_t, 0, + "Specify up to which pose on the predicted plan the feasibility should be checked each sampling interval", + 5, 0, 50) + +grp_trajectory.add("exact_arc_length", bool_t, 0, + "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.", + False) + +grp_trajectory.add("publish_feedback", bool_t, 0, + "Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)", + False) + +grp_trajectory.add("visualize_with_time_as_z_axis_scale", double_t, 0, + "If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.", + 0, 0, 1) + +# ViaPoints +grp_viapoints = gen.add_group("ViaPoints", type="tab") + +grp_viapoints.add("global_plan_viapoint_sep", double_t, 0, + "Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]", + -0.1, -0.1, 5.0) + +grp_viapoints.add("via_points_ordered", bool_t, 0, + "If true, the planner adheres to the order of via-points in the storage container", + False) + +# Robot +grp_robot = gen.add_group("Robot", type="tab") + +grp_robot.add("max_vel_x", double_t, 0, + "Maximum translational velocity of the robot", + 0.4, 0.01, 100) + +grp_robot.add("max_vel_x_backwards", double_t, 0, + "Maximum translational velocity of the robot for driving backwards", + 0.2, 0.01, 100) + +grp_robot.add("max_vel_theta", double_t, 0, + "Maximum angular velocity of the robot", + 0.3, 0.01, 100) + +grp_robot.add("acc_lim_x", double_t, 0, + "Maximum translational acceleration of the robot", + 0.5, 0.01, 100) + +grp_robot.add("acc_lim_theta", double_t, 0, + "Maximum angular acceleration of the robot", + 0.5, 0.01, 100) + +grp_robot.add("is_footprint_dynamic", bool_t, 0, + "If true, updated the footprint before checking trajectory feasibility", + False) + +grp_robot.add("use_proportional_saturation", bool_t, 0, + "If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually", + False) +grp_robot.add("transform_tolerance", double_t, 0, + "Tolerance when querying the TF Tree for a transformation (seconds)", + 0.5, 0.001, 20) + +# Robot/Carlike + +grp_robot_carlike = grp_robot.add_group("Carlike", type="hide") + +grp_robot_carlike.add("min_turning_radius", double_t, 0, + "Minimum turning radius of a carlike robot (diff-drive robot: zero)", + 0.0, 0.0, 50.0) + +grp_robot_carlike.add("max_steering_rate", double_t, 0, + "EXPERIMENTAL: Maximum steering rate of a carlike robot (THIS SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT) [deactivate: zero]", + 0.0, 0.0, 10.0) + +grp_robot_carlike.add("wheelbase", double_t, 0, + "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!", + 1.0, -10.0, 10.0) + +grp_robot_carlike.add("cmd_angle_instead_rotvel", bool_t, 0, + "Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')", + False) + +# Robot/Omni + +grp_robot_omni = grp_robot.add_group("Omnidirectional", type="hide") + +grp_robot_omni.add("max_vel_y", double_t, 0, + "Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)", + 0.0, 0.0, 100) + +grp_robot_omni.add("acc_lim_y", double_t, 0, + "Maximum strafing acceleration of the robot", + 0.5, 0.01, 100) + +# GoalTolerance +grp_goal = gen.add_group("GoalTolerance", type="tab") + +grp_goal.add("xy_goal_tolerance", double_t, 0, + "Allowed final euclidean distance to the goal position", + 0.2, 0.001, 10) + +grp_goal.add("yaw_goal_tolerance", double_t, 0, + "Allowed final orientation error to the goal orientation", + 0.1, 0.001, 3.2) + +grp_goal.add("free_goal_vel", bool_t, 0, + "Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed)", + False) + +grp_goal.add("trans_stopped_vel", double_t, 0, + "Below what maximum velocity we consider the robot to be stopped in translation", + 0.1, 0.0) + +grp_goal.add("theta_stopped_vel", double_t, 0, + "Below what maximum rotation velocity we consider the robot to be stopped in rotation", + 0.1, 0.0) + +# Obstacles +grp_obstacles = gen.add_group("Obstacles", type="tab") + +grp_obstacles.add("min_obstacle_dist", double_t, 0, + "Minimum desired separation from obstacles", + 0.5, 0, 10) + +grp_obstacles.add("inflation_dist", double_t, 0, + "Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)", + 0.6, 0, 15) + +grp_obstacles.add("dynamic_obstacle_inflation_dist", double_t, 0, + "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)", + 0.6, 0, 15) + +grp_obstacles.add("include_dynamic_obstacles", bool_t, 0, + "Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static.", + False) + +grp_obstacles.add("include_costmap_obstacles", bool_t, 0, + "Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)", + True) + +grp_obstacles.add("legacy_obstacle_association", bool_t, 0, + "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).", + False) + +grp_obstacles.add("obstacle_association_force_inclusion_factor", double_t, 0, + "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.", + 1.5, 0.0, 100.0) + +grp_obstacles.add("obstacle_association_cutoff_factor", double_t, 0, + "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.", + 5.0, 1.0, 100.0) + +grp_obstacles.add("costmap_obstacles_behind_robot_dist", double_t, 0, + "Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)", + 1.5, 0.0, 20.0) + +grp_obstacles.add("obstacle_poses_affected", int_t, 0, + "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", + 30, 0, 200) + +# Obstacle - Velocity ratio parameters +grp_obstacles_velocity_limit = grp_obstacles.add_group("Reduce velocity near obstacles") + +grp_obstacles_velocity_limit.add("obstacle_proximity_ratio_max_vel", double_t, 0, + "Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to static obstacles", + 1, 0, 1) + +grp_obstacles_velocity_limit.add("obstacle_proximity_lower_bound", double_t, 0, + "Distance to a static obstacle for which the velocity should be lower", + 0, 0, 10) + +grp_obstacles_velocity_limit.add("obstacle_proximity_upper_bound", double_t, 0, + "Distance to a static obstacle for which the velocity should be higher", + 0.5, 0, 10) + +# Optimization +grp_optimization = gen.add_group("Optimization", type="tab") + +grp_optimization.add("no_inner_iterations", int_t, 0, + "Number of solver iterations called in each outerloop iteration", + 5, 1, 100) + +grp_optimization.add("no_outer_iterations", int_t, 0, + "Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations", + 4, 1, 100) + +grp_optimization.add("optimization_activate", bool_t, 0, + "Activate the optimization", + True) + +grp_optimization.add("optimization_verbose", bool_t, 0, + "Print verbose information", + False) + +grp_optimization.add("penalty_epsilon", double_t, 0, + "Add a small safty margin to penalty functions for hard-constraint approximations", + 0.1, 0, 1.0) + +grp_optimization.add("weight_max_vel_x", double_t, 0, + "Optimization weight for satisfying the maximum allowed translational velocity", + 2, 0, 1000) + +grp_optimization.add("weight_max_vel_y", double_t, 0, + "Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)", + 2, 0, 1000) + +grp_optimization.add("weight_max_vel_theta", double_t, 0, + "Optimization weight for satisfying the maximum allowed angular velocity", + 1, 0, 1000) + +grp_optimization.add("weight_acc_lim_x", double_t, 0, + "Optimization weight for satisfying the maximum allowed translational acceleration", + 1, 0, 1000) + +grp_optimization.add("weight_acc_lim_y", double_t, 0, + "Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)", + 1, 0, 1000) + +grp_optimization.add("weight_acc_lim_theta", double_t, 0, + "Optimization weight for satisfying the maximum allowed angular acceleration", + 1, 0, 1000) + +grp_optimization.add("weight_kinematics_nh", double_t, 0, + "Optimization weight for satisfying the non-holonomic kinematics", + 1000 , 0, 10000) + +grp_optimization.add("weight_kinematics_forward_drive", double_t, 0, + "Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)", + 1, 0, 1000) + +grp_optimization.add("weight_kinematics_turning_radius", double_t, 0, + "Optimization weight for enforcing a minimum turning radius (carlike robots)", + 1, 0, 1000) + +grp_optimization.add("weight_max_steering_rate", double_t, 0, + "EXPERIMENTAL: Optimization weight for enforcing a minimum steering rate of a carlike robot (TRY TO KEEP THE WEIGHT LOW OR DEACTIVATE, SINCE IT SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT)", + 0.5, 0, 100) + +grp_optimization.add("weight_optimaltime", double_t, 0, + "Optimization weight for contracting the trajectory w.r.t. transition time", + 1, 0, 1000) + +grp_optimization.add("weight_shortest_path", double_t, 0, + "Optimization weight for contracting the trajectory w.r.t. path length", + 0, 0, 100) + +grp_optimization.add("weight_obstacle", double_t, 0, + "Optimization weight for satisfying a minimum seperation from obstacles", + 50, 0, 1000) + +grp_optimization.add("weight_inflation", double_t, 0, + "Optimization weight for the inflation penalty (should be small)", + 0.1, 0, 10) + +grp_optimization.add("weight_dynamic_obstacle", double_t, 0, + "Optimization weight for satisfying a minimum seperation from dynamic obstacles", + 50, 0, 1000) + +grp_optimization.add("weight_dynamic_obstacle_inflation", double_t, 0, + "Optimization weight for the inflation penalty of dynamic obstacles (should be small)", + 0.1, 0, 10) + +grp_optimization.add("weight_velocity_obstacle_ratio", double_t, 0, + "Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle", + 0, 0, 1000) + +grp_optimization.add("weight_viapoint", double_t, 0, + "Optimization weight for minimizing the distance to via-points", + 1, 0, 1000) + +grp_optimization.add("weight_adapt_factor", double_t, 0, + "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.", + 2, 1, 100) + +grp_optimization.add("obstacle_cost_exponent", double_t, 0, + "Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)", + 1, 0.01, 100) + + + +# Homotopy Class Planner +grp_hcp = gen.add_group("HCPlanning", type="tab") + +grp_hcp.add("enable_multithreading", bool_t, 0, + "Activate multiple threading for planning multiple trajectories in parallel", + True) + +grp_hcp.add("max_number_classes", int_t, 0, + "Specify the maximum number of allowed alternative homotopy classes (limits computational effort)", + 5, 1, 100) + +grp_hcp.add("max_number_plans_in_current_class", int_t, 0, + "Max number of trajectories to try that are in the same homotopy class as the current best trajectory (setting this to 2 or more helps avoid local minima). Must be <= max_number_classes", + 1, 1, 10) + +grp_hcp.add("selection_cost_hysteresis", double_t, 0, + "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)", + 1.0, 0, 2) + + +grp_hcp.add("selection_prefer_initial_plan", double_t, 0, + "Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.)", + 0.95, 0, 1) + +grp_hcp.add("selection_obst_cost_scale", double_t, 0, + "Extra scaling of obstacle cost terms just for selecting the 'best' candidate (new_obst_cost: obst_cost*factor)", + 2.0, 0, 1000) + +grp_hcp.add("selection_viapoint_cost_scale", double_t, 0, + "Extra scaling of via-point cost terms just for selecting the 'best' candidate. (new_viapt_cost: viapt_cost*factor)", + 1.0, 0, 100) + +grp_hcp.add("selection_alternative_time_cost", bool_t, 0, + "If true, time cost is replaced by the total transition time.", + False) + +grp_hcp.add("selection_dropping_probability", double_t, 0, + "At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies.", + 0.0, 0.0, 1.0) + +grp_hcp.add("switching_blocking_period", double_t, 0, + "Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed", + 0.0, 0.0, 60) + +grp_hcp.add("roadmap_graph_no_samples", int_t, 0, + "Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off", + 15, 1, 100) + +grp_hcp.add("roadmap_graph_area_width", double_t, 0, + "Specify the width of the area in which sampled will be generated between start and goal [m] (the height equals the start-goal distance)", + 5, 0.1, 20) + +grp_hcp.add("roadmap_graph_area_length_scale", double_t, 0, + "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!)", + 1.0, 0.5, 2) + +grp_hcp.add("h_signature_prescaler", double_t, 0, + "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.2 + +#include +#include +#include + +namespace teb_local_planner +{ + + +/** + * @class BaseTebUnaryEdge + * @brief Base edge connecting a single vertex in the TEB optimization problem + * + * This edge defines a base edge type for the TEB optimization problem. + * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config). + * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls. + * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class. + * @see BaseTebMultiEdge, BaseTebBinaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge + */ +template +class BaseTebUnaryEdge : public g2o::BaseUnaryEdge +{ +public: + + using typename g2o::BaseUnaryEdge::ErrorVector; + using g2o::BaseUnaryEdge::computeError; + + /** + * @brief Compute and return error / cost value. + * + * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost. + * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T + */ + ErrorVector& getError() + { + computeError(); + return _error; + } + + /** + * @brief Read values from input stream + */ + virtual bool read(std::istream& is) + { + // TODO generic read + return true; + } + + /** + * @brief Write values to an output stream + */ + virtual bool write(std::ostream& os) const + { + // TODO generic write + return os.good(); + } + + /** + * @brief Assign the TebConfig class for parameters. + * @param cfg TebConfig class + */ + void setTebConfig(const TebConfig& cfg) + { + cfg_ = &cfg; + } + +protected: + + using g2o::BaseUnaryEdge::_error; + using g2o::BaseUnaryEdge::_vertices; + + const TebConfig* cfg_; //!< Store TebConfig class for parameters + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/** + * @class BaseTebBinaryEdge + * @brief Base edge connecting two vertices in the TEB optimization problem + * + * This edge defines a base edge type for the TEB optimization problem. + * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config). + * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls. + * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class. + * @see BaseTebMultiEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge + */ +template +class BaseTebBinaryEdge : public g2o::BaseBinaryEdge +{ +public: + + using typename g2o::BaseBinaryEdge::ErrorVector; + using g2o::BaseBinaryEdge::computeError; + + /** + * @brief Compute and return error / cost value. + * + * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost. + * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T + */ + ErrorVector& getError() + { + computeError(); + return _error; + } + + /** + * @brief Read values from input stream + */ + virtual bool read(std::istream& is) + { + // TODO generic read + return true; + } + + /** + * @brief Write values to an output stream + */ + virtual bool write(std::ostream& os) const + { + // TODO generic write + return os.good(); + } + + /** + * @brief Assign the TebConfig class for parameters. + * @param cfg TebConfig class + */ + void setTebConfig(const TebConfig& cfg) + { + cfg_ = &cfg; + } + +protected: + + using g2o::BaseBinaryEdge::_error; + using g2o::BaseBinaryEdge::_vertices; + + const TebConfig* cfg_; //!< Store TebConfig class for parameters + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + + +/** + * @class BaseTebMultiEdge + * @brief Base edge connecting multiple vertices in the TEB optimization problem + * + * This edge defines a base edge type for the TEB optimization problem. + * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config). + * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls. + * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class. + * @see BaseTebBinaryEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge + */ +template +class BaseTebMultiEdge : public g2o::BaseMultiEdge +{ +public: + + using typename g2o::BaseMultiEdge::ErrorVector; + using g2o::BaseMultiEdge::computeError; + + // Overwrites resize() from the parent class + virtual void resize(size_t size) + { + g2o::BaseMultiEdge::resize(size); + + for(std::size_t i=0; i<_vertices.size(); ++i) + _vertices[i] = NULL; + } + + /** + * @brief Compute and return error / cost value. + * + * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost. + * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T + */ + ErrorVector& getError() + { + computeError(); + return _error; + } + + /** + * @brief Read values from input stream + */ + virtual bool read(std::istream& is) + { + // TODO generic read + return true; + } + + /** + * @brief Write values to an output stream + */ + virtual bool write(std::ostream& os) const + { + // TODO generic write + return os.good(); + } + + /** + * @brief Assign the TebConfig class for parameters. + * @param cfg TebConfig class + */ + void setTebConfig(const TebConfig& cfg) + { + cfg_ = &cfg; + } + +protected: + + using g2o::BaseMultiEdge::_error; + using g2o::BaseMultiEdge::_vertices; + + const TebConfig* cfg_; //!< Store TebConfig class for parameters + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + + + + + + +} // end namespace + +#endif diff --git a/edge_acceleration.h b/edge_acceleration.h new file mode 100644 index 00000000..8eccf6de --- /dev/null +++ b/edge_acceleration.h @@ -0,0 +1,734 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef EDGE_ACCELERATION_H_ +#define EDGE_ACCELERATION_H_ + +#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 + { + 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); + + + 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 + /* + * @brief Jacobi matrix of the cost function specified in computeError(). + */ + void linearizeOplus() + { + ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()"); + const VertexPointXY* conf1 = static_cast(_vertices[0]); + const VertexPointXY* conf2 = static_cast(_vertices[1]); + const VertexPointXY* conf3 = static_cast(_vertices[2]); + const VertexTimeDiff* deltaT1 = static_cast(_vertices[3]); + const VertexTimeDiff* deltaT2 = static_cast(_vertices[4]); + const VertexOrientation* angle1 = static_cast(_vertices[5]); + const VertexOrientation* angle2 = static_cast(_vertices[6]); + const VertexOrientation* angle3 = static_cast(_vertices[7]); + + Eigen::Vector2d deltaS1 = conf2->estimate() - conf1->estimate(); + Eigen::Vector2d deltaS2 = conf3->estimate() - conf2->estimate(); + double dist1 = deltaS1.norm(); + double dist2 = deltaS2.norm(); + + double sum_time = deltaT1->estimate() + deltaT2->estimate(); + double sum_time_inv = 1 / sum_time; + double dt1_inv = 1/deltaT1->estimate(); + double dt2_inv = 1/deltaT2->estimate(); + double aux0 = 2/sum_time_inv; + double aux1 = dist1 * deltaT1->estimate(); + double aux2 = dist2 * deltaT2->estimate(); + + double vel1 = dist1 * dt1_inv; + double vel2 = dist2 * dt2_inv; + double omega1 = g2o::normalize_theta( angle2->estimate() - angle1->estimate() ) * dt1_inv; + double omega2 = g2o::normalize_theta( angle3->estimate() - angle2->estimate() ) * dt2_inv; + double acc = (vel2 - vel1) * aux0; + double omegadot = (omega2 - omega1) * aux0; + double aux3 = -acc/2; + double aux4 = -omegadot/2; + + double dev_border_acc = penaltyBoundToIntervalDerivative(acc, tebConfig.robot_acceleration_max_trans,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order); + double dev_border_omegadot = penaltyBoundToIntervalDerivative(omegadot, tebConfig.robot_acceleration_max_rot,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order); + + _jacobianOplus[0].resize(2,2); // conf1 + _jacobianOplus[1].resize(2,2); // conf2 + _jacobianOplus[2].resize(2,2); // conf3 + _jacobianOplus[3].resize(2,1); // deltaT1 + _jacobianOplus[4].resize(2,1); // deltaT2 + _jacobianOplus[5].resize(2,1); // angle1 + _jacobianOplus[6].resize(2,1); // angle2 + _jacobianOplus[7].resize(2,1); // angle3 + + if (aux1==0) aux1=1e-20; + if (aux2==0) aux2=1e-20; + + if (dev_border_acc!=0) + { + // TODO: double aux = aux0 * dev_border_acc; + // double aux123 = aux / aux1; + _jacobianOplus[0](0,0) = aux0 * deltaS1[0] / aux1 * dev_border_acc; // acc x1 + _jacobianOplus[0](0,1) = aux0 * deltaS1[1] / aux1 * dev_border_acc; // acc y1 + _jacobianOplus[1](0,0) = -aux0 * ( deltaS1[0] / aux1 + deltaS2[0] / aux2 ) * dev_border_acc; // acc x2 + _jacobianOplus[1](0,1) = -aux0 * ( deltaS1[1] / aux1 + deltaS2[1] / aux2 ) * dev_border_acc; // acc y2 + _jacobianOplus[2](0,0) = aux0 * deltaS2[0] / aux2 * dev_border_acc; // acc x3 + _jacobianOplus[2](0,1) = aux0 * deltaS2[1] / aux2 * dev_border_acc; // acc y3 + _jacobianOplus[2](0,0) = 0; + _jacobianOplus[2](0,1) = 0; + _jacobianOplus[3](0,0) = aux0 * (aux3 + vel1 * dt1_inv) * dev_border_acc; // acc deltaT1 + _jacobianOplus[4](0,0) = aux0 * (aux3 - vel2 * dt2_inv) * dev_border_acc; // acc deltaT2 + } + else + { + _jacobianOplus[0](0,0) = 0; // acc x1 + _jacobianOplus[0](0,1) = 0; // acc y1 + _jacobianOplus[1](0,0) = 0; // acc x2 + _jacobianOplus[1](0,1) = 0; // acc y2 + _jacobianOplus[2](0,0) = 0; // acc x3 + _jacobianOplus[2](0,1) = 0; // acc y3 + _jacobianOplus[3](0,0) = 0; // acc deltaT1 + _jacobianOplus[4](0,0) = 0; // acc deltaT2 + } + + if (dev_border_omegadot!=0) + { + _jacobianOplus[3](1,0) = aux0 * ( aux4 + omega1 * dt1_inv ) * dev_border_omegadot; // omegadot deltaT1 + _jacobianOplus[4](1,0) = aux0 * ( aux4 - omega2 * dt2_inv ) * dev_border_omegadot; // omegadot deltaT2 + _jacobianOplus[5](1,0) = aux0 * dt1_inv * dev_border_omegadot; // omegadot angle1 + _jacobianOplus[6](1,0) = -aux0 * ( dt1_inv + dt2_inv ) * dev_border_omegadot; // omegadot angle2 + _jacobianOplus[7](1,0) = aux0 * dt2_inv * dev_border_omegadot; // omegadot angle3 + } + else + { + _jacobianOplus[3](1,0) = 0; // omegadot deltaT1 + _jacobianOplus[4](1,0) = 0; // omegadot deltaT2 + _jacobianOplus[5](1,0) = 0; // omegadot angle1 + _jacobianOplus[6](1,0) = 0; // omegadot angle2 + _jacobianOplus[7](1,0) = 0; // omegadot angle3 + } + + _jacobianOplus[0](1,0) = 0; // omegadot x1 + _jacobianOplus[0](1,1) = 0; // omegadot y1 + _jacobianOplus[1](1,0) = 0; // omegadot x2 + _jacobianOplus[1](1,1) = 0; // omegadot y2 + _jacobianOplus[2](1,0) = 0; // omegadot x3 + _jacobianOplus[2](1,1) = 0; // omegadot y3 + _jacobianOplus[5](0,0) = 0; // acc angle1 + _jacobianOplus[6](0,0) = 0; // acc angle2 + _jacobianOplus[7](0,0) = 0; // acc angle3 + } +#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: + + /** + * @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) + { + 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); + + 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 +}; + + + + +/** + * @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: + + /** + * @brief Construct edge. + */ + EdgeAccelerationGoal() + { + _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]); + + // VELOCITY & ACCELERATION + + 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! + } + + 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(); + + _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 +}; + + + + +/** + * @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: + + /** + * @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]); + } + +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(); + + 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(); + + 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: + + /** + * @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]); + + // VELOCITY & ACCELERATION + + 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(); + + _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 + +#endif /* EDGE_ACCELERATION_H_ */ diff --git a/edge_dynamic_obstacle.h b/edge_dynamic_obstacle.h new file mode 100644 index 00000000..c5197d14 --- /dev/null +++ b/edge_dynamic_obstacle.h @@ -0,0 +1,153 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann, Franz Albers + *********************************************************************/ + +#ifndef EDGE_DYNAMICOBSTACLE_H +#define EDGE_DYNAMICOBSTACLE_H + +#include +#include +#include +#include +#include +#include +#include + +namespace teb_local_planner +{ + +/** + * @class EdgeDynamicObstacle + * @brief Edge defining the cost function for keeping a distance from dynamic (moving) obstacles. + * + * The edge depends on two vertices \f$ \mathbf{s}_i, \Delta T_i \f$ and minimizes: \n + * \f$ \min \textrm{penaltyBelow}( dist2obstacle) \cdot weight \f$. \n + * \e dist2obstacle denotes the minimum distance to the obstacle trajectory (spatial and temporal). \n + * \e weight can be set using setInformation(). \n + * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow(). \n + * @see TebOptimalPlanner::AddEdgesDynamicObstacles + * @remarks Do not forget to call setTebConfig(), setVertexIdx() and + * @warning Experimental + */ +class EdgeDynamicObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPose> +{ +public: + + /** + * @brief Construct edge. + */ + EdgeDynamicObstacle() : t_(0) + { + } + + /** + * @brief Construct edge and specify the time for its associated pose (neccessary for computeError). + * @param t_ Estimated time until current pose is reached + */ + EdgeDynamicObstacle(double t) : t_(t) + { + } + + /** + * @brief Actual cost function + */ + void computeError() + { + ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeDynamicObstacle()"); + const VertexPose* bandpt = static_cast(_vertices[0]); + + double dist = robot_model_->estimateSpatioTemporalDistance(bandpt->pose(), _measurement, t_); + + _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); + _error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.dynamic_obstacle_inflation_dist, 0.0); + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeDynamicObstacle::computeError() _error[0]=%f\n",_error[0]); + } + + + /** + * @brief Set Obstacle for the underlying cost function + * @param obstacle Const pointer to an Obstacle or derived 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 + double t_; //!< Estimated time until current pose is reached + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + + + + +} // end namespace + +#endif diff --git a/edge_kinematics.h b/edge_kinematics.h new file mode 100644 index 00000000..694e602a --- /dev/null +++ b/edge_kinematics.h @@ -0,0 +1,230 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef _EDGE_KINEMATICS_H +#define _EDGE_KINEMATICS_H + +#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]); + } + +#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 + } +#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 + +#endif diff --git a/edge_obstacle.h b/edge_obstacle.h new file mode 100644 index 00000000..976c494a --- /dev/null +++ b/edge_obstacle.h @@ -0,0 +1,295 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ +#ifndef EDGE_OBSTACLE_H_ +#define EDGE_OBSTACLE_H_ + +#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; + } + + /** + * @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); + + // Original obstacle cost. + _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); + + if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0) + { + // Optional non-linear cost. Note the max cost (before weighting) is + // the same as the straight line version and that all other costs are + // below the straight line (for positive exponent), so it may be + // necessary to increase weight_obstacle and/or the inflation_weight + // when using larger exponents. + _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent); + } + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeObstacle::computeError() _error[0]=%f\n",_error[0]); + } + +#ifdef USE_ANALYTIC_JACOBI +#if 0 + + /** + * @brief Jacobi matrix of the cost function specified in computeError(). + */ + void linearizeOplus() + { + ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgePointObstacle()"); + const VertexPose* bandpt = static_cast(_vertices[0]); + + Eigen::Vector2d deltaS = *_measurement - bandpt->position(); + double angdiff = atan2(deltaS[1],deltaS[0])-bandpt->theta(); + + double dist_squared = deltaS.squaredNorm(); + double dist = sqrt(dist_squared); + + double aux0 = sin(angdiff); + double dev_left_border = penaltyBoundFromBelowDerivative(dist*fabs(aux0),cfg_->obstacles.min_obstacle_dist,cfg_->optim.penalty_epsilon); + + if (dev_left_border==0) + { + _jacobianOplusXi( 0 , 0 ) = 0; + _jacobianOplusXi( 0 , 1 ) = 0; + _jacobianOplusXi( 0 , 2 ) = 0; + return; + } + + double aux1 = -fabs(aux0) / dist; + double dev_norm_x = deltaS[0]*aux1; + double dev_norm_y = deltaS[1]*aux1; + + double aux2 = cos(angdiff) * g2o::sign(aux0); + double aux3 = aux2 / dist_squared; + double dev_proj_x = aux3 * deltaS[1] * dist; + double dev_proj_y = -aux3 * deltaS[0] * dist; + double dev_proj_angle = -aux2; + + _jacobianOplusXi( 0 , 0 ) = dev_left_border * ( dev_norm_x + dev_proj_x ); + _jacobianOplusXi( 0 , 1 ) = dev_left_border * ( dev_norm_y + dev_proj_y ); + _jacobianOplusXi( 0 , 2 ) = dev_left_border * dev_proj_angle; + } +#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 EdgeInflatedObstacle()"); + const VertexPose* bandpt = static_cast(_vertices[0]); + + double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement); + + // Original "straight line" obstacle cost. The max possible value + // before weighting is min_obstacle_dist + _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); + + if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0) + { + // Optional non-linear cost. Note the max cost (before weighting) is + // the same as the straight line version and that all other costs are + // below the straight line (for positive exponent), so it may be + // necessary to increase weight_obstacle and/or the inflation_weight + // when using larger exponents. + _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent); + } + + // Additional linear inflation cost + _error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.inflation_dist, 0.0); + + + ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeInflatedObstacle::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 + +#endif diff --git a/edge_prefer_rotdir.h b/edge_prefer_rotdir.h new file mode 100644 index 00000000..f744ef4d --- /dev/null +++ b/edge_prefer_rotdir.h @@ -0,0 +1,115 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ +#ifndef EDGE_PREFER_ROTDIR_H_ +#define EDGE_PREFER_ROTDIR_H_ + +#include +#include +#include +#include "g2o/core/base_unary_edge.h" + + +namespace teb_local_planner +{ + +/** + * @class EdgePreferRotDir + * @brief Edge defining the cost function for penalzing a specified turning direction, in particular left resp. right turns + * + * The edge depends on two consecutive vertices \f$ \mathbf{s}_i, \mathbf{s}_{i+1} \f$ and penalizes a given rotation direction + * based on the \e weight and \e dir (\f$ dir \in \{-1,1\} \f$) + * \e dir should be +1 to prefer left rotations and -1 to prefer right rotations \n + * \e weight can be set using setInformation(). \n + * @see TebOptimalPlanner::AddEdgePreferRotDir + */ +class EdgePreferRotDir : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose> +{ +public: + + /** + * @brief Construct edge. + */ + EdgePreferRotDir() + { + _measurement = 1; + } + + /** + * @brief Actual cost function + */ + void computeError() + { + const VertexPose* conf1 = static_cast(_vertices[0]); + const VertexPose* conf2 = static_cast(_vertices[1]); + + _error[0] = penaltyBoundFromBelow( _measurement*g2o::normalize_theta(conf2->theta()-conf1->theta()) , 0, 0); + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgePreferRotDir::computeError() _error[0]=%f\n",_error[0]); + } + + /** + * @brief Specify the prefered direction of rotation + * @param dir +1 to prefer the left side, -1 to prefer the right side + */ + void setRotDir(double dir) + { + _measurement = dir; + } + + /** Prefer rotations to the right */ + void preferRight() {_measurement = -1;} + + /** Prefer rotations to the right */ + void preferLeft() {_measurement = 1;} + + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + + + +} // end namespace + +#endif diff --git a/edge_shortest_path.h b/edge_shortest_path.h new file mode 100644 index 00000000..7a52c2f6 --- /dev/null +++ b/edge_shortest_path.h @@ -0,0 +1,89 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef EDGE_SHORTEST_PATH_H_ +#define EDGE_SHORTEST_PATH_H_ + +#include + +#include + +#include +#include + +#include + +namespace teb_local_planner { + +/** + * @class EdgeShortestPath + * @brief Edge defining the cost function for minimizing the Euclidean distance between two consectuive poses. + * + * @see TebOptimalPlanner::AddEdgesShortestPath + */ +class EdgeShortestPath : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose> { +public: + /** + * @brief Construct edge. + */ + EdgeShortestPath() { this->setMeasurement(0.); } + + /** + * @brief Actual cost function + */ + void computeError() { + ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeShortestPath()"); + const VertexPose *pose1 = static_cast(_vertices[0]); + const VertexPose *pose2 = static_cast(_vertices[1]); + _error[0] = (pose2->position() - pose1->position()).norm(); + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeShortestPath::computeError() _error[0]=%f\n", _error[0]); + } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // end namespace + +#endif /* EDGE_SHORTEST_PATH_H_ */ diff --git a/edge_time_optimal.h b/edge_time_optimal.h new file mode 100644 index 00000000..43e16373 --- /dev/null +++ b/edge_time_optimal.h @@ -0,0 +1,116 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef EDGE_TIMEOPTIMAL_H_ +#define EDGE_TIMEOPTIMAL_H_ + +#include + +#include + +#include +#include +#include +#include + +#include + +namespace teb_local_planner +{ + + +/** + * @class EdgeTimeOptimal + * @brief Edge defining the cost function for minimizing transition time of the trajectory. + * + * The edge depends on a single vertex \f$ \Delta T_i \f$ and minimizes: \n + * \f$ \min \Delta T_i^2 \cdot scale \cdot weight \f$. \n + * \e scale is determined using the penaltyEquality() function, since we experiences good convergence speeds with it. \n + * \e weight can be set using setInformation() (something around 1.0 seems to be fine). \n + * @see TebOptimalPlanner::AddEdgesTimeOptimal + * @remarks Do not forget to call setTebConfig() + */ +class EdgeTimeOptimal : public BaseTebUnaryEdge<1, double, VertexTimeDiff> +{ +public: + + /** + * @brief Construct edge. + */ + EdgeTimeOptimal() + { + this->setMeasurement(0.); + } + + /** + * @brief Actual cost function + */ + void computeError() + { + ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()"); + const VertexTimeDiff* timediff = static_cast(_vertices[0]); + + _error[0] = timediff->dt(); + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeTimeOptimal::computeError() _error[0]=%f\n",_error[0]); + } + +#ifdef USE_ANALYTIC_JACOBI + /** + * @brief Jacobi matrix of the cost function specified in computeError(). + */ + void linearizeOplus() + { + ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()"); + _jacobianOplusXi( 0 , 0 ) = 1; + } +#endif + + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +}; // end namespace + +#endif /* EDGE_TIMEOPTIMAL_H_ */ diff --git a/edge_velocity.h b/edge_velocity.h new file mode 100644 index 00000000..b01c7e36 --- /dev/null +++ b/edge_velocity.h @@ -0,0 +1,489 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef EDGE_VELOCITY_H +#define EDGE_VELOCITY_H + +#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) + { + 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.0 * (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); + + 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... + + /** + * @brief Jacobi matrix of the cost function specified in computeError(). + */ + void linearizeOplus() + { + 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]); + + Eigen::Vector2d deltaS = conf2->position() - conf1->position(); + double dist = deltaS.norm(); + double aux1 = dist*deltaT->estimate(); + double aux2 = 1/deltaT->estimate(); + + double vel = dist * aux2; + double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) * aux2; + + double dev_border_vel = penaltyBoundToIntervalDerivative(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon); + double dev_border_omega = penaltyBoundToIntervalDerivative(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon); + + _jacobianOplus[0].resize(2,3); // conf1 + _jacobianOplus[1].resize(2,3); // conf2 + _jacobianOplus[2].resize(2,1); // deltaT + +// if (aux1==0) aux1=1e-6; +// if (aux2==0) aux2=1e-6; + + if (dev_border_vel!=0) + { + double aux3 = dev_border_vel / aux1; + _jacobianOplus[0](0,0) = -deltaS[0] * aux3; // vel x1 + _jacobianOplus[0](0,1) = -deltaS[1] * aux3; // vel y1 + _jacobianOplus[1](0,0) = deltaS[0] * aux3; // vel x2 + _jacobianOplus[1](0,1) = deltaS[1] * aux3; // vel y2 + _jacobianOplus[2](0,0) = -vel * aux2 * dev_border_vel; // vel deltaT + } + else + { + _jacobianOplus[0](0,0) = 0; // vel x1 + _jacobianOplus[0](0,1) = 0; // vel y1 + _jacobianOplus[1](0,0) = 0; // vel x2 + _jacobianOplus[1](0,1) = 0; // vel y2 + _jacobianOplus[2](0,0) = 0; // vel deltaT + } + + if (dev_border_omega!=0) + { + double aux4 = aux2 * dev_border_omega; + _jacobianOplus[2](1,0) = -omega * aux4; // omega deltaT + _jacobianOplus[0](1,2) = -aux4; // omega angle1 + _jacobianOplus[1](1,2) = aux4; // omega angle2 + } + else + { + _jacobianOplus[2](1,0) = 0; // omega deltaT + _jacobianOplus[0](1,2) = 0; // omega angle1 + _jacobianOplus[1](1,2) = 0; // omega angle2 + } + + _jacobianOplus[0](1,0) = 0; // omega x1 + _jacobianOplus[0](1,1) = 0; // omega y1 + _jacobianOplus[1](1,0) = 0; // omega x2 + _jacobianOplus[1](1,1) = 0; // omega y2 + _jacobianOplus[0](0,2) = 0; // vel angle1 + _jacobianOplus[1](0,2) = 0; // vel angle2 + } +#endif +#endif + + +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(); + + _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]); + } + + /** + * @class EdgeSteeringRate + * @brief Edge defining the cost function for limiting the steering rate w.r.t. the current wheelbase parameter + * + * The edge depends on four vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2} \Delta T_i \f$ . + * @remarks This edge requires the TebConfig::Robot::whelbase parameter to be set. + * @remarks Do not forget to call setTebConfig() + */ +class EdgeSteeringRate : public BaseTebMultiEdge<1, double> +{ +public: + + /** + * @brief Construct edge. + */ + EdgeSteeringRate() + { + this->resize(5); // 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 EdgeSteeringRate()"); + const VertexPose* conf1 = static_cast(_vertices[0]); + const VertexPose* conf2 = static_cast(_vertices[1]); + const VertexPose* conf3 = static_cast(_vertices[2]); + const VertexTimeDiff* dt1 = static_cast(_vertices[3]); + const VertexTimeDiff* dt2 = static_cast(_vertices[4]); + + Eigen::Vector2d delta_s1 = conf2->estimate().position() - conf1->estimate().position(); + Eigen::Vector2d delta_s2 = conf3->estimate().position() - conf2->estimate().position(); + double dist1 = delta_s1.norm(); + double dist2 = delta_s2.norm(); + double angle_diff1 = g2o::normalize_theta( conf2->theta() - conf1->theta() ); + double angle_diff2 = g2o::normalize_theta( conf3->theta() - conf2->theta() ); + + double phi1, phi2; + if (std::abs(dist1) < 1e-12) + { + phi1 = 0; // TODO previous phi? + //ROS_INFO("phi 1 is zero!"); + } + else + { + //dist1 *= fast_sigmoid( 100.0 * (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta())) ); // consider direction + //if (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta()) < 0) + //dist1 = -dist1; + + if (cfg_->trajectory.exact_arc_length) + phi1 = std::atan(cfg_->robot.wheelbase / dist1 * 2.0*std::sin(angle_diff1/2.0)); + else + phi1 = std::atan(cfg_->robot.wheelbase / dist1 * angle_diff1); + + // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan) + // In case if we apply the sign to the angle directly, it seems to work: + phi1 *= fast_sigmoid( 100.0 * (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta())) ); // consider direction + } + + if (std::abs(dist2) < 1e-12) + { + phi2 = phi1; + ROS_INFO("phi 2 is phi1!"); + } + else + { + //dist2 *= fast_sigmoid( 100.0 * (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta())) ); // consider direction + //if (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta()) < 0) + // dist2 = -dist2; + + if (cfg_->trajectory.exact_arc_length) + phi2 = std::atan(cfg_->robot.wheelbase / dist2 * 2.0*std::sin(angle_diff2/2.0)); + else + phi2 = std::atan(cfg_->robot.wheelbase / dist2 * angle_diff2); + + // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan). + // In case if we apply the sign to the angle directly, it seems to work: + phi2 *= fast_sigmoid( 100.0 * (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta())) ); // consider direction + } + + _error[0] = penaltyBoundToInterval(g2o::normalize_theta(phi2 - phi1)*2.0 / (dt1->dt() + dt2->dt()), cfg_->robot.max_steering_rate, 0.0); + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRate::computeError() _error[0]\n",_error[0]); + } + +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +//! Corresponds to EdgeSteeringRate but with initial steering angle for the predecessor configuration +class EdgeSteeringRateStart : public BaseTebMultiEdge<1, double> +{ +public: + + /** + * @brief Construct edge. + */ + EdgeSteeringRateStart() + { + 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 EdgeSteeringRateStart()"); + const VertexPose* conf1 = static_cast(_vertices[0]); + const VertexPose* conf2 = static_cast(_vertices[1]); + const VertexTimeDiff* dt = static_cast(_vertices[2]); + + Eigen::Vector2d delta_s = conf2->estimate().position() - conf1->estimate().position(); + double dist = delta_s.norm(); + double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); + + double phi; + if (std::abs(dist) < 1e-12) + { + ROS_INFO("Start phi equals pervious phi!"); + phi = _measurement; + } + else + { + //dist *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction + //dist *= (double)g2o::sign( delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta()) ); // consider direction + + if (cfg_->trajectory.exact_arc_length) + phi = std::atan(cfg_->robot.wheelbase / dist * 2.0*std::sin(angle_diff/2.0)); + else + phi = std::atan(cfg_->robot.wheelbase / dist * angle_diff); + + // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan). + // In case if we apply the sign to the angle directly, it seems to work: + phi *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction + } + + _error[0] = penaltyBoundToInterval(g2o::normalize_theta(phi - _measurement) / dt->dt(), cfg_->robot.max_steering_rate, 0.0); + + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRateStart::computeError() _error[0]\n",_error[0]); + } + + void setInitialSteeringAngle(double steering_angle) + { + _measurement = steering_angle; + } + +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +//! Corresponds to EdgeSteeringRate but with initial steering angle for the successor configuration +class EdgeSteeringRateGoal : public BaseTebMultiEdge<1, double> +{ +public: + + /** + * @brief Construct edge. + */ + EdgeSteeringRateGoal() + { + 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 EdgeSteeringRateGoal()"); + const VertexPose* conf1 = static_cast(_vertices[0]); + const VertexPose* conf2 = static_cast(_vertices[1]); + const VertexTimeDiff* dt = static_cast(_vertices[2]); + + Eigen::Vector2d delta_s = conf2->estimate().position() - conf1->estimate().position(); + double dist = delta_s.norm(); + double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); + + double phi; + if (std::abs(dist) < 1e-12) + { + ROS_INFO("Goal phi is zero!"); + phi = 0; + } + else + { + //dist *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction + + if (cfg_->trajectory.exact_arc_length) + phi = std::atan(cfg_->robot.wheelbase / dist * 2.0*std::sin(angle_diff/2.0)); + else + phi = std::atan(cfg_->robot.wheelbase / dist * angle_diff); + + // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan). + // In case if we apply the sign to the angle directly, it seems to work: + phi *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction + } + + _error[0] = penaltyBoundToInterval(g2o::normalize_theta(_measurement - phi) / dt->dt(), cfg_->robot.max_steering_rate, 0.0); + + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRateGoal::computeError() _error[0]\n",_error[0]); + } + + void setGoalSteeringAngle(double steering_angle) + { + _measurement = steering_angle; + } + +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + + + +} // end namespace + +#endif diff --git a/edge_velocity_obstacle_ratio.h b/edge_velocity_obstacle_ratio.h new file mode 100644 index 00000000..ecac1aa4 --- /dev/null +++ b/edge_velocity_obstacle_ratio.h @@ -0,0 +1,166 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + *********************************************************************/ + +#pragma once + +#include +#include +#include +#include + +namespace teb_local_planner +{ + + +/** + * @class EdgeVelocityObstacleRatio + * @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 EdgeVelocityObstacleRatio : public BaseTebMultiEdge<2, const Obstacle*> +{ +public: + + /** + * @brief Construct edge. + */ + EdgeVelocityObstacleRatio() : + robot_model_(nullptr) + { + // The three vertices are two poses and one time difference + 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_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeVelocityObstacleRatio()"); + 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 *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction + + const double omega = angle_diff / deltaT->estimate(); + + double dist_to_obstacle = robot_model_->calculateDistance(conf1->pose(), _measurement); + + double ratio; + if (dist_to_obstacle < cfg_->obstacles.obstacle_proximity_lower_bound) + ratio = 0; + else if (dist_to_obstacle > cfg_->obstacles.obstacle_proximity_upper_bound) + ratio = 1; + else + ratio = (dist_to_obstacle - cfg_->obstacles.obstacle_proximity_lower_bound) / + (cfg_->obstacles.obstacle_proximity_upper_bound - cfg_->obstacles.obstacle_proximity_lower_bound); + ratio *= cfg_->obstacles.obstacle_proximity_ratio_max_vel; + + const double max_vel_fwd = ratio * cfg_->robot.max_vel_x; + const double max_omega = ratio * cfg_->robot.max_vel_theta; + _error[0] = penaltyBoundToInterval(vel, max_vel_fwd, 0); + _error[1] = penaltyBoundToInterval(omega, max_omega, 0); + + ROS_ASSERT_MSG(std::isfinite(_error[0]) || std::isfinite(_error[1]), "EdgeVelocityObstacleRatio::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 diff --git a/edge_via_point.h b/edge_via_point.h new file mode 100644 index 00000000..9ddefcb8 --- /dev/null +++ b/edge_via_point.h @@ -0,0 +1,120 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ +#ifndef EDGE_VIA_POINT_H_ +#define EDGE_VIA_POINT_H_ + +#include +#include + +#include "g2o/core/base_unary_edge.h" + + +namespace teb_local_planner +{ + +/** + * @class EdgeViaPoint + * @brief Edge defining the cost function for pushing a configuration towards a via point + * + * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n + * \f$ \min dist2point \cdot weight \f$. \n + * \e dist2point denotes the distance to the via point. \n + * \e weight can be set using setInformation(). \n + * @see TebOptimalPlanner::AddEdgesViaPoints + * @remarks Do not forget to call setTebConfig() and setViaPoint() + */ +class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPose> +{ +public: + + /** + * @brief Construct edge. + */ + EdgeViaPoint() + { + _measurement = NULL; + } + + /** + * @brief Actual cost function + */ + void computeError() + { + ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig(), setViaPoint() on EdgeViaPoint()"); + const VertexPose* bandpt = static_cast(_vertices[0]); + + _error[0] = (bandpt->position() - *_measurement).norm(); + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeViaPoint::computeError() _error[0]=%f\n",_error[0]); + } + + /** + * @brief Set pointer to associated via point for the underlying cost function + * @param via_point 2D position vector containing the position of the via point + */ + void setViaPoint(const Eigen::Vector2d* via_point) + { + _measurement = via_point; + } + + /** + * @brief Set all parameters at once + * @param cfg TebConfig class + * @param via_point 2D position vector containing the position of the via point + */ + void setParameters(const TebConfig& cfg, const Eigen::Vector2d* via_point) + { + cfg_ = &cfg; + _measurement = via_point; + } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + + + +} // end namespace + +#endif diff --git a/misc.h b/misc.h new file mode 100644 index 00000000..fadd60e1 --- /dev/null +++ b/misc.h @@ -0,0 +1,152 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef MISC_H +#define MISC_H + +#include +#include +#include + + +namespace teb_local_planner +{ + +#define SMALL_NUM 0.00000001 + +//! Symbols for left/none/right rotations +enum class RotType { left, none, right }; + +/** + * @brief Check whether two variables (double) are close to each other + * @param a the first value to compare + * @param b the second value to compare + * @param epsilon precision threshold + * @return \c true if |a-b| < epsilon, false otherwise + */ +inline bool is_close(double a, double b, double epsilon = 1e-4) +{ + return std::fabs(a - b) < epsilon; +} + +/** + * @brief Return the average angle of an arbitrary number of given angles [rad] + * @param angles vector containing all angles + * @return average / mean angle, that is normalized to [-pi, pi] + */ +inline double average_angles(const std::vector& angles) +{ + double x=0, y=0; + for (std::vector::const_iterator it = angles.begin(); it!=angles.end(); ++it) + { + x += cos(*it); + y += sin(*it); + } + if(x == 0 && y == 0) + return 0; + else + return std::atan2(y, x); +} + +/** @brief Small helper function: check if |a|<|b| */ +inline bool smaller_than_abs(double i, double j) {return std::fabs(i) +inline double distance_points2d(const P1& point1, const P2& point2) +{ + return std::sqrt( std::pow(point2.x-point1.x,2) + std::pow(point2.y-point1.y,2) ); +} + + +/** + * @brief Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d) + * @param v1 object containing public methods x() and y() + * @param v2 object containing fields x() and y() + * @return magnitude that would result in the 3D case (along the z-axis) +*/ +template +inline double cross2d(const V1& v1, const V2& v2) +{ + return v1.x()*v2.y() - v2.x()*v1.y(); +} + +/** + * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference. + * + * Return a constant reference for boths input variants (pointer or reference). + * @remarks Makes only sense in combination with the overload getConstReference(const T& val). + * @param ptr pointer of type T + * @tparam T arbitrary type + * @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion + */ +template +inline const T& get_const_reference(const T* ptr) {return *ptr;} + +/** + * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference. + * + * Return a constant reference for boths input variants (pointer or reference). + * @remarks Makes only sense in combination with the overload getConstReference(const T* val). + * @param val + * @param dummy SFINAE helper variable + * @tparam T arbitrary type + * @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion + */ +template +inline const T& get_const_reference(const T& val, typename boost::disable_if >::type* dummy = 0) {return val;} + +} // namespace teb_local_planner + +#endif /* MISC_H */ diff --git a/optimal_planner.cpp b/optimal_planner.cpp new file mode 100644 index 00000000..0b202188 --- /dev/null +++ b/optimal_planner.cpp @@ -0,0 +1,1373 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#include + +// g2o custom edges and vertices for the TEB planner +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + + +namespace teb_local_planner +{ + +// ============== 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(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::updateRobotModel(RobotFootprintModelPtr robot_model) +{ + robot_model_ = robot_model; +} + +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; +} + +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_SHORTEST_PATH", 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_STEERING_RATE", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_STEERING_RATE_START", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_STEERING_RATE_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(); + std::unique_ptr linear_solver(new TEBLinearSolver()); // see typedef in optimization.h + linear_solver->setBlockOrdering(true); + std::unique_ptr block_solver(new TEBBlockSolver(std::move(linear_solver))); + g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(block_solver)); + + 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) + { + //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; +} + +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()) + { + teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, 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 + && fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // 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, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation, + 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); +} + +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 + && fabs(g2o::normalize_theta(goal.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // 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; + } + + optimizer_->setComputeBatchStatistics(cfg_->recovery.divergence_detection_enable); + + // 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(); + + AddEdgesSteeringRate(); + + AddEdgesTimeOptimal(); + + AddEdgesShortestPath(); + + 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(); + + if (cfg_->optim.weight_velocity_obstacle_ratio > 0) + AddEdgesVelocityObstacleRatio(); + + 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 + if (optimizer_) + { + // we will delete all edges but keep the vertices. + // before doing so, we will delete the link from the vertices to the edges. + auto& vertices = optimizer_->vertices(); + for(auto& v : vertices) + v.second->edges().clear(); + + optimizer_->vertices().clear(); // necessary, 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 + obstacles_per_vertex_.resize(teb_.sizePoses()); + auto iter_obstacle = obstacles_per_vertex_.begin(); + for (int i=0; isetId(id_counter++); + optimizer_->addVertex(teb_.PoseVertex(i)); + if (teb_.sizeTimeDiffs()!=0 && isetId(id_counter++); + optimizer_->addVertex(teb_.TimeDiffVertex(i)); + } + iter_obstacle->clear(); + (iter_obstacle++)->reserve(obstacles_->size()); + } +} + + +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; + + auto iter_obstacle = obstacles_per_vertex_.begin(); + + auto create_edge = [inflated, &information, &information_inflated, this] (int index, const Obstacle* obstacle) { + if (inflated) + { + 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(), obstacle); + optimizer_->addEdge(dist_bandpt_obst); + } + else + { + 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(), obstacle); + optimizer_->addEdge(dist_bandpt_obst); + }; + }; + + // iterate all teb points, skipping the last and, if the EdgeVelocityObstacleRatio edges should not be created, the first one too + const int first_vertex = cfg_->optim.weight_velocity_obstacle_ratio == 0 ? 1 : 0; + for (int i = first_vertex; i < teb_.sizePoses() - 1; ++i) + { + double left_min_dist = std::numeric_limits::max(); + double right_min_dist = std::numeric_limits::max(); + ObstaclePtr left_obstacle; + ObstaclePtr right_obstacle; + + 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 robot model + double dist = robot_model_->calculateDistance(teb_.Pose(i), obst.get()); + + // force considering obstacle if really close to the current pose + if (dist < cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_force_inclusion_factor) + { + iter_obstacle->push_back(obst); + 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; + } + } + else + { + if (dist < right_min_dist) + { + right_min_dist = dist; + right_obstacle = obst; + } + } + } + + if (left_obstacle) + iter_obstacle->push_back(left_obstacle); + if (right_obstacle) + iter_obstacle->push_back(right_obstacle); + + // continue here to ignore obstacles for the first pose, but use them later to create the EdgeVelocityObstacleRatio edges + if (i == 0) + { + ++iter_obstacle; + continue; + } + + // create obstacle edges + for (const ObstaclePtr obst : *iter_obstacle) + create_edge(i, obst.get()); + ++iter_obstacle; + } +} + + +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(index)); + dist_bandpt_obst->setInformation(information_inflated); + 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(index)); + dist_bandpt_obst->setInformation(information); + 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::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". + } + } +} + +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); + } + } + 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) + { + 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); + } + + // now add the usual acceleration edge for each tuple of three teb poses + for (int i=0; i < n - 2; ++i) + { + 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); + } + + // 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); + } + + // 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); + } + } +} + + + +void TebOptimalPlanner::AddEdgesTimeOptimal() +{ + 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); + } +} + +void TebOptimalPlanner::AddEdgesShortestPath() +{ + if (cfg_->optim.weight_shortest_path==0) + return; // if weight equals zero skip adding edges! + + Eigen::Matrix information; + information.fill(cfg_->optim.weight_shortest_path); + + for (int i=0; i < teb_.sizePoses()-1; ++i) + { + EdgeShortestPath* shortest_path_edge = new EdgeShortestPath; + shortest_path_edge->setVertex(0,teb_.PoseVertex(i)); + shortest_path_edge->setVertex(1,teb_.PoseVertex(i+1)); + shortest_path_edge->setInformation(information); + shortest_path_edge->setTebConfig(*cfg_); + optimizer_->addEdge(shortest_path_edge); + } +} + + + +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::AddEdgesSteeringRate() +{ + if (cfg_->robot.max_steering_rate==0 || cfg_->optim.weight_max_steering_rate==0) + return; // if weight equals zero skip adding edges! + + // create edge for satisfiying kinematic constraints + Eigen::Matrix information_steering_rate; + information_steering_rate(0, 0) = cfg_->optim.weight_max_steering_rate; + + int n = teb_.sizePoses(); + + // check if an initial velocity should be taken into accound (we apply the same for the steering rate) + if (vel_start_.first) + { + EdgeSteeringRateStart* steering_rate_edge = new EdgeSteeringRateStart; + steering_rate_edge->setVertex(0,teb_.PoseVertex(0)); + steering_rate_edge->setVertex(1,teb_.PoseVertex(1)); + steering_rate_edge->setVertex(2,teb_.TimeDiffVertex(0)); + if (std::abs(vel_start_.second.linear.x) < 1e-6) + { + //ROS_INFO("TebOptimalPlanner::AddEdgesSteeringRate(): current v close to zero. Using last measured steering angle"); + steering_rate_edge->setInitialSteeringAngle(recent_steering_angle_); // TODO(roesmann): it would be better to measure the actual steering angle + } + else + { + recent_steering_angle_ = std::atan(cfg_->robot.wheelbase/vel_start_.second.linear.x * vel_start_.second.angular.z); + steering_rate_edge->setInitialSteeringAngle(recent_steering_angle_); + } + steering_rate_edge->setInformation(information_steering_rate); + steering_rate_edge->setTebConfig(*cfg_); + optimizer_->addEdge(steering_rate_edge); + } + + for (int i=0; i < n-2; i++) // ignore twiced start only + { + EdgeSteeringRate* steering_rate_edge = new EdgeSteeringRate; + steering_rate_edge->setVertex(0,teb_.PoseVertex(i)); + steering_rate_edge->setVertex(1,teb_.PoseVertex(i+1)); + steering_rate_edge->setVertex(2,teb_.PoseVertex(i+2)); + steering_rate_edge->setVertex(3,teb_.TimeDiffVertex(i)); + steering_rate_edge->setVertex(4,teb_.TimeDiffVertex(i+1)); + steering_rate_edge->setInformation(information_steering_rate); + steering_rate_edge->setTebConfig(*cfg_); + optimizer_->addEdge(steering_rate_edge); + } + + // check if a goal velocity should be taken into accound (we apply the same for the steering rate) + if (vel_goal_.first) + { + EdgeSteeringRateGoal* steering_rate_edge = new EdgeSteeringRateGoal; + steering_rate_edge->setVertex(0,teb_.PoseVertex(n-2)); + steering_rate_edge->setVertex(1,teb_.PoseVertex(n-1)); + steering_rate_edge->setVertex(2,teb_.TimeDiffVertex( teb_.sizeTimeDiffs()-1 )); + if (vel_goal_.second.linear.x==0.0) + steering_rate_edge->setGoalSteeringAngle(0.0); + else + steering_rate_edge->setGoalSteeringAngle(std::atan(cfg_->robot.wheelbase/vel_goal_.second.linear.x * vel_goal_.second.angular.z) ); + steering_rate_edge->setInformation(information_steering_rate); + steering_rate_edge->setTebConfig(*cfg_); + optimizer_->addEdge(steering_rate_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::AddEdgesVelocityObstacleRatio() +{ + Eigen::Matrix information; + information(0,0) = cfg_->optim.weight_velocity_obstacle_ratio; + information(1,1) = cfg_->optim.weight_velocity_obstacle_ratio; + information(0,1) = information(1,0) = 0; + + auto iter_obstacle = obstacles_per_vertex_.begin(); + + for (int index = 0; index < teb_.sizePoses() - 1; ++index) + { + for (const ObstaclePtr obstacle : (*iter_obstacle++)) + { + EdgeVelocityObstacleRatio* edge = new EdgeVelocityObstacleRatio; + edge->setVertex(0,teb_.PoseVertex(index)); + edge->setVertex(1,teb_.PoseVertex(index + 1)); + edge->setVertex(2,teb_.TimeDiffVertex(index)); + edge->setInformation(information); + edge->setParameters(*cfg_, robot_model_.get(), obstacle.get()); + optimizer_->addEdge(edge); + } + } +} + +bool TebOptimalPlanner::hasDiverged() const +{ + // Early returns if divergence detection is not active + if (!cfg_->recovery.divergence_detection_enable) + return false; + + auto stats_vector = optimizer_->batchStatistics(); + + // No statistics yet + if (stats_vector.empty()) + return false; + + // Grab the statistics of the final iteration + const auto last_iter_stats = stats_vector.back(); + + return last_iter_stats.chi2 > cfg_->recovery.divergence_detection_max_chi_squared; +} + +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++) + { + double cur_cost = (*it)->chi2(); + + if (dynamic_cast(*it) != nullptr + || dynamic_cast(*it) != nullptr + || dynamic_cast(*it) != nullptr) + { + cur_cost *= obst_cost_scale; + } + else if (dynamic_cast(*it) != nullptr) + { + cur_cost *= viapoint_cost_scale; + } + else if (dynamic_cast(*it) != nullptr && alternative_time_cost) + { + continue; // skip these edges if alternative_time_cost is active + } + cost_ += cur_cost; + } + + // 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, int look_ahead_poses) 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; + } + look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1)); + double dt = 0.0; + for(int counter = 0; counter < look_ahead_poses; ++counter) + { + dt += teb_.TimeDiff(counter); + if(dt >= cfg_->trajectory.dt_ref * look_ahead_poses) // TODO: change to look-ahead time? Refine trajectory? + { + look_ahead_poses = counter + 1; + break; + } + } + 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(look_ahead_poses), 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) == -1 ) + { + if (visualization_) + { + visualization_->publishInfeasibleRobotPose(teb().Pose(i), *robot_model_); + } + return false; + } + // Checks if the distance between two poses is higher than the robot radius or the orientation diff is bigger than the specified threshold + // and interpolates in that case. + // (if obstacles are pushing two consecutive poses away, the center between two consecutive poses might coincide with the obstacle ;-)! + if (i cfg_->trajectory.min_resolution_collision_check_angular || delta_dist.norm() > inscribed_radius) + { + int n_additional_samples = std::max(std::ceil(fabs(delta_rot) / cfg_->trajectory.min_resolution_collision_check_angular), + std::ceil(delta_dist.norm() / inscribed_radius)) - 1; + PoseSE2 intermediate_pose = teb().Pose(i); + for(int step = 0; step < n_additional_samples; ++step) + { + intermediate_pose.position() = intermediate_pose.position() + delta_dist / (n_additional_samples + 1.0); + intermediate_pose.theta() = g2o::normalize_theta(intermediate_pose.theta() + + delta_rot / (n_additional_samples + 1.0)); + if ( costmap_model->footprintCost(intermediate_pose.x(), intermediate_pose.y(), intermediate_pose.theta(), + footprint_spec, inscribed_radius, circumscribed_radius) == -1 ) + { + if (visualization_) + { + visualization_->publishInfeasibleRobotPose(intermediate_pose, *robot_model_); + } + return false; + } + } + } + } + } + return true; +} + +} // namespace teb_local_planner diff --git a/optimal_planner.h b/optimal_planner.h new file mode 100644 index 00000000..e24f6f27 --- /dev/null +++ b/optimal_planner.h @@ -0,0 +1,718 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef OPTIMAL_PLANNER_H_ +#define OPTIMAL_PLANNER_H_ + +#include + + +// teb stuff +#include +#include +#include +#include +#include +#include + +// g2o lib stuff +#include +#include +#include +#include +#include +#include +#include + +// messages +#include +#include +#include +#include + +#include +#include + +namespace teb_local_planner +{ + +//! Typedef for the block solver utilized for optimization +typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > TEBBlockSolver; + +//! Typedef for the linear solver utilized for optimization +typedef g2o::LinearSolverCSparse TEBLinearSolver; +//typedef g2o::LinearSolverCholmod TEBLinearSolver; + +//! Typedef for a container storing via-points +typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator > ViaPointContainer; + + +/** + * @class TebOptimalPlanner + * @brief This class optimizes an internal Timed Elastic Band trajectory using the g2o-framework. + * + * For an introduction and further details about the TEB optimization problem refer to: + * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012. + * - C. Rösmann et al.: Efficient trajectory optimization using a sparse model, ECMR, 2013. + * - R. Kümmerle et al.: G2o: A general framework for graph optimization, ICRA, 2011. + * + * @todo: Call buildGraph() only if the teb structure has been modified to speed up hot-starting from previous solutions. + * @todo: 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. + */ +class TebOptimalPlanner : public PlannerInterface +{ +public: + + /** + * @brief Default constructor + */ + TebOptimalPlanner(); + + /** + * @brief Construct and initialize the TEB optimal planner. + * @param cfg Const reference to the TebConfig class for internal parameters + * @param obstacles Container storing all relevant obstacles (see Obstacle) + * @param robot_model Shared pointer to the robot shape model used for optimization (optional) + * @param visual Shared pointer to the TebVisualization class (optional) + * @param via_points Container storing via-points (optional) + */ + TebOptimalPlanner(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared(), + TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL); + + /** + * @brief Destruct the optimal planner. + */ + virtual ~TebOptimalPlanner(); + + /** + * @brief Initializes the optimal planner + * @param cfg Const reference to the TebConfig class for internal parameters + * @param obstacles Container storing all relevant obstacles (see Obstacle) + * @param robot_model Shared pointer to the robot shape model used for optimization (optional) + * @param visual Shared pointer to the TebVisualization class (optional) + * @param via_points Container storing via-points (optional) + */ + void initialize(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared(), + TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL); + + /** + * @param robot_model Shared pointer to the robot shape model used for optimization (optional) + */ + void updateRobotModel(RobotFootprintModelPtr robot_model ); + + /** @name Plan a trajectory */ + //@{ + + /** + * @brief Plan a trajectory based on an initial reference plan. + * + * Call this method to create and optimize a trajectory that is initialized + * according to an initial reference plan (given as a container of poses). \n + * The method supports hot-starting from previous solutions, if avaiable: \n + * - If no trajectory exist yet, a new trajectory is initialized based on the initial plan, + * see TimedElasticBand::initTEBtoGoal + * - If a previous solution is avaiable, update the trajectory based on the initial plan, + * see bool TimedElasticBand::updateAndPruneTEB + * - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o + * @param initial_plan vector of geometry_msgs::PoseStamped + * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used) + * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, + * otherwise the final velocity will be zero (default: false) + * @return \c true if planning was successful, \c false otherwise + */ + virtual bool plan(const std::vector& initial_plan, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false); + + /** + * @brief Plan a trajectory between a given start and goal pose (tf::Pose version) + * + * Call this method to create and optimize a trajectory that is initialized between a given start and goal pose. \n + * The method supports hot-starting from previous solutions, if avaiable: \n + * - If no trajectory exist yet, a new trajectory is initialized between start and goal poses, + * see TimedElasticBand::initTEBtoGoal + * - If a previous solution is avaiable, update the trajectory @see bool TimedElasticBand::updateAndPruneTEB + * - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o + * @param start tf::Pose containing the start pose of the trajectory + * @param goal tf::Pose containing the goal pose of the trajectory + * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used) + * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, + * otherwise the final velocity will be zero (default: false) + * @return \c true if planning was successful, \c false otherwise + */ + virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false); + + /** + * @brief Plan a trajectory between a given start and goal pose + * + * Call this method to create and optimize a trajectory that is initialized between a given start and goal pose. \n + * The method supports hot-starting from previous solutions, if avaiable: \n + * - If no trajectory exist yet, a new trajectory is initialized between start and goal poses + * @see TimedElasticBand::initTEBtoGoal + * - If a previous solution is avaiable, update the trajectory @see bool TimedElasticBand::updateAndPruneTEB + * - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o + * @param start PoseSE2 containing the start pose of the trajectory + * @param goal PoseSE2 containing the goal pose of the trajectory + * @param start_vel Initial velocity at the start pose (twist message containing the translational and angular velocity). + * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, + * otherwise the final velocity will be zero (default: false) + * @return \c true if planning was successful, \c false otherwise + */ + virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false); + + + /** + * @brief Get the velocity command from a previously optimized plan to control the robot at the current sampling interval. + * @warning Call plan() first and check if the generated plan is feasible. + * @param[out] vx translational velocity [m/s] + * @param[out] vy strafing velocity which can be nonzero for holonomic robots[m/s] + * @param[out] omega rotational velocity [rad/s] + * @param[in] look_ahead_poses index of the final pose used to compute the velocity command. + * @return \c true if command is valid, \c false otherwise + */ + virtual bool getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const; + + + /** + * @brief Optimize a previously initialized trajectory (actual TEB optimization loop). + * + * optimizeTEB implements the main optimization loop. \n + * It consist of two nested loops: + * - The outer loop resizes the trajectory according to the temporal resolution by invoking TimedElasticBand::autoResize(). + * Afterwards the internal method optimizeGraph() is called that constitutes the innerloop. + * - The inner loop calls the solver (g2o framework, resp. sparse Levenberg-Marquardt) and iterates a specified + * number of optimization calls (\c iterations_innerloop). + * + * The outer loop is repeated \c iterations_outerloop times. \n + * The ratio of inner and outer loop iterations significantly defines the contraction behavior + * and convergence rate of the trajectory optimization. Based on our experiences, 2-6 innerloop iterations are sufficient. \n + * The number of outer loop iterations should be determined by considering the maximum CPU time required to match the control rate. \n + * Optionally, the cost vector can be calculated by specifying \c compute_cost_afterwards, see computeCurrentCost(). + * @remarks This method is usually called from a plan() method + * @param iterations_innerloop Number of iterations for the actual solver loop + * @param iterations_outerloop Specifies how often the trajectory should be resized followed by the inner solver loop. + * @param compute_cost_afterwards if \c true Calculate the cost vector according to computeCurrentCost(), + * the vector can be accessed afterwards using getCurrentCost(). + * @param obst_cost_scale Specify extra scaling for obstacle costs (only used if \c compute_cost_afterwards is true) + * @param viapoint_cost_scale Specify extra scaling for via-point costs (only used if \c compute_cost_afterwards is true) + * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time + * (only used if \c compute_cost_afterwards is true). + * @return \c true if the optimization terminates successfully, \c false otherwise + */ + bool optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards = false, + double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false); + + //@} + + + /** @name Desired initial and final velocity */ + //@{ + + + /** + * @brief Set the initial velocity at the trajectory's start pose (e.g. the robot's velocity) [twist overload]. + * @remarks Calling this function is not neccessary if the initial velocity is passed via the plan() method + * @param vel_start Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used, + * for holonomic robots also linear.y) + */ + void setVelocityStart(const geometry_msgs::Twist& vel_start); + + /** + * @brief Set the desired final velocity at the trajectory's goal pose. + * @remarks Call this function only if a non-zero velocity is desired and if \c free_goal_vel is set to \c false in plan() + * @param vel_goal twist message containing the translational and angular final velocity + */ + void setVelocityGoal(const geometry_msgs::Twist& vel_goal); + + /** + * @brief Set the desired final velocity at the trajectory's goal pose to be the maximum velocity limit + * @remarks Calling this function is not neccessary if \c free_goal_vel is set to \c false in plan() + */ + void setVelocityGoalFree() {vel_goal_.first = false;} + + //@} + + + /** @name Take obstacles into account */ + //@{ + + + /** + * @brief Assign a new set of obstacles + * @param obst_vector pointer to an obstacle container (can also be a nullptr) + * @remarks This method overrids the obstacle container optinally assigned in the constructor. + */ + void setObstVector(ObstContainer* obst_vector) {obstacles_ = obst_vector;} + + /** + * @brief Access the internal obstacle container. + * @return Const reference to the obstacle container + */ + const ObstContainer& getObstVector() const {return *obstacles_;} + + //@} + + /** @name Take via-points into account */ + //@{ + + + /** + * @brief Assign a new set of via-points + * @param via_points pointer to a via_point container (can also be a nullptr) + * @details Any previously set container will be overwritten. + */ + void setViaPoints(const ViaPointContainer* via_points) {via_points_ = via_points;} + + /** + * @brief Access the internal via-point container. + * @return Const reference to the via-point container + */ + const ViaPointContainer& getViaPoints() const {return *via_points_;} + + //@} + + + /** @name Visualization */ + //@{ + + /** + * @brief Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence) + * @param visualization shared pointer to a TebVisualization instance + * @see visualize + */ + void setVisualization(TebVisualizationPtr visualization); + + /** + * @brief Publish the local plan and pose sequence via ros topics (e.g. subscribe with rviz). + * + * Make sure to register a TebVisualization instance before using setVisualization() or an overlaoded constructor. + * @see setVisualization + */ + virtual void visualize(); + + //@} + + + /** @name Utility methods and more */ + //@{ + + /** + * @brief Reset the planner by clearing the internal graph and trajectory. + */ + virtual void clearPlanner() + { + clearGraph(); + teb_.clearTimedElasticBand(); + } + + /** + * @brief Prefer a desired initial turning direction (by penalizing the opposing one) + * + * A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two + * solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty. + * Initial means that the penalty is applied only to the first few poses of the trajectory. + * @param dir This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none) + */ + virtual void setPreferredTurningDir(RotType dir) {prefer_rotdir_=dir;} + + /** + * @brief Register the vertices and edges defined for the TEB to the g2o::Factory. + * + * This allows the user to export the internal graph to a text file for instance. + * Access the optimizer() for more details. + */ + static void registerG2OTypes(); + + /** + * @brief Access the internal TimedElasticBand trajectory. + * @warning In general, the underlying teb must not be modified directly. Use with care... + * @return reference to the teb + */ + TimedElasticBand& teb() {return teb_;}; + + /** + * @brief Access the internal TimedElasticBand trajectory (read-only). + * @return const reference to the teb + */ + const TimedElasticBand& teb() const {return teb_;}; + + /** + * @brief Access the internal g2o optimizer. + * @warning In general, the underlying optimizer must not be modified directly. Use with care... + * @return const shared pointer to the g2o sparse optimizer + */ + boost::shared_ptr optimizer() {return optimizer_;}; + + /** + * @brief Access the internal g2o optimizer (read-only). + * @return const shared pointer to the g2o sparse optimizer + */ + boost::shared_ptr optimizer() const {return optimizer_;}; + + /** + * @brief Check if last optimization was successful + * @return \c true if the last optimization returned without errors, + * otherwise \c false (also if no optimization has been called before). + */ + bool isOptimized() const {return optimized_;}; + + /** + * @brief Returns true if the planner has diverged. + */ + bool hasDiverged() const override; + + /** + * @brief Compute the cost vector of a given optimization problen (hyper-graph must exist). + * + * Use this method to obtain information about the current edge errors / costs (local cost functions). \n + * The vector of cost values is composed according to the different edge types (time_optimal, obstacles, ...). \n + * Refer to the method declaration for the detailed composition. \n + * The cost for the edges that minimize time differences (EdgeTimeOptimal) corresponds to the sum of all single + * squared time differneces: \f$ \sum_i \Delta T_i^2 \f$. Sometimes, the user may want to get a value that is proportional + * or identical to the actual trajectory transition time \f$ \sum_i \Delta T_i \f$. \n + * Set \c alternative_time_cost to true in order to get the cost calculated using the latter equation, but check the + * implemented definition, if the value is scaled to match the magnitude of other cost values. + * + * @todo Remove the scaling term for the alternative time cost. + * @todo Can we use the last error (chi2) calculated from g2o instead of calculating it by ourself? + * @see getCurrentCost + * @see optimizeTEB + * @param obst_cost_scale Specify extra scaling for obstacle costs. + * @param viapoint_cost_scale Specify extra scaling for via points. + * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time. + * @return TebCostVec containing the cost values + */ + void computeCurrentCost(double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false); + + /** + * Compute and return the cost of the current optimization graph (supports multiple trajectories) + * @param[out] cost current cost value for each trajectory + * [for a planner with just a single trajectory: size=1, vector will not be cleared] + * @param obst_cost_scale Specify extra scaling for obstacle costs + * @param viapoint_cost_scale Specify extra scaling for via points. + * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time + */ + virtual void computeCurrentCost(std::vector& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false) + { + computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost); + cost.push_back( getCurrentCost() ); + } + + /** + * @brief Access the cost vector. + * + * The accumulated cost value previously calculated using computeCurrentCost + * or by calling optimizeTEB with enabled cost flag. + * @return const reference to the TebCostVec. + */ + double getCurrentCost() const {return cost_;} + + + /** + * @brief Extract the velocity from consecutive poses and a time difference (including strafing velocity for holonomic robots) + * + * The velocity is extracted using finite differences. + * The direction of the translational velocity is also determined. + * @param pose1 pose at time k + * @param pose2 consecutive pose at time k+1 + * @param dt actual time difference between k and k+1 (must be >0 !!!) + * @param[out] vx translational velocity + * @param[out] vy strafing velocity which can be nonzero for holonomic robots + * @param[out] omega rotational velocity + */ + inline void extractVelocity(const PoseSE2& pose1, const PoseSE2& pose2, double dt, double& vx, double& vy, double& omega) const; + + /** + * @brief Compute the velocity profile of the trajectory + * + * This method computes the translational and rotational velocity for the complete + * planned trajectory. + * The first velocity is the one that is provided as initial velocity (fixed). + * Velocities at index k=2...end-1 are related to the transition from pose_{k-1} to pose_k. + * The last velocity is the final velocity (fixed). + * The number of Twist objects is therefore sizePoses()+1; + * In summary: + * v[0] = v_start, + * v[1,...end-1] = +-(pose_{k+1}-pose{k})/dt, + * v(end) = v_goal + * It can be used for evaluation and debugging purposes or + * for open-loop control. For computing the velocity required for controlling the robot + * to the next step refer to getVelocityCommand(). + * @param[out] velocity_profile velocity profile will be written to this vector (after clearing any existing content) with the size=no_poses+1 + */ + void getVelocityProfile(std::vector& velocity_profile) const; + + /** + * @brief Return the complete trajectory including poses, velocity profiles and temporal information + * + * It is useful for evaluation and debugging purposes or for open-loop control. + * Since the velocity obtained using difference quotients is the mean velocity between consecutive poses, + * the velocity at each pose at time stamp k is obtained by taking the average between both velocities. + * The velocity of the first pose is v_start (provided initial value) and the last one is v_goal (usually zero, if free_goal_vel is off). + * See getVelocityProfile() for the list of velocities between consecutive points. + * @todo The acceleration profile is not added at the moment. + * @param[out] trajectory the resulting trajectory + */ + void getFullTrajectory(std::vector& trajectory) const; + + /** + * @brief Check whether the planned trajectory is feasible or not. + * + * This method currently checks only that the trajectory, or a part of the trajectory is collision free. + * Obstacles are here represented as costmap instead of the internal ObstacleContainer. + * @param costmap_model Pointer to the costmap model + * @param footprint_spec The specification of the footprint of the robot in world coordinates + * @param inscribed_radius The radius of the inscribed circle of the robot + * @param circumscribed_radius The radius of the circumscribed circle of the robot + * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked. + * @return \c true, if the robot footprint along the first part of the trajectory intersects with + * any obstacle in the costmap, \c false otherwise. + */ + virtual bool isTrajectoryFeasible(base_local_planner::CostmapModel* costmap_model, const std::vector& footprint_spec, double inscribed_radius = 0.0, + double circumscribed_radius=0.0, int look_ahead_idx=-1); + + //@} + +protected: + + /** @name Hyper-Graph creation and optimization */ + //@{ + + /** + * @brief Build the hyper-graph representing the TEB optimization problem. + * + * This method creates the optimization problem according to the hyper-graph formulation. \n + * For more details refer to the literature cited in the TebOptimalPlanner class description. + * @see optimizeGraph + * @see clearGraph + * @param weight_multiplier Specify a weight multipler for selected weights in optimizeGraph + * This might be used for weight adapation strategies. + * Currently, only obstacle collision weights are considered. + * @return \c true, if the graph was created successfully, \c false otherwise. + */ + bool buildGraph(double weight_multiplier=1.0); + + /** + * @brief Optimize the previously constructed hyper-graph to deform / optimize the TEB. + * + * This method invokes the g2o framework to solve the optimization problem considering dedicated sparsity patterns. \n + * The current implementation calls a non-constrained sparse Levenberg-Marquardt algorithm. Constraints are considered + * by utilizing penalty approximations. Refer to the literature cited in the TebOptimalPlanner class description. + * @see buildGraph + * @see clearGraph + * @param no_iterations Number of solver iterations + * @param clear_after Clear the graph after optimization. + * @return \c true, if optimization terminates successfully, \c false otherwise. + */ + bool optimizeGraph(int no_iterations, bool clear_after=true); + + /** + * @brief Clear an existing internal hyper-graph. + * @see buildGraph + * @see optimizeGraph + */ + void clearGraph(); + + /** + * @brief Add all relevant vertices to the hyper-graph as optimizable variables. + * + * Vertices (if unfixed) represent the variables that will be optimized. \n + * In case of the Timed-Elastic-Band poses and time differences form the vertices of the hyper-graph. \n + * The order of insertion of vertices (to the graph) is important for efficiency, + * since it affect the sparsity pattern of the underlying hessian computed for optimization. + * @see VertexPose + * @see VertexTimeDiff + * @see buildGraph + * @see optimizeGraph + */ + void AddTEBVertices(); + + /** + * @brief Add all edges (local cost functions) for limiting the translational and angular velocity. + * @see EdgeVelocity + * @see buildGraph + * @see optimizeGraph + */ + void AddEdgesVelocity(); + + /** + * @brief Add all edges (local cost functions) for limiting the translational and angular acceleration. + * @see EdgeAcceleration + * @see EdgeAccelerationStart + * @see EdgeAccelerationGoal + * @see buildGraph + * @see optimizeGraph + */ + void AddEdgesAcceleration(); + + /** + * @brief Add all edges (local cost functions) for minimizing the transition time (resp. minimize time differences) + * @see EdgeTimeOptimal + * @see buildGraph + * @see optimizeGraph + */ + void AddEdgesTimeOptimal(); + + /** + * @brief Add all edges (local cost functions) for minimizing the path length + * @see EdgeShortestPath + * @see buildGraph + * @see optimizeGraph + */ + void AddEdgesShortestPath(); + + /** + * @brief Add all edges (local cost functions) related to keeping a distance from static obstacles + * @warning do not combine with AddEdgesInflatedObstacles + * @see EdgeObstacle + * @see buildGraph + * @see optimizeGraph + * @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight) + */ + void AddEdgesObstacles(double weight_multiplier=1.0); + + /** + * @brief Add all edges (local cost functions) related to keeping a distance from static obstacles (legacy association strategy) + * @see EdgeObstacle + * @see buildGraph + * @see optimizeGraph + * @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight) + */ + void AddEdgesObstaclesLegacy(double weight_multiplier=1.0); + + /** + * @brief Add all edges (local cost functions) related to minimizing the distance to via-points + * @see EdgeViaPoint + * @see buildGraph + * @see optimizeGraph + */ + void AddEdgesViaPoints(); + + /** + * @brief Add all edges (local cost functions) related to keeping a distance from dynamic (moving) obstacles. + * @warning experimental + * @todo Should we also add neighbors to decrease jiggling/oscillations + * @see EdgeDynamicObstacle + * @see buildGraph + * @see optimizeGraph + * @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight) + + */ + void AddEdgesDynamicObstacles(double weight_multiplier=1.0); + + /** + * @brief Add all edges (local cost functions) for satisfying kinematic constraints of a differential drive robot + * @warning do not combine with AddEdgesKinematicsCarlike() + * @see AddEdgesKinematicsCarlike + * @see buildGraph + * @see optimizeGraph + */ + void AddEdgesKinematicsDiffDrive(); + + /** + * @brief Add all edges (local cost functions) for satisfying kinematic constraints of a carlike robot + * @warning do not combine with AddEdgesKinematicsDiffDrive() + * @see AddEdgesKinematicsDiffDrive + * @see buildGraph + * @see optimizeGraph + */ + void AddEdgesKinematicsCarlike(); + + /** + * @brief Add all edges (local cost functions) for prefering a specifiy turning direction (by penalizing the other one) + * @see buildGraph + * @see optimizeGraph + */ + + void AddEdgesSteeringRate(); + + void AddEdgesPreferRotDir(); + + /** + * @brief Add all edges (local cost function) for reducing the velocity of a vertex due to its associated obstacles + * @see buildGraph + * @see optimizeGraph + */ + void AddEdgesVelocityObstacleRatio(); + + //@} + + + /** + * @brief Initialize and configure the g2o sparse optimizer. + * @return shared pointer to the g2o::SparseOptimizer instance + */ + boost::shared_ptr initOptimizer(); + + + // external objects (store weak pointers) + const TebConfig* cfg_; //!< Config class that stores and manages all related parameters + ObstContainer* obstacles_; //!< Store obstacles that are relevant for planning + const ViaPointContainer* via_points_; //!< Store via points for planning + std::vector obstacles_per_vertex_; //!< Store the obstacles associated with the n-1 initial vertices + + double cost_; //!< Store cost value of the current hyper-graph + RotType prefer_rotdir_; //!< Store whether to prefer a specific initial rotation in optimization (might be activated in case the robot oscillates) + + // internal objects (memory management owned) + TebVisualizationPtr visualization_; //!< Instance of the visualization class + TimedElasticBand teb_; //!< Actual trajectory object + RobotFootprintModelPtr robot_model_; //!< Robot model + boost::shared_ptr optimizer_; //!< g2o optimizer for trajectory optimization + std::pair vel_start_; //!< Store the initial velocity at the start pose + std::pair vel_goal_; //!< Store the final velocity at the goal pose + double recent_steering_angle_ = 0.0; //!< Store last measured steering angle (for car-like setup) + + bool initialized_; //!< Keeps track about the correct initialization of this class + bool optimized_; //!< This variable is \c true as long as the last optimization has been completed successful + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +//! Abbrev. for shared instances of the TebOptimalPlanner +typedef boost::shared_ptr TebOptimalPlannerPtr; +//! Abbrev. for shared const TebOptimalPlanner pointers +typedef boost::shared_ptr TebOptimalPlannerConstPtr; +//! Abbrev. for containers storing multiple teb optimal planners +typedef std::vector< TebOptimalPlannerPtr > TebOptPlannerContainer; + +} // namespace teb_local_planner + +#endif /* OPTIMAL_PLANNER_H_ */ diff --git a/penalties.h b/penalties.h new file mode 100644 index 00000000..68705584 --- /dev/null +++ b/penalties.h @@ -0,0 +1,193 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef PENALTIES_H +#define PENALTIES_H + +#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 < 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 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; + } +} + + +/** + * @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 diff --git a/teb_config.cpp b/teb_config.cpp new file mode 100644 index 00000000..71551b6f --- /dev/null +++ b/teb_config.cpp @@ -0,0 +1,378 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#include + +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("global_plan_prune_distance", trajectory.global_plan_prune_distance, trajectory.global_plan_prune_distance); + 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("force_reinit_new_goal_angular", trajectory.force_reinit_new_goal_angular, trajectory.force_reinit_new_goal_angular); + 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); + nh.param("min_resolution_collision_check_angular", trajectory.min_resolution_collision_check_angular, trajectory.min_resolution_collision_check_angular); + nh.param("control_look_ahead_poses", trajectory.control_look_ahead_poses, trajectory.control_look_ahead_poses); + + // 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_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); + nh.param("use_proportional_saturation", robot.use_proportional_saturation, robot.use_proportional_saturation); + nh.param("transform_tolerance", robot.transform_tolerance, robot.transform_tolerance); + + // 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("trans_stopped_vel", goal_tolerance.trans_stopped_vel, goal_tolerance.trans_stopped_vel); + nh.param("theta_stopped_vel", goal_tolerance.theta_stopped_vel, goal_tolerance.theta_stopped_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); + nh.param("obstacle_proximity_ratio_max_vel", obstacles.obstacle_proximity_ratio_max_vel, obstacles.obstacle_proximity_ratio_max_vel); + nh.param("obstacle_proximity_lower_bound", obstacles.obstacle_proximity_lower_bound, obstacles.obstacle_proximity_lower_bound); + nh.param("obstacle_proximity_upper_bound", obstacles.obstacle_proximity_upper_bound, obstacles.obstacle_proximity_upper_bound); + + // 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_rate", optim.weight_max_steering_rate, optim.weight_max_steering_rate); + nh.param("weight_optimaltime", optim.weight_optimaltime, optim.weight_optimaltime); + nh.param("weight_shortest_path", optim.weight_shortest_path, optim.weight_shortest_path); + 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_velocity_obstacle_ratio", optim.weight_velocity_obstacle_ratio, optim.weight_velocity_obstacle_ratio); + 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); + nh.param("obstacle_cost_exponent", optim.obstacle_cost_exponent, optim.obstacle_cost_exponent); + + // 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("max_number_plans_in_current_class", hcp.max_number_plans_in_current_class, hcp.max_number_plans_in_current_class); + 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("selection_dropping_probability", hcp.selection_dropping_probability, hcp.selection_dropping_probability); + 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); + nh.param("delete_detours_backwards", hcp.delete_detours_backwards, hcp.delete_detours_backwards); + nh.param("detours_orientation_tolerance", hcp.detours_orientation_tolerance, hcp.detours_orientation_tolerance); + nh.param("length_start_orientation_vector", hcp.length_start_orientation_vector, hcp.length_start_orientation_vector); + nh.param("max_ratio_detours_duration_best_duration", hcp.max_ratio_detours_duration_best_duration, hcp.max_ratio_detours_duration_best_duration); + + // 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); + nh.param("divergence_detection", recovery.divergence_detection_enable, recovery.divergence_detection_enable); + nh.param("divergence_detection_max_chi_squared", recovery.divergence_detection_max_chi_squared, recovery.divergence_detection_max_chi_squared); + + 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.force_reinit_new_goal_angular = cfg.force_reinit_new_goal_angular; + 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.max_steering_rate = cfg.max_steering_rate; + robot.wheelbase = cfg.wheelbase; + robot.cmd_angle_instead_rotvel = cfg.cmd_angle_instead_rotvel; + robot.use_proportional_saturation = cfg.use_proportional_saturation; + + // 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; + goal_tolerance.trans_stopped_vel = cfg.trans_stopped_vel; + goal_tolerance.theta_stopped_vel = cfg.theta_stopped_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; + obstacles.obstacle_proximity_ratio_max_vel = cfg.obstacle_proximity_ratio_max_vel; + obstacles.obstacle_proximity_lower_bound = cfg.obstacle_proximity_lower_bound; + obstacles.obstacle_proximity_upper_bound = cfg.obstacle_proximity_upper_bound; + + // 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_max_steering_rate = cfg.weight_max_steering_rate; + optim.weight_optimaltime = cfg.weight_optimaltime; + optim.weight_shortest_path = cfg.weight_shortest_path; + 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_velocity_obstacle_ratio = cfg.weight_velocity_obstacle_ratio; + optim.weight_viapoint = cfg.weight_viapoint; + optim.weight_adapt_factor = cfg.weight_adapt_factor; + optim.obstacle_cost_exponent = cfg.obstacle_cost_exponent; + + // Homotopy Class Planner + hcp.enable_multithreading = cfg.enable_multithreading; + hcp.max_number_classes = cfg.max_number_classes; + hcp.max_number_plans_in_current_class = cfg.max_number_plans_in_current_class; + 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.selection_dropping_probability = cfg.selection_dropping_probability; + 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; + recovery.divergence_detection_enable = cfg.divergence_detection_enable; + recovery.divergence_detection_max_chi_squared = cfg.divergence_detection_max_chi_squared; + + + 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"); + + // weights + if (optim.weight_optimaltime <= 0) + ROS_WARN("TebLocalPlannerROS() Param Warning: parameter weight_optimaltime shoud be > 0 (even if weight_shortest_path is in use)"); + +} + +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 diff --git a/teb_config.h b/teb_config.h new file mode 100644 index 00000000..2490e993 --- /dev/null +++ b/teb_config.h @@ -0,0 +1,429 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef TEB_CONFIG_H_ +#define TEB_CONFIG_H_ + +#include +#include +#include +#include + +#include + + +// Definitions +#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 + + //! 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!] + double global_plan_prune_distance; //!< Distance between robot and via_points of global plan which is used for pruning + 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) + double force_reinit_new_goal_angular; //!< Reinitialize the trajectory if a previous goal is updated with an angular difference of more than the specified value in radians (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) + double min_resolution_collision_check_angular; //! Min angular resolution used during the costmap collision check. If not respected, intermediate samples are added. [rad] + int control_look_ahead_poses; //! Index of the pose used to extract the velocity command + } 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 max_steering_rate; //!< EXPERIMENTAL: Maximum steering rate of a carlike robot (THIS SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT) [deactivate: 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. + double obstacle_cost_exponent; //!< Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default) + } 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) + int max_number_plans_in_current_class; //!< Specify the maximum number of trajectories to try that are in the same homotopy class as the current trajectory (helps avoid local minima) + 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 selection_dropping_probability; //!< At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies. + 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.2 this + } hcp; + + //! Recovery/backup related parameters + struct Recovery + { + bool shrink_horizon_backup; //!< Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues. + double shrink_horizon_min_duration; //!< Specify minimum duration for the reduced horizon in case an infeasible trajectory is detected. + bool oscillation_recovery; //!< Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards) + double oscillation_v_eps; //!< Threshold for the average normalized linear velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected + double oscillation_omega_eps; //!< Threshold for the average normalized angular velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected + double oscillation_recovery_min_duration; //!< Minumum duration [sec] for which the recovery mode is activated after an oscillation is detected. + double oscillation_filter_duration; //!< Filter length/duration [sec] for the detection of oscillations + bool divergence_detection_enable; //!< True to enable divergence detection. + int divergence_detection_max_chi_squared; //!< Maximum acceptable Mahalanobis distance above which it is assumed that the optimization diverged. + } recovery; //!< Parameters related to recovery and backup strategies + + + /** + * @brief Construct the TebConfig using default values. + * @warning If the \b rosparam server or/and \b dynamic_reconfigure (rqt_reconfigure) node are used, + * the default variables will be overwritten: \n + * E.g. if \e base_local_planner is utilized as plugin for the navigation stack, the initialize() method will register a + * dynamic_reconfigure server. A subset (not all but most) of the parameters are considered for dynamic modifications. + * All parameters considered by the dynamic_reconfigure server (and their \b default values) are + * set in \e PROJECT_SRC/cfg/TebLocalPlannerReconfigure.cfg. \n + * In addition the rosparam server can be queried to get parameters e.g. defiend in a launch file. + * The plugin source (or a possible binary source) can call loadRosParamFromNodeHandle() to update the parameters. + * In \e summary, default parameters are loaded in the following order (the right one overrides the left ones): \n + * TebConfig 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.global_plan_prune_distance = 1; + trajectory.exact_arc_length = false; + trajectory.force_reinit_new_goal_dist = 1; + trajectory.force_reinit_new_goal_angular = 0.5 * M_PI; + trajectory.feasibility_check_no_poses = 5; + trajectory.publish_feedback = false; + trajectory.min_resolution_collision_check_angular = M_PI; + trajectory.control_look_ahead_poses = 1; + + // 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.max_steering_rate = 0; + robot.wheelbase = 1.0; + robot.cmd_angle_instead_rotvel = false; + robot.is_footprint_dynamic = false; + robot.use_proportional_saturation = false; + + // GoalTolerance + + goal_tolerance.xy_goal_tolerance = 0.2; + goal_tolerance.yaw_goal_tolerance = 0.2; + goal_tolerance.free_goal_vel = false; + goal_tolerance.trans_stopped_vel = 0.1; + goal_tolerance.theta_stopped_vel = 0.1; + 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; + obstacles.obstacle_proximity_ratio_max_vel = 1; + obstacles.obstacle_proximity_lower_bound = 0; + obstacles.obstacle_proximity_upper_bound = 0.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_max_steering_rate = 1; + optim.weight_optimaltime = 1; + optim.weight_shortest_path = 0; + optim.weight_obstacle = 50; + optim.weight_inflation = 0.1; + optim.weight_dynamic_obstacle = 50; + optim.weight_dynamic_obstacle_inflation = 0.1; + optim.weight_velocity_obstacle_ratio = 0; + optim.weight_viapoint = 1; + optim.weight_prefer_rotdir = 50; + + optim.weight_adapt_factor = 2.0; + optim.obstacle_cost_exponent = 1.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.selection_dropping_probability = 0.0; + + 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; + hcp.delete_detours_backwards = true; + hcp.detours_orientation_tolerance = M_PI / 2.0; + hcp.length_start_orientation_vector = 0.4; + hcp.max_ratio_detours_duration_best_duration = 3.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/vertex_pose.h b/vertex_pose.h new file mode 100644 index 00000000..eeab99a5 --- /dev/null +++ b/vertex_pose.h @@ -0,0 +1,229 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef VERTEX_POSE_H_ +#define VERTEX_POSE_H_ + +#include +#include +#include +#include + +#include + +namespace teb_local_planner +{ + +/** + * @class VertexPose + * @brief This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized via g2o + * @see PoseSE2 + * @see VertexTimeDiff + */ +class VertexPose : public g2o::BaseVertex<3, PoseSE2 > +{ +public: + + /** + * @brief Default constructor + * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] + */ + VertexPose(bool fixed = false) + { + setToOriginImpl(); + setFixed(fixed); + } + + /** + * @brief Construct pose using a given PoseSE2 + * @param pose PoseSE2 defining the pose [x, y, angle_rad] + * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] + */ + VertexPose(const PoseSE2& pose, bool fixed = false) + { + _estimate = pose; + setFixed(fixed); + } + + /** + * @brief Construct pose using a given 2D position vector and orientation + * @param position Eigen::Vector2d containing x and y coordinates + * @param theta yaw-angle + * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] + */ + VertexPose(const Eigen::Ref& position, double theta, bool fixed = false) + { + _estimate.position() = position; + _estimate.theta() = theta; + setFixed(fixed); + } + + /** + * @brief Construct pose using single components x, y, and the yaw angle + * @param x x-coordinate + * @param y y-coordinate + * @param theta yaw angle in rad + * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] + */ + VertexPose(double x, double y, double theta, bool fixed = false) + { + _estimate.x() = x; + _estimate.y() = y; + _estimate.theta() = theta; + setFixed(fixed); + } + + /** + * @brief Access the pose + * @see estimate + * @return reference to the PoseSE2 estimate + */ + inline PoseSE2& pose() {return _estimate;} + + /** + * @brief Access the pose (read-only) + * @see estimate + * @return const reference to the PoseSE2 estimate + */ + inline const PoseSE2& pose() const {return _estimate;} + + + /** + * @brief Access the 2D position part + * @see estimate + * @return reference to the 2D position part + */ + inline Eigen::Vector2d& position() {return _estimate.position();} + + /** + * @brief Access the 2D position part (read-only) + * @see estimate + * @return const reference to the 2D position part + */ + inline const Eigen::Vector2d& position() const {return _estimate.position();} + + /** + * @brief Access the x-coordinate the pose + * @return reference to the x-coordinate + */ + inline double& x() {return _estimate.x();} + + /** + * @brief Access the x-coordinate the pose (read-only) + * @return const reference to the x-coordinate + */ + inline const double& x() const {return _estimate.x();} + + /** + * @brief Access the y-coordinate the pose + * @return reference to the y-coordinate + */ + inline double& y() {return _estimate.y();} + + /** + * @brief Access the y-coordinate the pose (read-only) + * @return const reference to the y-coordinate + */ + inline const double& y() const {return _estimate.y();} + + /** + * @brief Access the orientation part (yaw angle) of the pose + * @return reference to the yaw angle + */ + inline double& theta() {return _estimate.theta();} + + /** + * @brief Access the orientation part (yaw angle) of the pose (read-only) + * @return const reference to the yaw angle + */ + inline const double& theta() const {return _estimate.theta();} + + /** + * @brief Set the underlying estimate (2D vector) to zero. + */ + virtual void setToOriginImpl() override + { + _estimate.setZero(); + } + + /** + * @brief Define the update increment \f$ \mathbf{x}_{k+1} = \mathbf{x}_k + \Delta \mathbf{x} \f$. + * A simple addition for the position. + * The angle is first added to the previous estimated angle and afterwards normalized to the interval \f$ [-\pi \pi] \f$ + * @param update increment that should be added to the previous esimate + */ + virtual void oplusImpl(const double* update) override + { + _estimate.plus(update); + } + + /** + * @brief Read an estimate from an input stream. + * First the x-coordinate followed by y and the yaw angle. + * @param is input stream + * @return always \c true + */ + virtual bool read(std::istream& is) override + { + is >> _estimate.x() >> _estimate.y() >> _estimate.theta(); + return true; + } + + /** + * @brief Write the estimate to an output stream + * First the x-coordinate followed by y and the yaw angle. + * @param os output stream + * @return \c true if the export was successful, otherwise \c false + */ + virtual bool write(std::ostream& os) const override + { + os << _estimate.x() << " " << _estimate.y() << " " << _estimate.theta(); + return os.good(); + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} + +#endif // VERTEX_POSE_H_ diff --git a/vertex_timediff.h b/vertex_timediff.h new file mode 100644 index 00000000..4eead6c4 --- /dev/null +++ b/vertex_timediff.h @@ -0,0 +1,145 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef VERTEX_TIMEDIFF_H +#define VERTEX_TIMEDIFF_H + + +#include "g2o/config.h" +#include "g2o/core/base_vertex.h" +#include "g2o/core/hyper_graph_action.h" + +#include + +namespace teb_local_planner +{ + +/** + * @class VertexTimeDiff + * @brief This class stores and wraps a time difference \f$ \Delta T \f$ into a vertex that can be optimized via g2o + */ +class VertexTimeDiff : public g2o::BaseVertex<1, double> +{ +public: + + /** + * @brief Default constructor + * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] + */ + VertexTimeDiff(bool fixed = false) + { + setToOriginImpl(); + setFixed(fixed); + } + + /** + * @brief Construct the TimeDiff vertex with a value + * @param dt time difference value of the vertex + * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] + */ + VertexTimeDiff(double dt, bool fixed = false) + { + _estimate = dt; + setFixed(fixed); + } + + /** + * @brief Access the timediff value of the vertex + * @see estimate + * @return reference to dt + */ + inline double& dt() {return _estimate;} + + /** + * @brief Access the timediff value of the vertex (read-only) + * @see estimate + * @return const reference to dt + */ + inline const double& dt() const {return _estimate;} + + /** + * @brief Set the underlying TimeDiff estimate \f$ \Delta T \f$ to default. + */ + virtual void setToOriginImpl() override + { + _estimate = 0.1; + } + + /** + * @brief Define the update increment \f$ \Delta T_{k+1} = \Delta T_k + update \f$. + * A simple addition implements what we want. + * @param update increment that should be added to the previous esimate + */ + virtual void oplusImpl(const double* update) override + { + _estimate += *update; + } + + /** + * @brief Read an estimate of \f$ \Delta T \f$ from an input stream + * @param is input stream + * @return always \c true + */ + virtual bool read(std::istream& is) override + { + is >> _estimate; + return true; + } + + /** + * @brief Write the estimate \f$ \Delta T \f$ to an output stream + * @param os output stream + * @return \c true if the export was successful, otherwise \c false + */ + virtual bool write(std::ostream& os) const override + { + os << estimate(); + return os.good(); + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} + +#endif diff --git a/visualize_vel_and_steering.py b/visualize_vel_and_steering.py new file mode 100644 index 00000000..34157f1d --- /dev/null +++ b/visualize_vel_and_steering.py @@ -0,0 +1,90 @@ +#!/usr/bin/env python + +# This small script subscribes to the FeedbackMsg message of teb_local_planner +# and plots the current velocity. +# publish_feedback must be turned on such that the planner publishes this information. +# Author: christoph.roesmann@tu-dortmund.de + +import rospy, math +from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg +from geometry_msgs.msg import PolygonStamped, Point32 +import numpy as np +import matplotlib.pyplot as plotter + +def feedback_callback(data): + global trajectory + + if not data.trajectories: # empty + trajectory = [] + return + trajectory = data.trajectories[data.selected_trajectory_idx].trajectory + + +def plot_velocity_profile(fig, ax_v, ax_omega, ax_steering, t, v, omega, steering): + ax_v.cla() + ax_v.grid() + ax_v.set_ylabel('Trans. velocity [m/s]') + ax_v.plot(t, v, '-bx') + ax_omega.cla() + ax_omega.grid() + ax_omega.set_ylabel('Rot. velocity [rad/s]') + ax_omega.set_xlabel('Time [s]') + ax_omega.plot(t, omega, '-bx') + ax_steering.cla() + ax_steering.grid() + ax_steering.set_ylabel('Steering angle [rad]') + ax_steering.set_xlabel('Time [s]') + ax_steering.plot(t, steering, '-bx') + fig.canvas.draw() + + + +def velocity_plotter(): + global trajectory + rospy.init_node("visualize_vel_and_steering", anonymous=True) + + topic_name = "/test_optim_node/teb_feedback" + topic_name = rospy.get_param('~feedback_topic', topic_name) + rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) # define feedback topic here! + + wheelbase = 1.0 + wheelbase = rospy.get_param('~wheelbase', wheelbase) + + rospy.loginfo("Visualizing velocity profile published on '%s'.",topic_name) + rospy.loginfo("Make sure to enable rosparam 'publish_feedback' in the teb_local_planner.") + + # two subplots sharing the same t axis + fig, (ax_v, ax_omega, ax_steering) = plotter.subplots(3, sharex=True) + plotter.ion() + plotter.show() + + + r = rospy.Rate(2) # define rate here + while not rospy.is_shutdown(): + + t = [] + v = [] + omega = [] + steering = [] + + for point in trajectory: + t.append(point.time_from_start.to_sec()) + v.append(point.velocity.linear.x) + omega.append(point.velocity.angular.z) + if point.velocity.linear.x == 0: + steering.append( 0.0 ) + else: + steering.append( math.atan( wheelbase / point.velocity.linear.x * point.velocity.angular.z ) ) + + plot_velocity_profile(fig, ax_v, ax_omega, ax_steering, np.asarray(t), np.asarray(v), np.asarray(omega), np.asarray(steering)) + + r.sleep() + +if __name__ == '__main__': + try: + trajectory = [] + velocity_plotter() + except rospy.ROSInterruptException: + pass + + From 0f7b7936c2579967e48ed8f9fcf910256c5ff1cb Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 06:46:16 -0400 Subject: [PATCH 02/46] Delete TebLocalPlannerReconfigure.cfg --- cfg/TebLocalPlannerReconfigure.cfg | 432 ----------------------------- 1 file changed, 432 deletions(-) delete mode 100755 cfg/TebLocalPlannerReconfigure.cfg diff --git a/cfg/TebLocalPlannerReconfigure.cfg b/cfg/TebLocalPlannerReconfigure.cfg deleted file mode 100755 index c60e9104..00000000 --- a/cfg/TebLocalPlannerReconfigure.cfg +++ /dev/null @@ -1,432 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import * -#from local_planner_limits import add_generic_localplanner_params - -gen = ParameterGenerator() - -# This unusual line allows to reuse existing parameter definitions -# that concern all localplanners -#add_generic_localplanner_params(gen) - -# For integers and doubles: -# Name Type Reconfiguration level -# Description -# Default Min Max - - -grp_trajectory = gen.add_group("Trajectory", type="tab") - -# Trajectory -grp_trajectory.add("teb_autosize", bool_t, 0, - "Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended)", - True) - -grp_trajectory.add("dt_ref", double_t, 0, - "Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)", - 0.3, 0.01, 1) - -grp_trajectory.add("dt_hysteresis", double_t, 0, - "Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref", - 0.1, 0.002, 0.5) - -grp_trajectory.add("global_plan_overwrite_orientation", bool_t, 0, - "Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically", - True) - -grp_trajectory.add("allow_init_with_backwards_motion", bool_t, 0, - "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)", - False) - -grp_trajectory.add("max_global_plan_lookahead_dist", double_t, 0, - "Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]", - 3.0, 0, 50.0) - -grp_trajectory.add("force_reinit_new_goal_dist", double_t, 0, - "Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)", - 1.0, 0.0, 10.0) - -grp_trajectory.add("force_reinit_new_goal_angular", double_t, 0, - "Force the planner to reinitialize the trajectory if a previous goal is updated with a rotational difference of more than the specified value in radians (skip hot-starting)", - 0.78, 0.0, 4.0) - -grp_trajectory.add("feasibility_check_no_poses", int_t, 0, - "Specify up to which pose on the predicted plan the feasibility should be checked each sampling interval", - 5, 0, 50) - -grp_trajectory.add("exact_arc_length", bool_t, 0, - "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.", - False) - -grp_trajectory.add("publish_feedback", bool_t, 0, - "Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)", - False) - -grp_trajectory.add("visualize_with_time_as_z_axis_scale", double_t, 0, - "If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.", - 0, 0, 1) - -# ViaPoints -grp_viapoints = gen.add_group("ViaPoints", type="tab") - -grp_viapoints.add("global_plan_viapoint_sep", double_t, 0, - "Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]", - -0.1, -0.1, 5.0) - -grp_viapoints.add("via_points_ordered", bool_t, 0, - "If true, the planner adheres to the order of via-points in the storage container", - False) - -# Robot -grp_robot = gen.add_group("Robot", type="tab") - -grp_robot.add("max_vel_x", double_t, 0, - "Maximum translational velocity of the robot", - 0.4, 0.01, 100) - -grp_robot.add("max_vel_x_backwards", double_t, 0, - "Maximum translational velocity of the robot for driving backwards", - 0.2, 0.01, 100) - -grp_robot.add("max_vel_theta", double_t, 0, - "Maximum angular velocity of the robot", - 0.3, 0.01, 100) - -grp_robot.add("acc_lim_x", double_t, 0, - "Maximum translational acceleration of the robot", - 0.5, 0.01, 100) - -grp_robot.add("acc_lim_theta", double_t, 0, - "Maximum angular acceleration of the robot", - 0.5, 0.01, 100) - -grp_robot.add("is_footprint_dynamic", bool_t, 0, - "If true, updated the footprint before checking trajectory feasibility", - False) - -grp_robot.add("use_proportional_saturation", bool_t, 0, - "If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually", - False) -grp_robot.add("transform_tolerance", double_t, 0, - "Tolerance when querying the TF Tree for a transformation (seconds)", - 0.5, 0.001, 20) - -# Robot/Carlike - -grp_robot_carlike = grp_robot.add_group("Carlike", type="hide") - -grp_robot_carlike.add("min_turning_radius", double_t, 0, - "Minimum turning radius of a carlike robot (diff-drive robot: zero)", - 0.0, 0.0, 50.0) - -grp_robot_carlike.add("wheelbase", double_t, 0, - "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!", - 1.0, -10.0, 10.0) - -grp_robot_carlike.add("cmd_angle_instead_rotvel", bool_t, 0, - "Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')", - False) - -# Robot/Omni - -grp_robot_omni = grp_robot.add_group("Omnidirectional", type="hide") - -grp_robot_omni.add("max_vel_y", double_t, 0, - "Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)", - 0.0, 0.0, 100) - -grp_robot_omni.add("acc_lim_y", double_t, 0, - "Maximum strafing acceleration of the robot", - 0.5, 0.01, 100) - -# GoalTolerance -grp_goal = gen.add_group("GoalTolerance", type="tab") - -grp_goal.add("xy_goal_tolerance", double_t, 0, - "Allowed final euclidean distance to the goal position", - 0.2, 0.001, 10) - -grp_goal.add("yaw_goal_tolerance", double_t, 0, - "Allowed final orientation error to the goal orientation", - 0.1, 0.001, 3.2) - -grp_goal.add("free_goal_vel", bool_t, 0, - "Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed)", - False) - -grp_goal.add("trans_stopped_vel", double_t, 0, - "Below what maximum velocity we consider the robot to be stopped in translation", - 0.1, 0.0) - -grp_goal.add("theta_stopped_vel", double_t, 0, - "Below what maximum rotation velocity we consider the robot to be stopped in rotation", - 0.1, 0.0) - -# Obstacles -grp_obstacles = gen.add_group("Obstacles", type="tab") - -grp_obstacles.add("min_obstacle_dist", double_t, 0, - "Minimum desired separation from obstacles", - 0.5, 0, 10) - -grp_obstacles.add("inflation_dist", double_t, 0, - "Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)", - 0.6, 0, 15) - -grp_obstacles.add("dynamic_obstacle_inflation_dist", double_t, 0, - "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)", - 0.6, 0, 15) - -grp_obstacles.add("include_dynamic_obstacles", bool_t, 0, - "Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static.", - False) - -grp_obstacles.add("include_costmap_obstacles", bool_t, 0, - "Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)", - True) - -grp_obstacles.add("legacy_obstacle_association", bool_t, 0, - "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).", - False) - -grp_obstacles.add("obstacle_association_force_inclusion_factor", double_t, 0, - "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.", - 1.5, 0.0, 100.0) - -grp_obstacles.add("obstacle_association_cutoff_factor", double_t, 0, - "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.", - 5.0, 1.0, 100.0) - -grp_obstacles.add("costmap_obstacles_behind_robot_dist", double_t, 0, - "Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)", - 1.5, 0.0, 20.0) - -grp_obstacles.add("obstacle_poses_affected", int_t, 0, - "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", - 30, 0, 200) - -# Obstacle - Velocity ratio parameters -grp_obstacles_velocity_limit = grp_obstacles.add_group("Reduce velocity near obstacles") - -grp_obstacles_velocity_limit.add("obstacle_proximity_ratio_max_vel", double_t, 0, - "Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to static obstacles", - 1, 0, 1) - -grp_obstacles_velocity_limit.add("obstacle_proximity_lower_bound", double_t, 0, - "Distance to a static obstacle for which the velocity should be lower", - 0, 0, 10) - -grp_obstacles_velocity_limit.add("obstacle_proximity_upper_bound", double_t, 0, - "Distance to a static obstacle for which the velocity should be higher", - 0.5, 0, 10) - -# Optimization -grp_optimization = gen.add_group("Optimization", type="tab") - -grp_optimization.add("no_inner_iterations", int_t, 0, - "Number of solver iterations called in each outerloop iteration", - 5, 1, 100) - -grp_optimization.add("no_outer_iterations", int_t, 0, - "Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations", - 4, 1, 100) - -grp_optimization.add("optimization_activate", bool_t, 0, - "Activate the optimization", - True) - -grp_optimization.add("optimization_verbose", bool_t, 0, - "Print verbose information", - False) - -grp_optimization.add("penalty_epsilon", double_t, 0, - "Add a small safty margin to penalty functions for hard-constraint approximations", - 0.1, 0, 1.0) - -grp_optimization.add("weight_max_vel_x", double_t, 0, - "Optimization weight for satisfying the maximum allowed translational velocity", - 2, 0, 1000) - -grp_optimization.add("weight_max_vel_y", double_t, 0, - "Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)", - 2, 0, 1000) - -grp_optimization.add("weight_max_vel_theta", double_t, 0, - "Optimization weight for satisfying the maximum allowed angular velocity", - 1, 0, 1000) - -grp_optimization.add("weight_acc_lim_x", double_t, 0, - "Optimization weight for satisfying the maximum allowed translational acceleration", - 1, 0, 1000) - -grp_optimization.add("weight_acc_lim_y", double_t, 0, - "Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)", - 1, 0, 1000) - -grp_optimization.add("weight_acc_lim_theta", double_t, 0, - "Optimization weight for satisfying the maximum allowed angular acceleration", - 1, 0, 1000) - -grp_optimization.add("weight_kinematics_nh", double_t, 0, - "Optimization weight for satisfying the non-holonomic kinematics", - 1000 , 0, 10000) - -grp_optimization.add("weight_kinematics_forward_drive", double_t, 0, - "Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)", - 1, 0, 1000) - -grp_optimization.add("weight_kinematics_turning_radius", double_t, 0, - "Optimization weight for enforcing a minimum turning radius (carlike robots)", - 1, 0, 1000) - -grp_optimization.add("weight_optimaltime", double_t, 0, - "Optimization weight for contracting the trajectory w.r.t. transition time", - 1, 0, 1000) - -grp_optimization.add("weight_shortest_path", double_t, 0, - "Optimization weight for contracting the trajectory w.r.t. path length", - 0, 0, 100) - -grp_optimization.add("weight_obstacle", double_t, 0, - "Optimization weight for satisfying a minimum seperation from obstacles", - 50, 0, 1000) - -grp_optimization.add("weight_inflation", double_t, 0, - "Optimization weight for the inflation penalty (should be small)", - 0.1, 0, 10) - -grp_optimization.add("weight_dynamic_obstacle", double_t, 0, - "Optimization weight for satisfying a minimum seperation from dynamic obstacles", - 50, 0, 1000) - -grp_optimization.add("weight_dynamic_obstacle_inflation", double_t, 0, - "Optimization weight for the inflation penalty of dynamic obstacles (should be small)", - 0.1, 0, 10) - -grp_optimization.add("weight_velocity_obstacle_ratio", double_t, 0, - "Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle", - 0, 0, 1000) - -grp_optimization.add("weight_viapoint", double_t, 0, - "Optimization weight for minimizing the distance to via-points", - 1, 0, 1000) - -grp_optimization.add("weight_adapt_factor", double_t, 0, - "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.", - 2, 1, 100) - -grp_optimization.add("obstacle_cost_exponent", double_t, 0, - "Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)", - 1, 0.01, 100) - - - -# Homotopy Class Planner -grp_hcp = gen.add_group("HCPlanning", type="tab") - -grp_hcp.add("enable_multithreading", bool_t, 0, - "Activate multiple threading for planning multiple trajectories in parallel", - True) - -grp_hcp.add("max_number_classes", int_t, 0, - "Specify the maximum number of allowed alternative homotopy classes (limits computational effort)", - 5, 1, 100) - -grp_hcp.add("max_number_plans_in_current_class", int_t, 0, - "Max number of trajectories to try that are in the same homotopy class as the current best trajectory (setting this to 2 or more helps avoid local minima). Must be <= max_number_classes", - 1, 1, 10) - -grp_hcp.add("selection_cost_hysteresis", double_t, 0, - "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)", - 1.0, 0, 2) - - -grp_hcp.add("selection_prefer_initial_plan", double_t, 0, - "Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.)", - 0.95, 0, 1) - -grp_hcp.add("selection_obst_cost_scale", double_t, 0, - "Extra scaling of obstacle cost terms just for selecting the 'best' candidate (new_obst_cost: obst_cost*factor)", - 2.0, 0, 1000) - -grp_hcp.add("selection_viapoint_cost_scale", double_t, 0, - "Extra scaling of via-point cost terms just for selecting the 'best' candidate. (new_viapt_cost: viapt_cost*factor)", - 1.0, 0, 100) - -grp_hcp.add("selection_alternative_time_cost", bool_t, 0, - "If true, time cost is replaced by the total transition time.", - False) - -grp_hcp.add("selection_dropping_probability", double_t, 0, - "At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies.", - 0.0, 0.0, 1.0) - -grp_hcp.add("switching_blocking_period", double_t, 0, - "Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed", - 0.0, 0.0, 60) - -grp_hcp.add("roadmap_graph_no_samples", int_t, 0, - "Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off", - 15, 1, 100) - -grp_hcp.add("roadmap_graph_area_width", double_t, 0, - "Specify the width of the area in which sampled will be generated between start and goal [m] (the height equals the start-goal distance)", - 5, 0.1, 20) - -grp_hcp.add("roadmap_graph_area_length_scale", double_t, 0, - "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!)", - 1.0, 0.5, 2) - -grp_hcp.add("h_signature_prescaler", double_t, 0, - "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.2 Date: Mon, 10 May 2021 06:46:51 -0400 Subject: [PATCH 03/46] Add files via upload --- cfg/TebLocalPlannerReconfigure.cfg | 440 +++++++++++++++++++++++++++++ 1 file changed, 440 insertions(+) create mode 100644 cfg/TebLocalPlannerReconfigure.cfg diff --git a/cfg/TebLocalPlannerReconfigure.cfg b/cfg/TebLocalPlannerReconfigure.cfg new file mode 100644 index 00000000..5cf30257 --- /dev/null +++ b/cfg/TebLocalPlannerReconfigure.cfg @@ -0,0 +1,440 @@ +#!/usr/bin/env python + +from dynamic_reconfigure.parameter_generator_catkin import * +#from local_planner_limits import add_generic_localplanner_params + +gen = ParameterGenerator() + +# This unusual line allows to reuse existing parameter definitions +# that concern all localplanners +#add_generic_localplanner_params(gen) + +# For integers and doubles: +# Name Type Reconfiguration level +# Description +# Default Min Max + + +grp_trajectory = gen.add_group("Trajectory", type="tab") + +# Trajectory +grp_trajectory.add("teb_autosize", bool_t, 0, + "Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended)", + True) + +grp_trajectory.add("dt_ref", double_t, 0, + "Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)", + 0.3, 0.01, 1) + +grp_trajectory.add("dt_hysteresis", double_t, 0, + "Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref", + 0.1, 0.002, 0.5) + +grp_trajectory.add("global_plan_overwrite_orientation", bool_t, 0, + "Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically", + True) + +grp_trajectory.add("allow_init_with_backwards_motion", bool_t, 0, + "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)", + False) + +grp_trajectory.add("max_global_plan_lookahead_dist", double_t, 0, + "Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]", + 3.0, 0, 50.0) + +grp_trajectory.add("force_reinit_new_goal_dist", double_t, 0, + "Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)", + 1.0, 0.0, 10.0) + +grp_trajectory.add("force_reinit_new_goal_angular", double_t, 0, + "Force the planner to reinitialize the trajectory if a previous goal is updated with a rotational difference of more than the specified value in radians (skip hot-starting)", + 0.78, 0.0, 4.0) + +grp_trajectory.add("feasibility_check_no_poses", int_t, 0, + "Specify up to which pose on the predicted plan the feasibility should be checked each sampling interval", + 5, 0, 50) + +grp_trajectory.add("exact_arc_length", bool_t, 0, + "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.", + False) + +grp_trajectory.add("publish_feedback", bool_t, 0, + "Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)", + False) + +grp_trajectory.add("visualize_with_time_as_z_axis_scale", double_t, 0, + "If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.", + 0, 0, 1) + +# ViaPoints +grp_viapoints = gen.add_group("ViaPoints", type="tab") + +grp_viapoints.add("global_plan_viapoint_sep", double_t, 0, + "Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]", + -0.1, -0.1, 5.0) + +grp_viapoints.add("via_points_ordered", bool_t, 0, + "If true, the planner adheres to the order of via-points in the storage container", + False) + +# Robot +grp_robot = gen.add_group("Robot", type="tab") + +grp_robot.add("max_vel_x", double_t, 0, + "Maximum translational velocity of the robot", + 0.4, 0.01, 100) + +grp_robot.add("max_vel_x_backwards", double_t, 0, + "Maximum translational velocity of the robot for driving backwards", + 0.2, 0.01, 100) + +grp_robot.add("max_vel_theta", double_t, 0, + "Maximum angular velocity of the robot", + 0.3, 0.01, 100) + +grp_robot.add("acc_lim_x", double_t, 0, + "Maximum translational acceleration of the robot", + 0.5, 0.01, 100) + +grp_robot.add("acc_lim_theta", double_t, 0, + "Maximum angular acceleration of the robot", + 0.5, 0.01, 100) + +grp_robot.add("is_footprint_dynamic", bool_t, 0, + "If true, updated the footprint before checking trajectory feasibility", + False) + +grp_robot.add("use_proportional_saturation", bool_t, 0, + "If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually", + False) +grp_robot.add("transform_tolerance", double_t, 0, + "Tolerance when querying the TF Tree for a transformation (seconds)", + 0.5, 0.001, 20) + +# Robot/Carlike + +grp_robot_carlike = grp_robot.add_group("Carlike", type="hide") + +grp_robot_carlike.add("min_turning_radius", double_t, 0, + "Minimum turning radius of a carlike robot (diff-drive robot: zero)", + 0.0, 0.0, 50.0) + +grp_robot_carlike.add("max_steering_rate", double_t, 0, + "EXPERIMENTAL: Maximum steering rate of a carlike robot (THIS SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT) [deactivate: zero]", + 0.0, 1.0, 10.0) + +grp_robot_carlike.add("wheelbase", double_t, 0, + "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!", + 1.0, -10.0, 10.0) + +grp_robot_carlike.add("cmd_angle_instead_rotvel", bool_t, 0, + "Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')", + False) + +# Robot/Omni + +grp_robot_omni = grp_robot.add_group("Omnidirectional", type="hide") + +grp_robot_omni.add("max_vel_y", double_t, 0, + "Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)", + 0.0, 0.0, 100) + +grp_robot_omni.add("acc_lim_y", double_t, 0, + "Maximum strafing acceleration of the robot", + 0.5, 0.01, 100) + +# GoalTolerance +grp_goal = gen.add_group("GoalTolerance", type="tab") + +grp_goal.add("xy_goal_tolerance", double_t, 0, + "Allowed final euclidean distance to the goal position", + 0.2, 0.001, 10) + +grp_goal.add("yaw_goal_tolerance", double_t, 0, + "Allowed final orientation error to the goal orientation", + 0.1, 0.001, 3.2) + +grp_goal.add("free_goal_vel", bool_t, 0, + "Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed)", + False) + +grp_goal.add("trans_stopped_vel", double_t, 0, + "Below what maximum velocity we consider the robot to be stopped in translation", + 0.1, 0.0) + +grp_goal.add("theta_stopped_vel", double_t, 0, + "Below what maximum rotation velocity we consider the robot to be stopped in rotation", + 0.1, 0.0) + +# Obstacles +grp_obstacles = gen.add_group("Obstacles", type="tab") + +grp_obstacles.add("min_obstacle_dist", double_t, 0, + "Minimum desired separation from obstacles", + 0.5, 0, 10) + +grp_obstacles.add("inflation_dist", double_t, 0, + "Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)", + 0.6, 0, 15) + +grp_obstacles.add("dynamic_obstacle_inflation_dist", double_t, 0, + "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)", + 0.6, 0, 15) + +grp_obstacles.add("include_dynamic_obstacles", bool_t, 0, + "Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static.", + False) + +grp_obstacles.add("include_costmap_obstacles", bool_t, 0, + "Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)", + True) + +grp_obstacles.add("legacy_obstacle_association", bool_t, 0, + "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).", + False) + +grp_obstacles.add("obstacle_association_force_inclusion_factor", double_t, 0, + "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.", + 1.5, 0.0, 100.0) + +grp_obstacles.add("obstacle_association_cutoff_factor", double_t, 0, + "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.", + 5.0, 1.0, 100.0) + +grp_obstacles.add("costmap_obstacles_behind_robot_dist", double_t, 0, + "Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)", + 1.5, 0.0, 20.0) + +grp_obstacles.add("obstacle_poses_affected", int_t, 0, + "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", + 30, 0, 200) + +# Obstacle - Velocity ratio parameters +grp_obstacles_velocity_limit = grp_obstacles.add_group("Reduce velocity near obstacles") + +grp_obstacles_velocity_limit.add("obstacle_proximity_ratio_max_vel", double_t, 0, + "Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to static obstacles", + 1, 0, 1) + +grp_obstacles_velocity_limit.add("obstacle_proximity_lower_bound", double_t, 0, + "Distance to a static obstacle for which the velocity should be lower", + 0, 0, 10) + +grp_obstacles_velocity_limit.add("obstacle_proximity_upper_bound", double_t, 0, + "Distance to a static obstacle for which the velocity should be higher", + 0.5, 0, 10) + +# Optimization +grp_optimization = gen.add_group("Optimization", type="tab") + +grp_optimization.add("no_inner_iterations", int_t, 0, + "Number of solver iterations called in each outerloop iteration", + 5, 1, 100) + +grp_optimization.add("no_outer_iterations", int_t, 0, + "Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations", + 4, 1, 100) + +grp_optimization.add("optimization_activate", bool_t, 0, + "Activate the optimization", + True) + +grp_optimization.add("optimization_verbose", bool_t, 0, + "Print verbose information", + False) + +grp_optimization.add("penalty_epsilon", double_t, 0, + "Add a small safty margin to penalty functions for hard-constraint approximations", + 0.1, 0, 1.0) + +grp_optimization.add("weight_max_vel_x", double_t, 0, + "Optimization weight for satisfying the maximum allowed translational velocity", + 2, 0, 1000) + +grp_optimization.add("weight_max_vel_y", double_t, 0, + "Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)", + 2, 0, 1000) + +grp_optimization.add("weight_max_vel_theta", double_t, 0, + "Optimization weight for satisfying the maximum allowed angular velocity", + 1, 0, 1000) + +grp_optimization.add("weight_acc_lim_x", double_t, 0, + "Optimization weight for satisfying the maximum allowed translational acceleration", + 1, 0, 1000) + +grp_optimization.add("weight_acc_lim_y", double_t, 0, + "Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)", + 1, 0, 1000) + +grp_optimization.add("weight_acc_lim_theta", double_t, 0, + "Optimization weight for satisfying the maximum allowed angular acceleration", + 1, 0, 1000) + +grp_optimization.add("weight_kinematics_nh", double_t, 0, + "Optimization weight for satisfying the non-holonomic kinematics", + 1000 , 0, 10000) + +grp_optimization.add("weight_kinematics_forward_drive", double_t, 0, + "Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)", + 1, 0, 1000) + +grp_optimization.add("weight_kinematics_turning_radius", double_t, 0, + "Optimization weight for enforcing a minimum turning radius (carlike robots)", + 1, 0, 1000) + +grp_optimization.add("weight_max_steering_rate", double_t, 0, + "EXPERIMENTAL: Optimization weight for enforcing a minimum steering rate of a carlike robot (TRY TO KEEP THE WEIGHT LOW OR DEACTIVATE, SINCE IT SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT)", + 0.5, 10, 100) + +grp_optimization.add("weight_optimaltime", double_t, 0, + "Optimization weight for contracting the trajectory w.r.t. transition time", + 1, 0, 1000) + +grp_optimization.add("weight_shortest_path", double_t, 0, + "Optimization weight for contracting the trajectory w.r.t. path length", + 0, 0, 100) + +grp_optimization.add("weight_obstacle", double_t, 0, + "Optimization weight for satisfying a minimum seperation from obstacles", + 50, 0, 1000) + +grp_optimization.add("weight_inflation", double_t, 0, + "Optimization weight for the inflation penalty (should be small)", + 0.1, 0, 10) + +grp_optimization.add("weight_dynamic_obstacle", double_t, 0, + "Optimization weight for satisfying a minimum seperation from dynamic obstacles", + 50, 0, 1000) + +grp_optimization.add("weight_dynamic_obstacle_inflation", double_t, 0, + "Optimization weight for the inflation penalty of dynamic obstacles (should be small)", + 0.1, 0, 10) + +grp_optimization.add("weight_velocity_obstacle_ratio", double_t, 0, + "Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle", + 0, 0, 1000) + +grp_optimization.add("weight_viapoint", double_t, 0, + "Optimization weight for minimizing the distance to via-points", + 1, 0, 1000) + +grp_optimization.add("weight_adapt_factor", double_t, 0, + "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.", + 2, 1, 100) + +grp_optimization.add("obstacle_cost_exponent", double_t, 0, + "Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)", + 1, 0.01, 100) + + + +# Homotopy Class Planner +grp_hcp = gen.add_group("HCPlanning", type="tab") + +grp_hcp.add("enable_multithreading", bool_t, 0, + "Activate multiple threading for planning multiple trajectories in parallel", + True) + +grp_hcp.add("max_number_classes", int_t, 0, + "Specify the maximum number of allowed alternative homotopy classes (limits computational effort)", + 5, 1, 100) + +grp_hcp.add("max_number_plans_in_current_class", int_t, 0, + "Max number of trajectories to try that are in the same homotopy class as the current best trajectory (setting this to 2 or more helps avoid local minima). Must be <= max_number_classes", + 1, 1, 10) + +grp_hcp.add("selection_cost_hysteresis", double_t, 0, + "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)", + 1.0, 0, 2) + + +grp_hcp.add("selection_prefer_initial_plan", double_t, 0, + "Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.)", + 0.95, 0, 1) + +grp_hcp.add("selection_obst_cost_scale", double_t, 0, + "Extra scaling of obstacle cost terms just for selecting the 'best' candidate (new_obst_cost: obst_cost*factor)", + 2.0, 0, 1000) + +grp_hcp.add("selection_viapoint_cost_scale", double_t, 0, + "Extra scaling of via-point cost terms just for selecting the 'best' candidate. (new_viapt_cost: viapt_cost*factor)", + 1.0, 0, 100) + +grp_hcp.add("selection_alternative_time_cost", bool_t, 0, + "If true, time cost is replaced by the total transition time.", + False) + +grp_hcp.add("selection_dropping_probability", double_t, 0, + "At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies.", + 0.0, 0.0, 1.0) + +grp_hcp.add("switching_blocking_period", double_t, 0, + "Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed", + 0.0, 0.0, 60) + +grp_hcp.add("roadmap_graph_no_samples", int_t, 0, + "Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off", + 15, 1, 100) + +grp_hcp.add("roadmap_graph_area_width", double_t, 0, + "Specify the width of the area in which sampled will be generated between start and goal [m] (the height equals the start-goal distance)", + 5, 0.1, 20) + +grp_hcp.add("roadmap_graph_area_length_scale", double_t, 0, + "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!)", + 1.0, 0.5, 2) + +grp_hcp.add("h_signature_prescaler", double_t, 0, + "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.2 Date: Mon, 10 May 2021 06:47:49 -0400 Subject: [PATCH 04/46] Delete edge_acceleration.h --- .../g2o_types/edge_acceleration.h | 734 ------------------ 1 file changed, 734 deletions(-) delete mode 100644 include/teb_local_planner/g2o_types/edge_acceleration.h diff --git a/include/teb_local_planner/g2o_types/edge_acceleration.h b/include/teb_local_planner/g2o_types/edge_acceleration.h deleted file mode 100644 index 8eccf6de..00000000 --- a/include/teb_local_planner/g2o_types/edge_acceleration.h +++ /dev/null @@ -1,734 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef EDGE_ACCELERATION_H_ -#define EDGE_ACCELERATION_H_ - -#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 - { - 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); - - - 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 - /* - * @brief Jacobi matrix of the cost function specified in computeError(). - */ - void linearizeOplus() - { - ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()"); - const VertexPointXY* conf1 = static_cast(_vertices[0]); - const VertexPointXY* conf2 = static_cast(_vertices[1]); - const VertexPointXY* conf3 = static_cast(_vertices[2]); - const VertexTimeDiff* deltaT1 = static_cast(_vertices[3]); - const VertexTimeDiff* deltaT2 = static_cast(_vertices[4]); - const VertexOrientation* angle1 = static_cast(_vertices[5]); - const VertexOrientation* angle2 = static_cast(_vertices[6]); - const VertexOrientation* angle3 = static_cast(_vertices[7]); - - Eigen::Vector2d deltaS1 = conf2->estimate() - conf1->estimate(); - Eigen::Vector2d deltaS2 = conf3->estimate() - conf2->estimate(); - double dist1 = deltaS1.norm(); - double dist2 = deltaS2.norm(); - - double sum_time = deltaT1->estimate() + deltaT2->estimate(); - double sum_time_inv = 1 / sum_time; - double dt1_inv = 1/deltaT1->estimate(); - double dt2_inv = 1/deltaT2->estimate(); - double aux0 = 2/sum_time_inv; - double aux1 = dist1 * deltaT1->estimate(); - double aux2 = dist2 * deltaT2->estimate(); - - double vel1 = dist1 * dt1_inv; - double vel2 = dist2 * dt2_inv; - double omega1 = g2o::normalize_theta( angle2->estimate() - angle1->estimate() ) * dt1_inv; - double omega2 = g2o::normalize_theta( angle3->estimate() - angle2->estimate() ) * dt2_inv; - double acc = (vel2 - vel1) * aux0; - double omegadot = (omega2 - omega1) * aux0; - double aux3 = -acc/2; - double aux4 = -omegadot/2; - - double dev_border_acc = penaltyBoundToIntervalDerivative(acc, tebConfig.robot_acceleration_max_trans,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order); - double dev_border_omegadot = penaltyBoundToIntervalDerivative(omegadot, tebConfig.robot_acceleration_max_rot,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order); - - _jacobianOplus[0].resize(2,2); // conf1 - _jacobianOplus[1].resize(2,2); // conf2 - _jacobianOplus[2].resize(2,2); // conf3 - _jacobianOplus[3].resize(2,1); // deltaT1 - _jacobianOplus[4].resize(2,1); // deltaT2 - _jacobianOplus[5].resize(2,1); // angle1 - _jacobianOplus[6].resize(2,1); // angle2 - _jacobianOplus[7].resize(2,1); // angle3 - - if (aux1==0) aux1=1e-20; - if (aux2==0) aux2=1e-20; - - if (dev_border_acc!=0) - { - // TODO: double aux = aux0 * dev_border_acc; - // double aux123 = aux / aux1; - _jacobianOplus[0](0,0) = aux0 * deltaS1[0] / aux1 * dev_border_acc; // acc x1 - _jacobianOplus[0](0,1) = aux0 * deltaS1[1] / aux1 * dev_border_acc; // acc y1 - _jacobianOplus[1](0,0) = -aux0 * ( deltaS1[0] / aux1 + deltaS2[0] / aux2 ) * dev_border_acc; // acc x2 - _jacobianOplus[1](0,1) = -aux0 * ( deltaS1[1] / aux1 + deltaS2[1] / aux2 ) * dev_border_acc; // acc y2 - _jacobianOplus[2](0,0) = aux0 * deltaS2[0] / aux2 * dev_border_acc; // acc x3 - _jacobianOplus[2](0,1) = aux0 * deltaS2[1] / aux2 * dev_border_acc; // acc y3 - _jacobianOplus[2](0,0) = 0; - _jacobianOplus[2](0,1) = 0; - _jacobianOplus[3](0,0) = aux0 * (aux3 + vel1 * dt1_inv) * dev_border_acc; // acc deltaT1 - _jacobianOplus[4](0,0) = aux0 * (aux3 - vel2 * dt2_inv) * dev_border_acc; // acc deltaT2 - } - else - { - _jacobianOplus[0](0,0) = 0; // acc x1 - _jacobianOplus[0](0,1) = 0; // acc y1 - _jacobianOplus[1](0,0) = 0; // acc x2 - _jacobianOplus[1](0,1) = 0; // acc y2 - _jacobianOplus[2](0,0) = 0; // acc x3 - _jacobianOplus[2](0,1) = 0; // acc y3 - _jacobianOplus[3](0,0) = 0; // acc deltaT1 - _jacobianOplus[4](0,0) = 0; // acc deltaT2 - } - - if (dev_border_omegadot!=0) - { - _jacobianOplus[3](1,0) = aux0 * ( aux4 + omega1 * dt1_inv ) * dev_border_omegadot; // omegadot deltaT1 - _jacobianOplus[4](1,0) = aux0 * ( aux4 - omega2 * dt2_inv ) * dev_border_omegadot; // omegadot deltaT2 - _jacobianOplus[5](1,0) = aux0 * dt1_inv * dev_border_omegadot; // omegadot angle1 - _jacobianOplus[6](1,0) = -aux0 * ( dt1_inv + dt2_inv ) * dev_border_omegadot; // omegadot angle2 - _jacobianOplus[7](1,0) = aux0 * dt2_inv * dev_border_omegadot; // omegadot angle3 - } - else - { - _jacobianOplus[3](1,0) = 0; // omegadot deltaT1 - _jacobianOplus[4](1,0) = 0; // omegadot deltaT2 - _jacobianOplus[5](1,0) = 0; // omegadot angle1 - _jacobianOplus[6](1,0) = 0; // omegadot angle2 - _jacobianOplus[7](1,0) = 0; // omegadot angle3 - } - - _jacobianOplus[0](1,0) = 0; // omegadot x1 - _jacobianOplus[0](1,1) = 0; // omegadot y1 - _jacobianOplus[1](1,0) = 0; // omegadot x2 - _jacobianOplus[1](1,1) = 0; // omegadot y2 - _jacobianOplus[2](1,0) = 0; // omegadot x3 - _jacobianOplus[2](1,1) = 0; // omegadot y3 - _jacobianOplus[5](0,0) = 0; // acc angle1 - _jacobianOplus[6](0,0) = 0; // acc angle2 - _jacobianOplus[7](0,0) = 0; // acc angle3 - } -#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: - - /** - * @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) - { - 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); - - 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 -}; - - - - -/** - * @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: - - /** - * @brief Construct edge. - */ - EdgeAccelerationGoal() - { - _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]); - - // VELOCITY & ACCELERATION - - 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! - } - - 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(); - - _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 -}; - - - - -/** - * @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: - - /** - * @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]); - } - -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(); - - 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(); - - 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: - - /** - * @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]); - - // VELOCITY & ACCELERATION - - 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(); - - _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 - -#endif /* EDGE_ACCELERATION_H_ */ From e2b06f32bb1c22b8a00b96c0f69c86d2237e4c58 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 06:48:23 -0400 Subject: [PATCH 05/46] Add files via upload --- .../g2o_types/edge_acceleration.h | 734 ++++++++++++++++++ 1 file changed, 734 insertions(+) create mode 100644 include/teb_local_planner/g2o_types/edge_acceleration.h diff --git a/include/teb_local_planner/g2o_types/edge_acceleration.h b/include/teb_local_planner/g2o_types/edge_acceleration.h new file mode 100644 index 00000000..8eccf6de --- /dev/null +++ b/include/teb_local_planner/g2o_types/edge_acceleration.h @@ -0,0 +1,734 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef EDGE_ACCELERATION_H_ +#define EDGE_ACCELERATION_H_ + +#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 + { + 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); + + + 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 + /* + * @brief Jacobi matrix of the cost function specified in computeError(). + */ + void linearizeOplus() + { + ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()"); + const VertexPointXY* conf1 = static_cast(_vertices[0]); + const VertexPointXY* conf2 = static_cast(_vertices[1]); + const VertexPointXY* conf3 = static_cast(_vertices[2]); + const VertexTimeDiff* deltaT1 = static_cast(_vertices[3]); + const VertexTimeDiff* deltaT2 = static_cast(_vertices[4]); + const VertexOrientation* angle1 = static_cast(_vertices[5]); + const VertexOrientation* angle2 = static_cast(_vertices[6]); + const VertexOrientation* angle3 = static_cast(_vertices[7]); + + Eigen::Vector2d deltaS1 = conf2->estimate() - conf1->estimate(); + Eigen::Vector2d deltaS2 = conf3->estimate() - conf2->estimate(); + double dist1 = deltaS1.norm(); + double dist2 = deltaS2.norm(); + + double sum_time = deltaT1->estimate() + deltaT2->estimate(); + double sum_time_inv = 1 / sum_time; + double dt1_inv = 1/deltaT1->estimate(); + double dt2_inv = 1/deltaT2->estimate(); + double aux0 = 2/sum_time_inv; + double aux1 = dist1 * deltaT1->estimate(); + double aux2 = dist2 * deltaT2->estimate(); + + double vel1 = dist1 * dt1_inv; + double vel2 = dist2 * dt2_inv; + double omega1 = g2o::normalize_theta( angle2->estimate() - angle1->estimate() ) * dt1_inv; + double omega2 = g2o::normalize_theta( angle3->estimate() - angle2->estimate() ) * dt2_inv; + double acc = (vel2 - vel1) * aux0; + double omegadot = (omega2 - omega1) * aux0; + double aux3 = -acc/2; + double aux4 = -omegadot/2; + + double dev_border_acc = penaltyBoundToIntervalDerivative(acc, tebConfig.robot_acceleration_max_trans,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order); + double dev_border_omegadot = penaltyBoundToIntervalDerivative(omegadot, tebConfig.robot_acceleration_max_rot,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order); + + _jacobianOplus[0].resize(2,2); // conf1 + _jacobianOplus[1].resize(2,2); // conf2 + _jacobianOplus[2].resize(2,2); // conf3 + _jacobianOplus[3].resize(2,1); // deltaT1 + _jacobianOplus[4].resize(2,1); // deltaT2 + _jacobianOplus[5].resize(2,1); // angle1 + _jacobianOplus[6].resize(2,1); // angle2 + _jacobianOplus[7].resize(2,1); // angle3 + + if (aux1==0) aux1=1e-20; + if (aux2==0) aux2=1e-20; + + if (dev_border_acc!=0) + { + // TODO: double aux = aux0 * dev_border_acc; + // double aux123 = aux / aux1; + _jacobianOplus[0](0,0) = aux0 * deltaS1[0] / aux1 * dev_border_acc; // acc x1 + _jacobianOplus[0](0,1) = aux0 * deltaS1[1] / aux1 * dev_border_acc; // acc y1 + _jacobianOplus[1](0,0) = -aux0 * ( deltaS1[0] / aux1 + deltaS2[0] / aux2 ) * dev_border_acc; // acc x2 + _jacobianOplus[1](0,1) = -aux0 * ( deltaS1[1] / aux1 + deltaS2[1] / aux2 ) * dev_border_acc; // acc y2 + _jacobianOplus[2](0,0) = aux0 * deltaS2[0] / aux2 * dev_border_acc; // acc x3 + _jacobianOplus[2](0,1) = aux0 * deltaS2[1] / aux2 * dev_border_acc; // acc y3 + _jacobianOplus[2](0,0) = 0; + _jacobianOplus[2](0,1) = 0; + _jacobianOplus[3](0,0) = aux0 * (aux3 + vel1 * dt1_inv) * dev_border_acc; // acc deltaT1 + _jacobianOplus[4](0,0) = aux0 * (aux3 - vel2 * dt2_inv) * dev_border_acc; // acc deltaT2 + } + else + { + _jacobianOplus[0](0,0) = 0; // acc x1 + _jacobianOplus[0](0,1) = 0; // acc y1 + _jacobianOplus[1](0,0) = 0; // acc x2 + _jacobianOplus[1](0,1) = 0; // acc y2 + _jacobianOplus[2](0,0) = 0; // acc x3 + _jacobianOplus[2](0,1) = 0; // acc y3 + _jacobianOplus[3](0,0) = 0; // acc deltaT1 + _jacobianOplus[4](0,0) = 0; // acc deltaT2 + } + + if (dev_border_omegadot!=0) + { + _jacobianOplus[3](1,0) = aux0 * ( aux4 + omega1 * dt1_inv ) * dev_border_omegadot; // omegadot deltaT1 + _jacobianOplus[4](1,0) = aux0 * ( aux4 - omega2 * dt2_inv ) * dev_border_omegadot; // omegadot deltaT2 + _jacobianOplus[5](1,0) = aux0 * dt1_inv * dev_border_omegadot; // omegadot angle1 + _jacobianOplus[6](1,0) = -aux0 * ( dt1_inv + dt2_inv ) * dev_border_omegadot; // omegadot angle2 + _jacobianOplus[7](1,0) = aux0 * dt2_inv * dev_border_omegadot; // omegadot angle3 + } + else + { + _jacobianOplus[3](1,0) = 0; // omegadot deltaT1 + _jacobianOplus[4](1,0) = 0; // omegadot deltaT2 + _jacobianOplus[5](1,0) = 0; // omegadot angle1 + _jacobianOplus[6](1,0) = 0; // omegadot angle2 + _jacobianOplus[7](1,0) = 0; // omegadot angle3 + } + + _jacobianOplus[0](1,0) = 0; // omegadot x1 + _jacobianOplus[0](1,1) = 0; // omegadot y1 + _jacobianOplus[1](1,0) = 0; // omegadot x2 + _jacobianOplus[1](1,1) = 0; // omegadot y2 + _jacobianOplus[2](1,0) = 0; // omegadot x3 + _jacobianOplus[2](1,1) = 0; // omegadot y3 + _jacobianOplus[5](0,0) = 0; // acc angle1 + _jacobianOplus[6](0,0) = 0; // acc angle2 + _jacobianOplus[7](0,0) = 0; // acc angle3 + } +#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: + + /** + * @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) + { + 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); + + 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 +}; + + + + +/** + * @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: + + /** + * @brief Construct edge. + */ + EdgeAccelerationGoal() + { + _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]); + + // VELOCITY & ACCELERATION + + 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! + } + + 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(); + + _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 +}; + + + + +/** + * @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: + + /** + * @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]); + } + +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(); + + 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(); + + 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: + + /** + * @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]); + + // VELOCITY & ACCELERATION + + 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(); + + _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 + +#endif /* EDGE_ACCELERATION_H_ */ From 65beab385c7c744a68e5030ff01bfbee3f473706 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 06:49:36 -0400 Subject: [PATCH 06/46] Delete edge_velocity.h --- .../g2o_types/edge_velocity.h | 273 ------------------ 1 file changed, 273 deletions(-) delete mode 100755 include/teb_local_planner/g2o_types/edge_velocity.h diff --git a/include/teb_local_planner/g2o_types/edge_velocity.h b/include/teb_local_planner/g2o_types/edge_velocity.h deleted file mode 100755 index 47efcece..00000000 --- a/include/teb_local_planner/g2o_types/edge_velocity.h +++ /dev/null @@ -1,273 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef EDGE_VELOCITY_H -#define EDGE_VELOCITY_H - -#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) - { - 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); - - 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... - - /** - * @brief Jacobi matrix of the cost function specified in computeError(). - */ - void linearizeOplus() - { - 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]); - - Eigen::Vector2d deltaS = conf2->position() - conf1->position(); - double dist = deltaS.norm(); - double aux1 = dist*deltaT->estimate(); - double aux2 = 1/deltaT->estimate(); - - double vel = dist * aux2; - double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) * aux2; - - double dev_border_vel = penaltyBoundToIntervalDerivative(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon); - double dev_border_omega = penaltyBoundToIntervalDerivative(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon); - - _jacobianOplus[0].resize(2,3); // conf1 - _jacobianOplus[1].resize(2,3); // conf2 - _jacobianOplus[2].resize(2,1); // deltaT - -// if (aux1==0) aux1=1e-6; -// if (aux2==0) aux2=1e-6; - - if (dev_border_vel!=0) - { - double aux3 = dev_border_vel / aux1; - _jacobianOplus[0](0,0) = -deltaS[0] * aux3; // vel x1 - _jacobianOplus[0](0,1) = -deltaS[1] * aux3; // vel y1 - _jacobianOplus[1](0,0) = deltaS[0] * aux3; // vel x2 - _jacobianOplus[1](0,1) = deltaS[1] * aux3; // vel y2 - _jacobianOplus[2](0,0) = -vel * aux2 * dev_border_vel; // vel deltaT - } - else - { - _jacobianOplus[0](0,0) = 0; // vel x1 - _jacobianOplus[0](0,1) = 0; // vel y1 - _jacobianOplus[1](0,0) = 0; // vel x2 - _jacobianOplus[1](0,1) = 0; // vel y2 - _jacobianOplus[2](0,0) = 0; // vel deltaT - } - - if (dev_border_omega!=0) - { - double aux4 = aux2 * dev_border_omega; - _jacobianOplus[2](1,0) = -omega * aux4; // omega deltaT - _jacobianOplus[0](1,2) = -aux4; // omega angle1 - _jacobianOplus[1](1,2) = aux4; // omega angle2 - } - else - { - _jacobianOplus[2](1,0) = 0; // omega deltaT - _jacobianOplus[0](1,2) = 0; // omega angle1 - _jacobianOplus[1](1,2) = 0; // omega angle2 - } - - _jacobianOplus[0](1,0) = 0; // omega x1 - _jacobianOplus[0](1,1) = 0; // omega y1 - _jacobianOplus[1](1,0) = 0; // omega x2 - _jacobianOplus[1](1,1) = 0; // omega y2 - _jacobianOplus[0](0,2) = 0; // vel angle1 - _jacobianOplus[1](0,2) = 0; // vel angle2 - } -#endif -#endif - - -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(); - - _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 - -#endif From 70c802d6f1bb1d44b78366da71dd6240fc6b895d Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 06:58:54 -0400 Subject: [PATCH 07/46] Add files via upload --- .../g2o_types/edge_velocity.h | 489 ++++++++++++++++++ 1 file changed, 489 insertions(+) create mode 100644 include/teb_local_planner/g2o_types/edge_velocity.h diff --git a/include/teb_local_planner/g2o_types/edge_velocity.h b/include/teb_local_planner/g2o_types/edge_velocity.h new file mode 100644 index 00000000..b01c7e36 --- /dev/null +++ b/include/teb_local_planner/g2o_types/edge_velocity.h @@ -0,0 +1,489 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef EDGE_VELOCITY_H +#define EDGE_VELOCITY_H + +#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) + { + 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.0 * (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); + + 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... + + /** + * @brief Jacobi matrix of the cost function specified in computeError(). + */ + void linearizeOplus() + { + 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]); + + Eigen::Vector2d deltaS = conf2->position() - conf1->position(); + double dist = deltaS.norm(); + double aux1 = dist*deltaT->estimate(); + double aux2 = 1/deltaT->estimate(); + + double vel = dist * aux2; + double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) * aux2; + + double dev_border_vel = penaltyBoundToIntervalDerivative(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon); + double dev_border_omega = penaltyBoundToIntervalDerivative(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon); + + _jacobianOplus[0].resize(2,3); // conf1 + _jacobianOplus[1].resize(2,3); // conf2 + _jacobianOplus[2].resize(2,1); // deltaT + +// if (aux1==0) aux1=1e-6; +// if (aux2==0) aux2=1e-6; + + if (dev_border_vel!=0) + { + double aux3 = dev_border_vel / aux1; + _jacobianOplus[0](0,0) = -deltaS[0] * aux3; // vel x1 + _jacobianOplus[0](0,1) = -deltaS[1] * aux3; // vel y1 + _jacobianOplus[1](0,0) = deltaS[0] * aux3; // vel x2 + _jacobianOplus[1](0,1) = deltaS[1] * aux3; // vel y2 + _jacobianOplus[2](0,0) = -vel * aux2 * dev_border_vel; // vel deltaT + } + else + { + _jacobianOplus[0](0,0) = 0; // vel x1 + _jacobianOplus[0](0,1) = 0; // vel y1 + _jacobianOplus[1](0,0) = 0; // vel x2 + _jacobianOplus[1](0,1) = 0; // vel y2 + _jacobianOplus[2](0,0) = 0; // vel deltaT + } + + if (dev_border_omega!=0) + { + double aux4 = aux2 * dev_border_omega; + _jacobianOplus[2](1,0) = -omega * aux4; // omega deltaT + _jacobianOplus[0](1,2) = -aux4; // omega angle1 + _jacobianOplus[1](1,2) = aux4; // omega angle2 + } + else + { + _jacobianOplus[2](1,0) = 0; // omega deltaT + _jacobianOplus[0](1,2) = 0; // omega angle1 + _jacobianOplus[1](1,2) = 0; // omega angle2 + } + + _jacobianOplus[0](1,0) = 0; // omega x1 + _jacobianOplus[0](1,1) = 0; // omega y1 + _jacobianOplus[1](1,0) = 0; // omega x2 + _jacobianOplus[1](1,1) = 0; // omega y2 + _jacobianOplus[0](0,2) = 0; // vel angle1 + _jacobianOplus[1](0,2) = 0; // vel angle2 + } +#endif +#endif + + +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(); + + _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]); + } + + /** + * @class EdgeSteeringRate + * @brief Edge defining the cost function for limiting the steering rate w.r.t. the current wheelbase parameter + * + * The edge depends on four vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2} \Delta T_i \f$ . + * @remarks This edge requires the TebConfig::Robot::whelbase parameter to be set. + * @remarks Do not forget to call setTebConfig() + */ +class EdgeSteeringRate : public BaseTebMultiEdge<1, double> +{ +public: + + /** + * @brief Construct edge. + */ + EdgeSteeringRate() + { + this->resize(5); // 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 EdgeSteeringRate()"); + const VertexPose* conf1 = static_cast(_vertices[0]); + const VertexPose* conf2 = static_cast(_vertices[1]); + const VertexPose* conf3 = static_cast(_vertices[2]); + const VertexTimeDiff* dt1 = static_cast(_vertices[3]); + const VertexTimeDiff* dt2 = static_cast(_vertices[4]); + + Eigen::Vector2d delta_s1 = conf2->estimate().position() - conf1->estimate().position(); + Eigen::Vector2d delta_s2 = conf3->estimate().position() - conf2->estimate().position(); + double dist1 = delta_s1.norm(); + double dist2 = delta_s2.norm(); + double angle_diff1 = g2o::normalize_theta( conf2->theta() - conf1->theta() ); + double angle_diff2 = g2o::normalize_theta( conf3->theta() - conf2->theta() ); + + double phi1, phi2; + if (std::abs(dist1) < 1e-12) + { + phi1 = 0; // TODO previous phi? + //ROS_INFO("phi 1 is zero!"); + } + else + { + //dist1 *= fast_sigmoid( 100.0 * (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta())) ); // consider direction + //if (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta()) < 0) + //dist1 = -dist1; + + if (cfg_->trajectory.exact_arc_length) + phi1 = std::atan(cfg_->robot.wheelbase / dist1 * 2.0*std::sin(angle_diff1/2.0)); + else + phi1 = std::atan(cfg_->robot.wheelbase / dist1 * angle_diff1); + + // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan) + // In case if we apply the sign to the angle directly, it seems to work: + phi1 *= fast_sigmoid( 100.0 * (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta())) ); // consider direction + } + + if (std::abs(dist2) < 1e-12) + { + phi2 = phi1; + ROS_INFO("phi 2 is phi1!"); + } + else + { + //dist2 *= fast_sigmoid( 100.0 * (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta())) ); // consider direction + //if (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta()) < 0) + // dist2 = -dist2; + + if (cfg_->trajectory.exact_arc_length) + phi2 = std::atan(cfg_->robot.wheelbase / dist2 * 2.0*std::sin(angle_diff2/2.0)); + else + phi2 = std::atan(cfg_->robot.wheelbase / dist2 * angle_diff2); + + // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan). + // In case if we apply the sign to the angle directly, it seems to work: + phi2 *= fast_sigmoid( 100.0 * (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta())) ); // consider direction + } + + _error[0] = penaltyBoundToInterval(g2o::normalize_theta(phi2 - phi1)*2.0 / (dt1->dt() + dt2->dt()), cfg_->robot.max_steering_rate, 0.0); + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRate::computeError() _error[0]\n",_error[0]); + } + +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +//! Corresponds to EdgeSteeringRate but with initial steering angle for the predecessor configuration +class EdgeSteeringRateStart : public BaseTebMultiEdge<1, double> +{ +public: + + /** + * @brief Construct edge. + */ + EdgeSteeringRateStart() + { + 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 EdgeSteeringRateStart()"); + const VertexPose* conf1 = static_cast(_vertices[0]); + const VertexPose* conf2 = static_cast(_vertices[1]); + const VertexTimeDiff* dt = static_cast(_vertices[2]); + + Eigen::Vector2d delta_s = conf2->estimate().position() - conf1->estimate().position(); + double dist = delta_s.norm(); + double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); + + double phi; + if (std::abs(dist) < 1e-12) + { + ROS_INFO("Start phi equals pervious phi!"); + phi = _measurement; + } + else + { + //dist *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction + //dist *= (double)g2o::sign( delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta()) ); // consider direction + + if (cfg_->trajectory.exact_arc_length) + phi = std::atan(cfg_->robot.wheelbase / dist * 2.0*std::sin(angle_diff/2.0)); + else + phi = std::atan(cfg_->robot.wheelbase / dist * angle_diff); + + // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan). + // In case if we apply the sign to the angle directly, it seems to work: + phi *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction + } + + _error[0] = penaltyBoundToInterval(g2o::normalize_theta(phi - _measurement) / dt->dt(), cfg_->robot.max_steering_rate, 0.0); + + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRateStart::computeError() _error[0]\n",_error[0]); + } + + void setInitialSteeringAngle(double steering_angle) + { + _measurement = steering_angle; + } + +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +//! Corresponds to EdgeSteeringRate but with initial steering angle for the successor configuration +class EdgeSteeringRateGoal : public BaseTebMultiEdge<1, double> +{ +public: + + /** + * @brief Construct edge. + */ + EdgeSteeringRateGoal() + { + 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 EdgeSteeringRateGoal()"); + const VertexPose* conf1 = static_cast(_vertices[0]); + const VertexPose* conf2 = static_cast(_vertices[1]); + const VertexTimeDiff* dt = static_cast(_vertices[2]); + + Eigen::Vector2d delta_s = conf2->estimate().position() - conf1->estimate().position(); + double dist = delta_s.norm(); + double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); + + double phi; + if (std::abs(dist) < 1e-12) + { + ROS_INFO("Goal phi is zero!"); + phi = 0; + } + else + { + //dist *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction + + if (cfg_->trajectory.exact_arc_length) + phi = std::atan(cfg_->robot.wheelbase / dist * 2.0*std::sin(angle_diff/2.0)); + else + phi = std::atan(cfg_->robot.wheelbase / dist * angle_diff); + + // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan). + // In case if we apply the sign to the angle directly, it seems to work: + phi *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction + } + + _error[0] = penaltyBoundToInterval(g2o::normalize_theta(_measurement - phi) / dt->dt(), cfg_->robot.max_steering_rate, 0.0); + + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRateGoal::computeError() _error[0]\n",_error[0]); + } + + void setGoalSteeringAngle(double steering_angle) + { + _measurement = steering_angle; + } + +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + + + +} // end namespace + +#endif From 9d2a1827f64fbaf684150e0ccd9514d9d15ba31c Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 06:59:57 -0400 Subject: [PATCH 08/46] Delete misc.h --- include/teb_local_planner/misc.h | 152 ------------------------------- 1 file changed, 152 deletions(-) delete mode 100644 include/teb_local_planner/misc.h diff --git a/include/teb_local_planner/misc.h b/include/teb_local_planner/misc.h deleted file mode 100644 index 09f055af..00000000 --- a/include/teb_local_planner/misc.h +++ /dev/null @@ -1,152 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef MISC_H -#define MISC_H - -#include -#include -#include - - -namespace teb_local_planner -{ - -#define SMALL_NUM 0.00000001 - -//! Symbols for left/none/right rotations -enum class RotType { left, none, right }; - -/** - * @brief Check whether two variables (double) are close to each other - * @param a the first value to compare - * @param b the second value to compare - * @param epsilon precision threshold - * @return \c true if |a-b| < epsilon, false otherwise - */ -inline bool is_close(double a, double b, double epsilon = 1e-4) -{ - return std::fabs(a - b) < epsilon; -} - -/** - * @brief Return the average angle of an arbitrary number of given angles [rad] - * @param angles vector containing all angles - * @return average / mean angle, that is normalized to [-pi, pi] - */ -inline double average_angles(const std::vector& angles) -{ - double x=0, y=0; - for (std::vector::const_iterator it = angles.begin(); it!=angles.end(); ++it) - { - x += cos(*it); - y += sin(*it); - } - if(x == 0 && y == 0) - return 0; - else - return std::atan2(y, x); -} - -/** @brief Small helper function: check if |a|<|b| */ -inline bool smaller_than_abs(double i, double j) {return std::fabs(i) -inline double distance_points2d(const P1& point1, const P2& point2) -{ - return std::sqrt( std::pow(point2.x-point1.x,2) + std::pow(point2.y-point1.y,2) ); -} - - -/** - * @brief Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d) - * @param v1 object containing public methods x() and y() - * @param v2 object containing fields x() and y() - * @return magnitude that would result in the 3D case (along the z-axis) -*/ -template -inline double cross2d(const V1& v1, const V2& v2) -{ - return v1.x()*v2.y() - v2.x()*v1.y(); -} - -/** - * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference. - * - * Return a constant reference for boths input variants (pointer or reference). - * @remarks Makes only sense in combination with the overload getConstReference(const T& val). - * @param ptr pointer of type T - * @tparam T arbitrary type - * @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion - */ -template -inline const T& get_const_reference(const T* ptr) {return *ptr;} - -/** - * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference. - * - * Return a constant reference for boths input variants (pointer or reference). - * @remarks Makes only sense in combination with the overload getConstReference(const T* val). - * @param val - * @param dummy SFINAE helper variable - * @tparam T arbitrary type - * @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion - */ -template -inline const T& get_const_reference(const T& val, typename boost::disable_if >::type* dummy = 0) {return val;} - -} // namespace teb_local_planner - -#endif /* MISC_H */ From b514dc04926ae389d3ad783d3d231487d7584459 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:00:28 -0400 Subject: [PATCH 09/46] Add files via upload --- include/teb_local_planner/misc.h | 152 +++++++++++++++++++++++++++++++ 1 file changed, 152 insertions(+) create mode 100644 include/teb_local_planner/misc.h diff --git a/include/teb_local_planner/misc.h b/include/teb_local_planner/misc.h new file mode 100644 index 00000000..fadd60e1 --- /dev/null +++ b/include/teb_local_planner/misc.h @@ -0,0 +1,152 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef MISC_H +#define MISC_H + +#include +#include +#include + + +namespace teb_local_planner +{ + +#define SMALL_NUM 0.00000001 + +//! Symbols for left/none/right rotations +enum class RotType { left, none, right }; + +/** + * @brief Check whether two variables (double) are close to each other + * @param a the first value to compare + * @param b the second value to compare + * @param epsilon precision threshold + * @return \c true if |a-b| < epsilon, false otherwise + */ +inline bool is_close(double a, double b, double epsilon = 1e-4) +{ + return std::fabs(a - b) < epsilon; +} + +/** + * @brief Return the average angle of an arbitrary number of given angles [rad] + * @param angles vector containing all angles + * @return average / mean angle, that is normalized to [-pi, pi] + */ +inline double average_angles(const std::vector& angles) +{ + double x=0, y=0; + for (std::vector::const_iterator it = angles.begin(); it!=angles.end(); ++it) + { + x += cos(*it); + y += sin(*it); + } + if(x == 0 && y == 0) + return 0; + else + return std::atan2(y, x); +} + +/** @brief Small helper function: check if |a|<|b| */ +inline bool smaller_than_abs(double i, double j) {return std::fabs(i) +inline double distance_points2d(const P1& point1, const P2& point2) +{ + return std::sqrt( std::pow(point2.x-point1.x,2) + std::pow(point2.y-point1.y,2) ); +} + + +/** + * @brief Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d) + * @param v1 object containing public methods x() and y() + * @param v2 object containing fields x() and y() + * @return magnitude that would result in the 3D case (along the z-axis) +*/ +template +inline double cross2d(const V1& v1, const V2& v2) +{ + return v1.x()*v2.y() - v2.x()*v1.y(); +} + +/** + * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference. + * + * Return a constant reference for boths input variants (pointer or reference). + * @remarks Makes only sense in combination with the overload getConstReference(const T& val). + * @param ptr pointer of type T + * @tparam T arbitrary type + * @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion + */ +template +inline const T& get_const_reference(const T* ptr) {return *ptr;} + +/** + * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference. + * + * Return a constant reference for boths input variants (pointer or reference). + * @remarks Makes only sense in combination with the overload getConstReference(const T* val). + * @param val + * @param dummy SFINAE helper variable + * @tparam T arbitrary type + * @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion + */ +template +inline const T& get_const_reference(const T& val, typename boost::disable_if >::type* dummy = 0) {return val;} + +} // namespace teb_local_planner + +#endif /* MISC_H */ From 5b9f347aa554dcda9f0082086a426eb079887b2f Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:01:29 -0400 Subject: [PATCH 10/46] Add files via upload --- include/teb_local_planner/optimal_planner.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/include/teb_local_planner/optimal_planner.h b/include/teb_local_planner/optimal_planner.h index 28fdbb7b..e24f6f27 100644 --- a/include/teb_local_planner/optimal_planner.h +++ b/include/teb_local_planner/optimal_planner.h @@ -659,6 +659,9 @@ class TebOptimalPlanner : public PlannerInterface * @see buildGraph * @see optimizeGraph */ + + void AddEdgesSteeringRate(); + void AddEdgesPreferRotDir(); /** @@ -694,6 +697,7 @@ class TebOptimalPlanner : public PlannerInterface boost::shared_ptr optimizer_; //!< g2o optimizer for trajectory optimization std::pair vel_start_; //!< Store the initial velocity at the start pose std::pair vel_goal_; //!< Store the final velocity at the goal pose + double recent_steering_angle_ = 0.0; //!< Store last measured steering angle (for car-like setup) bool initialized_; //!< Keeps track about the correct initialization of this class bool optimized_; //!< This variable is \c true as long as the last optimization has been completed successful From f8a01f51c93eb4fd7444a1bfec4e8a50356107cc Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:02:43 -0400 Subject: [PATCH 11/46] Delete teb_config.h --- include/teb_local_planner/teb_config.h | 425 ------------------------- 1 file changed, 425 deletions(-) delete mode 100644 include/teb_local_planner/teb_config.h diff --git a/include/teb_local_planner/teb_config.h b/include/teb_local_planner/teb_config.h deleted file mode 100644 index 1d9e290f..00000000 --- a/include/teb_local_planner/teb_config.h +++ /dev/null @@ -1,425 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef TEB_CONFIG_H_ -#define TEB_CONFIG_H_ - -#include -#include -#include -#include - -#include - - -// Definitions -#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 - - //! 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!] - double global_plan_prune_distance; //!< Distance between robot and via_points of global plan which is used for pruning - 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) - double force_reinit_new_goal_angular; //!< Reinitialize the trajectory if a previous goal is updated with an angular difference of more than the specified value in radians (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) - double min_resolution_collision_check_angular; //! Min angular resolution used during the costmap collision check. If not respected, intermediate samples are added. [rad] - int control_look_ahead_poses; //! Index of the pose used to extract the velocity command - } 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. - double obstacle_cost_exponent; //!< Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default) - } 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) - int max_number_plans_in_current_class; //!< Specify the maximum number of trajectories to try that are in the same homotopy class as the current trajectory (helps avoid local minima) - 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 selection_dropping_probability; //!< At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies. - 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.2 this - } hcp; - - //! Recovery/backup related parameters - struct Recovery - { - bool shrink_horizon_backup; //!< Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues. - double shrink_horizon_min_duration; //!< Specify minimum duration for the reduced horizon in case an infeasible trajectory is detected. - bool oscillation_recovery; //!< Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards) - double oscillation_v_eps; //!< Threshold for the average normalized linear velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected - double oscillation_omega_eps; //!< Threshold for the average normalized angular velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected - double oscillation_recovery_min_duration; //!< Minumum duration [sec] for which the recovery mode is activated after an oscillation is detected. - double oscillation_filter_duration; //!< Filter length/duration [sec] for the detection of oscillations - bool divergence_detection_enable; //!< True to enable divergence detection. - int divergence_detection_max_chi_squared; //!< Maximum acceptable Mahalanobis distance above which it is assumed that the optimization diverged. - } recovery; //!< Parameters related to recovery and backup strategies - - - /** - * @brief Construct the TebConfig using default values. - * @warning If the \b rosparam server or/and \b dynamic_reconfigure (rqt_reconfigure) node are used, - * the default variables will be overwritten: \n - * E.g. if \e base_local_planner is utilized as plugin for the navigation stack, the initialize() method will register a - * dynamic_reconfigure server. A subset (not all but most) of the parameters are considered for dynamic modifications. - * All parameters considered by the dynamic_reconfigure server (and their \b default values) are - * set in \e PROJECT_SRC/cfg/TebLocalPlannerReconfigure.cfg. \n - * In addition the rosparam server can be queried to get parameters e.g. defiend in a launch file. - * The plugin source (or a possible binary source) can call loadRosParamFromNodeHandle() to update the parameters. - * In \e summary, default parameters are loaded in the following order (the right one overrides the left ones): \n - * TebConfig 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.global_plan_prune_distance = 1; - trajectory.exact_arc_length = false; - trajectory.force_reinit_new_goal_dist = 1; - trajectory.force_reinit_new_goal_angular = 0.5 * M_PI; - trajectory.feasibility_check_no_poses = 5; - trajectory.publish_feedback = false; - trajectory.min_resolution_collision_check_angular = M_PI; - trajectory.control_look_ahead_poses = 1; - - // 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; - robot.use_proportional_saturation = false; - - // GoalTolerance - - goal_tolerance.xy_goal_tolerance = 0.2; - goal_tolerance.yaw_goal_tolerance = 0.2; - goal_tolerance.free_goal_vel = false; - goal_tolerance.trans_stopped_vel = 0.1; - goal_tolerance.theta_stopped_vel = 0.1; - 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; - obstacles.obstacle_proximity_ratio_max_vel = 1; - obstacles.obstacle_proximity_lower_bound = 0; - obstacles.obstacle_proximity_upper_bound = 0.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_shortest_path = 0; - optim.weight_obstacle = 50; - optim.weight_inflation = 0.1; - optim.weight_dynamic_obstacle = 50; - optim.weight_dynamic_obstacle_inflation = 0.1; - optim.weight_velocity_obstacle_ratio = 0; - optim.weight_viapoint = 1; - optim.weight_prefer_rotdir = 50; - - optim.weight_adapt_factor = 2.0; - optim.obstacle_cost_exponent = 1.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.selection_dropping_probability = 0.0; - - 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; - hcp.delete_detours_backwards = true; - hcp.detours_orientation_tolerance = M_PI / 2.0; - hcp.length_start_orientation_vector = 0.4; - hcp.max_ratio_detours_duration_best_duration = 3.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 From 3af7869df13db49b5d79b9b7462df5a668024c71 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:02:59 -0400 Subject: [PATCH 12/46] Add files via upload --- include/teb_local_planner/teb_config.h | 429 +++++++++++++++++++++++++ 1 file changed, 429 insertions(+) create mode 100644 include/teb_local_planner/teb_config.h diff --git a/include/teb_local_planner/teb_config.h b/include/teb_local_planner/teb_config.h new file mode 100644 index 00000000..2490e993 --- /dev/null +++ b/include/teb_local_planner/teb_config.h @@ -0,0 +1,429 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef TEB_CONFIG_H_ +#define TEB_CONFIG_H_ + +#include +#include +#include +#include + +#include + + +// Definitions +#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 + + //! 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!] + double global_plan_prune_distance; //!< Distance between robot and via_points of global plan which is used for pruning + 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) + double force_reinit_new_goal_angular; //!< Reinitialize the trajectory if a previous goal is updated with an angular difference of more than the specified value in radians (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) + double min_resolution_collision_check_angular; //! Min angular resolution used during the costmap collision check. If not respected, intermediate samples are added. [rad] + int control_look_ahead_poses; //! Index of the pose used to extract the velocity command + } 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 max_steering_rate; //!< EXPERIMENTAL: Maximum steering rate of a carlike robot (THIS SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT) [deactivate: 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. + double obstacle_cost_exponent; //!< Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default) + } 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) + int max_number_plans_in_current_class; //!< Specify the maximum number of trajectories to try that are in the same homotopy class as the current trajectory (helps avoid local minima) + 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 selection_dropping_probability; //!< At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies. + 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.2 this + } hcp; + + //! Recovery/backup related parameters + struct Recovery + { + bool shrink_horizon_backup; //!< Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues. + double shrink_horizon_min_duration; //!< Specify minimum duration for the reduced horizon in case an infeasible trajectory is detected. + bool oscillation_recovery; //!< Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards) + double oscillation_v_eps; //!< Threshold for the average normalized linear velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected + double oscillation_omega_eps; //!< Threshold for the average normalized angular velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected + double oscillation_recovery_min_duration; //!< Minumum duration [sec] for which the recovery mode is activated after an oscillation is detected. + double oscillation_filter_duration; //!< Filter length/duration [sec] for the detection of oscillations + bool divergence_detection_enable; //!< True to enable divergence detection. + int divergence_detection_max_chi_squared; //!< Maximum acceptable Mahalanobis distance above which it is assumed that the optimization diverged. + } recovery; //!< Parameters related to recovery and backup strategies + + + /** + * @brief Construct the TebConfig using default values. + * @warning If the \b rosparam server or/and \b dynamic_reconfigure (rqt_reconfigure) node are used, + * the default variables will be overwritten: \n + * E.g. if \e base_local_planner is utilized as plugin for the navigation stack, the initialize() method will register a + * dynamic_reconfigure server. A subset (not all but most) of the parameters are considered for dynamic modifications. + * All parameters considered by the dynamic_reconfigure server (and their \b default values) are + * set in \e PROJECT_SRC/cfg/TebLocalPlannerReconfigure.cfg. \n + * In addition the rosparam server can be queried to get parameters e.g. defiend in a launch file. + * The plugin source (or a possible binary source) can call loadRosParamFromNodeHandle() to update the parameters. + * In \e summary, default parameters are loaded in the following order (the right one overrides the left ones): \n + * TebConfig 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.global_plan_prune_distance = 1; + trajectory.exact_arc_length = false; + trajectory.force_reinit_new_goal_dist = 1; + trajectory.force_reinit_new_goal_angular = 0.5 * M_PI; + trajectory.feasibility_check_no_poses = 5; + trajectory.publish_feedback = false; + trajectory.min_resolution_collision_check_angular = M_PI; + trajectory.control_look_ahead_poses = 1; + + // 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.max_steering_rate = 0; + robot.wheelbase = 1.0; + robot.cmd_angle_instead_rotvel = false; + robot.is_footprint_dynamic = false; + robot.use_proportional_saturation = false; + + // GoalTolerance + + goal_tolerance.xy_goal_tolerance = 0.2; + goal_tolerance.yaw_goal_tolerance = 0.2; + goal_tolerance.free_goal_vel = false; + goal_tolerance.trans_stopped_vel = 0.1; + goal_tolerance.theta_stopped_vel = 0.1; + 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; + obstacles.obstacle_proximity_ratio_max_vel = 1; + obstacles.obstacle_proximity_lower_bound = 0; + obstacles.obstacle_proximity_upper_bound = 0.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_max_steering_rate = 1; + optim.weight_optimaltime = 1; + optim.weight_shortest_path = 0; + optim.weight_obstacle = 50; + optim.weight_inflation = 0.1; + optim.weight_dynamic_obstacle = 50; + optim.weight_dynamic_obstacle_inflation = 0.1; + optim.weight_velocity_obstacle_ratio = 0; + optim.weight_viapoint = 1; + optim.weight_prefer_rotdir = 50; + + optim.weight_adapt_factor = 2.0; + optim.obstacle_cost_exponent = 1.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.selection_dropping_probability = 0.0; + + 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; + hcp.delete_detours_backwards = true; + hcp.detours_orientation_tolerance = M_PI / 2.0; + hcp.length_start_orientation_vector = 0.4; + hcp.max_ratio_detours_duration_best_duration = 3.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 From 3db0c66f9454e351a5ae2571d79872006b6a8c4d Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:04:18 -0400 Subject: [PATCH 13/46] Add files via upload --- scripts/visualize_vel_and_steering.py | 90 +++++++++++++++++++++++++++ 1 file changed, 90 insertions(+) create mode 100644 scripts/visualize_vel_and_steering.py diff --git a/scripts/visualize_vel_and_steering.py b/scripts/visualize_vel_and_steering.py new file mode 100644 index 00000000..34157f1d --- /dev/null +++ b/scripts/visualize_vel_and_steering.py @@ -0,0 +1,90 @@ +#!/usr/bin/env python + +# This small script subscribes to the FeedbackMsg message of teb_local_planner +# and plots the current velocity. +# publish_feedback must be turned on such that the planner publishes this information. +# Author: christoph.roesmann@tu-dortmund.de + +import rospy, math +from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg +from geometry_msgs.msg import PolygonStamped, Point32 +import numpy as np +import matplotlib.pyplot as plotter + +def feedback_callback(data): + global trajectory + + if not data.trajectories: # empty + trajectory = [] + return + trajectory = data.trajectories[data.selected_trajectory_idx].trajectory + + +def plot_velocity_profile(fig, ax_v, ax_omega, ax_steering, t, v, omega, steering): + ax_v.cla() + ax_v.grid() + ax_v.set_ylabel('Trans. velocity [m/s]') + ax_v.plot(t, v, '-bx') + ax_omega.cla() + ax_omega.grid() + ax_omega.set_ylabel('Rot. velocity [rad/s]') + ax_omega.set_xlabel('Time [s]') + ax_omega.plot(t, omega, '-bx') + ax_steering.cla() + ax_steering.grid() + ax_steering.set_ylabel('Steering angle [rad]') + ax_steering.set_xlabel('Time [s]') + ax_steering.plot(t, steering, '-bx') + fig.canvas.draw() + + + +def velocity_plotter(): + global trajectory + rospy.init_node("visualize_vel_and_steering", anonymous=True) + + topic_name = "/test_optim_node/teb_feedback" + topic_name = rospy.get_param('~feedback_topic', topic_name) + rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) # define feedback topic here! + + wheelbase = 1.0 + wheelbase = rospy.get_param('~wheelbase', wheelbase) + + rospy.loginfo("Visualizing velocity profile published on '%s'.",topic_name) + rospy.loginfo("Make sure to enable rosparam 'publish_feedback' in the teb_local_planner.") + + # two subplots sharing the same t axis + fig, (ax_v, ax_omega, ax_steering) = plotter.subplots(3, sharex=True) + plotter.ion() + plotter.show() + + + r = rospy.Rate(2) # define rate here + while not rospy.is_shutdown(): + + t = [] + v = [] + omega = [] + steering = [] + + for point in trajectory: + t.append(point.time_from_start.to_sec()) + v.append(point.velocity.linear.x) + omega.append(point.velocity.angular.z) + if point.velocity.linear.x == 0: + steering.append( 0.0 ) + else: + steering.append( math.atan( wheelbase / point.velocity.linear.x * point.velocity.angular.z ) ) + + plot_velocity_profile(fig, ax_v, ax_omega, ax_steering, np.asarray(t), np.asarray(v), np.asarray(omega), np.asarray(steering)) + + r.sleep() + +if __name__ == '__main__': + try: + trajectory = [] + velocity_plotter() + except rospy.ROSInterruptException: + pass + + From 95527d1c0677263ae63e820797c1771f837625a3 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:04:52 -0400 Subject: [PATCH 14/46] Delete optimal_planner.cpp --- src/optimal_planner.cpp | 1306 --------------------------------------- 1 file changed, 1306 deletions(-) delete mode 100644 src/optimal_planner.cpp diff --git a/src/optimal_planner.cpp b/src/optimal_planner.cpp deleted file mode 100644 index ce303c52..00000000 --- a/src/optimal_planner.cpp +++ /dev/null @@ -1,1306 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include - -// g2o custom edges and vertices for the TEB planner -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - - -namespace teb_local_planner -{ - -// ============== 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(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::updateRobotModel(RobotFootprintModelPtr robot_model) -{ - robot_model_ = robot_model; -} - -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; -} - -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_SHORTEST_PATH", 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(); - std::unique_ptr linear_solver(new TEBLinearSolver()); // see typedef in optimization.h - linear_solver->setBlockOrdering(true); - std::unique_ptr block_solver(new TEBBlockSolver(std::move(linear_solver))); - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(block_solver)); - - 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) - { - //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; -} - -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()) - { - teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, 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 - && fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // 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, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation, - 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); -} - -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 - && fabs(g2o::normalize_theta(goal.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // 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; - } - - optimizer_->setComputeBatchStatistics(cfg_->recovery.divergence_detection_enable); - - // 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(); - - AddEdgesShortestPath(); - - 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(); - - if (cfg_->optim.weight_velocity_obstacle_ratio > 0) - AddEdgesVelocityObstacleRatio(); - - 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 - if (optimizer_) - { - // we will delete all edges but keep the vertices. - // before doing so, we will delete the link from the vertices to the edges. - auto& vertices = optimizer_->vertices(); - for(auto& v : vertices) - v.second->edges().clear(); - - optimizer_->vertices().clear(); // necessary, 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 - obstacles_per_vertex_.resize(teb_.sizePoses()); - auto iter_obstacle = obstacles_per_vertex_.begin(); - for (int i=0; isetId(id_counter++); - optimizer_->addVertex(teb_.PoseVertex(i)); - if (teb_.sizeTimeDiffs()!=0 && isetId(id_counter++); - optimizer_->addVertex(teb_.TimeDiffVertex(i)); - } - iter_obstacle->clear(); - (iter_obstacle++)->reserve(obstacles_->size()); - } -} - - -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; - - auto iter_obstacle = obstacles_per_vertex_.begin(); - - auto create_edge = [inflated, &information, &information_inflated, this] (int index, const Obstacle* obstacle) { - if (inflated) - { - 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(), obstacle); - optimizer_->addEdge(dist_bandpt_obst); - } - else - { - 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(), obstacle); - optimizer_->addEdge(dist_bandpt_obst); - }; - }; - - // iterate all teb points, skipping the last and, if the EdgeVelocityObstacleRatio edges should not be created, the first one too - const int first_vertex = cfg_->optim.weight_velocity_obstacle_ratio == 0 ? 1 : 0; - for (int i = first_vertex; i < teb_.sizePoses() - 1; ++i) - { - double left_min_dist = std::numeric_limits::max(); - double right_min_dist = std::numeric_limits::max(); - ObstaclePtr left_obstacle; - ObstaclePtr right_obstacle; - - 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 robot model - double dist = robot_model_->calculateDistance(teb_.Pose(i), obst.get()); - - // force considering obstacle if really close to the current pose - if (dist < cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_force_inclusion_factor) - { - iter_obstacle->push_back(obst); - 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; - } - } - else - { - if (dist < right_min_dist) - { - right_min_dist = dist; - right_obstacle = obst; - } - } - } - - if (left_obstacle) - iter_obstacle->push_back(left_obstacle); - if (right_obstacle) - iter_obstacle->push_back(right_obstacle); - - // continue here to ignore obstacles for the first pose, but use them later to create the EdgeVelocityObstacleRatio edges - if (i == 0) - { - ++iter_obstacle; - continue; - } - - // create obstacle edges - for (const ObstaclePtr obst : *iter_obstacle) - create_edge(i, obst.get()); - ++iter_obstacle; - } -} - - -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(index)); - dist_bandpt_obst->setInformation(information_inflated); - 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(index)); - dist_bandpt_obst->setInformation(information); - 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::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". - } - } -} - -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); - } - } - 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) - { - 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); - } - - // now add the usual acceleration edge for each tuple of three teb poses - for (int i=0; i < n - 2; ++i) - { - 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); - } - - // 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); - } - - // 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); - } - } -} - - - -void TebOptimalPlanner::AddEdgesTimeOptimal() -{ - 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); - } -} - -void TebOptimalPlanner::AddEdgesShortestPath() -{ - if (cfg_->optim.weight_shortest_path==0) - return; // if weight equals zero skip adding edges! - - Eigen::Matrix information; - information.fill(cfg_->optim.weight_shortest_path); - - for (int i=0; i < teb_.sizePoses()-1; ++i) - { - EdgeShortestPath* shortest_path_edge = new EdgeShortestPath; - shortest_path_edge->setVertex(0,teb_.PoseVertex(i)); - shortest_path_edge->setVertex(1,teb_.PoseVertex(i+1)); - shortest_path_edge->setInformation(information); - shortest_path_edge->setTebConfig(*cfg_); - optimizer_->addEdge(shortest_path_edge); - } -} - - - -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::AddEdgesVelocityObstacleRatio() -{ - Eigen::Matrix information; - information(0,0) = cfg_->optim.weight_velocity_obstacle_ratio; - information(1,1) = cfg_->optim.weight_velocity_obstacle_ratio; - information(0,1) = information(1,0) = 0; - - auto iter_obstacle = obstacles_per_vertex_.begin(); - - for (int index = 0; index < teb_.sizePoses() - 1; ++index) - { - for (const ObstaclePtr obstacle : (*iter_obstacle++)) - { - EdgeVelocityObstacleRatio* edge = new EdgeVelocityObstacleRatio; - edge->setVertex(0,teb_.PoseVertex(index)); - edge->setVertex(1,teb_.PoseVertex(index + 1)); - edge->setVertex(2,teb_.TimeDiffVertex(index)); - edge->setInformation(information); - edge->setParameters(*cfg_, robot_model_.get(), obstacle.get()); - optimizer_->addEdge(edge); - } - } -} - -bool TebOptimalPlanner::hasDiverged() const -{ - // Early returns if divergence detection is not active - if (!cfg_->recovery.divergence_detection_enable) - return false; - - auto stats_vector = optimizer_->batchStatistics(); - - // No statistics yet - if (stats_vector.empty()) - return false; - - // Grab the statistics of the final iteration - const auto last_iter_stats = stats_vector.back(); - - return last_iter_stats.chi2 > cfg_->recovery.divergence_detection_max_chi_squared; -} - -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++) - { - double cur_cost = (*it)->chi2(); - - if (dynamic_cast(*it) != nullptr - || dynamic_cast(*it) != nullptr - || dynamic_cast(*it) != nullptr) - { - cur_cost *= obst_cost_scale; - } - else if (dynamic_cast(*it) != nullptr) - { - cur_cost *= viapoint_cost_scale; - } - else if (dynamic_cast(*it) != nullptr && alternative_time_cost) - { - continue; // skip these edges if alternative_time_cost is active - } - cost_ += cur_cost; - } - - // 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, int look_ahead_poses) 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; - } - look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1)); - double dt = 0.0; - for(int counter = 0; counter < look_ahead_poses; ++counter) - { - dt += teb_.TimeDiff(counter); - if(dt >= cfg_->trajectory.dt_ref * look_ahead_poses) // TODO: change to look-ahead time? Refine trajectory? - { - look_ahead_poses = counter + 1; - break; - } - } - 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(look_ahead_poses), 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) == -1 ) - { - if (visualization_) - { - visualization_->publishInfeasibleRobotPose(teb().Pose(i), *robot_model_); - } - return false; - } - // Checks if the distance between two poses is higher than the robot radius or the orientation diff is bigger than the specified threshold - // and interpolates in that case. - // (if obstacles are pushing two consecutive poses away, the center between two consecutive poses might coincide with the obstacle ;-)! - if (i cfg_->trajectory.min_resolution_collision_check_angular || delta_dist.norm() > inscribed_radius) - { - int n_additional_samples = std::max(std::ceil(fabs(delta_rot) / cfg_->trajectory.min_resolution_collision_check_angular), - std::ceil(delta_dist.norm() / inscribed_radius)) - 1; - PoseSE2 intermediate_pose = teb().Pose(i); - for(int step = 0; step < n_additional_samples; ++step) - { - intermediate_pose.position() = intermediate_pose.position() + delta_dist / (n_additional_samples + 1.0); - intermediate_pose.theta() = g2o::normalize_theta(intermediate_pose.theta() + - delta_rot / (n_additional_samples + 1.0)); - if ( costmap_model->footprintCost(intermediate_pose.x(), intermediate_pose.y(), intermediate_pose.theta(), - footprint_spec, inscribed_radius, circumscribed_radius) == -1 ) - { - if (visualization_) - { - visualization_->publishInfeasibleRobotPose(intermediate_pose, *robot_model_); - } - return false; - } - } - } - } - } - return true; -} - -} // namespace teb_local_planner From ff6998452811151f4484d12d4c21fa33e2863c0d Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:05:14 -0400 Subject: [PATCH 15/46] Add files via upload --- src/optimal_planner.cpp | 1373 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 1373 insertions(+) create mode 100644 src/optimal_planner.cpp diff --git a/src/optimal_planner.cpp b/src/optimal_planner.cpp new file mode 100644 index 00000000..0b202188 --- /dev/null +++ b/src/optimal_planner.cpp @@ -0,0 +1,1373 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#include + +// g2o custom edges and vertices for the TEB planner +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + + +namespace teb_local_planner +{ + +// ============== 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(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::updateRobotModel(RobotFootprintModelPtr robot_model) +{ + robot_model_ = robot_model; +} + +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; +} + +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_SHORTEST_PATH", 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_STEERING_RATE", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_STEERING_RATE_START", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_STEERING_RATE_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(); + std::unique_ptr linear_solver(new TEBLinearSolver()); // see typedef in optimization.h + linear_solver->setBlockOrdering(true); + std::unique_ptr block_solver(new TEBBlockSolver(std::move(linear_solver))); + g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(block_solver)); + + 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) + { + //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; +} + +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()) + { + teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, 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 + && fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // 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, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation, + 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); +} + +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 + && fabs(g2o::normalize_theta(goal.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // 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; + } + + optimizer_->setComputeBatchStatistics(cfg_->recovery.divergence_detection_enable); + + // 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(); + + AddEdgesSteeringRate(); + + AddEdgesTimeOptimal(); + + AddEdgesShortestPath(); + + 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(); + + if (cfg_->optim.weight_velocity_obstacle_ratio > 0) + AddEdgesVelocityObstacleRatio(); + + 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 + if (optimizer_) + { + // we will delete all edges but keep the vertices. + // before doing so, we will delete the link from the vertices to the edges. + auto& vertices = optimizer_->vertices(); + for(auto& v : vertices) + v.second->edges().clear(); + + optimizer_->vertices().clear(); // necessary, 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 + obstacles_per_vertex_.resize(teb_.sizePoses()); + auto iter_obstacle = obstacles_per_vertex_.begin(); + for (int i=0; isetId(id_counter++); + optimizer_->addVertex(teb_.PoseVertex(i)); + if (teb_.sizeTimeDiffs()!=0 && isetId(id_counter++); + optimizer_->addVertex(teb_.TimeDiffVertex(i)); + } + iter_obstacle->clear(); + (iter_obstacle++)->reserve(obstacles_->size()); + } +} + + +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; + + auto iter_obstacle = obstacles_per_vertex_.begin(); + + auto create_edge = [inflated, &information, &information_inflated, this] (int index, const Obstacle* obstacle) { + if (inflated) + { + 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(), obstacle); + optimizer_->addEdge(dist_bandpt_obst); + } + else + { + 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(), obstacle); + optimizer_->addEdge(dist_bandpt_obst); + }; + }; + + // iterate all teb points, skipping the last and, if the EdgeVelocityObstacleRatio edges should not be created, the first one too + const int first_vertex = cfg_->optim.weight_velocity_obstacle_ratio == 0 ? 1 : 0; + for (int i = first_vertex; i < teb_.sizePoses() - 1; ++i) + { + double left_min_dist = std::numeric_limits::max(); + double right_min_dist = std::numeric_limits::max(); + ObstaclePtr left_obstacle; + ObstaclePtr right_obstacle; + + 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 robot model + double dist = robot_model_->calculateDistance(teb_.Pose(i), obst.get()); + + // force considering obstacle if really close to the current pose + if (dist < cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_force_inclusion_factor) + { + iter_obstacle->push_back(obst); + 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; + } + } + else + { + if (dist < right_min_dist) + { + right_min_dist = dist; + right_obstacle = obst; + } + } + } + + if (left_obstacle) + iter_obstacle->push_back(left_obstacle); + if (right_obstacle) + iter_obstacle->push_back(right_obstacle); + + // continue here to ignore obstacles for the first pose, but use them later to create the EdgeVelocityObstacleRatio edges + if (i == 0) + { + ++iter_obstacle; + continue; + } + + // create obstacle edges + for (const ObstaclePtr obst : *iter_obstacle) + create_edge(i, obst.get()); + ++iter_obstacle; + } +} + + +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(index)); + dist_bandpt_obst->setInformation(information_inflated); + 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(index)); + dist_bandpt_obst->setInformation(information); + 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::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". + } + } +} + +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); + } + } + 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) + { + 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); + } + + // now add the usual acceleration edge for each tuple of three teb poses + for (int i=0; i < n - 2; ++i) + { + 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); + } + + // 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); + } + + // 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); + } + } +} + + + +void TebOptimalPlanner::AddEdgesTimeOptimal() +{ + 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); + } +} + +void TebOptimalPlanner::AddEdgesShortestPath() +{ + if (cfg_->optim.weight_shortest_path==0) + return; // if weight equals zero skip adding edges! + + Eigen::Matrix information; + information.fill(cfg_->optim.weight_shortest_path); + + for (int i=0; i < teb_.sizePoses()-1; ++i) + { + EdgeShortestPath* shortest_path_edge = new EdgeShortestPath; + shortest_path_edge->setVertex(0,teb_.PoseVertex(i)); + shortest_path_edge->setVertex(1,teb_.PoseVertex(i+1)); + shortest_path_edge->setInformation(information); + shortest_path_edge->setTebConfig(*cfg_); + optimizer_->addEdge(shortest_path_edge); + } +} + + + +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::AddEdgesSteeringRate() +{ + if (cfg_->robot.max_steering_rate==0 || cfg_->optim.weight_max_steering_rate==0) + return; // if weight equals zero skip adding edges! + + // create edge for satisfiying kinematic constraints + Eigen::Matrix information_steering_rate; + information_steering_rate(0, 0) = cfg_->optim.weight_max_steering_rate; + + int n = teb_.sizePoses(); + + // check if an initial velocity should be taken into accound (we apply the same for the steering rate) + if (vel_start_.first) + { + EdgeSteeringRateStart* steering_rate_edge = new EdgeSteeringRateStart; + steering_rate_edge->setVertex(0,teb_.PoseVertex(0)); + steering_rate_edge->setVertex(1,teb_.PoseVertex(1)); + steering_rate_edge->setVertex(2,teb_.TimeDiffVertex(0)); + if (std::abs(vel_start_.second.linear.x) < 1e-6) + { + //ROS_INFO("TebOptimalPlanner::AddEdgesSteeringRate(): current v close to zero. Using last measured steering angle"); + steering_rate_edge->setInitialSteeringAngle(recent_steering_angle_); // TODO(roesmann): it would be better to measure the actual steering angle + } + else + { + recent_steering_angle_ = std::atan(cfg_->robot.wheelbase/vel_start_.second.linear.x * vel_start_.second.angular.z); + steering_rate_edge->setInitialSteeringAngle(recent_steering_angle_); + } + steering_rate_edge->setInformation(information_steering_rate); + steering_rate_edge->setTebConfig(*cfg_); + optimizer_->addEdge(steering_rate_edge); + } + + for (int i=0; i < n-2; i++) // ignore twiced start only + { + EdgeSteeringRate* steering_rate_edge = new EdgeSteeringRate; + steering_rate_edge->setVertex(0,teb_.PoseVertex(i)); + steering_rate_edge->setVertex(1,teb_.PoseVertex(i+1)); + steering_rate_edge->setVertex(2,teb_.PoseVertex(i+2)); + steering_rate_edge->setVertex(3,teb_.TimeDiffVertex(i)); + steering_rate_edge->setVertex(4,teb_.TimeDiffVertex(i+1)); + steering_rate_edge->setInformation(information_steering_rate); + steering_rate_edge->setTebConfig(*cfg_); + optimizer_->addEdge(steering_rate_edge); + } + + // check if a goal velocity should be taken into accound (we apply the same for the steering rate) + if (vel_goal_.first) + { + EdgeSteeringRateGoal* steering_rate_edge = new EdgeSteeringRateGoal; + steering_rate_edge->setVertex(0,teb_.PoseVertex(n-2)); + steering_rate_edge->setVertex(1,teb_.PoseVertex(n-1)); + steering_rate_edge->setVertex(2,teb_.TimeDiffVertex( teb_.sizeTimeDiffs()-1 )); + if (vel_goal_.second.linear.x==0.0) + steering_rate_edge->setGoalSteeringAngle(0.0); + else + steering_rate_edge->setGoalSteeringAngle(std::atan(cfg_->robot.wheelbase/vel_goal_.second.linear.x * vel_goal_.second.angular.z) ); + steering_rate_edge->setInformation(information_steering_rate); + steering_rate_edge->setTebConfig(*cfg_); + optimizer_->addEdge(steering_rate_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::AddEdgesVelocityObstacleRatio() +{ + Eigen::Matrix information; + information(0,0) = cfg_->optim.weight_velocity_obstacle_ratio; + information(1,1) = cfg_->optim.weight_velocity_obstacle_ratio; + information(0,1) = information(1,0) = 0; + + auto iter_obstacle = obstacles_per_vertex_.begin(); + + for (int index = 0; index < teb_.sizePoses() - 1; ++index) + { + for (const ObstaclePtr obstacle : (*iter_obstacle++)) + { + EdgeVelocityObstacleRatio* edge = new EdgeVelocityObstacleRatio; + edge->setVertex(0,teb_.PoseVertex(index)); + edge->setVertex(1,teb_.PoseVertex(index + 1)); + edge->setVertex(2,teb_.TimeDiffVertex(index)); + edge->setInformation(information); + edge->setParameters(*cfg_, robot_model_.get(), obstacle.get()); + optimizer_->addEdge(edge); + } + } +} + +bool TebOptimalPlanner::hasDiverged() const +{ + // Early returns if divergence detection is not active + if (!cfg_->recovery.divergence_detection_enable) + return false; + + auto stats_vector = optimizer_->batchStatistics(); + + // No statistics yet + if (stats_vector.empty()) + return false; + + // Grab the statistics of the final iteration + const auto last_iter_stats = stats_vector.back(); + + return last_iter_stats.chi2 > cfg_->recovery.divergence_detection_max_chi_squared; +} + +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++) + { + double cur_cost = (*it)->chi2(); + + if (dynamic_cast(*it) != nullptr + || dynamic_cast(*it) != nullptr + || dynamic_cast(*it) != nullptr) + { + cur_cost *= obst_cost_scale; + } + else if (dynamic_cast(*it) != nullptr) + { + cur_cost *= viapoint_cost_scale; + } + else if (dynamic_cast(*it) != nullptr && alternative_time_cost) + { + continue; // skip these edges if alternative_time_cost is active + } + cost_ += cur_cost; + } + + // 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, int look_ahead_poses) 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; + } + look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1)); + double dt = 0.0; + for(int counter = 0; counter < look_ahead_poses; ++counter) + { + dt += teb_.TimeDiff(counter); + if(dt >= cfg_->trajectory.dt_ref * look_ahead_poses) // TODO: change to look-ahead time? Refine trajectory? + { + look_ahead_poses = counter + 1; + break; + } + } + 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(look_ahead_poses), 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) == -1 ) + { + if (visualization_) + { + visualization_->publishInfeasibleRobotPose(teb().Pose(i), *robot_model_); + } + return false; + } + // Checks if the distance between two poses is higher than the robot radius or the orientation diff is bigger than the specified threshold + // and interpolates in that case. + // (if obstacles are pushing two consecutive poses away, the center between two consecutive poses might coincide with the obstacle ;-)! + if (i cfg_->trajectory.min_resolution_collision_check_angular || delta_dist.norm() > inscribed_radius) + { + int n_additional_samples = std::max(std::ceil(fabs(delta_rot) / cfg_->trajectory.min_resolution_collision_check_angular), + std::ceil(delta_dist.norm() / inscribed_radius)) - 1; + PoseSE2 intermediate_pose = teb().Pose(i); + for(int step = 0; step < n_additional_samples; ++step) + { + intermediate_pose.position() = intermediate_pose.position() + delta_dist / (n_additional_samples + 1.0); + intermediate_pose.theta() = g2o::normalize_theta(intermediate_pose.theta() + + delta_rot / (n_additional_samples + 1.0)); + if ( costmap_model->footprintCost(intermediate_pose.x(), intermediate_pose.y(), intermediate_pose.theta(), + footprint_spec, inscribed_radius, circumscribed_radius) == -1 ) + { + if (visualization_) + { + visualization_->publishInfeasibleRobotPose(intermediate_pose, *robot_model_); + } + return false; + } + } + } + } + } + return true; +} + +} // namespace teb_local_planner From cd587b5bc7d891d40a2af2bdb1eb95bcbc3991a3 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:05:38 -0400 Subject: [PATCH 16/46] Delete teb_config.cpp --- src/teb_config.cpp | 374 --------------------------------------------- 1 file changed, 374 deletions(-) delete mode 100644 src/teb_config.cpp diff --git a/src/teb_config.cpp b/src/teb_config.cpp deleted file mode 100644 index 8d4c0623..00000000 --- a/src/teb_config.cpp +++ /dev/null @@ -1,374 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include - -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("global_plan_prune_distance", trajectory.global_plan_prune_distance, trajectory.global_plan_prune_distance); - 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("force_reinit_new_goal_angular", trajectory.force_reinit_new_goal_angular, trajectory.force_reinit_new_goal_angular); - 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); - nh.param("min_resolution_collision_check_angular", trajectory.min_resolution_collision_check_angular, trajectory.min_resolution_collision_check_angular); - nh.param("control_look_ahead_poses", trajectory.control_look_ahead_poses, trajectory.control_look_ahead_poses); - - // 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); - nh.param("use_proportional_saturation", robot.use_proportional_saturation, robot.use_proportional_saturation); - nh.param("transform_tolerance", robot.transform_tolerance, robot.transform_tolerance); - - // 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("trans_stopped_vel", goal_tolerance.trans_stopped_vel, goal_tolerance.trans_stopped_vel); - nh.param("theta_stopped_vel", goal_tolerance.theta_stopped_vel, goal_tolerance.theta_stopped_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); - nh.param("obstacle_proximity_ratio_max_vel", obstacles.obstacle_proximity_ratio_max_vel, obstacles.obstacle_proximity_ratio_max_vel); - nh.param("obstacle_proximity_lower_bound", obstacles.obstacle_proximity_lower_bound, obstacles.obstacle_proximity_lower_bound); - nh.param("obstacle_proximity_upper_bound", obstacles.obstacle_proximity_upper_bound, obstacles.obstacle_proximity_upper_bound); - - // 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_shortest_path", optim.weight_shortest_path, optim.weight_shortest_path); - 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_velocity_obstacle_ratio", optim.weight_velocity_obstacle_ratio, optim.weight_velocity_obstacle_ratio); - 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); - nh.param("obstacle_cost_exponent", optim.obstacle_cost_exponent, optim.obstacle_cost_exponent); - - // 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("max_number_plans_in_current_class", hcp.max_number_plans_in_current_class, hcp.max_number_plans_in_current_class); - 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("selection_dropping_probability", hcp.selection_dropping_probability, hcp.selection_dropping_probability); - 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); - nh.param("delete_detours_backwards", hcp.delete_detours_backwards, hcp.delete_detours_backwards); - nh.param("detours_orientation_tolerance", hcp.detours_orientation_tolerance, hcp.detours_orientation_tolerance); - nh.param("length_start_orientation_vector", hcp.length_start_orientation_vector, hcp.length_start_orientation_vector); - nh.param("max_ratio_detours_duration_best_duration", hcp.max_ratio_detours_duration_best_duration, hcp.max_ratio_detours_duration_best_duration); - - // 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); - nh.param("divergence_detection", recovery.divergence_detection_enable, recovery.divergence_detection_enable); - nh.param("divergence_detection_max_chi_squared", recovery.divergence_detection_max_chi_squared, recovery.divergence_detection_max_chi_squared); - - 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.force_reinit_new_goal_angular = cfg.force_reinit_new_goal_angular; - 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; - robot.use_proportional_saturation = cfg.use_proportional_saturation; - - // 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; - goal_tolerance.trans_stopped_vel = cfg.trans_stopped_vel; - goal_tolerance.theta_stopped_vel = cfg.theta_stopped_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; - obstacles.obstacle_proximity_ratio_max_vel = cfg.obstacle_proximity_ratio_max_vel; - obstacles.obstacle_proximity_lower_bound = cfg.obstacle_proximity_lower_bound; - obstacles.obstacle_proximity_upper_bound = cfg.obstacle_proximity_upper_bound; - - // 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_shortest_path = cfg.weight_shortest_path; - 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_velocity_obstacle_ratio = cfg.weight_velocity_obstacle_ratio; - optim.weight_viapoint = cfg.weight_viapoint; - optim.weight_adapt_factor = cfg.weight_adapt_factor; - optim.obstacle_cost_exponent = cfg.obstacle_cost_exponent; - - // Homotopy Class Planner - hcp.enable_multithreading = cfg.enable_multithreading; - hcp.max_number_classes = cfg.max_number_classes; - hcp.max_number_plans_in_current_class = cfg.max_number_plans_in_current_class; - 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.selection_dropping_probability = cfg.selection_dropping_probability; - 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; - recovery.divergence_detection_enable = cfg.divergence_detection_enable; - recovery.divergence_detection_max_chi_squared = cfg.divergence_detection_max_chi_squared; - - - 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"); - - // weights - if (optim.weight_optimaltime <= 0) - ROS_WARN("TebLocalPlannerROS() Param Warning: parameter weight_optimaltime shoud be > 0 (even if weight_shortest_path is in use)"); - -} - -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 From ded1c9a6a2a4756f5e8ca568aeac551705cfa658 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:05:57 -0400 Subject: [PATCH 17/46] Add files via upload --- src/teb_config.cpp | 378 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 378 insertions(+) create mode 100644 src/teb_config.cpp diff --git a/src/teb_config.cpp b/src/teb_config.cpp new file mode 100644 index 00000000..71551b6f --- /dev/null +++ b/src/teb_config.cpp @@ -0,0 +1,378 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#include + +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("global_plan_prune_distance", trajectory.global_plan_prune_distance, trajectory.global_plan_prune_distance); + 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("force_reinit_new_goal_angular", trajectory.force_reinit_new_goal_angular, trajectory.force_reinit_new_goal_angular); + 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); + nh.param("min_resolution_collision_check_angular", trajectory.min_resolution_collision_check_angular, trajectory.min_resolution_collision_check_angular); + nh.param("control_look_ahead_poses", trajectory.control_look_ahead_poses, trajectory.control_look_ahead_poses); + + // 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_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); + nh.param("use_proportional_saturation", robot.use_proportional_saturation, robot.use_proportional_saturation); + nh.param("transform_tolerance", robot.transform_tolerance, robot.transform_tolerance); + + // 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("trans_stopped_vel", goal_tolerance.trans_stopped_vel, goal_tolerance.trans_stopped_vel); + nh.param("theta_stopped_vel", goal_tolerance.theta_stopped_vel, goal_tolerance.theta_stopped_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); + nh.param("obstacle_proximity_ratio_max_vel", obstacles.obstacle_proximity_ratio_max_vel, obstacles.obstacle_proximity_ratio_max_vel); + nh.param("obstacle_proximity_lower_bound", obstacles.obstacle_proximity_lower_bound, obstacles.obstacle_proximity_lower_bound); + nh.param("obstacle_proximity_upper_bound", obstacles.obstacle_proximity_upper_bound, obstacles.obstacle_proximity_upper_bound); + + // 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_rate", optim.weight_max_steering_rate, optim.weight_max_steering_rate); + nh.param("weight_optimaltime", optim.weight_optimaltime, optim.weight_optimaltime); + nh.param("weight_shortest_path", optim.weight_shortest_path, optim.weight_shortest_path); + 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_velocity_obstacle_ratio", optim.weight_velocity_obstacle_ratio, optim.weight_velocity_obstacle_ratio); + 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); + nh.param("obstacle_cost_exponent", optim.obstacle_cost_exponent, optim.obstacle_cost_exponent); + + // 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("max_number_plans_in_current_class", hcp.max_number_plans_in_current_class, hcp.max_number_plans_in_current_class); + 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("selection_dropping_probability", hcp.selection_dropping_probability, hcp.selection_dropping_probability); + 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); + nh.param("delete_detours_backwards", hcp.delete_detours_backwards, hcp.delete_detours_backwards); + nh.param("detours_orientation_tolerance", hcp.detours_orientation_tolerance, hcp.detours_orientation_tolerance); + nh.param("length_start_orientation_vector", hcp.length_start_orientation_vector, hcp.length_start_orientation_vector); + nh.param("max_ratio_detours_duration_best_duration", hcp.max_ratio_detours_duration_best_duration, hcp.max_ratio_detours_duration_best_duration); + + // 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); + nh.param("divergence_detection", recovery.divergence_detection_enable, recovery.divergence_detection_enable); + nh.param("divergence_detection_max_chi_squared", recovery.divergence_detection_max_chi_squared, recovery.divergence_detection_max_chi_squared); + + 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.force_reinit_new_goal_angular = cfg.force_reinit_new_goal_angular; + 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.max_steering_rate = cfg.max_steering_rate; + robot.wheelbase = cfg.wheelbase; + robot.cmd_angle_instead_rotvel = cfg.cmd_angle_instead_rotvel; + robot.use_proportional_saturation = cfg.use_proportional_saturation; + + // 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; + goal_tolerance.trans_stopped_vel = cfg.trans_stopped_vel; + goal_tolerance.theta_stopped_vel = cfg.theta_stopped_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; + obstacles.obstacle_proximity_ratio_max_vel = cfg.obstacle_proximity_ratio_max_vel; + obstacles.obstacle_proximity_lower_bound = cfg.obstacle_proximity_lower_bound; + obstacles.obstacle_proximity_upper_bound = cfg.obstacle_proximity_upper_bound; + + // 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_max_steering_rate = cfg.weight_max_steering_rate; + optim.weight_optimaltime = cfg.weight_optimaltime; + optim.weight_shortest_path = cfg.weight_shortest_path; + 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_velocity_obstacle_ratio = cfg.weight_velocity_obstacle_ratio; + optim.weight_viapoint = cfg.weight_viapoint; + optim.weight_adapt_factor = cfg.weight_adapt_factor; + optim.obstacle_cost_exponent = cfg.obstacle_cost_exponent; + + // Homotopy Class Planner + hcp.enable_multithreading = cfg.enable_multithreading; + hcp.max_number_classes = cfg.max_number_classes; + hcp.max_number_plans_in_current_class = cfg.max_number_plans_in_current_class; + 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.selection_dropping_probability = cfg.selection_dropping_probability; + 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; + recovery.divergence_detection_enable = cfg.divergence_detection_enable; + recovery.divergence_detection_max_chi_squared = cfg.divergence_detection_max_chi_squared; + + + 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"); + + // weights + if (optim.weight_optimaltime <= 0) + ROS_WARN("TebLocalPlannerROS() Param Warning: parameter weight_optimaltime shoud be > 0 (even if weight_shortest_path is in use)"); + +} + +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 From 83beccd15bf68d3e129db84fec6749c04a422e63 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:06:20 -0400 Subject: [PATCH 18/46] Delete TebLocalPlannerReconfigure.cfg --- TebLocalPlannerReconfigure.cfg | 440 --------------------------------- 1 file changed, 440 deletions(-) delete mode 100644 TebLocalPlannerReconfigure.cfg diff --git a/TebLocalPlannerReconfigure.cfg b/TebLocalPlannerReconfigure.cfg deleted file mode 100644 index c66a4f1a..00000000 --- a/TebLocalPlannerReconfigure.cfg +++ /dev/null @@ -1,440 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import * -#from local_planner_limits import add_generic_localplanner_params - -gen = ParameterGenerator() - -# This unusual line allows to reuse existing parameter definitions -# that concern all localplanners -#add_generic_localplanner_params(gen) - -# For integers and doubles: -# Name Type Reconfiguration level -# Description -# Default Min Max - - -grp_trajectory = gen.add_group("Trajectory", type="tab") - -# Trajectory -grp_trajectory.add("teb_autosize", bool_t, 0, - "Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended)", - True) - -grp_trajectory.add("dt_ref", double_t, 0, - "Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)", - 0.3, 0.01, 1) - -grp_trajectory.add("dt_hysteresis", double_t, 0, - "Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref", - 0.1, 0.002, 0.5) - -grp_trajectory.add("global_plan_overwrite_orientation", bool_t, 0, - "Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically", - True) - -grp_trajectory.add("allow_init_with_backwards_motion", bool_t, 0, - "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)", - False) - -grp_trajectory.add("max_global_plan_lookahead_dist", double_t, 0, - "Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]", - 3.0, 0, 50.0) - -grp_trajectory.add("force_reinit_new_goal_dist", double_t, 0, - "Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)", - 1.0, 0.0, 10.0) - -grp_trajectory.add("force_reinit_new_goal_angular", double_t, 0, - "Force the planner to reinitialize the trajectory if a previous goal is updated with a rotational difference of more than the specified value in radians (skip hot-starting)", - 0.78, 0.0, 4.0) - -grp_trajectory.add("feasibility_check_no_poses", int_t, 0, - "Specify up to which pose on the predicted plan the feasibility should be checked each sampling interval", - 5, 0, 50) - -grp_trajectory.add("exact_arc_length", bool_t, 0, - "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.", - False) - -grp_trajectory.add("publish_feedback", bool_t, 0, - "Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)", - False) - -grp_trajectory.add("visualize_with_time_as_z_axis_scale", double_t, 0, - "If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.", - 0, 0, 1) - -# ViaPoints -grp_viapoints = gen.add_group("ViaPoints", type="tab") - -grp_viapoints.add("global_plan_viapoint_sep", double_t, 0, - "Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]", - -0.1, -0.1, 5.0) - -grp_viapoints.add("via_points_ordered", bool_t, 0, - "If true, the planner adheres to the order of via-points in the storage container", - False) - -# Robot -grp_robot = gen.add_group("Robot", type="tab") - -grp_robot.add("max_vel_x", double_t, 0, - "Maximum translational velocity of the robot", - 0.4, 0.01, 100) - -grp_robot.add("max_vel_x_backwards", double_t, 0, - "Maximum translational velocity of the robot for driving backwards", - 0.2, 0.01, 100) - -grp_robot.add("max_vel_theta", double_t, 0, - "Maximum angular velocity of the robot", - 0.3, 0.01, 100) - -grp_robot.add("acc_lim_x", double_t, 0, - "Maximum translational acceleration of the robot", - 0.5, 0.01, 100) - -grp_robot.add("acc_lim_theta", double_t, 0, - "Maximum angular acceleration of the robot", - 0.5, 0.01, 100) - -grp_robot.add("is_footprint_dynamic", bool_t, 0, - "If true, updated the footprint before checking trajectory feasibility", - False) - -grp_robot.add("use_proportional_saturation", bool_t, 0, - "If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually", - False) -grp_robot.add("transform_tolerance", double_t, 0, - "Tolerance when querying the TF Tree for a transformation (seconds)", - 0.5, 0.001, 20) - -# Robot/Carlike - -grp_robot_carlike = grp_robot.add_group("Carlike", type="hide") - -grp_robot_carlike.add("min_turning_radius", double_t, 0, - "Minimum turning radius of a carlike robot (diff-drive robot: zero)", - 0.0, 0.0, 50.0) - -grp_robot_carlike.add("max_steering_rate", double_t, 0, - "EXPERIMENTAL: Maximum steering rate of a carlike robot (THIS SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT) [deactivate: zero]", - 0.0, 0.0, 10.0) - -grp_robot_carlike.add("wheelbase", double_t, 0, - "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!", - 1.0, -10.0, 10.0) - -grp_robot_carlike.add("cmd_angle_instead_rotvel", bool_t, 0, - "Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')", - False) - -# Robot/Omni - -grp_robot_omni = grp_robot.add_group("Omnidirectional", type="hide") - -grp_robot_omni.add("max_vel_y", double_t, 0, - "Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)", - 0.0, 0.0, 100) - -grp_robot_omni.add("acc_lim_y", double_t, 0, - "Maximum strafing acceleration of the robot", - 0.5, 0.01, 100) - -# GoalTolerance -grp_goal = gen.add_group("GoalTolerance", type="tab") - -grp_goal.add("xy_goal_tolerance", double_t, 0, - "Allowed final euclidean distance to the goal position", - 0.2, 0.001, 10) - -grp_goal.add("yaw_goal_tolerance", double_t, 0, - "Allowed final orientation error to the goal orientation", - 0.1, 0.001, 3.2) - -grp_goal.add("free_goal_vel", bool_t, 0, - "Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed)", - False) - -grp_goal.add("trans_stopped_vel", double_t, 0, - "Below what maximum velocity we consider the robot to be stopped in translation", - 0.1, 0.0) - -grp_goal.add("theta_stopped_vel", double_t, 0, - "Below what maximum rotation velocity we consider the robot to be stopped in rotation", - 0.1, 0.0) - -# Obstacles -grp_obstacles = gen.add_group("Obstacles", type="tab") - -grp_obstacles.add("min_obstacle_dist", double_t, 0, - "Minimum desired separation from obstacles", - 0.5, 0, 10) - -grp_obstacles.add("inflation_dist", double_t, 0, - "Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)", - 0.6, 0, 15) - -grp_obstacles.add("dynamic_obstacle_inflation_dist", double_t, 0, - "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)", - 0.6, 0, 15) - -grp_obstacles.add("include_dynamic_obstacles", bool_t, 0, - "Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static.", - False) - -grp_obstacles.add("include_costmap_obstacles", bool_t, 0, - "Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)", - True) - -grp_obstacles.add("legacy_obstacle_association", bool_t, 0, - "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).", - False) - -grp_obstacles.add("obstacle_association_force_inclusion_factor", double_t, 0, - "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.", - 1.5, 0.0, 100.0) - -grp_obstacles.add("obstacle_association_cutoff_factor", double_t, 0, - "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.", - 5.0, 1.0, 100.0) - -grp_obstacles.add("costmap_obstacles_behind_robot_dist", double_t, 0, - "Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)", - 1.5, 0.0, 20.0) - -grp_obstacles.add("obstacle_poses_affected", int_t, 0, - "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", - 30, 0, 200) - -# Obstacle - Velocity ratio parameters -grp_obstacles_velocity_limit = grp_obstacles.add_group("Reduce velocity near obstacles") - -grp_obstacles_velocity_limit.add("obstacle_proximity_ratio_max_vel", double_t, 0, - "Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to static obstacles", - 1, 0, 1) - -grp_obstacles_velocity_limit.add("obstacle_proximity_lower_bound", double_t, 0, - "Distance to a static obstacle for which the velocity should be lower", - 0, 0, 10) - -grp_obstacles_velocity_limit.add("obstacle_proximity_upper_bound", double_t, 0, - "Distance to a static obstacle for which the velocity should be higher", - 0.5, 0, 10) - -# Optimization -grp_optimization = gen.add_group("Optimization", type="tab") - -grp_optimization.add("no_inner_iterations", int_t, 0, - "Number of solver iterations called in each outerloop iteration", - 5, 1, 100) - -grp_optimization.add("no_outer_iterations", int_t, 0, - "Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations", - 4, 1, 100) - -grp_optimization.add("optimization_activate", bool_t, 0, - "Activate the optimization", - True) - -grp_optimization.add("optimization_verbose", bool_t, 0, - "Print verbose information", - False) - -grp_optimization.add("penalty_epsilon", double_t, 0, - "Add a small safty margin to penalty functions for hard-constraint approximations", - 0.1, 0, 1.0) - -grp_optimization.add("weight_max_vel_x", double_t, 0, - "Optimization weight for satisfying the maximum allowed translational velocity", - 2, 0, 1000) - -grp_optimization.add("weight_max_vel_y", double_t, 0, - "Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)", - 2, 0, 1000) - -grp_optimization.add("weight_max_vel_theta", double_t, 0, - "Optimization weight for satisfying the maximum allowed angular velocity", - 1, 0, 1000) - -grp_optimization.add("weight_acc_lim_x", double_t, 0, - "Optimization weight for satisfying the maximum allowed translational acceleration", - 1, 0, 1000) - -grp_optimization.add("weight_acc_lim_y", double_t, 0, - "Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)", - 1, 0, 1000) - -grp_optimization.add("weight_acc_lim_theta", double_t, 0, - "Optimization weight for satisfying the maximum allowed angular acceleration", - 1, 0, 1000) - -grp_optimization.add("weight_kinematics_nh", double_t, 0, - "Optimization weight for satisfying the non-holonomic kinematics", - 1000 , 0, 10000) - -grp_optimization.add("weight_kinematics_forward_drive", double_t, 0, - "Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)", - 1, 0, 1000) - -grp_optimization.add("weight_kinematics_turning_radius", double_t, 0, - "Optimization weight for enforcing a minimum turning radius (carlike robots)", - 1, 0, 1000) - -grp_optimization.add("weight_max_steering_rate", double_t, 0, - "EXPERIMENTAL: Optimization weight for enforcing a minimum steering rate of a carlike robot (TRY TO KEEP THE WEIGHT LOW OR DEACTIVATE, SINCE IT SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT)", - 0.5, 0, 100) - -grp_optimization.add("weight_optimaltime", double_t, 0, - "Optimization weight for contracting the trajectory w.r.t. transition time", - 1, 0, 1000) - -grp_optimization.add("weight_shortest_path", double_t, 0, - "Optimization weight for contracting the trajectory w.r.t. path length", - 0, 0, 100) - -grp_optimization.add("weight_obstacle", double_t, 0, - "Optimization weight for satisfying a minimum seperation from obstacles", - 50, 0, 1000) - -grp_optimization.add("weight_inflation", double_t, 0, - "Optimization weight for the inflation penalty (should be small)", - 0.1, 0, 10) - -grp_optimization.add("weight_dynamic_obstacle", double_t, 0, - "Optimization weight for satisfying a minimum seperation from dynamic obstacles", - 50, 0, 1000) - -grp_optimization.add("weight_dynamic_obstacle_inflation", double_t, 0, - "Optimization weight for the inflation penalty of dynamic obstacles (should be small)", - 0.1, 0, 10) - -grp_optimization.add("weight_velocity_obstacle_ratio", double_t, 0, - "Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle", - 0, 0, 1000) - -grp_optimization.add("weight_viapoint", double_t, 0, - "Optimization weight for minimizing the distance to via-points", - 1, 0, 1000) - -grp_optimization.add("weight_adapt_factor", double_t, 0, - "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.", - 2, 1, 100) - -grp_optimization.add("obstacle_cost_exponent", double_t, 0, - "Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)", - 1, 0.01, 100) - - - -# Homotopy Class Planner -grp_hcp = gen.add_group("HCPlanning", type="tab") - -grp_hcp.add("enable_multithreading", bool_t, 0, - "Activate multiple threading for planning multiple trajectories in parallel", - True) - -grp_hcp.add("max_number_classes", int_t, 0, - "Specify the maximum number of allowed alternative homotopy classes (limits computational effort)", - 5, 1, 100) - -grp_hcp.add("max_number_plans_in_current_class", int_t, 0, - "Max number of trajectories to try that are in the same homotopy class as the current best trajectory (setting this to 2 or more helps avoid local minima). Must be <= max_number_classes", - 1, 1, 10) - -grp_hcp.add("selection_cost_hysteresis", double_t, 0, - "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)", - 1.0, 0, 2) - - -grp_hcp.add("selection_prefer_initial_plan", double_t, 0, - "Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.)", - 0.95, 0, 1) - -grp_hcp.add("selection_obst_cost_scale", double_t, 0, - "Extra scaling of obstacle cost terms just for selecting the 'best' candidate (new_obst_cost: obst_cost*factor)", - 2.0, 0, 1000) - -grp_hcp.add("selection_viapoint_cost_scale", double_t, 0, - "Extra scaling of via-point cost terms just for selecting the 'best' candidate. (new_viapt_cost: viapt_cost*factor)", - 1.0, 0, 100) - -grp_hcp.add("selection_alternative_time_cost", bool_t, 0, - "If true, time cost is replaced by the total transition time.", - False) - -grp_hcp.add("selection_dropping_probability", double_t, 0, - "At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies.", - 0.0, 0.0, 1.0) - -grp_hcp.add("switching_blocking_period", double_t, 0, - "Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed", - 0.0, 0.0, 60) - -grp_hcp.add("roadmap_graph_no_samples", int_t, 0, - "Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off", - 15, 1, 100) - -grp_hcp.add("roadmap_graph_area_width", double_t, 0, - "Specify the width of the area in which sampled will be generated between start and goal [m] (the height equals the start-goal distance)", - 5, 0.1, 20) - -grp_hcp.add("roadmap_graph_area_length_scale", double_t, 0, - "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!)", - 1.0, 0.5, 2) - -grp_hcp.add("h_signature_prescaler", double_t, 0, - "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.2 Date: Mon, 10 May 2021 07:06:44 -0400 Subject: [PATCH 19/46] Delete base_teb_edges.h --- base_teb_edges.h | 278 ----------------------------------------------- 1 file changed, 278 deletions(-) delete mode 100644 base_teb_edges.h diff --git a/base_teb_edges.h b/base_teb_edges.h deleted file mode 100644 index 0920c7bc..00000000 --- a/base_teb_edges.h +++ /dev/null @@ -1,278 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef _BASE_TEB_EDGES_H_ -#define _BASE_TEB_EDGES_H_ - -#include - -#include -#include -#include - -namespace teb_local_planner -{ - - -/** - * @class BaseTebUnaryEdge - * @brief Base edge connecting a single vertex in the TEB optimization problem - * - * This edge defines a base edge type for the TEB optimization problem. - * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config). - * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls. - * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class. - * @see BaseTebMultiEdge, BaseTebBinaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge - */ -template -class BaseTebUnaryEdge : public g2o::BaseUnaryEdge -{ -public: - - using typename g2o::BaseUnaryEdge::ErrorVector; - using g2o::BaseUnaryEdge::computeError; - - /** - * @brief Compute and return error / cost value. - * - * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost. - * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T - */ - ErrorVector& getError() - { - computeError(); - return _error; - } - - /** - * @brief Read values from input stream - */ - virtual bool read(std::istream& is) - { - // TODO generic read - return true; - } - - /** - * @brief Write values to an output stream - */ - virtual bool write(std::ostream& os) const - { - // TODO generic write - return os.good(); - } - - /** - * @brief Assign the TebConfig class for parameters. - * @param cfg TebConfig class - */ - void setTebConfig(const TebConfig& cfg) - { - cfg_ = &cfg; - } - -protected: - - using g2o::BaseUnaryEdge::_error; - using g2o::BaseUnaryEdge::_vertices; - - const TebConfig* cfg_; //!< Store TebConfig class for parameters - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/** - * @class BaseTebBinaryEdge - * @brief Base edge connecting two vertices in the TEB optimization problem - * - * This edge defines a base edge type for the TEB optimization problem. - * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config). - * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls. - * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class. - * @see BaseTebMultiEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge - */ -template -class BaseTebBinaryEdge : public g2o::BaseBinaryEdge -{ -public: - - using typename g2o::BaseBinaryEdge::ErrorVector; - using g2o::BaseBinaryEdge::computeError; - - /** - * @brief Compute and return error / cost value. - * - * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost. - * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T - */ - ErrorVector& getError() - { - computeError(); - return _error; - } - - /** - * @brief Read values from input stream - */ - virtual bool read(std::istream& is) - { - // TODO generic read - return true; - } - - /** - * @brief Write values to an output stream - */ - virtual bool write(std::ostream& os) const - { - // TODO generic write - return os.good(); - } - - /** - * @brief Assign the TebConfig class for parameters. - * @param cfg TebConfig class - */ - void setTebConfig(const TebConfig& cfg) - { - cfg_ = &cfg; - } - -protected: - - using g2o::BaseBinaryEdge::_error; - using g2o::BaseBinaryEdge::_vertices; - - const TebConfig* cfg_; //!< Store TebConfig class for parameters - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - -/** - * @class BaseTebMultiEdge - * @brief Base edge connecting multiple vertices in the TEB optimization problem - * - * This edge defines a base edge type for the TEB optimization problem. - * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config). - * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls. - * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class. - * @see BaseTebBinaryEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge - */ -template -class BaseTebMultiEdge : public g2o::BaseMultiEdge -{ -public: - - using typename g2o::BaseMultiEdge::ErrorVector; - using g2o::BaseMultiEdge::computeError; - - // Overwrites resize() from the parent class - virtual void resize(size_t size) - { - g2o::BaseMultiEdge::resize(size); - - for(std::size_t i=0; i<_vertices.size(); ++i) - _vertices[i] = NULL; - } - - /** - * @brief Compute and return error / cost value. - * - * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost. - * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T - */ - ErrorVector& getError() - { - computeError(); - return _error; - } - - /** - * @brief Read values from input stream - */ - virtual bool read(std::istream& is) - { - // TODO generic read - return true; - } - - /** - * @brief Write values to an output stream - */ - virtual bool write(std::ostream& os) const - { - // TODO generic write - return os.good(); - } - - /** - * @brief Assign the TebConfig class for parameters. - * @param cfg TebConfig class - */ - void setTebConfig(const TebConfig& cfg) - { - cfg_ = &cfg; - } - -protected: - - using g2o::BaseMultiEdge::_error; - using g2o::BaseMultiEdge::_vertices; - - const TebConfig* cfg_; //!< Store TebConfig class for parameters - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - - - - - -} // end namespace - -#endif From 2b7e95cf339b7e75d6c7945287b44a7d8452378a Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:06:55 -0400 Subject: [PATCH 20/46] Delete edge_dynamic_obstacle.h --- edge_dynamic_obstacle.h | 153 ---------------------------------------- 1 file changed, 153 deletions(-) delete mode 100644 edge_dynamic_obstacle.h diff --git a/edge_dynamic_obstacle.h b/edge_dynamic_obstacle.h deleted file mode 100644 index c5197d14..00000000 --- a/edge_dynamic_obstacle.h +++ /dev/null @@ -1,153 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann, Franz Albers - *********************************************************************/ - -#ifndef EDGE_DYNAMICOBSTACLE_H -#define EDGE_DYNAMICOBSTACLE_H - -#include -#include -#include -#include -#include -#include -#include - -namespace teb_local_planner -{ - -/** - * @class EdgeDynamicObstacle - * @brief Edge defining the cost function for keeping a distance from dynamic (moving) obstacles. - * - * The edge depends on two vertices \f$ \mathbf{s}_i, \Delta T_i \f$ and minimizes: \n - * \f$ \min \textrm{penaltyBelow}( dist2obstacle) \cdot weight \f$. \n - * \e dist2obstacle denotes the minimum distance to the obstacle trajectory (spatial and temporal). \n - * \e weight can be set using setInformation(). \n - * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow(). \n - * @see TebOptimalPlanner::AddEdgesDynamicObstacles - * @remarks Do not forget to call setTebConfig(), setVertexIdx() and - * @warning Experimental - */ -class EdgeDynamicObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPose> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeDynamicObstacle() : t_(0) - { - } - - /** - * @brief Construct edge and specify the time for its associated pose (neccessary for computeError). - * @param t_ Estimated time until current pose is reached - */ - EdgeDynamicObstacle(double t) : t_(t) - { - } - - /** - * @brief Actual cost function - */ - void computeError() - { - ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeDynamicObstacle()"); - const VertexPose* bandpt = static_cast(_vertices[0]); - - double dist = robot_model_->estimateSpatioTemporalDistance(bandpt->pose(), _measurement, t_); - - _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); - _error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.dynamic_obstacle_inflation_dist, 0.0); - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeDynamicObstacle::computeError() _error[0]=%f\n",_error[0]); - } - - - /** - * @brief Set Obstacle for the underlying cost function - * @param obstacle Const pointer to an Obstacle or derived 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 - double t_; //!< Estimated time until current pose is reached - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - - - -} // end namespace - -#endif From ba573b8706eeb46ed7976cee23bcae7d50ae9b7a Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:07:07 -0400 Subject: [PATCH 21/46] Delete edge_kinematics.h --- edge_kinematics.h | 230 ---------------------------------------------- 1 file changed, 230 deletions(-) delete mode 100644 edge_kinematics.h diff --git a/edge_kinematics.h b/edge_kinematics.h deleted file mode 100644 index 694e602a..00000000 --- a/edge_kinematics.h +++ /dev/null @@ -1,230 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef _EDGE_KINEMATICS_H -#define _EDGE_KINEMATICS_H - -#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]); - } - -#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 - } -#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 - -#endif From c734a85e9c9ac701157da0bedb66cd68f662e108 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:07:18 -0400 Subject: [PATCH 22/46] Delete edge_prefer_rotdir.h --- edge_prefer_rotdir.h | 115 ------------------------------------------- 1 file changed, 115 deletions(-) delete mode 100644 edge_prefer_rotdir.h diff --git a/edge_prefer_rotdir.h b/edge_prefer_rotdir.h deleted file mode 100644 index f744ef4d..00000000 --- a/edge_prefer_rotdir.h +++ /dev/null @@ -1,115 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ -#ifndef EDGE_PREFER_ROTDIR_H_ -#define EDGE_PREFER_ROTDIR_H_ - -#include -#include -#include -#include "g2o/core/base_unary_edge.h" - - -namespace teb_local_planner -{ - -/** - * @class EdgePreferRotDir - * @brief Edge defining the cost function for penalzing a specified turning direction, in particular left resp. right turns - * - * The edge depends on two consecutive vertices \f$ \mathbf{s}_i, \mathbf{s}_{i+1} \f$ and penalizes a given rotation direction - * based on the \e weight and \e dir (\f$ dir \in \{-1,1\} \f$) - * \e dir should be +1 to prefer left rotations and -1 to prefer right rotations \n - * \e weight can be set using setInformation(). \n - * @see TebOptimalPlanner::AddEdgePreferRotDir - */ -class EdgePreferRotDir : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose> -{ -public: - - /** - * @brief Construct edge. - */ - EdgePreferRotDir() - { - _measurement = 1; - } - - /** - * @brief Actual cost function - */ - void computeError() - { - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - - _error[0] = penaltyBoundFromBelow( _measurement*g2o::normalize_theta(conf2->theta()-conf1->theta()) , 0, 0); - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgePreferRotDir::computeError() _error[0]=%f\n",_error[0]); - } - - /** - * @brief Specify the prefered direction of rotation - * @param dir +1 to prefer the left side, -1 to prefer the right side - */ - void setRotDir(double dir) - { - _measurement = dir; - } - - /** Prefer rotations to the right */ - void preferRight() {_measurement = -1;} - - /** Prefer rotations to the right */ - void preferLeft() {_measurement = 1;} - - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - - -} // end namespace - -#endif From d5213679f1a220015886e71451320c7a85c44ed3 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:07:29 -0400 Subject: [PATCH 23/46] Delete edge_shortest_path.h --- edge_shortest_path.h | 89 -------------------------------------------- 1 file changed, 89 deletions(-) delete mode 100644 edge_shortest_path.h diff --git a/edge_shortest_path.h b/edge_shortest_path.h deleted file mode 100644 index 7a52c2f6..00000000 --- a/edge_shortest_path.h +++ /dev/null @@ -1,89 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef EDGE_SHORTEST_PATH_H_ -#define EDGE_SHORTEST_PATH_H_ - -#include - -#include - -#include -#include - -#include - -namespace teb_local_planner { - -/** - * @class EdgeShortestPath - * @brief Edge defining the cost function for minimizing the Euclidean distance between two consectuive poses. - * - * @see TebOptimalPlanner::AddEdgesShortestPath - */ -class EdgeShortestPath : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose> { -public: - /** - * @brief Construct edge. - */ - EdgeShortestPath() { this->setMeasurement(0.); } - - /** - * @brief Actual cost function - */ - void computeError() { - ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeShortestPath()"); - const VertexPose *pose1 = static_cast(_vertices[0]); - const VertexPose *pose2 = static_cast(_vertices[1]); - _error[0] = (pose2->position() - pose1->position()).norm(); - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeShortestPath::computeError() _error[0]=%f\n", _error[0]); - } - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -} // end namespace - -#endif /* EDGE_SHORTEST_PATH_H_ */ From 701ee943f0fe75f09f3d8c2e8b4bf508accedf6e Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:07:39 -0400 Subject: [PATCH 24/46] Delete edge_velocity.h --- edge_velocity.h | 489 ------------------------------------------------ 1 file changed, 489 deletions(-) delete mode 100644 edge_velocity.h diff --git a/edge_velocity.h b/edge_velocity.h deleted file mode 100644 index b01c7e36..00000000 --- a/edge_velocity.h +++ /dev/null @@ -1,489 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef EDGE_VELOCITY_H -#define EDGE_VELOCITY_H - -#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) - { - 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.0 * (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); - - 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... - - /** - * @brief Jacobi matrix of the cost function specified in computeError(). - */ - void linearizeOplus() - { - 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]); - - Eigen::Vector2d deltaS = conf2->position() - conf1->position(); - double dist = deltaS.norm(); - double aux1 = dist*deltaT->estimate(); - double aux2 = 1/deltaT->estimate(); - - double vel = dist * aux2; - double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) * aux2; - - double dev_border_vel = penaltyBoundToIntervalDerivative(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon); - double dev_border_omega = penaltyBoundToIntervalDerivative(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon); - - _jacobianOplus[0].resize(2,3); // conf1 - _jacobianOplus[1].resize(2,3); // conf2 - _jacobianOplus[2].resize(2,1); // deltaT - -// if (aux1==0) aux1=1e-6; -// if (aux2==0) aux2=1e-6; - - if (dev_border_vel!=0) - { - double aux3 = dev_border_vel / aux1; - _jacobianOplus[0](0,0) = -deltaS[0] * aux3; // vel x1 - _jacobianOplus[0](0,1) = -deltaS[1] * aux3; // vel y1 - _jacobianOplus[1](0,0) = deltaS[0] * aux3; // vel x2 - _jacobianOplus[1](0,1) = deltaS[1] * aux3; // vel y2 - _jacobianOplus[2](0,0) = -vel * aux2 * dev_border_vel; // vel deltaT - } - else - { - _jacobianOplus[0](0,0) = 0; // vel x1 - _jacobianOplus[0](0,1) = 0; // vel y1 - _jacobianOplus[1](0,0) = 0; // vel x2 - _jacobianOplus[1](0,1) = 0; // vel y2 - _jacobianOplus[2](0,0) = 0; // vel deltaT - } - - if (dev_border_omega!=0) - { - double aux4 = aux2 * dev_border_omega; - _jacobianOplus[2](1,0) = -omega * aux4; // omega deltaT - _jacobianOplus[0](1,2) = -aux4; // omega angle1 - _jacobianOplus[1](1,2) = aux4; // omega angle2 - } - else - { - _jacobianOplus[2](1,0) = 0; // omega deltaT - _jacobianOplus[0](1,2) = 0; // omega angle1 - _jacobianOplus[1](1,2) = 0; // omega angle2 - } - - _jacobianOplus[0](1,0) = 0; // omega x1 - _jacobianOplus[0](1,1) = 0; // omega y1 - _jacobianOplus[1](1,0) = 0; // omega x2 - _jacobianOplus[1](1,1) = 0; // omega y2 - _jacobianOplus[0](0,2) = 0; // vel angle1 - _jacobianOplus[1](0,2) = 0; // vel angle2 - } -#endif -#endif - - -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(); - - _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]); - } - - /** - * @class EdgeSteeringRate - * @brief Edge defining the cost function for limiting the steering rate w.r.t. the current wheelbase parameter - * - * The edge depends on four vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2} \Delta T_i \f$ . - * @remarks This edge requires the TebConfig::Robot::whelbase parameter to be set. - * @remarks Do not forget to call setTebConfig() - */ -class EdgeSteeringRate : public BaseTebMultiEdge<1, double> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeSteeringRate() - { - this->resize(5); // 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 EdgeSteeringRate()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - const VertexPose* conf3 = static_cast(_vertices[2]); - const VertexTimeDiff* dt1 = static_cast(_vertices[3]); - const VertexTimeDiff* dt2 = static_cast(_vertices[4]); - - Eigen::Vector2d delta_s1 = conf2->estimate().position() - conf1->estimate().position(); - Eigen::Vector2d delta_s2 = conf3->estimate().position() - conf2->estimate().position(); - double dist1 = delta_s1.norm(); - double dist2 = delta_s2.norm(); - double angle_diff1 = g2o::normalize_theta( conf2->theta() - conf1->theta() ); - double angle_diff2 = g2o::normalize_theta( conf3->theta() - conf2->theta() ); - - double phi1, phi2; - if (std::abs(dist1) < 1e-12) - { - phi1 = 0; // TODO previous phi? - //ROS_INFO("phi 1 is zero!"); - } - else - { - //dist1 *= fast_sigmoid( 100.0 * (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta())) ); // consider direction - //if (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta()) < 0) - //dist1 = -dist1; - - if (cfg_->trajectory.exact_arc_length) - phi1 = std::atan(cfg_->robot.wheelbase / dist1 * 2.0*std::sin(angle_diff1/2.0)); - else - phi1 = std::atan(cfg_->robot.wheelbase / dist1 * angle_diff1); - - // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan) - // In case if we apply the sign to the angle directly, it seems to work: - phi1 *= fast_sigmoid( 100.0 * (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta())) ); // consider direction - } - - if (std::abs(dist2) < 1e-12) - { - phi2 = phi1; - ROS_INFO("phi 2 is phi1!"); - } - else - { - //dist2 *= fast_sigmoid( 100.0 * (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta())) ); // consider direction - //if (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta()) < 0) - // dist2 = -dist2; - - if (cfg_->trajectory.exact_arc_length) - phi2 = std::atan(cfg_->robot.wheelbase / dist2 * 2.0*std::sin(angle_diff2/2.0)); - else - phi2 = std::atan(cfg_->robot.wheelbase / dist2 * angle_diff2); - - // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan). - // In case if we apply the sign to the angle directly, it seems to work: - phi2 *= fast_sigmoid( 100.0 * (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta())) ); // consider direction - } - - _error[0] = penaltyBoundToInterval(g2o::normalize_theta(phi2 - phi1)*2.0 / (dt1->dt() + dt2->dt()), cfg_->robot.max_steering_rate, 0.0); - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRate::computeError() _error[0]\n",_error[0]); - } - -public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - -//! Corresponds to EdgeSteeringRate but with initial steering angle for the predecessor configuration -class EdgeSteeringRateStart : public BaseTebMultiEdge<1, double> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeSteeringRateStart() - { - 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 EdgeSteeringRateStart()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - const VertexTimeDiff* dt = static_cast(_vertices[2]); - - Eigen::Vector2d delta_s = conf2->estimate().position() - conf1->estimate().position(); - double dist = delta_s.norm(); - double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); - - double phi; - if (std::abs(dist) < 1e-12) - { - ROS_INFO("Start phi equals pervious phi!"); - phi = _measurement; - } - else - { - //dist *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction - //dist *= (double)g2o::sign( delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta()) ); // consider direction - - if (cfg_->trajectory.exact_arc_length) - phi = std::atan(cfg_->robot.wheelbase / dist * 2.0*std::sin(angle_diff/2.0)); - else - phi = std::atan(cfg_->robot.wheelbase / dist * angle_diff); - - // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan). - // In case if we apply the sign to the angle directly, it seems to work: - phi *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction - } - - _error[0] = penaltyBoundToInterval(g2o::normalize_theta(phi - _measurement) / dt->dt(), cfg_->robot.max_steering_rate, 0.0); - - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRateStart::computeError() _error[0]\n",_error[0]); - } - - void setInitialSteeringAngle(double steering_angle) - { - _measurement = steering_angle; - } - -public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - -//! Corresponds to EdgeSteeringRate but with initial steering angle for the successor configuration -class EdgeSteeringRateGoal : public BaseTebMultiEdge<1, double> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeSteeringRateGoal() - { - 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 EdgeSteeringRateGoal()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - const VertexTimeDiff* dt = static_cast(_vertices[2]); - - Eigen::Vector2d delta_s = conf2->estimate().position() - conf1->estimate().position(); - double dist = delta_s.norm(); - double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); - - double phi; - if (std::abs(dist) < 1e-12) - { - ROS_INFO("Goal phi is zero!"); - phi = 0; - } - else - { - //dist *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction - - if (cfg_->trajectory.exact_arc_length) - phi = std::atan(cfg_->robot.wheelbase / dist * 2.0*std::sin(angle_diff/2.0)); - else - phi = std::atan(cfg_->robot.wheelbase / dist * angle_diff); - - // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan). - // In case if we apply the sign to the angle directly, it seems to work: - phi *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction - } - - _error[0] = penaltyBoundToInterval(g2o::normalize_theta(_measurement - phi) / dt->dt(), cfg_->robot.max_steering_rate, 0.0); - - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRateGoal::computeError() _error[0]\n",_error[0]); - } - - void setGoalSteeringAngle(double steering_angle) - { - _measurement = steering_angle; - } - -public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - - -} // end namespace - -#endif From 187859c9a403a682a70f850c1e7f8b8660f2ff76 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:07:48 -0400 Subject: [PATCH 25/46] Delete edge_velocity_obstacle_ratio.h --- edge_velocity_obstacle_ratio.h | 166 --------------------------------- 1 file changed, 166 deletions(-) delete mode 100644 edge_velocity_obstacle_ratio.h diff --git a/edge_velocity_obstacle_ratio.h b/edge_velocity_obstacle_ratio.h deleted file mode 100644 index ecac1aa4..00000000 --- a/edge_velocity_obstacle_ratio.h +++ /dev/null @@ -1,166 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - *********************************************************************/ - -#pragma once - -#include -#include -#include -#include - -namespace teb_local_planner -{ - - -/** - * @class EdgeVelocityObstacleRatio - * @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 EdgeVelocityObstacleRatio : public BaseTebMultiEdge<2, const Obstacle*> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeVelocityObstacleRatio() : - robot_model_(nullptr) - { - // The three vertices are two poses and one time difference - 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_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeVelocityObstacleRatio()"); - 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 *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction - - const double omega = angle_diff / deltaT->estimate(); - - double dist_to_obstacle = robot_model_->calculateDistance(conf1->pose(), _measurement); - - double ratio; - if (dist_to_obstacle < cfg_->obstacles.obstacle_proximity_lower_bound) - ratio = 0; - else if (dist_to_obstacle > cfg_->obstacles.obstacle_proximity_upper_bound) - ratio = 1; - else - ratio = (dist_to_obstacle - cfg_->obstacles.obstacle_proximity_lower_bound) / - (cfg_->obstacles.obstacle_proximity_upper_bound - cfg_->obstacles.obstacle_proximity_lower_bound); - ratio *= cfg_->obstacles.obstacle_proximity_ratio_max_vel; - - const double max_vel_fwd = ratio * cfg_->robot.max_vel_x; - const double max_omega = ratio * cfg_->robot.max_vel_theta; - _error[0] = penaltyBoundToInterval(vel, max_vel_fwd, 0); - _error[1] = penaltyBoundToInterval(omega, max_omega, 0); - - ROS_ASSERT_MSG(std::isfinite(_error[0]) || std::isfinite(_error[1]), "EdgeVelocityObstacleRatio::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 From 02bb9ec3bd6a215288282c95c11af4c1b5ae9909 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:07:56 -0400 Subject: [PATCH 26/46] Delete edge_time_optimal.h --- edge_time_optimal.h | 116 -------------------------------------------- 1 file changed, 116 deletions(-) delete mode 100644 edge_time_optimal.h diff --git a/edge_time_optimal.h b/edge_time_optimal.h deleted file mode 100644 index 43e16373..00000000 --- a/edge_time_optimal.h +++ /dev/null @@ -1,116 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef EDGE_TIMEOPTIMAL_H_ -#define EDGE_TIMEOPTIMAL_H_ - -#include - -#include - -#include -#include -#include -#include - -#include - -namespace teb_local_planner -{ - - -/** - * @class EdgeTimeOptimal - * @brief Edge defining the cost function for minimizing transition time of the trajectory. - * - * The edge depends on a single vertex \f$ \Delta T_i \f$ and minimizes: \n - * \f$ \min \Delta T_i^2 \cdot scale \cdot weight \f$. \n - * \e scale is determined using the penaltyEquality() function, since we experiences good convergence speeds with it. \n - * \e weight can be set using setInformation() (something around 1.0 seems to be fine). \n - * @see TebOptimalPlanner::AddEdgesTimeOptimal - * @remarks Do not forget to call setTebConfig() - */ -class EdgeTimeOptimal : public BaseTebUnaryEdge<1, double, VertexTimeDiff> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeTimeOptimal() - { - this->setMeasurement(0.); - } - - /** - * @brief Actual cost function - */ - void computeError() - { - ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()"); - const VertexTimeDiff* timediff = static_cast(_vertices[0]); - - _error[0] = timediff->dt(); - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeTimeOptimal::computeError() _error[0]=%f\n",_error[0]); - } - -#ifdef USE_ANALYTIC_JACOBI - /** - * @brief Jacobi matrix of the cost function specified in computeError(). - */ - void linearizeOplus() - { - ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()"); - _jacobianOplusXi( 0 , 0 ) = 1; - } -#endif - - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -}; // end namespace - -#endif /* EDGE_TIMEOPTIMAL_H_ */ From 4f1b93cf5518e968858d1d7e74173ff93da752ee Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:08:06 -0400 Subject: [PATCH 27/46] Delete edge_via_point.h --- edge_via_point.h | 120 ----------------------------------------------- 1 file changed, 120 deletions(-) delete mode 100644 edge_via_point.h diff --git a/edge_via_point.h b/edge_via_point.h deleted file mode 100644 index 9ddefcb8..00000000 --- a/edge_via_point.h +++ /dev/null @@ -1,120 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ -#ifndef EDGE_VIA_POINT_H_ -#define EDGE_VIA_POINT_H_ - -#include -#include - -#include "g2o/core/base_unary_edge.h" - - -namespace teb_local_planner -{ - -/** - * @class EdgeViaPoint - * @brief Edge defining the cost function for pushing a configuration towards a via point - * - * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n - * \f$ \min dist2point \cdot weight \f$. \n - * \e dist2point denotes the distance to the via point. \n - * \e weight can be set using setInformation(). \n - * @see TebOptimalPlanner::AddEdgesViaPoints - * @remarks Do not forget to call setTebConfig() and setViaPoint() - */ -class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPose> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeViaPoint() - { - _measurement = NULL; - } - - /** - * @brief Actual cost function - */ - void computeError() - { - ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig(), setViaPoint() on EdgeViaPoint()"); - const VertexPose* bandpt = static_cast(_vertices[0]); - - _error[0] = (bandpt->position() - *_measurement).norm(); - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeViaPoint::computeError() _error[0]=%f\n",_error[0]); - } - - /** - * @brief Set pointer to associated via point for the underlying cost function - * @param via_point 2D position vector containing the position of the via point - */ - void setViaPoint(const Eigen::Vector2d* via_point) - { - _measurement = via_point; - } - - /** - * @brief Set all parameters at once - * @param cfg TebConfig class - * @param via_point 2D position vector containing the position of the via point - */ - void setParameters(const TebConfig& cfg, const Eigen::Vector2d* via_point) - { - cfg_ = &cfg; - _measurement = via_point; - } - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - - -} // end namespace - -#endif From d08fca812d60a5b1885a49745e51ab9e7e848d27 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:08:20 -0400 Subject: [PATCH 28/46] Delete edge_acceleration.h --- edge_acceleration.h | 734 -------------------------------------------- 1 file changed, 734 deletions(-) delete mode 100644 edge_acceleration.h diff --git a/edge_acceleration.h b/edge_acceleration.h deleted file mode 100644 index 8eccf6de..00000000 --- a/edge_acceleration.h +++ /dev/null @@ -1,734 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef EDGE_ACCELERATION_H_ -#define EDGE_ACCELERATION_H_ - -#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 - { - 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); - - - 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 - /* - * @brief Jacobi matrix of the cost function specified in computeError(). - */ - void linearizeOplus() - { - ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()"); - const VertexPointXY* conf1 = static_cast(_vertices[0]); - const VertexPointXY* conf2 = static_cast(_vertices[1]); - const VertexPointXY* conf3 = static_cast(_vertices[2]); - const VertexTimeDiff* deltaT1 = static_cast(_vertices[3]); - const VertexTimeDiff* deltaT2 = static_cast(_vertices[4]); - const VertexOrientation* angle1 = static_cast(_vertices[5]); - const VertexOrientation* angle2 = static_cast(_vertices[6]); - const VertexOrientation* angle3 = static_cast(_vertices[7]); - - Eigen::Vector2d deltaS1 = conf2->estimate() - conf1->estimate(); - Eigen::Vector2d deltaS2 = conf3->estimate() - conf2->estimate(); - double dist1 = deltaS1.norm(); - double dist2 = deltaS2.norm(); - - double sum_time = deltaT1->estimate() + deltaT2->estimate(); - double sum_time_inv = 1 / sum_time; - double dt1_inv = 1/deltaT1->estimate(); - double dt2_inv = 1/deltaT2->estimate(); - double aux0 = 2/sum_time_inv; - double aux1 = dist1 * deltaT1->estimate(); - double aux2 = dist2 * deltaT2->estimate(); - - double vel1 = dist1 * dt1_inv; - double vel2 = dist2 * dt2_inv; - double omega1 = g2o::normalize_theta( angle2->estimate() - angle1->estimate() ) * dt1_inv; - double omega2 = g2o::normalize_theta( angle3->estimate() - angle2->estimate() ) * dt2_inv; - double acc = (vel2 - vel1) * aux0; - double omegadot = (omega2 - omega1) * aux0; - double aux3 = -acc/2; - double aux4 = -omegadot/2; - - double dev_border_acc = penaltyBoundToIntervalDerivative(acc, tebConfig.robot_acceleration_max_trans,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order); - double dev_border_omegadot = penaltyBoundToIntervalDerivative(omegadot, tebConfig.robot_acceleration_max_rot,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order); - - _jacobianOplus[0].resize(2,2); // conf1 - _jacobianOplus[1].resize(2,2); // conf2 - _jacobianOplus[2].resize(2,2); // conf3 - _jacobianOplus[3].resize(2,1); // deltaT1 - _jacobianOplus[4].resize(2,1); // deltaT2 - _jacobianOplus[5].resize(2,1); // angle1 - _jacobianOplus[6].resize(2,1); // angle2 - _jacobianOplus[7].resize(2,1); // angle3 - - if (aux1==0) aux1=1e-20; - if (aux2==0) aux2=1e-20; - - if (dev_border_acc!=0) - { - // TODO: double aux = aux0 * dev_border_acc; - // double aux123 = aux / aux1; - _jacobianOplus[0](0,0) = aux0 * deltaS1[0] / aux1 * dev_border_acc; // acc x1 - _jacobianOplus[0](0,1) = aux0 * deltaS1[1] / aux1 * dev_border_acc; // acc y1 - _jacobianOplus[1](0,0) = -aux0 * ( deltaS1[0] / aux1 + deltaS2[0] / aux2 ) * dev_border_acc; // acc x2 - _jacobianOplus[1](0,1) = -aux0 * ( deltaS1[1] / aux1 + deltaS2[1] / aux2 ) * dev_border_acc; // acc y2 - _jacobianOplus[2](0,0) = aux0 * deltaS2[0] / aux2 * dev_border_acc; // acc x3 - _jacobianOplus[2](0,1) = aux0 * deltaS2[1] / aux2 * dev_border_acc; // acc y3 - _jacobianOplus[2](0,0) = 0; - _jacobianOplus[2](0,1) = 0; - _jacobianOplus[3](0,0) = aux0 * (aux3 + vel1 * dt1_inv) * dev_border_acc; // acc deltaT1 - _jacobianOplus[4](0,0) = aux0 * (aux3 - vel2 * dt2_inv) * dev_border_acc; // acc deltaT2 - } - else - { - _jacobianOplus[0](0,0) = 0; // acc x1 - _jacobianOplus[0](0,1) = 0; // acc y1 - _jacobianOplus[1](0,0) = 0; // acc x2 - _jacobianOplus[1](0,1) = 0; // acc y2 - _jacobianOplus[2](0,0) = 0; // acc x3 - _jacobianOplus[2](0,1) = 0; // acc y3 - _jacobianOplus[3](0,0) = 0; // acc deltaT1 - _jacobianOplus[4](0,0) = 0; // acc deltaT2 - } - - if (dev_border_omegadot!=0) - { - _jacobianOplus[3](1,0) = aux0 * ( aux4 + omega1 * dt1_inv ) * dev_border_omegadot; // omegadot deltaT1 - _jacobianOplus[4](1,0) = aux0 * ( aux4 - omega2 * dt2_inv ) * dev_border_omegadot; // omegadot deltaT2 - _jacobianOplus[5](1,0) = aux0 * dt1_inv * dev_border_omegadot; // omegadot angle1 - _jacobianOplus[6](1,0) = -aux0 * ( dt1_inv + dt2_inv ) * dev_border_omegadot; // omegadot angle2 - _jacobianOplus[7](1,0) = aux0 * dt2_inv * dev_border_omegadot; // omegadot angle3 - } - else - { - _jacobianOplus[3](1,0) = 0; // omegadot deltaT1 - _jacobianOplus[4](1,0) = 0; // omegadot deltaT2 - _jacobianOplus[5](1,0) = 0; // omegadot angle1 - _jacobianOplus[6](1,0) = 0; // omegadot angle2 - _jacobianOplus[7](1,0) = 0; // omegadot angle3 - } - - _jacobianOplus[0](1,0) = 0; // omegadot x1 - _jacobianOplus[0](1,1) = 0; // omegadot y1 - _jacobianOplus[1](1,0) = 0; // omegadot x2 - _jacobianOplus[1](1,1) = 0; // omegadot y2 - _jacobianOplus[2](1,0) = 0; // omegadot x3 - _jacobianOplus[2](1,1) = 0; // omegadot y3 - _jacobianOplus[5](0,0) = 0; // acc angle1 - _jacobianOplus[6](0,0) = 0; // acc angle2 - _jacobianOplus[7](0,0) = 0; // acc angle3 - } -#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: - - /** - * @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) - { - 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); - - 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 -}; - - - - -/** - * @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: - - /** - * @brief Construct edge. - */ - EdgeAccelerationGoal() - { - _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]); - - // VELOCITY & ACCELERATION - - 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! - } - - 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(); - - _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 -}; - - - - -/** - * @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: - - /** - * @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]); - } - -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(); - - 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(); - - 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: - - /** - * @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]); - - // VELOCITY & ACCELERATION - - 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(); - - _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 - -#endif /* EDGE_ACCELERATION_H_ */ From a1311f05eaf3042fc1464dffb5ea876c1f1018cd Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:08:31 -0400 Subject: [PATCH 29/46] Delete optimal_planner.cpp --- optimal_planner.cpp | 1373 ------------------------------------------- 1 file changed, 1373 deletions(-) delete mode 100644 optimal_planner.cpp diff --git a/optimal_planner.cpp b/optimal_planner.cpp deleted file mode 100644 index 0b202188..00000000 --- a/optimal_planner.cpp +++ /dev/null @@ -1,1373 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include - -// g2o custom edges and vertices for the TEB planner -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - - -namespace teb_local_planner -{ - -// ============== 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(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::updateRobotModel(RobotFootprintModelPtr robot_model) -{ - robot_model_ = robot_model; -} - -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; -} - -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_SHORTEST_PATH", 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_STEERING_RATE", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_STEERING_RATE_START", new g2o::HyperGraphElementCreator); - factory->registerType("EDGE_STEERING_RATE_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(); - std::unique_ptr linear_solver(new TEBLinearSolver()); // see typedef in optimization.h - linear_solver->setBlockOrdering(true); - std::unique_ptr block_solver(new TEBBlockSolver(std::move(linear_solver))); - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(block_solver)); - - 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) - { - //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; -} - -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()) - { - teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, 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 - && fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // 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, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation, - 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); -} - -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 - && fabs(g2o::normalize_theta(goal.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // 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; - } - - optimizer_->setComputeBatchStatistics(cfg_->recovery.divergence_detection_enable); - - // 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(); - - AddEdgesSteeringRate(); - - AddEdgesTimeOptimal(); - - AddEdgesShortestPath(); - - 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(); - - if (cfg_->optim.weight_velocity_obstacle_ratio > 0) - AddEdgesVelocityObstacleRatio(); - - 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 - if (optimizer_) - { - // we will delete all edges but keep the vertices. - // before doing so, we will delete the link from the vertices to the edges. - auto& vertices = optimizer_->vertices(); - for(auto& v : vertices) - v.second->edges().clear(); - - optimizer_->vertices().clear(); // necessary, 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 - obstacles_per_vertex_.resize(teb_.sizePoses()); - auto iter_obstacle = obstacles_per_vertex_.begin(); - for (int i=0; isetId(id_counter++); - optimizer_->addVertex(teb_.PoseVertex(i)); - if (teb_.sizeTimeDiffs()!=0 && isetId(id_counter++); - optimizer_->addVertex(teb_.TimeDiffVertex(i)); - } - iter_obstacle->clear(); - (iter_obstacle++)->reserve(obstacles_->size()); - } -} - - -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; - - auto iter_obstacle = obstacles_per_vertex_.begin(); - - auto create_edge = [inflated, &information, &information_inflated, this] (int index, const Obstacle* obstacle) { - if (inflated) - { - 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(), obstacle); - optimizer_->addEdge(dist_bandpt_obst); - } - else - { - 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(), obstacle); - optimizer_->addEdge(dist_bandpt_obst); - }; - }; - - // iterate all teb points, skipping the last and, if the EdgeVelocityObstacleRatio edges should not be created, the first one too - const int first_vertex = cfg_->optim.weight_velocity_obstacle_ratio == 0 ? 1 : 0; - for (int i = first_vertex; i < teb_.sizePoses() - 1; ++i) - { - double left_min_dist = std::numeric_limits::max(); - double right_min_dist = std::numeric_limits::max(); - ObstaclePtr left_obstacle; - ObstaclePtr right_obstacle; - - 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 robot model - double dist = robot_model_->calculateDistance(teb_.Pose(i), obst.get()); - - // force considering obstacle if really close to the current pose - if (dist < cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_force_inclusion_factor) - { - iter_obstacle->push_back(obst); - 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; - } - } - else - { - if (dist < right_min_dist) - { - right_min_dist = dist; - right_obstacle = obst; - } - } - } - - if (left_obstacle) - iter_obstacle->push_back(left_obstacle); - if (right_obstacle) - iter_obstacle->push_back(right_obstacle); - - // continue here to ignore obstacles for the first pose, but use them later to create the EdgeVelocityObstacleRatio edges - if (i == 0) - { - ++iter_obstacle; - continue; - } - - // create obstacle edges - for (const ObstaclePtr obst : *iter_obstacle) - create_edge(i, obst.get()); - ++iter_obstacle; - } -} - - -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(index)); - dist_bandpt_obst->setInformation(information_inflated); - 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(index)); - dist_bandpt_obst->setInformation(information); - 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::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". - } - } -} - -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); - } - } - 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) - { - 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); - } - - // now add the usual acceleration edge for each tuple of three teb poses - for (int i=0; i < n - 2; ++i) - { - 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); - } - - // 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); - } - - // 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); - } - } -} - - - -void TebOptimalPlanner::AddEdgesTimeOptimal() -{ - 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); - } -} - -void TebOptimalPlanner::AddEdgesShortestPath() -{ - if (cfg_->optim.weight_shortest_path==0) - return; // if weight equals zero skip adding edges! - - Eigen::Matrix information; - information.fill(cfg_->optim.weight_shortest_path); - - for (int i=0; i < teb_.sizePoses()-1; ++i) - { - EdgeShortestPath* shortest_path_edge = new EdgeShortestPath; - shortest_path_edge->setVertex(0,teb_.PoseVertex(i)); - shortest_path_edge->setVertex(1,teb_.PoseVertex(i+1)); - shortest_path_edge->setInformation(information); - shortest_path_edge->setTebConfig(*cfg_); - optimizer_->addEdge(shortest_path_edge); - } -} - - - -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::AddEdgesSteeringRate() -{ - if (cfg_->robot.max_steering_rate==0 || cfg_->optim.weight_max_steering_rate==0) - return; // if weight equals zero skip adding edges! - - // create edge for satisfiying kinematic constraints - Eigen::Matrix information_steering_rate; - information_steering_rate(0, 0) = cfg_->optim.weight_max_steering_rate; - - int n = teb_.sizePoses(); - - // check if an initial velocity should be taken into accound (we apply the same for the steering rate) - if (vel_start_.first) - { - EdgeSteeringRateStart* steering_rate_edge = new EdgeSteeringRateStart; - steering_rate_edge->setVertex(0,teb_.PoseVertex(0)); - steering_rate_edge->setVertex(1,teb_.PoseVertex(1)); - steering_rate_edge->setVertex(2,teb_.TimeDiffVertex(0)); - if (std::abs(vel_start_.second.linear.x) < 1e-6) - { - //ROS_INFO("TebOptimalPlanner::AddEdgesSteeringRate(): current v close to zero. Using last measured steering angle"); - steering_rate_edge->setInitialSteeringAngle(recent_steering_angle_); // TODO(roesmann): it would be better to measure the actual steering angle - } - else - { - recent_steering_angle_ = std::atan(cfg_->robot.wheelbase/vel_start_.second.linear.x * vel_start_.second.angular.z); - steering_rate_edge->setInitialSteeringAngle(recent_steering_angle_); - } - steering_rate_edge->setInformation(information_steering_rate); - steering_rate_edge->setTebConfig(*cfg_); - optimizer_->addEdge(steering_rate_edge); - } - - for (int i=0; i < n-2; i++) // ignore twiced start only - { - EdgeSteeringRate* steering_rate_edge = new EdgeSteeringRate; - steering_rate_edge->setVertex(0,teb_.PoseVertex(i)); - steering_rate_edge->setVertex(1,teb_.PoseVertex(i+1)); - steering_rate_edge->setVertex(2,teb_.PoseVertex(i+2)); - steering_rate_edge->setVertex(3,teb_.TimeDiffVertex(i)); - steering_rate_edge->setVertex(4,teb_.TimeDiffVertex(i+1)); - steering_rate_edge->setInformation(information_steering_rate); - steering_rate_edge->setTebConfig(*cfg_); - optimizer_->addEdge(steering_rate_edge); - } - - // check if a goal velocity should be taken into accound (we apply the same for the steering rate) - if (vel_goal_.first) - { - EdgeSteeringRateGoal* steering_rate_edge = new EdgeSteeringRateGoal; - steering_rate_edge->setVertex(0,teb_.PoseVertex(n-2)); - steering_rate_edge->setVertex(1,teb_.PoseVertex(n-1)); - steering_rate_edge->setVertex(2,teb_.TimeDiffVertex( teb_.sizeTimeDiffs()-1 )); - if (vel_goal_.second.linear.x==0.0) - steering_rate_edge->setGoalSteeringAngle(0.0); - else - steering_rate_edge->setGoalSteeringAngle(std::atan(cfg_->robot.wheelbase/vel_goal_.second.linear.x * vel_goal_.second.angular.z) ); - steering_rate_edge->setInformation(information_steering_rate); - steering_rate_edge->setTebConfig(*cfg_); - optimizer_->addEdge(steering_rate_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::AddEdgesVelocityObstacleRatio() -{ - Eigen::Matrix information; - information(0,0) = cfg_->optim.weight_velocity_obstacle_ratio; - information(1,1) = cfg_->optim.weight_velocity_obstacle_ratio; - information(0,1) = information(1,0) = 0; - - auto iter_obstacle = obstacles_per_vertex_.begin(); - - for (int index = 0; index < teb_.sizePoses() - 1; ++index) - { - for (const ObstaclePtr obstacle : (*iter_obstacle++)) - { - EdgeVelocityObstacleRatio* edge = new EdgeVelocityObstacleRatio; - edge->setVertex(0,teb_.PoseVertex(index)); - edge->setVertex(1,teb_.PoseVertex(index + 1)); - edge->setVertex(2,teb_.TimeDiffVertex(index)); - edge->setInformation(information); - edge->setParameters(*cfg_, robot_model_.get(), obstacle.get()); - optimizer_->addEdge(edge); - } - } -} - -bool TebOptimalPlanner::hasDiverged() const -{ - // Early returns if divergence detection is not active - if (!cfg_->recovery.divergence_detection_enable) - return false; - - auto stats_vector = optimizer_->batchStatistics(); - - // No statistics yet - if (stats_vector.empty()) - return false; - - // Grab the statistics of the final iteration - const auto last_iter_stats = stats_vector.back(); - - return last_iter_stats.chi2 > cfg_->recovery.divergence_detection_max_chi_squared; -} - -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++) - { - double cur_cost = (*it)->chi2(); - - if (dynamic_cast(*it) != nullptr - || dynamic_cast(*it) != nullptr - || dynamic_cast(*it) != nullptr) - { - cur_cost *= obst_cost_scale; - } - else if (dynamic_cast(*it) != nullptr) - { - cur_cost *= viapoint_cost_scale; - } - else if (dynamic_cast(*it) != nullptr && alternative_time_cost) - { - continue; // skip these edges if alternative_time_cost is active - } - cost_ += cur_cost; - } - - // 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, int look_ahead_poses) 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; - } - look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1)); - double dt = 0.0; - for(int counter = 0; counter < look_ahead_poses; ++counter) - { - dt += teb_.TimeDiff(counter); - if(dt >= cfg_->trajectory.dt_ref * look_ahead_poses) // TODO: change to look-ahead time? Refine trajectory? - { - look_ahead_poses = counter + 1; - break; - } - } - 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(look_ahead_poses), 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) == -1 ) - { - if (visualization_) - { - visualization_->publishInfeasibleRobotPose(teb().Pose(i), *robot_model_); - } - return false; - } - // Checks if the distance between two poses is higher than the robot radius or the orientation diff is bigger than the specified threshold - // and interpolates in that case. - // (if obstacles are pushing two consecutive poses away, the center between two consecutive poses might coincide with the obstacle ;-)! - if (i cfg_->trajectory.min_resolution_collision_check_angular || delta_dist.norm() > inscribed_radius) - { - int n_additional_samples = std::max(std::ceil(fabs(delta_rot) / cfg_->trajectory.min_resolution_collision_check_angular), - std::ceil(delta_dist.norm() / inscribed_radius)) - 1; - PoseSE2 intermediate_pose = teb().Pose(i); - for(int step = 0; step < n_additional_samples; ++step) - { - intermediate_pose.position() = intermediate_pose.position() + delta_dist / (n_additional_samples + 1.0); - intermediate_pose.theta() = g2o::normalize_theta(intermediate_pose.theta() + - delta_rot / (n_additional_samples + 1.0)); - if ( costmap_model->footprintCost(intermediate_pose.x(), intermediate_pose.y(), intermediate_pose.theta(), - footprint_spec, inscribed_radius, circumscribed_radius) == -1 ) - { - if (visualization_) - { - visualization_->publishInfeasibleRobotPose(intermediate_pose, *robot_model_); - } - return false; - } - } - } - } - } - return true; -} - -} // namespace teb_local_planner From fd7b7565e39266805c2db3dfed3b697c002ed7ff Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:08:43 -0400 Subject: [PATCH 30/46] Delete edge_obstacle.h --- edge_obstacle.h | 295 ------------------------------------------------ 1 file changed, 295 deletions(-) delete mode 100644 edge_obstacle.h diff --git a/edge_obstacle.h b/edge_obstacle.h deleted file mode 100644 index 976c494a..00000000 --- a/edge_obstacle.h +++ /dev/null @@ -1,295 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ -#ifndef EDGE_OBSTACLE_H_ -#define EDGE_OBSTACLE_H_ - -#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; - } - - /** - * @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); - - // Original obstacle cost. - _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); - - if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0) - { - // Optional non-linear cost. Note the max cost (before weighting) is - // the same as the straight line version and that all other costs are - // below the straight line (for positive exponent), so it may be - // necessary to increase weight_obstacle and/or the inflation_weight - // when using larger exponents. - _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent); - } - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeObstacle::computeError() _error[0]=%f\n",_error[0]); - } - -#ifdef USE_ANALYTIC_JACOBI -#if 0 - - /** - * @brief Jacobi matrix of the cost function specified in computeError(). - */ - void linearizeOplus() - { - ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgePointObstacle()"); - const VertexPose* bandpt = static_cast(_vertices[0]); - - Eigen::Vector2d deltaS = *_measurement - bandpt->position(); - double angdiff = atan2(deltaS[1],deltaS[0])-bandpt->theta(); - - double dist_squared = deltaS.squaredNorm(); - double dist = sqrt(dist_squared); - - double aux0 = sin(angdiff); - double dev_left_border = penaltyBoundFromBelowDerivative(dist*fabs(aux0),cfg_->obstacles.min_obstacle_dist,cfg_->optim.penalty_epsilon); - - if (dev_left_border==0) - { - _jacobianOplusXi( 0 , 0 ) = 0; - _jacobianOplusXi( 0 , 1 ) = 0; - _jacobianOplusXi( 0 , 2 ) = 0; - return; - } - - double aux1 = -fabs(aux0) / dist; - double dev_norm_x = deltaS[0]*aux1; - double dev_norm_y = deltaS[1]*aux1; - - double aux2 = cos(angdiff) * g2o::sign(aux0); - double aux3 = aux2 / dist_squared; - double dev_proj_x = aux3 * deltaS[1] * dist; - double dev_proj_y = -aux3 * deltaS[0] * dist; - double dev_proj_angle = -aux2; - - _jacobianOplusXi( 0 , 0 ) = dev_left_border * ( dev_norm_x + dev_proj_x ); - _jacobianOplusXi( 0 , 1 ) = dev_left_border * ( dev_norm_y + dev_proj_y ); - _jacobianOplusXi( 0 , 2 ) = dev_left_border * dev_proj_angle; - } -#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 EdgeInflatedObstacle()"); - const VertexPose* bandpt = static_cast(_vertices[0]); - - double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement); - - // Original "straight line" obstacle cost. The max possible value - // before weighting is min_obstacle_dist - _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); - - if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0) - { - // Optional non-linear cost. Note the max cost (before weighting) is - // the same as the straight line version and that all other costs are - // below the straight line (for positive exponent), so it may be - // necessary to increase weight_obstacle and/or the inflation_weight - // when using larger exponents. - _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent); - } - - // Additional linear inflation cost - _error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.inflation_dist, 0.0); - - - ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeInflatedObstacle::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 - -#endif From ee5c9d3e5971b4b654f8db14048e1cbca80e3c65 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:08:54 -0400 Subject: [PATCH 31/46] Delete teb_config.cpp --- teb_config.cpp | 378 ------------------------------------------------- 1 file changed, 378 deletions(-) delete mode 100644 teb_config.cpp diff --git a/teb_config.cpp b/teb_config.cpp deleted file mode 100644 index 71551b6f..00000000 --- a/teb_config.cpp +++ /dev/null @@ -1,378 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include - -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("global_plan_prune_distance", trajectory.global_plan_prune_distance, trajectory.global_plan_prune_distance); - 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("force_reinit_new_goal_angular", trajectory.force_reinit_new_goal_angular, trajectory.force_reinit_new_goal_angular); - 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); - nh.param("min_resolution_collision_check_angular", trajectory.min_resolution_collision_check_angular, trajectory.min_resolution_collision_check_angular); - nh.param("control_look_ahead_poses", trajectory.control_look_ahead_poses, trajectory.control_look_ahead_poses); - - // 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_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); - nh.param("use_proportional_saturation", robot.use_proportional_saturation, robot.use_proportional_saturation); - nh.param("transform_tolerance", robot.transform_tolerance, robot.transform_tolerance); - - // 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("trans_stopped_vel", goal_tolerance.trans_stopped_vel, goal_tolerance.trans_stopped_vel); - nh.param("theta_stopped_vel", goal_tolerance.theta_stopped_vel, goal_tolerance.theta_stopped_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); - nh.param("obstacle_proximity_ratio_max_vel", obstacles.obstacle_proximity_ratio_max_vel, obstacles.obstacle_proximity_ratio_max_vel); - nh.param("obstacle_proximity_lower_bound", obstacles.obstacle_proximity_lower_bound, obstacles.obstacle_proximity_lower_bound); - nh.param("obstacle_proximity_upper_bound", obstacles.obstacle_proximity_upper_bound, obstacles.obstacle_proximity_upper_bound); - - // 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_rate", optim.weight_max_steering_rate, optim.weight_max_steering_rate); - nh.param("weight_optimaltime", optim.weight_optimaltime, optim.weight_optimaltime); - nh.param("weight_shortest_path", optim.weight_shortest_path, optim.weight_shortest_path); - 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_velocity_obstacle_ratio", optim.weight_velocity_obstacle_ratio, optim.weight_velocity_obstacle_ratio); - 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); - nh.param("obstacle_cost_exponent", optim.obstacle_cost_exponent, optim.obstacle_cost_exponent); - - // 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("max_number_plans_in_current_class", hcp.max_number_plans_in_current_class, hcp.max_number_plans_in_current_class); - 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("selection_dropping_probability", hcp.selection_dropping_probability, hcp.selection_dropping_probability); - 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); - nh.param("delete_detours_backwards", hcp.delete_detours_backwards, hcp.delete_detours_backwards); - nh.param("detours_orientation_tolerance", hcp.detours_orientation_tolerance, hcp.detours_orientation_tolerance); - nh.param("length_start_orientation_vector", hcp.length_start_orientation_vector, hcp.length_start_orientation_vector); - nh.param("max_ratio_detours_duration_best_duration", hcp.max_ratio_detours_duration_best_duration, hcp.max_ratio_detours_duration_best_duration); - - // 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); - nh.param("divergence_detection", recovery.divergence_detection_enable, recovery.divergence_detection_enable); - nh.param("divergence_detection_max_chi_squared", recovery.divergence_detection_max_chi_squared, recovery.divergence_detection_max_chi_squared); - - 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.force_reinit_new_goal_angular = cfg.force_reinit_new_goal_angular; - 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.max_steering_rate = cfg.max_steering_rate; - robot.wheelbase = cfg.wheelbase; - robot.cmd_angle_instead_rotvel = cfg.cmd_angle_instead_rotvel; - robot.use_proportional_saturation = cfg.use_proportional_saturation; - - // 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; - goal_tolerance.trans_stopped_vel = cfg.trans_stopped_vel; - goal_tolerance.theta_stopped_vel = cfg.theta_stopped_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; - obstacles.obstacle_proximity_ratio_max_vel = cfg.obstacle_proximity_ratio_max_vel; - obstacles.obstacle_proximity_lower_bound = cfg.obstacle_proximity_lower_bound; - obstacles.obstacle_proximity_upper_bound = cfg.obstacle_proximity_upper_bound; - - // 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_max_steering_rate = cfg.weight_max_steering_rate; - optim.weight_optimaltime = cfg.weight_optimaltime; - optim.weight_shortest_path = cfg.weight_shortest_path; - 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_velocity_obstacle_ratio = cfg.weight_velocity_obstacle_ratio; - optim.weight_viapoint = cfg.weight_viapoint; - optim.weight_adapt_factor = cfg.weight_adapt_factor; - optim.obstacle_cost_exponent = cfg.obstacle_cost_exponent; - - // Homotopy Class Planner - hcp.enable_multithreading = cfg.enable_multithreading; - hcp.max_number_classes = cfg.max_number_classes; - hcp.max_number_plans_in_current_class = cfg.max_number_plans_in_current_class; - 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.selection_dropping_probability = cfg.selection_dropping_probability; - 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; - recovery.divergence_detection_enable = cfg.divergence_detection_enable; - recovery.divergence_detection_max_chi_squared = cfg.divergence_detection_max_chi_squared; - - - 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"); - - // weights - if (optim.weight_optimaltime <= 0) - ROS_WARN("TebLocalPlannerROS() Param Warning: parameter weight_optimaltime shoud be > 0 (even if weight_shortest_path is in use)"); - -} - -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 From 2cfc9b854c3028367d43a0876972bd459fcf055a Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:09:03 -0400 Subject: [PATCH 32/46] Delete penalties.h --- penalties.h | 193 ---------------------------------------------------- 1 file changed, 193 deletions(-) delete mode 100644 penalties.h diff --git a/penalties.h b/penalties.h deleted file mode 100644 index 68705584..00000000 --- a/penalties.h +++ /dev/null @@ -1,193 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef PENALTIES_H -#define PENALTIES_H - -#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 < 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 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; - } -} - - -/** - * @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 From de64a77fed0a38bcd01cc2f3bcf61db1893a8f8f Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:09:13 -0400 Subject: [PATCH 33/46] Delete misc.h --- misc.h | 152 --------------------------------------------------------- 1 file changed, 152 deletions(-) delete mode 100644 misc.h diff --git a/misc.h b/misc.h deleted file mode 100644 index fadd60e1..00000000 --- a/misc.h +++ /dev/null @@ -1,152 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef MISC_H -#define MISC_H - -#include -#include -#include - - -namespace teb_local_planner -{ - -#define SMALL_NUM 0.00000001 - -//! Symbols for left/none/right rotations -enum class RotType { left, none, right }; - -/** - * @brief Check whether two variables (double) are close to each other - * @param a the first value to compare - * @param b the second value to compare - * @param epsilon precision threshold - * @return \c true if |a-b| < epsilon, false otherwise - */ -inline bool is_close(double a, double b, double epsilon = 1e-4) -{ - return std::fabs(a - b) < epsilon; -} - -/** - * @brief Return the average angle of an arbitrary number of given angles [rad] - * @param angles vector containing all angles - * @return average / mean angle, that is normalized to [-pi, pi] - */ -inline double average_angles(const std::vector& angles) -{ - double x=0, y=0; - for (std::vector::const_iterator it = angles.begin(); it!=angles.end(); ++it) - { - x += cos(*it); - y += sin(*it); - } - if(x == 0 && y == 0) - return 0; - else - return std::atan2(y, x); -} - -/** @brief Small helper function: check if |a|<|b| */ -inline bool smaller_than_abs(double i, double j) {return std::fabs(i) -inline double distance_points2d(const P1& point1, const P2& point2) -{ - return std::sqrt( std::pow(point2.x-point1.x,2) + std::pow(point2.y-point1.y,2) ); -} - - -/** - * @brief Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d) - * @param v1 object containing public methods x() and y() - * @param v2 object containing fields x() and y() - * @return magnitude that would result in the 3D case (along the z-axis) -*/ -template -inline double cross2d(const V1& v1, const V2& v2) -{ - return v1.x()*v2.y() - v2.x()*v1.y(); -} - -/** - * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference. - * - * Return a constant reference for boths input variants (pointer or reference). - * @remarks Makes only sense in combination with the overload getConstReference(const T& val). - * @param ptr pointer of type T - * @tparam T arbitrary type - * @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion - */ -template -inline const T& get_const_reference(const T* ptr) {return *ptr;} - -/** - * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference. - * - * Return a constant reference for boths input variants (pointer or reference). - * @remarks Makes only sense in combination with the overload getConstReference(const T* val). - * @param val - * @param dummy SFINAE helper variable - * @tparam T arbitrary type - * @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion - */ -template -inline const T& get_const_reference(const T& val, typename boost::disable_if >::type* dummy = 0) {return val;} - -} // namespace teb_local_planner - -#endif /* MISC_H */ From e9dccda255187626ee4239df9f76caa6d0b422bb Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:09:24 -0400 Subject: [PATCH 34/46] Delete optimal_planner.h --- optimal_planner.h | 718 ---------------------------------------------- 1 file changed, 718 deletions(-) delete mode 100644 optimal_planner.h diff --git a/optimal_planner.h b/optimal_planner.h deleted file mode 100644 index e24f6f27..00000000 --- a/optimal_planner.h +++ /dev/null @@ -1,718 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef OPTIMAL_PLANNER_H_ -#define OPTIMAL_PLANNER_H_ - -#include - - -// teb stuff -#include -#include -#include -#include -#include -#include - -// g2o lib stuff -#include -#include -#include -#include -#include -#include -#include - -// messages -#include -#include -#include -#include - -#include -#include - -namespace teb_local_planner -{ - -//! Typedef for the block solver utilized for optimization -typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > TEBBlockSolver; - -//! Typedef for the linear solver utilized for optimization -typedef g2o::LinearSolverCSparse TEBLinearSolver; -//typedef g2o::LinearSolverCholmod TEBLinearSolver; - -//! Typedef for a container storing via-points -typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator > ViaPointContainer; - - -/** - * @class TebOptimalPlanner - * @brief This class optimizes an internal Timed Elastic Band trajectory using the g2o-framework. - * - * For an introduction and further details about the TEB optimization problem refer to: - * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012. - * - C. Rösmann et al.: Efficient trajectory optimization using a sparse model, ECMR, 2013. - * - R. Kümmerle et al.: G2o: A general framework for graph optimization, ICRA, 2011. - * - * @todo: Call buildGraph() only if the teb structure has been modified to speed up hot-starting from previous solutions. - * @todo: 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. - */ -class TebOptimalPlanner : public PlannerInterface -{ -public: - - /** - * @brief Default constructor - */ - TebOptimalPlanner(); - - /** - * @brief Construct and initialize the TEB optimal planner. - * @param cfg Const reference to the TebConfig class for internal parameters - * @param obstacles Container storing all relevant obstacles (see Obstacle) - * @param robot_model Shared pointer to the robot shape model used for optimization (optional) - * @param visual Shared pointer to the TebVisualization class (optional) - * @param via_points Container storing via-points (optional) - */ - TebOptimalPlanner(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared(), - TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL); - - /** - * @brief Destruct the optimal planner. - */ - virtual ~TebOptimalPlanner(); - - /** - * @brief Initializes the optimal planner - * @param cfg Const reference to the TebConfig class for internal parameters - * @param obstacles Container storing all relevant obstacles (see Obstacle) - * @param robot_model Shared pointer to the robot shape model used for optimization (optional) - * @param visual Shared pointer to the TebVisualization class (optional) - * @param via_points Container storing via-points (optional) - */ - void initialize(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared(), - TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL); - - /** - * @param robot_model Shared pointer to the robot shape model used for optimization (optional) - */ - void updateRobotModel(RobotFootprintModelPtr robot_model ); - - /** @name Plan a trajectory */ - //@{ - - /** - * @brief Plan a trajectory based on an initial reference plan. - * - * Call this method to create and optimize a trajectory that is initialized - * according to an initial reference plan (given as a container of poses). \n - * The method supports hot-starting from previous solutions, if avaiable: \n - * - If no trajectory exist yet, a new trajectory is initialized based on the initial plan, - * see TimedElasticBand::initTEBtoGoal - * - If a previous solution is avaiable, update the trajectory based on the initial plan, - * see bool TimedElasticBand::updateAndPruneTEB - * - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o - * @param initial_plan vector of geometry_msgs::PoseStamped - * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used) - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, - * otherwise the final velocity will be zero (default: false) - * @return \c true if planning was successful, \c false otherwise - */ - virtual bool plan(const std::vector& initial_plan, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false); - - /** - * @brief Plan a trajectory between a given start and goal pose (tf::Pose version) - * - * Call this method to create and optimize a trajectory that is initialized between a given start and goal pose. \n - * The method supports hot-starting from previous solutions, if avaiable: \n - * - If no trajectory exist yet, a new trajectory is initialized between start and goal poses, - * see TimedElasticBand::initTEBtoGoal - * - If a previous solution is avaiable, update the trajectory @see bool TimedElasticBand::updateAndPruneTEB - * - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o - * @param start tf::Pose containing the start pose of the trajectory - * @param goal tf::Pose containing the goal pose of the trajectory - * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used) - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, - * otherwise the final velocity will be zero (default: false) - * @return \c true if planning was successful, \c false otherwise - */ - virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false); - - /** - * @brief Plan a trajectory between a given start and goal pose - * - * Call this method to create and optimize a trajectory that is initialized between a given start and goal pose. \n - * The method supports hot-starting from previous solutions, if avaiable: \n - * - If no trajectory exist yet, a new trajectory is initialized between start and goal poses - * @see TimedElasticBand::initTEBtoGoal - * - If a previous solution is avaiable, update the trajectory @see bool TimedElasticBand::updateAndPruneTEB - * - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o - * @param start PoseSE2 containing the start pose of the trajectory - * @param goal PoseSE2 containing the goal pose of the trajectory - * @param start_vel Initial velocity at the start pose (twist message containing the translational and angular velocity). - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, - * otherwise the final velocity will be zero (default: false) - * @return \c true if planning was successful, \c false otherwise - */ - virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false); - - - /** - * @brief Get the velocity command from a previously optimized plan to control the robot at the current sampling interval. - * @warning Call plan() first and check if the generated plan is feasible. - * @param[out] vx translational velocity [m/s] - * @param[out] vy strafing velocity which can be nonzero for holonomic robots[m/s] - * @param[out] omega rotational velocity [rad/s] - * @param[in] look_ahead_poses index of the final pose used to compute the velocity command. - * @return \c true if command is valid, \c false otherwise - */ - virtual bool getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const; - - - /** - * @brief Optimize a previously initialized trajectory (actual TEB optimization loop). - * - * optimizeTEB implements the main optimization loop. \n - * It consist of two nested loops: - * - The outer loop resizes the trajectory according to the temporal resolution by invoking TimedElasticBand::autoResize(). - * Afterwards the internal method optimizeGraph() is called that constitutes the innerloop. - * - The inner loop calls the solver (g2o framework, resp. sparse Levenberg-Marquardt) and iterates a specified - * number of optimization calls (\c iterations_innerloop). - * - * The outer loop is repeated \c iterations_outerloop times. \n - * The ratio of inner and outer loop iterations significantly defines the contraction behavior - * and convergence rate of the trajectory optimization. Based on our experiences, 2-6 innerloop iterations are sufficient. \n - * The number of outer loop iterations should be determined by considering the maximum CPU time required to match the control rate. \n - * Optionally, the cost vector can be calculated by specifying \c compute_cost_afterwards, see computeCurrentCost(). - * @remarks This method is usually called from a plan() method - * @param iterations_innerloop Number of iterations for the actual solver loop - * @param iterations_outerloop Specifies how often the trajectory should be resized followed by the inner solver loop. - * @param compute_cost_afterwards if \c true Calculate the cost vector according to computeCurrentCost(), - * the vector can be accessed afterwards using getCurrentCost(). - * @param obst_cost_scale Specify extra scaling for obstacle costs (only used if \c compute_cost_afterwards is true) - * @param viapoint_cost_scale Specify extra scaling for via-point costs (only used if \c compute_cost_afterwards is true) - * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time - * (only used if \c compute_cost_afterwards is true). - * @return \c true if the optimization terminates successfully, \c false otherwise - */ - bool optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards = false, - double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false); - - //@} - - - /** @name Desired initial and final velocity */ - //@{ - - - /** - * @brief Set the initial velocity at the trajectory's start pose (e.g. the robot's velocity) [twist overload]. - * @remarks Calling this function is not neccessary if the initial velocity is passed via the plan() method - * @param vel_start Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used, - * for holonomic robots also linear.y) - */ - void setVelocityStart(const geometry_msgs::Twist& vel_start); - - /** - * @brief Set the desired final velocity at the trajectory's goal pose. - * @remarks Call this function only if a non-zero velocity is desired and if \c free_goal_vel is set to \c false in plan() - * @param vel_goal twist message containing the translational and angular final velocity - */ - void setVelocityGoal(const geometry_msgs::Twist& vel_goal); - - /** - * @brief Set the desired final velocity at the trajectory's goal pose to be the maximum velocity limit - * @remarks Calling this function is not neccessary if \c free_goal_vel is set to \c false in plan() - */ - void setVelocityGoalFree() {vel_goal_.first = false;} - - //@} - - - /** @name Take obstacles into account */ - //@{ - - - /** - * @brief Assign a new set of obstacles - * @param obst_vector pointer to an obstacle container (can also be a nullptr) - * @remarks This method overrids the obstacle container optinally assigned in the constructor. - */ - void setObstVector(ObstContainer* obst_vector) {obstacles_ = obst_vector;} - - /** - * @brief Access the internal obstacle container. - * @return Const reference to the obstacle container - */ - const ObstContainer& getObstVector() const {return *obstacles_;} - - //@} - - /** @name Take via-points into account */ - //@{ - - - /** - * @brief Assign a new set of via-points - * @param via_points pointer to a via_point container (can also be a nullptr) - * @details Any previously set container will be overwritten. - */ - void setViaPoints(const ViaPointContainer* via_points) {via_points_ = via_points;} - - /** - * @brief Access the internal via-point container. - * @return Const reference to the via-point container - */ - const ViaPointContainer& getViaPoints() const {return *via_points_;} - - //@} - - - /** @name Visualization */ - //@{ - - /** - * @brief Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence) - * @param visualization shared pointer to a TebVisualization instance - * @see visualize - */ - void setVisualization(TebVisualizationPtr visualization); - - /** - * @brief Publish the local plan and pose sequence via ros topics (e.g. subscribe with rviz). - * - * Make sure to register a TebVisualization instance before using setVisualization() or an overlaoded constructor. - * @see setVisualization - */ - virtual void visualize(); - - //@} - - - /** @name Utility methods and more */ - //@{ - - /** - * @brief Reset the planner by clearing the internal graph and trajectory. - */ - virtual void clearPlanner() - { - clearGraph(); - teb_.clearTimedElasticBand(); - } - - /** - * @brief Prefer a desired initial turning direction (by penalizing the opposing one) - * - * A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two - * solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty. - * Initial means that the penalty is applied only to the first few poses of the trajectory. - * @param dir This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none) - */ - virtual void setPreferredTurningDir(RotType dir) {prefer_rotdir_=dir;} - - /** - * @brief Register the vertices and edges defined for the TEB to the g2o::Factory. - * - * This allows the user to export the internal graph to a text file for instance. - * Access the optimizer() for more details. - */ - static void registerG2OTypes(); - - /** - * @brief Access the internal TimedElasticBand trajectory. - * @warning In general, the underlying teb must not be modified directly. Use with care... - * @return reference to the teb - */ - TimedElasticBand& teb() {return teb_;}; - - /** - * @brief Access the internal TimedElasticBand trajectory (read-only). - * @return const reference to the teb - */ - const TimedElasticBand& teb() const {return teb_;}; - - /** - * @brief Access the internal g2o optimizer. - * @warning In general, the underlying optimizer must not be modified directly. Use with care... - * @return const shared pointer to the g2o sparse optimizer - */ - boost::shared_ptr optimizer() {return optimizer_;}; - - /** - * @brief Access the internal g2o optimizer (read-only). - * @return const shared pointer to the g2o sparse optimizer - */ - boost::shared_ptr optimizer() const {return optimizer_;}; - - /** - * @brief Check if last optimization was successful - * @return \c true if the last optimization returned without errors, - * otherwise \c false (also if no optimization has been called before). - */ - bool isOptimized() const {return optimized_;}; - - /** - * @brief Returns true if the planner has diverged. - */ - bool hasDiverged() const override; - - /** - * @brief Compute the cost vector of a given optimization problen (hyper-graph must exist). - * - * Use this method to obtain information about the current edge errors / costs (local cost functions). \n - * The vector of cost values is composed according to the different edge types (time_optimal, obstacles, ...). \n - * Refer to the method declaration for the detailed composition. \n - * The cost for the edges that minimize time differences (EdgeTimeOptimal) corresponds to the sum of all single - * squared time differneces: \f$ \sum_i \Delta T_i^2 \f$. Sometimes, the user may want to get a value that is proportional - * or identical to the actual trajectory transition time \f$ \sum_i \Delta T_i \f$. \n - * Set \c alternative_time_cost to true in order to get the cost calculated using the latter equation, but check the - * implemented definition, if the value is scaled to match the magnitude of other cost values. - * - * @todo Remove the scaling term for the alternative time cost. - * @todo Can we use the last error (chi2) calculated from g2o instead of calculating it by ourself? - * @see getCurrentCost - * @see optimizeTEB - * @param obst_cost_scale Specify extra scaling for obstacle costs. - * @param viapoint_cost_scale Specify extra scaling for via points. - * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time. - * @return TebCostVec containing the cost values - */ - void computeCurrentCost(double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false); - - /** - * Compute and return the cost of the current optimization graph (supports multiple trajectories) - * @param[out] cost current cost value for each trajectory - * [for a planner with just a single trajectory: size=1, vector will not be cleared] - * @param obst_cost_scale Specify extra scaling for obstacle costs - * @param viapoint_cost_scale Specify extra scaling for via points. - * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time - */ - virtual void computeCurrentCost(std::vector& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false) - { - computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost); - cost.push_back( getCurrentCost() ); - } - - /** - * @brief Access the cost vector. - * - * The accumulated cost value previously calculated using computeCurrentCost - * or by calling optimizeTEB with enabled cost flag. - * @return const reference to the TebCostVec. - */ - double getCurrentCost() const {return cost_;} - - - /** - * @brief Extract the velocity from consecutive poses and a time difference (including strafing velocity for holonomic robots) - * - * The velocity is extracted using finite differences. - * The direction of the translational velocity is also determined. - * @param pose1 pose at time k - * @param pose2 consecutive pose at time k+1 - * @param dt actual time difference between k and k+1 (must be >0 !!!) - * @param[out] vx translational velocity - * @param[out] vy strafing velocity which can be nonzero for holonomic robots - * @param[out] omega rotational velocity - */ - inline void extractVelocity(const PoseSE2& pose1, const PoseSE2& pose2, double dt, double& vx, double& vy, double& omega) const; - - /** - * @brief Compute the velocity profile of the trajectory - * - * This method computes the translational and rotational velocity for the complete - * planned trajectory. - * The first velocity is the one that is provided as initial velocity (fixed). - * Velocities at index k=2...end-1 are related to the transition from pose_{k-1} to pose_k. - * The last velocity is the final velocity (fixed). - * The number of Twist objects is therefore sizePoses()+1; - * In summary: - * v[0] = v_start, - * v[1,...end-1] = +-(pose_{k+1}-pose{k})/dt, - * v(end) = v_goal - * It can be used for evaluation and debugging purposes or - * for open-loop control. For computing the velocity required for controlling the robot - * to the next step refer to getVelocityCommand(). - * @param[out] velocity_profile velocity profile will be written to this vector (after clearing any existing content) with the size=no_poses+1 - */ - void getVelocityProfile(std::vector& velocity_profile) const; - - /** - * @brief Return the complete trajectory including poses, velocity profiles and temporal information - * - * It is useful for evaluation and debugging purposes or for open-loop control. - * Since the velocity obtained using difference quotients is the mean velocity between consecutive poses, - * the velocity at each pose at time stamp k is obtained by taking the average between both velocities. - * The velocity of the first pose is v_start (provided initial value) and the last one is v_goal (usually zero, if free_goal_vel is off). - * See getVelocityProfile() for the list of velocities between consecutive points. - * @todo The acceleration profile is not added at the moment. - * @param[out] trajectory the resulting trajectory - */ - void getFullTrajectory(std::vector& trajectory) const; - - /** - * @brief Check whether the planned trajectory is feasible or not. - * - * This method currently checks only that the trajectory, or a part of the trajectory is collision free. - * Obstacles are here represented as costmap instead of the internal ObstacleContainer. - * @param costmap_model Pointer to the costmap model - * @param footprint_spec The specification of the footprint of the robot in world coordinates - * @param inscribed_radius The radius of the inscribed circle of the robot - * @param circumscribed_radius The radius of the circumscribed circle of the robot - * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked. - * @return \c true, if the robot footprint along the first part of the trajectory intersects with - * any obstacle in the costmap, \c false otherwise. - */ - virtual bool isTrajectoryFeasible(base_local_planner::CostmapModel* costmap_model, const std::vector& footprint_spec, double inscribed_radius = 0.0, - double circumscribed_radius=0.0, int look_ahead_idx=-1); - - //@} - -protected: - - /** @name Hyper-Graph creation and optimization */ - //@{ - - /** - * @brief Build the hyper-graph representing the TEB optimization problem. - * - * This method creates the optimization problem according to the hyper-graph formulation. \n - * For more details refer to the literature cited in the TebOptimalPlanner class description. - * @see optimizeGraph - * @see clearGraph - * @param weight_multiplier Specify a weight multipler for selected weights in optimizeGraph - * This might be used for weight adapation strategies. - * Currently, only obstacle collision weights are considered. - * @return \c true, if the graph was created successfully, \c false otherwise. - */ - bool buildGraph(double weight_multiplier=1.0); - - /** - * @brief Optimize the previously constructed hyper-graph to deform / optimize the TEB. - * - * This method invokes the g2o framework to solve the optimization problem considering dedicated sparsity patterns. \n - * The current implementation calls a non-constrained sparse Levenberg-Marquardt algorithm. Constraints are considered - * by utilizing penalty approximations. Refer to the literature cited in the TebOptimalPlanner class description. - * @see buildGraph - * @see clearGraph - * @param no_iterations Number of solver iterations - * @param clear_after Clear the graph after optimization. - * @return \c true, if optimization terminates successfully, \c false otherwise. - */ - bool optimizeGraph(int no_iterations, bool clear_after=true); - - /** - * @brief Clear an existing internal hyper-graph. - * @see buildGraph - * @see optimizeGraph - */ - void clearGraph(); - - /** - * @brief Add all relevant vertices to the hyper-graph as optimizable variables. - * - * Vertices (if unfixed) represent the variables that will be optimized. \n - * In case of the Timed-Elastic-Band poses and time differences form the vertices of the hyper-graph. \n - * The order of insertion of vertices (to the graph) is important for efficiency, - * since it affect the sparsity pattern of the underlying hessian computed for optimization. - * @see VertexPose - * @see VertexTimeDiff - * @see buildGraph - * @see optimizeGraph - */ - void AddTEBVertices(); - - /** - * @brief Add all edges (local cost functions) for limiting the translational and angular velocity. - * @see EdgeVelocity - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesVelocity(); - - /** - * @brief Add all edges (local cost functions) for limiting the translational and angular acceleration. - * @see EdgeAcceleration - * @see EdgeAccelerationStart - * @see EdgeAccelerationGoal - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesAcceleration(); - - /** - * @brief Add all edges (local cost functions) for minimizing the transition time (resp. minimize time differences) - * @see EdgeTimeOptimal - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesTimeOptimal(); - - /** - * @brief Add all edges (local cost functions) for minimizing the path length - * @see EdgeShortestPath - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesShortestPath(); - - /** - * @brief Add all edges (local cost functions) related to keeping a distance from static obstacles - * @warning do not combine with AddEdgesInflatedObstacles - * @see EdgeObstacle - * @see buildGraph - * @see optimizeGraph - * @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight) - */ - void AddEdgesObstacles(double weight_multiplier=1.0); - - /** - * @brief Add all edges (local cost functions) related to keeping a distance from static obstacles (legacy association strategy) - * @see EdgeObstacle - * @see buildGraph - * @see optimizeGraph - * @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight) - */ - void AddEdgesObstaclesLegacy(double weight_multiplier=1.0); - - /** - * @brief Add all edges (local cost functions) related to minimizing the distance to via-points - * @see EdgeViaPoint - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesViaPoints(); - - /** - * @brief Add all edges (local cost functions) related to keeping a distance from dynamic (moving) obstacles. - * @warning experimental - * @todo Should we also add neighbors to decrease jiggling/oscillations - * @see EdgeDynamicObstacle - * @see buildGraph - * @see optimizeGraph - * @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight) - - */ - void AddEdgesDynamicObstacles(double weight_multiplier=1.0); - - /** - * @brief Add all edges (local cost functions) for satisfying kinematic constraints of a differential drive robot - * @warning do not combine with AddEdgesKinematicsCarlike() - * @see AddEdgesKinematicsCarlike - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesKinematicsDiffDrive(); - - /** - * @brief Add all edges (local cost functions) for satisfying kinematic constraints of a carlike robot - * @warning do not combine with AddEdgesKinematicsDiffDrive() - * @see AddEdgesKinematicsDiffDrive - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesKinematicsCarlike(); - - /** - * @brief Add all edges (local cost functions) for prefering a specifiy turning direction (by penalizing the other one) - * @see buildGraph - * @see optimizeGraph - */ - - void AddEdgesSteeringRate(); - - void AddEdgesPreferRotDir(); - - /** - * @brief Add all edges (local cost function) for reducing the velocity of a vertex due to its associated obstacles - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesVelocityObstacleRatio(); - - //@} - - - /** - * @brief Initialize and configure the g2o sparse optimizer. - * @return shared pointer to the g2o::SparseOptimizer instance - */ - boost::shared_ptr initOptimizer(); - - - // external objects (store weak pointers) - const TebConfig* cfg_; //!< Config class that stores and manages all related parameters - ObstContainer* obstacles_; //!< Store obstacles that are relevant for planning - const ViaPointContainer* via_points_; //!< Store via points for planning - std::vector obstacles_per_vertex_; //!< Store the obstacles associated with the n-1 initial vertices - - double cost_; //!< Store cost value of the current hyper-graph - RotType prefer_rotdir_; //!< Store whether to prefer a specific initial rotation in optimization (might be activated in case the robot oscillates) - - // internal objects (memory management owned) - TebVisualizationPtr visualization_; //!< Instance of the visualization class - TimedElasticBand teb_; //!< Actual trajectory object - RobotFootprintModelPtr robot_model_; //!< Robot model - boost::shared_ptr optimizer_; //!< g2o optimizer for trajectory optimization - std::pair vel_start_; //!< Store the initial velocity at the start pose - std::pair vel_goal_; //!< Store the final velocity at the goal pose - double recent_steering_angle_ = 0.0; //!< Store last measured steering angle (for car-like setup) - - bool initialized_; //!< Keeps track about the correct initialization of this class - bool optimized_; //!< This variable is \c true as long as the last optimization has been completed successful - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -//! Abbrev. for shared instances of the TebOptimalPlanner -typedef boost::shared_ptr TebOptimalPlannerPtr; -//! Abbrev. for shared const TebOptimalPlanner pointers -typedef boost::shared_ptr TebOptimalPlannerConstPtr; -//! Abbrev. for containers storing multiple teb optimal planners -typedef std::vector< TebOptimalPlannerPtr > TebOptPlannerContainer; - -} // namespace teb_local_planner - -#endif /* OPTIMAL_PLANNER_H_ */ From 7deda052c32d4143852d5f82b4d09325d3c7e147 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:09:36 -0400 Subject: [PATCH 35/46] Delete vertex_pose.h --- vertex_pose.h | 229 -------------------------------------------------- 1 file changed, 229 deletions(-) delete mode 100644 vertex_pose.h diff --git a/vertex_pose.h b/vertex_pose.h deleted file mode 100644 index eeab99a5..00000000 --- a/vertex_pose.h +++ /dev/null @@ -1,229 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef VERTEX_POSE_H_ -#define VERTEX_POSE_H_ - -#include -#include -#include -#include - -#include - -namespace teb_local_planner -{ - -/** - * @class VertexPose - * @brief This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized via g2o - * @see PoseSE2 - * @see VertexTimeDiff - */ -class VertexPose : public g2o::BaseVertex<3, PoseSE2 > -{ -public: - - /** - * @brief Default constructor - * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] - */ - VertexPose(bool fixed = false) - { - setToOriginImpl(); - setFixed(fixed); - } - - /** - * @brief Construct pose using a given PoseSE2 - * @param pose PoseSE2 defining the pose [x, y, angle_rad] - * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] - */ - VertexPose(const PoseSE2& pose, bool fixed = false) - { - _estimate = pose; - setFixed(fixed); - } - - /** - * @brief Construct pose using a given 2D position vector and orientation - * @param position Eigen::Vector2d containing x and y coordinates - * @param theta yaw-angle - * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] - */ - VertexPose(const Eigen::Ref& position, double theta, bool fixed = false) - { - _estimate.position() = position; - _estimate.theta() = theta; - setFixed(fixed); - } - - /** - * @brief Construct pose using single components x, y, and the yaw angle - * @param x x-coordinate - * @param y y-coordinate - * @param theta yaw angle in rad - * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] - */ - VertexPose(double x, double y, double theta, bool fixed = false) - { - _estimate.x() = x; - _estimate.y() = y; - _estimate.theta() = theta; - setFixed(fixed); - } - - /** - * @brief Access the pose - * @see estimate - * @return reference to the PoseSE2 estimate - */ - inline PoseSE2& pose() {return _estimate;} - - /** - * @brief Access the pose (read-only) - * @see estimate - * @return const reference to the PoseSE2 estimate - */ - inline const PoseSE2& pose() const {return _estimate;} - - - /** - * @brief Access the 2D position part - * @see estimate - * @return reference to the 2D position part - */ - inline Eigen::Vector2d& position() {return _estimate.position();} - - /** - * @brief Access the 2D position part (read-only) - * @see estimate - * @return const reference to the 2D position part - */ - inline const Eigen::Vector2d& position() const {return _estimate.position();} - - /** - * @brief Access the x-coordinate the pose - * @return reference to the x-coordinate - */ - inline double& x() {return _estimate.x();} - - /** - * @brief Access the x-coordinate the pose (read-only) - * @return const reference to the x-coordinate - */ - inline const double& x() const {return _estimate.x();} - - /** - * @brief Access the y-coordinate the pose - * @return reference to the y-coordinate - */ - inline double& y() {return _estimate.y();} - - /** - * @brief Access the y-coordinate the pose (read-only) - * @return const reference to the y-coordinate - */ - inline const double& y() const {return _estimate.y();} - - /** - * @brief Access the orientation part (yaw angle) of the pose - * @return reference to the yaw angle - */ - inline double& theta() {return _estimate.theta();} - - /** - * @brief Access the orientation part (yaw angle) of the pose (read-only) - * @return const reference to the yaw angle - */ - inline const double& theta() const {return _estimate.theta();} - - /** - * @brief Set the underlying estimate (2D vector) to zero. - */ - virtual void setToOriginImpl() override - { - _estimate.setZero(); - } - - /** - * @brief Define the update increment \f$ \mathbf{x}_{k+1} = \mathbf{x}_k + \Delta \mathbf{x} \f$. - * A simple addition for the position. - * The angle is first added to the previous estimated angle and afterwards normalized to the interval \f$ [-\pi \pi] \f$ - * @param update increment that should be added to the previous esimate - */ - virtual void oplusImpl(const double* update) override - { - _estimate.plus(update); - } - - /** - * @brief Read an estimate from an input stream. - * First the x-coordinate followed by y and the yaw angle. - * @param is input stream - * @return always \c true - */ - virtual bool read(std::istream& is) override - { - is >> _estimate.x() >> _estimate.y() >> _estimate.theta(); - return true; - } - - /** - * @brief Write the estimate to an output stream - * First the x-coordinate followed by y and the yaw angle. - * @param os output stream - * @return \c true if the export was successful, otherwise \c false - */ - virtual bool write(std::ostream& os) const override - { - os << _estimate.x() << " " << _estimate.y() << " " << _estimate.theta(); - return os.good(); - } - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -} - -#endif // VERTEX_POSE_H_ From 1c7e4a8983c5944445d79eb252eadf086669a21d Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:09:50 -0400 Subject: [PATCH 36/46] Delete visualize_vel_and_steering.py --- visualize_vel_and_steering.py | 90 ----------------------------------- 1 file changed, 90 deletions(-) delete mode 100644 visualize_vel_and_steering.py diff --git a/visualize_vel_and_steering.py b/visualize_vel_and_steering.py deleted file mode 100644 index 34157f1d..00000000 --- a/visualize_vel_and_steering.py +++ /dev/null @@ -1,90 +0,0 @@ -#!/usr/bin/env python - -# This small script subscribes to the FeedbackMsg message of teb_local_planner -# and plots the current velocity. -# publish_feedback must be turned on such that the planner publishes this information. -# Author: christoph.roesmann@tu-dortmund.de - -import rospy, math -from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg -from geometry_msgs.msg import PolygonStamped, Point32 -import numpy as np -import matplotlib.pyplot as plotter - -def feedback_callback(data): - global trajectory - - if not data.trajectories: # empty - trajectory = [] - return - trajectory = data.trajectories[data.selected_trajectory_idx].trajectory - - -def plot_velocity_profile(fig, ax_v, ax_omega, ax_steering, t, v, omega, steering): - ax_v.cla() - ax_v.grid() - ax_v.set_ylabel('Trans. velocity [m/s]') - ax_v.plot(t, v, '-bx') - ax_omega.cla() - ax_omega.grid() - ax_omega.set_ylabel('Rot. velocity [rad/s]') - ax_omega.set_xlabel('Time [s]') - ax_omega.plot(t, omega, '-bx') - ax_steering.cla() - ax_steering.grid() - ax_steering.set_ylabel('Steering angle [rad]') - ax_steering.set_xlabel('Time [s]') - ax_steering.plot(t, steering, '-bx') - fig.canvas.draw() - - - -def velocity_plotter(): - global trajectory - rospy.init_node("visualize_vel_and_steering", anonymous=True) - - topic_name = "/test_optim_node/teb_feedback" - topic_name = rospy.get_param('~feedback_topic', topic_name) - rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) # define feedback topic here! - - wheelbase = 1.0 - wheelbase = rospy.get_param('~wheelbase', wheelbase) - - rospy.loginfo("Visualizing velocity profile published on '%s'.",topic_name) - rospy.loginfo("Make sure to enable rosparam 'publish_feedback' in the teb_local_planner.") - - # two subplots sharing the same t axis - fig, (ax_v, ax_omega, ax_steering) = plotter.subplots(3, sharex=True) - plotter.ion() - plotter.show() - - - r = rospy.Rate(2) # define rate here - while not rospy.is_shutdown(): - - t = [] - v = [] - omega = [] - steering = [] - - for point in trajectory: - t.append(point.time_from_start.to_sec()) - v.append(point.velocity.linear.x) - omega.append(point.velocity.angular.z) - if point.velocity.linear.x == 0: - steering.append( 0.0 ) - else: - steering.append( math.atan( wheelbase / point.velocity.linear.x * point.velocity.angular.z ) ) - - plot_velocity_profile(fig, ax_v, ax_omega, ax_steering, np.asarray(t), np.asarray(v), np.asarray(omega), np.asarray(steering)) - - r.sleep() - -if __name__ == '__main__': - try: - trajectory = [] - velocity_plotter() - except rospy.ROSInterruptException: - pass - - From 5cdc0d2d4649e679a1c4d29e87c57fb83f8485f9 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:10:01 -0400 Subject: [PATCH 37/46] Delete vertex_timediff.h --- vertex_timediff.h | 145 ---------------------------------------------- 1 file changed, 145 deletions(-) delete mode 100644 vertex_timediff.h diff --git a/vertex_timediff.h b/vertex_timediff.h deleted file mode 100644 index 4eead6c4..00000000 --- a/vertex_timediff.h +++ /dev/null @@ -1,145 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef VERTEX_TIMEDIFF_H -#define VERTEX_TIMEDIFF_H - - -#include "g2o/config.h" -#include "g2o/core/base_vertex.h" -#include "g2o/core/hyper_graph_action.h" - -#include - -namespace teb_local_planner -{ - -/** - * @class VertexTimeDiff - * @brief This class stores and wraps a time difference \f$ \Delta T \f$ into a vertex that can be optimized via g2o - */ -class VertexTimeDiff : public g2o::BaseVertex<1, double> -{ -public: - - /** - * @brief Default constructor - * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] - */ - VertexTimeDiff(bool fixed = false) - { - setToOriginImpl(); - setFixed(fixed); - } - - /** - * @brief Construct the TimeDiff vertex with a value - * @param dt time difference value of the vertex - * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] - */ - VertexTimeDiff(double dt, bool fixed = false) - { - _estimate = dt; - setFixed(fixed); - } - - /** - * @brief Access the timediff value of the vertex - * @see estimate - * @return reference to dt - */ - inline double& dt() {return _estimate;} - - /** - * @brief Access the timediff value of the vertex (read-only) - * @see estimate - * @return const reference to dt - */ - inline const double& dt() const {return _estimate;} - - /** - * @brief Set the underlying TimeDiff estimate \f$ \Delta T \f$ to default. - */ - virtual void setToOriginImpl() override - { - _estimate = 0.1; - } - - /** - * @brief Define the update increment \f$ \Delta T_{k+1} = \Delta T_k + update \f$. - * A simple addition implements what we want. - * @param update increment that should be added to the previous esimate - */ - virtual void oplusImpl(const double* update) override - { - _estimate += *update; - } - - /** - * @brief Read an estimate of \f$ \Delta T \f$ from an input stream - * @param is input stream - * @return always \c true - */ - virtual bool read(std::istream& is) override - { - is >> _estimate; - return true; - } - - /** - * @brief Write the estimate \f$ \Delta T \f$ to an output stream - * @param os output stream - * @return \c true if the export was successful, otherwise \c false - */ - virtual bool write(std::ostream& os) const override - { - os << estimate(); - return os.good(); - } - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -} - -#endif From 4f350f0e4ecc37568493f62be56840f2c2653a5e Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 10 May 2021 07:10:12 -0400 Subject: [PATCH 38/46] Delete teb_config.h --- teb_config.h | 429 --------------------------------------------------- 1 file changed, 429 deletions(-) delete mode 100644 teb_config.h diff --git a/teb_config.h b/teb_config.h deleted file mode 100644 index 2490e993..00000000 --- a/teb_config.h +++ /dev/null @@ -1,429 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef TEB_CONFIG_H_ -#define TEB_CONFIG_H_ - -#include -#include -#include -#include - -#include - - -// Definitions -#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 - - //! 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!] - double global_plan_prune_distance; //!< Distance between robot and via_points of global plan which is used for pruning - 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) - double force_reinit_new_goal_angular; //!< Reinitialize the trajectory if a previous goal is updated with an angular difference of more than the specified value in radians (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) - double min_resolution_collision_check_angular; //! Min angular resolution used during the costmap collision check. If not respected, intermediate samples are added. [rad] - int control_look_ahead_poses; //! Index of the pose used to extract the velocity command - } 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 max_steering_rate; //!< EXPERIMENTAL: Maximum steering rate of a carlike robot (THIS SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT) [deactivate: 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. - double obstacle_cost_exponent; //!< Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default) - } 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) - int max_number_plans_in_current_class; //!< Specify the maximum number of trajectories to try that are in the same homotopy class as the current trajectory (helps avoid local minima) - 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 selection_dropping_probability; //!< At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies. - 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.2 this - } hcp; - - //! Recovery/backup related parameters - struct Recovery - { - bool shrink_horizon_backup; //!< Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues. - double shrink_horizon_min_duration; //!< Specify minimum duration for the reduced horizon in case an infeasible trajectory is detected. - bool oscillation_recovery; //!< Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards) - double oscillation_v_eps; //!< Threshold for the average normalized linear velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected - double oscillation_omega_eps; //!< Threshold for the average normalized angular velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected - double oscillation_recovery_min_duration; //!< Minumum duration [sec] for which the recovery mode is activated after an oscillation is detected. - double oscillation_filter_duration; //!< Filter length/duration [sec] for the detection of oscillations - bool divergence_detection_enable; //!< True to enable divergence detection. - int divergence_detection_max_chi_squared; //!< Maximum acceptable Mahalanobis distance above which it is assumed that the optimization diverged. - } recovery; //!< Parameters related to recovery and backup strategies - - - /** - * @brief Construct the TebConfig using default values. - * @warning If the \b rosparam server or/and \b dynamic_reconfigure (rqt_reconfigure) node are used, - * the default variables will be overwritten: \n - * E.g. if \e base_local_planner is utilized as plugin for the navigation stack, the initialize() method will register a - * dynamic_reconfigure server. A subset (not all but most) of the parameters are considered for dynamic modifications. - * All parameters considered by the dynamic_reconfigure server (and their \b default values) are - * set in \e PROJECT_SRC/cfg/TebLocalPlannerReconfigure.cfg. \n - * In addition the rosparam server can be queried to get parameters e.g. defiend in a launch file. - * The plugin source (or a possible binary source) can call loadRosParamFromNodeHandle() to update the parameters. - * In \e summary, default parameters are loaded in the following order (the right one overrides the left ones): \n - * TebConfig 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.global_plan_prune_distance = 1; - trajectory.exact_arc_length = false; - trajectory.force_reinit_new_goal_dist = 1; - trajectory.force_reinit_new_goal_angular = 0.5 * M_PI; - trajectory.feasibility_check_no_poses = 5; - trajectory.publish_feedback = false; - trajectory.min_resolution_collision_check_angular = M_PI; - trajectory.control_look_ahead_poses = 1; - - // 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.max_steering_rate = 0; - robot.wheelbase = 1.0; - robot.cmd_angle_instead_rotvel = false; - robot.is_footprint_dynamic = false; - robot.use_proportional_saturation = false; - - // GoalTolerance - - goal_tolerance.xy_goal_tolerance = 0.2; - goal_tolerance.yaw_goal_tolerance = 0.2; - goal_tolerance.free_goal_vel = false; - goal_tolerance.trans_stopped_vel = 0.1; - goal_tolerance.theta_stopped_vel = 0.1; - 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; - obstacles.obstacle_proximity_ratio_max_vel = 1; - obstacles.obstacle_proximity_lower_bound = 0; - obstacles.obstacle_proximity_upper_bound = 0.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_max_steering_rate = 1; - optim.weight_optimaltime = 1; - optim.weight_shortest_path = 0; - optim.weight_obstacle = 50; - optim.weight_inflation = 0.1; - optim.weight_dynamic_obstacle = 50; - optim.weight_dynamic_obstacle_inflation = 0.1; - optim.weight_velocity_obstacle_ratio = 0; - optim.weight_viapoint = 1; - optim.weight_prefer_rotdir = 50; - - optim.weight_adapt_factor = 2.0; - optim.obstacle_cost_exponent = 1.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.selection_dropping_probability = 0.0; - - 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; - hcp.delete_detours_backwards = true; - hcp.detours_orientation_tolerance = M_PI / 2.0; - hcp.length_start_orientation_vector = 0.4; - hcp.max_ratio_detours_duration_best_duration = 3.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 From 092830290daf7ac0548ab15203624e561c1c4a01 Mon Sep 17 00:00:00 2001 From: chelseabright96 <82758782+chelseabright96@users.noreply.github.com> Date: Mon, 31 May 2021 20:21:50 +0200 Subject: [PATCH 39/46] Update visualize_vel_and_steering.py --- scripts/visualize_vel_and_steering.py | 31 ++++++++++++++++++++++----- 1 file changed, 26 insertions(+), 5 deletions(-) diff --git a/scripts/visualize_vel_and_steering.py b/scripts/visualize_vel_and_steering.py index 34157f1d..de89fcd1 100644 --- a/scripts/visualize_vel_and_steering.py +++ b/scripts/visualize_vel_and_steering.py @@ -20,7 +20,7 @@ def feedback_callback(data): trajectory = data.trajectories[data.selected_trajectory_idx].trajectory -def plot_velocity_profile(fig, ax_v, ax_omega, ax_steering, t, v, omega, steering): +def plot_velocity_profile(fig, ax_v, ax_omega, ax_steering, ax_rate, t, v, s, omega, steering, total_steering, av_steerrate, sum_steerrate): ax_v.cla() ax_v.grid() ax_v.set_ylabel('Trans. velocity [m/s]') @@ -28,17 +28,25 @@ def plot_velocity_profile(fig, ax_v, ax_omega, ax_steering, t, v, omega, steerin ax_omega.cla() ax_omega.grid() ax_omega.set_ylabel('Rot. velocity [rad/s]') - ax_omega.set_xlabel('Time [s]') + #ax_omega.set_xlabel('Time [s]') ax_omega.plot(t, omega, '-bx') ax_steering.cla() ax_steering.grid() ax_steering.set_ylabel('Steering angle [rad]') - ax_steering.set_xlabel('Time [s]') ax_steering.plot(t, steering, '-bx') + ax_steering.set_title('Total steering: {}'.format(total_steering)) + ax_rate.cla() #Plot steering rate against time + ax_rate.grid() + ax_rate.set_ylabel("Steering rate") + ax_rate.plot(t,s, '-bx') + ax_rate.set_xlabel('Time [s]') + ax_rate.set_title("Average steering rate: {}, sum of steering rate: {}".format(av_steerrate, sum_steerrate)) fig.canvas.draw() + + def velocity_plotter(): global trajectory rospy.init_node("visualize_vel_and_steering", anonymous=True) @@ -53,7 +61,7 @@ def velocity_plotter(): rospy.loginfo("Visualizing velocity profile published on '%s'.",topic_name) rospy.loginfo("Make sure to enable rosparam 'publish_feedback' in the teb_local_planner.") - # two subplots sharing the same t axis + # four subplots sharing the same t axis fig, (ax_v, ax_omega, ax_steering) = plotter.subplots(3, sharex=True) plotter.ion() plotter.show() @@ -75,8 +83,21 @@ def velocity_plotter(): steering.append( 0.0 ) else: steering.append( math.atan( wheelbase / point.velocity.linear.x * point.velocity.angular.z ) ) + + #Steering rate + s =[] + for i in range(np.size(np.asarray(steering))-1): + s.append((steering[i+1]-steering[i])/(t[i+1]-t[i])) + s.append(0) + + sum_steerrate=np.sum(np.absolute(np.asarray(s))) + av_steerrate=sum_steerrate/np.size(np.asarray(s)) #Average steering rate + + total_steering=np.sum(np.absolute(np.asarray(steering))) #Sum of steering angles - plot_velocity_profile(fig, ax_v, ax_omega, ax_steering, np.asarray(t), np.asarray(v), np.asarray(omega), np.asarray(steering)) + plot_velocity_profile(fig, ax_v, ax_omega, ax_steering, ax_rate, np.asarray(t), np.asarray(v), np.asarray(s), np.asarray(omega), np.asarray(steering), total_steering, av_steerrate, sum_steerrate) + + r.sleep() From abebee1bb4f94d4208310e59a444a52e67388c77 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Tue, 1 Jun 2021 10:19:44 -0400 Subject: [PATCH 40/46] Update visualize_vel_and_steering.py --- scripts/visualize_vel_and_steering.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/visualize_vel_and_steering.py b/scripts/visualize_vel_and_steering.py index de89fcd1..f5d24665 100644 --- a/scripts/visualize_vel_and_steering.py +++ b/scripts/visualize_vel_and_steering.py @@ -62,7 +62,7 @@ def velocity_plotter(): rospy.loginfo("Make sure to enable rosparam 'publish_feedback' in the teb_local_planner.") # four subplots sharing the same t axis - fig, (ax_v, ax_omega, ax_steering) = plotter.subplots(3, sharex=True) + fig, (ax_v, ax_omega, ax_steering, ax_rate) = plotter.subplots(4, sharex=True) plotter.ion() plotter.show() From 11c56d57fb8c4043d4e09ff3176b85c04bd69b16 Mon Sep 17 00:00:00 2001 From: chelseabright96 <82758782+chelseabright96@users.noreply.github.com> Date: Tue, 8 Jun 2021 20:04:46 +0200 Subject: [PATCH 41/46] Update publish_test_obstacles.py --- scripts/publish_test_obstacles.py | 82 ++++++++++++++++++++++--------- 1 file changed, 58 insertions(+), 24 deletions(-) diff --git a/scripts/publish_test_obstacles.py b/scripts/publish_test_obstacles.py index 800803d9..6bdb2118 100755 --- a/scripts/publish_test_obstacles.py +++ b/scripts/publish_test_obstacles.py @@ -17,41 +17,75 @@ def publish_obstacle_msg(): obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map - # Add point obstacle + # Add line obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[0].id = 0 - obstacle_msg.obstacles[0].polygon.points = [Point32()] - obstacle_msg.obstacles[0].polygon.points[0].x = 1.5 - obstacle_msg.obstacles[0].polygon.points[0].y = 0 - obstacle_msg.obstacles[0].polygon.points[0].z = 0 - + line_start = Point32() + line_start.x = -4.5 + line_start.y = 1 + #line_start.y = -3 + line_end = Point32() + line_end.x = -1.5 + line_end.y = 1 + #line_end.y = -4 + obstacle_msg.obstacles[0].polygon.points = [line_start, line_end] - # Add line obstacle + # Add line obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[1].id = 1 line_start = Point32() - line_start.x = -2.5 - line_start.y = 0.5 + line_start.x = -4.5 + line_start.y = -1 + line_start.y = -1 #line_start.y = -3 line_end = Point32() - line_end.x = -2.5 - line_end.y = 2 + line_end.x = 4.5 + line_end.y = -1 #line_end.y = -4 obstacle_msg.obstacles[1].polygon.points = [line_start, line_end] - - # Add polygon obstacle + + # Add line obstacle + # obstacle_msg.obstacles.append(ObstacleMsg()) + # obstacle_msg.obstacles[2].id = 2 + # line_start = Point32() + # line_start.x = 1.5 + # line_start.y = -1 + #line_start.y = -3 + # line_end = Point32() + # line_end.x = 4.5 + # line_end.y = -1 + #line_end.y = -4 + # obstacle_msg.obstacles[2].polygon.points = [line_start, line_end] + + # Add line obstacle obstacle_msg.obstacles.append(ObstacleMsg()) - obstacle_msg.obstacles[1].id = 2 - v1 = Point32() - v1.x = -1 - v1.y = -1 - v2 = Point32() - v2.x = -0.5 - v2.y = -1.5 - v3 = Point32() - v3.x = 0 - v3.y = -1 - obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3] + obstacle_msg.obstacles[2].id = 2 + line_start = Point32() + line_start.x = 1.5 + line_start.y = 1 + #line_start.y = -3 + line_end = Point32() + line_end.x = 4.5 + line_end.y = 1 + #line_end.y = -4 + obstacle_msg.obstacles[2].polygon.points = [line_start, line_end] + + # Add line obstacle + obstacle_msg.obstacles.append(ObstacleMsg()) + obstacle_msg.obstacles[3].id = 3 + line_start = Point32() + line_start.x = 0 + line_start.y = -1 + #line_start.y = -3 + line_end = Point32() + line_end.x = 0 + line_end.y = 0.5 + #line_end.y = -4 + obstacle_msg.obstacles[3].polygon.points = [line_start, line_end] + + + + r = rospy.Rate(10) # 10hz From 73ca5d8aba75c48a63f619b373c520a3d27c3410 Mon Sep 17 00:00:00 2001 From: chelseabright96 <82758782+chelseabright96@users.noreply.github.com> Date: Mon, 14 Jun 2021 12:30:06 +0200 Subject: [PATCH 42/46] Update visualize_vel_and_steering.py --- scripts/visualize_vel_and_steering.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/scripts/visualize_vel_and_steering.py b/scripts/visualize_vel_and_steering.py index f5d24665..ceb2bdab 100644 --- a/scripts/visualize_vel_and_steering.py +++ b/scripts/visualize_vel_and_steering.py @@ -89,6 +89,14 @@ def velocity_plotter(): for i in range(np.size(np.asarray(steering))-1): s.append((steering[i+1]-steering[i])/(t[i+1]-t[i])) s.append(0) + + #Steering rate approximated by central differences + #s =[] + #s.append(0) + #for i in range(np.size(np.asarray(steering))-2): + # s.append((steering[i+2]-steering[i])/(2*(-t[i]+t[i+2]))) + 0s.append(0) + sum_steerrate=np.sum(np.absolute(np.asarray(s))) av_steerrate=sum_steerrate/np.size(np.asarray(s)) #Average steering rate From c692266df1028f0706c1370adca7b014aa884137 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 28 Jun 2021 16:41:39 +0200 Subject: [PATCH 43/46] Delete TebLocalPlannerReconfigure.cfg --- cfg/TebLocalPlannerReconfigure.cfg | 440 ----------------------------- 1 file changed, 440 deletions(-) delete mode 100644 cfg/TebLocalPlannerReconfigure.cfg diff --git a/cfg/TebLocalPlannerReconfigure.cfg b/cfg/TebLocalPlannerReconfigure.cfg deleted file mode 100644 index 5cf30257..00000000 --- a/cfg/TebLocalPlannerReconfigure.cfg +++ /dev/null @@ -1,440 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import * -#from local_planner_limits import add_generic_localplanner_params - -gen = ParameterGenerator() - -# This unusual line allows to reuse existing parameter definitions -# that concern all localplanners -#add_generic_localplanner_params(gen) - -# For integers and doubles: -# Name Type Reconfiguration level -# Description -# Default Min Max - - -grp_trajectory = gen.add_group("Trajectory", type="tab") - -# Trajectory -grp_trajectory.add("teb_autosize", bool_t, 0, - "Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended)", - True) - -grp_trajectory.add("dt_ref", double_t, 0, - "Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)", - 0.3, 0.01, 1) - -grp_trajectory.add("dt_hysteresis", double_t, 0, - "Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref", - 0.1, 0.002, 0.5) - -grp_trajectory.add("global_plan_overwrite_orientation", bool_t, 0, - "Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically", - True) - -grp_trajectory.add("allow_init_with_backwards_motion", bool_t, 0, - "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)", - False) - -grp_trajectory.add("max_global_plan_lookahead_dist", double_t, 0, - "Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]", - 3.0, 0, 50.0) - -grp_trajectory.add("force_reinit_new_goal_dist", double_t, 0, - "Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)", - 1.0, 0.0, 10.0) - -grp_trajectory.add("force_reinit_new_goal_angular", double_t, 0, - "Force the planner to reinitialize the trajectory if a previous goal is updated with a rotational difference of more than the specified value in radians (skip hot-starting)", - 0.78, 0.0, 4.0) - -grp_trajectory.add("feasibility_check_no_poses", int_t, 0, - "Specify up to which pose on the predicted plan the feasibility should be checked each sampling interval", - 5, 0, 50) - -grp_trajectory.add("exact_arc_length", bool_t, 0, - "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.", - False) - -grp_trajectory.add("publish_feedback", bool_t, 0, - "Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)", - False) - -grp_trajectory.add("visualize_with_time_as_z_axis_scale", double_t, 0, - "If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.", - 0, 0, 1) - -# ViaPoints -grp_viapoints = gen.add_group("ViaPoints", type="tab") - -grp_viapoints.add("global_plan_viapoint_sep", double_t, 0, - "Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]", - -0.1, -0.1, 5.0) - -grp_viapoints.add("via_points_ordered", bool_t, 0, - "If true, the planner adheres to the order of via-points in the storage container", - False) - -# Robot -grp_robot = gen.add_group("Robot", type="tab") - -grp_robot.add("max_vel_x", double_t, 0, - "Maximum translational velocity of the robot", - 0.4, 0.01, 100) - -grp_robot.add("max_vel_x_backwards", double_t, 0, - "Maximum translational velocity of the robot for driving backwards", - 0.2, 0.01, 100) - -grp_robot.add("max_vel_theta", double_t, 0, - "Maximum angular velocity of the robot", - 0.3, 0.01, 100) - -grp_robot.add("acc_lim_x", double_t, 0, - "Maximum translational acceleration of the robot", - 0.5, 0.01, 100) - -grp_robot.add("acc_lim_theta", double_t, 0, - "Maximum angular acceleration of the robot", - 0.5, 0.01, 100) - -grp_robot.add("is_footprint_dynamic", bool_t, 0, - "If true, updated the footprint before checking trajectory feasibility", - False) - -grp_robot.add("use_proportional_saturation", bool_t, 0, - "If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually", - False) -grp_robot.add("transform_tolerance", double_t, 0, - "Tolerance when querying the TF Tree for a transformation (seconds)", - 0.5, 0.001, 20) - -# Robot/Carlike - -grp_robot_carlike = grp_robot.add_group("Carlike", type="hide") - -grp_robot_carlike.add("min_turning_radius", double_t, 0, - "Minimum turning radius of a carlike robot (diff-drive robot: zero)", - 0.0, 0.0, 50.0) - -grp_robot_carlike.add("max_steering_rate", double_t, 0, - "EXPERIMENTAL: Maximum steering rate of a carlike robot (THIS SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT) [deactivate: zero]", - 0.0, 1.0, 10.0) - -grp_robot_carlike.add("wheelbase", double_t, 0, - "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!", - 1.0, -10.0, 10.0) - -grp_robot_carlike.add("cmd_angle_instead_rotvel", bool_t, 0, - "Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')", - False) - -# Robot/Omni - -grp_robot_omni = grp_robot.add_group("Omnidirectional", type="hide") - -grp_robot_omni.add("max_vel_y", double_t, 0, - "Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)", - 0.0, 0.0, 100) - -grp_robot_omni.add("acc_lim_y", double_t, 0, - "Maximum strafing acceleration of the robot", - 0.5, 0.01, 100) - -# GoalTolerance -grp_goal = gen.add_group("GoalTolerance", type="tab") - -grp_goal.add("xy_goal_tolerance", double_t, 0, - "Allowed final euclidean distance to the goal position", - 0.2, 0.001, 10) - -grp_goal.add("yaw_goal_tolerance", double_t, 0, - "Allowed final orientation error to the goal orientation", - 0.1, 0.001, 3.2) - -grp_goal.add("free_goal_vel", bool_t, 0, - "Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed)", - False) - -grp_goal.add("trans_stopped_vel", double_t, 0, - "Below what maximum velocity we consider the robot to be stopped in translation", - 0.1, 0.0) - -grp_goal.add("theta_stopped_vel", double_t, 0, - "Below what maximum rotation velocity we consider the robot to be stopped in rotation", - 0.1, 0.0) - -# Obstacles -grp_obstacles = gen.add_group("Obstacles", type="tab") - -grp_obstacles.add("min_obstacle_dist", double_t, 0, - "Minimum desired separation from obstacles", - 0.5, 0, 10) - -grp_obstacles.add("inflation_dist", double_t, 0, - "Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)", - 0.6, 0, 15) - -grp_obstacles.add("dynamic_obstacle_inflation_dist", double_t, 0, - "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)", - 0.6, 0, 15) - -grp_obstacles.add("include_dynamic_obstacles", bool_t, 0, - "Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static.", - False) - -grp_obstacles.add("include_costmap_obstacles", bool_t, 0, - "Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)", - True) - -grp_obstacles.add("legacy_obstacle_association", bool_t, 0, - "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).", - False) - -grp_obstacles.add("obstacle_association_force_inclusion_factor", double_t, 0, - "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.", - 1.5, 0.0, 100.0) - -grp_obstacles.add("obstacle_association_cutoff_factor", double_t, 0, - "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.", - 5.0, 1.0, 100.0) - -grp_obstacles.add("costmap_obstacles_behind_robot_dist", double_t, 0, - "Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)", - 1.5, 0.0, 20.0) - -grp_obstacles.add("obstacle_poses_affected", int_t, 0, - "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", - 30, 0, 200) - -# Obstacle - Velocity ratio parameters -grp_obstacles_velocity_limit = grp_obstacles.add_group("Reduce velocity near obstacles") - -grp_obstacles_velocity_limit.add("obstacle_proximity_ratio_max_vel", double_t, 0, - "Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to static obstacles", - 1, 0, 1) - -grp_obstacles_velocity_limit.add("obstacle_proximity_lower_bound", double_t, 0, - "Distance to a static obstacle for which the velocity should be lower", - 0, 0, 10) - -grp_obstacles_velocity_limit.add("obstacle_proximity_upper_bound", double_t, 0, - "Distance to a static obstacle for which the velocity should be higher", - 0.5, 0, 10) - -# Optimization -grp_optimization = gen.add_group("Optimization", type="tab") - -grp_optimization.add("no_inner_iterations", int_t, 0, - "Number of solver iterations called in each outerloop iteration", - 5, 1, 100) - -grp_optimization.add("no_outer_iterations", int_t, 0, - "Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations", - 4, 1, 100) - -grp_optimization.add("optimization_activate", bool_t, 0, - "Activate the optimization", - True) - -grp_optimization.add("optimization_verbose", bool_t, 0, - "Print verbose information", - False) - -grp_optimization.add("penalty_epsilon", double_t, 0, - "Add a small safty margin to penalty functions for hard-constraint approximations", - 0.1, 0, 1.0) - -grp_optimization.add("weight_max_vel_x", double_t, 0, - "Optimization weight for satisfying the maximum allowed translational velocity", - 2, 0, 1000) - -grp_optimization.add("weight_max_vel_y", double_t, 0, - "Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)", - 2, 0, 1000) - -grp_optimization.add("weight_max_vel_theta", double_t, 0, - "Optimization weight for satisfying the maximum allowed angular velocity", - 1, 0, 1000) - -grp_optimization.add("weight_acc_lim_x", double_t, 0, - "Optimization weight for satisfying the maximum allowed translational acceleration", - 1, 0, 1000) - -grp_optimization.add("weight_acc_lim_y", double_t, 0, - "Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)", - 1, 0, 1000) - -grp_optimization.add("weight_acc_lim_theta", double_t, 0, - "Optimization weight for satisfying the maximum allowed angular acceleration", - 1, 0, 1000) - -grp_optimization.add("weight_kinematics_nh", double_t, 0, - "Optimization weight for satisfying the non-holonomic kinematics", - 1000 , 0, 10000) - -grp_optimization.add("weight_kinematics_forward_drive", double_t, 0, - "Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)", - 1, 0, 1000) - -grp_optimization.add("weight_kinematics_turning_radius", double_t, 0, - "Optimization weight for enforcing a minimum turning radius (carlike robots)", - 1, 0, 1000) - -grp_optimization.add("weight_max_steering_rate", double_t, 0, - "EXPERIMENTAL: Optimization weight for enforcing a minimum steering rate of a carlike robot (TRY TO KEEP THE WEIGHT LOW OR DEACTIVATE, SINCE IT SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT)", - 0.5, 10, 100) - -grp_optimization.add("weight_optimaltime", double_t, 0, - "Optimization weight for contracting the trajectory w.r.t. transition time", - 1, 0, 1000) - -grp_optimization.add("weight_shortest_path", double_t, 0, - "Optimization weight for contracting the trajectory w.r.t. path length", - 0, 0, 100) - -grp_optimization.add("weight_obstacle", double_t, 0, - "Optimization weight for satisfying a minimum seperation from obstacles", - 50, 0, 1000) - -grp_optimization.add("weight_inflation", double_t, 0, - "Optimization weight for the inflation penalty (should be small)", - 0.1, 0, 10) - -grp_optimization.add("weight_dynamic_obstacle", double_t, 0, - "Optimization weight for satisfying a minimum seperation from dynamic obstacles", - 50, 0, 1000) - -grp_optimization.add("weight_dynamic_obstacle_inflation", double_t, 0, - "Optimization weight for the inflation penalty of dynamic obstacles (should be small)", - 0.1, 0, 10) - -grp_optimization.add("weight_velocity_obstacle_ratio", double_t, 0, - "Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle", - 0, 0, 1000) - -grp_optimization.add("weight_viapoint", double_t, 0, - "Optimization weight for minimizing the distance to via-points", - 1, 0, 1000) - -grp_optimization.add("weight_adapt_factor", double_t, 0, - "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.", - 2, 1, 100) - -grp_optimization.add("obstacle_cost_exponent", double_t, 0, - "Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)", - 1, 0.01, 100) - - - -# Homotopy Class Planner -grp_hcp = gen.add_group("HCPlanning", type="tab") - -grp_hcp.add("enable_multithreading", bool_t, 0, - "Activate multiple threading for planning multiple trajectories in parallel", - True) - -grp_hcp.add("max_number_classes", int_t, 0, - "Specify the maximum number of allowed alternative homotopy classes (limits computational effort)", - 5, 1, 100) - -grp_hcp.add("max_number_plans_in_current_class", int_t, 0, - "Max number of trajectories to try that are in the same homotopy class as the current best trajectory (setting this to 2 or more helps avoid local minima). Must be <= max_number_classes", - 1, 1, 10) - -grp_hcp.add("selection_cost_hysteresis", double_t, 0, - "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)", - 1.0, 0, 2) - - -grp_hcp.add("selection_prefer_initial_plan", double_t, 0, - "Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.)", - 0.95, 0, 1) - -grp_hcp.add("selection_obst_cost_scale", double_t, 0, - "Extra scaling of obstacle cost terms just for selecting the 'best' candidate (new_obst_cost: obst_cost*factor)", - 2.0, 0, 1000) - -grp_hcp.add("selection_viapoint_cost_scale", double_t, 0, - "Extra scaling of via-point cost terms just for selecting the 'best' candidate. (new_viapt_cost: viapt_cost*factor)", - 1.0, 0, 100) - -grp_hcp.add("selection_alternative_time_cost", bool_t, 0, - "If true, time cost is replaced by the total transition time.", - False) - -grp_hcp.add("selection_dropping_probability", double_t, 0, - "At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies.", - 0.0, 0.0, 1.0) - -grp_hcp.add("switching_blocking_period", double_t, 0, - "Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed", - 0.0, 0.0, 60) - -grp_hcp.add("roadmap_graph_no_samples", int_t, 0, - "Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off", - 15, 1, 100) - -grp_hcp.add("roadmap_graph_area_width", double_t, 0, - "Specify the width of the area in which sampled will be generated between start and goal [m] (the height equals the start-goal distance)", - 5, 0.1, 20) - -grp_hcp.add("roadmap_graph_area_length_scale", double_t, 0, - "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!)", - 1.0, 0.5, 2) - -grp_hcp.add("h_signature_prescaler", double_t, 0, - "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.2 Date: Mon, 28 Jun 2021 16:42:16 +0200 Subject: [PATCH 44/46] Add files via upload --- cfg/TebLocalPlannerReconfigure.cfg | 440 +++++++++++++++++++++++++++++ 1 file changed, 440 insertions(+) create mode 100644 cfg/TebLocalPlannerReconfigure.cfg diff --git a/cfg/TebLocalPlannerReconfigure.cfg b/cfg/TebLocalPlannerReconfigure.cfg new file mode 100644 index 00000000..039d1bb2 --- /dev/null +++ b/cfg/TebLocalPlannerReconfigure.cfg @@ -0,0 +1,440 @@ +#!/usr/bin/env python + +from dynamic_reconfigure.parameter_generator_catkin import * +#from local_planner_limits import add_generic_localplanner_params + +gen = ParameterGenerator() + +# This unusual line allows to reuse existing parameter definitions +# that concern all localplanners +#add_generic_localplanner_params(gen) + +# For integers and doubles: +# Name Type Reconfiguration level +# Description +# Default Min Max + + +grp_trajectory = gen.add_group("Trajectory", type="tab") + +# Trajectory +grp_trajectory.add("teb_autosize", bool_t, 0, + "Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended)", + True) + +grp_trajectory.add("dt_ref", double_t, 0, + "Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)", + 0.3, 0.01, 1) + +grp_trajectory.add("dt_hysteresis", double_t, 0, + "Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref", + 0.1, 0.002, 0.5) + +grp_trajectory.add("global_plan_overwrite_orientation", bool_t, 0, + "Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically", + True) + +grp_trajectory.add("allow_init_with_backwards_motion", bool_t, 0, + "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)", + False) + +grp_trajectory.add("max_global_plan_lookahead_dist", double_t, 0, + "Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]", + 3.0, 0, 50.0) + +grp_trajectory.add("force_reinit_new_goal_dist", double_t, 0, + "Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)", + 1.0, 0.0, 10.0) + +grp_trajectory.add("force_reinit_new_goal_angular", double_t, 0, + "Force the planner to reinitialize the trajectory if a previous goal is updated with a rotational difference of more than the specified value in radians (skip hot-starting)", + 0.78, 0.0, 4.0) + +grp_trajectory.add("feasibility_check_no_poses", int_t, 0, + "Specify up to which pose on the predicted plan the feasibility should be checked each sampling interval", + 5, 0, 50) + +grp_trajectory.add("exact_arc_length", bool_t, 0, + "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.", + False) + +grp_trajectory.add("publish_feedback", bool_t, 0, + "Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)", + False) + +grp_trajectory.add("visualize_with_time_as_z_axis_scale", double_t, 0, + "If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.", + 0, 0, 1) + +# ViaPoints +grp_viapoints = gen.add_group("ViaPoints", type="tab") + +grp_viapoints.add("global_plan_viapoint_sep", double_t, 0, + "Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]", + -0.1, -0.1, 5.0) + +grp_viapoints.add("via_points_ordered", bool_t, 0, + "If true, the planner adheres to the order of via-points in the storage container", + False) + +# Robot +grp_robot = gen.add_group("Robot", type="tab") + +grp_robot.add("max_vel_x", double_t, 0, + "Maximum translational velocity of the robot", + 0.4, 0.01, 100) + +grp_robot.add("max_vel_x_backwards", double_t, 0, + "Maximum translational velocity of the robot for driving backwards", + 0.2, 0.01, 100) + +grp_robot.add("max_vel_theta", double_t, 0, + "Maximum angular velocity of the robot", + 0.3, 0.01, 100) + +grp_robot.add("acc_lim_x", double_t, 0, + "Maximum translational acceleration of the robot", + 0.5, 0.01, 100) + +grp_robot.add("acc_lim_theta", double_t, 0, + "Maximum angular acceleration of the robot", + 0.5, 0.01, 100) + +grp_robot.add("is_footprint_dynamic", bool_t, 0, + "If true, updated the footprint before checking trajectory feasibility", + False) + +grp_robot.add("use_proportional_saturation", bool_t, 0, + "If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually", + False) +grp_robot.add("transform_tolerance", double_t, 0, + "Tolerance when querying the TF Tree for a transformation (seconds)", + 0.5, 0.001, 20) + +# Robot/Carlike + +grp_robot_carlike = grp_robot.add_group("Carlike", type="hide") + +grp_robot_carlike.add("min_turning_radius", double_t, 0, + "Minimum turning radius of a carlike robot (diff-drive robot: zero)", + 0.0, 0.0, 50.0) + +grp_robot_carlike.add("max_steering_rate", double_t, 0, + "EXPERIMENTAL: Maximum steering rate of a carlike robot (THIS SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT) [deactivate: zero]", + 1.5, 1.0, 10.0) + +grp_robot_carlike.add("wheelbase", double_t, 0, + "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!", + 1.0, -10.0, 10.0) + +grp_robot_carlike.add("cmd_angle_instead_rotvel", bool_t, 0, + "Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')", + False) + +# Robot/Omni + +grp_robot_omni = grp_robot.add_group("Omnidirectional", type="hide") + +grp_robot_omni.add("max_vel_y", double_t, 0, + "Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)", + 0.0, 0.0, 100) + +grp_robot_omni.add("acc_lim_y", double_t, 0, + "Maximum strafing acceleration of the robot", + 0.5, 0.01, 100) + +# GoalTolerance +grp_goal = gen.add_group("GoalTolerance", type="tab") + +grp_goal.add("xy_goal_tolerance", double_t, 0, + "Allowed final euclidean distance to the goal position", + 0.2, 0.001, 10) + +grp_goal.add("yaw_goal_tolerance", double_t, 0, + "Allowed final orientation error to the goal orientation", + 0.1, 0.001, 3.2) + +grp_goal.add("free_goal_vel", bool_t, 0, + "Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed)", + False) + +grp_goal.add("trans_stopped_vel", double_t, 0, + "Below what maximum velocity we consider the robot to be stopped in translation", + 0.1, 0.0) + +grp_goal.add("theta_stopped_vel", double_t, 0, + "Below what maximum rotation velocity we consider the robot to be stopped in rotation", + 0.1, 0.0) + +# Obstacles +grp_obstacles = gen.add_group("Obstacles", type="tab") + +grp_obstacles.add("min_obstacle_dist", double_t, 0, + "Minimum desired separation from obstacles", + 0.5, 0, 10) + +grp_obstacles.add("inflation_dist", double_t, 0, + "Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)", + 0.6, 0, 15) + +grp_obstacles.add("dynamic_obstacle_inflation_dist", double_t, 0, + "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)", + 0.6, 0, 15) + +grp_obstacles.add("include_dynamic_obstacles", bool_t, 0, + "Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static.", + False) + +grp_obstacles.add("include_costmap_obstacles", bool_t, 0, + "Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)", + True) + +grp_obstacles.add("legacy_obstacle_association", bool_t, 0, + "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).", + False) + +grp_obstacles.add("obstacle_association_force_inclusion_factor", double_t, 0, + "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.", + 1.5, 0.0, 100.0) + +grp_obstacles.add("obstacle_association_cutoff_factor", double_t, 0, + "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.", + 5.0, 1.0, 100.0) + +grp_obstacles.add("costmap_obstacles_behind_robot_dist", double_t, 0, + "Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)", + 1.5, 0.0, 20.0) + +grp_obstacles.add("obstacle_poses_affected", int_t, 0, + "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", + 30, 0, 200) + +# Obstacle - Velocity ratio parameters +grp_obstacles_velocity_limit = grp_obstacles.add_group("Reduce velocity near obstacles") + +grp_obstacles_velocity_limit.add("obstacle_proximity_ratio_max_vel", double_t, 0, + "Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to static obstacles", + 1, 0, 1) + +grp_obstacles_velocity_limit.add("obstacle_proximity_lower_bound", double_t, 0, + "Distance to a static obstacle for which the velocity should be lower", + 0, 0, 10) + +grp_obstacles_velocity_limit.add("obstacle_proximity_upper_bound", double_t, 0, + "Distance to a static obstacle for which the velocity should be higher", + 0.5, 0, 10) + +# Optimization +grp_optimization = gen.add_group("Optimization", type="tab") + +grp_optimization.add("no_inner_iterations", int_t, 0, + "Number of solver iterations called in each outerloop iteration", + 5, 1, 100) + +grp_optimization.add("no_outer_iterations", int_t, 0, + "Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations", + 4, 1, 100) + +grp_optimization.add("optimization_activate", bool_t, 0, + "Activate the optimization", + True) + +grp_optimization.add("optimization_verbose", bool_t, 0, + "Print verbose information", + False) + +grp_optimization.add("penalty_epsilon", double_t, 0, + "Add a small safty margin to penalty functions for hard-constraint approximations", + 0.1, 0, 1.0) + +grp_optimization.add("weight_max_vel_x", double_t, 0, + "Optimization weight for satisfying the maximum allowed translational velocity", + 2, 0, 1000) + +grp_optimization.add("weight_max_vel_y", double_t, 0, + "Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)", + 2, 0, 1000) + +grp_optimization.add("weight_max_vel_theta", double_t, 0, + "Optimization weight for satisfying the maximum allowed angular velocity", + 1, 0, 1000) + +grp_optimization.add("weight_acc_lim_x", double_t, 0, + "Optimization weight for satisfying the maximum allowed translational acceleration", + 1, 0, 1000) + +grp_optimization.add("weight_acc_lim_y", double_t, 0, + "Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)", + 1, 0, 1000) + +grp_optimization.add("weight_acc_lim_theta", double_t, 0, + "Optimization weight for satisfying the maximum allowed angular acceleration", + 1, 0, 1000) + +grp_optimization.add("weight_kinematics_nh", double_t, 0, + "Optimization weight for satisfying the non-holonomic kinematics", + 1000 , 0, 10000) + +grp_optimization.add("weight_kinematics_forward_drive", double_t, 0, + "Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)", + 1, 0, 1000) + +grp_optimization.add("weight_kinematics_turning_radius", double_t, 0, + "Optimization weight for enforcing a minimum turning radius (carlike robots)", + 1, 0, 1000) + +grp_optimization.add("weight_max_steering_rate", double_t, 0, + "EXPERIMENTAL: Optimization weight for enforcing a minimum steering rate of a carlike robot (TRY TO KEEP THE WEIGHT LOW OR DEACTIVATE, SINCE IT SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT)", + 0, 0, 1000) + +grp_optimization.add("weight_optimaltime", double_t, 0, + "Optimization weight for contracting the trajectory w.r.t. transition time", + 1, 0, 1000) + +grp_optimization.add("weight_shortest_path", double_t, 0, + "Optimization weight for contracting the trajectory w.r.t. path length", + 0, 0, 100) + +grp_optimization.add("weight_obstacle", double_t, 0, + "Optimization weight for satisfying a minimum seperation from obstacles", + 50, 0, 1000) + +grp_optimization.add("weight_inflation", double_t, 0, + "Optimization weight for the inflation penalty (should be small)", + 0.1, 0, 10) + +grp_optimization.add("weight_dynamic_obstacle", double_t, 0, + "Optimization weight for satisfying a minimum seperation from dynamic obstacles", + 50, 0, 1000) + +grp_optimization.add("weight_dynamic_obstacle_inflation", double_t, 0, + "Optimization weight for the inflation penalty of dynamic obstacles (should be small)", + 0.1, 0, 10) + +grp_optimization.add("weight_velocity_obstacle_ratio", double_t, 0, + "Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle", + 0, 0, 1000) + +grp_optimization.add("weight_viapoint", double_t, 0, + "Optimization weight for minimizing the distance to via-points", + 1, 0, 1000) + +grp_optimization.add("weight_adapt_factor", double_t, 0, + "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.", + 2, 1, 100) + +grp_optimization.add("obstacle_cost_exponent", double_t, 0, + "Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)", + 1, 0.01, 100) + + + +# Homotopy Class Planner +grp_hcp = gen.add_group("HCPlanning", type="tab") + +grp_hcp.add("enable_multithreading", bool_t, 0, + "Activate multiple threading for planning multiple trajectories in parallel", + True) + +grp_hcp.add("max_number_classes", int_t, 0, + "Specify the maximum number of allowed alternative homotopy classes (limits computational effort)", + 5, 1, 100) + +grp_hcp.add("max_number_plans_in_current_class", int_t, 0, + "Max number of trajectories to try that are in the same homotopy class as the current best trajectory (setting this to 2 or more helps avoid local minima). Must be <= max_number_classes", + 1, 1, 10) + +grp_hcp.add("selection_cost_hysteresis", double_t, 0, + "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)", + 1.0, 0, 2) + + +grp_hcp.add("selection_prefer_initial_plan", double_t, 0, + "Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.)", + 0.95, 0, 1) + +grp_hcp.add("selection_obst_cost_scale", double_t, 0, + "Extra scaling of obstacle cost terms just for selecting the 'best' candidate (new_obst_cost: obst_cost*factor)", + 2.0, 0, 1000) + +grp_hcp.add("selection_viapoint_cost_scale", double_t, 0, + "Extra scaling of via-point cost terms just for selecting the 'best' candidate. (new_viapt_cost: viapt_cost*factor)", + 1.0, 0, 100) + +grp_hcp.add("selection_alternative_time_cost", bool_t, 0, + "If true, time cost is replaced by the total transition time.", + False) + +grp_hcp.add("selection_dropping_probability", double_t, 0, + "At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies.", + 0.0, 0.0, 1.0) + +grp_hcp.add("switching_blocking_period", double_t, 0, + "Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed", + 0.0, 0.0, 60) + +grp_hcp.add("roadmap_graph_no_samples", int_t, 0, + "Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off", + 15, 1, 100) + +grp_hcp.add("roadmap_graph_area_width", double_t, 0, + "Specify the width of the area in which sampled will be generated between start and goal [m] (the height equals the start-goal distance)", + 5, 0.1, 20) + +grp_hcp.add("roadmap_graph_area_length_scale", double_t, 0, + "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!)", + 1.0, 0.5, 2) + +grp_hcp.add("h_signature_prescaler", double_t, 0, + "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.2 Date: Mon, 28 Jun 2021 16:44:27 +0200 Subject: [PATCH 45/46] Delete edge_velocity.h --- .../g2o_types/edge_velocity.h | 489 ------------------ 1 file changed, 489 deletions(-) delete mode 100644 include/teb_local_planner/g2o_types/edge_velocity.h diff --git a/include/teb_local_planner/g2o_types/edge_velocity.h b/include/teb_local_planner/g2o_types/edge_velocity.h deleted file mode 100644 index b01c7e36..00000000 --- a/include/teb_local_planner/g2o_types/edge_velocity.h +++ /dev/null @@ -1,489 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef EDGE_VELOCITY_H -#define EDGE_VELOCITY_H - -#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) - { - 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.0 * (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); - - 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... - - /** - * @brief Jacobi matrix of the cost function specified in computeError(). - */ - void linearizeOplus() - { - 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]); - - Eigen::Vector2d deltaS = conf2->position() - conf1->position(); - double dist = deltaS.norm(); - double aux1 = dist*deltaT->estimate(); - double aux2 = 1/deltaT->estimate(); - - double vel = dist * aux2; - double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) * aux2; - - double dev_border_vel = penaltyBoundToIntervalDerivative(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon); - double dev_border_omega = penaltyBoundToIntervalDerivative(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon); - - _jacobianOplus[0].resize(2,3); // conf1 - _jacobianOplus[1].resize(2,3); // conf2 - _jacobianOplus[2].resize(2,1); // deltaT - -// if (aux1==0) aux1=1e-6; -// if (aux2==0) aux2=1e-6; - - if (dev_border_vel!=0) - { - double aux3 = dev_border_vel / aux1; - _jacobianOplus[0](0,0) = -deltaS[0] * aux3; // vel x1 - _jacobianOplus[0](0,1) = -deltaS[1] * aux3; // vel y1 - _jacobianOplus[1](0,0) = deltaS[0] * aux3; // vel x2 - _jacobianOplus[1](0,1) = deltaS[1] * aux3; // vel y2 - _jacobianOplus[2](0,0) = -vel * aux2 * dev_border_vel; // vel deltaT - } - else - { - _jacobianOplus[0](0,0) = 0; // vel x1 - _jacobianOplus[0](0,1) = 0; // vel y1 - _jacobianOplus[1](0,0) = 0; // vel x2 - _jacobianOplus[1](0,1) = 0; // vel y2 - _jacobianOplus[2](0,0) = 0; // vel deltaT - } - - if (dev_border_omega!=0) - { - double aux4 = aux2 * dev_border_omega; - _jacobianOplus[2](1,0) = -omega * aux4; // omega deltaT - _jacobianOplus[0](1,2) = -aux4; // omega angle1 - _jacobianOplus[1](1,2) = aux4; // omega angle2 - } - else - { - _jacobianOplus[2](1,0) = 0; // omega deltaT - _jacobianOplus[0](1,2) = 0; // omega angle1 - _jacobianOplus[1](1,2) = 0; // omega angle2 - } - - _jacobianOplus[0](1,0) = 0; // omega x1 - _jacobianOplus[0](1,1) = 0; // omega y1 - _jacobianOplus[1](1,0) = 0; // omega x2 - _jacobianOplus[1](1,1) = 0; // omega y2 - _jacobianOplus[0](0,2) = 0; // vel angle1 - _jacobianOplus[1](0,2) = 0; // vel angle2 - } -#endif -#endif - - -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(); - - _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]); - } - - /** - * @class EdgeSteeringRate - * @brief Edge defining the cost function for limiting the steering rate w.r.t. the current wheelbase parameter - * - * The edge depends on four vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2} \Delta T_i \f$ . - * @remarks This edge requires the TebConfig::Robot::whelbase parameter to be set. - * @remarks Do not forget to call setTebConfig() - */ -class EdgeSteeringRate : public BaseTebMultiEdge<1, double> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeSteeringRate() - { - this->resize(5); // 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 EdgeSteeringRate()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - const VertexPose* conf3 = static_cast(_vertices[2]); - const VertexTimeDiff* dt1 = static_cast(_vertices[3]); - const VertexTimeDiff* dt2 = static_cast(_vertices[4]); - - Eigen::Vector2d delta_s1 = conf2->estimate().position() - conf1->estimate().position(); - Eigen::Vector2d delta_s2 = conf3->estimate().position() - conf2->estimate().position(); - double dist1 = delta_s1.norm(); - double dist2 = delta_s2.norm(); - double angle_diff1 = g2o::normalize_theta( conf2->theta() - conf1->theta() ); - double angle_diff2 = g2o::normalize_theta( conf3->theta() - conf2->theta() ); - - double phi1, phi2; - if (std::abs(dist1) < 1e-12) - { - phi1 = 0; // TODO previous phi? - //ROS_INFO("phi 1 is zero!"); - } - else - { - //dist1 *= fast_sigmoid( 100.0 * (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta())) ); // consider direction - //if (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta()) < 0) - //dist1 = -dist1; - - if (cfg_->trajectory.exact_arc_length) - phi1 = std::atan(cfg_->robot.wheelbase / dist1 * 2.0*std::sin(angle_diff1/2.0)); - else - phi1 = std::atan(cfg_->robot.wheelbase / dist1 * angle_diff1); - - // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan) - // In case if we apply the sign to the angle directly, it seems to work: - phi1 *= fast_sigmoid( 100.0 * (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta())) ); // consider direction - } - - if (std::abs(dist2) < 1e-12) - { - phi2 = phi1; - ROS_INFO("phi 2 is phi1!"); - } - else - { - //dist2 *= fast_sigmoid( 100.0 * (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta())) ); // consider direction - //if (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta()) < 0) - // dist2 = -dist2; - - if (cfg_->trajectory.exact_arc_length) - phi2 = std::atan(cfg_->robot.wheelbase / dist2 * 2.0*std::sin(angle_diff2/2.0)); - else - phi2 = std::atan(cfg_->robot.wheelbase / dist2 * angle_diff2); - - // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan). - // In case if we apply the sign to the angle directly, it seems to work: - phi2 *= fast_sigmoid( 100.0 * (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta())) ); // consider direction - } - - _error[0] = penaltyBoundToInterval(g2o::normalize_theta(phi2 - phi1)*2.0 / (dt1->dt() + dt2->dt()), cfg_->robot.max_steering_rate, 0.0); - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRate::computeError() _error[0]\n",_error[0]); - } - -public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - -//! Corresponds to EdgeSteeringRate but with initial steering angle for the predecessor configuration -class EdgeSteeringRateStart : public BaseTebMultiEdge<1, double> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeSteeringRateStart() - { - 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 EdgeSteeringRateStart()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - const VertexTimeDiff* dt = static_cast(_vertices[2]); - - Eigen::Vector2d delta_s = conf2->estimate().position() - conf1->estimate().position(); - double dist = delta_s.norm(); - double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); - - double phi; - if (std::abs(dist) < 1e-12) - { - ROS_INFO("Start phi equals pervious phi!"); - phi = _measurement; - } - else - { - //dist *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction - //dist *= (double)g2o::sign( delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta()) ); // consider direction - - if (cfg_->trajectory.exact_arc_length) - phi = std::atan(cfg_->robot.wheelbase / dist * 2.0*std::sin(angle_diff/2.0)); - else - phi = std::atan(cfg_->robot.wheelbase / dist * angle_diff); - - // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan). - // In case if we apply the sign to the angle directly, it seems to work: - phi *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction - } - - _error[0] = penaltyBoundToInterval(g2o::normalize_theta(phi - _measurement) / dt->dt(), cfg_->robot.max_steering_rate, 0.0); - - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRateStart::computeError() _error[0]\n",_error[0]); - } - - void setInitialSteeringAngle(double steering_angle) - { - _measurement = steering_angle; - } - -public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - -//! Corresponds to EdgeSteeringRate but with initial steering angle for the successor configuration -class EdgeSteeringRateGoal : public BaseTebMultiEdge<1, double> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeSteeringRateGoal() - { - 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 EdgeSteeringRateGoal()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - const VertexTimeDiff* dt = static_cast(_vertices[2]); - - Eigen::Vector2d delta_s = conf2->estimate().position() - conf1->estimate().position(); - double dist = delta_s.norm(); - double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); - - double phi; - if (std::abs(dist) < 1e-12) - { - ROS_INFO("Goal phi is zero!"); - phi = 0; - } - else - { - //dist *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction - - if (cfg_->trajectory.exact_arc_length) - phi = std::atan(cfg_->robot.wheelbase / dist * 2.0*std::sin(angle_diff/2.0)); - else - phi = std::atan(cfg_->robot.wheelbase / dist * angle_diff); - - // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan). - // In case if we apply the sign to the angle directly, it seems to work: - phi *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction - } - - _error[0] = penaltyBoundToInterval(g2o::normalize_theta(_measurement - phi) / dt->dt(), cfg_->robot.max_steering_rate, 0.0); - - - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRateGoal::computeError() _error[0]\n",_error[0]); - } - - void setGoalSteeringAngle(double steering_angle) - { - _measurement = steering_angle; - } - -public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - - -} // end namespace - -#endif From 394c263a69daf376faf3781b0dc4309ecf8053c5 Mon Sep 17 00:00:00 2001 From: felix657 <82760626+felix657@users.noreply.github.com> Date: Mon, 28 Jun 2021 16:45:19 +0200 Subject: [PATCH 46/46] Add files via upload --- .../g2o_types/edge_velocity.h | 492 ++++++++++++++++++ 1 file changed, 492 insertions(+) create mode 100644 include/teb_local_planner/g2o_types/edge_velocity.h diff --git a/include/teb_local_planner/g2o_types/edge_velocity.h b/include/teb_local_planner/g2o_types/edge_velocity.h new file mode 100644 index 00000000..5aebf342 --- /dev/null +++ b/include/teb_local_planner/g2o_types/edge_velocity.h @@ -0,0 +1,492 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef EDGE_VELOCITY_H +#define EDGE_VELOCITY_H + +#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) + { + 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.0 * (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); + + 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... + + /** + * @brief Jacobi matrix of the cost function specified in computeError(). + */ + void linearizeOplus() + { + 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]); + + Eigen::Vector2d deltaS = conf2->position() - conf1->position(); + double dist = deltaS.norm(); + double aux1 = dist*deltaT->estimate(); + double aux2 = 1/deltaT->estimate(); + + double vel = dist * aux2; + double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) * aux2; + + double dev_border_vel = penaltyBoundToIntervalDerivative(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon); + double dev_border_omega = penaltyBoundToIntervalDerivative(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon); + + _jacobianOplus[0].resize(2,3); // conf1 + _jacobianOplus[1].resize(2,3); // conf2 + _jacobianOplus[2].resize(2,1); // deltaT + +// if (aux1==0) aux1=1e-6; +// if (aux2==0) aux2=1e-6; + + if (dev_border_vel!=0) + { + double aux3 = dev_border_vel / aux1; + _jacobianOplus[0](0,0) = -deltaS[0] * aux3; // vel x1 + _jacobianOplus[0](0,1) = -deltaS[1] * aux3; // vel y1 + _jacobianOplus[1](0,0) = deltaS[0] * aux3; // vel x2 + _jacobianOplus[1](0,1) = deltaS[1] * aux3; // vel y2 + _jacobianOplus[2](0,0) = -vel * aux2 * dev_border_vel; // vel deltaT + } + else + { + _jacobianOplus[0](0,0) = 0; // vel x1 + _jacobianOplus[0](0,1) = 0; // vel y1 + _jacobianOplus[1](0,0) = 0; // vel x2 + _jacobianOplus[1](0,1) = 0; // vel y2 + _jacobianOplus[2](0,0) = 0; // vel deltaT + } + + if (dev_border_omega!=0) + { + double aux4 = aux2 * dev_border_omega; + _jacobianOplus[2](1,0) = -omega * aux4; // omega deltaT + _jacobianOplus[0](1,2) = -aux4; // omega angle1 + _jacobianOplus[1](1,2) = aux4; // omega angle2 + } + else + { + _jacobianOplus[2](1,0) = 0; // omega deltaT + _jacobianOplus[0](1,2) = 0; // omega angle1 + _jacobianOplus[1](1,2) = 0; // omega angle2 + } + + _jacobianOplus[0](1,0) = 0; // omega x1 + _jacobianOplus[0](1,1) = 0; // omega y1 + _jacobianOplus[1](1,0) = 0; // omega x2 + _jacobianOplus[1](1,1) = 0; // omega y2 + _jacobianOplus[0](0,2) = 0; // vel angle1 + _jacobianOplus[1](0,2) = 0; // vel angle2 + } +#endif +#endif + + +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(); + + _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 +}; + + + /** + * @class EdgeSteeringRate + * @brief Edge defining the cost function for limiting the steering rate w.r.t. the current wheelbase parameter + * + * The edge depends on four vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2} \Delta T_i \f$ . + * @remarks This edge requires the TebConfig::Robot::whelbase parameter to be set. + * @remarks Do not forget to call setTebConfig() + */ +class EdgeSteeringRate : public BaseTebMultiEdge<1, double> +{ +public: + + /** + * @brief Construct edge. + */ + EdgeSteeringRate() + { + this->resize(5); // 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 EdgeSteeringRate()"); + const VertexPose* conf1 = static_cast(_vertices[0]); + const VertexPose* conf2 = static_cast(_vertices[1]); + const VertexPose* conf3 = static_cast(_vertices[2]); + const VertexTimeDiff* dt1 = static_cast(_vertices[3]); + const VertexTimeDiff* dt2 = static_cast(_vertices[4]); + + Eigen::Vector2d delta_s1 = conf2->estimate().position() - conf1->estimate().position(); + Eigen::Vector2d delta_s2 = conf3->estimate().position() - conf2->estimate().position(); + double dist1 = delta_s1.norm(); + double dist2 = delta_s2.norm(); + double angle_diff1 = g2o::normalize_theta( conf2->theta() - conf1->theta() ); + double angle_diff2 = g2o::normalize_theta( conf3->theta() - conf2->theta() ); + + double phi1, phi2; + if (std::abs(dist1) < 1e-12) + { + phi1 = 0; // TODO previous phi? + //ROS_INFO("phi 1 is zero!"); + } + else + { + //dist1 *= fast_sigmoid( 100.0 * (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta())) ); // consider direction + //if (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta()) < 0) + //dist1 = -dist1; + + if (cfg_->trajectory.exact_arc_length) + phi1 = std::atan(cfg_->robot.wheelbase / dist1 * 2.0*std::sin(angle_diff1/2.0)); + else + phi1 = std::atan(cfg_->robot.wheelbase / dist1 * angle_diff1); + + // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan) + // In case if we apply the sign to the angle directly, it seems to work: + phi1 *= fast_sigmoid( 100.0 * (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta())) ); // consider direction + } + + if (std::abs(dist2) < 1e-12) + { + phi2 = phi1; + ROS_INFO("phi 2 is phi1!"); + } + else + { + //dist2 *= fast_sigmoid( 100.0 * (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta())) ); // consider direction + //if (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta()) < 0) + // dist2 = -dist2; + + if (cfg_->trajectory.exact_arc_length) + phi2 = std::atan(cfg_->robot.wheelbase / dist2 * 2.0*std::sin(angle_diff2/2.0)); + else + phi2 = std::atan(cfg_->robot.wheelbase / dist2 * angle_diff2); + + // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan). + // In case if we apply the sign to the angle directly, it seems to work: + phi2 *= fast_sigmoid( 100.0 * (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta())) ); // consider direction + } + + _error[0] = penaltyBoundToInterval(g2o::normalize_theta(phi2 - phi1)*2.0 / (dt1->dt() + dt2->dt()), cfg_->robot.max_steering_rate, 0.0); + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRate::computeError() _error[0]\n",_error[0]); + } + +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +//! Corresponds to EdgeSteeringRate but with initial steering angle for the predecessor configuration +class EdgeSteeringRateStart : public BaseTebMultiEdge<1, double> +{ +public: + + /** + * @brief Construct edge. + */ + EdgeSteeringRateStart() + { + 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 EdgeSteeringRateStart()"); + const VertexPose* conf1 = static_cast(_vertices[0]); + const VertexPose* conf2 = static_cast(_vertices[1]); + const VertexTimeDiff* dt = static_cast(_vertices[2]); + + Eigen::Vector2d delta_s = conf2->estimate().position() - conf1->estimate().position(); + double dist = delta_s.norm(); + double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); + + double phi; + if (std::abs(dist) < 1e-12) + { + ROS_INFO("Start phi equals pervious phi!"); + phi = _measurement; + } + else + { + //dist *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction + //dist *= (double)g2o::sign( delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta()) ); // consider direction + + if (cfg_->trajectory.exact_arc_length) + phi = std::atan(cfg_->robot.wheelbase / dist * 2.0*std::sin(angle_diff/2.0)); + else + phi = std::atan(cfg_->robot.wheelbase / dist * angle_diff); + + // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan). + // In case if we apply the sign to the angle directly, it seems to work: + phi *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction + } + + _error[0] = penaltyBoundToInterval(g2o::normalize_theta(phi - _measurement) / dt->dt(), cfg_->robot.max_steering_rate, 0.0); + + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRateStart::computeError() _error[0]\n",_error[0]); + } + + void setInitialSteeringAngle(double steering_angle) + { + _measurement = steering_angle; + } + +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +//! Corresponds to EdgeSteeringRate but with initial steering angle for the successor configuration +class EdgeSteeringRateGoal : public BaseTebMultiEdge<1, double> +{ +public: + + /** + * @brief Construct edge. + */ + EdgeSteeringRateGoal() + { + 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 EdgeSteeringRateGoal()"); + const VertexPose* conf1 = static_cast(_vertices[0]); + const VertexPose* conf2 = static_cast(_vertices[1]); + const VertexTimeDiff* dt = static_cast(_vertices[2]); + + Eigen::Vector2d delta_s = conf2->estimate().position() - conf1->estimate().position(); + double dist = delta_s.norm(); + double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); + + double phi; + if (std::abs(dist) < 1e-12) + { + ROS_INFO("Goal phi is zero!"); + phi = 0; + } + else + { + //dist *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction + + if (cfg_->trajectory.exact_arc_length) + phi = std::atan(cfg_->robot.wheelbase / dist * 2.0*std::sin(angle_diff/2.0)); + else + phi = std::atan(cfg_->robot.wheelbase / dist * angle_diff); + + // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan). + // In case if we apply the sign to the angle directly, it seems to work: + phi *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction + } + + _error[0] = penaltyBoundToInterval(g2o::normalize_theta(_measurement - phi) / dt->dt(), cfg_->robot.max_steering_rate, 0.0); + + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRateGoal::computeError() _error[0]\n",_error[0]); + } + + void setGoalSteeringAngle(double steering_angle) + { + _measurement = steering_angle; + } + +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + + + +} // end namespace + +#endif