std::vector<int> score{ 10, 20, 30, 40 };

auto it_1 = std::find(score.begin(), score.end(), 30 );
if(it_1 != score.end() )
    qDebug()<< "found !";
else qDebug()<< "no found !";

auto it_2 = std::find_if(score.begin(), score.end(), [](int& m){return m>20;}  );
if(it_2 != score.end() )
    qDebug()<< "found: " << *it_2;
else qDebug()<< "no found !";

#include <iostream>     // std::cout
#include <iterator>     // std::next
#include <list>         // std::list
using namespace std;
int main() {
    //创建并初始化一个 list 容器
    std::list<int> mylist{ 1,2,3,4,5 };
    std::list<int>::iterator it = mylist.end();
    //获取一个距离 it 迭代器 2 个元素的迭代器，由于 2 为正数，newit 位于 it 左侧
    auto newit = prev(it, 2);
    cout << "prev(it, 2) = " << *newit << endl;   // 4

    //n为负数，newit 位于 it 右侧
    it = mylist.begin();
    newit = prev(it, -2);
    cout << "prev(it, -2) = " << *newit;   // 3
    return 0;
}

#include "ceres/ceres.h"

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;

struct  Functor
{
	template <typename T> bool operator()(const T* const x, T* residual) const
	{
		residual[0] = 10.0 - x[0];
		residual[1] = 3.0 - x[1];
		// 直接让第2维度为0，优化后的x[1]还是初值2
		// residual[1] = T(0.0);
		return true;
	}
};

struct  TestLocalParam
{
	template<typename T> bool operator()(const T* x, const T* delta, T* x_plus_delta) const{
		x_plus_delta[0] = x[0] + 0.4 * delta[0];
		x_plus_delta[1] = x[1] + 0.4 * delta[1];
		return true;
	}
};

// double x = 0.5;
double x[] = {0.5, 2.0};

Problem problem;

problem.AddParameterBlock(x, 2, 
  (new ceres::AutoDiffLocalParameterization
  	// 真正的维度是2， 参与优化的维度是1
  	<TestLocalParam, 2, 1> ())
	);
// AddParameterBlock 修改之后，无论下面的AutoDiffCostFunction参数是多少，只优化第1维度


// 残差的维度，参数的维度
// 如果此时是 2,1. 报错
CostFunction* cost_func = new AutoDiffCostFunction<Functor, 2, 2>(new Functor);
// 参数：CostFunction，可选的LossFunction，将CostFunction连接到一组参数块
problem.AddResidualBlock(cost_func, nullptr, x);

  Solver::Options  option;
  option.minimizer_progress_to_stdout = true;
  option.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

  Solver::Summary summary;
  Solve(option, &problem, &summary);
  
  std::cout << summary.BriefReport() << std::endl<<std::endl;
  // std::cout << summary.FullReport() << std::endl<<std::endl;

  // std::cout << "initial x: " << initial_x <<"   x: "<< x << std::endl;
  std::cout <<"x[0]: "<< x[0] <<"   x[1]: "<< x[1]<< std::endl;
  return 0;
}

#include <opencv2/opencv.hpp>

// 真实值
double a =1.0, b=2.0, c=1.0;
// abc的初值
double abc[3]  = {0,0,0};

// 生成带有噪声的模拟数据
vector<double> x_data, y_data;
int N = 240;  // 数据数量
double w_sigma = 1.0;  // 高斯标准差
cv::RNG  rng;
for(unsigned int i =0; i<N; i++)
{
    double x = i/100.0;
    x_data.push_back(x);
    y_data.push_back(exp(a*x*x + b*x + c) + rng.gaussian(w_sigma)  );
}

struct Functor
{
    Functor(double x, double y):
    m_x(x), m_y(y) {    }
    #if 0
    // 开始我是这么写的，没弄清残差的维度
    template<typename T> bool operator()( const T* const a, 
                                          const T* const b, 
                                          const T* const c, 
                                          T* residual)  const
    {
        residual[0] = m_y - exp(a[0] * m_x * m_x + b[0] * m_x + c[0]);
        return true;
    }
    #endif

    // 因为要传入数据(x,y)， 所以要定义两个成员变量
    template<typename T> bool operator()( const T* const abc, T* residual)  const
    {
        residual[0] = m_y - exp(abc[0] * m_x * m_x + abc[1] * m_x + abc[2]);
        return true;
    }
    const double m_x, m_y;
};

Problem problem;
for(unsigned int i=0; i<N; i++)
{
	// 残差维度， 所估计变量的维度
  CostFunction* cost_func = new AutoDiffCostFunction<Functor,1,3>(new Functor(x_data[i], y_data[i]) );
  problem.AddResidualBlock(cost_func, nullptr, abc);
}
Solver::Options option;
option.minimizer_progress_to_stdout = true;
option.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

Solver::Summary summary;
Solve(option, &problem, &summary);

cout << summary.BriefReport() << endl <<endl;
cout << "a: "<< abc[0] <<"  b: "<< abc[1]<< "  c: "<< abc[2] <<endl;

add_action_files(
   DIRECTORY action
   FILES
   DoDishes.action
 )

