def cartclass_to_cartarg(self, cartclass, cartarg):
cartarg = go_to.cartesian_pose_arg(position=[
cartclass.position_x, cartclass.position_y, cartclass.position_z
],
orientation=[
cartclass.orientation_x,
cartclass.orientation_y,
cartclass.orientation_z,
cartclass.orientation_w
])
return cartarg
def __init__(self):
self.old_time = time.time()
self.learner_quotient = 0
self.interaction_counter = 0
self.total_interactions = 0
self.time_elapsed = 0
self.learner_interaction = False
self.old_time = time.time()
self.new_time = time.time()
self.waypoints = []
self.waypoints_back = []
self.waypoint_limit = 99
self.effort = []
self.new_angle = 0
self.old_angle = 0
self.changed_angle = 0
self.eventnumber = 0
self.iframe_sequence_pub = rospy.Publisher('/iframe_sequence',
Int32,
queue_size=10)
self.unique_input_pub = rospy.Publisher('/unique_input',
String,
queue_size=10)
self.zeroG_pub = rospy.Publisher('/zeroG_topic', String, queue_size=10)
self.gripper_pub = rospy.Publisher('/gripper_control',
Int32,
queue_size=10)
self.lights_pub = rospy.Publisher('/cuff_light', String, queue_size=10)
self.interaction_pub = rospy.Publisher('/interaction_topic',
Int32,
queue_size=10)
rospy.Subscriber('/upper_cuff_button', Int32,
self.upper_cuff_button_callback)
rospy.Subscriber('/lower_cuff_button', Int32,
self.lower_cuff_button_callback)
rospy.Subscriber('/bot_sequence', Int32, self.callback)
rospy.Subscriber('/save_info', String, self.save_info_callback)
rospy.Subscriber('/right_navigator_button', String,
self.navigator_callback)
rospy.init_node('Sawyer_Sparrow_comm_node', anonymous=True)
self.limb = intera_interface.Limb('right')
self._gripper = get_current_gripper_interface()
self._is_clicksmart = isinstance(self._gripper,
SimpleClickSmartGripper)
self.startPose = self.limb.endpoint_pose()
self.startPose_container = cartPose()
self.startPose_arg = go_to.cartesian_pose_arg()
self.startJointAngles = self.limb.joint_angles()
self.newJointAngles_arg = go_to.joint_angle_arg()
self.newCartPose_arg = go_to.cartesian_pose_arg()
self.newCartPose_arg2 = go_to.cartesian_pose_arg()
self.newCartPose = cartPose()
self.newJointAngles = jointAngles()
self.waypoint_init_container = cartPose()
self.waypoint_init_arg = go_to.cartesian_pose_arg()
#Check if gripper is attached properly
self.grip = gripper_cuff.GripperConnect()
self.savedLocations = []
#Open gripper
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
#set constrained mode
zeroG.constrained(zeroG_all_constraints)
# self.startPose = self.limb.endpoint_pose()
# print(self.limb.endpoint_pose())
# self.startPose_container = self.pose_to_cartclass(pose = self.startPose, cartclass = self.startPose_container)
# print(self.startPose_container.position_z)
# print(self.startPose_container.position_z)
# self.startPose_arg = self.cartclass_to_cartarg(cartclass = self.startPose_container, cartarg = self.startPose_arg)
# print(self.startPose_container.position_z)
# go_to.cartesian_pose(self.startPose_arg)
#Check files and set text file to save to
if (file_save):
self.home = os.path.expanduser("~")
self.dir_path = self.home + '/Learner_Responses/module' + str(
module_number) + '/'
if not os.path.exists(self.dir_path):
os.makedirs(self.dir_path)
self.file_number = 0
self.file_path = self.dir_path + 'test_' + str(
self.file_number) + '.txt'
while (os.path.exists(self.file_path)):
self.file_number += 1
self.file_path = self.dir_path + 'test_' + str(
self.file_number) + '.txt'
self.file = open(self.file_path, 'w')
self.file.write('pretest \r\n' + str(self.old_time) + '\r\n')
self.file.close()
go_to.joint_angles(empty_angle_arg)
# #Complete grip
# grip_msg.data = 0
# self.gripper_pub.publish(grip_msg)
#Test code here
#effort testing
# self.waypoints = []
# self.waypoint_limit = 5
#initialize navigator for user inputs
navigator.right()
# #Wrong waypoints were sent.
# for x in range(0, int(unique_msg.data[0])+1):
# #goes one additional waypoint more than the waypoint of intersection
# go_to.cartesian_pose(self.waypoints[x])
# del self.waypoints[:]
# int_msg.data = 39
# self.iframe_sequence_pub.publish(int_msg)
# elif(data.data == 40):
# rospy.sleep(10)
if __name__ == '__main__':
empty_angle_arg = go_to.joint_angle_arg()
empty_cartesian_arg = go_to.cartesian_pose_arg()
child = pexpect.spawn(
'rostopic pub -r 10 /robot/limb/right/suppress_cuff_interaction std_msgs/Empty'
)
#variables to be used
init_cartesian_arg = go_to.cartesian_pose_arg(joint_angles=[
-0.438147460938, -0.678481445312, -0.433721679687, 1.33986621094,
-0.763953125, -1.132484375, 0.959416015625
])
init_joint_arg = go_to.joint_angle_arg(joint_angles=[
-0.438147460938, -0.678481445312, -0.433721679687, 1.33986621094,
-0.763953125, -1.132484375, 0.959416015625
])
fail_init_joint_arg = go_to.joint_angle_arg(joint_angles=[
-0.33585546875, -0.379596679687, -1.60570117188, 1.12274511719,
# int_msg.data = 3
# self.iframe_sequence_pub.publish(int_msg)
# elif(data.data == 4):
# rospy.sleep(0.1)
# self.slideSequence()
# self.slideSequence() #calls 6
# self.slideSequence()
# rospy.sleep(0.1)
# int_msg.data = 4
# self.iframe_sequence_pub.publish(int_msg)
if __name__ == '__main__':
empty_angle_arg = go_to.joint_angle_arg()
empty_cartesian_arg = go_to.cartesian_pose_arg()
#variables to be used
init_cartesian_arg = go_to.cartesian_pose_arg(joint_angles=[
-0.438147460938, -0.678481445312, -0.433721679687, 1.33986621094,
-0.763953125, -1.132484375, 0.959416015625
])
init_joint_arg = go_to.joint_angle_arg(joint_angles=[
-0.438147460938, -0.678481445312, -0.433721679687, 1.33986621094,
-0.763953125, -1.132484375, 0.959416015625
])
joint_buttons = go_to.joint_angle_arg(joint_angles=[
-0.0393427734375, -0.71621875, 0.022359375, 1.811921875,
-0.116017578125, 2.77036035156, -4.48708789063
],
def __init__(self):
self.slideEvent = True
self.slideClose = False
self.movedBoxes = 0
self.ready = False
self.last_msg = Int32()
self.zeroG_pub = rospy.Publisher('/zeroG_topic', String, queue_size=10)
self.gripper_pub = rospy.Publisher('/gripper_control',
Int32,
queue_size=10)
self.lights_pub = rospy.Publisher('/cuff_light', String, queue_size=10)
self.return_pub = rospy.Publisher('/sawyer_slider_return',
Int32,
queue_size=2)
rospy.Subscriber('/sawyer_sequence', Int32, self.callback)
#rospy.Subscriber('/sawyer_mode', Int32, self.sawyerMode_callback)
#rospy.Subscriber('/sawyer_time', Float32, self.sawyerTime_callback)
rospy.init_node('Sawyer_node', anonymous=True)
self.limb = intera_interface.Limb('right')
self._gripper = get_current_gripper_interface()
self._is_clicksmart = isinstance(self._gripper,
SimpleClickSmartGripper)
self.startPose = self.limb.endpoint_pose()
self.startPose_container = cartPose()
self.startPose_arg = go_to.cartesian_pose_arg()
self.startJointAngles = self.limb.joint_angles()
self.newCartPose_container = cartPose()
self.newJointAngles_arg = go_to.joint_angle_arg()
self.newCartPose_arg = go_to.cartesian_pose_arg()
self.newCartPose_arg2 = go_to.cartesian_pose_arg()
self.newCartPose = cartPose()
self.newJointAngles = jointAngles()
self.waypoint_init_container = cartPose()
self.waypoint_init_arg = go_to.cartesian_pose_arg()
#Check if gripper is attached properly
self.grip = gripper_cuff.GripperConnect()
#Open gripper
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
if (go_to.joint_angles(empty_angle_arg)):
int_msg.data = 1
self.return_pub.publish(int_msg)
# go_to.joint_angles(step2_conveyor_arg)
# self.startPose = self.limb.endpoint_pose()
# self.startPose_container = self.pose_to_cartclass(self.startPose, self.startPose_container)
# self.startPose_arg = self.cartclass_to_cartarg(self.startPose_container, self.startPose_arg)
# self.startPose_container.position_z = z_conveyor + box_height - gripper_width
# self.newCartPose_arg = self.cartclass_to_cartarg(self.startPose_container, self.newCartPose_arg)
# if(go_to.cartesian_pose(self.newCartPose_arg)):
# grip_msg.data = 0
# self.gripper_pub.publish(grip_msg)
# rospy.sleep(1)
# go_to.cartesian_pose(self.startPose_arg)
# go_to.cartesian_pose(self.newCartPose_arg)
# grip_msg.data = 1
# self.gripper_pub.publish(grip_msg)
# go_to.cartesian_pose(self.startPose_arg)
navigator.right()
# elif(data.data == 6):
# rospy.sleep(0.1)
# status_msg.data = 0
# self.return_pub.publish(status_msg)
# go_to.cartesian_pose(pickup_carriage_cart_arg)
# grip_msg.data = 0
# self.gripper_pub.publish(grip_msg)
# go_to.cartesian_pose(above_carriage_cart_arg)
# status_msg.data = 1
# self.return_pub.publish(status_msg)
if __name__ == '__main__':
empty_angle_arg = go_to.joint_angle_arg()
empty_cartesian_arg = go_to.cartesian_pose_arg()
#variables to be used
init_cartesian_arg = go_to.cartesian_pose_arg(joint_angles=[
-0.438147460938, -0.678481445312, -0.433721679687, 1.33986621094,
-0.763953125, -1.132484375, 0.959416015625
])
init_joint_arg = go_to.joint_angle_arg(joint_angles=[
-0.33585546875, -0.379596679687, -1.60570117188, 1.12274511719,
-1.9501328125, -1.73130761719, 1.83596582031
])
joint_buttons = go_to.joint_angle_arg(joint_angles=[
-0.0393427734375, -0.71621875, 0.022359375, 1.811921875,
-0.116017578125, 2.77036035156, -4.48708789063
],