ros中使用自定义状态机

                               ros中使用自定义状态机

定义状态机的类：NavigationStateMachine

#include <ros/ros.h>
#include<ros/package.h>
#include <iostream>
//状态机
#include <functional> // 导入 functional 头文件
#include <unordered_map> // 导入 unordered_map 头文件
#include <string> // 导入 string 头文件
#include <boost/any.hpp> // 导入 boost any 头文件
#include <boost/bind.hpp>
#include <boost/thread.hpp>
// 导航状态机类
    class NavigationStateMachine {
    public:
        // 构造函数
        NavigationStateMachine(ros::NodeHandle &nh) : nh_(nh), current_state_("") {}


   //  NavigationStateMachine 类的 add_state 方法以接受参数的向量
        void add_state(const std::string &name, std::function<void(const std::vector<boost::any> &)> state_function) {
            state_functions_[name] = state_function;
        }


  //  NavigationStateMachine 类的 set_state 方法以存储参数向量
        int set_state(const std::string &name, const std::vector<boost::any> &params = {}) {
            current_state_ = name;
            state_params_[name] = params;
            return state_params_[name].size();
        }



        //获取当前状态
        std::string  get_current_state() const {
          return current_state_;
            
        }


        //获取当前状态的参数
        bool get_current_state_param(int index,boost::any &age) const {
            if(index<state_params_.at(current_state_).size()){
                std::vector<boost::any> args = state_params_.at(current_state_);
                age =  args.at(index);
                return true;
            }else{
                return false;
            }

        }


        // 修改 NavigationStateMachine 类的 execute 方法以传递参数向量
        void execute() {


            if (!current_state_.empty() && state_functions_.count(current_state_)) {


                state_functions_[current_state_](state_params_[current_state_]);

            } else {
                ROS_ERROR("Invalid state: %s", current_state_.c_str());


            }
        }

    private:
        ros::NodeHandle nh_; // 节点句柄
        std::string current_state_; // 当前状态的名称
        //boost::any current_args_; // 当前状态的参数 被state_params_ 替换成多参数传递
        std::unordered_map<std::string, std::vector<boost::any>> state_params_;//增加多个参数传递,当前状态的参数
        //std::unordered_map<std::string, std::function<void(const boost::any &)>> state_functions_; // 存储状态名和对应的函数
        std::unordered_map<std::string, std::function<void(const std::vector<boost::any> &)>> state_functions_; // 存储状态名和对应的函数

    };

增加状态

        //状态机
        state_machine_.add_state("navigate_to_goalPath", std::bind(&global_planner::navigate_to_goalpath, this, std::placeholders::_1));//加入状态 navigate_to_goalPath 导航状态
        state_machine_.add_state("navigate_to_goalPathAgain", std::bind(&global_planner::navigate_to_goalpath, this, std::placeholders::_1));//加入状态 navigate_to_goalPathAgain 导航状态重复发布导航
        state_machine_.add_state("navigate_to_goalpathOverturn", std::bind(&global_planner::navigate_to_goalpathOverturn, this, std::placeholders::_1));//加入状态 navigate_to_goalPathAgain 导航状态重复发布导航
        state_machine_.add_state("charging", std::bind(&global_planner::chargeFun, this, std::placeholders::_1));//给充电桩发送充电指令
        state_machine_.add_state("monitor_progress", std::bind(&global_planner::monitor_progress, this, std::placeholders::_1));//加入状态 monitor_progress 监控进度状态
     // state_machine_.add_state("monitor_progress", std::bind(&global_planner::monitor_progress, this, std::placeholders::_1));//加入状态 monitor_progress 监控进度状态
        //初始化给一个控制
        geometry_msgs::PoseStamped start_pose,target_pose,target_pose_default;
        state_machine_.set_state("idle", {start_pose,target_pose,target_pose_default,std::string("")});//闲置状态,初始化给一个控制

 获取当前状态

    // 获取当前状态的成员函数
                std::string  current_state = state_machine_.get_current_state();
                std::cout << "2.current_state: " << current_state << std::endl;