<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<run_depend>actionlib</run_depend>
<run_depend>actionlib_msgs</run_depend>

// action定义在 scheduler/action/Scheduler.action
typedef actionlib::SimpleActionServer<scheduler::SchedulerAction> Server;
actionlib::SimpleActionServer<scheduler::SchedulerAction>  as_;

// 注册抢占回调函数
as_.registerPreemptCallback(boost::bind(&Explore::preemptCB, this))
as_.registerGoalCallback(boost::bind(&Explore::goalCB, this));


void Explore::preemptCB()
{
  ROS_INFO("%s: Preempted", action_name_.c_str());
  explore_ = false;
  explore_state_ = STOP;
  ac_.cancelAllGoals();
  if(as_.isActive())
	as_.setPreempted();
}

void Explore::goalCB()
{
  explore_state_ = FULL_ROTATE;
  as_.acceptNewGoal();
}

void actionlib::SimpleActionClient< ActionSpec >::sendGoal(const Goal& goal,
    SimpleDoneCallback       done_cb = SimpleDoneCallback(),
    SimpleActiveCallback     active_cb = SimpleActiveCallback(),
    SimpleFeedbackCallback   feedback_cb = SimpleFeedbackCallback() 
)

cout<< ac_.getState().toString() <<endl;

// 获取goal的状态信息
// PENDING, ACTIVE, RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST(isn't tracking a goal)
SimpleClientGoalState   getState () const
// 输出上面的几个状态，大写字母，对调试很有用
std::string toString() const

[ INFO] [1648276976.479444090]: move_base client send goal: type: 1
target_pose: 
  header: 
    seq: 11
    stamp: 1648276976.479361050
    frame_id: map
  pose: 
    position: 
      x: -3.9901
      y: 0.427604
      z: 0
    orientation: 
      x: 0
      y: 0
      z: 0
      w: 1

[ INFO] [1648276976.492017723]: move base task just went active
[ WARN] [1648276979.458537608]: 32 receive cmd: position: 
  x: -4.01633
  y: 0.440816
  z: 0
orientation: 
  x: 0
  y: 0
  z: 0
  w: 1

[ INFO] [1648276979.494004129]: actionlib result state: PREEMPTED
[ INFO] [1648276979.494157917]: actionlib result: result: 0
result_pose: 
  header: 
    seq: 0
    stamp: 0.000000000
    frame_id: 
  pose: 
    position: 
      x: 0
      y: 0
      z: 0
    orientation: 
      x: 0
      y: 0
      z: 0
      w: 0

[ INFO] [1648276979.494228945]: actionDoneCb state PREEMPTED
[ INFO] [1648276979.494308493]: move_base client send goal: type: 1
target_pose: 
  header: 
    seq: 12
    stamp: 1648276979.494253319
    frame_id: map
  pose: 
    position: 
      x: 0
      y: 0
      z: 0
    orientation: 
      x: 0
      y: 0
      z: 0
      w: 0

[ERROR] [1648276979.504876032]: BUG: Got a transition to CommState [PENDING] when our in SimpleGoalState [DONE]
[ERROR] [1648276979.504963821]: BUG: Got a transition to CommState [RECALLING] when our in SimpleGoalState [DONE]
[ERROR] [1648276979.514087412]: BUG: Got a transition to CommState [PREEMPTING] when in SimpleGoalState [DONE]
[ERROR] [1648276979.515084871]: BUG: Got a second transition to DONE

class Son;
class Father
{
public:
    ~Father() {cout<<" father 析构"<<endl; }
    shared_ptr<Son> son_;
};
class Son {
    public:
    ~Son() {cout<<" son 析构"<<endl; }
    shared_ptr<Father> father_;
};

auto father = make_shared<Father>();
auto son = make_shared<Son>();
father->son_ = son;
son->father_ = father;

std::vector<int> vec {1,2,3,4,5};
for (auto n :vec)
    std::cout << n;
 
int arr[10] = { 1, 2, 3, 4, 5};
for (auto n : arr)
    std::cout << n;

std::vector<int> vec {1,2,3,4,5};
cout << "修改前" << endl;
for (auto& n :vec)
    std::cout << n++;
cout << endl;

cout << "修改后" << endl;
for (auto j : vec)
    std::cout << j;

std::filesystem::path current_path = std::filesystem::current_path();

string str = { "C++ as const test" };
cout << is_const<decltype(str)>::value << endl;     // 0

const string str_const = std::as_const(str);
cout << is_const<decltype(str_const)>::value << endl;  // 1

int a = {3+4};
int a{6+8};
int a(6+8);

int roc = {};
int roc{};

vector<int> c{ 1, 2, 3 };

class example
{
    int a = 1;
    double b{ 1.2 };
};

enum class SensorType {
      RANGE = 0,
      IMU,
      ODOMETRY,
      FIXED_FRAME_POSE,
      LANDMARK,
      LOCAL_SLAM_RESULT
    };

class MapBuilderInterface {
 public:
using LocalSlamResultCallback =
  TrajectoryBuilderInterface::LocalSlamResultCallback;

using SensorId = TrajectoryBuilderInterface::SensorId;

using Callback =
      std::function<void(const std::string&, std::unique_ptr<Data>)>;