astar node: add two more launch args: problem_world_bbox_margin and p…

…roblem_world_bbox_ignore_obstacles

mrpt_tps_astar_planner/launch/tps_astar_planner.launch.py

@@ -46,23 +46,23 @@ def generate_launch_description():
     mid_waypoints_allowed_distance = DeclareLaunchArgument(
         'mid_waypoints_allowed_distance', default_value='0.5',
         description='allowed_distance field of middle waypoints of the interpolated path')
-    
+
     final_waypoint_allowed_distance = DeclareLaunchArgument(
         'final_waypoint_allowed_distance', default_value='0.4',
         description='allowed_distance field of final waypoint of the interpolated path')
-    
+
     mid_waypoints_allow_skip = DeclareLaunchArgument(
         'mid_waypoints_allow_skip', default_value='true',
         description='allow_skip field of middle waypoints of the interpolated path')
-    
+
     final_waypoint_allow_skip = DeclareLaunchArgument(
         'final_waypoint_allow_skip', default_value='false',
         description='allow_skip field of final waypoint of the interpolated path')
-    
+
     mid_waypoints_ignore_heading = DeclareLaunchArgument(
         'mid_waypoints_ignore_heading', default_value='false',
         description='ignore_heading field of middle waypoints of the interpolated path')
-    
+
     final_waypoint_ignore_heading = DeclareLaunchArgument(
         'final_waypoint_ignore_heading', default_value='false',
         description='ignore_heading field of final waypoint of the interpolated path')
@@ -83,6 +83,14 @@ def generate_launch_description():
         'prefer_waypoints_parameters', default_value=os.path.join(myDir, 'configs', 'params', 'costmap-prefer-waypoints.yaml'),
         description='Path to prefer_waypoints_parameters.yaml configuration file')
 
+    problem_world_bbox_margin_arg = DeclareLaunchArgument(
+        'problem_world_bbox_margin', default_value='2.0',
+        description='The distance [m] to add as a margin all around the computed problem world bounding box')
+
+    problem_world_bbox_ignore_obstacles_arg = DeclareLaunchArgument(
+        'problem_world_bbox_ignore_obstacles', default_value='False',
+        description='If True, the bounding box of obstacles is ignored while computing the problem world bounding box')
+
     # Node configuration
     tps_astar_nav_node = Node(
         package='mrpt_tps_astar_planner',
@@ -100,12 +108,22 @@ def generate_launch_description():
             {'topic_wp_seq_pub': LaunchConfiguration('topic_wp_seq_pub')},
             {'frame_id_robot': LaunchConfiguration('frame_id_robot')},
             {'frame_id_map': LaunchConfiguration('frame_id_map')},
-            {'mid_waypoints_allowed_distance' : LaunchConfiguration('mid_waypoints_allowed_distance')},
-            {'final_waypoint_allowed_distance' : LaunchConfiguration('final_waypoint_allowed_distance')},
-            {'mid_waypoints_allow_skip' : LaunchConfiguration('mid_waypoints_allow_skip')},
-            {'final_waypoint_allow_skip' : LaunchConfiguration('final_waypoint_allow_skip')},
-            {'mid_waypoints_ignore_heading' : LaunchConfiguration('mid_waypoints_ignore_heading')},
-            {'final_waypoint_ignore_heading' : LaunchConfiguration('final_waypoint_ignore_heading')},
+            {'mid_waypoints_allowed_distance': LaunchConfiguration(
+                'mid_waypoints_allowed_distance')},
+            {'final_waypoint_allowed_distance': LaunchConfiguration(
+                'final_waypoint_allowed_distance')},
+            {'mid_waypoints_allow_skip': LaunchConfiguration(
+                'mid_waypoints_allow_skip')},
+            {'final_waypoint_allow_skip': LaunchConfiguration(
+                'final_waypoint_allow_skip')},
+            {'mid_waypoints_ignore_heading': LaunchConfiguration(
+                'mid_waypoints_ignore_heading')},
+            {'final_waypoint_ignore_heading': LaunchConfiguration(
+                'final_waypoint_ignore_heading')},
+            {'problem_world_bbox_margin': LaunchConfiguration(
+                'problem_world_bbox_margin')},
+            {'problem_world_bbox_ignore_obstacles': LaunchConfiguration(
+                'problem_world_bbox_ignore_obstacles')},
             # Param files:
             {'planner_parameters': LaunchConfiguration('planner_parameters')},
             {'global_costmap_parameters': LaunchConfiguration(
@@ -126,15 +144,17 @@ def generate_launch_description():
         topic_wp_seq_pub,
         frame_id_robot,
         frame_id_map,
-        mid_waypoints_allowed_distance,     
-        final_waypoint_allowed_distance,     
-        mid_waypoints_allow_skip,    
-        final_waypoint_allow_skip,     
-        mid_waypoints_ignore_heading,     
+        mid_waypoints_allowed_distance,
+        final_waypoint_allowed_distance,
+        mid_waypoints_allow_skip,
+        final_waypoint_allow_skip,
+        mid_waypoints_ignore_heading,
         final_waypoint_ignore_heading,
         planner_parameters_arg,
         ptg_ini_arg,
         global_costmap_parameters_arg,
         prefer_waypoints_parameters_arg,
+        problem_world_bbox_margin_arg,
+        problem_world_bbox_ignore_obstacles_arg,
         tps_astar_nav_node
     ])

mrpt_tps_astar_planner/src/mrpt_tps_astar_planner_node.cpp

@@ -153,6 +153,9 @@ class TPS_Astar_Planner_Node : public rclcpp::Node
 	/// Parameters file for planner
 	std::string planner_params_file_ = "planner-params.yaml";
 
+	float problem_world_bbox_margin_ = 2.0f;
+	bool problem_world_bbox_ignore_obstacles_ = false;
+
 	/// Waypoint parameters
 	double mid_waypoints_allowed_distance_ = 0.5;
 	double final_waypoint_allowed_distance_ = 0.4;
@@ -462,6 +465,18 @@ void TPS_Astar_Planner_Node::read_parameters()
 		this->get_logger(), "mid_waypoints_allowed_distance: %.03f",
 		mid_waypoints_allowed_distance_);
 
