def cbFSMState(self, msg):
self.state = msg.state
#print 'state skrg {0}'.format(self.sub_state_mode)
#print 'ngulang'
while self.state == "INTERSECTION_CONTROL": #and len(trafficlights2) > 0:
#print 'state skrg 123 {0}'.format(self.sub_state_mode)
#print 'lewat detect merah'
if self.yellow_detected != 0 or self.green_detected != 0:
print 'Kuning{0}'.format(self.yellow_detected)
print 'Hijau{0}'.format(self.green_detected)
msg_state = FSMState()
msg_state.header.stamp = msg.header.stamp
msg_state.state = "LANE_FOLLOWING_AVOID"
print 'state berubah jadi {0}'.format(msg_state)
self.pub_state_mode.publish(msg_state)
if self.red_detected != 0 and (self.yellow_detected == 0
or self.green_detected == 0):
print 'Merah{0}'.format(self.red_detected)
def pose_callback(self, msg):
if self.direction is None:
return
position = msg.pose.pose.position
rot = msg.pose.pose.orientation
euler = euler_from_quaternion([rot.x, rot.y, rot.z, rot.w])
pose = (position.x, position.y, euler[2])
# Check switching to lane following - Right turn.
if((self.direction == "R" and pose[2]<(-np.pi/2 + np.pi/20)) \
or (self.direction == "L" and (pose[2] > (np.pi/2 - np.pi/20) or pose[1]>0.2)) \
or (self.direction == "S" and pose[0]>0.2)):
fsm_msg = FSMState()
fsm_msg.state = "LANE_FOLLOWING"
self.fsm_pub.publish(fsm_msg)
switch_msg = Bool()
switch_msg.data = False
self.int_switch_pub.publish(switch_msg)
switch_msg = BoolStamped()
switch_msg.data = True
self.lc_switch_pub.publish(switch_msg)
rospy.logwarn("Switched back to lane following")
def test_turn(self):
self.setup()
mode = FSMState()
mode.state = "INTERSECTION_CONTROL"
startTime = rospy.Time.now()
endTime = startTime + rospy.Duration.from_sec(0.2)
while rospy.Time.now() < endTime:
self.publish_mode.publish(mode)
#need to wait 1 second before publishing new turn type
rospy.sleep(1)
if self.type.lower() == 'left':
self.turn_left_serv()
elif self.type.lower() == 'right':
self.turn_right_serv()
else:
self.turn_forward_serv()
count = 0
while not self.done:
count += 1
rospy.loginfo("Waiting for intersection_done")
rospy.sleep(0.2)
if count > 50:
self.assertEqual(True, False,
"Timed out waiting for intersection")
return
stop = Twist2DStamped()
stop.v = 0
stop.omega = 0
startTime = rospy.Time.now()
end_time = startTime + rospy.Duration.from_sec(1)
rospy.loginfo("start_time = %s, end_time=%s" % (startTime, end_time))
while rospy.Time.now() < end_time:
self.pub_wheels.publish(stop)
rospy.sleep(0.1)
self.final = self.lane
self.calculate()
def timer_callback(self, event):
# If direction unknown ping intersection switch for camera.
if not self.direction_known:
fsm_msg = FSMState()
fsm_msg.state = "NORMAL_JOYSTICK_CONTROL"
self.fsm_pub.publish(fsm_msg)
switch_msg = Bool()
switch_msg.data = True
self.int_switch_pub.publish(switch_msg)
switch_msg = BoolStamped()
switch_msg.data = False
self.lc_switch_pub.publish(switch_msg)
rospy.loginfo("Switched to intersection navigation")
return
# Else publish direction and intersection type.
itype_msg = String()
itype_msg.data = self.itype
self.itype_pub.publish(itype_msg)
dir_msg = String()
dir_msg.data = self.direction
self.direction_pub.publish(dir_msg)
idmessage = Int16MultiArray(data=self.tagids)
self.tagid_pub.publish(idmessage)
def test_turn(self):
self.setup()
mode = FSMState()
mode.state = "INTERSECTION_CONTROL"
startTime = rospy.Time.now()
endTime = startTime + rospy.Duration.from_sec(0.2)
while rospy.Time.now() < endTime:
self.publish_mode.publish(mode)
#need to wait 1 second before publishing new turn type
rospy.sleep(1)
if self.type.lower() == 'left':
self.turn_left_serv()
elif self.type.lower() == 'right':
self.turn_right_serv()
else:
self.turn_forward_serv()
count = 0
while not self.done:
count += 1
rospy.loginfo("Waiting for intersection_done")
rospy.sleep(0.2)
if count > 50:
self.assertEquals(True, False, "Timed out waiting for intersection")
return
stop = Twist2DStamped()
stop.v = 0
stop.omega = 0
startTime = rospy.Time.now()
end_time = startTime + rospy.Duration.from_sec(1)
rospy.loginfo("start_time = %s, end_time=%s" %(startTime, end_time))
while rospy.Time.now() < end_time:
self.pub_wheels.publish(stop)
rospy.sleep(0.1)
self.final = self.lane
self.calculate()