Improve astar navigation demo

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
 

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):

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
     ])