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 new file mode 100644 index 0000000..46754bd --- /dev/null +++ b/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() \ No newline at end of file diff --git a/src/authoring/test/test_reset.py b/src/authoring/test/test_reset.py new file mode 100644 index 0000000..5ff46cf --- /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(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() \ No newline at end of file