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.py

@@ -0,0 +1,76 @@
+#!/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():
+    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.2  # Sideways
+        hybrid_pose1.pose.position.z=-0.38 # 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.2 # Forward
+        hybrid_pose2.pose.position.y=0.4  # Sideways
+        hybrid_pose2.pose.position.z=-0.5 # 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 
+
+        poses1 = HybridPoseArray()
+        poses1.poses = [hybrid_pose1] 
+
+        poses2 = HybridPoseArray()
+        poses2.poses = [hybrid_pose2]
+
+        action = Action(type=16,  # PLACE
+                        poses=poses1, 
+                        item=String(data="BOLT") # NOT SURE IF THIS IS CORRECT
+                        )
+
+        action2 = Action(type=6,  # PLACE
+                        poses=poses2, 
+                        item=String(data="BOLT") # NOT SURE IF THIS IS CORRECT
+                        )
+
+
+        cmd = Command()
+        cmd.type = 2  # EXEC
+        cmd.core_action = [action, action2]
+
+
+        rospy.loginfo(cmd)
+        pub.publish(cmd)
+        rate.sleep()
+
+
+if __name__ == '__main__':
+    rospy.init_node('test_twist', anonymous=True)
+
+    test()
+

src/authoring/test/test2.py

@@ -0,0 +1,57 @@
+#!/usr/bin/env python3
+
+import copy
+
+import rospy
+from std_msgs.msg import String, Float64
+from authoring_msgs.msg import Command, Action 
+from panda_ros_msgs.msg import HybridPose, HybridPoseArray
+
+
+def test2():
+    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()
+        #oses.poses = [hybrid_pose] 
+        joint_space = [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
+        action = Action(type=8,  # PICK
+                        joint_pose = joint_space,
+                        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)
+
+    test2()
+

src/authoring/test/test_move_force.py

@@ -0,0 +1,59 @@
+#!/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_move_force():
+    pub = rospy.Publisher('/parser/command', Command, queue_size=1)
+    rate = rospy.Rate(10)  # 10Hz
+
+
+    # 为了确保消息能顺利传递，至少发送两条消息
+    for i in range(2):
+        # 构造目标位姿
+        hybrid_pose = HybridPose()
+        hybrid_pose.sel_vector = [1, 1, 1, 0, 0, 0]
+        hybrid_pose.pose.position.x = 0.6
+        hybrid_pose.pose.position.y = 0.0
+        hybrid_pose.pose.position.z = 0.2
+        # 设置目标朝向（无旋转）
+        hybrid_pose.header.stamp = rospy.Time.now() + rospy.Duration(5.0)
+        hybrid_pose.pose.orientation.w = 1
+
+
+
+        # 填充 constraint_frame（通常设置为单位四元数表示无旋转）
+        hybrid_pose.constraint_frame.w = 1
+
+        # 将目标位姿放入 HybridPoseArray
+        poses = HybridPoseArray()
+        poses.poses = [hybrid_pose]
+
+        # 构造一个 Action 消息
+        action = Action(
+            type=16,  # MOVE_FORCE，对应定义中的数值（例如 16）
+            poses=poses,
+            item=String(data="BOLT")  # 可选，标识操作对象
+        )
+        # 设置 sim 标志：True 表示仿真模式（这样服务器会走仿真相关的处理逻辑）
+        #action.sim.data = True
+
+        # 构造 Command 消息，类型 2 通常表示 EXEC 执行命令
+        cmd = Command()
+        cmd.type = 2  # EXEC 模式
+        cmd.core_action = [action]
+
+        rospy.loginfo("Publishing MOVE_FORCE command:\n%s", cmd)
+        pub.publish(cmd)
+        rate.sleep()
+
+
+if __name__ == '__main__':
+    rospy.init_node('test_move_force_node', anonymous=True)
+    rospy.sleep(3.0)
+    test_move_force()

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()
+