From 34ee817550e24af84e137ef2248d65a5d6abde7e Mon Sep 17 00:00:00 2001
From: jieshoudaxue <1641395022@qq.com>
Date: Thu, 26 Oct 2023 21:21:57 +0800
Subject: [PATCH] [up] action sample

---
 action_sample/launch/start.launch     | 19 ++++++++++++++++
 action_sample/src/DoDishes_client.cpp | 32 +++++++++++++++++++++++++++
 action_sample/src/DoDishes_server.cpp | 29 ++++++++++++++++++++++++
 3 files changed, 80 insertions(+)
 create mode 100644 action_sample/launch/start.launch

diff --git a/action_sample/launch/start.launch b/action_sample/launch/start.launch
new file mode 100644
index 0000000..d52d21d
--- /dev/null
+++ b/action_sample/launch/start.launch
@@ -0,0 +1,19 @@
+<launch>
+
+<node
+    pkg="action_sample"
+    type="DoDishes_client"
+    name="do_dishes_client"
+    required="true"
+    output="screen"
+/>
+
+<node
+    pkg="action_sample"
+    type="DoDishes_server"
+    name="do_dishes_server"
+    respawn="true"
+    output="screen"
+/>
+
+</launch>
\ No newline at end of file
diff --git a/action_sample/src/DoDishes_client.cpp b/action_sample/src/DoDishes_client.cpp
index 9c493eb..173df76 100644
--- a/action_sample/src/DoDishes_client.cpp
+++ b/action_sample/src/DoDishes_client.cpp
@@ -1,6 +1,38 @@
 #include <actionlib/client/simple_action_client.h>
 #include "action_sample/DoDishesAction.h"
 
+void doneCb(const actionlib::SimpleClientGoalState& state,
+            const action_sample::DoDishesResultConstPtr& result) {
+  ROS_INFO("%d dishes is clean, state is %s", result->total_dishes_cleaned, state.toString().c_str());
+  ros::shutdown();
+}
+
+void activeCb() {
+  ROS_INFO("job is active");
+}
+
+void feedbackCb(const action_sample::DoDishesFeedbackConstPtr& feedback) {
+  ROS_INFO("percent is %f", feedback->percent_complete);
+}
+
+
 int main(int argc, char** argv) {
+  ros::init(argc, argv, "do_dishes_client");
+
+  actionlib::SimpleActionClient<action_sample::DoDishesAction> client("do_dishes", true);
+
+  ROS_INFO("waiting for action server to start.");
+  client.waitForServer();
+  ROS_INFO("action server started, sending goal.");
+
+  action_sample::DoDishesGoal goal;
+  goal.dishwasher_id = 1;
+
+  client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);
+
+  ros::spin();
+
+
+
   return 0;
 }
\ No newline at end of file
diff --git a/action_sample/src/DoDishes_server.cpp b/action_sample/src/DoDishes_server.cpp
index 34068e1..054238a 100644
--- a/action_sample/src/DoDishes_server.cpp
+++ b/action_sample/src/DoDishes_server.cpp
@@ -2,6 +2,35 @@
 #include <actionlib/server/simple_action_server.h>
 #include "action_sample/DoDishesAction.h"
 
+void execute(const action_sample::DoDishesGoalConstPtr& goal, actionlib::SimpleActionServer<action_sample::DoDishesAction>* server) {
+  ros::Rate rate(1);
+
+  action_sample::DoDishesFeedback feedback;
+
+  ROS_INFO("dishwasher %d is working", goal->dishwasher_id);
+
+  for (int i = 0; i < 10; i++) {
+    feedback.percent_complete = i * 10;
+    server->publishFeedback(feedback);
+    rate.sleep();
+  }
+
+  action_sample::DoDishesResult result;
+  result.total_dishes_cleaned = 188;
+
+  ROS_INFO("dishwasher %d finish working", goal->dishwasher_id);
+  server->setSucceeded(result);
+}
+
 int main(int argc, char** argv) {
+  ros::init(argc, argv, "do_dishes_server");
+  ros::NodeHandle nh;
+
+  actionlib::SimpleActionServer<action_sample::DoDishesAction> server(nh, "do_dishes", std::bind(&execute, std::placeholders::_1, &server), false);
+
+  server.start();
+
+  ros::spin();
+
   return 0;
 }
\ No newline at end of file