状态机的编写（使用C++）

最新推荐文章于 2024-06-22 17:19:57 发布
最新推荐文章于 2024-06-22 17:19:57 发布
阅读量2.1k 收藏 12
点赞数 2
分类专栏： ROS
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/john123741/article/details/79300561
版权

这几天写了一些无人车的状态机的程序，其实就是从python上面翻译过来的，忽然发现python写东西很方便，不需要考虑数据类型什么的，而且python有一个smach的语句，C++没有，状态机需要自己手动实现，做起来细节很多，主要是语法方面不熟导致了很多错误

ROS里面不要用sleep和usleep函数，如果需要等待，请使用ros::Rate

此外，还有一个地方搞不懂的就是，我一开始写的时候是初始化和执行放在两个不同的函数当中，但是在两个函数里面声明的变量其实不是同一个变量，他们的成员变量并不是同一个，即使在声明变量的时候放在加上static可不行，可能static只初始化一次这个操作需要在同一函数的的当中，这样我只能把这来那个个函数合并起来了，但是如果真的需要分开，那该怎么写呢？

附上代码供以后参阅：

#include <string>
#include <unistd.h>
#include <csignal>
#include "std_msgs/String.h"
#include "ros/ros.h"  

using std::__cxx11::string;

void exit(int signum)
{
    printf("You choose to stop me.");
    exit(0);
}

string state;
class Idle
{

    private:
    ros::NodeHandle n;
    string goal;
    ros::Subscriber sub_;
    public:

     void callback(const std_msgs::String::ConstPtr& msg)
    {

        if (state=="IDLE")
        {
            goal=msg->data;
        }

    }
    public:
    void init(const ros::NodeHandle  &n1)
    {   

        n=n1;
        sub_=n.subscribe("Idle/goal", 1000, &Idle::callback,this);

    }
    string execute()
    {

        state="IDLE";
        ros::Rate loop_rate(5);

        while (true)
        {

            ros::spinOnce();
            loop_rate.sleep();
            if (goal=="a")
            {
                ROS_INFO("Having a new goal %s",goal.c_str()); 
                goal="";
                return "got_goal";
            }
            std::cout << "goal=" << goal << std::endl;
            ROS_INFO("In state Idle : %s",goal.c_str());

        }

        return "" ;
    }
};


class Planner
{
    private:
    string global_plan;
    ros::NodeHandle n;
    ros::Subscriber sub_;
    public:

     void callback(const std_msgs::String::ConstPtr& msg)
    {
        if (state=="PLANNER")
        {
            global_plan=msg->data;
        }
    }
    void init(const ros::NodeHandle  &n1)
    {
        n=n1;
        sub_=n.subscribe("/Planner/global_plan", 1000, &Planner::callback,this);
    }
    string execute()
    {
        state="PLANNER";
        ros::Rate loop_rate(5);
        while (true)
        {
            ros::spinOnce();
            loop_rate.sleep();
            if (global_plan=="a")
            {
                ROS_INFO("Having a new topological plan");
                global_plan="";
                return "got_plan";
            }
            ROS_INFO("In state Planner: %s",global_plan.c_str());

        }
    }   
};

class Approach_start
{
    private:
    ros::NodeHandle n;
    ros::Subscriber sub_;
    public:
     string succeeded_flag;

     void callback(const std_msgs::String::ConstPtr& msg)
    {
        if (state=="APPROACH_START")
        {
            succeeded_flag=msg->data;
        }
    }
    void init(const ros::NodeHandle  &n1)
    {
        n=n1;
        sub_=n.subscribe("/Approach_to_startnode/succeeded", 1000, &Approach_start::callback,this);
    }
    string execute()
    {
        state="APPROACH_START";
        ros::Rate loop_rate(5);
        while (true)
        {
            ros::spinOnce();
            loop_rate.sleep();
            if (succeeded_flag=="a")
            {
                ROS_INFO("Arrived starting node");
                succeeded_flag="";
                return "succeeded";
            }
                ROS_INFO("In state Approach_to_startnode");

        }
    }   
};

class Approach_end
{
    private:
        ros::NodeHandle n;
        ros::Subscriber sub_;
    public :
     string succeeded_flag;
     void callback(const std_msgs::String::ConstPtr& msg)
    {
        if (state=="APPROACH_END")
        {
            succeeded_flag=msg->data;
        }
    }
    void init(const ros::NodeHandle  &n1)
    {
        n=n1;
        sub_=n.subscribe("/Approach_to_endnode/succeeded", 1000, &Approach_end::callback,this);
    }
    string execute()
    {
        state="APPROACH_END";
        ros::Rate loop_rate(5);
        while (true)
        {

            ros::spinOnce();
            loop_rate.sleep();
            if (succeeded_flag=="a")
            {
                ROS_INFO("Arrived ending node");
                succeeded_flag="";
                return "reach_goal";
            }
            ROS_INFO("In state Approach_to_endnode");

        }
    }   
};

