From 2781c3b17c5e36bc4a71225338e5869c72d95e20 Mon Sep 17 00:00:00 2001 From: Mo Xu Date: Wed, 5 Feb 2025 01:48:51 -0600 Subject: [PATCH 1/4] test reset but don't know HOME --- src/authoring/test/test_reset.py | 56 ++++++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 src/authoring/test/test_reset.py diff --git a/src/authoring/test/test_reset.py b/src/authoring/test/test_reset.py new file mode 100644 index 0000000..4b3a1fe --- /dev/null +++ b/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(2): + + 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="WHITEBOARD_ERASER") # 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() \ No newline at end of file From 9295f07a9daea45172aeab0f7c347ae24170c2e2 Mon Sep 17 00:00:00 2001 From: Mo Xu Date: Wed, 5 Feb 2025 14:01:44 -0600 Subject: [PATCH 2/4] reset --- src/authoring/test/test_reset.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/authoring/test/test_reset.py b/src/authoring/test/test_reset.py index 4b3a1fe..4282b13 100644 --- a/src/authoring/test/test_reset.py +++ b/src/authoring/test/test_reset.py @@ -36,7 +36,7 @@ def test_reset(): action = Action(type=Action.RESET, # RESET poses=poses, - item=String(data="WHITEBOARD_ERASER") # NOT SURE IF THIS IS CORRECT + item=String(data="RESET") # NOT SURE IF THIS IS CORRECT ) From 7ca29140619396a2c56b96126318afcfa1d19d4f Mon Sep 17 00:00:00 2001 From: Patron Date: Tue, 11 Feb 2025 19:23:42 -0600 Subject: [PATCH 3/4] test move_angle --- src/authoring/test/test_move_angle.py | 42 +++++++++++++++++++++++++++ src/authoring/test/test_reset.py | 2 +- 2 files changed, 43 insertions(+), 1 deletion(-) create mode 100644 src/authoring/test/test_move_angle.py diff --git a/src/authoring/test/test_move_angle.py b/src/authoring/test/test_move_angle.py new file mode 100644 index 0000000..14a9669 --- /dev/null +++ b/src/authoring/test/test_move_angle.py @@ -0,0 +1,42 @@ +""" +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) + 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): + # default RESET angle [0., -0.34, 0., -1.66, 0., 1.32, 0.8] + + test_pose = [0., -0.34, 0.25, -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('reset', anonymous=True) + + test_reset() \ No newline at end of file diff --git a/src/authoring/test/test_reset.py b/src/authoring/test/test_reset.py index 4282b13..5ff46cf 100644 --- a/src/authoring/test/test_reset.py +++ b/src/authoring/test/test_reset.py @@ -15,7 +15,7 @@ def test_reset(): # For some reason, a single message does not go through so need to # send at least 2. - for i in range(2): + for i in range(1): p0 = HybridPose() p0.sel_vector = [1,1,1,0,0,0] From 1dd68805ed8f2947dbc5401bae68a626472622cd Mon Sep 17 00:00:00 2001 From: Mya Schroder Date: Tue, 11 Feb 2025 20:42:43 -0600 Subject: [PATCH 4/4] add latch option --- frames.gv | 24 ++++++++++++++++++++++++ src/authoring/CMakeLists.txt | 2 ++ src/authoring/nodes/planner | 1 + src/authoring/test/test_move_angle.py | 16 +++++++++------- 4 files changed, 36 insertions(+), 7 deletions(-) create mode 100644 frames.gv diff --git a/frames.gv b/frames.gv new file mode 100644 index 0000000..345def8 --- /dev/null +++ b/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"; +} \ No newline at end of file diff --git a/src/authoring/CMakeLists.txt b/src/authoring/CMakeLists.txt index d383de5..3d375c5 100644 --- a/src/authoring/CMakeLists.txt +++ b/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 diff --git a/src/authoring/nodes/planner b/src/authoring/nodes/planner index 43b1e53..b4a5f33 100755 --- a/src/authoring/nodes/planner +++ b/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 diff --git a/src/authoring/test/test_move_angle.py b/src/authoring/test/test_move_angle.py index 14a9669..46754bd 100644 --- a/src/authoring/test/test_move_angle.py +++ b/src/authoring/test/test_move_angle.py @@ -10,15 +10,18 @@ def test_move_angle(): - pub = rospy.Publisher('/parser/command', Command, queue_size=1) + 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. - for i in range(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.34, 0.25, -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, @@ -32,11 +35,10 @@ def test_move_angle(): cmd.core_action = [action] - rospy.loginfo(cmd) + # rospy.loginfo(cmd) pub.publish(cmd) rate.sleep() if __name__ == '__main__': - rospy.init_node('reset', anonymous=True) - - test_reset() \ No newline at end of file + rospy.init_node('move_angle', anonymous=True) + test_move_angle() \ No newline at end of file