基于smach的状态机设计【1】

smach是ros中的一种状态机的功能包，这里面我们将来深入认识一下这个状态机的运行和各种动作的设计过程。
安装samch的相关内容：

sudo apt install ros-noetic-executive-smach*

首先我们来看一下官方的例子
去git所有的源码来：

git clone https://github.com/lizhiwei0304/ROS_SMACH_TUTORIALS.git

或者可以一步步自己创建，首先创建功能包和相关的依赖：

catkin_create_pkg smach_usecase rospy roscpp std_msgs std_srvs smach smach_ros turtlesim

创建启动turtlesim的节点turtle_nodes.launch：

<launch>
	<node pkg="turtlesim" name="turtlesim" type="turtlesim" />

</launch>

对于一个基本的smach，认识一下结构：

#/usr/bin/env python
import rospy
import smach
from smach import StateMachine

def main():
	# 初始化节点
	rospy.init_node("smach_example")
	# 定义smach的最终结果
	sm_root = StateMachine(outcomes=[])
	with sm_root:
			# 按照顺序来添加状态
			pass
	# smach的执行程序，按照顺序执行所有内容
	outcome = sm_root.execute()
	rospy.spin()
if __main__ == "__main__":
	main()

要有完整的状态机，我们需要向这里面添加相关的内容来进行实现，由于smach中一般是通过class来定义状态机的，所以后面的状态都使用类来进行定义。
首先第一个服务是reset，由turtlesim节点直接提供的，需要的可以去看turtlesim的源码，看它的服务是怎么定义的。
turtlesim源码：https://github.com/ros/ros_tutorials.git
以及动作状态（turtle_actionlib）内容：
https://github.com/ros/common_tutorials.git
第二个服务也是turtlesim直接定义的，spawn来生成小乌龟。
这里之前没有怎么接触过服务和动作，所以也会对相对代码进行解析。
首先服务是分服务端和客户端的，服务端提供了这个服务以及这个服务的具体作用，客户端则会去请求（call）这个服务来实现某种功能，向topic一样，该发送发送，该接收的接收
启动turtlesim_node后的所有服务内容：

rosservice list

可以去ros index官网看一下这些服务的作用：

前面的是话题的内容,后面是服务和参数的内容,这也刚好对应了ros的三大通信模式.(动作不在里面是因为动作和上面的通信过程很多是重复的)
clear服务的服务类型是std_srvs.Empty
empty就完全是empty的，不需要传递服务的信息就能请求，当然也就不需要反馈。
第二个服务的服务类型是turtlesim.srv.Spawn
turtlesim/Spawn.srv如下：

float32 x
float32 y
float32 theta
string name # Optional.  A unique name will be created and returned if this is empty
---
string name

注意这只是总的服务的定义，客户端请求它时也就是需要一个请求的服务类型，包含的是前四个内容，最后一个则是反馈的内容,也就是执行的情况，实际生成的turtle的名字。
kill服务则是清除指定的turtle，服务类型是turtlesim/Kill，turtlesim/Kill.srv如下:

string name
---

其余服务类型都是同样就行分析。
那么现在疑问就来了，怎么去定义的这些服务，然后又怎么才能去请求这些服务呢？
不急，我们来看看turtlesim的源码来解答。

turtle_frame.h:

/*
 * @Author: CYUN && cyun@tju.edu.cn
 * @Date: 2024-08-28 13:08:01
 * @LastEditors: CYUN && cyun@tju.enu.cn
 * @LastEditTime: 2024-08-28 19:43:19
 * @FilePath: /src/ros_tutorials/turtlesim/include/turtlesim/turtle_frame.h
 * @Description: 
 * 
 * Copyright (c) 2024 by Tianjin University, All Rights Reserved. 
 */
#include <QFrame> // Qt框架头文件，用于创建窗口部件
#include <QImage> // 用于处理图像
#include <QPainter> // 用于绘制图像
#include <QPaintEvent> // 处理绘图事件
#include <QTimer> // 定时器，用于定期更新图像
#include <QVector> // Qt的向量容器类

