def recover():
pub = rospy.Publisher('/franka_control/error_recovery/goal',
ErrorRecoveryActionGoal,
queue_size=10)
goal = ErrorRecoveryActionGoal()
goal.goal = {}
pub.publish(goal)
def recover(self):
"""
Call the error reset action server.
"""
if self.simulation:
raise ValueError(
'Simulations do not require error recovery, '
'set simulation=True if you want to run on the real robot.')
self.reset_publisher.publish(ErrorRecoveryActionGoal())
rospy.sleep(3.0)
def check_rosout(log):
client = actionlib.SimpleActionClient("franka_control/error_recovery",
ErrorRecoveryAction)
client.wait_for_server()
goal = ErrorRecoveryActionGoal()
robot_state = log.level
goal.header.stamp.secs = 0
goal.header.stamp.nsecs = 0
goal.header.frame_id = ''
goal.goal_id.stamp.secs = 0
goal.goal_id.stamp.nsecs = 0
goal.goal_id.id = ''
goal.goal = {}
print 'Pandas Level: ', robot_state
if robot_state == 8:
print 'Sending state recover to our kind Panda!'
# 2s delay to wait for the robot state to be published as ERROR on the Logging and then send the Action
time.sleep(2)
client.send_goal(goal)
'''
def _toggle_enabled(self, status):
pub = rospy.Publisher('{}/franka_control/error_recovery/goal'.format(self._ns), ErrorRecoveryActionGoal,
queue_size=10)
if self._enabled == status:
rospy.loginfo("Robot is already %s"%self.state())
franka_dataflow.wait_for(
test=lambda: self._enabled == status,
timeout=5.0,
timeout_msg=("Failed to %sable robot" %
('en' if status else 'dis',)),
body=lambda: pub.publish(ErrorRecoveryActionGoal()),
)
rospy.loginfo("Robot %s", ('Enabled' if status else 'Disabled'))
def loops(self, plantId):
err_rec_msg = ErrorRecoveryActionGoal()
msg_true = Bool()
msg_true.data = True
no_error = True
pose = self.pose_start
success_u = self.plan_to_pose(pose.position, pose.orientation)
while (success_u == False):
self.error_recovery_pub.publish(err_rec_msg)
time.sleep(0.2)
success_u = self.plan_to_pose(pose.position, pose.orientation)
for pose in self.pose_list:
self.touch_id += 1
success_u = self.plan_to_pose(pose.position, pose.orientation)
print("Touch id " + str(self.touch_id) + " success: " +
str(success_u))
while (success_u == False):
self.error_recovery_pub.publish(err_rec_msg)
time.sleep(0.2)
success_u = self.plan_to_pose(pose.position, pose.orientation)
time.sleep(0.4)
time.sleep(0.5)
msg_touchid = String()
msg_touchid.data = str(1000 * plantId + self.touch_id)
self.touch_id_pub.publish(msg_touchid)
self.start_recording_pub.publish(msg_true)
time.sleep(0.3)
filename = self.outfolder + "pose_" + msg_touchid.data + ".txt"
with open(filename, 'w') as f:
f.write("position:\n")
f.write("x: " + str(pose.position.x) + "\n")
f.write("y: " + str(pose.position.y) + "\n")
f.write("z: " + str(pose.position.z) + "\n")
f.write("orientation:\n")
f.write("x: " + str(pose.orientation.x) + "\n")
f.write("y: " + str(pose.orientation.y) + "\n")
f.write("z: " + str(pose.orientation.z) + "\n")
f.write("w: " + str(pose.orientation.w) + "\n")
def main():
try:
experiment = plantRecord()
experiment.group.set_max_velocity_scaling_factor(0.1)
roll = 180
pitch = 0
yaw = -45
q = euler_to_quaternion(yaw, pitch, roll)
pose_start = experiment.group.get_current_pose().pose
pose_start.orientation.x = q.x
pose_start.orientation.y = q.y
pose_start.orientation.z = q.z
pose_start.orientation.w = q.w
pose_start.position.x = 0.1
pose_start.position.y = 0.0
pose_start.position.z = 0.8
success = False
i = 0
while (success == False) and (i < 5):
plan, fraction = experiment.group.compute_cartesian_path(
[pose_start], # waypoints to follow
0.01, # eef_step
0.0)
print("fraction:")
print(fraction)
execute = raw_input("execute?")
if (execute == 'y') or (execute == 'Y'):
success = experiment.execute_plan(plan)
elif (execute == 'n'):
return
i = i + 1
if i == 5:
return
experiment.pose_start = experiment.group.get_current_pose().pose
time.sleep(2.)
# record points
pose_list = []
while True:
execute = raw_input("Record new point? [y/n]. No. of points: " +
str(len(pose_list)) + "\n")
if (execute == 'y') or (execute == 'Y'):
print("\n")
execute = raw_input(
"Press ENTER when ready to record pose... ")
pose_list.append(experiment.group.get_current_pose().pose)
elif (execute == 'n') or (execute == 'N') or (execute == 'q'):
print("Done recording. Recorded " + str(len(pose_list)) +
" poses.")
break
print("Open safety switch.")
while True:
execute = raw_input("Ready? [y/n]")
if (execute == 'y') or (execute == 'Y'):
err_rec_msg = ErrorRecoveryActionGoal()
experiment.error_recovery_pub.publish(err_rec_msg)
break
else:
print("retrying... when safety switch opened, type y")
experiment.pose_list = pose_list
i_plant = 0
while True:
execute = raw_input("Record another plant? ")
if (execute == 'y') or (execute == 'Y'):
print("\n")
execute = raw_input("Press ENTER when new plant is placed...")
i_plant += 1
experiment.loops(i_plant)
elif (execute == 'n') or (execute == 'N') or (execute == 'q'):
print("Done recording. Recorded " + str(i_plant) + " plants.")
break
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
print "this is a KeyboardInterrupt that we never catch"
print("============ Python experiment demo complete!")
return
def recover(self):
"""
Call the error reset action server.
"""
self.reset_publisher.publish(ErrorRecoveryActionGoal())
rospy.sleep(3.0)
def reset(self, msg):
self.carry_on_routine = True
recovery_goal = ErrorRecoveryActionGoal()
self.recovery_pub.publish(recovery_goal)
def loops(self, yaws, z_levels, z_step = 0.001, reps = 0):
err_rec_msg = ErrorRecoveryActionGoal()
msg_true = Bool()
msg_true.data = True
no_error = True
roll = -180.0
pitch = 0.0
yaw = -135.0
yaw_initial = -135.0
pos_up = self.group.get_current_pose().pose.position
pos_up.z = pos_up.z + 0.03
q = self.group.get_current_pose().pose.orientation
try:
pos_down = copy.deepcopy(pos_up)
for dyaw in yaws:
yaw_ = yaw + dyaw
print([yaw_, pitch, roll])
q = euler_to_quaternion(yaw_, pitch, roll)
print(q)
success_u = False
success_u = self.plan_to_pose(pos_up, q)
print(success_u)
while (success_u == False):
self.error_recovery_pub.publish(err_rec_msg)
time.sleep(0.2)
success_u = self.plan_to_pose(pos_up, q)
time.sleep(0.4)
fail = False
msg_touchid = String()
msg_touchid.data = str(-1)
print("=========")
print(yaw_)
for iz in xrange(z_levels):
pos_down.x = pos_up.x
pos_down.y = pos_up.y
delta_z = - (z_step * iz)
pos_down.z = pos_up.z + delta_z - 0.03
# print(pos_down.z)
rep = 0
fail = False
success_d = False
success_u = False
while (rep < reps) and (fail == False):
print('rep:')
print(rep)
self.start_recording_pub.publish(msg_true)
success_d = self.plan_to_pose(pos_down, q)
time.sleep(0.3) #kad dode dole
success_u = self.plan_to_pose(pos_up, q)
time.sleep(0.1) # kad dode gore
fail = not success_u or not success_d
if (fail == True):
self.error_recovery_pub.publish(err_rec_msg)
time.sleep(0.2)
self.go_to_pose(pos_up, q)
time.sleep(0.4)
fail = False
msg_touchid = String()
msg_touchid.data = str(-1)
else:
rep += 1
self.touch_id += 1
msg_touchid = String()
msg_touchid.data = str(self.touch_id)
self.touch_id_pub.publish(msg_touchid)
time.sleep(0.1)
self.stop_recording_pub.publish(msg_true)
outFolder = "/home/franka/ivona_dipl/labels/"
#print('ovo je yaw')
#print(yaw)
#print('ovo je yaw_')
#print(yaw_)
yaw_spremi= yaw_-yaw
print('yaw_ koji se sprema')
print(yaw_spremi)
# da se ponisti
yaw_depth_arr = [yaw_spremi, delta_z]
pathOut = outFolder + "yaw_depth_" + str(self.touch_id) + ".txt"
with open(pathOut, 'wb') as fp:
pickle.dump(yaw_depth_arr, fp)
except KeyboardInterrupt:
return
def sendErrorRecover(self):
msg = ErrorRecoveryActionGoal()
self.error_recover_publisher.publish(msg)