ros简版Action通讯SimpleAction

 

一.python

1.非ui界面版

1.1client.py

#!/usr/bin/env python
# coding:utf-8
import rospy
from actionlib import SimpleActionClient
from demo_actions.msg import CountNumberAction, CountNumberGoal,CountNumberResult
from actionlib import GoalStatus

# 结果回调
def done_cb(state,result):
    if state==GoalStatus.SUCCEEDED:
        rospy.loginfo('succeed {}'.format(result.count))
    elif state==GoalStatus.ABORTED:
        rospy.loginfo('aborted {}'.format(result.count))
    elif state==GoalStatus.PREEMPTED:
        rospy.loginfo('cancel {}'.format(result.count))


# 激活
def active_cb():
    rospy.loginfo('active')

# 进度
def feedback_cb(feedback):
    rospy.loginfo('feedback {}'.format(feedback.percent))


if __name__ == '__main__':
    # 创建节点
    rospy.init_node("action_client")
    # action的名字
    actionName = 'action_py'
    # 创建client端
    client = SimpleActionClient(actionName, CountNumberAction)
    # 等待
    client.wait_for_server()
    # 目标
    goal = CountNumberGoal()
    goal.max = 10
    goal.duration = 1
    # 发送目标
    client.send_goal(goal,done_cb,active_cb,feedback_cb)
    # 睡眠两秒钟取消
    rate = rospy.Rate(0.5)
    rate.sleep()
    client.cancel_goal()
    # 阻塞
    rospy.spin()

1.2 server.py

#!/usr/bin/env python
# coding:utf-8
import rospy
from actionlib import SimpleActionServer
from demo_actions.msg import CountNumberAction,CountNumberFeedback,CountNumberResult

def execute_cb(goal):
    rospy.loginfo('receive goal')
    max = goal.max
    duration = goal.duration
    # 结果
    result = CountNumberResult()

    rate = rospy.Rate(duration)
    for i in range(1,max):
        # 发布进度
        feedback = CountNumberFeedback()
        feedback.percent = float(i)/max
        server.publish_feedback(feedback)
        # 取消判断
        if server.is_preempt_requested():
            result.count = i
            server.set_aborted(result)
            return

        # 睡眠
        rate.sleep()

    # 成功
    result.count = max
    server.set_succeeded(result)

if __name__ == '__main__':
    # 创建节点
    rospy.init_node("action_server")
    # action的名字
    actionName = 'action_py'
    # 创建server
    server = SimpleActionServer(actionName,CountNumberAction,execute_cb,auto_start=False)
    # 启动server
    server.start()
    # 阻塞
    rospy.spin()

 

2.ui版

2.1 MainWIndowClient.py

#!/usr/bin/env python
# coding:utf-8
import rospy
from PyQt5.QtWidgets import *
from actionlib import SimpleActionClient
from demo_actions.msg import CountNumberAction, CountNumberGoal,CountNumberResult
from actionlib import GoalStatus

class MainWindow(QWidget):
    def __init__(self):
        super(MainWindow, self).__init__()
        print 'helo'
        self.setWindowTitle("hello")
        layout = QFormLayout()
        self.setLayout(layout)
        maxEdit = QLineEdit()
        durationEdit = QLineEdit()
        activeL = QLabel()
        feedbackL = QLabel()
        doneL = QLabel()
        sendBtn = QPushButton('发送目标')
        cancelBtn = QPushButton('取消目标')
        layout.addRow('max目标:',maxEdit)
        layout.addRow('duration目标:',durationEdit)
        layout.addRow('激活状态:',activeL)
        layout.addRow('进度:',feedbackL)
        layout.addRow('完成状态:',doneL)
        layout.addRow('',sendBtn)
        layout.addRow('',cancelBtn)

        # action的名字
        actionName = 'action_py'
        # 创建client端
        self.client = SimpleActionClient(actionName, CountNumberAction)

        sendBtn.clicked.connect(self.send)
        cancelBtn.clicked.connect(self.cancel)

    def cancel(self):
        self.client.cancel_goal()


    def send(self):
        # 目标
        goal = CountNumberGoal()
        goal.max = 10
        goal.duration = 1
        # 发送目标
        self.client.send_goal(goal,self.done_cb,self.active_cb,self.feedback_cb)

    # 结果回调
    def done_cb(self,state,result):
        if state==GoalStatus.SUCCEEDED:
            rospy.loginfo('succeed {}'.format(result.count))
        elif state==GoalStatus.ABORTED:
            rospy.loginfo('aborted {}'.format(result.count))
        elif state==GoalStatus.PREEMPTED:
            rospy.loginfo('cancel {}'.format(result.count))


    # 激活
    def active_cb(self):
        rospy.loginfo('active')

    # 进度
    def feedback_cb(self,feedback):
        rospy.loginfo('feedback {}'.format(feedback.percent))

