diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..1fc297a --- /dev/null +++ b/.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 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..d1fc194 --- /dev/null +++ b/.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" + ] +} \ No newline at end of file diff --git a/src/authoring/test/test.py b/src/authoring/test/test.py new file mode 100755 index 0000000..6a83d11 --- /dev/null +++ b/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() + \ No newline at end of file diff --git a/src/authoring/test/test2.py b/src/authoring/test/test2.py new file mode 100755 index 0000000..40890c1 --- /dev/null +++ b/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() + \ No newline at end of file diff --git a/src/authoring/test/test_move_force.py b/src/authoring/test/test_move_force.py new file mode 100755 index 0000000..f7f3fc0 --- /dev/null +++ b/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() diff --git a/src/authoring/test/test_moveunknown.py b/src/authoring/test/test_moveunknown.py new file mode 100755 index 0000000..4f75763 --- /dev/null +++ b/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() + \ No newline at end of file diff --git a/src/authoring/test/test_pick.py b/src/authoring/test/test_pick.py new file mode 100755 index 0000000..3f025c3 --- /dev/null +++ b/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() + \ No newline at end of file diff --git a/src/authoring/test/test_place.py b/src/authoring/test/test_place.py new file mode 100755 index 0000000..cc50599 --- /dev/null +++ b/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() + \ No newline at end of file diff --git a/src/authoring/test/test_stop.py b/src/authoring/test/test_stop.py new file mode 100755 index 0000000..1f22521 --- /dev/null +++ b/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() + \ No newline at end of file