Add ArucoHeadScan action messages

stretch_demos/CMakeLists.txt

@@ -8,6 +8,7 @@ project(stretch_demos)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
+  genmsg
   actionlib
   actionlib_msgs
   geometry_msgs
@@ -69,17 +70,16 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
+add_action_files(
+  FILES
+  VisualServo.action
+)
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   actionlib_msgs#   geometry_msgs#   nav_msgs#   std_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  actionlib_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##

stretch_demos/action/VisualServo.action

@@ -0,0 +1,19 @@
+# goal
+# target frame defines the transform frame that needs to be compared with source_frame
+string target_frame
+
+# target frame defines the transform frame that needs to be compared with target_frame
+string source_frame
+
+---
+# result
+# result is the success flag
+bool result
+
+---
+# feedback
+# angle error is the angular error between source and target frames
+float64 angle_error
+
+# distance error is the euclidean error between source and target frames
+float64 distance_error

stretch_demos/package.xml

@@ -81,6 +81,8 @@
   <exec_depend>tf</exec_depend>
   <exec_depend>tf2</exec_depend>
   <exec_depend>ros_numpy</exec_depend>
+  <build_depend>py_trees_ros</build_depend>
+  <build_depend>rqt_py_trees</build_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->