获取当前参数（一）（多参数，多类型传递 std::vector<boost::any> params）

                    boost::any age0,age1,age2,age3;
                    state_machine_.get_current_state_param(0,age0);//起始点
                    state_machine_.get_current_state_param(1,age1);//目标点
                    state_machine_.get_current_state_param(2,age2);//默认点
                    state_machine_.get_current_state_param(3,age3);
                    geometry_msgs::PoseStamped start_pose = boost::any_cast<geometry_msgs::PoseStamped>(age0);
                    geometry_msgs::PoseStamped target_pose = boost::any_cast<geometry_msgs::PoseStamped>(age1);
                    geometry_msgs::PoseStamped target_pose_default = boost::any_cast<geometry_msgs::PoseStamped>(age2);//默认点位，可以从数据库中读取，可以回充电装
                    std::string  current_string = boost::any_cast<std::string>(age3);

 

获取当前参数（二）（多参数，多类型传递 std::vector<boost::any> params）

#include <iostream>
#include <boost/any.hpp>
#include <geometry_msgs/PoseStamped.h>

template<typename T>
bool get_param_as(const boost::any& param, T& result) {
    if (param.type() == typeid(T)) {
        result = boost::any_cast<T>(param);
        return true;
    }
    return false;
}

int main() {

    boost::any age0,age1,age2,age3;
    state_machine_.get_current_state_param(0,age0);
    state_machine_.get_current_state_param(1,age1);
    state_machine_.get_current_state_param(2,age2);
    state_machine_.get_current_state_param(3,age3);

    geometry_msgs::PoseStamped start_pose;
    if (get_param_as(age0, start_pose)) {
        std::cout << "1. Successfully got start_pose." << std::endl;
    } else {
        std::cerr << "1. Failed to get start_pose. Wrong type." << std::endl;
    }

    geometry_msgs::PoseStamped target_pose;
    if (get_param_as(age1, target_pose)) {
        std::cout << "2. Successfully got target_pose." << std::endl;
    } else {
        std::cerr << "2. Failed to get target_pose. Wrong type." << std::endl;
    }

    geometry_msgs::PoseStamped target_pose_default;
    if (get_param_as(age2, target_pose_default)) {
        std::cout << "3. Successfully got target_pose_default." << std::endl;
    } else {
        std::cerr << "3. Failed to get target_pose_default. Wrong type." << std::endl;
    }

    std::string current_string;
    if (get_param_as(age3, current_string)) {
        std::cout << "4. Successfully got current_string: " << current_string << std::endl;
    } else {
        std::cerr << "4. Failed to get current_string. Wrong type." << std::endl;
    }

    return 0;
}

获取当前参数（三）（多参数，多类型传递 std::vector<boost::any> params）

template<typename T>
T any_cast(const boost::any & operand)
{
    try
    {
        return boost::any_cast<T>(operand);
    }
    catch (boost::bad_any_cast &)
    {
       ROS_ERROR("获取数据 boost::any 中的数据类型转换失败 ,Failed to cast %s to %s", operand.type().name(), typeid(T).name());
        throw;
    }
}

geometry_msgs::PoseStamped start_pose, target_pose, target_pose_default;
std::string current_string;
 boost::any age0,age1,age2,age3;

try
{

state_machine_.get_current_state_param(0, age0);  // 起始点
state_machine_.get_current_state_param(1, age1);  // 目标点
state_machine_.get_current_state_param(2, age2);  // 默认点
state_machine_.get_current_state_param(3, age3);  // 获取当前状态的参数


    start_pose = any_cast<geometry_msgs::PoseStamped>(age0);
    target_pose = any_cast<geometry_msgs::PoseStamped>(age1);
    target_pose_default = any_cast<geometry_msgs::PoseStamped>(age2);
    current_string = any_cast<std::string>(age3);
}
catch(...)
{
    // handle the exception
}

切换当前状态

geometry_msgs::PoseStamped start_pose;
geometry_msgs::PoseStamped target_pose;
geometry_msgs::PoseStamped target_pose_default;
//切换当前状态
state_machine_.set_state("navigate_to_goalpathOverturn", {start_pose,target_pose, target_pose_default,std::string("charging")});///charging 充电

