Mo prim testing

frames.gv

@@ -0,0 +1,24 @@
+digraph G {
+"panda_link0" -> "external_view"[label="Broadcaster: /external_view_static_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1739325085.418 sec old)\nBuffer length: 0.000 sec\n"];
+"camera_base" -> "internal_camera_base"[label="Broadcaster: /internal_camera_static_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1739325085.418 sec old)\nBuffer length: 0.000 sec\n"];
+"panda_link8" -> "camera_base"[label="Broadcaster: /panda_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1739325085.418 sec old)\nBuffer length: 0.000 sec\n"];
+"internal_rgb_camera_link" -> "rotated_camera"[label="Broadcaster: /rotated_camera_static_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1739325085.418 sec old)\nBuffer length: 0.000 sec\n"];
+"panda_hand" -> "sim_panda_gripper"[label="Broadcaster: /sim_ee_static_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1739325085.418 sec old)\nBuffer length: 0.000 sec\n"];
+"panda_link8" -> "panda_hand"[label="Broadcaster: /panda_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1739325085.418 sec old)\nBuffer length: 0.000 sec\n"];
+"camera_base" -> "camera_body"[label="Broadcaster: /panda_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1739325085.418 sec old)\nBuffer length: 0.000 sec\n"];
+"camera_base" -> "camera_visor"[label="Broadcaster: /panda_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1739325085.418 sec old)\nBuffer length: 0.000 sec\n"];
+"ft_sensor_link" -> "panda_link8"[label="Broadcaster: /panda_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1739325085.418 sec old)\nBuffer length: 0.000 sec\n"];
+"panda_link7" -> "ft_sensor_link"[label="Broadcaster: /panda_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1739325085.418 sec old)\nBuffer length: 0.000 sec\n"];
+edge [style=invis];
+ subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
+"Recorded at time: 1739325085.418"[ shape=plaintext ] ;
+ }->"panda_link0";
+edge [style=invis];
+ subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
+"Recorded at time: 1739325085.418"[ shape=plaintext ] ;
+ }->"internal_rgb_camera_link";
+edge [style=invis];
+ subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
+"Recorded at time: 1739325085.418"[ shape=plaintext ] ;
+ }->"panda_link7";
+}

src/authoring/CMakeLists.txt

@@ -106,6 +106,8 @@ catkin_install_python(PROGRAMS
     nodes/rviz_manager
     test/test_twist.py
     test/test_wipe.py
+    test/test_reset.py
+    test/test_move_angle.py
     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
 install(TARGETS pandaJacob

src/authoring/nodes/planner

@@ -661,6 +661,7 @@ class Planner(object):
             print("done cancelling")
 
     def on_command(self,msg):
+        print("Get Command ")
         self._pause = False
         self._type = msg.type
         starting = 0

src/authoring/test/test_move_angle.py

@@ -0,0 +1,44 @@
+"""
+This currently does NOT work.
+"""
+import copy
+
+import rospy
+from std_msgs.msg import String
+from authoring_msgs.msg import Command, Action 
+from panda_ros_msgs.msg import HybridPose, HybridPoseArray
+
+
+def test_move_angle():
+    pub = rospy.Publisher('/parser/command', Command, queue_size=1, latch=True)
+    rate = rospy.Rate(10) # 10hz
+
+    # For some reason, a single message does not go through so need to
+    # send at least 2.
+    N = 1
+    print("************** N =", N)
+    for i in range(N):
+        # default RESET angle [0., -0.34, 0., -1.66, 0., 1.32, 0.8]
+
+        test_pose = [0., -0.3, 0., -1.66, 0., 1.32, 0.8]
+        test_pose = [0., -0.34, 0., -1.66, 0., 1.32, 0.8]
+
+        action = Action(type=Action.MOVE_ANGLE,  # move to specified joint_pose
+                        joint_pose=test_pose, 
+                        item=String(data="MOVE_ANGLE") # NOT SURE IF THIS IS CORRECT
+                        )
+
+
+
+        cmd = Command()
+        cmd.type = 2  # EXEC
+        cmd.core_action = [action]
+
+
+        # rospy.loginfo(cmd)
+        pub.publish(cmd)
+        rate.sleep()
+
+if __name__ == '__main__':
+    rospy.init_node('move_angle', anonymous=True)
+    test_move_angle()

src/authoring/test/test_reset.py

@@ -0,0 +1,56 @@
+"""
+This currently does NOT work.
+"""
+import copy
+
+import rospy
+from std_msgs.msg import String
+from authoring_msgs.msg import Command, Action 
+from panda_ros_msgs.msg import HybridPose, HybridPoseArray
+
+
+def test_reset():
+    pub = rospy.Publisher('/parser/command', Command, queue_size=1)
+    rate = rospy.Rate(10) # 10hz
+
+    # For some reason, a single message does not go through so need to
+    # send at least 2.
+    for i in range(1):
+
+        p0 = HybridPose()
+        p0.sel_vector = [1,1,1,0,0,0]
+        p0.pose.position.x=0.5 # Forward
+        p0.pose.position.y=0.0  # Sideways
+        p0.pose.position.z=-0.25 # Up
+
+        # This is facing Downward (x=0, y=0, z=0, w=1)
+        p0.pose.orientation.w=1
+
+        # The mover does something where it transforms based on the constraint frame (quaternion)
+        # To have no transform, the following should be set: x=0, y=0, z=0, w=1
+        p0.constraint_frame.w = 1 
+
+        poses = HybridPoseArray()
+
+        poses.poses = [p0 ] # Home position
+
+        action = Action(type=Action.RESET,  # RESET
+                        poses=poses, 
+                        item=String(data="RESET") # NOT SURE IF THIS IS CORRECT
+                        )
+
+
+
+        cmd = Command()
+        cmd.type = 2  # EXEC
+        cmd.core_action = [action]
+
+
+        rospy.loginfo(cmd)
+        pub.publish(cmd)
+        rate.sleep()
+
+if __name__ == '__main__':
+    rospy.init_node('reset', anonymous=True)
+
+    test_reset()