Updating gesture test

Author: chrismbirmingham

harmoni_actuators/harmoni_gesture/launch/gesture_service.launch

@@ -3,8 +3,10 @@
 	<arg name="use_simulator" default="true"/>
 	<arg name="use_simulator_with_robot" default="false"/>
 	<arg name="instance_id" default="qt"/>
+
 	<arg name="robot_joints" default="qt_robot/joints/state_rad"/>
 	<arg name="simulated_joints" default="simulated_joints"/>
+	
 	<group if="$(arg use_gesture)">
 			<rosparam file="$(find harmoni_gesture)/config/configuration.yaml" subst_value="True"/>
 			<param name="/qt/default_param/path" value ="$(find harmoni_gesture)/data"/>

harmoni_actuators/harmoni_gesture/nodes/gesture_service.py

@@ -23,6 +23,7 @@ class GestureService(HarmoniServiceManager):
     def __init__(self, name, param):
         """ Initialization of variables and gesture parameters """
         super().__init__(name)
+        rospy.loginfo("In the gesture service.")
         self.gestures_name = []
         self.gestures_duration = []
         self.gesture_list_received = False
@@ -44,8 +45,10 @@ def __init__(self, name, param):
             self._gesture_done_callback,
             queue_size=1,
         )
+        rospy.loginfo("Publishers and subscribers set up")
         self.state = State.INIT
-        self.setup_gesture()
+        #self.setup_gesture()
+        rospy.loginfo("Service init completed")
         return
 
     def _gesture_done_callback(self, data):
@@ -104,7 +107,7 @@ def do(self, data):
             else:
                 gesture_data = data
                 self.gesture_pub.publish(str(data))
-            print(gesture_data)
+            rospy.loginfo(f"Publishing to gesture pub: {gesture_data}")
             if gesture_data:
                 while not self.gesture_done:
                     self.state = State.REQUEST
@@ -115,6 +118,7 @@ def do(self, data):
             rospy.logwarn("Gesture failed")
             self.state = State.FAILED
             self.actuation_completed = True
+        rospy.loginfo("Returning response to 'do'")
         return {"response": self.state}
 
     def _get_gesture_data(self, data):
@@ -141,7 +145,7 @@ def _get_gesture_data(self, data):
         ordered_gesture_data = list(
             sorted(behavior_set, key=lambda face: face["start"])
         )
-        print(ordered_gesture_data)
+        print("ordered gesture data:", ordered_gesture_data)
         validated_gesture = []
         for gest in ordered_gesture_data:
             validated_gesture.append(gest["id"])

harmoni_actuators/harmoni_gesture/test/gesture.test

@@ -1,20 +1,27 @@
 <launch>
     <arg name="robot_joints" default="qt_robot/joints/state_rad"/>
 	<arg name="simulated_joints" default="simulated_joints"/>
+	<arg name="gesture" default="bye"/>
+
     <rosparam file="$(find harmoni_gesture)/config/configuration.yaml" subst_value="True"/>
     <param name="instance_id" value="qt"/>
     <param name="/qt/default_param/path" value ="$(find harmoni_gesture)/data"/>
+
     <node pkg="harmoni_gesture" type="gesture_service.py" name="harmoni_gesture_qt" output="screen"/>
     <node pkg="harmoni_gesture" type="qt_gesture_interface.py" name="harmoni_gesture_qt_interface" output="screen"/>
+    
     <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
     <node pkg="harmoni_gesture" type="qt_joint_state_publisher.py" name="robot_joint_state_publisher_qt" output="screen"/>
+    
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
         <remap from="joint_states" to="$(arg simulated_joints)"/>
     </node>
+
     <arg name="rvizconfig" default="$(find urdf_tutorial)/rviz/urdf.rviz" />
     <arg name="model" default="$(find harmoni_gesture)/urdf/qt.urdf"/>
     <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
-		<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
-	<param name="test_gesture_input" value="{'gesture':'QT/bye', 'timing': 0.5}"/>
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+
+	<param name="test_gesture_input" value="{'gesture':'QT/$(arg gesture)', 'timing': 5}"/>
     <test test-name="test_gesture" pkg="harmoni_gesture" type="rostest_gesture.py" />
-</launch>
+</launch>

harmoni_core/harmoni_pattern/nodes/sequential_pattern.py

@@ -93,6 +93,7 @@ def _setup_clients(self):
         )
 
         for name, client in self.service_clients.items():
+            rospy.loginfo(f"Waiting on: {name}")
             client.setup_client(name, self._result_callback, self._feedback_callback)
         rospy.loginfo("Behavior interface action clients have been set up!")
         return