def __init__(self):
rospy.init_node("project33")
self.ch = ControlHandle("project33")
self.state = False
self.server_state = rospy.Service("/project/activate", SendBool,
self.callback_state)
class Subject33Pitch:
def __init__(self):
rospy.init_node("project33")
self.ch = ControlHandle("project33")
self.state = False
self.server_state = rospy.Service("/project/activate", SendBool,
self.callback_state)
def activate(self):
self.ch.activate(False)
while self.ch.ok(): # Loop activate node
rospy.sleep(0.25)
reset_state = True
while self.state: # Lopp working
if reset_state:
self.ch.activate(True)
self.ch.sleep()
self.ch.reset_all(False) # Reset only orientation
self.ch.sleep()
self.ch.absolute_pitch(0)
self.ch.sleep()
self.ch.absolute_roll(0)
self.ch.sleep()
self.ch.absolute_depth(-1.0)
self.ch.sleep()
self.ch.set_mask()
self.ch.sleep()
reset_state = False
self.ch.pub("Waiting z ok position in one")
self.ch.sleep()
while (not self.ch.check_error(
z=0.05)) and self.ch.ok() and self.state:
self.ch.sleep()
self.ch.pub("Command addition force positive forward")
start_time = rospy.get_rostime()
while self.ch.ok() and self.state:
self.ch.add_force(x=1.5)
self.ch.sleep()
if (rospy.get_rostime() - start_time).to_sec() > 100:
break
self.ch.add_force(x=0)
self.ch.pub("Finish yaw testing")
self.ch.absolute_depth(-0.15)
self.ch.set_mask()
self.ch.activate(False)
self.ch.pub("Finish stop process")
self.state = False
if self.ch.ok():
print("Stop process by ros shutdon")
break
def callback_state(self, request):
self.state = request.data
return SendBoolResponse()