Localizer working with DEM and gps_goal added to launch file

src/subsystems/navigation/athena_map/config/dem_costmap.yaml

@@ -1,8 +1,8 @@
 dem_costmap_converter:
   ros__parameters:
-    dem_file_path: "src/subsystems/navigation/athena_map/maps/north_site800m.tif"                  # override via launch-arg if you like
+    dem_file_path: ""      
     map_resolution: 1.0
     max_passable_slope_degrees: 15.0
     output_frame: map
-    origin_x: 518370.0
-    origin_y: 4253272.0
+    origin_x: -400.0
+    origin_y: -400.0

src/subsystems/navigation/athena_map/launch/dem_costmap.launch.py

@@ -10,13 +10,19 @@
 def generate_launch_description():
     pkg_dir = get_package_share_directory('athena_map')
     config_file = os.path.join(pkg_dir, 'config', 'dem_costmap.yaml')
+    dem_file = os.path.join(pkg_dir, 'maps', 'north_site800m.tif')
 
     dem_node = Node(
         package='athena_map',
         executable='map_node',
         name='dem_costmap_converter',
         output='screen',
-        parameters=[config_file]
+        parameters=[
+            config_file,
+            {
+                'dem_file_path': dem_file
+            }
+        ]
     )
 
     return LaunchDescription([

src/subsystems/navigation/athena_map/src/map_node.cpp

@@ -209,9 +209,6 @@ class DEMCostmapConverter : public rclcpp::Node
         if (costmap_ready_) {
             costmap_msg_.header.stamp = this->get_clock()->now();
             costmap_publisher_->publish(costmap_msg_);
-            RCLCPP_INFO(this->get_logger(), "Published costmap: %dx%d, resolution: %.2f, origin: (%.2f, %.2f)", 
-                        costmap_msg_.info.width, costmap_msg_.info.height, costmap_msg_.info.resolution,
-                        costmap_msg_.info.origin.position.x, costmap_msg_.info.origin.position.y);
         } else {
             RCLCPP_DEBUG(this->get_logger(), "Costmap not ready, skipping publish");
         }

src/subsystems/navigation/athena_planner/config/nav2_params.yaml

@@ -154,9 +154,9 @@ global_costmap:
       global_frame: map
       update_frequency: 1.0
       publish_frequency: 1.0
-      resolution: 0.5
-      width: 100
-      height: 100
+      resolution: 1.0
+      width: 800
+      height: 800
       rolling_window: true
       track_unknown_space: false
       always_send_full_costmap: true

src/subsystems/navigation/athena_planner/launch/navigation.launch.py

@@ -17,9 +17,15 @@ def generate_launch_description():
     athena_planner_share = get_package_share_directory('athena_planner')
     nav2_nav = os.path.join(athena_planner_share, 'launch', 'nav2_nodes.launch.py')
 
+    athena_map_share = get_package_share_directory('athena_map')
+    dem_costmap_launch_file = os.path.join(athena_map_share, 'launch', 'dem_costmap.launch.py')
+
     localizer_share = get_package_share_directory('localizer')
     localizer_launch_file = os.path.join(localizer_share, 'launch', 'localizer.launch.py')
 
+    gps_goal_share = get_package_share_directory('gps_goal')
+    gps_goal_launch_file = os.path.join(gps_goal_share, 'launch', 'gps_goal_server.launch.py')
+
     default_params = PathJoinSubstitution([
         FindPackageShare('athena_planner'), 'config', 'nav2_params.yaml'
     ])
@@ -39,14 +45,20 @@ def generate_launch_description():
         ],
     )
 
+    dem_costmap_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(dem_costmap_launch_file)
+    )
+
     localizer_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(localizer_launch_file),
         launch_arguments={
             'use_sim_time': 'true',
         }.items()
     )
 
-
+    gps_goal_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(gps_goal_launch_file)
+    )
 
     return LaunchDescription([
         DeclareLaunchArgument(
@@ -62,9 +74,9 @@ def generate_launch_description():
             description='Log level for nav2 nodes'),
 
         twist_stamper_node,
+        dem_costmap_launch, 
         localizer_launch,
-
-       #IncludeLaunchDescription(PythonLaunchDescriptionSource(dem_launch)),
+        gps_goal_launch,
 
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(nav2_nav),