[controller_server-2]          77:   template<typename _Res, typename _MemPtr, typename _Tp>
[controller_server-2] #6    Source "/code/ros2_ws/src/navigation2/nav2_controller/src/controller_server.cpp", line 404, in computeControl [0x7f9bc496a5af]
[controller_server-2]         402:       updateGlobalPath();
[controller_server-2]         403: 
[controller_server-2]       > 404:       computeAndPublishVelocity();
[controller_server-2]         405: 
[controller_server-2]         406:       if (isGoalReached()) {
[controller_server-2]         407:         RCLCPP_INFO(get_logger(), "Reached the goal!");
[controller_server-2] #5    Source "/code/ros2_ws/src/navigation2/nav2_controller/src/controller_server.cpp", line 483, in computeAndPublishVelocity [0x7f9bc496bee6]
[controller_server-2]         481:   try {
[controller_server-2]         482:     cmd_vel_2d =
[controller_server-2]       > 483:       controllers_[current_controller_]->computeVelocityCommands(
[controller_server-2]         484:       pose,
[controller_server-2]         485:       nav_2d_utils::twist2Dto3D(twist),
[controller_server-2]         486:       goal_checkers_[current_goal_checker_].get());
[controller_server-2] #4    Source "/code/ros2_ws/src/navigation2/nav2_mppi_controller/src/controller.cpp", line 98, in computeVelocityCommands [0x7f9b8a749357]
[controller_server-2]          95:   nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose);
[controller_server-2]          96: 
[controller_server-2]          97:   geometry_msgs::msg::TwistStamped cmd =
[controller_server-2]       >  98:     optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal_checker);
[controller_server-2]          99: 
[controller_server-2]         100: #ifdef BENCHMARK_TESTING
[controller_server-2]         101:   auto end = std::chrono::system_clock::now();
[controller_server-2] #3    Source "/code/ros2_ws/src/navigation2/nav2_mppi_controller/src/optimizer.cpp", line 139, in evalControl [0x7f9b8a783bd7]
[controller_server-2]         136:   prepare(robot_pose, robot_speed, plan, goal_checker);
[controller_server-2]         137: 
[controller_server-2]         138:   do {
[controller_server-2]       > 139:     optimize();
[controller_server-2]         140:   } while (fallback(critics_data_.fail_flag));
[controller_server-2]         141: 
[controller_server-2]         142:   utils::savitskyGolayFilter(control_sequence_, control_history_, settings_);
[controller_server-2] #2    Source "/code/ros2_ws/src/navigation2/nav2_mppi_controller/src/optimizer.cpp", line 156, in optimize [0x7f9b8a783b62]
[controller_server-2]         153: {
[controller_server-2]         154:   for (size_t i = 0; i < settings_.iteration_count; ++i) {
[controller_server-2]         155:     generateNoisedTrajectories();
[controller_server-2]       > 156:     critic_manager_.evalTrajectoriesScores(critics_data_);
[controller_server-2]         157:     updateControlSequence();
[controller_server-2]         158:   }
[controller_server-2]         159: }
[controller_server-2] #1    Source "/code/ros2_ws/src/navigation2/nav2_mppi_controller/src/critic_manager.cpp", line 74, in evalTrajectoriesScores [0x7f9b8a7b31bf]
[controller_server-2]          71:     if (data.fail_flag) {
[controller_server-2]          72:       break;
[controller_server-2]          73:     }
[controller_server-2]       >  74:     critics_[q]->score(data);
[controller_server-2]          75:   }
[controller_server-2]          76: }
[controller_server-2] #0  | Source "/code/ros2_ws/src/navigation2/nav2_mppi_controller/src/critics/path_follow_critic.cpp", line 44, in setPathCostsIfNotSet
[controller_server-2]     |    43:   utils::setPathFurthestPointIfNotSet(data);
[controller_server-2]     | >  44:   utils::setPathCostsIfNotSet(data, costmap_ros_);
[controller_server-2]     |    45:   const size_t path_size = data.path.x.shape(0) - 1;
[controller_server-2]     | Source "/code/ros2_ws/src/navigation2/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp", line 410, in findPathCosts
[controller_server-2]     |   408: {
[controller_server-2]     |   409:   if (!data.path_pts_valid) {
[controller_server-2]     | > 410:     findPathCosts(data, costmap_ros);
[controller_server-2]     |   411:   }
[controller_server-2]     |   412: }
[controller_server-2]     | Source "/code/ros2_ws/src/navigation2/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp", line 385, in operator=
[controller_server-2]     |   383:       using namespace nav2_costmap_2d; // NOLINT
[controller_server-2]     |   384:       case (LETHAL_OBSTACLE):
[controller_server-2]     | > 385:         (*data.path_pts_valid)[idx] = false;
[controller_server-2]     |   386:         continue;
[controller_server-2]     |   387:       case (INSCRIBED_INFLATED_OBSTACLE):
[controller_server-2]       Source "/usr/include/c++/11/bits/stl_bvector.h", line 95, in score [0x7f9b940a4451]
[controller_server-2]          92:       if (__x)
[controller_server-2]          93: 	*_M_p |= _M_mask;
[controller_server-2]          94:       else
[controller_server-2]       >  95: 	*_M_p &= ~_M_mask;
[controller_server-2]          96:       return *this;
[controller_server-2]          97:     }
[controller_server-2] Segmentation fault (Address not mapped to object [(nil)])
[ERROR] [controller_server-2]: process has died [pid 2557471, exit code -11, cmd '/code/ros2_ws/install/nav2_controller/lib/nav2_controller/controller_server --ros-args -p use_sim_time:=True --params-file /tmp/unaliased_st_s9n4u'].