class Repeat
{
    private:
        ros::NodeHandle n;
        ros::Subscriber sub_;
    public :
    string succeeded_flag;
    void callback(const std_msgs::String::ConstPtr& msg)
    {
        if (state=="REPEAT")
        {
            succeeded_flag=msg->data;
        }
    }
    void init(const ros::NodeHandle  &n1)
    {
        n=n1;
        sub_=n.subscribe("/Repeat/succeeded", 1000 , &Repeat::callback,this);
    }
    string execute()
    {
        state="REPEAT";
        ros::Rate loop_rate(5);
        while (true)
        {
            ros::spinOnce();
            loop_rate.sleep();
            if (succeeded_flag=="a")
            {
                ROS_INFO("Repeat the taught content");
                succeeded_flag="";
                return "succeeded";
            }
            ROS_INFO("In state Repeat");

        }
    }   
};

class Obstacle
{
    private:
        ros::NodeHandle n;
        ros::Subscriber sub_;
    public:
    string cleared_flag;
    void callback(const std_msgs::String::ConstPtr& msg)
    {
        if (state=="OBSTACLE_AVOID")
        {
            cleared_flag==msg->data;        
        }
    }
    void init(const ros::NodeHandle  &n1)
    {
        n=n1;
        sub_=n.subscribe("/Obstacle/cleared", 1000, &Obstacle::callback,this);
    }
    string execute()
    {
        state="OBSTACLE_AVOID";
        ros::Rate loop_rate(5);
        while (true)
        {
            ros::spinOnce();
            loop_rate.sleep();
            if (cleared_flag=="a")
            {
                ROS_INFO("Now free to go");
                cleared_flag="";
                return "cleared";
            }
            ROS_INFO("In state Obstacle");

        }
    }
    void run(){
    }
};

class Resume
{
    private:
        ros::NodeHandle n;
        ros::Subscriber sub_;
    public:
     string resumed_flag;
     void callback(const std_msgs::String::ConstPtr& msg)
    {
        if (state=="RESUME")
        {
            resumed_flag=msg->data; 
        }
    }
    void init(const ros::NodeHandle  &n1)
    {
        n=n1;
        sub_=n.subscribe("/Resume/resume_goal", 1000, &Resume::callback,this);
    }
    string execute()
    {
        state="APPROACH_START";
        ros::Rate loop_rate(5);
        while (true)
        {
            ros::spinOnce();
            loop_rate.sleep();
            if (resumed_flag=="a")
            {
                ROS_INFO("Now resume to new goal");
                resumed_flag="";
                return "resume_goal";
            }
            ROS_INFO("In state Resume");

        }
    }   
};



void work(const ros::NodeHandle &n)
{
    static Idle tmp_idle;
    tmp_idle.init(n);

    static Planner tmp_planner;
    tmp_planner.init(n);

    static Approach_start tmp_approach_start;
    tmp_approach_start.init(n);

    static Approach_end tmp_approach_end;
    tmp_approach_end.init(n);

    static Repeat tmp_repeat;
    tmp_repeat.init(n);

    static Obstacle tmp_obstacle;
    tmp_obstacle.init(n);

    static Resume tmp_resume;   
    tmp_resume.init(n);
    while (true)
    {

        string res;
        if (state=="IDLE")
        {
            res=tmp_idle.execute();
            if (res=="got_goal") state="PLANNER";
        }
        else
        if (state=="PLANNER")
        {
            res=tmp_planner.execute();
            if (res=="got_goal") state="PLANNER";
        }
        else
        if (state=="APPROACH_START")
        {
            res=tmp_approach_start.execute();
            if (res=="succeeded") state="REPEAT";
            else
            if (res=="detect_obstacle") state="detect_obstacle";
        }
        else
        if (state=="REPEAT")
        {
            res=tmp_repeat.execute();
            if (res=="succeeded") state="APPROACH_END";
            else
            if (res=="detect_obstacle") state="detect_obstacle";
        }
        else
        if (state=="APPROACH_END")
        {
            res=tmp_approach_end.execute();
            if (res=="reach_goal") state="reach_goal";
            else 
            if (res=="detect_obstacle") state="detect_obstacle";
        }
        else
        if (state=="EXECUTE")
        {
            //res=???.execute();
            if (res=="reach_goal") state="IDLE";
            else
            if (res=="detect_obstacle") state="OBSTACLE_AVOID";
        }
        else
        if (state=="OBSTACLE_AVOID")
        {
            res=tmp_obstacle.execute();
            if (res=="cleared") state="RESUME";
            else 
            if (res=="stuck") state="stuck";
        }
        else
        if (state=="RESUME")
        {
            res=tmp_resume.execute();
            if (res=="resume_goal") state="PLANNER";
        }
    }
}