#ifndef Q_MOC_RUN // 宏定义，防止MOC错误，通常与Qt和Boost库相关
#include <ros/ros.h>
#include <std_srvs/Empty.h> // ROS的标准空服务
#include <turtlesim/Spawn.h> // ROS的turtlesim生成服务
#include <turtlesim/Kill.h> // ROS的turtlesim删除服务
#include <map> // C++标准库中的map容器，用于存储键值对

#include <turtlesim/turtle.h> // Turtlesim的Turtle类
#endif

namespace turtlesim
{
class TurtleFrame : public QFrame
{
    Q_OBJECT // 使得该类能够使用Qt的信号和槽机制
public:
    TurtleFrame(QWidget* parent = 0, Qt::WindowFlags f = 0); // 构造函数
    ~TurtleFrame(); // 析构函数

    // 生成海龟的方法
    std::string spawnTurtle(const std::string& name, float x, float y, float angle);
    std::string spawnTurtle(const std::string& name, float x, float y, float angle, size_t index);

protected:
    void paintEvent(QPaintEvent* event); // 重载的函数，用于在窗口上绘制图像

private slots:
    void onUpdate(); // 定时器触发的更新槽

private:
    void updateTurtles(); // 更新所有海龟的状态
    void clear(); // 清除内容
    bool hasTurtle(const std::string& name); // 检查是否存在给定名字的海龟

    // ROS的服务回调函数
    bool clearCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
    bool resetCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
    bool spawnCallback(turtlesim::Spawn::Request&, turtlesim::Spawn::Response&);
    bool killCallback(turtlesim::Kill::Request&, turtlesim::Kill::Response&);

    // 新增的服务回调函数：
    bool spawnMeCallback(turtlesim::Spawn::Request&, turtlesim::Spawn::Response&);
    bool killMeCallback(turtlesim::Kill::Request&, turtlesim::Kill::Response&);

    ros::NodeHandle nh_; // ROS节点句柄
    ros::NodeHandle private_nh_; // 私有节点句柄
    QTimer* update_timer_; // 定时器指针
    QImage path_image_; // 存储绘制海龟轨迹的图像
    QPainter path_painter_; // 用于在path_image_上绘制路径
    uint64_t frame_count_; // 记录绘制帧数量

    ros::WallTime last_turtle_update_; // 记录上次更新海龟状态的时间

    // ROS服务服务器
    ros::ServiceServer clear_srv_; // 清除服务
    ros::ServiceServer reset_srv_; // 重置服务
    ros::ServiceServer spawn_srv_; // 生成服务
    ros::ServiceServer kill_srv_; // 删除服务

    // 新增服务：
    ros::ServiceServer spawn_me_srv; // 自定义生成服务
    ros::ServiceServer kill_me_srv; // 自定义删除服务

    typedef std::map<std::string, TurtlePtr> M_Turtle; // 用于存储所有海龟的map
    M_Turtle turtles_; // 存储所有的海龟实例
    uint32_t id_counter_; // 生成的海龟计数器

    QVector<QImage> turtle_images_; // 存储海龟图像
    float meter_; // 每米的像素
    float width_in_meters_; // 窗口宽度，以米为单位
    float height_in_meters_; // 窗口高度，以米为单位
};
}

turtle_frame.cpp:

/*
 * @Author: CYUN && cyun@tju.enu.cn
 * @Date: 2024-08-28 13:08:01
 * @LastEditors: CYUN && cyun@tju.enu.cn
 * @LastEditTime: 2024-08-28 19:46:46
 * @FilePath: /src/ros_tutorials/turtlesim/src/turtle_frame.cpp
 * @Description: 
 * 
 * Copyright (c) 2024 by Tianjin University, All Rights Reserved. 
 */
// ctrl+win+i键使用快捷注释

