def __init__(self):
self.actual = FSMState()
self.actual.state = FSMState.LANE_FOLLOWING
self.actual.header.stamp = rospy.Time.now()
self.in_lane = False
self.at_stop_line = False
self.intersection_go = False
self.intersection_done = False
self.vehicle_detected = False
self.obstacle_found = False
# Save the name of the node
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initialzing." %(self.node_name))
# Setup publishers
self.pub_topic_mode = rospy.Publisher("~mode",FSMState, queue_size=1, latch=True)
self.pub_topic_intersection_done = rospy.Publisher("~intersection_done",Bool, queue_size=1)
self.pub_topic_intersection_go = rospy.Publisher("~intersection_go",Bool, queue_size=1)
self.pub_topic_april_tags_switch = rospy.Publisher("apriltags/switch",BoolStamped, queue_size=1, latch=True)
# Setup subscribers
self.sub_topic_in_lane = rospy.Subscriber("~lane_pose", LanePose, self.cbInLane, queue_size=1)
self.sub_topic_at_stop_line = rospy.Subscriber("~stop_line_reading", StopLineReading, self.cbAtStopLine, queue_size=1)
self.sub_topic_intersection_go = rospy.Subscriber("~clearance_to_go", CoordinationClearance, self.cbIntersectionGo, queue_size=1)
self.sub_topic_intersection_done = rospy.Subscriber("~intersection_done", BoolStamped, self.cbIntersectionDone, queue_size=1)
self.sub_topic_vehicle_detected = rospy.Subscriber("~vehicle_detected",Bool,self.cbVehicleDetected, queue_size=1)
self.sub_topic_obstacle_found = rospy.Subscriber("~object_too_close", BoolStamped, self.cbObstacleFound, queue_size=1)
# Read parameters
rospy.loginfo("[%s] Initialzed." %(self.node_name))
def publish(self):
if self.mode is not None:
self.mode_pub.publish(FSMState(state=self.mode))
self.signals_detection_pub.publish(
SignalsDetection(front=self.opposite_veh,
right=self.right_veh,
traffic_light_state=self.traffic_light))
def __init__(self):
self.node_name = rospy.get_name()
# Build transition dictionray
self.states_dict = rospy.get_param("~states",{})
# Validate state and global transitions
if not self._validateStates(self.states_dict):
rospy.signal_shutdown("[%s] Incoherent definition." %self.node_name)
return
# Load global transitions
self.global_transitions_dict = rospy.get_param("~global_transitions", {})
if not self._validateGlobalTransitions(self.global_transitions_dict,self.states_dict.keys()):
rospy.signal_shutdown("[%s] Incoherent definition." %self.node_name)
return
# Setup initial state
self.state_msg = FSMState()
self.state_msg.state = rospy.get_param("~initial_state","")
self.state_msg.header.stamp = rospy.Time.now()
# Setup publisher and publish initial state
self.pub_state = rospy.Publisher("~mode",FSMState,queue_size=1,latch=True)
# Provide service
self.srv_state = rospy.Service("~set_state",SetFSMState,self.cbSrvSetState)
# Construct publishers
self.pub_dict = dict()
nodes = rospy.get_param("~nodes")
self.active_nodes = None
for node_name, topic_name in nodes.items():
self.pub_dict[node_name] = rospy.Publisher(topic_name, BoolStamped, queue_size=1, latch=True)
# print self.pub_dict
# Process events definition
param_events_dict = rospy.get_param("~events",{})
# Validate events definition
if not self._validateEvents(param_events_dict):
rospy.signal_shutdown("[%s] Invalid event definition." %self.node_name)
return
self.sub_list = list()
self.event_trigger_dict = dict()
for event_name, event_dict in param_events_dict.items():
topic_name = event_dict["topic"]
msg_type = event_dict["msg_type"]
self.event_trigger_dict[event_name] = event_dict["trigger"]
# TODO so far I can't figure out how to put msg_type instead of BoolStamped.
# importlib might help. But it might get too complicated since different type
self.sub_list.append(rospy.Subscriber("%s"%(topic_name), BoolStamped, self.cbEvent, callback_args=event_name))
rospy.loginfo("[%s] Initialized." %self.node_name)
# Publish initial state
self.publish()
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 publish(self):
self.mode_pub.publish(FSMState(state=self.mode))
self.intersection_pub.publish(
IntersectionDetection(type=self.intersection))
self.traffic_light_pub.publish(
TrafficLightDetection(color=self.traffic_light))
self.right_veh_pub.publish(VehicleDetection(detection=self.right_veh))
self.opposite_veh_pub.publish(
VehicleDetection(detection=self.opposite_veh))
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing " % (self.node_name))
# self.mode_msg = ControlMode()
self.mode_msg = FSMState()
#self.mode_msg.header.stamp = rospy.Time.now()
# self.mode_msg.mode = ControlMode.LANE_FOLLOWING
self.mode_msg.state = FSMState.LANE_FOLLOWING
self.cmd_dict = {}
# self.cmd_dict[ControlMode.LANE_FOLLOWING] = None
# self.cmd_dict[ControlMode.INTERSECTION_CONTROL] = None
# self.cmd_dict[ControlMode.COORDINATION_CONTROL] = None
self.cmd_dict[FSMState.LANE_FOLLOWING] = None
self.cmd_dict[FSMState.INTERSECTION_CONTROL] = None
self.cmd_dict[FSMState.COORDINATION] = None
self.mode_name_dict = {}
# self.mode_name_dict[ControlMode.LANE_FOLLOWING] = "LANE_FOLLOWING"
# self.mode_name_dict[ControlMode.INTERSECTION_CONTROL] = "INTERSECTION_CONTROL"
# self.mode_name_dict[ControlMode.COORDINATION_CONTROL] = "COORDINATION_CONTROL"
self.mode_name_dict[FSMState.LANE_FOLLOWING] = "LANE_FOLLOWING"
self.mode_name_dict[
FSMState.INTERSECTION_CONTROL] = "INTERSECTION_CONTROL"
self.mode_name_dict[FSMState.COORDINATION] = "COORDINATION_CONTROL"
# Setup publishers
self.pub_wheels_cmd = rospy.Publisher("~wheels_cmd",
WheelsCmdStamped,
queue_size=1)
# Setup subscribers
self.sub_mode = rospy.Subscriber("~mode",
FSMState,
self.cbMode,
queue_size=1)
self.sub_lane = rospy.Subscriber("~wheels_cmd_lane",
WheelsCmdStamped,
self.cbWheelsCmd,
queue_size=1,
callback_args=FSMState.LANE_FOLLOWING)
self.sub_interestion = rospy.Subscriber(
"~wheels_cmd_intersection",
WheelsCmdStamped,
self.cbWheelsCmd,
queue_size=1,
callback_args=FSMState.INTERSECTION_CONTROL)
self.sub_coordination = rospy.Subscriber(
"~wheels_cmd_coordination",
WheelsCmdStamped,
self.cbWheelsCmd,
queue_size=1,
callback_args=FSMState.COORDINATION)
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 __init__(self):
self.node_name = rospy.get_name()
# Load transition dictionray
self.states_dict = rospy.get_param("~states")
# Validate state transitions
if not self.validateStates(self.states_dict):
# Print error message and shutdown incoherent
rospy.signal_shutdown("[%s] Incoherent definition." %
self.node_name)
return
# Setup initial state
self.state_msg = FSMState()
self.state_msg.state = rospy.get_param("~initial_state")
self.state_msg.header.stamp = rospy.Time.now()
# Setup publisher and publish initial state
self.pub_state = rospy.Publisher("~mode",
FSMState,
queue_size=1,
latch=True)
# Provide service
self.srv_state = rospy.Service("~set_state", SetFSMState,
self.cbSrvSetState)
# Construct publishers
self.pub_dict = dict()
nodes = rospy.get_param("~nodes")
self.active_nodes = None
for node_name, topic_name in nodes.items():
self.pub_dict[node_name] = rospy.Publisher(topic_name,
BoolStamped,
queue_size=1,
latch=True)
# Construct subscribers
param_events_dict = rospy.get_param("~events")
self.sub_list = list()
for event_name, topic_name in param_events_dict.items():
self.sub_list.append(
rospy.Subscriber("%s" % (topic_name),
BoolStamped,
self.cbEvent,
callback_args=event_name))
rospy.loginfo("[%s] Initialized." % self.node_name)
# Publish initial state
self.publish()
def __init__(self):
# Save the name of the node
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initialzing." %(self.node_name))
# Build transition dictionary
self.states_dict = rospy.get_param("~states", {})
# Validate state and global transitions
if not self._validateStates(self.states_dict):
rospy.signal_shutdown("[%s] Incoherent definition." %self.node_name)
# Load global transitions
self.global_transitions_dict = rospy.get_param("~global_transitions", {})
if not self._validateGlobalTransitions(self.global_transitions_dict,self.states_dict.keys()):
rospy.signal_shutdown("[%s] Incoherent definition." %self.node_name)
return
# Setup initial state
self.state_msg = FSMState()
self.state_msg.state = rospy.get_param("~initial_state", "")
self.state_msg.header.stamp = rospy.Time.now()
# Setup publishers
self.pub_state = rospy.Publisher("~mode",FSMState, queue_size=1, latch=True)
# Construct publishers
self.pub_dict = dict()
nodes = rospy.get_param("~nodes")
self.active_nodes = None
for node_name, topic_name in nodes.items():
self.pub_dict[node_name] = rospy.Publisher(topic_name, BoolStamped, queue_size=1, latch=True)
# Process events definition
param_events_dict = rospy.get_param("~events", {})
if not self._validateEvents(param_events_dict):
rospy.signal_shutdown("[%s] Invalid event definition." %self.node_name)
return
# Setup subscriber
self.sub_list = list()
self.event_trigger_dict = dict()
for event_name, event_dict in param_events_dict.items():
topic_name = event_dict["topic"]
msg_type = event_dict["msg_type"]
self.event_trigger_dict[event_name] = event_dict["trigger"]
self.sub_list.append(rospy.Subscriber("%s"%(topic_name), BoolStamped, self.cbEvent, callback_args=event_name))
rospy.loginfo("[%s] Initialzed." %(self.node_name))
# Publish initial state
self.publish()
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()
def __init__(self):
self.actual = FSMState()
self.actual.state = self.actual.LANE_FOLLOWING
self.in_lane = False
self.at_stop_line = False
self.intersection_go = False
self.intersection_done = False
# Save the name of the node
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initialzing." % (self.node_name))
# Setup publishers
self.pub_topic_mode = rospy.Publisher("~mode",
FSMState,
queue_size=1,
latch=True)
self.pub_topic_intersection_done = rospy.Publisher(
"~intersection_done", Bool, queue_size=1)
self.pub_topic_intersection_go = rospy.Publisher("~intersection_go",
Bool,
queue_size=1)
# Setup subscribers
self.sub_topic_in_lane = rospy.Subscriber("~in_lane",
Bool,
self.cbInLane,
queue_size=1)
self.sub_topic_at_stop_line = rospy.Subscriber("~at_stop_line",
Bool,
self.cbAtStopLine,
queue_size=1)
self.sub_topic_intersection_go = rospy.Subscriber(
"~intersection_go", Bool, self.cbIntersectionGo, queue_size=1)
self.sub_topic_intersection_done = rospy.Subscriber(
"~intersection_done", Bool, self.cbIntersectionDone, queue_size=1)
# Read parameters
self.pub_timestep = self.setupParameter("~pub_timestep", 1.0)
# Create a timer that calls the cbTimer function every 1.0 second
#self.timer = rospy.Timer(rospy.Duration.from_sec(self.pub_timestep),self.cbTimer)
rospy.loginfo("[%s] Initialzed." % (self.node_name))
self.rate = rospy.Rate(30) # 10hz
def publish_mode(self):
if self.mode is not None:
self.mode_pub.publish(FSMState(state=self.mode))