2.2

MainWindowServer.py

#!/usr/bin/env python
# coding:utf-8
import rospy
from actionlib import SimpleActionServer
from demo_actions.msg import CountNumberAction,CountNumberFeedback,CountNumberResult
from PyQt5.QtWidgets import *
from PyQt5.QtCore import QTimer


class MainWindow(QWidget):
    def __init__(self):
        super(MainWindow, self).__init__()
        self.timer = QTimer(self)
        self.timer.setInterval(100)
        self.timer.timeout.connect(self.timeout)
        self.timer.start()

        self.setFixedSize(200,100)
        layout = QFormLayout()
        self.setLayout(layout)
        maxL = QLabel()
        durationL = QLabel()
        btn = QPushButton("中止")
        layout.addRow("max:",maxL)
        layout.addRow("duration:",durationL)
        layout.addRow("",btn)
        self.isAbort = False

        # action的名字
        actionName = 'action_py'
        # 创建server
        self.server = SimpleActionServer(actionName,CountNumberAction,self.execute_cb,auto_start=False)
        # 启动server
        self.server.start()
        btn.clicked.connect(self.abort)

    def timeout(self):
        if rospy.is_shutdown():
            QApplication.quit()

    def abort(self):
        self.isAbort = True

    def execute_cb(self,goal):
        rospy.loginfo('receive goal')
        self.isAbort = False
        max = goal.max
        duration = goal.duration
        # 结果
        result = CountNumberResult()

        rate = rospy.Rate(duration)
        for i in range(1,max):
            # 发布进度
            feedback = CountNumberFeedback()
            feedback.percent = float(i)/max
            self.server.publish_feedback(feedback)
            # 取消判断
            if self.isAbort:
                result.count = i
                self.server.set_aborted(result)
                return

            # 取消判断
            if self.server.is_preempt_requested():
                result.count = i
                self.server.set_preempted(result)
                return

            # 睡眠
            rate.sleep()

        # 成功
        result.count = max
        self.server.set_succeeded(result)

2.3

client_gui.py

#!/usr/bin/env python
# coding:utf-8

from MainWIndowClient import *
import sys

if __name__ == '__main__':
    # 创建节点
    rospy.init_node("action_client")

    app = QApplication(sys.argv)
    w = MainWindow()
    w.show()
    sys.exit(app.exec_())

2.4

server_gui.py

#!/usr/bin/env python
# coding:utf-8
import rospy
from MainWindowServer import *

import sys


if __name__ == '__main__':
    # 创建节点
    rospy.init_node("action_server")

    app = QApplication(sys.argv)
    w = MainWindow()
    w.show()
    exec_ = app.exec_()

    sys.exit(exec_)

 

二.c++

1.非ui版

1.1 simple_client.cpp

//
// Created by wt on 2020/7/3.
//
#include <iostream>
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <demo_actions/CountNumberAction.h>

using namespace std;
//结果回调 (包含了很多状态  成功  取消  错误)
void done_cb(const actionlib::SimpleClientGoalState &state,
        const demo_actions::CountNumberResult::ConstPtr &result) {
    if (state==actionlib::SimpleClientGoalState::SUCCEEDED){//服务端setSucced
        ROS_INFO_STREAM("succeed"<<result->count);
    }else if(state==actionlib::SimpleClientGoalState::ABORTED){//setAborted
        ROS_INFO_STREAM("aborted"<<result->count);
    }else if(state==actionlib::SimpleClientGoalState::PREEMPTED){//setAborted
        ROS_INFO_STREAM("preempted"<<result->count);
    }
}
//激活回调
void active_cb(){
    ROS_INFO_STREAM("active");
}

