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