多线程执行状态机

        boost::thread parse_thread(boost::bind(&global_planner::thread_msg, this));

//多线程
    void global_planner::thread_msg() {
        ros::Rate r(10);
        while (ros::ok()) {
            std::cout << "=======1多线程======global_planner=====thread_msg============state_machine_.get_current_state() " <<state_machine_.get_current_state() <<std::endl;
            state_machine_.execute();
            ros::spinOnce();
            std::cout << "=======2多线程======global_planner=====thread_msg==================state_machine_.get_current_state() " <<state_machine_.get_current_state() << std::endl;

            // ros::spinOnce();
            //std::this_thread::sleep_for(std::chrono::milliseconds(100));
            r.sleep();
        }
    }

获取 boost::any 中存储数据的类型

                   
boost::any age3;
state_machine_.get_current_state_param(3,age3);//获取当前状态的参数
 std::cout << "5. age3: " << age3.type().name() << std::endl;//获取boost::any age3 类型，v 表示该 boost::any 对象没有存储任何值，因此无法确定其类型。这里 i 是 int 类型的缩写。NSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE 这是一个 std::string 类型的类型名，其中 N 表示该类型是在命名空间中定义的。具体来说，NSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE 表示 std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >。
//如果类是std::string 类型 （NSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE）就获取直
std::string  current_string = boost::any_cast<std::string>(age3);

改成std::map<std::string,boost::any> &params 这种方式传参数

// 导航状态机类
    class NavigationStateMachine {
    public:
        // 构造函数
        NavigationStateMachine(ros::NodeHandle &nh) : nh_(nh), current_state_("") {}

        //  NavigationStateMachine 类的 add_state 方法以接受参数的向量
        void add_state(const std::string &name, std::function<void( std::map<std::string,boost::any> &)> state_function) {
            state_functions_[name] = state_function;
        }

       //  NavigationStateMachine 类的 set_state 方法以存储参数向量
        int set_state( std::string &name,  std::map<std::string,boost::any> &params ) {
            current_state_ = name;
            state_params_[name] = params;
            return state_params_[name].size();
        }


        // 获取当前状态的成员函数
        std::string get_current_state() const {
            return current_state_;
        }
        //获取当前状态的参数
        bool get_current_state_param(string inner_key,boost::any &age)  {
            if (containsKey(state_params_, current_state_, inner_key)) {
                // 键存在
                std::map<std::string,boost::any> args = state_params_.at(current_state_);
                age =  args.at(inner_key);
                return true;
            } else {
                // 键不存在
                return false;
            }

        }
        //判断map判断key是否存在
        //                std::unordered_map<std::string, std::map<std::string,boost::any>>
        bool containsKey(std::unordered_map<std::string, std::map<std::string,boost::any>>& myMap,  std::string& outerKey,  std::string& innerKey) {
            auto it = myMap.find(outerKey);
            if (it != myMap.end()) {
                std::map<std::string, boost::any>& innerMap = it->second;
                return innerMap.find(innerKey) != innerMap.end();
            }
            return false;
        }

        //  NavigationStateMachine 类的 execute 方法以传递参数向量
        void execute() {
            if (!current_state_.empty() && state_functions_.count(current_state_)) {
               // std::cout << "=======2.开始执行状态机 execute ==================状态机当前状态 " <<current_state_ << std::endl;
                state_functions_[current_state_](state_params_[current_state_]);
            }
        }

    private:
        ros::NodeHandle nh_; // 节点句柄
        std::string current_state_; // 当前状态的名称
        std::unordered_map<std::string, std::map<std::string,boost::any>> state_params_;//增加多个参数传递,当前状态的参数
        std::unordered_map<std::string, std::function<void( std::map<std::string,boost::any> &)>> state_functions_; // 存储状态名和对应的函数

    };

ros中行为树实现导航任务

https://blog.csdn.net/qq_15204179/article/details/130643759