//进度回调
void feedback_cb(const demo_actions::CountNumberFeedback::ConstPtr & feedback){
    ROS_INFO_STREAM("feedback"<<feedback->percent);
}

int main(int argc, char *argv[]) {
    //节点名
    string nodeName = "action_client";
    //初始化节点
    ros::init(argc, argv, nodeName,ros::init_options::AnonymousName);
    //创建节点
    ros::NodeHandle node;
    //通信名
    string actionName = "action_cpp";
    //创建客户端
    actionlib::SimpleActionClient<demo_actions::CountNumberAction> client(node, actionName);
    //等待
    client.waitForServer();
    //目标
    demo_actions::CountNumberGoal goal;
    goal.max = 10;
    goal.duration = 1;
    //发送目标(发布topic)
    client.sendGoal(goal,done_cb,active_cb,feedback_cb);
    //等2秒钟 取消任务
    ros::Rate rate(0.5);
    rate.sleep();
    //取消任务
//    client.cancelGoal();
    //事件轮询
    ros::spin();
    return 0;
}

1.2

simple_server.cpp

//
// Created by wt on 2020/7/3.
//
#include <iostream>
#include <ros/ros.h>
//action通信server端
#include <actionlib/server/simple_action_server.h>
#include <demo_actions/CountNumberAction.h>

using namespace std;

//参数：发送的目标指针
void execute_callback(const demo_actions::CountNumberGoal::ConstPtr &goal,
                      actionlib::SimpleActionServer<demo_actions::CountNumberAction> *server) {
    ROS_INFO_STREAM("receive goal");
    //获取目标中的数据
    long max = goal->max;
    double duration = goal->duration;
    //回调结果
    demo_actions::CountNumberResult result;
    //rate
    ros::Rate rate(duration);
    for (int i = 1; i < max; ++i) {
//        if(i==2){
//            //认为服务器出错
//            result.count = -1;
//            server->setAborted(result);
//            return;
//        }

        /*-------------------------- 取消 --------------------------*/
        //是否请求被取消
        if(server->isPreemptRequested()){
            //必须要知道客户端已经取消了任务，才能通过这个方法返回结果
            result.count = i;
            server->setPreempted(result);
            return;
        }

        //进度
        demo_actions::CountNumberFeedback feedback;
        feedback.percent = (double)i/(double)max;
        //发布进度
        server->publishFeedback(feedback);
        //睡眠
        rate.sleep();
    }
    //成功
    result.count = max;
    server->setSucceeded(result);

}

int main(int argc, char *argv[]) {
    //节点名
    string nodeName = "action_server";
    //初始化节点
    ros::init(argc, argv, nodeName);
    //创建节点
    ros::NodeHandle node;
    //通信名
    string actionName = "action_cpp";
    //创建服务端
    actionlib::SimpleActionServer<demo_actions::CountNumberAction> server(actionName, boost::bind(execute_callback,_1,&server), false);
    //启动服务
    server.start();

    //事件轮询
    ros::spin();
    return 0;
}

2.ui界面版

2.1

MainWindowClient.h

//
// Created by wt on 2020/7/5.
//

#ifndef DEMO_USE_ACTION_MAINWINDOWCLIENT_H
#define DEMO_USE_ACTION_MAINWINDOWCLIENT_H
#include <QWidget>
#include <QFormLayout>
#include <QLineEdit>
#include <QLabel>
#include <QPushButton>
#include <actionlib/client/simple_action_client.h>
#include <demo_actions/CountNumberAction.h>
#include <actionlib/server/simple_action_server.h>
#include <QTimer>
class MainWindowClient: public QWidget {
private:
    QFormLayout layout;
    QLineEdit maxEdit;
    QLineEdit durationEdit;
    QLabel activeL;
    QLabel feedbackL;
    QLabel doneL;
    QPushButton sendBtn;
    QPushButton cancelBtn;
    actionlib::SimpleActionClient<demo_actions::CountNumberAction> *client;
    QTimer timer;

public:
    MainWindowClient(ros::NodeHandle node,QWidget* parent = Q_NULLPTR);

