| |
|---|
| | # publishes : decided mode |
|---|
| | self.pub_decided_mode = rospy.Publisher('/core/decided_mode', UInt8, queue_size=1) |
|---|
| | self.InvokedObject = Enum('InvokedObject', 'traffic_light traffic_sign') |
|---|
| | self.TrafficSign = Enum('TrafficSign', 'intersection left right construction stop parking tunnel') |
|---|
| | # self.CurrentMode = Enum('CurrentMode', 'idle lane_following traffic_light intersection construction parking level_crossing tunnel') |
|---|
| | self.CurrentMode = Enum('CurrentMode', 'idle lane_following traffic_light intersection construction parking level_crossing tunnel lane1 lane2 lane3 lane4 lane5') |
|---|
| | self.fnInitMode() |
|---|
| | self.current_mode = self.CurrentMode.lane_following.value |
|---|
| | self.fnPublishMode() |
|---|
| | self.countintersection = 0 |
|---|
| | # Invoke if traffic light is detected |
|---|
| | def cbInvokedByTrafficLight(self, invoke_type_msg): |
|---|
| | rospy.loginfo("mode Invoke Traffic light detected") |
|---|
| |
|---|
| | self.current_mode = self.CurrentMode.lane_following.value |
|---|
| | rospy.loginfo("mode Init Mode") |
|---|
| | self.fnPublishMode() |
|---|
| | |
|---|
| | def fnInitMode(self): # starts only when the program is started initially or any mission is completed |
|---|
| | self.current_mode = self.CurrentMode.lane_following.value |
|---|
| | self.fnPublishMode() |
|---|
| | def fnDecideMode(self, invoked_object, msg_data): # starts only when the traffic sign / traffic light is detected & current_mode is lane_following |
|---|
| | rospy.loginfo("mode ------------------------------") |
|---|
| | rospy.loginfo("mode current_mode=%d",self.current_mode) |
|---|
| | rospy.loginfo("mode invoked_object=%d",invoked_object) |
|---|
| |
|---|
| | elif invoked_object == self.InvokedObject.traffic_sign.value: |
|---|
| | if msg_data.data == self.TrafficSign.intersection.value: |
|---|
| | self.countintersection = 0 |
|---|
| | rospy.loginfo("mode detect sign : intersection") |
|---|
| | self.current_mode = self.CurrentMode.lane2.value |
|---|
| | self.current_mode = self.CurrentMode.lane1.value |
|---|
| | elif msg_data.data == self.TrafficSign.left.value: |
|---|
| | self.countintersection = self.countintersection - 1 |
|---|
| | rospy.loginfo("mode detect sign : intersection left %d",self.countintersection) |
|---|
| | self.current_mode = self.CurrentMode.intersection.value |
|---|
| | self.current_mode = self.CurrentMode.lane1.value |
|---|
| | elif msg_data.data == self.TrafficSign.right.value: |
|---|
| | self.countintersection = self.countintersection + 1 |
|---|
| | rospy.loginfo("mode detect sign : inteersection right %d",self.countintersection) |
|---|
| | self.current_mode = self.CurrentMode.intersection.value |
|---|
| | self.current_mode = self.CurrentMode.lane1.value |
|---|
| | elif msg_data.data == self.TrafficSign.construction.value: |
|---|
| | rospy.loginfo("mode detect sign : construction") |
|---|
| | self.current_mode = self.CurrentMode.construction.value |
|---|
| | elif msg_data.data == self.TrafficSign.parking.value: |
|---|
| |
|---|
| | self.current_mode = self.CurrentMode.level_crossing.value |
|---|
| | elif msg_data.data == self.TrafficSign.tunnel.value: |
|---|
| | rospy.loginfo("mode detect sign : tunnel") |
|---|
| | self.current_mode = self.CurrentMode.tunnel.value |
|---|
| | if self.current_mode == self.CurrentMode.intersection.value and abs(self.countintersection) > 2: |
|---|
| | if self.current_mode == self.CurrentMode.lane1.value and abs(self.countintersection) > 2: |
|---|
| | rospy.sleep(1) |
|---|
| | rospy.loginfo("mode intersection and count intersection") |
|---|
| | self.current_mode = self.CurrentMode.construction.value |
|---|
| | else: |
|---|
| |
|---|
| | |
