From b29d5366c685e93646c84a396dd2e9a5ebda789d Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sat, 5 Oct 2024 19:29:05 +0200 Subject: [PATCH] Improve astar navigation demo --- .../params/reactive2d_default.ini | 35 ++-- .../configs/ini/ptgs_jackal.ini | 2 +- .../configs/ini/reactive2d_config.ini | 195 ------------------ .../demo_astar_planner_gridmap.launch.py | 34 +++ mrpt_tutorials/rviz2/gridmap.rviz | 37 +++- 5 files changed, 93 insertions(+), 210 deletions(-) delete mode 100644 mrpt_tps_astar_planner/configs/ini/reactive2d_config.ini diff --git a/mrpt_reactivenav2d/params/reactive2d_default.ini b/mrpt_reactivenav2d/params/reactive2d_default.ini index 2509318..06bd7e8 100644 --- a/mrpt_reactivenav2d/params/reactive2d_default.ini +++ b/mrpt_reactivenav2d/params/reactive2d_default.ini @@ -6,20 +6,29 @@ ; http://wiki.ros.org/mrpt_reactivenav2d ; ------------------------------------------------------------------------ +# Max linear vel (m/s): +@define ROBOT_MAX_V 1.0 +# Max angular vel (deg/s): +@define ROBOT_MAX_W 60.0 +# Max distance to "foresee" obstacles (m): +@define NAV_MAX_REF_DIST 5.0 + + [CAbstractNavigator] dist_to_target_for_sending_event = 0.000000 // Default value=0, means use the `targetAllowedDistance` passed by the user in the navigation request. alarm_seems_not_approaching_target_timeout = 30.000000 // navigator timeout (seconds) [Default=30 sec] [CWaypointsNavigator] -max_distance_to_allow_skip_waypoint = -1.000000 // Max distance to `foresee` waypoints [meters]. (<0: unlimited) +max_distance_to_allow_skip_waypoint = 1.0 // Max distance to `foresee` waypoints [meters]. (<0: unlimited) min_timesteps_confirm_skip_waypoints = 1 // Min timesteps a `future` waypoint must be seen as reachable to become the active one. waypoint_angle_tolerance = 5.0 // Angular error tolerance for waypoints with an assigned heading [deg] +multitarget_look_ahead = 4 // >=0 number of waypoints to forward to the underlying navigation engine, to ease obstacles avoidance when a waypoint is blocked (Default=0 : none). [CAbstractPTGBasedReactive] -robotMax_V_mps = 1.000 // Max. linear speed (m/s) [Default=-1 (not set), will raise exception if needed and not set] -robotMax_W_degps = 60.0 // Max. angular speed (rad/s) [Default=-1 (not set), will raise exception if needed and not set] +robotMax_V_mps = ${ROBOT_MAX_V} // Max. linear speed (m/s) [Default=-1 (not set), will raise exception if needed and not set] +robotMax_W_degps = ${ROBOT_MAX_W} // Max. angular speed (rad/s) [Default=-1 (not set), will raise exception if needed and not set] #robotMinCurvRadius = -1.000000 // Min. radius of curvature of paths (m) [Default=-1 (not set), will raise exception if needed and not set] holonomic_method = CHolonomicFullEval // C++ class name of the holonomic navigation method to run in the transformed TP-Space. @@ -67,7 +76,7 @@ factorNormalizeOrNot = [0 , 0 , 0 , 0 , 1 , 1, 0 ] TOO_CLOSE_OBSTACLE = 0.150000 // Directions with collision-free distances below this threshold are not elegible. TARGET_SLOW_APPROACHING_DISTANCE = 0.100000 // Start to reduce speed when closer than this to target. -OBSTACLE_SLOW_DOWN_DISTANCE = 0.150000 // Start to reduce speed when clearance is below this value ([0,1] ratio wrt obstacle reference/max distance) +OBSTACLE_SLOW_DOWN_DISTANCE = 0.75 // Start to reduce speed when clearance is below this value ([0,1] ratio wrt obstacle reference/max distance) HYSTERESIS_SECTOR_COUNT = 5.000000 // Range of `sectors` (directions) for hysteresis over succesive timesteps LOG_SCORE_MATRIX = false // Save the entire score matrix in log files @@ -168,29 +177,29 @@ PTG_COUNT = 3 PTG0_Type = CPTG_DiffDrive_C PTG0_resolution = 0.05 # Look-up-table cell size or resolution (in meters) -PTG0_refDistance= 10.0 # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths +PTG0_refDistance= ${NAV_MAX_REF_DIST} # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths PTG0_num_paths= 121 -PTG0_v_max_mps = 1.0 -PTG0_w_max_dps = 60 +PTG0_v_max_mps = ${ROBOT_MAX_V} +PTG0_w_max_dps = ${ROBOT_MAX_W} PTG0_K = 1.0 PTG0_score_priority = 1.0 PTG1_Type = CPTG_DiffDrive_alpha PTG1_resolution = 0.05 # Look-up-table cell size or resolution (in meters) -PTG1_refDistance= 10.0 # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths +PTG1_refDistance= ${NAV_MAX_REF_DIST} # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths PTG1_num_paths = 121 -PTG1_v_max_mps = 1.0 -PTG1_w_max_dps = 60 +PTG1_v_max_mps = ${ROBOT_MAX_V} +PTG1_w_max_dps = ${ROBOT_MAX_W} PTG1_cte_a0v_deg = 57 PTG1_cte_a0w_deg = 57 PTG1_score_priority = 1.0 PTG2_Type = CPTG_DiffDrive_C PTG2_resolution = 0.05 # Look-up-table cell size or resolution (in meters) -PTG2_refDistance= 10.0 # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths +PTG2_refDistance= ${NAV_MAX_REF_DIST} # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths PTG2_num_paths = 121 -PTG2_v_max_mps = 1.0 -PTG2_w_max_dps = 60 +PTG2_v_max_mps = ${ROBOT_MAX_V} +PTG2_w_max_dps = ${ROBOT_MAX_W} PTG2_K = -1.0 PTG2_score_priority = 0.5 diff --git a/mrpt_tps_astar_planner/configs/ini/ptgs_jackal.ini b/mrpt_tps_astar_planner/configs/ini/ptgs_jackal.ini index dd66cbb..1fe039f 100644 --- a/mrpt_tps_astar_planner/configs/ini/ptgs_jackal.ini +++ b/mrpt_tps_astar_planner/configs/ini/ptgs_jackal.ini @@ -3,7 +3,7 @@ # ------------------------------------------------------------------------ # Max linear vel (m/s): -@define ROBOT_MAX_V 1.2 +@define ROBOT_MAX_V 1.0 # Max angular vel (deg/s): @define ROBOT_MAX_W 60.0 # Max distance to "foresee" obstacles (m): diff --git a/mrpt_tps_astar_planner/configs/ini/reactive2d_config.ini b/mrpt_tps_astar_planner/configs/ini/reactive2d_config.ini deleted file mode 100644 index cf97341..0000000 --- a/mrpt_tps_astar_planner/configs/ini/reactive2d_config.ini +++ /dev/null @@ -1,195 +0,0 @@ -; ------------------------------------------------------------------------ -; Example configuration file for MRPT's Reactive Navigation engine -; See ROS node documentation: -; Updated to MRPT >=1.5.0. For older versions, see other git branches -; -; http://wiki.ros.org/mrpt_reactivenav2d -; ------------------------------------------------------------------------ - -[CAbstractNavigator] -dist_to_target_for_sending_event = 0.000000 // Default value=0, means use the `targetAllowedDistance` passed by the user in the navigation request. -alarm_seems_not_approaching_target_timeout = 30.000000 // navigator timeout (seconds) [Default=30 sec] - - -[CWaypointsNavigator] -max_distance_to_allow_skip_waypoint = 2.0 // Max distance to `foresee` waypoints [meters]. (<0: unlimited) -min_timesteps_confirm_skip_waypoints = 1.0 // Min timesteps a `future` waypoint must be seen as reachable to become the active one. -waypoint_angle_tolerance = 25.0 // Angular error tolerance for waypoints with an assigned heading [deg] - - -[CAbstractPTGBasedReactive] -robotMax_V_mps = 1.200 // Max. linear speed (m/s) [Default=-1 (not set), will raise exception if needed and not set] -robotMax_W_degps = 60.0 // Max. angular speed (rad/s) [Default=-1 (not set), will raise exception if needed and not set] -#robotMinCurvRadius = -1.000000 // Min. radius of curvature of paths (m) [Default=-1 (not set), will raise exception if needed and not set] - -holonomic_method = CHolonomicFullEval // C++ class name of the holonomic navigation method to run in the transformed TP-Space. -# List of known classes: -# - `CHolonomicFullEval` -# - `CHolonomicND` -# - `CHolonomicVFF` - -motion_decider_method = CMultiObjMotionOpt_Scalarization // C++ class name of the motion decider method. -# List of known classes: -# - `CMultiObjMotionOpt_Scalarization` - - - -ref_distance = 10.000000 // Maximum distance up to obstacles will be considered (D_{max} in papers). -#speedfilter_tau = 0.000000 // Time constant (in seconds) for the low-pass filter applied to kinematic velocity commands (default=0: no filtering) -secure_distance_start = 0.050000 // In normalized distance [0,1], start/end of a ramp function that scales the holonomic navigator output velocity. -secure_distance_end = 0.150000 // In normalized distance [0,1], start/end of a ramp function that scales the holonomic navigator output velocity. -use_delays_model = false // Whether to use robot pose inter/extrapolation to improve accuracy (Default:false) -max_distance_predicted_actual_path = 0.150000 // Max distance [meters] to discard current PTG and issue a new vel cmd (default= 0.05) -min_normalized_free_space_for_ptg_continuation = 0.200000 // Min normalized dist [0,1] after current pose in a PTG continuation to allow it. - -enable_obstacle_filtering = false // Enabled obstacle filtering (params in its own section) -evaluate_clearance = true - - -[CPointCloudFilterByDistance] -min_dist = 0.100000 -angle_tolerance = 5.000000 -too_old_seconds = 1.000000 -previous_keyframes = 1 // (Default: 1) How many previous keyframes will be compared with the latest pointcloud. -max_deletion_ratio = 0.400000 // (Default: 0.4) If the ratio [0,1] of points considered invalid (`deletion` ) is larger than this ratio, no point will be deleted since it would be too suspicious and may indicate a failure of this filter. - - -[CHolonomicFullEval] -# [0]=Free space -# [1]=Dist. in sectors -# [2]=Closer to target (Euclidean) -# [3]=Hysteresis -# [4]=clearance along path -# [5]=like [2] but without being decimated if path obstructed -# [6]=closeness of k_target -factorWeights = [0.25 , 0.1 , 1 , 0.05 , 0.5, 1, 1 ] -factorNormalizeOrNot = [0 , 0 , 0 , 0 , 1 , 1, 0 ] - -TOO_CLOSE_OBSTACLE = 0.020000 // Directions with collision-free distances below this threshold are not elegible. -TARGET_SLOW_APPROACHING_DISTANCE = 0.100000 // Start to reduce speed when closer than this to target. -OBSTACLE_SLOW_DOWN_DISTANCE = 0.250000 // Start to reduce speed when clearance is below this value ([0,1] ratio wrt obstacle reference/max distance) -HYSTERESIS_SECTOR_COUNT = 5.000000 // Range of `sectors` (directions) for hysteresis over succesive timesteps -LOG_SCORE_MATRIX = false // Save the entire score matrix in log files - -clearance_threshold_ratio = 0.10 // Ratio [0,1], times path_count, gives the minimum number of paths at each side of a target direction to be accepted as desired direction -gap_width_ratio_threshold = 0.20 // Ratio [0,1], times path_count, gives the minimum gap width to accept a direct motion towards target. - -PHASE_COUNT = 3 // Number of evaluation phases to run (params for each phase below) -PHASE1_FACTORS = [6] // Indices of the factors above to be considered in this phase -PHASE1_THRESHOLD = 0.5 // Phase scores must be above this relative range threshold [0,1] to be considered in next phase (Default:`0.75`) -PHASE2_FACTORS = [4] // Indices of the factors above to be considered in this phase -PHASE2_THRESHOLD = 0.7 // Phase scores must be above this relative range threshold [0,1] to be considered in next phase (Default:`0.75`) -PHASE3_FACTORS = [0 2] // Indices of the factors above to be considered in this phase -PHASE3_THRESHOLD = 0.7 // Phase scores must be above this relative range threshold [0,1] to be considered in next phase (Default:`0.75`) - - -[CHolonomicND] -WIDE_GAP_SIZE_PERCENT = 0.250000 -MAX_SECTOR_DIST_FOR_D2_PERCENT = 0.250000 -RISK_EVALUATION_SECTORS_PERCENT = 0.100000 -RISK_EVALUATION_DISTANCE = 0.400000 // In normalized ps-meters [0,1] -TOO_CLOSE_OBSTACLE = 0.150000 // For stopping gradually -TARGET_SLOW_APPROACHING_DISTANCE = 0.600000 // In normalized ps-meters -factorWeights = 1.00 0.50 2.00 0.40 // [0]=Free space, [1]=Dist. in sectors, [2]=Closer to target (Euclidean), [3]=Hysteresis - - -[CHolonomicVFF] -TARGET_SLOW_APPROACHING_DISTANCE = 0.100000 // For stopping gradually -TARGET_ATTRACTIVE_FORCE = 20.000000 // Dimension-less (may have to be tuned depending on the density of obstacle sampling) - -[CMultiObjectiveMotionOptimizerBase] -# Next follows a list of `score%i_{name,formula}` pairs for i=1,...,N -# Each one defines an exprtk formula for one of the scores that will be evaluated for each candidate movement. -# Multiobjective optimizers will then use those scores to select the best candidate, -# possibly using more parameters that follow below. -# See list of all available variables in documentation of mrpt::nav::CAbstractPTGBasedReactive at https://reference.mrpt.org/devel/classmrpt_1_1nav_1_1_c_abstract_p_t_g_based_reactive.html - -score1_name = euclidean_nearness -score1_formula = 1/(1+dist_eucl_min^2) - -score2_name = hysteresis_score -score2_formula = hysteresis - -score3_name = path_index_near_target -score3_formula = \ -var dif:=abs(target_k-move_k); \ - if (dif>(num_paths/2)) \ - { \ - dif:=num_paths-dif; \ - }; \ - exp(-abs(dif / (num_paths/10.0))); - -score4_name = ptg_priority_score -score4_formula = ptg_priority - -score5_name = collision_free_distance_score -score5_formula = \ - var effective_trg_d_norm := max(0,target_d_norm-move_cur_d); \ - if (collision_free_distance>(effective_trg_d_norm+0.05), \ - 1.0, \ - collision_free_distance) - -# Next follows a list of `movement_assert%i` exprtk expressions for i=1,...,N -# defining expressions for conditions that any candidate movement must fulfill -# in order to get through the evaluation process. *All* assert conditions must be satisfied. -#movement_assert1 = XXX - -# Comma-separated list of scores to normalize so the highest is 1.0. -scores_to_normalize = euclidean_nearness -#target_distance - -[CMultiObjMotionOpt_Scalarization] -# A formula that takes all/a subset of scores and generates a scalar global score. -scalar_score_formula = ptg_priority_score*( \ - 3.0*collision_free_distance_score + \ - 5.0*euclidean_nearness + \ - 0.1*hysteresis_score + \ - 8.0*path_index_near_target \ - ) - - -[CReactiveNavigationSystem] -min_obstacles_height = 0.000000 // Minimum `z` coordinate of obstacles to be considered fo collision checking -max_obstacles_height = 2.000000 // Maximum `z` coordinate of obstacles to be considered fo collision checking - -# PTGs: See classes derived from mrpt::nav::CParameterizedTrajectoryGenerator ( http://reference.mrpt.org/svn/classmrpt_1_1nav_1_1_c_parameterized_trajectory_generator.html) -# refer to papers for details. -#------------------------------------------------------------------------------ -PTG_COUNT = 3 - -PTG0_Type = CPTG_DiffDrive_C -PTG0_resolution = 0.05 # Look-up-table cell size or resolution (in meters) -PTG0_refDistance= 8.0 # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths -PTG0_num_paths= 251 -PTG0_v_max_mps = 0.75*(1-abs(dir)/3.2) -PTG0_w_max_dps = 60 -PTG0_K = 1.0 -PTG0_score_priority = 1.0 - -PTG1_Type = CPTG_DiffDrive_C -PTG1_resolution = 0.05 # Look-up-table cell size or resolution (in meters) -PTG1_refDistance= 8.0 # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths -PTG1_num_paths = 251 -PTG1_v_max_mps = 0.3*(1-abs(dir)/3.2) -PTG1_w_max_dps = 60 -PTG1_K = -1.0 -PTG1_score_priority = 0.25 - -PTG2_Type = CPTG_DiffDrive_alpha -PTG2_resolution = 0.05 # Look-up-table cell size or resolution (in meters) -PTG2_refDistance= 8.0 # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths -PTG2_num_paths = 251 -PTG2_v_max_mps = 0.75*(1-abs(dir)/3.2) -PTG2_w_max_dps = 60 -PTG2_cte_a0v_deg = 57 -PTG2_cte_a0w_deg = 57 -PTG2_score_priority = 0.25 - - -# Default 2D robot shape for collision checks: (ignored in ROS, superseded by node parameters) -# Each PTG will use only one of either (a) polygonal 2D shape or, (b) radius of a circular shape -#RobotModel_shape2D_xs = 0.254 0.254 -0.254 -0.254 -RobotModel_shape2D_xs = 0.24 0.24 -0.24 -0.24 -RobotModel_shape2D_ys = -0.19 0.19 0.19 -0.19 -#RobotModel_shape2D_ys = -0.215 0.215 0.215 -0.215 -RobotModel_circular_shape_radius = 0.5 diff --git a/mrpt_tutorials/launch/demo_astar_planner_gridmap.launch.py b/mrpt_tutorials/launch/demo_astar_planner_gridmap.launch.py index be210e2..73ad44c 100644 --- a/mrpt_tutorials/launch/demo_astar_planner_gridmap.launch.py +++ b/mrpt_tutorials/launch/demo_astar_planner_gridmap.launch.py @@ -54,6 +54,38 @@ def generate_launch_description(): }] ) + # Launch for mrpt_pointcloud_pipeline: + pointcloud_pipeline_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('mrpt_pointcloud_pipeline'), 'launch', + 'pointcloud_pipeline.launch.py')]), + launch_arguments={ + 'log_level': 'INFO', + 'scan_topic_name': '/laser1, /laser2', + 'points_topic_name': '/lidar1_points', + 'filter_output_topic_name': '/local_map_pointcloud', + 'time_window': '0.20', + 'show_gui': 'False', + 'frameid_robot': 'base_link', + 'frameid_reference': 'odom', + }.items() + ) + + # Launch for mrpt_reactivenav2d: + node_rnav2d_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('mrpt_reactivenav2d'), 'launch', + 'rnav.launch.py')]), + launch_arguments={ + 'log_level': 'INFO', + # 'save_nav_log': 'True', + 'frameid_robot': 'base_link', + 'frameid_reference': 'map', + 'topic_reactive_nav_goal': '/rnav_goal', # we don't publish this topic + 'topic_wp_seq': '/waypoints', # only this one instead. + }.items() + ) + rviz2_node = Node( package='rviz2', executable='rviz2', @@ -65,5 +97,7 @@ def generate_launch_description(): mrpt_map_launch, mrpt_astar_planner_launch, mvsim_node, + pointcloud_pipeline_launch, + node_rnav2d_launch, rviz2_node ]) diff --git a/mrpt_tutorials/rviz2/gridmap.rviz b/mrpt_tutorials/rviz2/gridmap.rviz index 5c6406e..8b76761 100644 --- a/mrpt_tutorials/rviz2/gridmap.rviz +++ b/mrpt_tutorials/rviz2/gridmap.rviz @@ -7,6 +7,7 @@ Panels: - /Global Options1 - /Status1 - /Grid1/Offset1 + - /PointCloud21 Splitter Ratio: 0.5 Tree Height: 668 - Class: rviz_common/Selection @@ -258,6 +259,40 @@ Visualization Manager: Reliability Policy: Reliable Value: /goal_pose Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 248; 228; 92 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.30000001192092896 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_map_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -313,7 +348,7 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 11.62798023223877 + Scale: 10.814533233642578 Target Frame: Value: TopDownOrtho (rviz_default_plugins) X: 43.450077056884766