diff --git a/cfg/TebLocalPlannerReconfigure.cfg b/cfg/TebLocalPlannerReconfigure.cfg index 2c0be552..77453b80 100755 --- a/cfg/TebLocalPlannerReconfigure.cfg +++ b/cfg/TebLocalPlannerReconfigure.cfg @@ -18,15 +18,15 @@ gen = ParameterGenerator() grp_trajectory = gen.add_group("Trajectory", type="tab") # Trajectory -grp_trajectory.add("teb_autosize", bool_t, 0, +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, +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, +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) @@ -37,69 +37,81 @@ grp_trajectory.add("global_plan_overwrite_orientation", bool_t, 0, 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) - + 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) - + 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) - + 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) - + 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) + 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) + -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) + False) # Robot grp_robot = gen.add_group("Robot", type="tab") -grp_robot.add("max_vel_x", double_t, 0, +grp_robot.add("max_vel_x", double_t, 0, "Maximum translational velocity of the robot", - 0.4, 0.01, 100) + 0.4, 0.01, 100) -grp_robot.add("max_vel_x_backwards", double_t, 0, +grp_robot.add("min_vel_x", double_t, 0, + "Minimum translational velocity of the robot", + 0.0, 0.0, 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) - + 0.2, 0.01, 100) + +grp_robot.add("min_vel_x_backwards", double_t, 0, + "Minimum translational velocity of the robot for driving backwards", + 0.01, 0.01, 100) + grp_robot.add("max_vel_theta", double_t, 0, - "Maximum angular velocity of the robot", - 0.3, 0.01, 100) + "Maximum angular velocity of the robot", + 0.3, 0.01, 100) -grp_robot.add("acc_lim_x", double_t, 0, +grp_robot.add("min_vel_theta", double_t, 0, + "Min angular velocity of the robot", + 0.0, 0.00, 100) + +grp_robot.add("acc_lim_x", double_t, 0, "Maximum translational acceleration of the robot", - 0.5, 0.01, 100) - + 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) - + "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) @@ -116,50 +128,55 @@ grp_robot.add("transform_tolerance", double_t, 0, 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) + "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) + "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, +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) + 0.0, 0.0, 100) + +grp_robot_omni.add("min_vel_y", double_t, 0, + "Minimum translational velocity of the robot", + 0.0, 0.0, 100) -grp_robot_omni.add("acc_lim_y", double_t, 0, + +grp_robot_omni.add("acc_lim_y", double_t, 0, "Maximum strafing acceleration of the robot", - 0.5, 0.01, 100) + 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.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)", +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) - + # 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("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)", @@ -173,28 +190,28 @@ 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)", +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).", + +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) + "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) - + "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) + "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", +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 @@ -213,83 +230,83 @@ grp_obstacles_velocity_limit.add("obstacle_proximity_upper_bound", double_t, 0, 0.5, 0, 10) # Optimization -grp_optimization = gen.add_group("Optimization", type="tab") +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", + "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", +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", +grp_optimization.add("optimization_verbose", bool_t, 0, + "Print verbose information", False) - -grp_optimization.add("penalty_epsilon", double_t, 0, + +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", + 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)", + +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, + +grp_optimization.add("weight_acc_lim_theta", double_t, 0, "Optimization weight for satisfying the maximum allowed angular acceleration", - 1, 0, 1000) + 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 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) + 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, +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, + 50, 0, 1000) + +grp_optimization.add("weight_inflation", double_t, 0, "Optimization weight for the inflation penalty (should be small)", - 0.1, 0, 10) + 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", 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)", @@ -299,58 +316,58 @@ 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_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("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 = gen.add_group("HCPlanning", type="tab") + grp_hcp.add("enable_multithreading", bool_t, 0, - "Activate multiple threading for planning multiple trajectories in parallel", + "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)", + "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_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_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_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_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.", +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.", + "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, @@ -358,47 +375,47 @@ grp_hcp.add("switching_blocking_period", double_t, 0, 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", + "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("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& transformed_plan, double min_separation); - - + + /** * @brief Callback for the dynamic_reconfigure node. - * + * * This callback allows to modify parameters dynamically at runtime without restarting the node * @param config Reference to the dynamic reconfigure config * @param level Dynamic reconfigure level */ void reconfigureCB(TebLocalPlannerReconfigureConfig& config, uint32_t level); - - + + /** - * @brief Callback for custom obstacles that are not obtained from the costmap + * @brief Callback for custom obstacles that are not obtained from the costmap * @param obst_msg pointer to the message containing a list of polygon shaped obstacles */ void customObstacleCB(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg); - + /** * @brief Callback for custom via-points * @param via_points_msg pointer to the message containing a list of via-points @@ -283,7 +283,7 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap /** * @brief Prune global plan such that already passed poses are cut off - * + * * The pose of the robot is transformed into the frame of the global plan by taking the most recent tf transform. * If no valid transformation can be found, the method returns \c false. * The global plan is pruned until the distance to the robot is at least \c dist_behind_robot. @@ -298,12 +298,12 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap */ bool pruneGlobalPlan(const tf2_ros::Buffer& tf, const geometry_msgs::PoseStamped& global_pose, std::vector& global_plan, double dist_behind_robot=1); - + /** * @brief Transforms the global plan of the robot from the planner frame to the local frame (modified). - * - * The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h - * such that the index of the current goal pose is returned as well as + * + * The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h + * such that the index of the current goal pose is returned as well as * the transformation between the global plan and the planning frame. * @param tf A reference to a tf buffer * @param global_plan The plan to be transformed @@ -320,13 +320,13 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap const geometry_msgs::PoseStamped& global_pose, const costmap_2d::Costmap2D& costmap, const std::string& global_frame, double max_plan_length, std::vector& transformed_plan, int* current_goal_idx = NULL, geometry_msgs::TransformStamped* tf_plan_to_global = NULL) const; - + /** * @brief Estimate the orientation of a pose from the global_plan that is treated as a local goal for the local planner. - * + * * If the current (local) goal point is not the final one (global) - * substitute the goal orientation by the angle of the direction vector between - * the local goal and the subsequent pose of the global plan. + * substitute the goal orientation by the angle of the direction vector between + * the local goal and the subsequent pose of the global plan. * This is often helpful, if the global planner does not consider orientations. \n * A moving average filter is utilized to smooth the orientation. * @param global_plan The global plan @@ -338,11 +338,11 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap */ double estimateLocalGoalOrientation(const std::vector& global_plan, const geometry_msgs::PoseStamped& local_goal, int current_goal_idx, const geometry_msgs::TransformStamped& tf_plan_to_global, int moving_average_length=3) const; - - + + /** * @brief Saturate the translational and angular velocity to given limits. - * + * * The limit of the translational velocity for backwards driving can be changed independently. * Do not choose max_vel_x_backwards <= 0. If no backward driving is desired, change the optimization weight for * penalizing backwards driving instead. @@ -353,14 +353,18 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap * @param max_vel_y Maximum strafing velocity (for holonomic robots) * @param max_vel_theta Maximum (absolute) angular velocity * @param max_vel_x_backwards Maximum translational velocity for backwards driving + * @param min_vel_x Minimum translational velocity for forward driving + * @param min_vel_y Minimum strafing velocity (for holonomic robots) + * @param min_vel_theta Minimum (absolute) angular velocity + * @param min_vel_x_backwards Minimum translational velocity for backwards driving */ - void saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y, - double max_vel_theta, double max_vel_x_backwards) const; + void saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double min_vel_x, double max_vel_y, + double min_vel_y, double max_vel_theta, double min_vel_theta, double max_vel_x_backwards, double min_vel_x_backwards) const; + - /** * @brief Convert translational and rotational velocities to a steering angle of a carlike robot - * + * * The conversion is based on the following equations: * - The turning radius is defined by \f$ R = v/omega \f$ * - For a car like robot withe a distance L between both axles, the relation is: \f$ tan(\phi) = L/R \f$ @@ -373,10 +377,10 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap * @return Resulting steering angle in [rad] inbetween [-pi/2, pi/2] */ double convertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius = 0) const; - + /** * @brief Validate current parameter values of the footprint for optimization, obstacle distance and the costmap footprint - * + * * This method prints warnings if validation fails. * @remarks Currently, we only validate the inscribed radius of the footprints * @param opt_inscribed_radius Inscribed radius of the RobotFootprintModel for optimization @@ -384,12 +388,12 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap * @param min_obst_dist desired distance to obstacles */ void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist); - - + + void configureBackupModes(std::vector& transformed_plan, int& goal_idx); - + private: // Definition of member variables @@ -397,22 +401,22 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap costmap_2d::Costmap2DROS* costmap_ros_; //!< Pointer to the costmap ros wrapper, received from the navigation stack costmap_2d::Costmap2D* costmap_; //!< Pointer to the 2d costmap (obtained from the costmap ros wrapper) tf2_ros::Buffer* tf_; //!< pointer to tf buffer - + // internal objects (memory management owned) PlannerInterfacePtr planner_; //!< Instance of the underlying optimal planner class ObstContainer obstacles_; //!< Obstacle vector that should be considered during local trajectory optimization ViaPointContainer via_points_; //!< Container of via-points that should be considered during local trajectory optimization TebVisualizationPtr visualization_; //!< Instance of the visualization class (local/global plan, obstacles, ...) - boost::shared_ptr costmap_model_; + boost::shared_ptr costmap_model_; TebConfig cfg_; //!< Config class that stores and manages all related parameters FailureDetector failure_detector_; //!< Detect if the robot got stucked - + std::vector global_plan_; //!< Store the current global plan - + base_local_planner::OdometryHelperRos odom_helper_; //!< Provides an interface to receive the current velocity from the robot - + pluginlib::ClassLoader costmap_converter_loader_; //!< Load costmap converter plugins at runtime - boost::shared_ptr costmap_converter_; //!< Store the current costmap_converter + boost::shared_ptr costmap_converter_; //!< Store the current costmap_converter boost::shared_ptr< dynamic_reconfigure::Server > dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime ros::Subscriber custom_obst_sub_; //!< Subscriber for custom obstacles received via a ObstacleMsg. @@ -432,24 +436,22 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap ros::Time time_last_oscillation_; //!< Store at which time stamp the last oscillation was detected RotType last_preferred_rotdir_; //!< Store recent preferred turning direction geometry_msgs::Twist last_cmd_; //!< Store the last control command generated in computeVelocityCommands() - - std::vector footprint_spec_; //!< Store the footprint of the robot + + std::vector footprint_spec_; //!< Store the footprint of the robot double robot_inscribed_radius_; //!< The radius of the inscribed circle of the robot (collision possible) double robot_circumscribed_radius; //!< The radius of the circumscribed circle of the robot - + std::string global_frame_; //!< The frame in which the controller will run std::string robot_base_frame_; //!< Used as the base frame id of the robot std::string name_; //!< For use with the ros nodehandle - + // flags bool initialized_; //!< Keeps track about the correct initialization of this class public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; - + }; // end namespace teb_local_planner #endif // TEB_LOCAL_PLANNER_ROS_H_ - - diff --git a/src/teb_config.cpp b/src/teb_config.cpp index f85a35e9..1cea2a03 100644 --- a/src/teb_config.cpp +++ b/src/teb_config.cpp @@ -40,13 +40,13 @@ 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); @@ -68,12 +68,16 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh) 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("min_vel_x", robot.min_vel_x, robot.min_vel_x); + nh.param("min_vel_x_backwards", robot.min_vel_x_backwards, robot.min_vel_x_backwards); + nh.param("min_vel_y", robot.min_vel_y, robot.min_vel_y); + nh.param("min_vel_theta", robot.min_vel_theta, robot.min_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); @@ -106,7 +110,7 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh) 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); @@ -126,42 +130,42 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh) 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", 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("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("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_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("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_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); @@ -176,9 +180,9 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& 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; @@ -193,12 +197,16 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg) 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 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.min_vel_x = cfg.min_vel_x; + robot.min_vel_x_backwards = cfg.min_vel_x_backwards; + robot.min_vel_y = cfg.min_vel_y; + robot.min_vel_theta = cfg.min_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; @@ -206,12 +214,12 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg) 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; - + // Obstacles obstacles.min_obstacle_dist = cfg.min_obstacle_dist; obstacles.inflation_dist = cfg.inflation_dist; @@ -226,7 +234,7 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg) 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; @@ -252,10 +260,10 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg) 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_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; @@ -264,7 +272,7 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg) 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; @@ -274,93 +282,106 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg) hcp.viapoints_all_candidates = cfg.viapoints_all_candidates; hcp.visualize_hc_graph = cfg.visualize_hc_graph; hcp.visualize_with_time_as_z_axis_scale = cfg.visualize_with_time_as_z_axis_scale; - + // Recovery - + recovery.shrink_horizon_backup = cfg.shrink_horizon_backup; recovery.oscillation_recovery = cfg.oscillation_recovery; - + checkParameters(); } - - + + void TebConfig::checkParameters() const { // positive backward velocity? if (robot.max_vel_x_backwards <= 0) ROS_WARN("TebLocalPlannerROS() Param Warning: Do not choose max_vel_x_backwards to be <=0. Disable backwards driving by increasing the optimization weight for penalyzing backwards driving."); - + // bounds smaller than penalty epsilon if (robot.max_vel_x <= optim.penalty_epsilon) ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!"); - + if (robot.max_vel_x_backwards <= optim.penalty_epsilon) ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_x_backwards <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!"); - + if (robot.max_vel_theta <= optim.penalty_epsilon) ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!"); - + + // minimum speed bigger than maximum speed + if (robot.min_vel_x > robot.max_vel_x) + ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_x is smaller than min_vel_x , please adjust so that max_vel_x is bigger!"); + + if (robot.min_vel_y > robot.max_vel_y) + ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_y is smaller than min_vel_y , please adjust so that max_vel_y is bigger!"); + + if (robot.min_vel_theta > robot.max_vel_theta) + ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_theta is smaller than min_vel_theta , please adjust so that max_vel_theta is bigger!"); + + if (robot.min_vel_x_backwards > robot.max_vel_x_backwards) + ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_x_backwards is smaller than min_vel_x_backwards , please adjust so that max_vel_x_backwards is bigger!"); + 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/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp index 6e5414f2..9c4fb446 100644 --- a/src/teb_local_planner_ros.cpp +++ b/src/teb_local_planner_ros.cpp @@ -64,7 +64,7 @@ PLUGINLIB_EXPORT_CLASS(teb_local_planner::TebLocalPlannerROS, mbf_costmap_core:: namespace teb_local_planner { - + TebLocalPlannerROS::TebLocalPlannerROS() : costmap_ros_(NULL), tf_(NULL), costmap_model_(NULL), costmap_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"), @@ -91,23 +91,23 @@ void TebLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm { // check if the plugin is already initialized if(!initialized_) - { + { name_ = name; // create Node Handle with name of plugin (as used in move_base for loading) ros::NodeHandle nh("~/" + name); - + // get parameters of TebConfig via the nodehandle and override the default config - cfg_.loadRosParamFromNodeHandle(nh); - + cfg_.loadRosParamFromNodeHandle(nh); + // reserve some memory for obstacles obstacles_.reserve(500); - - // create visualization instance - visualization_ = TebVisualizationPtr(new TebVisualization(nh, cfg_)); - + + // create visualization instance + visualization_ = TebVisualizationPtr(new TebVisualization(nh, cfg_)); + // create robot footprint/contour model for optimization RobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh); - + // create the planner instance if (cfg_.hcp.enable_homotopy_class_planning) { @@ -119,12 +119,12 @@ void TebLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm planner_ = PlannerInterfacePtr(new TebOptimalPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_)); ROS_INFO("Parallel planning in distinctive topologies disabled."); } - + // init other variables tf_ = tf; costmap_ros_ = costmap_ros; costmap_ = costmap_ros_->getCostmap(); // locking should be done in MoveBase. - + costmap_model_ = boost::make_shared(*costmap_); global_frame_ = costmap_ros_->getGlobalFrameID(); @@ -143,9 +143,9 @@ void TebLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm costmap_converter_->setOdomTopic(cfg_.odom_topic); costmap_converter_->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name)); costmap_converter_->setCostmap2D(costmap_); - + costmap_converter_->startWorker(ros::Rate(cfg_.obstacles.costmap_converter_rate), costmap_, cfg_.obstacles.costmap_converter_spin_thread); - ROS_INFO_STREAM("Costmap conversion plugin " << cfg_.obstacles.costmap_converter_plugin << " loaded."); + ROS_INFO_STREAM("Costmap conversion plugin " << cfg_.obstacles.costmap_converter_plugin << " loaded."); } catch(pluginlib::PluginlibException& ex) { @@ -153,14 +153,14 @@ void TebLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm costmap_converter_.reset(); } } - else + else ROS_INFO("No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles."); - - + + // Get footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices. footprint_spec_ = costmap_ros_->getRobotFootprint(); - costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius); - + costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius); + // init the odom helper to receive the robot's velocity from odom messages odom_helper_.setOdomTopic(cfg_.odom_topic); @@ -168,22 +168,22 @@ void TebLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm dynamic_recfg_ = boost::make_shared< dynamic_reconfigure::Server >(nh); dynamic_reconfigure::Server::CallbackType cb = boost::bind(&TebLocalPlannerROS::reconfigureCB, this, _1, _2); dynamic_recfg_->setCallback(cb); - + // validate optimization footprint and costmap footprint validateFootprints(robot_model->getInscribedRadius(), robot_inscribed_radius_, cfg_.obstacles.min_obstacle_dist); - + // setup callback for custom obstacles custom_obst_sub_ = nh.subscribe("obstacles", 1, &TebLocalPlannerROS::customObstacleCB, this); // setup callback for custom via-points via_points_sub_ = nh.subscribe("via_points", 1, &TebLocalPlannerROS::customViaPointsCB, this); - + // initialize failure detector ros::NodeHandle nh_move_base("~"); double controller_frequency = 5; nh_move_base.param("controller_frequency", controller_frequency, controller_frequency); failure_detector_.setBufferLength(std::round(cfg_.recovery.oscillation_filter_duration*controller_frequency)); - + // set initialized flag initialized_ = true; @@ -211,11 +211,11 @@ bool TebLocalPlannerROS::setPlan(const std::vector& global_plan_ = orig_global_plan; // we do not clear the local planner here, since setPlan is called frequently whenever the global planner updates the plan. - // the local planner checks whether it is required to reinitialize the trajectory or not within each velocity computation step. - + // the local planner checks whether it is required to reinitialize the trajectory or not within each velocity computation step. + // reset goal_reached_ flag goal_reached_ = false; - + return true; } @@ -248,20 +248,20 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt cmd_vel.header.stamp = ros::Time::now(); cmd_vel.header.frame_id = robot_base_frame_; cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0; - goal_reached_ = false; - + goal_reached_ = false; + // Get robot pose geometry_msgs::PoseStamped robot_pose; costmap_ros_->getRobotPose(robot_pose); robot_pose_ = PoseSE2(robot_pose.pose); - + // Get robot velocity geometry_msgs::PoseStamped robot_vel_tf; odom_helper_.getRobotVel(robot_vel_tf); robot_vel_.linear.x = robot_vel_tf.pose.position.x; robot_vel_.linear.y = robot_vel_tf.pose.position.y; robot_vel_.angular.z = tf2::getYaw(robot_vel_tf.pose.orientation); - + // prune global plan to cut off parts of the past (spatially before the robot) pruneGlobalPlan(*tf_, robot_pose, global_plan_, cfg_.trajectory.global_plan_prune_distance); @@ -269,7 +269,7 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt std::vector transformed_plan; int goal_idx; geometry_msgs::TransformStamped tf_plan_to_global; - if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist, + if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist, transformed_plan, &goal_idx, &tf_plan_to_global)) { ROS_WARN("Could not transform the global plan to the frame of the controller"); @@ -294,12 +294,12 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt goal_reached_ = true; return mbf_msgs::ExePathResult::SUCCESS; } - - + + // check if we should enter any backup mode and apply settings configureBackupModes(transformed_plan, goal_idx); - - + + // Return false if the transformed global plan is empty if (transformed_plan.empty()) { @@ -307,7 +307,7 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt message = "Transformed plan is empty"; return mbf_msgs::ExePathResult::INVALID_PATH; } - + // Get current goal point (last point of the transformed plan) robot_goal_.x() = transformed_plan.back().pose.position.x; robot_goal_.y() = transformed_plan.back().pose.position.y; @@ -319,7 +319,7 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt tf2::Quaternion q; q.setRPY(0, 0, robot_goal_.theta()); tf2::convert(q, transformed_plan.back().pose.orientation); - } + } else { robot_goal_.theta() = tf2::getYaw(transformed_plan.back().pose.orientation); @@ -331,23 +331,23 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt transformed_plan.insert(transformed_plan.begin(), geometry_msgs::PoseStamped()); // insert start (not yet initialized) } transformed_plan.front() = robot_pose; // update start - + // clear currently existing obstacles obstacles_.clear(); - + // Update obstacle container with costmap information or polygons provided by a costmap_converter plugin if (costmap_converter_) updateObstacleContainerWithCostmapConverter(); else updateObstacleContainerWithCostmap(); - + // also consider custom obstacles (must be called after other updates, since the container is not cleared) updateObstacleContainerWithCustomObstacles(); - - + + // Do not allow config changes during the following optimization step boost::mutex::scoped_lock cfg_lock(cfg_.configMutex()); - + // Now perform the actual planning // bool success = planner_->plan(robot_pose_, robot_goal_, robot_vel_, cfg_.goal_tolerance.free_goal_vel); // straight line init bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel); @@ -355,14 +355,14 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt { planner_->clearPlanner(); // force reinitialization for next time ROS_WARN("teb_local_planner was not able to obtain a local plan for the current setting."); - + ++no_infeasible_plans_; // increase number of infeasible solutions in a row time_last_infeasible_plan_ = ros::Time::now(); last_cmd_ = cmd_vel.twist; message = "teb_local_planner was not able to obtain a local plan"; return mbf_msgs::ExePathResult::NO_VALID_CMD; } - + // Check feasibility (but within the first few states only) if(cfg_.robot.is_footprint_dynamic) { @@ -379,7 +379,7 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt // now we reset everything to start again with the initialization of new trajectories. planner_->clearPlanner(); ROS_WARN("TebLocalPlannerROS: trajectory is not feasible. Resetting planner..."); - + ++no_infeasible_plans_; // increase number of infeasible solutions in a row time_last_infeasible_plan_ = ros::Time::now(); last_cmd_ = cmd_vel.twist; @@ -399,10 +399,10 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt message = "teb_local_planner velocity command invalid"; return mbf_msgs::ExePathResult::NO_VALID_CMD; } - + // Saturate velocity, if the optimization results violates the constraints (could be possible due to soft constraints). saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, - cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards); + cfg_.robot.max_vel_x, cfg_.robot.min_vel_x, cfg_.robot.max_vel_y, cfg_.robot.min_vel_y, cfg_.robot.max_vel_theta, cfg_.robot.min_vel_theta, cfg_.robot.max_vel_x_backwards, cfg_.robot.min_vel_x_backwards); // convert rot-vel to steering angle if desired (carlike robot). // The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint @@ -423,14 +423,14 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt return mbf_msgs::ExePathResult::NO_VALID_CMD; } } - + // a feasible solution should be found, reset counter no_infeasible_plans_ = 0; - + // store last command (for recovery analysis etc.) last_cmd_ = cmd_vel.twist; - - // Now visualize everything + + // Now visualize everything planner_->visualize(); visualization_->publishObstacles(obstacles_); visualization_->publishViaPoints(via_points_); @@ -453,12 +453,12 @@ bool TebLocalPlannerROS::isGoalReached() void TebLocalPlannerROS::updateObstacleContainerWithCostmap() -{ +{ // Add costmap obstacles if desired if (cfg_.obstacles.include_costmap_obstacles) { Eigen::Vector2d robot_orient = robot_pose_.orientationUnitVec(); - + for (unsigned int i=0; igetSizeInCellsX()-1; ++i) { for (unsigned int j=0; jgetSizeInCellsY()-1; ++j) @@ -467,12 +467,12 @@ void TebLocalPlannerROS::updateObstacleContainerWithCostmap() { Eigen::Vector2d obs; costmap_->mapToWorld(i,j,obs.coeffRef(0), obs.coeffRef(1)); - + // check if obstacle is interesting (e.g. not far behind the robot) Eigen::Vector2d obs_dir = obs-robot_pose_.position(); if ( obs_dir.dot(robot_orient) < 0 && obs_dir.norm() > cfg_.obstacles.costmap_obstacles_behind_robot_dist ) continue; - + obstacles_.push_back(ObstaclePtr(new PointObstacle(obs))); } } @@ -484,7 +484,7 @@ void TebLocalPlannerROS::updateObstacleContainerWithCostmapConverter() { if (!costmap_converter_) return; - + //Get obstacles from costmap converter costmap_converter::ObstacleArrayConstPtr obstacles = costmap_converter_->getObstacles(); if (!obstacles) @@ -535,7 +535,7 @@ void TebLocalPlannerROS::updateObstacleContainerWithCustomObstacles() { // We only use the global header to specify the obstacle coordinate system instead of individual ones Eigen::Affine3d obstacle_to_map_eig; - try + try { geometry_msgs::TransformStamped obstacle_to_map = tf_->lookupTransform(global_frame_, ros::Time(0), custom_obstacle_msg_.header.frame_id, ros::Time(0), @@ -547,7 +547,7 @@ void TebLocalPlannerROS::updateObstacleContainerWithCustomObstacles() ROS_ERROR("%s",ex.what()); obstacle_to_map_eig.setIdentity(); } - + for (size_t i=0; i 0 ) // circle @@ -604,24 +604,24 @@ void TebLocalPlannerROS::updateObstacleContainerWithCustomObstacles() void TebLocalPlannerROS::updateViaPointsContainer(const std::vector& transformed_plan, double min_separation) { via_points_.clear(); - + if (min_separation<=0) return; - + std::size_t prev_idx = 0; for (std::size_t i=1; i < transformed_plan.size(); ++i) // skip first one, since we do not need any point before the first min_separation [m] { // check separation to the previous via-point inserted if (distance_points2d( transformed_plan[prev_idx].pose.position, transformed_plan[i].pose.position ) < min_separation) continue; - + // add via-point via_points_.push_back( Eigen::Vector2d( transformed_plan[i].pose.position.x, transformed_plan[i].pose.position.y ) ); prev_idx = i; } - + } - + Eigen::Vector2d TebLocalPlannerROS::tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel) { Eigen::Vector2d vel; @@ -629,22 +629,22 @@ Eigen::Vector2d TebLocalPlannerROS::tfPoseToEigenVector2dTransRot(const tf::Pose vel.coeffRef(1) = tf::getYaw(tf_vel.getRotation()); return vel; } - - + + bool TebLocalPlannerROS::pruneGlobalPlan(const tf2_ros::Buffer& tf, const geometry_msgs::PoseStamped& global_pose, std::vector& global_plan, double dist_behind_robot) { if (global_plan.empty()) return true; - + try { // transform robot pose into the plan frame (we do not wait here, since pruning not crucial, if missed a few times) geometry_msgs::TransformStamped global_to_plan_transform = tf.lookupTransform(global_plan.front().header.frame_id, global_pose.header.frame_id, ros::Time(0)); geometry_msgs::PoseStamped robot; tf2::doTransform(global_pose, robot, global_to_plan_transform); - + double dist_thresh_sq = dist_behind_robot*dist_behind_robot; - + // iterate plan until a pose close the robot is found std::vector::iterator it = global_plan.begin(); std::vector::iterator erase_end = it; @@ -662,7 +662,7 @@ bool TebLocalPlannerROS::pruneGlobalPlan(const tf2_ros::Buffer& tf, const geomet } if (erase_end == global_plan.end()) return false; - + if (erase_end != global_plan.begin()) global_plan.erase(global_plan.begin(), erase_end); } @@ -673,7 +673,7 @@ bool TebLocalPlannerROS::pruneGlobalPlan(const tf2_ros::Buffer& tf, const geomet } return true; } - + bool TebLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const std::vector& global_plan, const geometry_msgs::PoseStamped& global_pose, const costmap_2d::Costmap2D& costmap, const std::string& global_frame, double max_plan_length, @@ -685,7 +685,7 @@ bool TebLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st transformed_plan.clear(); - try + try { if (global_plan.empty()) { @@ -707,12 +707,12 @@ bool TebLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st costmap.getSizeInCellsY() * costmap.getResolution() / 2.0); dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are // located on the border of the local costmap - + int i = 0; double sq_dist_threshold = dist_threshold * dist_threshold; double sq_dist = 1e10; - + //we need to loop to a point on the plan that is within a certain distance of the robot for(int j=0; j < (int)global_plan.size(); ++j) { @@ -728,11 +728,11 @@ bool TebLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st i = j; } } - + geometry_msgs::PoseStamped newer_pose; - + double plan_length = 0; // check cumulative Euclidean distance along the plan - + //now we'll transform until points are outside of our distance threshold while(i < (int)global_plan.size() && sq_dist <= sq_dist_threshold && (max_plan_length<=0 || plan_length <= max_plan_length)) { @@ -744,14 +744,14 @@ bool TebLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x; double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y; sq_dist = x_diff * x_diff + y_diff * y_diff; - + // caclulate distance to previous pose if (i>0 && max_plan_length>0) plan_length += distance_points2d(global_plan[i-1].pose.position, global_plan[i].pose.position); ++i; } - + // if we are really close to the goal (>0) // the resulting transformed plan can be empty. In that case we explicitly inject the global goal. if (transformed_plan.empty()) @@ -759,7 +759,7 @@ bool TebLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st tf2::doTransform(global_plan.back(), newer_pose, plan_to_global_transform); transformed_plan.push_back(newer_pose); - + // Return the index of the current goal point (inside the distance threshold) if (current_goal_idx) *current_goal_idx = int(global_plan.size())-1; } @@ -768,7 +768,7 @@ bool TebLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st // Return the index of the current goal point (inside the distance threshold) if (current_goal_idx) *current_goal_idx = i-1; // subtract 1, since i was increased once before leaving the loop } - + // Return the transformation from the global plan to the global planning frame if desired if (tf_plan_to_global) *tf_plan_to_global = plan_to_global_transform; } @@ -777,12 +777,12 @@ bool TebLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st ROS_ERROR("No Transform available Error: %s\n", ex.what()); return false; } - catch(tf::ConnectivityException& ex) + catch(tf::ConnectivityException& ex) { ROS_ERROR("Connectivity Error: %s\n", ex.what()); return false; } - catch(tf::ExtrapolationException& ex) + catch(tf::ExtrapolationException& ex) { ROS_ERROR("Extrapolation Error: %s\n", ex.what()); if (global_plan.size() > 0) @@ -794,14 +794,14 @@ bool TebLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st return true; } - - - + + + double TebLocalPlannerROS::estimateLocalGoalOrientation(const std::vector& global_plan, const geometry_msgs::PoseStamped& local_goal, int current_goal_idx, const geometry_msgs::TransformStamped& tf_plan_to_global, int moving_average_length) const { int n = (int)global_plan.size(); - + // check if we are near the global goal already if (current_goal_idx > n-moving_average_length-2) { @@ -817,48 +817,58 @@ double TebLocalPlannerROS::estimateLocalGoalOrientation(const std::vector candidates; geometry_msgs::PoseStamped tf_pose_k = local_goal; geometry_msgs::PoseStamped tf_pose_kp1; - + int range_end = current_goal_idx + moving_average_length; for (int i = current_goal_idx; i < range_end; ++i) { // Transform pose of the global plan to the planning frame tf2::doTransform(global_plan.at(i+1), tf_pose_kp1, tf_plan_to_global); - // calculate yaw angle + // calculate yaw angle candidates.push_back( std::atan2(tf_pose_kp1.pose.position.y - tf_pose_k.pose.position.y, tf_pose_kp1.pose.position.x - tf_pose_k.pose.position.x ) ); - - if (i max_vel_x || min_vel_y > max_vel_y || min_vel_theta > max_vel_theta || min_vel_x_backwards > max_vel_x_backwards) + { + ROS_WARN_ONCE("TebLocalPlannerROS(): Do not choose max_vel to be lower than min_vel of the x, y, theta or backwards x."); + } // Limit translational velocity for forward driving if (vx > max_vel_x) ratio_x = max_vel_x / vx; - + else if (vx < min_vel_x && vx > 0.0) + ratio_x = min_vel_x / vx; // limit strafing velocity if (vy > max_vel_y || vy < -max_vel_y) ratio_y = std::abs(vy / max_vel_y); - + else if (std::abs(vy) < min_vel_y && std::abs(vy) > 0.0) + ratio_y = std::abs( min_vel_y / vy); // Limit angular velocity if (omega > max_vel_theta || omega < -max_vel_theta) ratio_omega = std::abs(max_vel_theta / omega); - + else if (std::abs(omega) < min_vel_theta && std::abs(omega) > 0.0) + ratio_omega = std::abs( min_vel_theta / omega); + + // Limit backwards velocity if (max_vel_x_backwards<=0) { @@ -866,6 +876,8 @@ void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega, } else if (vx < -max_vel_x_backwards) ratio_x = - max_vel_x_backwards / vx; + else if (vx > -min_vel_x_backwards && vx < 0 && std::abs(min_vel_x_backwards) > 0.0) + ratio_x = - min_vel_x_backwards / vx; if (cfg_.robot.use_proportional_saturation) { @@ -881,21 +893,21 @@ void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega, omega *= ratio_omega; } } - - + + double TebLocalPlannerROS::convertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius) const { if (omega==0 || v==0) return 0; - + double radius = v/omega; - + if (fabs(radius) < min_turning_radius) - radius = double(g2o::sign(radius)) * min_turning_radius; + radius = double(g2o::sign(radius)) * min_turning_radius; return std::atan(wheelbase / radius); } - + void TebLocalPlannerROS::validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist) { @@ -904,15 +916,15 @@ void TebLocalPlannerROS::validateFootprints(double opt_inscribed_radius, double "than the inscribed radius of the robot's footprint in the costmap parameters (%f, including 'footprint_padding'). " "Infeasible optimziation results might occur frequently!", opt_inscribed_radius, min_obst_dist, costmap_inscribed_radius); } - - - + + + void TebLocalPlannerROS::configureBackupModes(std::vector& transformed_plan, int& goal_idx) { ros::Time current_time = ros::Time::now(); - + // reduced horizon backup mode - if (cfg_.recovery.shrink_horizon_backup && + if (cfg_.recovery.shrink_horizon_backup && goal_idx < (int)transformed_plan.size()-1 && // we do not reduce if the goal is already selected (because the orientation might change -> can introduce oscillations) (no_infeasible_plans_>0 || (current_time - time_last_infeasible_plan_).toSec() < cfg_.recovery.shrink_horizon_min_duration )) // keep short horizon for at least a few seconds { @@ -922,25 +934,25 @@ void TebLocalPlannerROS::configureBackupModes(std::vector 9) { ROS_INFO_COND(no_infeasible_plans_==10, "Infeasible trajectory detected 10 times in a row: further reducing horizon..."); horizon_reduction /= 2; } - + // we have a small overhead here, since we already transformed 50% more of the trajectory. - // But that's ok for now, since we do not need to make transformGlobalPlan more complex + // But that's ok for now, since we do not need to make transformGlobalPlan more complex // and a reduced horizon should occur just rarely. int new_goal_idx_transformed_plan = int(transformed_plan.size()) - horizon_reduction - 1; goal_idx -= horizon_reduction; if (new_goal_idx_transformed_plan>0 && goal_idx >= 0) transformed_plan.erase(transformed_plan.begin()+new_goal_idx_transformed_plan, transformed_plan.end()); else - goal_idx += horizon_reduction; // this should not happen, but safety first ;-) + goal_idx += horizon_reduction; // this should not happen, but safety first ;-) } - - + + // detect and resolve oscillations if (cfg_.recovery.oscillation_recovery) { @@ -950,13 +962,13 @@ void TebLocalPlannerROS::configureBackupModes(std::vectorsetPreferredTurningDir(last_preferred_rotdir_); } else if (!recently_oscillated && last_preferred_rotdir_ != RotType::none) // clear recovery behavior @@ -980,12 +992,12 @@ void TebLocalPlannerROS::configureBackupModes(std::vector(); } - - // point + + // point if (model_name.compare("point") == 0) { ROS_INFO("Footprint model 'point' loaded for trajectory optimization."); return boost::make_shared(); } - + // circular if (model_name.compare("circular") == 0) { @@ -1031,21 +1043,21 @@ RobotFootprintModelPtr TebLocalPlannerROS::getRobotFootprintFromParamServer(cons double radius; if (!nh.getParam("footprint_model/radius", radius)) { - ROS_ERROR_STREAM("Footprint model 'circular' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() + ROS_ERROR_STREAM("Footprint model 'circular' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() << "/footprint_model/radius' does not exist. Using point-model instead."); return boost::make_shared(); } ROS_INFO_STREAM("Footprint model 'circular' (radius: " << radius <<"m) loaded for trajectory optimization."); return boost::make_shared(radius); } - + // line if (model_name.compare("line") == 0) { // check parameters if (!nh.hasParam("footprint_model/line_start") || !nh.hasParam("footprint_model/line_end")) { - ROS_ERROR_STREAM("Footprint model 'line' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() + ROS_ERROR_STREAM("Footprint model 'line' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() << "/footprint_model/line_start' and/or '.../line_end' do not exist. Using point-model instead."); return boost::make_shared(); } @@ -1055,21 +1067,21 @@ RobotFootprintModelPtr TebLocalPlannerROS::getRobotFootprintFromParamServer(cons nh.getParam("footprint_model/line_end", line_end); if (line_start.size() != 2 || line_end.size() != 2) { - ROS_ERROR_STREAM("Footprint model 'line' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() + ROS_ERROR_STREAM("Footprint model 'line' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() << "/footprint_model/line_start' and/or '.../line_end' do not contain x and y coordinates (2D). Using point-model instead."); return boost::make_shared(); } - + ROS_INFO_STREAM("Footprint model 'line' (line_start: [" << line_start[0] << "," << line_start[1] <<"]m, line_end: [" << line_end[0] << "," << line_end[1] << "]m) loaded for trajectory optimization."); return boost::make_shared(Eigen::Map(line_start.data()), Eigen::Map(line_end.data())); } - + // two circles if (model_name.compare("two_circles") == 0) { // check parameters - if (!nh.hasParam("footprint_model/front_offset") || !nh.hasParam("footprint_model/front_radius") + if (!nh.hasParam("footprint_model/front_offset") || !nh.hasParam("footprint_model/front_radius") || !nh.hasParam("footprint_model/rear_offset") || !nh.hasParam("footprint_model/rear_radius")) { ROS_ERROR_STREAM("Footprint model 'two_circles' cannot be loaded for trajectory optimization, since params '" << nh.getNamespace() @@ -1081,7 +1093,7 @@ RobotFootprintModelPtr TebLocalPlannerROS::getRobotFootprintFromParamServer(cons nh.getParam("footprint_model/front_radius", front_radius); nh.getParam("footprint_model/rear_offset", rear_offset); nh.getParam("footprint_model/rear_radius", rear_radius); - ROS_INFO_STREAM("Footprint model 'two_circles' (front_offset: " << front_offset <<"m, front_radius: " << front_radius + ROS_INFO_STREAM("Footprint model 'two_circles' (front_offset: " << front_offset <<"m, front_radius: " << front_radius << "m, rear_offset: " << rear_offset << "m, rear_radius: " << rear_radius << "m) loaded for trajectory optimization."); return boost::make_shared(front_offset, front_radius, rear_offset, rear_radius); } @@ -1094,7 +1106,7 @@ RobotFootprintModelPtr TebLocalPlannerROS::getRobotFootprintFromParamServer(cons XmlRpc::XmlRpcValue footprint_xmlrpc; if (!nh.getParam("footprint_model/vertices", footprint_xmlrpc) ) { - ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() + ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() << "/footprint_model/vertices' does not exist. Using point-model instead."); return boost::make_shared(); } @@ -1106,7 +1118,7 @@ RobotFootprintModelPtr TebLocalPlannerROS::getRobotFootprintFromParamServer(cons Point2dContainer polygon = makeFootprintFromXMLRPC(footprint_xmlrpc, "/footprint_model/vertices"); ROS_INFO_STREAM("Footprint model 'polygon' loaded for trajectory optimization."); return boost::make_shared(polygon); - } + } catch(const std::exception& ex) { ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization: " << ex.what() << ". Using point-model instead."); @@ -1115,21 +1127,21 @@ RobotFootprintModelPtr TebLocalPlannerROS::getRobotFootprintFromParamServer(cons } else { - ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() + ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '" << nh.getNamespace() << "/footprint_model/vertices' does not define an array of coordinates. Using point-model instead."); return boost::make_shared(); } - + } - + // otherwise ROS_WARN_STREAM("Unknown robot footprint model specified with parameter '" << nh.getNamespace() << "/footprint_model/type'. Using point model instead."); return boost::make_shared(); } - - - - + + + + Point2dContainer TebLocalPlannerROS::makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name) { // Make sure we have an array of at least 3 elements. @@ -1141,10 +1153,10 @@ Point2dContainer TebLocalPlannerROS::makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least " "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]"); } - + Point2dContainer footprint; Eigen::Vector2d pt; - + for (int i = 0; i < footprint_xmlrpc.size(); ++i) { // Make sure each element of the list is an array of size 2. (x and y coordinates) @@ -1182,5 +1194,3 @@ double TebLocalPlannerROS::getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const } } // end namespace teb_local_planner - -