std::string goal;
void callback_Idle(const std_msgs::String::ConstPtr& msg)
    {
        if (state=="IDLE")
        {
            goal=msg->data;
        }

    }
int main(int argc, char **argv)
{
    signal(SIGINT, exit);
    signal(SIGTERM, exit);  

    ros::init(argc, argv, "test_smach2_node");  
    ros::NodeHandle n;

    state="IDLE";
    work(n);

    return 0;
}
tmkkkkk
关注
  • 2
    点赞
  • 踩
  • 12
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
专栏目录
c++设计模式-行为型模式-状态模式
06-20
c++设计模式-行为型模式-状态模式；qt工程；c++简单源码； 状态（State）模式的定义：对有状态的对象，把复杂的“判断逻辑”提取到不同的状态对象中，允许状态对象在其内部状态发生改变时改变其行为。
C++有限状态机实现计算器小程序
08-30
在C++编程中，有限状态机（Finite State Machine, FSM）是一种设计模式，常用于处理具有固定数量状态和转换规则的系统。在这个特定的案例中，我们使用FSM来实现一个简单的计算器小程序，该计算器支持整数和小数的四...
状态机编程
物联网爱好者
05-12 57
有限状态机（finite state machine，简称状态机）是嵌入式开发中最重要，且最常用的编程模式之一。系统具备数量有限的状态；在某一时刻，系统总是处于一种状态之下；系统的状态会根据某些事件或条件进行转换；每个状态下，系统都会执行相应的动作。// 记录LED状态// 初始化I/O口// 等待按键按下// 当按键按下时，点亮或熄灭LEDledState=!ledState;ledState;delay(500);
C++ 实现一个简单的状态机和行为树结合示例
最新发布
Where there is life, there is hope.
06-22 819
用纯 C++ 实现一个简单的状态机和行为树示例，不依赖外部库。 结合有限状态机和行为树，并引入条件判断和随机性，设计一个简单的门控制系统。
C++实现状态机
sinat_28305511的博客
07-06 1628
状态机的原理，应用以及C++实现
c++写状态机
qq_30519005的博客
01-18 4130
用c++的状态模式写状态机 状态是固定的，事件可扩展 由事件触发，然后根据状态有不同的处理 状态表 空闲 对接中 对接成功 对接失败 补给中 补给成功 补给失败 停止补给中 停止补给成功 停止补给失败 断开对接中 对接事件 断开对接事件 补给事件 停止补给事件 代码 fsm.h #ifndef FSM_H #define FSM_H #include <unordered_map
用C++来实现有限状态机（附代码）
李肖遥的专栏
06-25 2425
关注、星标公众号，直达精彩内容来源：网络素材整理：李肖遥这篇文章主要为大家详细介绍了C++有限状态机的相关资料，文中示例代码介绍的非常详细，具有一定的参考价值，感兴趣的小伙伴们可以参考一下。目录有限状态机四大要素C++函数指针实现总结前提：因为最近打算学设计模式的状态模式，但不是很明白有限状态机和状态模式之间的关系，索性用C++实现了一个简单案例复习了一下FSM，...
C++ 状态机
wanglei_11的博客
06-08 811
C++ 状态机
C语言状态机
08-08
- **有限状态机（FSM）类**：如果使用C++，可以创建一个类来封装状态机的所有组件，但在C语言中，可以使用函数指针模拟类的行为。 6. **状态机的流程**：程序会有一个主循环，不断地检查当前状态，并根据发生的...
C++实现飞机大战
05-24
这通常通过事件驱动模型完成，使用循环检查输入设备的状态，根据不同的按键或鼠标动作更新游戏状态。 7. **对象管理**：游戏中的飞机、子弹、敌人等都是对象，它们有自己的属性（如位置、速度、生命值等）和行为...
fengsuyi.rar_矩阵 状态机
09-19
这个程序可能使用C、C++或Python等编程语言编写，具体的实现细节将依赖于开发环境、嵌入式平台和需求。理解并分析这个程序可以帮助我们学习如何将状态机的概念应用于实际项目中，特别是对于那些需要高效处理输入事件...
SMACC:一个事件驱动的异步行为状态机库，用于用C ++编写的实时ROS（机器人操作系统）应用程序
05-02
特拉维斯CI： ROS发行版 Travis建立状态 文献资料 动能 旋律 理性的 硕士（韵律） Docker容器 SMACC SMACC是一个事件驱动的异步行为状态机库，适用于用C ++编写的实时ROS（机器人操作系统）应用程序，旨在允许程序员以直观，系统的方式构建多组件机器人的机器人控制应用程序。 SMACC受Harel状态图和启发。 SMACC建立在Boost StateChart库的顶部。 特征 由ROS支持： SMACC专为与ROS配合使用而开发。 它开箱即用地支持ROS主题，服务和操作。 用C ++编写：到目前为止，ROS缺少一个库来用C ++开发任务级行为状态机。 尽管已经使用诸如python之类的脚本语言开发了库，但是这些库不适合需要实时需求的实际工业环境。 正交：正交最初由David Harel于1987年提出，正交对于开发用于复杂机器人系统的状态机绝对至关重要
zhuangtaiji.rar_状态机_状态机PPT
09-19
例如，可能会介绍如何使用伪代码或具体编程语言（如C++或Python）来实现一个简单的状态机，比如一个自动化售货机的状态管理。 此外，PPT可能还会涉及以下主题： - 状态机的类型：有限状态机（FSM）、Mealy型状态机...
C++简单的状态机实现
meishujian的博客
11-13 2016
这个状态机是利用c++的模板类的特性来实现的。 下面直接给出源码 /* statemachine.h*/ #ifndef STM #define STM #include <iostream> typedef enum EventActionResult { EventFailed, EventProcessedOK }; template<class T> ...
C++写一个简单的状态机实现
renjianhhong的博客
06-15 1276
在这个示例中，我们添加了一个名为Event的枚举以表示可能的事件。我们还为StateMachine类添加了一个handleEvent方法，该方法接收一个Event参数并根据事件类型调用相应的函数（如start、pause等）。现在，在main函数中，您可以通过调用stateMachine.handleEvent并传递相应的事件来测试状态跳转。在C++中实现一个事件驱动的状态机，您可以在StateMachine类中添加事件枚举和处理事件的方法。
C++有限状态机的实现
weixin_43862733的博客
07-07 2623
//待完善 有限状态机是一个很常用的技术，在流程控制和游戏AI中都比较实用，因为状态机编程简单又很符合直觉。与有限状态机类似的是设计模式中的状态模式。本文是参考《Programming Game AI by Example》 一、 记得最开始工作时候也接触过有限状态机，当时是一个长长的用switch写成的状态机，理解它的时候真的很困难。 所以现在使用一套内置规则到状态内部去，来控制状态的转换。 现在就来制作一个有限状态机。 作为一个关于使用状态机创建一个智能体的实际案例，我们先模拟这样一个场景。是作为一个简
C++(42)-FSM-有限状态机
aggie4628的专栏
02-21 1623
简单说了一下状态机，主要想把数据与运行代码分开。
[C/C++]状态机
小蜗牛之家
09-19 2071
方法： 1.用于接收事件，并根据事件类型到相关的表中分配处理函数 所以只需要把事件类型（通常是消息），传入状态机进行分配即可 2.通常只是读取的一些操作，不涉及更改的，只是读取显示的，都放在预处理中处理（就是和状态无关） 3.状态机编程的一些基础原则： 1.除了到初始化状态和退出状态，其他所有的状态，之前的迁移，必须保证每个状态有且仅有一个消息能够使得状态发生转移所以画状态转移图的时候...
一个有限状态机的C++实现
HULK一线技术杂谈
10-16 1216
女主宣言有限状态机：表示有限个状态以及在这些状态之间的转移和动作等行为的数学模型。它对数字系统的设计具有十分重要的作用。常见的计算机就是使用有限状态机作为计算模型的；电脑...
使用UE4游戏引擎学习C++编程
游戏开发需要了解游戏的基本概念，包括游戏引擎、游戏循环、游戏状态机、游戏物理引擎等。游戏引擎是游戏开发的核心部分，提供了游戏开发所需的基本功能和工具。 **数学基础** 数学是游戏开发的基础，包括向量、...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值