Can MPPI follow the path with time stamps? about navigation2 HOT 7 OPEN

    using namespace xt::placeholders; // NOLINT
    using xt::evaluation_strategy::immediate;

    void TimeCritic::initialize()
    {
        auto getParam = parameters_handler_->getParamGetter(name_);
        getParam(path_dt_, "path_dt", 0.1f);
        getParam(power_, "cost_power", 1);
        getParam(weight_, "cost_weight", 10.0);
        getParam(weight_1_, "cost_weight_1", 10.0);
        getParam(weight_2_, "cost_weight_2", 10.0);

        RCLCPP_INFO(
            logger_,
            "TimeCritic instantiated with %d power and %f weight",
            power_, weight_);
    }

    void TimeCritic::score(CriticData &data)
    {
        std::cout << "============ goes into TimeCritic ============" << std::endl;
        // print the time of pose stamped in data
        std::cout << "============ pose time: " << data.state.pose.header.stamp.sec << "." << data.state.pose.header.stamp.nanosec << std::endl;
        float control_dt = data.model_dt;

        utils::setPathFurthestPointIfNotSet(data);
        const size_t path_segments_count = *data.furthest_reached_path_point; // up to furthest only

        if (path_segments_count < 2) // offset_from_furthest_
        {
            return;
        }
        // std::cout << "============ [TimeCritic] ckpt1" << std::endl;

        const auto P_x = xt::view(data.path.x, xt::range(_, -1));      // path points
        const auto P_y = xt::view(data.path.y, xt::range(_, -1));      // path points
        const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1)); // path points

        std::cout << "P_x shape: " << P_x.shape(0) << ", " << P_x.shape(1) << std::endl;

        std::cout << "============ P_x, P_y: " << std::endl;
        for (unsigned int i = 1; i < P_x.shape(0); i++)
        {
            std::cout << P_x(i) << ", " << P_y(i) << ";  ";
        }
        std::cout << "============ " << std::endl;

        const size_t batch_size = data.trajectories.x.shape(0);
        const size_t time_steps = data.trajectories.x.shape(1);
        auto &&cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
        // std::cout << "============ [TimeCritic] ckpt2" << std::endl;
        // Find integrated distance in the path
        std::vector<float> path_integrated_distances(path_segments_count, 0.0f);
        float dx = 0.0f, dy = 0.0f, dyaw = 0.0f;

        for (unsigned int i = 1; i != path_segments_count; i++)
        {
            dx = P_x(i) - P_x(i - 1);
            dy = P_y(i) - P_y(i - 1);
            float curr_dist = sqrtf(dx * dx + dy * dy);
            path_integrated_distances[i] = path_integrated_distances[i - 1] + curr_dist;
        }
        // std::cout << "============ [TimeCritic] ckpt3" << std::endl;
        size_t step_interval = path_dt_ / control_dt; // 0.1 / 0.05
        float Tx = 0.0f, Ty = 0.0f;
        size_t path_pt = 0;
        float num_samples = 0.0f;
        float summed_path_cost = 0.0f;
        // float lambda = 0.0f;
        for (size_t t = 0; t < batch_size; ++t)
        {
            // std::cout << "============ [TimeCritic] ckpt4" << std::endl;
            // path_pt = utils::findClosestPathPt(
            //     path_integrated_distances, 0.0, 0);
            path_pt = 0;
            num_samples = 0.0f;
            summed_path_cost = 0.0f;
            const auto T_x = xt::view(data.trajectories.x, t, xt::all());
            const auto T_y = xt::view(data.trajectories.y, t, xt::all());
            const auto T_yaw = xt::view(data.trajectories.yaws, t, xt::all());

            // std::cout << "============ [TimeCritic] ckpt4.5" << std::endl;
            for (size_t p = step_interval; p < time_steps; p += step_interval)
            {
                // lambda = 1.0f - static_cast<float>(p) / time_steps;

                path_pt += 1;
                if (path_pt >= P_x.shape(0))
                {
                    path_pt = P_x.shape(0) - 1;
                }

                Tx = T_x(p);
                Ty = T_y(p);
                // std::cout << "============ [TimeCritic] ckpt4.8" << std::endl;
                // if ((*data.path_pts_valid)[path_pt])
                // {
                // std::cout << "============ [TimeCritic] ckpt4.9" << std::endl;
                dx = P_x(path_pt) - Tx;
                dy = P_y(path_pt) - Ty;
                // std::cout << "============ [TimeCritic] ckpt4.99" << std::endl;
                num_samples += 1.0f;

                dyaw = angles::shortest_angular_distance(P_yaw(path_pt), T_yaw(p));
                summed_path_cost += sqrtf(dx * dx + dy * dy + 2.0 * dyaw * dyaw);
                // summed_path_cost += sqrtf(dx * dx + dy * dy);
                // }
            }
            // std::cout << "============ [TimeCritic] ckpt5" << std::endl;
            if (num_samples > 0)
            {
                cost[t] = summed_path_cost / num_samples;
            }
            else
            {
                cost[t] = 0.0f;
            }
        }
        data.costs += xt::pow(std::move(cost) * weight_1_, power_);
        // std::cout << "============ [TimeCritic] ckpt6" << std::endl;
    }

  ros__parameters:
    controller_frequency: 30.0  # 控制器的更新频率
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"  # 指定使用 MPPI 控制器
      time_steps: 40  # 预测轨迹的时间步数
      model_dt: 0.05  # 每个时间步的持续时间（秒）
      batch_size: 3000  # 每次迭代中采样的轨迹数量
      vx_std: 0.3  # X轴速度的采样标准差
      vy_std: 0.3  # Y轴速度的采样标准差（仅限全向运动模型）
      wz_std: 0.7  # 角速度的采样标准差
      vx_max: 0.8  # 最大前进速度（米/秒）
      vx_min: -0.8  # 最大后退速度（米/秒）
      wz_max: 3.5  # 最大旋转速度（弧度/秒）
      iteration_count: 1  # MPPI 算法中的迭代次数
      enforce_path_inversion: false
      prune_distance: 2.4  # 路径修剪距离
      transform_tolerance: 0.1  # 数据变换的时间容忍度
      temperature: 0.01  # 轨迹成本的选择性（越接近0考虑的成本越低）
      gamma: 0.01  # 平滑性和低能量之间的权衡
      motion_model: "Ackermann"  # 使用的运动模型（DiffDrive, Omni, Ackermann）
      visualize: true  # 是否发布用于可视化的调试轨迹
      reset_period: 1.0  # 优化器重置所需的不活动时间
      regenerate_noises: false  # 是否在每次迭代中重新生成噪声
      AckermannConstraints:
        min_turning_r: 0.24
      # 以下是各种评价函数（critics）的配置
      critics: ["TimeCritic", "ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
      ConstraintCritic:
        cost_weight: 0.0
        cost_power: 1
      GoalCritic:
        cost_weight: 0.0
        cost_power: 1
        threshold_to_consider: 0.1
      GoalAngleCritic:
        cost_weight: 3.0
        cost_power: 1
        threshold_to_consider: 0.5
      PreferForwardCritic:
        cost_weight: 0.0 # 0.0
        cost_power: 1
        threshold_to_consider: 0.5
      ObstaclesCritic:
        consider_footprint: false
        collision_cost: 10000.0
        collision_margin_distance: 0.1
        inflation_radius: 0.55
        cost_scaling_factor: 10.0
      PathAlignCritic:
        cost_weight: 50.0 # 20.0
        cost_power: 1
        max_path_occupancy_ratio: 0.05
        threshold_to_consider: 0.1
        offset_from_furthest: 20
        use_path_orientations: true
      PathFollowCritic:
        cost_weight: 50.0 # 50.0
        cost_power: 1
        threshold_to_consider: 0.1
        offset_from_furthest: 20
      PathAngleCritic:
        cost_weight: 0.0 # 5.0
        cost_power: 1
        threshold_to_consider: 0.5
        forward_preference: false
      TimeCritic:
        path_dt: 0.1
        cost_weight: 0.0 # 5.0
        cost_weight_1: 50.0 # 100.0
        cost_weight_2: 0.0 # 10.0
        cost_power: 1