    ~MainWindowClient();

    //发送目标
    void send();
    //取消
    void cancel();
    //完成回调
    void done_cb(const actionlib::SimpleClientGoalState &state,
                 const demo_actions::CountNumberResult::ConstPtr &result);
    //激活
    void active_cb();
    //进度
    void feedback_cb(const demo_actions::CountNumberFeedback::ConstPtr & feedback);
    void timeout();
};


#endif //DEMO_USE_ACTION_MAINWINDOWCLIENT_H

2.2

MainWindowClient.cpp

//
// Created by wt on 2020/7/5.
//

#include "MainWindowClient.h"

MainWindowClient::MainWindowClient(ros::NodeHandle node,QWidget* parent):QWidget(parent),sendBtn("发送目标"),
cancelBtn("取消目标"){
    setLayout(&layout);
    layout.addRow("max目标：",&maxEdit);
    layout.addRow("duration目标：",&durationEdit);
    layout.addRow("激活状态：",&activeL);
    layout.addRow("进度：",&feedbackL);
    layout.addRow("完成状态：",&doneL);
    layout.addRow("",&sendBtn);
    layout.addRow("",&cancelBtn);

    client = new actionlib::SimpleActionClient<demo_actions::CountNumberAction>(node,"action_cpp");
    client->waitForServer();//这里不需要用这个,会阻塞窗口, 用client->is*** 来判断是否连上了
    //定时器
    timer.setInterval(10);
    timer.start();
    //定时信号
    connect(&timer,&QTimer::timeout,this,&WindoClient::timeout);

    //信号和槽
    connect(&sendBtn,&QPushButton::clicked,this,&MainWindowClient::send);
    connect(&cancelBtn,&QPushButton::clicked,this,&MainWindowClient::cancel);
}

MainWindowClient::~MainWindowClient() {
ros::shutdown();

}

void MainWindowClient::send() {
    demo_actions::CountNumberGoal goal;
    goal.max = maxEdit.text().toLong();
    goal.duration = durationEdit.text().toDouble();
    client->sendGoal(goal,boost::bind(&MainWindowClient::done_cb,this,_1,_2),boost::bind(&MainWindowClient::active_cb,this),
            boost::bind(&MainWindowClient::feedback_cb,this,_1));
}

void MainWindowClient::done_cb(const actionlib::SimpleClientGoalState &state,
             const demo_actions::CountNumberResult::ConstPtr &result){
    if (state==actionlib::SimpleClientGoalState::SUCCEEDED){//服务端setSucced
        ROS_INFO_STREAM("succeed"<<result->count);
        doneL.setText("成功");
    }else if(state==actionlib::SimpleClientGoalState::ABORTED){//setAborted
        ROS_INFO_STREAM("aborted"<<result->count);
        doneL.setText("服务出错");
    }else if(state==actionlib::SimpleClientGoalState::PREEMPTED){//setAborted
        ROS_INFO_STREAM("preempted"<<result->count);
        doneL.setText("取消");
    }
}
void MainWindowClient::active_cb() {
    ROS_INFO_STREAM("active");
    activeL.setText("激活");
}

void MainWindowClient::feedback_cb(const demo_actions::CountNumberFeedback::ConstPtr & feedback){
    ROS_INFO_STREAM("feedback  "<<feedback->percent);
    feedbackL.setText(QString::number(feedback->percent));
}

void MainWindowClient::cancel() {
    //取消目标
    client->cancelGoal();
}

//定时的槽函数
void WindoClient::timeout(){
    //处理消息
    if(ros::isShuttingDown()){
        QApplication::quit();
    }


}

2.3

MainWindowServer.h