#include <turtlesim/turtle_frame.h>
#include <QPointF> // QPointF用于表示浮点数的2D点
#include <ros/package.h> // ros包工具
#include <cstdlib> // c++标准库，用于调用srand和rand
#include <ctime> // 时间相关函数

#define DEFAULT_BG_R 0x45
#define DEFAULT_BG_G 0x56
#define DEFAULT_BG_B 0xff

namespace turtlesim
{
// 构造函数
TurtleFrame::TurtleFrame(QWidget* parent, Qt::WindowFlags f)
: QFrame(parent, f) // 调用基类QFrame的构造函数
, path_image_(500, 500, QImage::Format_ARGB32) // 初始化轨迹图像大小和格式
, path_painter_(&path_image_) // 将path_painter_指向path_image_
, frame_count_(0) // 初始化帧计数
, id_counter_(0) // 初始化海龟计数器
, private_nh_("~") // 初始化私有节点句柄
{
    setFixedSize(500, 500); // 设置窗口大小
    setWindowTitle("TurtleSim"); // 设置标题

    srand(time(NULL)); // 用当前时间初始化随机数生成器
    update_timer_ = new QTimer(this); // 创建定时器
    update_timer_->setInterval(16); // 定时器间隔，16ms
    update_timer_->start(); // 启动定时器

    connect(update_timer_, SIGNAL(timeout()), this, SLOT(onUpdate())); // 将定时器超时信号连接到 onUpdate 槽

    // 设置背景颜色：
    if (!private_nh_.hasParam("background_r"))
    {
        private_nh_.setParam("background_r", DEFAULT_BG_R);
    }
    if (!private_nh_.hasParam("background_g"))
    {
        private_nh_.setParam("background_g", DEFAULT_BG_G);
    }
    if (!private_nh_.hasParam("background_b"))
    {
        private_nh_.setParam("background_b", DEFAULT_BG_B);
    }

    QVector<QString> turtles;
    // 初始化海龟图像文件
    turtles.append("box-turtle.png");
    turtles.append("robot-turtle.png");
    turtles.append("sea-turtle.png");
    turtles.append("diamondback.png");
    turtles.append("electric.png");
    turtles.append("fuerte.png");
    turtles.append("groovy.png");
    turtles.append("hydro.svg");
    turtles.append("indigo.svg");
    turtles.append("jade.png");
    turtles.append("kinetic.png");
    turtles.append("lunar.png");
    turtles.append("melodic.png");
    turtles.append("noetic.png");

    // 获取ros包路径，加载图像文件：
    QString images_path = (ros::package::getPath("turtlesim") + "/images/").c_str();
    for (int i = 0; i < turtles.size(); ++i)
    {
        QImage img;
        img.load(images_path + turtles[i]);
        turtle_images_.append(img); // 将图像添加到列表中
    }
    meter_ = turtle_images_[0].height(); // 每米的像素
    clear(); // 初始化图像

    clear_srv_ = nh_.advertiseService("clear", &TurtleFrame::clearCallback, this);
    reset_srv_ = nh_.advertiseService("reset", &TurtleFrame::resetCallback, this);
    spawn_srv_ = nh_.advertiseService("spawn", &TurtleFrame::spawnCallback, this);
    kill_srv_ = nh_.advertiseService("kill", &TurtleFrame::killCallback, this);

    // 新增服务：
    kill_me_srv = nh_.advertiseService("kill_me", &TurtleFrame::killMeCallback, this);
    spawn_me_srv = nh_.advertiseService("spawn_me", &TurtleFrame::spawnMeCallback, this);

    ROS_INFO("Starting turtlesim with node name %s", ros::this_node::getName().c_str()); // 输出节点名称
    width_in_meters_ = (width() - 1) / meter_;
    height_in_meters_ = (height() - 1) / meter_;
    // 生成第一只海龟
    spawnTurtle("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0);
    // 可选，生成所有可用类型的海龟（当前禁用）
    if (false)
    {
        for (int index = 0; index < turtles.size(); ++index)
        {
            QString name = turtles[index];
            name = name.split(".").first();
            name.replace(QString("-"), QString(""));
            spawnTurtle(name.toStdString(), 1.0 + 1.5 * (index % 7), 1.0 + 1.5 * (index / 7), PI / 2.0, index);
        }
    }
}
// 析构函数
TurtleFrame::~TurtleFrame()
{
    delete update_timer_;
}

// turtle的服务回调函数
bool TurtleFrame::spawnCallback(turtlesim::Spawn::Request& req, turtlesim::Spawn::Response& res)
{
    std::string name = spawnTurtle(req.name, req.x, req.y, req.theta);// 生成turtle
    if (name.empty())
    {
        ROS_ERROR("A turtle named [%s] already exists", req.name.c_str());
        return false;
    }

    res.name = name;// 返回海龟的名字
    return true;
}

// 回调函数的操作内容都相同：
bool TurtleFrame::spawnMeCallback(turtlesim::Spawn::Request& req, turtlesim::Spawn::Response& res)
{
    std::string name = spawnTurtle(req.name, req.x, req.y, req.theta);// 生成turtle
    if (name.empty())
    {
        ROS_ERROR("A turtle named [%s] already exists", req.name.c_str());
        return false;
    }

    res.name = name;// 返回海龟的名字
    return true;
}
bool TurtleFrame::killMeCallback(turtlesim::Kill::Request& req, turtlesim::Kill::Response&)
{
    M_Turtle::iterator it = turtles_.find(req.name);
    if (it == turtles_.end())
    {
        ROS_ERROR("Tried to kill turtle [%s], which does not exist", req.name.c_str());
        return false;
    }

    turtles_.erase(it); // 删除海龟
    update(); //更新显示
    return true;
}

bool TurtleFrame::killCallback(turtlesim::Kill::Request& req, turtlesim::Kill::Response&)
{
    M_Turtle::iterator it = turtles_.find(req.name);
    if (it == turtles_.end())
    {
        ROS_ERROR("Tried to kill turtle [%s], which does not exist", req.name.c_str());
        return false;
    }

    turtles_.erase(it); // 删除海龟
    update(); //更新显示
    return true;
}

// 检查是否存在给定名字的海龟
bool TurtleFrame::hasTurtle(const std::string& name)
{
    return turtles_.find(name) != turtles_.end();
}

// 生成海龟
std::string TurtleFrame::spawnTurtle(const std::string& name, float x, float y, float angle)
{
  return spawnTurtle(name, x, y, angle, rand() % turtle_images_.size());
}
// 生成海龟函数带索引
std::string TurtleFrame::spawnTurtle(const std::string& name, float x, float y, float angle, size_t index)
{
    std::string real_name = name;
    if (real_name.empty())
    {
        do
        {
            std::stringstream ss;
            ss << "turtle" << ++id_counter_;
            real_name = ss.str();
        } while (hasTurtle(real_name));
        
    }
    else
    {
        if (hasTurtle(real_name))
        {
            return "";
        }
    }
    // 创建海龟并加入到map中
    TurtlePtr t(new Turtle(ros::NodeHandle(real_name), turtle_images_[index], QPointF(x, height_in_meters_ - y), angle));
    turtles_[real_name] = t;
    update();

    ROS_INFO("Spawning turtle [%s] at x=[%f], y=[%f], theta=[%f]", real_name.c_str(), x, y, angle);
    return real_name;
}

// 重载paintEvent函数用于绘制窗口内容
void TurtleFrame::paintEvent(QPaintEvent*)
{
    QPainter painter(this);
    painter.drawImage(QPoint(0, 0), path_image_); // 绘制轨迹图像
    M_Turtle::iterator it = turtles_.begin();
    M_Turtle::iterator end = turtles_.end();
    for (; it != end; ++it)
    {
        it->second->paint(painter);
    }
}

// 清除屏幕路径
void TurtleFrame::clear()
{
    int r = DEFAULT_BG_R;
    int g = DEFAULT_BG_G;
    int b = DEFAULT_BG_B;

    private_nh_.getParam("background_r", r);
    private_nh_.getParam("background_g", g);
    private_nh_.getParam("background_b", b);

    path_image_.fill(qRgb(r, g, b));
    update();
}

void TurtleFrame::onUpdate()
{
    ros::spinOnce();
    updateTurtles();

    if (!ros::ok())
    {
        close();
    }
}

// 更新所有海龟的状态
void TurtleFrame::updateTurtles()
{
    if (last_turtle_update_.isZero())
    {
        last_turtle_update_ = ros::WallTime::now();
        return;
    }
    bool modified = false;
    M_Turtle::iterator it = turtles_.begin();
    M_Turtle::iterator end = turtles_.end();
    for (; it != end; ++it)
    {
        // 更新每只海龟位置和状态
        modified |= it->second->update(0.001 * update_timer_->interval(), path_painter_, path_image_, width_in_meters_, height_in_meters_);
    }
    if (modified)
    {
        update();
    }
    ++frame_count_; // 增加帧计数
}

// 清除海龟轨迹的服务回调：
bool TurtleFrame::resetCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
    ROS_INFO("Resetting turtlesim.");
    turtles_.clear();
    id_counter_ = 0;
    spawnTurtle("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0); // 生成新海龟
    clear();
    return true;
}

bool TurtleFrame::clearCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
  ROS_INFO("Clearing turtlesim.");
  clear();
  return true;
}

}

