def main(self):
self.control.reset_state()
self.control.publish_data("Now I will run code doing mission gate")
# self.mission_gate.step00_checkDeep()
# if( not rospy.is_shutdown() ):
# self.mission_gate.step01_rotateAndFindGate()
# if( not rospy.is_shutdown() ):
# self.mission_gate.step01_5_lockYawToGate()
# if( not rospy.is_shutdown() ):
# self.mission_gate.step02_forwardWithMoveLeftRight()
self.mission_gate.start_mission()
self.control.publish_data(
"Finish to search gate I will move forward with serach path")
self.control.publish_data(
"I will move forward by parameter of gate with find path")
self.control.relative_xy(7, 0)
count = 0
# This step will use to movement with rotation yaw
self.control.update_target()
collect_yaw = self.control.target_pose[5]
self.control.publish_data("SPIN PASS GATE")
while (not self.control.check_xy(0.1, 0.1)):
self.rate.sleep()
self.vision_path.call_data()
self.vision_path.echo_data()
if (self.vision_path.num_point != 0):
self.control.get_state()
count += 1
else:
count = 0
if (count == 2):
self.control.publish_data(
"I found path 2 round at here reset now")
self.control.reset_state()
self.control.publish_data("Sleep 2 second wait to reset state")
rospy.sleep(2)
self.control.publish_data("Wakeup I will waiting yaw")
while (not self.control.check_yaw(0.15)):
self.rate.sleep()
relative_x = self.vision_path.y_point[0] * 0.8 / 100
relative_y = self.vision_path.x_point[0] * -1.2 / 100
self.control.publish_data("Move go to path ( x , y ) : " +
repr((relative_x, relative_y)))
self.control.relative_xy(relative_x, relative_y)
while (not self.control.check_xy(0.15, 0.15)):
self.rate.sleep()
break
if (self.control.check_yaw(0.6)):
self.control.publish_data("Adding spin yaw")
self.control.relative_yaw(math.pi / 1.5)
# End part of movement with rotation yaw
self.control.publish_data("Return absolute yaw " + str(collect_yaw))
self.control.absolute_yaw(collect_yaw)
while (not self.control.check_yaw(0.15)):
self.rate.sleep()
if (count == 2):
self.control.publish_data("I start path by ever found path")
else:
self.control.publish_data("I start path by never found path")
result = self.mission_path.find_path()
if (result):
self.control.publish_data("Congratulation we know you pass path")
else:
self.control.publish_data("It bad you failure mission path")
self.control.absolute_yaw(
zeabus_math.bound_radian(collect_yaw + (math.pi / 2)))
self.control.publish_data(
"Waiting yaw before send process to buoy_straight")
while (not self.control.check_yaw(0.15)):
self.rate.sleep()
self.mission_buoy.start_mission()
self.control.publish_data(
"Finish play buoy next I will play path move up")
self.control.absolute_z(-1)
while (not self.control.check_z(0.15)):
self.rate.sleep()
self.control.publish_data("Waiting xy")
while (not self.control.check_xy(0.15, 0.15)):
self.rate.sleep()
self.control.publish_data("Waiting yaw")
while (not self.control.check_yaw(0.15)):
self.rate.sleep()
self.control.publish_data("Move forward and searching 2.5 meter")
self.control.relative_xy(2.5, 0)
# Start path move forward and searching
count = 0
while (not self.control.check_xy(0.1, 0.1)):
self.rate.sleep()
self.vision_path.call_data()
self.vision_path.echo_data()
if (self.vision_path.num_point != 0):
count += 1
else:
count = 0
if (count == 2):
self.control.publish_data(
"I found path 2 round at here reset now")
self.control.reset_state()
self.control.publish_data("Sleep 2 second wait to reset state")
rospy.sleep(2)
break
# End part to search
if (count == 2):
self.control.publish_data("I start path by ever found path")
else:
self.control.publish_data("I start path by never found path")
result = self.mission_path.find_path()
if (result):
self.control.publish_data("Congratulation we know you pass path")
else:
self.control.publish_data("It bad you failure mission path")
self.control.absolute_yaw(
zeabus_math.bound_radian(collect_yaw - (math.pi / 2)))
# End part of play all mission
self.control.publish_data("Finish all strategy mission")
self.control.deactivate(["x", "y", "z", "roll", "pitch", "yaw"])
def setup_point( self ): # status_mission = 2
self.control.publish_data( "Now I move first point to picutre" )
while( ( not rospy.is_shutdown() ) ):
self.rate.sleep()
self.control.publish_data( "I waiting xy")
while( not self.control.check_xy( 0.1, 0.1 ) ):
self.rate.sleep()
self.control.publish_data( "I waiting yaw")
while( not self.control.check_yaw( 0.10 ) ):
self.rate.sleep()
self.vision.call_data()
self.vision.echo_data()
relative_x = 0
relative_y = 0
ok_x = False
ok_y = False
if( self.vision.num_point == 0 ):
relative_y = 0
elif( self.vision.x_point[0] > 20 ):
relative_y = -0.15
elif( self.vision.x_point[0] < -20 ):
relative_y = +0.15
else:
ok_y = True
if( self.vision.num_point == 0):
relative_x = 0.25
elif( self.vision.y_point[0] > 20 ):
relative_x = 0.15
elif( self.vision.y_point[0] < -20 ):
relative_x = -0.15
else:
ok_x = True
if( ok_x and ok_y ):
self.control.publish_data( "xy is ok next I will collect point" )
break
else:
self.control.publish_data("T will move x : y "
+ str( relative_x ) + " : "
+ str( relative_y ) )
self.control.relative_xy( relative_x , relative_y )
count = 0
temp_yaw = []
while( not rospy.is_shutdown() and count < 1):
self.rate.sleep()
self.control.publish_data( "Waiting yaw for calculate" )
while( not self.control.check_yaw( 0.1 ) ):
self.rate.sleep()
self.control.publish_data( "Calculate yaw" )
self.vision.call_data()
self.vision.echo_data()
temp_yaw.append(
self.control.current_pose[5]
- ( ( math.pi / 2 )
- self.vision.rotation[0] ) )
count += 1
self.control.publish_data("temp_yaw " + repr( temp_yaw ) )
self.status_mission = 3
self.first_yaw = zeabus_math.bound_radian( sum( temp_yaw ) / 1 )
self.control.publish_data( "Now I will absolute yaw is " + str( self.first_yaw ) )
self.control.absolute_yaw( self.first_yaw )
self.moving_on_path()
def not_use_dvl( self ):
self.control.publish_data( "STRATEGY not use DVL start mission by target at exposed")
self.control.update_target()
collective_target_yaw = zeabus_math.bound_radian(
self.control.target[5] + STRATEGY_ROTATION_STAKE )
self.control.absolute_z( STRATEGY_EXPOSED_FIND )
self.control.publish_data( "STRATEGY command depth to " + str( STRATEGY_EXPOSED_FIND ) )
self.control.sleep()
self.control.relative_yaw( STRATEGY_ROTATION_EXPOSED )
self.control.publish_data( "STRATEGY rotation for find target coffin to exposed " + str(
STRATEGY_ROTATION_EXPOSED ) )
self.control.sleep()
while not self.control.check_yaw( 0.12 ):
self.rate.sleep()
while not self.control.check_z( 0.12 ):
self.rate.sleep()
self.control.deactivate( ('x', 'y') )
start_time = rospy.get_rostime()
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
while not rospy.is_shutdown() and diff_time < STRATEGY_TIME_SURVEY:
self.rate.sleep()
self.control.force_xy( 0 , STRATEGY_FORCE_SURVEY )
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
self.control.publish_data( "STRATEGY Survey for expose on time " + str( diff_time ) )
self.control.force_xy( 0 , 0 )
self.control.activate( ('x' , 'y' ) )
self.control.publish_data( "STRATEGY Waiting yaw before forward")
while not self.control.check_yaw( 0.12 ) :
self.rate.sleep()
self.control.deactivate( ('x' , 'y') )
self.control.publish_data( "STRATEGY forward to find exposed" )
count_found = 0
start_time = rospy.get_rostime()
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
while (not rospy.is_shutdown()) and diff_time < STRATEGY_TIME_FORWARD and count_found < 5 :
self.rate.sleep()
self.vision_coffin.call_data()
self.vision_coffin.echo_data()
if self.vision.result['num_object'] > 0 :
count_found += 1
force_x = 0
force_y = 0
ok_x = False
ok_y = False
self.control.publish_data( "STRATEGY found target command force "
+ repr( (force_x , force_y ) ) )
if self.vision_coffin.result['center_x'] < -20 :
force_y = SURVER_LEFT
elif self.vision_coffin.result['center_x'] > 20 :
force_y = SURVEY_RIGHT
else:
ok_y = True
if( self.vision_coffin.result['center_y'] < -20 ):
force_x = SURVEY_BACKWARD
elif self.vision_coffin.result['center_y'] > 20:
force_x = SURVEY_FORWARD
else:
ok_x = True
if( ok_x and ok_y ):
count_found = 5
self.control.publish_data( "STRATEGY now center point of coffin")
else:
self.control.force_xy( force_x , force_y )
self.control.publish_data( "STRATEGY Have picture command force " + repr( (
force_x , force_y ) ) )
else:
count_found = 0
self.control.force_xy( SURVEY_FORWARD , 0 )
self.control.publish_data("STRATEGY Don't found target command survey forward")
diff_time = (rospy.get_rostime() - start_time).to_sec()
self.control.activate( [ 'x' , 'y' ] )
result = False
if( count_found == 5 ):
self.control.publish_data( "STRATEGY finish and found object let operator")
self.mission_exposed.operator()
result = True
else:
self.mission_exposed.publish_data( "STRATEGY finish forward search let find object")
result = self.mission_exposed.find()
if result :
self.control.publish_data( "STRATEGY success mission I will try to continue stake" )
self.control.absolute_yaw( collective_target_yaw )
self.control.publish_data( "STRATEGY command absolute yaw "
+ str( collective_target_yaw ) )
self.control.sleep()
while not self.control.check_yaw( 0.12 ):
self.rate.sleep()
self.control.absolute_z( STRATEGY_STAKE_DEPTH )
self.control.publish_data( "STRATEGY command absolute depth is "
+ str( STRATEGY_STAKE_DEPTH ) )
self.control.deactivate( ('x' , 'y' ) )
start_time = rospy.get_rostime()
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
self.control.publish_data("STRATEGY forward before survey to stake mission")
while ( not rospy.is_shutdown() ) and diff_time < STRATEGY_STAKE_TIME_FORWARD :
self.rate.sleep()
self.control.force_xy( STRATEGY_STAKE_FORCE_FORWARD , 0 )
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
self.control.publish_data( "STRATEGY backward to stake on time " + str(diff_time) )
self.control.activate( ('x' , 'y') )
self.control.force_false()
self.control.publish_data( "STRATEGY waiting yaw")
while not self.control.check_yaw( 0.15 ):
self.rate.sleep()
self.control.deactivate( ( 'x' , 'y' ) )
stat_time = rospy.get_rostime()
diff_time = 0
count_found = 0
while ( not rospy.is_shutdown() ) and diff_time < STRATEGY_STAKE_TIME_SURVEY :
self.rate.sleep()
self.control.force_xy( 0.0 , STRATEGY_STAKE_FORCE_SURVEY )
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
self.vision_stake.call_data()
self.vision_stake.echo_data()
if self.vision_stake.result['found']:
count_found += 1
if count_found == 3 :
self.control.publish_data( "STRATEGY find stake I will target on that")
count_unfound = 0
self.control.force_xy( 0 , 0 )
while not rospy.is_shutdown() and count_unfound < 3 :
self.rate.sleep()
self.vision_stake.call_data( STAKE_FIND_TARGET )
self.vision_stake.echo_data()
force_x = 0
force_y = 0
ok_x = False
ok_y = False
if self.vision_stake.result['found'] :
count_unfound = 0
if self.vision_stake.result['center'][0] > 20 :
force_y = TARGET_RIGHT
elif self.vision_stake.result['center'][1] < -20 :
force_y = TARGET_LEFT
else:
ok_y = True
if self.vision.result['area'] < STRATEGY_STAKE_AREA_FOCUS :
force_x = TARGET_FORWARD
else:
ok_x = True
if ok_x and ok_y :
self.control.force_xy( 0 , 0 )
self.control.publish_data( "STRATEGY get process to stake")
self.mission_stake.operator()
break
else:
self.control.publish_data( "STRATEGY focuse target "
+ repr( ( force_x , force_y ) ) )
else:
count_unfound += 1
self.control.publish_data( "STRATEGY Don't found target "
+ str( count_unfound ) )
self.control.force_xy( 0 , 0 )
if count_unfound == 3:
self.control.publish_data( "Strategy Continue until time out")
count_found = 0
else:
self.control.publish_data( "STRATEGY Finish play break by after play")
break
else:
count_found = 0
self.control.publish_data( "STRATEGY count found " + str( count_found ) )
else:
self.control.publish_data( "STRATEGY faliure mission exposed don't play stake" )
self.control.publish_data( "STRATEGY finish function play not_use_dvl")
def use_dvl( self ):
self.control.publish_data( "STRATEGY use DVL start mission by target at exposed")
self.control.update_target()
collective_target_yaw = zeabus_math.bound_radian(
self.control.target[ 5 ] + STRATEGY_ROTATION_STAKE )
self.control.absolute_z( STRATEGY_EXPOSED_FIND )
self.control.publish_data( "STRATEGY command depth to " + str( STRATEGY_EXPOSED_FIND ) )
self.control.sleep()
self.control.relative_yaw( STRATEGY_ROTATION_EXPOSED )
self.control.publish_data( "STRATEGY rotation for find target coffin to exposed " + str(
STRATEGY_ROTATION_EXPOSED ) )
self.control.sleep()
while not self.control.check_yaw( 0.15 ):
self.rate.sleep()
self.control.publish_data( "STRATEGY distance survey is " + str( STRATEGY_DISTANCE_SURVEY ) )
self.control.relative_xy( 0 , STRATEGY_DISTANCE_SURVEY )
self.control.sleep()
while not self.control.check_xy( 0.15 , 0.15 ):
self.rate.sleep()
self.control.publish_data( "STRATEGY distance forward is " + str(STRATEGY_DISTANCE_FORWARD) )
self.control.relative_xy( STRATEGY_DISTANCE_FORWARD , 0 )
self.control.update_target()
collective_point = ( self.control.target_pose[0] , self.control.target_pose[1] )
play_mission = False
count_found = 0
while not self.control.check_xy( 0.15 , 0.15 ) :
self.rate.sleep()
self.vision_coffin.call_data()
self.vision_coffin.echo_data()
if self.vision_coffin.result['num_object'] > 0 :
count_found += 1
if count_found == 3:
self.control.publish_data( "STRATEGY Find coffind stop survey and make center")
self.control.deactivate( ('x' , 'y' ) )
count_unfound = 0
while ( not rospy.is_shutdown() ) and count_unfound < 3 :
self.rate.sleep()
self.vision_coffin.call_data()
self.vision_coffin.echo_data()
if self.vision_coffin.result['num_object'] > 0 :
count_unfound = 0
force_x = 0
force_y = 0
ok_x = False
ok_y = False
if self.vision_coffin.result['center_x'] < -20 :
force_y = SURVEY_LEFT
elif self.vision_coffin.result['center_x' ] > 20 :
force_y = SURVEY_RIGHT
else:
ok_y = True
if self.vision_coffin.result['center_y'] < -20 :
force_x = SURVEY_BACKWARD
elif self.vision_coffin.result['center_y'] > 20 :
force_x = SURVEY_FORWARD
else:
ok_x = True
if ok_x and ok_y :
count_found = 3
self.control.publish_data( "STRATEGY now center point of coffin")
self.control.force_xy( 0 , 0 )
break
else:
self.control.force_xy( force_x , force_y )
self.control.publish_data( "STRATEGY command force "
+ repr( (force_x , force_y ) ) )
else:
count_unfound += 1
count_found = 0
self.control.activate( ('x' , 'y' ) )
if count_unfound == 3 :
self.control.publish_data( "STRATEGY think target it is noise continue")
self.control.absolute_xy( collective_point[0] , collective_point[1] )
self.control.sleep()
else:
break
else:
self.control.publish_data( "STRATEGY found coffin object is "
+ str( count_found ))
else:
count_found = 0
result = False
if count_found == 3 :
self.mission_exposed.operator()
result = True
else:
result = self.mission_exposed.find()
if result:
self.control.publish_data( "STRATEGY finish play exposed consider try to play stake")
self.control.absolute_yaw( collective_target_yaw )
self.control.publish_data( "STRATEGY command target yaw is "
+ str( collective_target_yaw ) )
self.control.absolute_z( STRATEGY_STAKE_DEPTH )
self.control.publish_data( "STRATEGY command depth to "
+ str( STRATEGY_STAKE_DEPTH ) )
self.control.sleep()
while not self.control.check_yaw( 0.15 ):
self.rate.sleep()
self.control.relative_xy( 0 , STRATEGY_STAKE_DISTANCE_SURVEY )
self.control.publish_data( "STRATEGY command survey is "
+ str( STRATEGY_STAKE_DISTANCE_SURVEY ) )
self.control.sleep()
while not self.control.check_xy( 0.15 ):
self.rate.sleep()
self.control.relative_xy( STRATEGY_STAKE_DISTANCE_FORWARD , 0 )
self.control.publish_data( "STRATEGY command forward is "
+ str( STRATEGY_STAKE_DISTANCE_FORWARD ) )
self.control.sleep()
self.control.update_target()
collective_point = ( self.control.target_pose[0] , self.control.target_pose[1] )
count_found = 0
while ( not self.control.check_xy( 0.15 ) ) and count_found < 3 :
self.rate.sleep()
self.vision_stake.call_data( STAKE_FIND_TARGET )
self.vision_stake.echo_data()
if self.vision_stake.result['found'] :
count_found += 1
if count_found == 3 :
self.control.publish_data( "STRATEGY Now found target stop and focus")
count_unfound = 0
self.control.deactivate( ('x' , 'y' ) )
while ( not rospy.is_shutdown() ) and count_unfound < 3 :
self.rate.sleep()
self.vision_stake.call_data( STAKE_FIND_TARGET )
self.vision_stake.echo_data()
if self.vision.result['found'] :
count_unfound = 0
force_x = 0
force_y = 0
ok_x = False
ok_z = False
if self.vision.result['center'][0] > 10 :
force_y = TARGET_LEFT
elif self.vision.result['center'][0] < -10 :
force_y = TARGET_RIGHT
else:
ok_x = True
if self.vision.result['area'] < STRATEGY_STAKE_AREA_FOCUS / 1.5:
force_x = TARGET_FORWARD
else:
ok_z = True
if ok_x and ok_z :
count_found = 3
self.control.force_xy( 0 , 0 )
self.mission_stake.operator()
break
else:
self.control.force_xy( force_x , force_y )
self.control.publish_data("STRATEGY command force is " + repr(
force_x , force_y ) )
else:
count_unfound += 1
self.control.activate( ('x' , 'y' ) )
if count_unfound == 3:
self.control.publish_data( "STRATEGY target disapper" )
self.control.absolute_xy( collective_point[0] , collective_point[1] )
self.control.sleep()
else:
self.control.publish_data( "STRATEGY Give process to stake")
else:
self.control.publish_data( "STRATEGY found target " + str( count_found ) )
else:
count_found = 0
else:
self.control.publish_data( "STRATEGY don't continue to play because exposed false")
def moving_on_path(self): # status_mission = 3
# Move to center of path
self.control.publish_data(
"MOVING_ON_PATH move to center of first point")
while ((not rospy.is_shutdown())):
self.rate.sleep()
self.vision.call_data()
self.vision.echo_data()
force_x = 0
force_y = 0
target_point = 0
if (self.vision.x_point[0] < -15):
force_y = TARGET_LEFT
elif (self.vision.x_point[0] > 15):
force_y = TARGET_RIGHT
elif (self.vision.y_point[0] < -15):
force_x = TARGET_BACKWARD
elif (self.vision.y_point[0] > 15):
force_x = TARGET_FORWARD
elif (not self.control.check_yaw(0.12)):
self.control.publish_data(
"MOVING_ON_PATH POINT 1 now center Waiting yaw")
else:
self.control.publish_data(
"MOVING_ON_PATH POINT 1 now center move direct")
start_time = rospy.get_rostime()
while ((not rospy.is_shutdown())
and self.vision.num_point != 3):
self.rate.sleep()
self.vision.echo_data()
self.vision.call_data()
self.control.force_xy(1, 0)
self.control.publish_data(
"MOVING_ON_PATH POINT 1 move forward")
self.control.force_xy(1, 0)
break
self.control.publish_data("MOVING_ON_PATH POINT 1 command " +
repr((force_x, force_y)))
self.control.force_xy(force_x, force_y)
if PATH_MODE:
self.control.publish_data(
"MOVING_ON_PATH SECTION try move to center of point 2")
start_time = rospy.get_rostime()
diff_time = (rospy.get_rostime() - start_time).to_sec()
while ((not rospy.is_shutdown) and diff_time < 7):
self.rate.sleep()
self.control.publish_data(
"MOVING_ON_PATH try to find 3 point " + str(diff_time))
self.control.force_xy(TARGET_FORWARD, 0)
diff_time = (rospy.get_rostime() - start_time).to_sec()
self.control.force_xy(0, 0)
self.control.publish_data(
"MOVING_ON_PATH Finish little mode to center")
count_unfound = 0
while ((not rospy.is_shutdown())):
self.rate.sleep()
self.vision.call_data()
self.vision.echo_data()
force_x = 0
force_y = 0
if PATH_MODE:
target_point = self.vision.num_point - 1
if target_point < 0:
self.control.publish_data(
"MOVING_ON_PATH Think more forward make num 0")
self.control.force_xy(TARGET_BACKWARD, 0)
continue
else:
target_point = 1
if (self.vision.x_point[1] < -10):
force_y = TARGET_LEFT
elif (self.vision.x_point[1] > 10):
force_y = TARGET_RIGHT
elif (self.vision.y_point[1] < -20):
force_x = TARGET_BACKWARD
elif (self.vision.y_point[1] > 00):
force_x = TARGET_FORWARD
else:
self.control.publish_data(
"MOVING_ON_PATH POINT 2 now center of point 2")
break
self.control.publish_data("MOVING_ON_PATH POINT 2 command " +
repr((force_x, force_y)))
self.control.force_xy(force_x, force_y)
self.control.publish_data(
"MOVING_ON_PATH I will deactivate yaw for rotation")
self.control.deactivate(['x', 'y', 'yaw'])
now_tune = False
while (not rospy.is_shutdown()):
self.rate.sleep()
self.vision.call_data()
self.vision.echo_data()
diff = zeabus_math.bound_radian(
self.vision.rotation[self.vision.num_point - 2] -
(math.pi / 2))
if ((abs(self.vision.x_point[1]) > 80
or abs(self.vision.y_point[1]) > 80) or now_tune):
force_x = 0
force_y = 0
ok_x = False
ok_y = False
if (not now_tune):
self.control.deactivate(['x', 'y'])
if self.vision.x_point[1] > 20:
force_y = TARGET_RIGHT
elif self.vision.x_point[1] < -20:
force_y = TARGET_LEFT
else:
ok_x = True
if self.vision.y_point[1] > 20:
force_x = TARGET_FORWARD
elif self.vision.y_point[1] < -20:
force_x = TARGET_BACKWARD
else:
ok_y = True
self.control.publish_data(
"MOVING_ON_PATH mode rotation force " +
repr((force_x, force_y)))
self.control.force_xy(force_x, force_y)
if ok_x and ok_y:
now_tune = False
self.control.force_xy(0, 0)
self.control.deactivate(['x', 'y', 'yaw'])
continue
self.control.publish_data("MOVING_ON_PATH diff yaw is " +
str(diff))
if (abs(diff) < PATH_OK_DIFF_YAW):
self.control.force_false()
self.control.publish_data("Now rotation are ok")
self.control.sleep()
self.control.activate(['x', 'y', 'yaw'])
self.control.relative_yaw(0, False)
self.control.sleep()
break
elif (diff > 0):
self.control.publish_data("MOVING_ON_PATH rotation left")
self.control.force_xy_yaw(0, 0, PATH_FORCE_YAW)
else:
self.control.publish_data("MOVING_ON_PATH rotation right")
self.control.force_xy_yaw(0, 0, -1.0 * PATH_FORCE_YAW)
self.control.publish_data(
"MOVING_ON_PATH Now rotation finish next is moving on path")
self.control.deactivate(['x', 'y'])
self.control.absolute_z(PATH_END_DEPTH)
start_time = rospy.get_rostime()
diff_time = (rospy.get_rostime() - start_time).to_sec()
while (not rospy.is_shutdown() and diff_time < PATH_LAST_TIME):
self.rate.sleep()
self.control.force_xy(SURVEY_FORWARD, 0)
diff_time = (rospy.get_rostime() - start_time).to_sec()
self.control.publish_data("MOVING_ON_PATH diff , limit " +
repr((diff_time, PATH_LAST_TIME)))
self.control.activate(['x', 'y'])
self.control.relative_xy(0, 0)
def setup_point(self): # status_mission = 2
self.control.publish_data("SETUP Move to first point")
self.control.deactivate(['x', 'y'])
while ((not rospy.is_shutdown())):
self.rate.sleep()
self.vision.call_data()
self.vision.echo_data()
relative_x = 0
relative_y = 0
ok_x = False
ok_y = False
if (self.vision.num_point == 0):
relative_y = 0
elif (self.vision.x_point[0] > 10):
relative_y = TARGET_RIGHT
elif (self.vision.x_point[0] < -10):
relative_y = TARGET_LEFT
else:
ok_y = True
if (self.vision.num_point == 0):
relative_x = 0
elif (self.vision.y_point[0] > 0):
relative_x = TARGET_FORWARD
elif (self.vision.y_point[0] < -20):
relative_x = TARGET_BACKWARD
else:
ok_x = True
if (ok_x and ok_y):
self.control.publish_data("SETUP xy are ok")
self.control.force_xy(0, 0)
break
else:
self.control.publish_data("SETUP command force x : y === " +
str(relative_x) + " : " +
str(relative_y))
self.control.force_xy(relative_x, relative_y)
self.control.publish_data("SETUP I will deactivate yaw for rotation")
self.control.deactivate(['x', 'y', 'yaw'])
now_tune = False
while (not rospy.is_shutdown()):
self.rate.sleep()
self.vision.call_data()
self.vision.echo_data()
if ((abs(self.vision.x_point[0]) > 80
or abs(self.vision.y_point[0]) > 80) or now_tune):
force_x = 0
force_y = 0
ok_x = False
ok_y = False
if not now_tune:
self.control.deactivate(['x', 'y'])
now_tune = True
if self.vision.x_point[0] > 10:
force_y = TARGET_RIGHT
elif self.vision.x_point[0] < -10:
force_y = TARGET_LEFT
else:
ok_x = True
if self.vision.y_point[0] > 10:
force_x = TARGET_FORWARD
elif self.vision.y_point[0] < -10:
force_x = TARGET_BACKWARD
else:
ok_y = True
self.control.publish_data("SETUP mode rotation force " +
repr((force_x, force_y)))
self.control.force_xy(force_x, force_y)
if ok_x and ok_y:
self.control.force_xy(0, 0)
self.control.deactivate(['x', 'y', 'yaw'])
now_tune = False
continue
diff = zeabus_math.bound_radian(self.vision.rotation[0] -
(math.pi / 2))
self.control.publish_data("SETUP diff yaw is " + str(diff))
if (abs(diff) < PATH_OK_DIFF_YAW):
self.control.force_false()
self.control.publish_data("Now rotation are ok")
self.control.sleep()
self.control.activate(['x', 'y', 'yaw'])
break
elif (diff > 0):
self.control.publish_data("SETUP rotation left")
self.control.force_xy_yaw(0, 0.0, PATH_FORCE_YAW)
else:
self.control.publish_data("SETUP rotation right")
self.control.force_xy_yaw(0, 0.0, -1.0 * PATH_FORCE_YAW)
self.control.publish_data(
"SETUP Now rotation finish next is moving on path")
self.control.deactivate(['x', 'y'])
self.moving_on_path()