//
// Created by wt on 2020/7/5.
//
#ifndef DEMO_USE_ACTION_MAINWINDOWSERVER_H
#define DEMO_USE_ACTION_MAINWINDOWSERVER_H
#include <QWidget>
#include <QFormLayout>
#include <QPushButton>
#include <QLabel>
#include <actionlib/server/simple_action_server.h>
#include <demo_actions/CountNumberAction.h>

class MainWindowServer: public QWidget {
private:
    QFormLayout layout;
    QLabel maxL;
    QLabel dL;
    QPushButton btn;
    actionlib::SimpleActionServer<demo_actions::CountNumberAction> *server;
    //记录是否服务出错
    bool isAborted = false;
public:
    MainWindowServer(QWidget* parent = Q_NULLPTR);

    ~MainWindowServer();
    //回调
    void execute_callback(const demo_actions::CountNumberGoal::ConstPtr &goal);
    //服务出错
    void aborted();
};

#endif //DEMO_USE_ACTION_MAINWINDOWSERVER_H

2.4

MainWindowServer.cpp

//
// Created by wt on 2020/7/5.
//

#include "MainWindowServer.h"

MainWindowServer::MainWindowServer(QWidget* parent):QWidget(parent),btn("aborted") {
    setFixedSize(200,100);
    setLayout(&layout);
    layout.addRow("max:",&maxL);
    layout.addRow("duration:",&dL);
    layout.addRow("",&btn);
    //创建server端
    server = new actionlib::SimpleActionServer<demo_actions::CountNumberAction>("action_cpp",boost::bind(&MainWindowServer::execute_callback,this,_1), false);
    server->start();

    //服务端出错
    connect(&btn,&QPushButton::clicked,this,&MainWindowServer::aborted);
}

MainWindowServer::~MainWindowServer() {
    delete server;
}

void MainWindowServer::execute_callback(const demo_actions::CountNumberGoal::ConstPtr &goal) {
    ROS_INFO_STREAM("receive goal");
    //获取目标中的数据
    long max = goal->max;
    double duration = goal->duration;
    //设置控件
    maxL.setText(QString::number(max));
    dL.setText(QString::number(duration));

    //回调结果
    demo_actions::CountNumberResult result;
    //rate
    ros::Rate rate(duration);
    for (int i = 1; i < max; ++i) {
        if(this->isAborted){
            //认为服务器出错
            result.count = -1;
            server->setAborted(result);
            return;
        }

        /*-------------------------- 取消 --------------------------*/
        //是否请求被取消
        if(server->isPreemptRequested()){
            //必须要知道客户端已经取消了任务，才能通过这个方法返回结果
            result.count = i;
            server->setPreempted(result);
            return;
        }

        //进度
        demo_actions::CountNumberFeedback feedback;
        feedback.percent = (double)i/(double)max;
        //发布进度
        server->publishFeedback(feedback);
        //睡眠
        rate.sleep();
    }
    //成功
    result.count = max;
    server->setSucceeded(result);
}

void MainWindowServer::aborted() {
    this->isAborted = true;
}

2.5

client_ui.cpp

//
// Created by wt on 2020/7/5.
//
#include <iostream>
#include <ros/ros.h>
#include <QApplication>
#include "MainWindowClient.h"
using namespace std;

int main(int argc,char *argv[]){
    //节点名
    string nodeName = "action_client";
    //初始化节点
    ros::init(argc,argv,nodeName);
    //创建节点
    ros::NodeHandle node;
    /*-------------------------- qt --------------------------*/
    QApplication app(argc,argv);
    MainWindowClient w(node);
    w.show();
    return QApplication::exec();
}

2.6

//
// Created by wt on 2020/7/5.
//
#include <iostream>
#include <ros/ros.h>
#include <QApplication>
#include "MainWindowServer.h"
using namespace std;

int main(int argc,char *argv[]){
    //节点名
    string nodeName = "action_server";
    //初始化节点
    ros::init(argc,argv,nodeName);
    //创建节点
    ros::NodeHandle node;
    /*-------------------------- asy --------------------------*/
    ros::AsyncSpinner spinner(1);
    spinner.start();
    /*-------------------------- qt --------------------------*/
    QApplication app(argc,argv);
    MainWindowServer w;
    w.show();
    return QApplication::exec();
}