这两个就是关于turtle的服务端的内容了，如果和我一样没有系统的学习过c++的话，看起来还是比较费劲的，那么最好的方式就是按照自己的代码习惯来重写一遍并且附上相关的注释了，以上就是重写完后的。

注意由于这些服务以及随着turtlesim_node的启动自动启动了，那么我们可以重新去添加自己的服务并且实现相关的功能。比如我们添加一个/spawn_me的服务，然后自定的srv类型与之前相同.仿照他的内容添加了两个服务：
spawn_me和kill_me

用rqt去call它依然可以实现相关功能：


底下那个（0，0，0）就是我们call出来的，服务也就创建成功了。当然，这里面也包含自定义服务类型，和自定义msg类似，这里就不在赘述了。
这就是服务端的书写，至于客户端怎么去请求它，这里先不深入了，状态机定义过程中会有这个请求，到时候再研究，继续看我们动作状态怎么去请求到的。
action又比service更进一步，将整个动作行为进行了扩展，我们还是以turtle的shape动作为例来学习。
源码位于turtle_actionlib中，
定义的action主要是Shape.action:

#goal definition
int32 edges
float32 radius
---
#result definition
float32 interior_angle
float32 apothem
---
#feedback

包含三个部分，goal，definition和feedback
这个动作是让小乌龟来绘制形状的，告诉边数，告诉半径就能给出相关的结果，然后没有需要反馈的内容。
动作是用来长时间的一个使用的一种服务，但是很特别的是他的实现和请求都是使用的topic的形式，并不是自成一体的通信方式。很明显，这其实也是一种特别的服务，所以动作不属于ros的三大通讯模式。

