From 00e18932f65d8122bd76d2cf23e53ccfa70feee5 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Fri, 9 Apr 2021 13:06:10 +0000
Subject: [PATCH] initial commit for action server which can spoof actions (for
 testing)

---
 sr_utilities_common/CMakeLists.txt            |  6 ++
 sr_utilities_common/package.xml               |  4 +
 .../test_action_server_hand.cpp               | 85 +++++++++++++++++++
 3 files changed, 95 insertions(+)
 create mode 100644 sr_utilities_common/src/sr_utilities_common/test_action_server_hand.cpp

diff --git a/sr_utilities_common/CMakeLists.txt b/sr_utilities_common/CMakeLists.txt
index 4a317ad7..eed6066c 100644
--- a/sr_utilities_common/CMakeLists.txt
+++ b/sr_utilities_common/CMakeLists.txt
@@ -12,6 +12,8 @@ find_package(catkin REQUIRED COMPONENTS
     roslint    
     std_msgs
     topic_tools
+    actionlib_msgs
+    actionlib
 )
 
 ## System dependencies are found with CMake's conventions
@@ -108,6 +110,7 @@ catkin_package(
   LIBRARIES wait_for_param ros_heartbeat
   CATKIN_DEPENDS
     roscpp
+    actionlib_msgs
 #  DEPENDS system_lib
 )
 
@@ -131,12 +134,14 @@ add_library(wait_for_param src/${PROJECT_NAME}/wait_for_param.cpp)
 ## either from message generation or dynamic reconfigure
 add_dependencies(ros_heartbeat ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(wait_for_param ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+#add_dependencies(test_action_server_hand ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 # The recommended prefix ensures that target names across packages don't collide
 add_executable(ros_heartbeat_server_example scripts/${PROJECT_NAME}/ros_heartbeat_server_example.cpp)
 add_executable(relay src/${PROJECT_NAME}/relay.cpp)
+add_executable(test_action_server_hand src/${PROJECT_NAME}/test_action_server_hand.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -151,6 +156,7 @@ add_executable(relay src/${PROJECT_NAME}/relay.cpp)
 ## Specify libraries to link a library or executable target against
 target_link_libraries(ros_heartbeat ${catkin_LIBRARIES})
 target_link_libraries(wait_for_param ${catkin_LIBRARIES})
+target_link_libraries(test_action_server_hand ${catkin_LIBRARIES})
 
 target_link_libraries(ros_heartbeat_server_example
   ros_heartbeat
diff --git a/sr_utilities_common/package.xml b/sr_utilities_common/package.xml
index 5b3ed892..3388567d 100644
--- a/sr_utilities_common/package.xml
+++ b/sr_utilities_common/package.xml
@@ -23,11 +23,15 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>rostest</build_depend>
   <build_depend>topic_tools</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>actionlib</build_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>python3-boto3</run_depend>
   <run_depend>python-requests</run_depend>
   <run_depend>topic_tools</run_depend>
+  <run_depend>actionlib_msgs</run_depend>
+  <run_depend>actionlib</run_depend>
 
 </package>
diff --git a/sr_utilities_common/src/sr_utilities_common/test_action_server_hand.cpp b/sr_utilities_common/src/sr_utilities_common/test_action_server_hand.cpp
new file mode 100644
index 00000000..af4c10b1
--- /dev/null
+++ b/sr_utilities_common/src/sr_utilities_common/test_action_server_hand.cpp
@@ -0,0 +1,85 @@
+#include <ros/ros.h>
+#include <actionlib/server/simple_action_server.h>
+#include <actionlib_tutorials/FibonacciAction.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <trajectory_msgs/JointTrajectory.h>
+
+class TestHandAction
+{
+protected:
+
+  ros::NodeHandle nh_;
+  actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_; // NodeHandle instance must be created before this line. Otherwise strange error occurs.
+  std::string action_name_;
+  // create messages that are used to published feedback/result
+  control_msgs::FollowJointTrajectoryActionFeedback feedback_;
+  control_msgs::FollowJointTrajectoryActionResult result_;
+
+public:
+
+  TestHandAction(std::string name) :
+    as_(nh_, name, boost::bind(&TestHandAction::executeCB, this, _1), false),
+    action_name_(name)
+  {
+    as_.start();
+  }
+
+  ~TestHandAction(void)
+  {
+  }
+
+  void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal)
+  {
+    // helper variables
+    ros::Rate r(10);
+    bool success = true;
+
+    // push_back the seeds for the fibonacci sequence
+    feedback_.feedback.desired = goal->trajectory.points[0];
+
+    // publish info to the console for the user
+    //ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);
+
+    // start executing the action
+    for(int i=1; i<5; i++)
+    {
+      // check that preempt has not been requested by the client
+      if (as_.isPreemptRequested() || !ros::ok())
+      {
+        ROS_INFO("%s: Preempted", action_name_.c_str());
+        // set the action state to preempted
+        as_.setPreempted();
+        success = false;
+        break;
+      }
+      feedback_.feedback.actual = feedback_.feedback.desired;
+      // publish the feedback
+      as_.publishFeedback(feedback_.feedback);
+      // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
+      r.sleep();
+    }
+
+    if(success)
+    {
+      //result_.sequence = feedback_.sequence;
+      ROS_INFO("%s: Succeeded", action_name_.c_str());
+      // set the action state to succeeded
+      as_.setSucceeded(result_.result);
+    }
+  }
+
+
+};
+
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "TestHandAction");
+
+  TestHandAction test_hand_action_right("/rh_trajectory_controller/follow_joint_trajectory");
+  TestHandAction test_hand_action_left("/lh_trajectory_controller/follow_joint_trajectory");
+  ros::spin();
+
+  return 0;
+}
+