Progress with astar node

mrpt_tps_astar_planner/README.md

@@ -0,0 +1,56 @@
+# mrpt_tps_astar_planner
+
+## Overview
+This package provides a ROS 2 node that uses the PTG-based A* planner in mrpt_path_planning
+to publish waypoint sequences moving a non-holonomic robot from A to B, taking into account
+its real shape, orientation, and kinematic constraints.
+
+## How to cite
+
+<details>
+    TBD!
+</details>
+
+
+## Configuration
+
+Write me!
+
+## Demos
+
+Write me!
+
+## Node: mrpt_tps_astar_planner_node
+
+### Working rationale
+
+Uses A* over a SE(2) lattice and PTGs to sample collision-free paths. The implementation is an anytime algorithm.
+
+
+### ROS 2 parameters
+
+* `topic_goal_sub`: The name of the topic to subscribe for goal poses (`geometry_msgs/PoseStamped`).
+
+* `show_gui`: Shows its own MRPT GUI with the planned paths.
+
+### Subscribed topics
+* xxx
+
+### Published topics
+* `` (`mrpt_msgs::msg::WaypointSequence`)
+
+### Services
+
+Write me!
+
+### Template ROS 2 launch files
+
+This package provides [launch/tps_astar_planner.launch.py](launch/tps_astar_planner.launch.py):
+
+    ros2 launch tps_astar_planner tps_astar_planner.launch.py
+
+which can be used in user projects to launch the planner, by setting these [launch arguments](https://docs.ros.org/en/rolling/Tutorials/Intermediate/Launch/Using-Substitutions.html):
+
+* ``XXX``: XXX
+
+

mrpt_tps_astar_planner/launch/tps_astar_planner.launch.py

@@ -5,20 +5,15 @@
 from launch.substitutions import Command
 from ament_index_python.packages import get_package_share_directory
 
+
 def generate_launch_description():
     # Declare launch arguments
-    nav_goal = DeclareLaunchArgument(
-        'nav_goal', default_value='[0.0, 0.0, 0.0]',
-        description='Navigation goal position')
-    start_pose = DeclareLaunchArgument(
-        'start_pose', default_value='[0.0, 0.0, 0.0]',
-        description='Starting pose')
-    start_vel = DeclareLaunchArgument(
-        'start_vel', default_value='[0.0, 0.0, 0.0]',
-        description='Starting velocity')
-    mrpt_gui = DeclareLaunchArgument(
-        'mrpt_gui', default_value='false',
-        description='Enable MRPT GUI')
+    topic_goal_sub = DeclareLaunchArgument(
+        'topic_goal_sub', default_value='tps_astar_nav_goal',
+        description='Goal subscription topic')
+    show_gui = DeclareLaunchArgument(
+        'show_gui', default_value='false',
+        description='Enable package GUI')
     topic_map_sub = DeclareLaunchArgument(
         'topic_map_sub', default_value='map',
         description='Map subscription topic')
@@ -48,20 +43,15 @@ def generate_launch_description():
         name='mrpt_tps_astar_planner_node',
         output='screen',
         parameters=[
-            get_package_share_directory('mrpt_tps_astar_planner') + '/configs/params/planner-params.yaml',
-            get_package_share_directory('mrpt_tps_astar_planner') + '/configs/params/costmap-obstacles.yaml',
-            get_package_share_directory('mrpt_tps_astar_planner') + '/configs/params/nav-engine-params.yaml',
-            get_package_share_directory('mrpt_tps_astar_planner') + '/configs/params/costmap-obstacles.yaml',
-            get_package_share_directory('mrpt_tps_astar_planner') + '/configs/params/costmap-prefer-waypoints.yaml',
-            get_package_share_directory('mrpt_tps_astar_planner') + '/configs/ini/ptgs_jackal.ini',
-            {'nav_goal': LaunchConfiguration('nav_goal')},
-            {'start_pose': LaunchConfiguration('start_pose')},
-            {'start_vel': LaunchConfiguration('start_vel')},
-            {'mrpt_gui': LaunchConfiguration('mrpt_gui')},
+            #            get_package_share_directory('mrpt_tps_astar_planner') + '/configs/params/planner-params.yaml',
+            {'topic_goal_sub': LaunchConfiguration('topic_goal_sub')},
+            {'show_gui': LaunchConfiguration('show_gui')},
             {'topic_map_sub': LaunchConfiguration('topic_map_sub')},
-            {'topic_localization_sub': LaunchConfiguration('topic_localization_sub')},
+            {'topic_localization_sub': LaunchConfiguration(
+                'topic_localization_sub')},
             {'topic_odometry_sub': LaunchConfiguration('topic_odometry_sub')},
-            {'topic_obstacles_sub': LaunchConfiguration('topic_obstacles_sub')},
+            {'topic_obstacles_sub': LaunchConfiguration(
+                'topic_obstacles_sub')},
             {'topic_replan_sub': LaunchConfiguration('topic_replan_sub')},
             {'topic_cmd_vel_pub': LaunchConfiguration('topic_cmd_vel_pub')},
             {'topic_wp_seq_pub': LaunchConfiguration('topic_wp_seq_pub')}
@@ -70,10 +60,8 @@ def generate_launch_description():
 
     # Launch description
     return LaunchDescription([
-        nav_goal,
-        start_pose,
-        start_vel,
-        mrpt_gui,
+        topic_goal_sub,
+        show_gui,
         topic_map_sub,
         topic_localization_sub,
         topic_odometry_sub,

mrpt_tps_astar_planner/src/mrpt_tps_astar_planner_node.cpp

@@ -120,14 +120,11 @@ class TPS_Astar_Planner_Node : public rclcpp::Node
 	/// Mutex for obstacle data
 	std::mutex obstacles_cs_;
 
-	/// Debug flag
-	bool debug_ = true;
-
 	/// Flag for MRPT GUI
 	bool gui_mrpt_ = false;
 
 	/// goal topic subscriber name
-	std::string topic_goal_sub_;
+	std::string topic_goal_sub_ = "tps_astar_nav_goal";
 
 	/// map topic subscriber name
 	std::string topic_map_sub_;
@@ -264,32 +261,32 @@ TPS_Astar_Planner_Node::TPS_Astar_Planner_Node() : rclcpp::Node(NODE_NAME)
 	// -----------------------
 
 	sub_goal_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
-		topic_goal_sub_, 1,
+		topic_goal_sub_, qos,
 		[this](const geometry_msgs::msg::PoseStamped::SharedPtr msg)
 		{ this->callback_goal(msg); });
 
 	sub_map_ = this->create_subscription<nav_msgs::msg::OccupancyGrid>(
-		topic_map_sub_, 1,
+		topic_map_sub_, qos,
 		[this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
 		{ this->callback_map(msg); });
 
 	sub_replan_ = this->create_subscription<
 		geometry_msgs::msg::PoseWithCovarianceStamped>(
-		topic_replan_sub_, 1,
+		topic_replan_sub_, qos,
 		[this](
 			const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
 		{ this->callback_replan(msg); });
 
 	sub_obstacles_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
-		topic_obstacles_sub_, 1,
+		topic_obstacles_sub_, qos,
 		[this](const sensor_msgs::msg::PointCloud2::SharedPtr msg)
 		{ this->callback_obstacles(msg); });
 
 	// Init ROS publishers:
 	// -----------------------
 
 	pub_wp_seq_ = this->create_publisher<mrpt_msgs::msg::WaypointSequence>(
-		topic_wp_seq_pub_, 1);
+		topic_wp_seq_pub_, qos);
 
 	initialize_planner();
 }
@@ -338,13 +335,13 @@ void TPS_Astar_Planner_Node::read_parameters()
 		this->get_logger(), "[TPS_Astar_Planner_Node] starting velocity ="
 								<< start_vel_.asString());
 
-	this->declare_parameter<bool>("mrpt_gui", false);
-	this->get_parameter("mrpt_gui", gui_mrpt_);
+	this->declare_parameter<bool>("show_gui", false);
+	this->get_parameter("show_gui", gui_mrpt_);
 	RCLCPP_INFO(
 		this->get_logger(), "MRPT GUI Enabled: %s",
 		gui_mrpt_ ? "true" : "false");
 
-	this->declare_parameter<std::string>("topic_goal_sub", "");
+	this->declare_parameter<std::string>("topic_goal_sub", topic_goal_sub_);
 	this->get_parameter("topic_goal_sub", topic_goal_sub_);
 	RCLCPP_INFO(
 		this->get_logger(), "topic_goal_sub %s", topic_goal_sub_.c_str());
@@ -439,23 +436,17 @@ void TPS_Astar_Planner_Node::callback_goal(
 			return;
 		}
 
-		tf2::Quaternion quat(
-			_goal->pose.orientation.x, _goal->pose.orientation.y,
-			_goal->pose.orientation.z, _goal->pose.orientation.w);
-		tf2::Matrix3x3 mat(quat);
-		double roll, pitch, yaw;
-		mat.getRPY(roll, pitch, yaw);
-
-		nav_goal_.x = _goal->pose.position.x;
-		nav_goal_.y = _goal->pose.position.y;
-		nav_goal_.phi = yaw;
+		const auto p = mrpt::ros2bridge::fromROS(_goal->pose);
+		nav_goal_ = mrpt::math::TPose2D(p.asTPose());
 
+#if 0  // JLBC: does this make sense with pointcloud obstacles?
 		if (!is_pose_within_map_bounds(nav_goal_))
 		{
 			RCLCPP_WARN(
 				this->get_logger(), "Received goal is outside of the map");
 			return;
 		}
+#endif
 
 		path_plan_done_ = do_path_plan(start_pose_, nav_goal_);
 	}
@@ -595,7 +586,7 @@ void TPS_Astar_Planner_Node::update_map(
 	RCLCPP_INFO_STREAM(this->get_logger(), "Setting gridmap for planning");
 	grid_map_ = std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(obsPts);
 
-	path_plan_done_ = do_path_plan(start_pose_, nav_goal_);
+	// path_plan_done_ = do_path_plan(start_pose_, nav_goal_);
 }
 
 bool TPS_Astar_Planner_Node::do_path_plan(