+	this->declare_parameter<float>("problem_world_bbox_margin", problem_world_bbox_margin_);
+	this->get_parameter("problem_world_bbox_margin", problem_world_bbox_margin_);
+	RCLCPP_INFO(this->get_logger(), "problem_world_bbox_margin: %.03f", problem_world_bbox_margin_);
+
+	this->declare_parameter<bool>(
+		"problem_world_bbox_ignore_obstacles", problem_world_bbox_ignore_obstacles_);
+	this->get_parameter(
+		"problem_world_bbox_ignore_obstacles", problem_world_bbox_ignore_obstacles_);
+	RCLCPP_INFO_STREAM(
+		this->get_logger(),
+		"problem_world_bbox_ignore_obstacles: " << problem_world_bbox_ignore_obstacles_);
+
 	this->declare_parameter<double>(
 		"final_waypoint_allowed_distance", final_waypoint_allowed_distance_);
 	this->get_parameter("final_waypoint_allowed_distance", final_waypoint_allowed_distance_);
@@ -700,8 +715,11 @@ TPS_Astar_Planner_Node::PlanResult TPS_Astar_Planner_Node::do_path_plan(
 		auto obs = mpp::ObstacleSource::FromStaticPointcloud(e.obstacle_points);
 		pi.obstacles.emplace_back(obs);
 
-		auto bb = obs->obstacles()->boundingBox();
-		bbox = bbox.unionWith(bb);
+		if (!problem_world_bbox_ignore_obstacles_)
+		{
+			auto bb = obs->obstacles()->boundingBox();
+			bbox = bbox.unionWith(bb);
+		}
 
 		obstacleSources++;
 		totalObstaclePoints += e.obstacle_points->size();
@@ -714,7 +732,8 @@ TPS_Astar_Planner_Node::PlanResult TPS_Astar_Planner_Node::do_path_plan(
 	lckObs.unlock();
 
 	{
-		const auto bboxMargin = mrpt::math::TPoint3Df(2.0, 2.0, .0);
+		const auto bboxMargin =
+			mrpt::math::TPoint3Df(problem_world_bbox_margin_, problem_world_bbox_margin_, .0f);
 		const auto ptStart = mrpt::math::TPoint3Df(pi.stateStart.pose.x, pi.stateStart.pose.y, 0);
 		const auto ptGoal = mrpt::math::TPoint3Df(
 			pi.stateGoal.asSE2KinState().pose.x, pi.stateGoal.asSE2KinState().pose.y, 0);

mrpt_tutorials/launch/demo_astar_planner_gridmap.launch.py

@@ -27,6 +27,7 @@ def generate_launch_description():
             'topic_obstacles_gridmap_sub': '/mrpt_map/map_gridmap',
             'topic_static_maps': '/mrpt_map/map_gridmap',
             'topic_wp_seq_pub': '/waypoints',
+            # 'problem_world_bbox_ignore_obstacles': 'True',
         }.items()
     )