From 845b65e2b06e4e6ad227ec4fca9731bd2ec222a3 Mon Sep 17 00:00:00 2001 From: Alperen Date: Tue, 23 Nov 2021 10:43:21 +0300 Subject: [PATCH 1/4] added minimum translational velocity constraint --- cfg/TebLocalPlannerReconfigure.cfg | 6 ++- include/teb_local_planner/teb_config.h | 1 + .../teb_local_planner/teb_local_planner_ros.h | 2 +- src/teb_config.cpp | 4 ++ src/teb_local_planner_ros.cpp | 39 +++++++++++++++---- 5 files changed, 42 insertions(+), 10 deletions(-) diff --git a/cfg/TebLocalPlannerReconfigure.cfg b/cfg/TebLocalPlannerReconfigure.cfg index c60e9104..5f033a16 100755 --- a/cfg/TebLocalPlannerReconfigure.cfg +++ b/cfg/TebLocalPlannerReconfigure.cfg @@ -86,7 +86,11 @@ grp_robot.add("max_vel_x", double_t, 0, 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", double_t, 0, + "Minimum translational velocity of the robot", + 0.4, 0.01, 100) grp_robot.add("max_vel_theta", double_t, 0, "Maximum angular velocity of the robot", diff --git a/include/teb_local_planner/teb_config.h b/include/teb_local_planner/teb_config.h index 1d9e290f..50564b23 100644 --- a/include/teb_local_planner/teb_config.h +++ b/include/teb_local_planner/teb_config.h @@ -104,6 +104,7 @@ class TebConfig bool is_footprint_dynamic; // max_vel_x) - ratio_x = max_vel_x / vx; - + double ratio_x = 1, ratio_omega = 1, ratio_y = 1, ratio_x_min = 1; + bool direction = true; + + // backward driving + if(vx < 0) + { + direction = false ; + + if (vx < -max_vel_x_backwards) + ratio_x = - max_vel_x_backwards / vx; + else if (vx > -min_vel_x) + ratio_x = - min_vel_x / vx; + + } + else // forward driving + { + direction = true; + + if (vx > max_vel_x) + ratio_x = max_vel_x / vx; + else if (vx < min_vel_x) + { + 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); @@ -901,9 +922,11 @@ void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega, } else { - vx *= ratio_x; + + vx *= ratio_x; vy *= ratio_y; omega *= ratio_omega; + } } From dcce60272bdc94a60b711d4ea29eb5a3e0ae1dda Mon Sep 17 00:00:00 2001 From: Alperen Date: Tue, 23 Nov 2021 10:45:23 +0300 Subject: [PATCH 2/4] added cfg file --- cfg/planner.yaml | 113 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 113 insertions(+) create mode 100644 cfg/planner.yaml diff --git a/cfg/planner.yaml b/cfg/planner.yaml new file mode 100644 index 00000000..b4ddb7e2 --- /dev/null +++ b/cfg/planner.yaml @@ -0,0 +1,113 @@ +# ########### Teb Local Planner +TebLocalPlannerROS: + +# odom_topic: odom + odom_topic: odom + map_frame: /odom + # Trajectory + + teb_autosize: True + dt_ref: 0.3 + dt_hysteresis: 0.1 + max_samples: 500 + global_plan_overwrite_orientation: True + allow_init_with_backwards_motion: True + max_global_plan_lookahead_dist: 3.0 + global_plan_viapoint_sep: -1 + global_plan_prune_distance: 1 + exact_arc_length: True + feasibility_check_no_poses: 5 + publish_feedback: False + + # Robot + + max_vel_x: 0.055 + max_vel_x_backwards: 0.055 + min_vel_x: 0.04 + max_vel_y: 0.0 + max_vel_theta: 0.45 + acc_lim_x: 0.5 + acc_lim_theta: 0.2 + min_turning_radius: 0.2 # diff-drive robot (can turn on place!) + + footprint_model: + type: "point" + + # GoalTolerance + + xy_goal_tolerance: 0.2 + yaw_goal_tolerance: 0.1 + free_goal_vel: False + complete_global_plan: True + + # Obstacles + + min_obstacle_dist: 0.25 # This value must also include our robot radius, since footprint_model is set to "point". + inflation_dist: 0.6 + include_costmap_obstacles: True + costmap_obstacles_behind_robot_dist: 1.5 + obstacle_poses_affected: 15 + + dynamic_obstacle_inflation_dist: 0.6 + include_dynamic_obstacles: True + + costmap_converter_plugin: "" + costmap_converter_spin_thread: True + costmap_converter_rate: 5 + + # Optimization + + no_inner_iterations: 5 + no_outer_iterations: 4 + optimization_activate: True + optimization_verbose: False + penalty_epsilon: 0.01 + obstacle_cost_exponent: 4 + weight_max_vel_x: 2 + weight_max_vel_theta: 1 + weight_acc_lim_x: 1 + weight_acc_lim_theta: 1 + weight_kinematics_nh: 1000 + weight_kinematics_forward_drive: 1 + weight_kinematics_turning_radius: 1 + weight_optimaltime: 1 # must be > 0 + weight_shortest_path: 0 + weight_obstacle: 100 + weight_inflation: 0.2 + weight_dynamic_obstacle: 10 + weight_dynamic_obstacle_inflation: 0.2 + weight_viapoint: 1 + weight_adapt_factor: 2 + + # Homotopy Class Planner + + enable_homotopy_class_planning: True + enable_multithreading: True + max_number_classes: 4 + selection_cost_hysteresis: 1.0 + selection_prefer_initial_plan: 0.9 + selection_obst_cost_scale: 100.0 + selection_alternative_time_cost: False + + roadmap_graph_no_samples: 15 + roadmap_graph_area_width: 5 + roadmap_graph_area_length_scale: 1.0 + h_signature_prescaler: 0.5 + h_signature_threshold: 0.1 + obstacle_heading_threshold: 0.45 + switching_blocking_period: 0.0 + viapoints_all_candidates: True + delete_detours_backwards: True + max_ratio_detours_duration_best_duration: 3.0 + visualize_hc_graph: False + visualize_with_time_as_z_axis_scale: False + +# Recovery + + shrink_horizon_backup: True + shrink_horizon_min_duration: 10 + oscillation_recovery: True + oscillation_v_eps: 0.1 + oscillation_omega_eps: 0.1 + oscillation_recovery_min_duration: 10 + oscillation_filter_duration: 10 \ No newline at end of file From f5e9580be231412f077f33e1f6ca9bc97b31b3b6 Mon Sep 17 00:00:00 2001 From: AlperenKosem Date: Fri, 26 Nov 2021 16:01:36 +0300 Subject: [PATCH 3/4] redundancy removed/edited --- cfg/TebLocalPlannerReconfigure.cfg | 10 +-- cfg/planner.yaml | 113 ------------------------- include/teb_local_planner/teb_config.h | 3 +- src/teb_config.cpp | 4 +- src/teb_local_planner_ros.cpp | 11 +-- 5 files changed, 11 insertions(+), 130 deletions(-) delete mode 100644 cfg/planner.yaml diff --git a/cfg/TebLocalPlannerReconfigure.cfg b/cfg/TebLocalPlannerReconfigure.cfg index 5f033a16..780f5c16 100755 --- a/cfg/TebLocalPlannerReconfigure.cfg +++ b/cfg/TebLocalPlannerReconfigure.cfg @@ -82,15 +82,15 @@ 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) + 0.4, 0.01, 100) + +grp_robot.add("min_vel_x", double_t, 0, + "Minimum translational velocity of the robot", + 0.01, 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("min_vel_x", double_t, 0, - "Minimum translational velocity of the robot", - 0.4, 0.01, 100) grp_robot.add("max_vel_theta", double_t, 0, "Maximum angular velocity of the robot", diff --git a/cfg/planner.yaml b/cfg/planner.yaml deleted file mode 100644 index b4ddb7e2..00000000 --- a/cfg/planner.yaml +++ /dev/null @@ -1,113 +0,0 @@ -# ########### Teb Local Planner -TebLocalPlannerROS: - -# odom_topic: odom - odom_topic: odom - map_frame: /odom - # Trajectory - - teb_autosize: True - dt_ref: 0.3 - dt_hysteresis: 0.1 - max_samples: 500 - global_plan_overwrite_orientation: True - allow_init_with_backwards_motion: True - max_global_plan_lookahead_dist: 3.0 - global_plan_viapoint_sep: -1 - global_plan_prune_distance: 1 - exact_arc_length: True - feasibility_check_no_poses: 5 - publish_feedback: False - - # Robot - - max_vel_x: 0.055 - max_vel_x_backwards: 0.055 - min_vel_x: 0.04 - max_vel_y: 0.0 - max_vel_theta: 0.45 - acc_lim_x: 0.5 - acc_lim_theta: 0.2 - min_turning_radius: 0.2 # diff-drive robot (can turn on place!) - - footprint_model: - type: "point" - - # GoalTolerance - - xy_goal_tolerance: 0.2 - yaw_goal_tolerance: 0.1 - free_goal_vel: False - complete_global_plan: True - - # Obstacles - - min_obstacle_dist: 0.25 # This value must also include our robot radius, since footprint_model is set to "point". - inflation_dist: 0.6 - include_costmap_obstacles: True - costmap_obstacles_behind_robot_dist: 1.5 - obstacle_poses_affected: 15 - - dynamic_obstacle_inflation_dist: 0.6 - include_dynamic_obstacles: True - - costmap_converter_plugin: "" - costmap_converter_spin_thread: True - costmap_converter_rate: 5 - - # Optimization - - no_inner_iterations: 5 - no_outer_iterations: 4 - optimization_activate: True - optimization_verbose: False - penalty_epsilon: 0.01 - obstacle_cost_exponent: 4 - weight_max_vel_x: 2 - weight_max_vel_theta: 1 - weight_acc_lim_x: 1 - weight_acc_lim_theta: 1 - weight_kinematics_nh: 1000 - weight_kinematics_forward_drive: 1 - weight_kinematics_turning_radius: 1 - weight_optimaltime: 1 # must be > 0 - weight_shortest_path: 0 - weight_obstacle: 100 - weight_inflation: 0.2 - weight_dynamic_obstacle: 10 - weight_dynamic_obstacle_inflation: 0.2 - weight_viapoint: 1 - weight_adapt_factor: 2 - - # Homotopy Class Planner - - enable_homotopy_class_planning: True - enable_multithreading: True - max_number_classes: 4 - selection_cost_hysteresis: 1.0 - selection_prefer_initial_plan: 0.9 - selection_obst_cost_scale: 100.0 - selection_alternative_time_cost: False - - roadmap_graph_no_samples: 15 - roadmap_graph_area_width: 5 - roadmap_graph_area_length_scale: 1.0 - h_signature_prescaler: 0.5 - h_signature_threshold: 0.1 - obstacle_heading_threshold: 0.45 - switching_blocking_period: 0.0 - viapoints_all_candidates: True - delete_detours_backwards: True - max_ratio_detours_duration_best_duration: 3.0 - visualize_hc_graph: False - visualize_with_time_as_z_axis_scale: False - -# Recovery - - shrink_horizon_backup: True - shrink_horizon_min_duration: 10 - oscillation_recovery: True - oscillation_v_eps: 0.1 - oscillation_omega_eps: 0.1 - oscillation_recovery_min_duration: 10 - oscillation_filter_duration: 10 \ No newline at end of file diff --git a/include/teb_local_planner/teb_config.h b/include/teb_local_planner/teb_config.h index 50564b23..cc77d0ba 100644 --- a/include/teb_local_planner/teb_config.h +++ b/include/teb_local_planner/teb_config.h @@ -92,6 +92,7 @@ class TebConfig struct Robot { double max_vel_x; //!< Maximum translational velocity of the robot + double min_vel_x; //!< Minimum 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 @@ -104,7 +105,7 @@ class TebConfig bool is_footprint_dynamic; // -min_vel_x) ratio_x = - min_vel_x / vx; - } else // forward driving { - direction = true; - if (vx > max_vel_x) ratio_x = max_vel_x / vx; else if (vx < min_vel_x) @@ -922,7 +916,6 @@ void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega, } else { - vx *= ratio_x; vy *= ratio_y; omega *= ratio_omega; From 4a852b0d6e17fdf3df75cec4602e0273c07e62bc Mon Sep 17 00:00:00 2001 From: AlperenKosem Date: Mon, 29 Nov 2021 10:50:02 +0300 Subject: [PATCH 4/4] Added minimum translational Velocity Limit --- cfg/TebLocalPlannerReconfigure.cfg | 6 +++--- include/teb_local_planner/teb_config.h | 1 - .../teb_local_planner/teb_local_planner_ros.h | 5 +++-- src/teb_config.cpp | 3 +-- src/teb_local_planner_ros.cpp | 17 +++++++---------- 5 files changed, 14 insertions(+), 18 deletions(-) diff --git a/cfg/TebLocalPlannerReconfigure.cfg b/cfg/TebLocalPlannerReconfigure.cfg index 780f5c16..b08c87e4 100755 --- a/cfg/TebLocalPlannerReconfigure.cfg +++ b/cfg/TebLocalPlannerReconfigure.cfg @@ -82,15 +82,15 @@ 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) + 0.4, 0.01, 100) grp_robot.add("min_vel_x", double_t, 0, "Minimum translational velocity of the robot", - 0.01, 0.01, 100) + 0.00, 0.00, 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("max_vel_theta", double_t, 0, "Maximum angular velocity of the robot", diff --git a/include/teb_local_planner/teb_config.h b/include/teb_local_planner/teb_config.h index cc77d0ba..a501b014 100644 --- a/include/teb_local_planner/teb_config.h +++ b/include/teb_local_planner/teb_config.h @@ -105,7 +105,6 @@ class TebConfig bool is_footprint_dynamic; // -min_vel_x) - ratio_x = - min_vel_x / vx; + ratio_x = -min_vel_x / vx; } else // forward driving { if (vx > max_vel_x) ratio_x = max_vel_x / vx; else if (vx < min_vel_x) - { - ratio_x = min_vel_x / vx; - } + 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); @@ -916,10 +914,9 @@ void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega, } else { - vx *= ratio_x; + vx *= ratio_x; vy *= ratio_y; omega *= ratio_omega; - } }