MPPIOptimizer.hpp

Go to the documentation of this file.

// Copyright 2025 Intelligent Robotics Lab
//
// This file is part of the project Easy Navigation (EasyNav in short)
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
// #pragma once
#ifndef EASYNAV_MPPI_CONTROLLER__MPPIOPTIMIZER_HPP_
#define EASYNAV_MPPI_CONTROLLER__MPPIOPTIMIZER_HPP_
 
#include <utility>
#include <vector>
#include <random>
 
#include "geometry_msgs/msg/pose.hpp"
#include "nav_msgs/msg/path.hpp"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
 
namespace easynav
{

struct MPPIResult
{
  double v; 
  double w; 
  std::vector<std::vector<std::pair<double, double>>> all_trajectories; 
  std::vector<std::pair<double, double>> best_trajectory; 
};
 
struct TrajectorySample
{
  double v;     
  double w;     
  double cost;  
};
 
class MPPIOptimizer
{
public:
  MPPIOptimizer(
    double num_samples, double horizon_steps, double dt, double lambda,
    double max_lin_vel = 1.0, double max_ang_vel = 1.0, double max_lin_acc = 1.0,
    double max_ang_acc = 1.0, double fov = M_PI / 2.0, double safety_radius = 0.6);

  MPPIResult compute_control(
    const geometry_msgs::msg::Pose & current_pose,
    const nav_msgs::msg::Path & path,
    const pcl::PointCloud<pcl::PointXYZ> & points);
 
private:
  double num_samples_;    
  double horizon_steps_;  
  double dt_;             
  double lambda_;         
  double max_lin_vel_;    
  double max_ang_vel_;    
  double max_lin_acc_;    
  double max_ang_acc_;    
  double fov_;            
  double safety_radius_;  
  double last_v_ = 0.0;   
  double last_w_ = 0.0;   
 
  std::default_random_engine rng_; 
  std::normal_distribution<double> normal_ = std::normal_distribution<double>(0.0, 0.5);    
  std::normal_distribution<double> v_noise_ = std::normal_distribution<double>(0.0, 0.05);  
  std::normal_distribution<double> w_noise_ = std::normal_distribution<double>(0.0, 0.02);  

  double compute_cost(
    const std::vector<std::pair<double, double>> & trajectory,
    const nav_msgs::msg::Path & path,
    double v, double w, double initial_yaw,
    const pcl::PointCloud<pcl::PointXYZ> & points);

  std::vector<std::pair<double, double>> simulate_trajectory(
    double x, double y, double yaw,
    double v, double w, const nav_msgs::msg::Path & path, int steps);

  double heading_error(
    double robot_yaw,
    double target_x, double target_y,
    double robot_x, double robot_y);

  double shortest_angular_distance(double from, double to);
 
};
 
}  // namespace easynav
 
#endif  // EASYNAV_MPPI_CONTROLLER__MPPIOPTIMIZER_HPP_