ros::this_node::getName()是用来获取当前节点名字的，我们把客户端写到一个cpp里面：

#include <ros/ros.h>
#include <turtlesim/Pose.h>
#include <actionlib/server/simple_action_server.h>
#include <cmath>
#include <math.h>
#include <angles/angles.h>

#include <geometry_msgs/Twist.h>
#include <turtle_actionlib/ShapeAction.h> // 这是定义的action编译后的头文件

// This class computes the command_velocities of the turtle to draw regular polygons 
class ShapeAction
{
public:
  ShapeAction(std::string name) : 
    as_(nh_, name, false),
    action_name_(name)
  {
    //注册的动作内容
    as_.registerGoalCallback(boost::bind(&ShapeAction::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&ShapeAction::preemptCB, this));

    //订阅和发布内容，由于是action，所以一定是输出了内容的
    sub_ = nh_.subscribe("/turtle1/pose", 1, &ShapeAction::controlCB, this);
    pub_ = nh_.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1);

    as_.start();
  }

  ~ShapeAction(void)
  {
  }

	// 分别执行了两个动作
  void goalCB()
  {
    // accept the new goal
    turtle_actionlib::ShapeGoal goal = *as_.acceptNewGoal();
    //save the goal as private variables
    edges_ = goal.edges;
    radius_ = goal.radius;

    // reset helper variables
    interior_angle_ = ((edges_-2)*M_PI)/edges_;
    apothem_ = radius_*cos(M_PI/edges_);
    //compute the side length of the polygon
    side_len_ = apothem_ * 2* tan( M_PI/edges_);
    //store the result values
    result_.apothem = apothem_;
    result_.interior_angle = interior_angle_;
    edge_progress_ =0;
    start_edge_ = true;
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // set the action state to preempted
    as_.setPreempted();
  }

  void controlCB(const turtlesim::Pose::ConstPtr& msg)
  {
    // make sure that the action hasn't been canceled
    if (!as_.isActive())
      return;
  
    if (edge_progress_ < edges_)
    {
      // scalar values for drive the turtle faster and straighter
      double l_scale = 6.0;
      double a_scale = 6.0;
      double error_tol = 0.00001;

      if (start_edge_)
      {
        start_x_ = msg->x;
        start_y_ = msg->y;
        start_theta_ = msg->theta;
        start_edge_ = false;
      }

      // compute the distance and theta error for the shape
      dis_error_ = side_len_ - fabs(sqrt((start_x_- msg->x)*(start_x_-msg->x) + (start_y_-msg->y)*(start_y_-msg->y)));
      theta_error_ = angles::normalize_angle_positive(M_PI - interior_angle_ - (msg->theta - start_theta_));
     
      if (dis_error_ > error_tol)
      {
        command_.linear.x = l_scale*dis_error_;
        command_.angular.z = 0;
      }
      else if (dis_error_ < error_tol && fabs(theta_error_)> error_tol)
      { 
        command_.linear.x = 0;
        command_.angular.z = a_scale*theta_error_;
      }
      else if (dis_error_ < error_tol && fabs(theta_error_)< error_tol)
      {
        command_.linear.x = 0;
        command_.angular.z = 0;
        start_edge_ = true;
        edge_progress_++;
      }  
      else
      {
        command_.linear.x = l_scale*dis_error_;
        command_.angular.z = a_scale*theta_error_;
      } 
      // publish the velocity command
      pub_.publish(command_);
      
    } 
    else
    {          
      ROS_INFO("%s: Succeeded", action_name_.c_str());
      // set the action state to succeeded
      as_.setSucceeded(result_);
    }   
  }

protected:
  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<turtle_actionlib::ShapeAction> as_;
  std::string action_name_;
  double radius_, apothem_, interior_angle_, side_len_;
  double start_x_, start_y_, start_theta_;
  double dis_error_, theta_error_;
  int edges_ , edge_progress_;
  bool start_edge_;
  geometry_msgs::Twist command_;
  turtle_actionlib::ShapeResult result_;
  ros::Subscriber sub_;
  ros::Publisher pub_;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "turtle_shape");

  ShapeAction shape(ros::this_node::getName());
  ros::spin();

  return 0;
}

我们创建的这个动作是以topic来表现出来的：

这里就是turtle_shape的那两个话题。
然后怎么请求这个服务呢，也是通过topic来实现的：

rostopic pub /turtle_shape/goal turtle_actionlib/ShapeActionGoal '{goal: {edges: 4, radius: 1.5}}'

发布一下这些信息就能启动这个action了:

到这里我们对状态机内部的内容有了一个整体的了解了，接下来就是在状态机中去结合这些动作状态和服务状态完成一系列复杂的任务了。