Enable multiple obstacle sources (grids and points)

mrpt-ros-pkg · Jul 14, 2024 · edcf8c9 · edcf8c9
1 parent ec28b06
commit edcf8c9
Show file tree

Hide file tree

Showing 3 changed files with 186 additions and 208 deletions.
diff --git a/mrpt_tps_astar_planner/README.md b/mrpt_tps_astar_planner/README.md
@@ -33,6 +33,10 @@ Uses A* over a SE(2) lattice and PTGs to sample collision-free paths. The implem
 
 * `show_gui`: Shows its own MRPT GUI with the planned paths.
 
+* `topic_gridmap_sub`: One or more (comma separated) topic names to subscribe for occupancy grid maps.
+
+* `topic_obstacle_points_sub`: One or more (comma separated) topic names to subscribe for obstacle points.
+
 ### Subscribed topics
 * xxx
 

diff --git a/mrpt_tps_astar_planner/launch/tps_astar_planner.launch.py b/mrpt_tps_astar_planner/launch/tps_astar_planner.launch.py
@@ -14,27 +14,23 @@ def generate_launch_description():
     topic_goal_sub = DeclareLaunchArgument(
         'topic_goal_sub', default_value='/goal_pose',
         description='Goal subscription topic')
+
     show_gui = DeclareLaunchArgument(
         'show_gui', default_value='false',
         description='Enable package GUI')
+
     topic_obstacles_gridmap_sub = DeclareLaunchArgument(
-        'topic_obstacles_gridmap_sub', default_value='/obstacles_gridmap',
-        description='Map subscription topic')
+        'topic_obstacles_gridmap_sub', default_value='',
+        description='Topic(s) (comma-separated) to subscribe for incoming occupancy grid maps with obstacles')
+
     topic_obstacles_sub = DeclareLaunchArgument(
-        'topic_obstacles_sub', default_value='/obstacles_pointcloud',
-        description='Obstacles subscription topic')
-    topic_localization_sub = DeclareLaunchArgument(
-        'topic_localization_sub', default_value='/mrpt_pose',
-        description='Localization subscription topic')
-    topic_odometry_sub = DeclareLaunchArgument(
-        'topic_odometry_sub', default_value='odom',
-        description='Odometry subscription topic')
+        'topic_obstacles_sub', default_value='',
+        description='Topic(s) (comma-separated) to subscribe for incoming point clouds with obstacles')
+
     topic_replan_sub = DeclareLaunchArgument(
         'topic_replan_sub', default_value='/replan',
         description='Replan subscription topic')
-    topic_cmd_vel_pub = DeclareLaunchArgument(
-        'topic_cmd_vel_pub', default_value='/enq_motion',
-        description='Command velocity publish topic')
+
     topic_wp_seq_pub = DeclareLaunchArgument(
         'topic_wp_seq_pub', default_value='/waypoints',
         description='Waypoints sequence publish topic')
@@ -66,13 +62,9 @@ def generate_launch_description():
             {'show_gui': LaunchConfiguration('show_gui')},
             {'topic_obstacles_gridmap_sub': LaunchConfiguration(
                 'topic_obstacles_gridmap_sub')},
-            {'topic_localization_sub': LaunchConfiguration(
-                'topic_localization_sub')},
-            {'topic_odometry_sub': LaunchConfiguration('topic_odometry_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')},
             # Param files:
             {'planner_parameters': LaunchConfiguration('planner_parameters')},
@@ -89,11 +81,8 @@ def generate_launch_description():
         topic_goal_sub,
         show_gui,
         topic_obstacles_gridmap_sub,
-        topic_localization_sub,
-        topic_odometry_sub,
         topic_obstacles_sub,
         topic_replan_sub,
-        topic_cmd_vel_pub,
         topic_wp_seq_pub,
         planner_parameters_arg,
         ptg_ini_arg,