def __init__(self):
smach.State.__init__(self,
outcomes=['SUCCESS', 'FAIL', 'REPEAT', 'BACK'])
self.reachImageTimeout = status_handler.reachImageTimeout
self.timeoutCounter = 0
self.rate = rospy.Rate(1)
self.stateMsg = StateMsg()
def __init__(self): smach.State.__init__(self, outcomes=['SUCCESS', 'FAIL', 'REPEAT', 'GO_APPROACH']) self.findArtagTimeout = status_handler.findArtagTimeout self.timeoutCounter = 0 self.rate = rospy.Rate(1) self.stateMsg = StateMsg() self.goBack = status_handler.goBack self.goBack = False
def __init__(self):
smach.State.__init__(self,
outcomes=[
'SUCCESS', 'IMAGE_INTERRUPT', 'FAIL',
'REPEAT', 'SUCCES12'
])
self.rate = rospy.Rate(1)
self.stateMsg = StateMsg()
def __init__(self): smach.State.__init__(self, outcomes=['TO_GPS', 'FAIL','REPEAT']) self.readyTimeout = status_handler.readyTimeout self.timeoutCounter = 0 self.rate = rospy.Rate(1) self.stateMsg = StateMsg()
def __init__(self): smach.State.__init__(self, outcomes=['REPEAT', 'FAIL', 'SUCCESS']) self.initaliseTimeout = status_handler.initaliseTimeout self.timeoutCounter = status_handler.initaliseTimeout self.rate = rospy.Rate(1) self.stateMsg = StateMsg()
def __init__(self): smach.State.__init__(self, outcomes=['SUCCESS', 'FAIL','REPEAT_APPROACH']) self.rate = rospy.Rate(1) self.stateMsg = StateMsg()
def __init__(self):
rospy.init_node('ball_search', anonymous=False)
self.currPosX=0
self.currPosY=0
self.currPosZ=0
self.yaw=0
self.startMsg = "s10f"
self.stopMsg = "s01f"
self.resetMsg = "s00f"
self.rotate_once=1 # deleted in code
# if servo completed its rotation this will be true
self.send_once=1
self.R=0.5
self.ball_is_found=0
self.dir = 1
self.sangle = 0 #servo angle
self.sc = 4
self.left = False #left artag
self.right = False #right artag
self.rotate_done = None
self.half_rotate = False
self.clockwise = None
self.speed = 0
self.approach_counter = 0
self.state=StateMsg()
self.search_done = False
self.counter_saw =0
#self.imuAngle = 0
#self.donedone = Servocamera.done_servo_rotation()
print("waiting move base client...")
self.client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
self.client.wait_for_server()
print("client is on")
rate = rospy.Rate(10) # 1hz
self.search_topic = '/artag_search_done'
#tell the action client that we want to spin a thread by default
self.Pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.Servo_pub = rospy.Publisher('/servo_control', String, queue_size=10)
self.done_pub = rospy.Publisher('/artag_search_done', String, queue_size=10)
self.rover_rotated = False
self.servo_rotating = False
self.servo_rotated = False
self.servo_rotation_count = 0
while not rospy.is_shutdown():
rospy.Subscriber('/outdoor_waypoint_nav/odometry/filtered',Odometry, self.robotPoseSubscriber)
rospy.Subscriber('/imu/data', Imu, self.robotPoseSubscriber)
rospy.Subscriber('/rover_state_topic',StateMsg, self.stateSubscriber)
#rospy.Subscriber('/stage_counter_topic', String, self.stageSubscriber)
rospy.Subscriber('/servo_angle', String, self.angleSubscriber)
#rospy.Subscriber('/odometry/filtered', Odometry, self.odomSubsriber)
#print(self.state.state)
#print("rotate_done:", self.rotate_done)
#print("SC: ", self.sc)
#rospy.Subscriber('/move_base/result', MoveBaseActionResult, self.moveSubscriber)
#print(self.state.state)
if (self.state.state==self.state.FIND_ARTAG):
rospy.Subscriber('/px_coordinates', String, self.artag_Subscriber)
rospy.Subscriber('/px_coordinates1', String, self.artag_Subscriber1)
rospy.Subscriber('/servodone', String, self.done_rotate_Subscriber)
#print("searching")
print("L: ", self.left, " R: ", self.right)
print("stage counter:", self.sc)
#print("servo_rotated: ", self.servo_rotated)
print("servo rotating: ", self.servo_rotating)
print("servo_rotation_count: ", self.servo_rotation_count)
if(self.left == True or self.right == True):
self.counter_saw += 1
if self.servo_rotating == False:
print("SERVO ROTATING: FALSEEEEEE")
if self.servo_rotation_count == 0:
if(self.left == False and self.right == False and self.counter_saw > 0):
self.start_servo_rotation()
self.servo_rotating = True
print("BURALAR DUTLUUUUK")
continue
if self.rover_rotated == False and self.servo_rotation_count == 1 and self.left == False and self.right == False:
self.clockwise = False
self.rotate(180)
time.sleep(3)
self.servo_rotated = False
self.rover_rotated = True
print("rover rotated")
if(self.servo_rotated == False and self.rover_rotated == True):
if(self.left == False and self.right == False and self.counter_saw > 0):
self.start_servo_rotation()
time.sleep(5)
continue
if(self.rover_rotated == True and self.servo_rotation_count == 2):
print("Left:", self.left)
print("Right:", self.right)
print("SC:", self.sc)
#print("jnbxfjbnfxjbmnbjnbjxnbjxfnbkxjnb jfnbjkfnbkjnbkjcvnbjcnbkjbnkcnbkcngbkdghbkjdnbfhnmvnbjkghjfcnbfjkfhgbfjvb gjdgbhdjkdvbngdjmvnbgdjmvdb mvfsmycebntjtghekjgtnbwtkxfwnbwdjkwhnbw jtmxvcn btwjcmtnbw tjcwvnb tjwdnhtjkbw nvcwtwtwttery")
if(self.left == False and self.right == False):
print("SALYANGOZ----------SALYANGOZ------------SALYANGOZ------------SALYANGOZ")
self.go_forward()
self.clockwise = False
self.rotate(90)
self.rover_rotated = False
self.servo_rotated = False
if(self.sc >= 4 and self.left == True and self.right == False):
print("LEEFFFTTT ------- LEFTT ----------LEEFFFTTT ---------LEEFFFTTT")
self.speed = 2
self.go_forward()
time.sleep(4)
self.client.cancel_goal()
self.clockwise = True
self.rotate(90)
time.sleep(3)
self.speed = 2
self.go_forward()
time.sleep(4)
self.client.cancel_goal()
self.clockwise = False
self.rotate(90)
time.sleep(3)
if(self.sc >= 4 and self.right == True and self.left == False):
print("RIGHTTTT ------- RIGHTTTT ------------- RIGHTTTT---------RIGHTT")
self.speed = 2
self.go_forward()
time.sleep(4)
self.client.cancel_goal()
#self.client.cancel_all_goals()
self.clockwise = False
self.rotate(90)
time.sleep(3)
self.speed = 2
self.go_forward()
time.sleep(4)
self.client.cancel_goal()
self.clockwise = True
self.rotate(90)
time.sleep(3)
if self.search_done == True:
self.done_pub.publish("1")
else:
self.done_pub.publish("0")
rate.sleep()
def __init__(self):
rospy.init_node('ball_search', anonymous=False)
self.currPosX=0
self.currPosY=0
self.currPosZ=0
self.yaw=0
self.rotate_once=1 # deleted in code
self.servo_rotated = False # if servo completed its rotation this will be true
self.send_once=1
self.R=0.5
self.ball_is_found=0
self.dir = 1
self.sangle = 0 #servo angle
self.sc = None
self.left = None #left artag
self.right = None #right artag
self.state=StateMsg()
print("waiting move base client...")
self.client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
self.client.wait_for_server()
print("client is on")
rate = rospy.Rate(10) # 1hz
#tell the action client that we want to spin a thread by default
self.Pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.Servo_pub = rospy.Publisher('/servo_control', String, queue_size=10)
while not rospy.is_shutdown():
rospy.Subscriber('/outdoor_waypoint_nav/odometry/filtered',Odometry, self.robotPoseSubscriber)
rospy.Subscriber('/rover_state_topic',StateMsg, self.stateSubscriber)
rospy.Subscriber('/stage_counter_topic', String, self.stageSubscriber)
rospy.Subscriber('/px_coordinates', String, self.artag_Subscriber)
rospy.Subscriber('px_coordinates1', String, self.artag_Subscriber1)
print(self.state.state)
#rospy.Subscriber('/move_base/result',MoveBaseActionResult, self.moveSubscriber)
#print(self.state.state)
if(self.state.state==self.state.FIND_ARTAG):
print("searching")
self.look_around()
servo_rotated = True
self.look_around()
servo_rotated = True
rotate(180)
if(self.state.state==self.state.FIND_ARTAG):
self.go_forward()
if(self.state.state==self.state.FIND_ARTAG):
self.rotate(90)
if(self.sc >= 4 and self.left == True and self.right == False):
self.go_forward()
self.rotate(-90)
self.go_forward()
self.rotate(90)
if(self.sc >= 4 and self.right == True and self.left == False):
self.go_forward()
self.rotate(90)
self.go_forward()
self.rotate(-90)
else:
print("waiting")
rate.sleep()
def __init__(self):
rospy.init_node('ball_search', anonymous=False)
self.currPosX = 0
self.currPosY = 0
self.currPosZ = 0
self.yaw = 0
self.startMsg = "s10f"
self.stopMsg = "s01f"
self.rotate_once = 1 # deleted in code
# if servo completed its rotation this will be true
self.send_once = 1
self.R = 0.5
self.ball_is_found = 0
self.dir = 1
self.sangle = 0 #servo angle
self.sc = None
self.left = None #left artag
self.right = None #right artag
self.rotate_done = None
self.half_rotate = False
self.state = StateMsg()
#self.donedone = Servocamera.done_servo_rotation()
print("waiting move base client...")
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.client.wait_for_server()
print("client is on")
rate = rospy.Rate(10) # 1hz
#tell the action client that we want to spin a thread by default
self.Pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.Servo_pub = rospy.Publisher('/servo_control',
String,
queue_size=10)
self.rover_rotated = False
self.servo_rotating = False
self.servo_rotated = False
self.servo_rotation_count = 0
while not rospy.is_shutdown():
rospy.Subscriber('/outdoor_waypoint_nav/odometry/filtered',
Odometry, self.robotPoseSubscriber)
rospy.Subscriber('/rover_state_topic', StateMsg,
self.stateSubscriber)
rospy.Subscriber('/stage_counter_topic', String,
self.stageSubscriber)
rospy.Subscriber('/servo_angle', String, self.angleSubscriber)
#rospy.Subscriber('/imu/data', Imu, self.imuSubscriber)
#print(self.state.state)
#print("rotate_done:", self.rotate_done)
#rospy.Subscriber('/move_base/result', MoveBaseActionResult, self.moveSubscriber) ##
#print(self.state.state)
if (self.state.state == self.state.FIND_ARTAG):
rospy.Subscriber('/px_coordinates', String,
self.artag_Subscriber)
rospy.Subscriber('/px_coordinates1', String,
self.artag_Subscriber1)
rospy.Subscriber('/servodone', String,
self.done_rotate_Subscriber)
print("searching")
#print("servo_rotated: ", self.servo_rotated)
print("servo rotating: ", self.servo_rotating)
print("servo_rotation_count: ", self.servo_rotation_count)
if self.servo_rotating == False:
if self.servo_rotation_count == 0:
self.start_servo_rotation()
self.servo_rotating = True
print("mgbncmb")
continue
if self.rover_rotated == False and self.servo_rotation_count == 1:
self.rotate(90)
time.sleep(15)
self.servo_rotated = False
self.rover_rotated = True
print("rover rotated")
if (self.servo_rotated == False
and self.rover_rotated == True):
self.start_servo_rotation()
time.sleep(5)
continue
if (self.rover_rotated == True
and self.servo_rotation_count == 2):
print("ZIGZAG veya SALYANGOZ")
#print("jnbxfjbnfxjbmnbjnbjxnbjxfnbkxjnb jfnbjkfnbkjnbkjcvnbjcnbkjbnkcnbkcngbkdghbkjdnbfhnmvnbjkghjfcnbfjkfhgbfjvb gjdgbhdjkdvbngdjmvnbgdjmvdb mvfsmycebntjtghekjgtnbwtkxfwnbwdjkwhnbw jtmxvcn btwjcmtnbw tjcwvnb tjwdnhtjkbw nvcwtwtwttery")
self.go_forward()
self.rotate(90)
if (self.sc >= 4 and self.left == True
and self.right == False):
self.go_forward()
self.rotate(-90)
self.go_forward()
self.rotate(90)
if (self.sc >= 4 and self.right == True
and self.left == False):
self.go_forward()
self.rotate(90)
self.go_forward()
self.rotate(-90)
self.rover_rotated = False
self.servo_rotation_count = 3
self.servo_rotated = False
elif (self.state.state == self.state.REACH_ARTAG
or self.state.state == self.state.APPROACH):
print("REACH_ARTAG!!!!!!!")
else:
print("waiting")
rate.sleep()
def __init__(self):
rospy.init_node('approach', anonymous=False)
self.currPosX=0
self.currPosY=0
self.currPosZ=0
self.yaw=0
self.bearingToartag=0.0
self.bearingToartag_old=0.0
self.counter=0
self.send_once=1
self.artag_is_found=0
self.msg="-"
self.state=StateMsg()
self.move_msg=MoveBaseActionResult()
self.twist = Twist()
self.client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
print("waiting client server")
self.client.wait_for_server()
print(" client is on")
rate = rospy.Rate(10) # 10hz
#tell the action client that we want to spin a thread by default
self.Pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) #Publish Nav Goal to ROS topic
self.Pub2=rospy.Publisher('/artag_reach_topic',String,queue_size=10) #Publisher oluşturduk
done_topic = rospy.get_param('RoverSmach20/sub_topics/sub_done_approach', '/done_topic')
#self.count = 0
self.done_pub = rospy.Publisher(self.done_topic, String, queue_size = 10)
while not rospy.is_shutdown():
rospy.Subscriber('/outdoor_waypoint_nav/odometry/filtered',Odometry, self.robotPoseSubscriber)
rospy.Subscriber('/rover_state_topic',RoverStateMsg, self.stateSubscriber)
rospy.Subscriber('/stage_counter_topic', String, self.stagec_callback)
rospy.Subscriber('/bearing_to_ball',String, self.ballYawSubscriber)
rospy.Subscriber('/move_base/result',MoveBaseActionResult, self.moveSubscriber)
print(str(self.state.state))
#self.msg=self.ballYawSubscriber
'''self.state.state=self.state.REACH_IMAGE ''' #changed
if(self.state.state==self.state.REACH_IMAGE or self.state.state==self.state.APPROACH):
#print(self.state.state) #eklendi
if(self.msg=="-"): #(self.msg=="-")
print("artag is not found")
twist_empty=Twist()
self.Pub.publish(twist_empty)
self.Pub2.publish("0") #eklendi
else:
print(self.msg) #self.msg
bear = abs(float(self.msg))
self.bearingToartag= float(self.msg)*pi /180
self.Pub2.publish("0") #eklendi
if bear> 5:
self.rotate_to_ball_2()
elif bear <= 5:
self.twist.angular.z=0
self.Pub.publish(self.twist)
self.Pub2.publish("0") #eklendi
#if(self.count<3):#3
if(self.sc ==3):
self.go_forward_for3()
self.done_topic.publish("1")
if(self.sc >= 4):
self.go_forward()
self.done_topic.publish("1")
while not rospy.is_shutdown():
print("Succesful")
self.Pub2.publish("1")
def __init__(self):
rospy.init_node('approach', anonymous=False)
self.currPosX=0
self.currPosY=0
self.currPosZ=0
self.yaw=0
self.bearingToartag=0.0
self.bearingToartag_old=0.0
self.counter=0
self.send_once=1
self.artag_is_found=0
self.sc = 3
self.sum = 0
self.average = 0
self.bearing_counter = 0 #new
self.approaching = False
self.msg="-"
self.state=StateMsg()
self.move_msg= MoveBaseActionResult()
self.twist = Twist()
self.client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
print("waiting client server")
self.client.wait_for_server()
print(" client is on")
rate = rospy.Rate(10) # 10hz
#tell the action client that we want to spin a thread by default
self.Pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) #Publish Nav Goal to ROS topic
self.Pub2=rospy.Publisher('/artag_reach_topic',String,queue_size=10) #Publisher oluşturduk
self.done_topic = rospy.get_param('RoverSmach20/sub_topics/sub_done_approach', '/done_topic')
#self.count = 0
self.done_pub = rospy.Publisher(self.done_topic, String, queue_size = 10)
self.middle_point = []
self.artag_search_finish = False
while not rospy.is_shutdown():
rospy.Subscriber('/outdoor_waypoint_nav/odometry/filtered',Odometry, self.robotPoseSubscriber)
rospy.Subscriber('/rover_state_topic',StateMsg, self.stateSubscriber)
rospy.Subscriber('/stage_counter_topic', String, self.stagec_callback)
rospy.Subscriber('/bearing_to_artag',String, self.artagYawSubscriber)
rospy.Subscriber('/artag_search_done',String, self.artagSearchSubscriber)
#rospy.Subscriber('/move_base/result',MoveBaseActionResult, self.moveSubscriber)
print(self.artag_search_finish)
#self.msg=self.artagYawSubscriber
'''self.state.state=self.state.REACH_IMAGE ''' #changed
if self.artag_search_finish == True:#if(self.state.state==self.state.REACH_ARTAG or self.state.state==self.state.APPROACH):
#print(self.state.state) #eklendi
#print(self.state)
if(self.msg=="-"):
if self.approaching == False:
print("artag is not found")
twist_empty=Twist()
self.Pub.publish(twist_empty)
self.Pub2.publish("0") #eklendi
else:
self.approaching = False
self.go_forward_for3()
else:
'''for i in range(10):
rospy.Subscriber('/bearing_to_artag',String, self.artagYawSubscriber)
self.middle_point.append(int(self.msg))
self.sum = self.sum + middle_point[i]
self.average = self.sum/10
print(middle_point[i])'''
print(self.msg) #self.msg
#bear = abs(float(self.msg))
bear = abs(float(self.msg))#abs(self.average)
self.bearingToartag= float(self.msg)*pi /180 # AVERAGE REMOVED
self.Pub2.publish("0") #eklendi
if bear> 5 and self.bearing_counter == 0:
self.approaching = False
self.rotate_to_ball_2()
elif bear <= 5:
self.approaching = True
self.twist.angular.z=0
self.Pub.publish(self.twist)
self.Pub2.publish("0") #eklendi
#if(self.count<3):#3
if(self.sc ==3):
self.go_forward()
self.done_pub.publish("1")
if(self.sc >= 4):
self.go_forward()
self.done_pub.publish("1")
while not rospy.is_shutdown():
print("Succesful")
#self.Pub2.publish("1")
break
#degistirildi
self.bearing_counter += 1