ros简版Action通讯SimpleAction

￡神魔★判官ぃ 2023-02-23 08:51 150阅读 0赞

##                        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();
    }