astar planner: add refine() step and parameter to optionally disable it

mrpt-ros-pkg · Sep 30, 2024 · d7fb717 · d7fb717
1 parent feb6d43
commit d7fb717
Show file tree

Hide file tree

Showing 3 changed files with 48 additions and 12 deletions.
diff --git a/mrpt_tps_astar_planner/launch/tps_astar_planner.launch.py b/mrpt_tps_astar_planner/launch/tps_astar_planner.launch.py
@@ -91,6 +91,10 @@ def generate_launch_description():
         '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')
 
+    astar_skip_refine_arg = DeclareLaunchArgument(
+        'astar_skip_refine', default_value='False',
+        description='If True, the refine stage after A* will be skipped')
+
     # Node configuration
     tps_astar_nav_node = Node(
         package='mrpt_tps_astar_planner',
@@ -124,6 +128,7 @@ def generate_launch_description():
                 'problem_world_bbox_margin')},
             {'problem_world_bbox_ignore_obstacles': LaunchConfiguration(
                 'problem_world_bbox_ignore_obstacles')},
+            {'astar_skip_refine': LaunchConfiguration('astar_skip_refine')},
             # Param files:
             {'planner_parameters': LaunchConfiguration('planner_parameters')},
             {'global_costmap_parameters': LaunchConfiguration(
@@ -156,5 +161,6 @@ def generate_launch_description():
         prefer_waypoints_parameters_arg,
         problem_world_bbox_margin_arg,
         problem_world_bbox_ignore_obstacles_arg,
+        astar_skip_refine_arg,
         tps_astar_nav_node
     ])
diff --git a/mrpt_tps_astar_planner/src/mrpt_tps_astar_planner_node.cpp b/mrpt_tps_astar_planner/src/mrpt_tps_astar_planner_node.cpp
@@ -156,6 +156,8 @@ class TPS_Astar_Planner_Node : public rclcpp::Node
 	float problem_world_bbox_margin_ = 2.0f;
 	bool problem_world_bbox_ignore_obstacles_ = false;
 
+	bool astar_skip_refine_ = false;
+
 	/// Waypoint parameters
 	double mid_waypoints_allowed_distance_ = 0.5;
 	double final_waypoint_allowed_distance_ = 0.4;
@@ -477,6 +479,10 @@ void TPS_Astar_Planner_Node::read_parameters()
 		this->get_logger(),
 		"problem_world_bbox_ignore_obstacles: " << problem_world_bbox_ignore_obstacles_);
 
+	this->declare_parameter<bool>("astar_skip_refine", astar_skip_refine_);
+	this->get_parameter("astar_skip_refine", astar_skip_refine_);
+	RCLCPP_INFO_STREAM(this->get_logger(), "astar_skip_refine: " << astar_skip_refine_);
+
 	this->declare_parameter<double>(
 		"final_waypoint_allowed_distance", final_waypoint_allowed_distance_);
 	this->get_parameter("final_waypoint_allowed_distance", final_waypoint_allowed_distance_);
@@ -755,19 +761,23 @@ TPS_Astar_Planner_Node::PlanResult TPS_Astar_Planner_Node::do_path_plan(
 											<< "-" << pi.worldBboxMax.asString());
 
 	// Insert custom progress callback:
-	planner_->progressCallback_ = [](const mpp::ProgressCallbackData& pcd)
+	planner_->progressCallback_ = [this](const mpp::ProgressCallbackData& pcd)
 	{
-		std::cout << "[progressCallback] bestCostFromStart: " << pcd.bestCostFromStart
-				  << " bestCostToGoal: " << pcd.bestCostToGoal
-				  << " bestPathLength: " << pcd.bestPath.size() << std::endl;
+		RCLCPP_INFO_STREAM(
+			this->get_logger(),
+			"[progressCallback] bestCostFromStart: " << pcd.bestCostFromStart
+													 << " bestCostToGoal: " << pcd.bestCostToGoal
+													 << " bestPathLength: " << pcd.bestPath.size());
 	};
 
 	const mpp::PlannerOutput plan = planner_->plan(pi);
 
-	std::cout << "\nDone.\n";
-	std::cout << "Success: " << (plan.success ? "YES" : "NO") << "\n";
-	std::cout << "Plan has " << plan.motionTree.edges_to_children.size() << " overall edges, "
-			  << plan.motionTree.nodes().size() << " nodes\n";
+	RCLCPP_INFO_STREAM(
+		this->get_logger(), "Done.\n"
+								<< "Success: " << (plan.success ? "YES" : "NO") << "\n"
+								<< "Plan has " << plan.motionTree.edges_to_children.size()
+								<< " overall edges, " << plan.motionTree.nodes().size()
+								<< " nodes");
 
 	if (!plan.bestNodeId.has_value())
 	{
@@ -786,9 +796,9 @@ TPS_Astar_Planner_Node::PlanResult TPS_Astar_Planner_Node::do_path_plan(
 	// backtrack:
 	auto [plannedPath, pathEdges] = plan.motionTree.backtrack_path(*plan.bestNodeId);
 
-#if 0  // JLBC: disabled to check if this is causing troubles
-mpp::refine_trajectory(plannedPath, pathEdges, planner_input.ptgs);
-#endif
+	// refine trajectory:
+	if (!astar_skip_refine_)
+		mpp::refine_trajectory(plannedPath, pathEdges, plan.originalInput.ptgs);
 
 	// Show plan in a GUI for debugging
 	if (plan.success && gui_mrpt_)

diff --git a/mrpt_tutorials/rviz2/gridmap.rviz b/mrpt_tutorials/rviz2/gridmap.rviz
@@ -238,6 +238,26 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: /waypoints_path
       Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz_default_plugins/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -293,7 +313,7 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: 11.861957550048828
+      Scale: 11.62798023223877
       Target Frame: <Fixed Frame>
       Value: TopDownOrtho (rviz_default_plugins)
       X: 43.450077056884766