def action_close_dome(self):
roof_msg = roof()
# STARTING
# OPENING
self.highPin(self.open_pin)
msg = self.readRasperryStatus()
while msg["status"] != "CLOSE" and msg["status"] != "ABORTED":
if self.roof_status.state != self.STATES["EMERGENCY_STOP"]:
roof_msg = self.createRoofMSG(msg)
self.pub.publish(roof_msg)
self.rate.sleep()
else:
rospy.logwarn(
"Due an Emergency stop message, the close of the dome has been stopped!"
)
emergency_stop = True
break
msg = self.readRasperryStatus()
rospy.loginfo("Roof status: " + msg["status"])
if emergency_stop == False:
# getting the last message
msg = msg = self.readRasperryStatus()
rospy.loginfo(
"Roof final state = [action performed: OPENING | status:" +
msg["status"] + " ]")
else:
rospy.logwarn("Executing emergency stop")
return roof_msg
def action_open(self):
roof_msg = roof()
msg = self.arduino.write("openDome")
msg = self.readLineFromArduino()
msg = self.parseArduinoLecture(msg)
emergency_stop = False
while msg["status"] != "FINISH" and msg["status"] != "ABORTED":
if self.roof_status.state != self.STATES["EMERGENCY_STOP"]:
roof_msg = self.createRoofMSG(msg)
self.pub.publish(roof_msg)
self.rate.sleep()
else:
rospy.logwarn(
"Due an Emergency stop message, the opening of the dome has been stopped!"
)
emergency_stop = True
break
msg = self.readLineFromArduino()
msg = self.parseArduinoLecture(msg)
rospy.loginfo("Roof status: " + msg["status"])
if emergency_stop == False:
# getting the last message
msg = self.readLineFromArduino().split(":")
rospy.loginfo("Roof final state = [action performed:" + msg[0] +
" | status:" + msg[1] + " | next action:" + msg[2] +
"]")
else:
rospy.logwarn("Executing emergency stop")
return roof_msg
def createRoofMSG(self,msgs_list):
roof_msg = roof()
try:
roof_msg.open_button = self.str2bool(msgs_list["open_button"])
except KeyError, e:
rospy.loginfo("Unable to find key open_button from msg received by Arduino")
roof_msg.open_button = False
def action_open(self):
roof_msg = roof()
msg = self.arduino.write("openDome")
msg = self.readLineFromArduino()
msg = self.parseArduinoLecture(msg)
emergency_stop = False
while msg["status"] != "FINISH" and msg["status"] != "ABORTED":
if self.roof_status.state != self.STATES["EMERGENCY_STOP"]:
roof_msg = self.createRoofMSG(msg)
self.pub.publish(roof_msg)
self.rate.sleep()
else:
rospy.logwarn("Due an Emergency stop message, the opening of the dome has been stopped!")
emergency_stop = True
break;
msg = self.readLineFromArduino()
msg = self.parseArduinoLecture(msg)
rospy.loginfo("Roof status: " + msg["status"])
if emergency_stop == False:
# getting the last message
msg = self.readLineFromArduino().split(":")
rospy.loginfo("Roof final state = [action performed:" + msg[0] + " | status:" + msg[1] +" | next action:" + msg[2] + "]")
else:
rospy.logwarn("Executing emergency stop")
return roof_msg
def action_close_dome(self):
roof_msg = roof()
# STARTING
# OPENING
self.highPin(self.open_pin)
msg = self.readRasperryStatus()
while msg["status"] != "CLOSE" and msg["status"] != "ABORTED":
if self.roof_status.state != self.STATES["EMERGENCY_STOP"]:
roof_msg = self.createRoofMSG(msg)
self.pub.publish(roof_msg)
self.rate.sleep()
else:
rospy.logwarn("Due an Emergency stop message, the close of the dome has been stopped!")
emergency_stop = True
break;
msg = self.readRasperryStatus()
rospy.loginfo("Roof status: " + msg["status"])
if emergency_stop == False:
# getting the last message
msg = msg = self.readRasperryStatus()
rospy.loginfo("Roof final state = [action performed: OPENING | status:" + msg["status"] +" ]")
else:
rospy.logwarn("Executing emergency stop")
return roof_msg
def createRoofMSG(self, msgs_list):
roof_msg = roof()
try:
roof_msg.open_button = self.str2bool(msgs_list["open_button"])
except KeyError, e:
rospy.loginfo(
"Unable to find key open_button from msg received by Arduino")
roof_msg.open_button = False
def getInitialState(self): roof_initial_msg = roof() # This make a change on the pin status so that the interruption will be triggered and a message will be received by Serial port self.blinkGPIO() # raw lecture from the arduino hardware_info = self.readLineFromArduino() roof_initial_msg = self.createRoofMSG(self.parseArduinoLecture(hardware_info)) self.pub.publish(roof_initial_msg) return roof_initial_msg
def handle_open(self,req):
rospy.loginfo("TCS is requesting to open roof at speed: " + str(req.speed))
if self.mode == "LOCAL":
rospy.loginfo("System is local only, so is not possible to operate it remotely")
roof_msg = roof()
roof_msg.state = "System is local only, so is not possible to operate it remotely"
rospy.logwarn(roof_msg.state)
return roof_msg
# it is only possible to open if the status of the TCS is PARKED, CLOSED, ABORTED or STOP. If an emergency stop was pressed is because there is something or someone in danger and need to be phisically disabled.
if self.roof_status.state == self.STATES["PARKED"] or self.roof_status.state == self.STATES["CLOSED"] or self.roof_status.state == self.STATES["ABORTED"] or self.roof_status.state == self.STATES["STOP"]:
if self.roof_status.state == self.STATES["EMERGENCY_STOP"]: self.roof_status = self.last_roof_status
# wait until action is completed
roof_msg = self.action_open()
return roof_msg
else:
roof_msg = roof()
roof_msg.state = "Not possible to open due roof is in another state: " + self.roof_status.state
rospy.logwarn(roof_msg.state)
return roof_msg
def getInitialState(self):
roof_initial_msg = roof()
# This make a change on the pin status so that the interruption will be triggered and a message will be received by Serial port
self.blinkGPIO()
# raw lecture from the arduino
hardware_info = self.readLineFromArduino()
roof_initial_msg = self.createRoofMSG(
self.parseArduinoLecture(hardware_info))
self.pub.publish(roof_initial_msg)
return roof_initial_msg
def handle_close(self,req):
rospy.loginfo("TCS is requesting to close roof at speed: " + str(req.speed))
if self.mode == "LOCAL":
rospy.loginfo("System is local only, so is not possible to operate it remotely")
roof_msg = roof()
roof_msg.state = "System is local only, so is not possible to operate it remotely"
rospy.logwarn(roof_msg.state)
return roof_msg
#print "TCS is requesting to close roof at speed: ", req.speed
if self.roof_status.state == self.STATES["PARKED"] or self.roof_status.state == self.STATES["OPEN"] or self.roof_status.state == self.STATES["ABORTED"] or self.roof_status.state == self.STATES["STOP"]:
#if self.roof_status.state == self.STATES[8]: self.roof_status = self.last_roof_status
#lateral_door_msg = self.action_close_lateral_door()
#rospy.loginfo(lateral_door_msg)
roof_msg = self.action_close_dome()
return roof_msg
else:
roof_msg = roof()
roof_msg.state = "Not possible to close due roof is in another state: " + self.roof_status.state
rospy.logwarn(roof_msg.state)
return roof_msg
def handle_refresh_dome_status(self, req):
rospy.loginfo("A new request of refresh have been received" + str(req))
roof_msg = roof()
# if this is not running under Raspberry PI, will just write by the serial port the command 'status'
self.blinkGPIO()
msg = self.readLineFromArduino()
msg = self.parseArduinoLecture(msg)
roof_msg = self.createRoofMSG(msg)
self.pub.publish(roof_msg)
rospy.loginfo("Roof status: " + roof_msg.state)
return roof_msg
def handle_refresh_dome_status(self,req):
rospy.loginfo("A new request of refresh have been received" + str(req))
roof_msg = roof()
# if this is not running under Raspberry PI, will just write by the serial port the command 'status'
self.blinkGPIO()
msg = self.readLineFromArduino()
msg = self.parseArduinoLecture(msg)
roof_msg = self.createRoofMSG(msg)
self.pub.publish(roof_msg)
rospy.loginfo("Roof status: " + roof_msg.state)
return roof_msg
def createRoofMSG(self,msgs_list): roof_msg = roof() roof_msg.open_button = True if self.readPinValue(self.open_pin) == 1 else False roof_msg.close_button = True if self.readPinValue(self.close_pin) == 1 else False roof_msg.opening_sensor = True if self.readPinValue(self.opening_sensor_pin) else False roof_msg.closing_sensor = True if self.readPinValue(self.closing_sensor_pin) else False roof_msg.safety_sensor = True if self.mode == "LOCAL" else False roof_msg.meteorologic_sensor = True # TODO: Must be implemented or deleted roof_msg.state = msgs_list["status"] roof_msg.action = msgs_list["action"] return roof_msg
def createRoofMSG(self, msgs_list):
roof_msg = roof()
roof_msg.open_button = True if self.readPinValue(
self.open_pin) == 1 else False
roof_msg.close_button = True if self.readPinValue(
self.close_pin) == 1 else False
roof_msg.opening_sensor = True if self.readPinValue(
self.opening_sensor_pin) else False
roof_msg.closing_sensor = True if self.readPinValue(
self.closing_sensor_pin) else False
roof_msg.safety_sensor = True if self.mode == "LOCAL" else False
roof_msg.meteorologic_sensor = True # TODO: Must be implemented or deleted
roof_msg.state = msgs_list["status"]
roof_msg.action = msgs_list["action"]
return roof_msg
def action_close_lateral_door(self):
roof_msg = roof()
msg = self.arduino.write("closeLateralDoor")
msg = self.readLineFromArduino()
while msg != "FINISH":
if self.roof_status.state != self.STATES["EMERGENCY_STOP"]:
roof_msg = self.createRoofMSG("closeLateralDoor",msg,True,False,True,1)
self.pub.publish(roof_msg)
rospy.loginfo(msg)
# This frequency should be modified once the hardware is connected
r = rospy.Rate(0.5)
r.sleep()
else:
rospy.logwarn("Due an Emergency stop message, the opening of the lateral door has been stopped!")
break;
msg = self.readLineFromArduino()
rospy.loginfo("Lateral door status: " + msg)
# getting the last message
msg = self.readLineFromArduino()
rospy.loginfo("Lateral door final state: " + msg)
return roof_msg