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 = RoverStateMsg()
def __init__(self):
smach.State.__init__(self, outcomes=['SUCCESS', 'FAIL', 'REPEAT'])
self.findImageTimeout = status_handler.findImageTimeout
self.timeoutCounter = 0
self.rate = rospy.Rate(1)
self.stateMsg = RoverStateMsg()
self.goBack = status_handler.goBack
self.goBack = False
def __init__(self):
rospy.init_node('ball_handler', anonymous=False)
self.currPosX = 0
self.currPosY = 0
self.currPosZ = 0
self.yaw = 0
self.bearingToball = 0.0
self.bearingToball_old = 0.0
self.counter = 0
self.send_once = 1
self.ball_is_found = 0
self.msg = "-"
self.state = RoverStateMsg()
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.count = 0
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('/bearing_to_ball', String,
self.ballYawSubscriber)
rospy.Subscriber('/move_base/result', MoveBaseActionResult,
self.moveSubscriber)
print(str(self.state.state))
if (self.state.state == self.state.REACH_IMAGE):
if (self.msg == "-"):
print("ball is not found")
twist_empty = Twist()
self.Pub.publish(twist_empty)
else:
bear = abs(float(self.msg))
self.bearingToball = float(self.msg) * pi / 180
print(self.msg)
if bear > 5:
self.rotate_to_ball_2()
elif bear <= 5:
self.twist.angular.z = 0
self.Pub.publish(self.twist)
if (self.count < 3):
self.go_forward()
else:
while not rospy.is_shutdown():
print("Succesful")
def __init__(self):
rospy.init_node('ball_search', anonymous=False)
self.rotate_once = 1
self.dist = 1
self.state = RoverStateMsg()
self.go_back_counter = 0
self.firstPosX = 0
self.firstPosY = 0
self.state.state = self.state.FIND_IMAGE
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('rover_navigation/cmd_vel',
Twist,
queue_size=10)
while not rospy.is_shutdown():
rospy.Subscriber('/rover_state_topic', RoverStateMsg,
self.stateSubscriber)
#rospy.Subscriber('/move_base/result',MoveBaseActionResult, self.moveSubscriber)
#print(self.state.state)
if (self.state.state == self.state.FIND_IMAGE):
if (self.rotate_once == 1):
print("searching")
self.go_forward()
self.rotate(90)
self.rotate(90)
self.rotate(90)
self.rotate(90)
self.rotate_once = 0
if (self.state.state == self.state.FIND_IMAGE):
self.go_forward()
self.go_back_counter += 1
if (self.go_back_counter > 8):
print(str(self.go_back_counter))
self.go_back_counter = 0
self.rotate_once = 1
self.send_once = 1
self.do_once = 1
self.go_to_point(self.firstPosX, self.firstPosY)
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.rotate_once = 1
self.send_once = 1
self.R = 0.5
self.ball_is_found = 0
self.state = RoverStateMsg()
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)
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('/move_base/result',MoveBaseActionResult, self.moveSubscriber)
#print(self.state.state)
if (self.state.state == self.state.FIND_IMAGE):
if (self.rotate_once == 1):
print("searching")
self.go_forward()
self.rotate(180)
self.rotate(-180)
self.rotate_once = 0
if (self.state.state == self.state.FIND_IMAGE):
self.go_forward()
if (self.state.state == self.state.FIND_IMAGE):
self.rotate(90)
else:
print("waiting")
rate.sleep()
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 = RoverStateMsg()
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 = RoverStateMsg()
def __init__(self):
smach.State.__init__(self, outcomes=['SUCCESS', 'REPEAT', 'FAIL'])
self.rate = rospy.Rate(1)
self.stateMsg = RoverStateMsg()