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

这几天写了一些无人车的状态机的程序，其实就是从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";}elseif (state=="PLANNER"){res=tmp_planner.execute();if (res=="got_goal") state="PLANNER";}elseif (state=="APPROACH_START"){res=tmp_approach_start.execute();if (res=="succeeded") state="REPEAT";elseif (res=="detect_obstacle") state="detect_obstacle";}elseif (state=="REPEAT"){res=tmp_repeat.execute();if (res=="succeeded") state="APPROACH_END";elseif (res=="detect_obstacle") state="detect_obstacle";}elseif (state=="APPROACH_END"){res=tmp_approach_end.execute();if (res=="reach_goal") state="reach_goal";else if (res=="detect_obstacle") state="detect_obstacle";}elseif (state=="EXECUTE"){//res=???.execute();if (res=="reach_goal") state="IDLE";elseif (res=="detect_obstacle") state="OBSTACLE_AVOID";}elseif (state=="OBSTACLE_AVOID"){res=tmp_obstacle.execute();if (res=="cleared") state="RESUME";else if (res=="stuck") state="stuck";}elseif (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;
}

状态机的编写（使用C++）相关推荐

Verilog状态机的编写学习
http://bbs.ednchina.com/BLOG_ARTICLE_53109.HTM 时序电路的状态是一个状态变量集合,这些状态变量在任意时刻的值都包含了为确定电路的未来行为而必需考虑的所有历 ...
三段式状态机_Verilog实战篇（5）——FIFO amp; 状态机
今天的暑期培训课依旧是由邸志雄老师主讲:FIFO基本原理及状态机的编写和应用. 下面先说一下FIFO: 一.FIFO 常见参数主要有: 1. FIFO的宽度:即 FIFO 一次读写操作的数 ...
在C#中用RX库和await来实现直观的状态机
在程序的设计过程中,我们经常会遇到一些需要使用状态机的场景,相信状态机的编写和维护是令每一个程序员都非常头大的事情.到了C# 5.0后,由于引进了await语法糖,我们可以通过await和Reacti ...
verilog状态机
Verilog是硬件描述语言,硬件电路是并行执行的,当需要按照流程或者步骤来完成某个功能时,代码中通常会使用很多个if嵌套语句来实现,这样就增加了代码的复杂度,以及降低了代码的可读性,这个时候就可以使 ...
FPGA学习之状态机
1. 理论学习状态机简写为FSM,也称为同步有限状态机,我们简称为状态机.所以说同步时因为状态机中所有的状态跳转都是在时钟的作用下进行的,而有限则是说状态机的个数有限的.状态机分为两大类,即Moor ...
FPGA数字系统设计（8）——可综合电路及状态机
一.可综合电路行为级可综合语法和数据流级语法合在一起被称为RTL级,该级别的模型是可以被综合成电路进而实现的. 1.module 和endmodule 作为模块声明的关键字,必然是可以被综合的. 2 ...
HDLBits答案(14)_Verilog有限状态机(1)
Verilog有限状态机(1) HDLBits链接前言今天来到了重要的部分:状态机.对该部分内容,可能不会一次更新一个小节:一方面是题目难度,另一方面是代码量过大:所以该节会分批更新,大家见谅. ...
【转】异步编程：.NET 4.5 基于任务的异步编程模型(TAP)
最近我为大家陆续介绍了"IAsyncResult异步编程模型 (APM)"和"基于事件的异步编程模式(EAP)"两种异步编程模型.在.NET4.0 中Micro ...
【转】1.8异步编程：.NET 4.5 基于任务的异步编程模型(TAP)
传送门:异步编程系列目录-- 最近我为大家陆续介绍了"IAsyncResult异步编程模型 (APM)"和"基于事件的异步编程模式(EAP)"两种异步编程模型. ...
FPGA数据传输模块设计
摘要 FPGA适合于大量数据处理的应用,广泛应用于嵌入式系统.本文设计的FPGA模块需要对GPS.便携打印机和串口数据进行处理,将详细介绍如何设计FPGA和不同外设之间的数据传输.同时,在RTL编码中 ...

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

状态机的编写（使用C++）相关推荐

最新文章

热门文章