add some primitives tests

.vscode/c_cpp_properties.json

@@ -0,0 +1,23 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/opt/ros/noetic/include/**",
+        "/home/kindred/catkin_ws/src/atr_pkg/include/**",
+        "/home/kindred/catkin_ws/src/ssr_pkg/include/**",
+        "/home/kindred/catkin_ws/src/wpr_simulation/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

.vscode/settings.json

@@ -0,0 +1,8 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
+}

src/authoring/test/test_moveunknown.py

@@ -0,0 +1,69 @@
+#!/usr/bin/env python3
+
+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_moveunknown():
+    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(2):
+
+        hybrid_pose1 = HybridPose()
+        hybrid_pose1.sel_vector = [1,1,1,0,0,0]
+        hybrid_pose1.pose.position.x=0.52 # Forward
+        hybrid_pose1.pose.position.y=0.0  # Sideways
+        hybrid_pose1.pose.position.z=-0.24 # Up
+
+        # This is facing Downward (x=0, y=0, z=0, w=1)
+        hybrid_pose1.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
+        hybrid_pose1.constraint_frame.w = 1 
+
+        hybrid_pose2 = HybridPose()
+        hybrid_pose2.sel_vector = [1,1,1,0,0,0]
+        hybrid_pose2.pose.position.x=0.52 # Forward
+        hybrid_pose2.pose.position.y=0.0  # Sideways
+        hybrid_pose2.pose.position.z=-0.30 # Up
+
+        # This is facing Downward (x=0, y=0, z=0, w=1)
+        hybrid_pose2.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
+        hybrid_pose2.constraint_frame.w = 1 
+
+        poses = HybridPoseArray()
+        poses.poses = [hybrid_pose1, hybrid_pose2] 
+
+        action = Action(type=21,  # MOVE_UNKNOWN
+                        poses=poses, 
+                        item=String(data="BOLT") # 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('test_twist', anonymous=True)
+
+    test_moveunknown()
+

src/authoring/test/test_pick.py

@@ -0,0 +1,57 @@
+#!/usr/bin/env python3
+
+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_pick():
+    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(2):
+
+        hybrid_pose = HybridPose()
+        hybrid_pose.sel_vector = [1,1,1,0,0,0]
+        hybrid_pose.pose.position.x=0.52 # Forward
+        hybrid_pose.pose.position.y=0.0  # Sideways
+        hybrid_pose.pose.position.z=-0.24 # Up
+
+        # This is facing Downward (x=0, y=0, z=0, w=1)
+        hybrid_pose.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
+        hybrid_pose.constraint_frame.w = 1 
+
+
+        poses = HybridPoseArray()
+        poses.poses = [hybrid_pose] 
+
+        action = Action(type=0,  # PICK
+                        poses=poses, 
+                        item=String(data="BOLT") # 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('test_twist', anonymous=True)
+
+    test_pick()
+

src/authoring/test/test_place.py

@@ -0,0 +1,57 @@
+#!/usr/bin/env python3
+
+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_place():
+    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(2):
+
+        hybrid_pose = HybridPose()
+        hybrid_pose.sel_vector = [1,1,1,0,0,0]
+        hybrid_pose.pose.position.x=0.52 # Forward
+        hybrid_pose.pose.position.y=0.2  # Sideways
+        hybrid_pose.pose.position.z=-0.38 # Up
+
+        # This is facing Downward (x=0, y=0, z=0, w=1)
+        hybrid_pose.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
+        hybrid_pose.constraint_frame.w = 1 
+
+
+        poses = HybridPoseArray()
+        poses.poses = [hybrid_pose] 
+
+        action = Action(type=2,  # PLACE
+                        poses=poses, 
+                        item=String(data="BOLT") # 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('test_twist', anonymous=True)
+
+    test_place()
+

src/authoring/test/test_stop.py

@@ -0,0 +1,104 @@
+#!/usr/bin/env python3
+
+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_stop():
+    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(2):
+
+        hybrid_pose1 = HybridPose()
+        hybrid_pose1.sel_vector = [1,1,1,0,0,0]
+        hybrid_pose1.pose.position.x=0.52 # Forward
+        hybrid_pose1.pose.position.y=0.0  # Sideways
+        hybrid_pose1.pose.position.z=-0.24 # Up
+
+        # This is facing Downward (x=0, y=0, z=0, w=1)
+        hybrid_pose1.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
+        hybrid_pose1.constraint_frame.w = 1 
+
+
+        poses = HybridPoseArray()
+        poses.poses = [hybrid_pose1] 
+
+        action1 = Action(type=0,  # PICK
+                        poses=poses, 
+                        item=String(data="BOLT") # NOT SURE IF THIS IS CORRECT
+                        )
+
+        hybrid_pose2 = HybridPose()
+        hybrid_pose2.sel_vector = [1,1,1,0,0,0]
+        hybrid_pose2.pose.position.x=0.52 # Forward
+        hybrid_pose2.pose.position.y=0.2  # Sideways
+        hybrid_pose2.pose.position.z=-0.38 # Up
+
+        # This is facing Downward (x=0, y=0, z=0, w=1)
+        hybrid_pose2.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
+        hybrid_pose2.constraint_frame.w = 1 
+
+
+        poses = HybridPoseArray()
+        poses.poses = [hybrid_pose2] 
+
+
+        action2 = Action(type=2,  # PLACE
+                        poses=poses, 
+                        item=String(data="BOLT") # NOT SURE IF THIS IS CORRECT
+                        )
+
+
+
+        hybrid_pose3 = HybridPose()
+        hybrid_pose3.sel_vector = [1,1,1,0,0,0]
+        hybrid_pose3.pose.position.x=0.0 # Forward
+        hybrid_pose3.pose.position.y=0.0  # Sideways
+        hybrid_pose3.pose.position.z=0.0 # Up
+
+        # This is facing Downward (x=0, y=0, z=0, w=1)
+        hybrid_pose3.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
+        hybrid_pose3.constraint_frame.w = 1 
+
+
+        poses = HybridPoseArray()
+        poses.poses = [hybrid_pose3] 
+
+        action3 = Action(type=14,  # STOP
+                        poses=poses, 
+                        item=String(data="BOLT") # NOT SURE IF THIS IS CORRECT
+                        )
+
+
+
+        cmd = Command()
+        cmd.type = 2  # EXEC
+        cmd.core_action = [action1, action2, action3]
+
+
+        rospy.loginfo(cmd)
+        pub.publish(cmd)
+        rate.sleep()
+
+
+if __name__ == '__main__':
+    rospy.init_node('test_twist', anonymous=True)
+
+    test_stop()
+