def on_transition(self, goal_handle): result = msgRosIotResult() index = 0 for i in self._goal_handles: if self._goal_handles[i] == goal_handle: index = 1 break
def on_goal(self, goal_handle): """ This function will be called when Action Server receives a Goal from /action_ros_iot client This pushes the data incoming from the Action client into the respective sheets via the http protocol. It can also push data to MQTT topics if the protocol given is "mqtt" Parameters: goal_handle (object): It is the goal handle that corresponds to the goal that is sent by the action client """ goal = goal_handle.get_goal() rospy.loginfo("Received new goal from Client in IOT BRIDGE") print('goaaaaaaaaaaaal', goal) #pushing data to google sheet if goal.protocol == 'http': goal_handle.set_accepted() try: parameters = json.loads(goal.message) except: rospy.logerr(goal) URL = "https://script.google.com/macros/s/AKfycbw5xylppoda-8HPjt2Tzq4ShU_Xef-Ik-hEtBPcPk0gdGw8095j4RZ7/exec" response = requests.get(URL, params=parameters) print response.content URL = "https://script.google.com/macros/s/AKfycbzdIlFmA4AtvM18f7WBMwdzzvkrS4HvBC2xb2LCMG8Jah6BE9IWAtod/exec" response = requests.get(URL, params=parameters) print response.content result = msgRosIotResult() result.flag_success = True goal_handle.set_succeeded(result) # Validate incoming goal parameters elif goal.protocol == "mqtt": if ((goal.mode == "pub") or (goal.mode == "sub")): goal_handle.set_accepted() # Start a new thread to process new goal from the client # (For Asynchronous Processing of Goals) # 'self.process_goal' - is the function pointer which points # to a function that will process incoming Goals thread = threading.Thread(name="worker", target=self.process_goal, args=(goal_handle, )) thread.start() else: goal_handle.set_rejected() return else: goal_handle.set_rejected() return
def __init__(self): self._ac = actionlib.SimpleActionClient('/action_turtle', msgTurtleAction) self._ac.wait_for_server() rospy.loginfo("Simple Action server is up, we can send new goals!") self._curr_goal_done = 1 self._result = msgRosIotResult() self.ros_iot_client = ActionClientRosIoTBridge()
def process_goal(self, goal_handle): flag_success = False result = msgRosIotResult() goal_id = goal_handle.get_goal_id() rospy.loginfo("Processing goal : " + str(goal_id.id)) goal = goal_handle.get_goal() # Goal Processing if(goal.protocol == "mqtt"): rospy.logwarn("MQTT") if(goal.mode == "pub"): rospy.logwarn("MQTT PUB Goal ID: " + str(goal_id.id)) rospy.logwarn(goal.topic + " > " + goal.message) ret = iot.mqtt_publish( self._config_mqtt_server_url, self._config_mqtt_server_port, goal.topic, goal.message, self._config_mqtt_qos ) if(ret == 0): rospy.loginfo("MQTT Publish Successful.") result.flag_success = True else: rospy.logerr("MQTT Failed to Publish") result.flag_success = False elif(goal.mode == "sub"): rospy.logwarn("MQTT SUB Goal ID: " + str(goal_id.id)) rospy.logwarn(goal.topic) ret = iot.mqtt_subscribe_thread_start( self.mqtt_sub_callback, self._config_mqtt_server_url, self._config_mqtt_server_port, goal.topic, self._config_mqtt_qos ) if(ret == 0): rospy.loginfo("MQTT Subscribe Thread Started") result.flag_success = True else: rospy.logerr("Failed to start MQTT Subscribe Thread") result.flag_success = False rospy.loginfo("Send goal result to client") if (result.flag_success == True): rospy.loginfo("Succeeded") goal_handle.set_succeeded(result) else: rospy.loginfo("Goal Failed. Aborting.") goal_handle.set_aborted(result) rospy.loginfo("Goal ID: " + str(goal_id.id) + " Goal Processing Done.")
def on_transition(self, goal_handle): '''ON TRANSITON FUNCTION USED BY SEND GOAL IOT AS CALLBACK.''' result = msgRosIotResult() index = 0 for i in self._goal_handles: if self._goal_handles[i] == goal_handle: index = 1 break
def on_transition(self, goal_handle): '''**************************METHOD DOCSTRING************************** Name: on_transition Objective: This function processes the received goal from client and prints status on screen Arguments: goal_handle from Action Client Returns: None ***********************************************************************''' # from on_goal() to on_transition(). goal_handle generated by send_goal() is used here. result = msgRosIotResult() index = 0 for i in self._goal_handles: if self._goal_handles[i] == goal_handle: index = i break rospy.loginfo("Transition Callback. Client Goal Handle #: " + str(index)) rospy.loginfo("Comm. State: " + str(goal_handle.get_comm_state())) rospy.loginfo("Goal Status: " + str(goal_handle.get_goal_status())) # Comm State - Monitors the State Machine of the Client which is different from Server's # Comm State = 2 -> Active # Comm State = 3 -> Wating for Result # Comm State = 7 -> Done # if (Comm State == ACTIVE) if goal_handle.get_comm_state() == 2: rospy.loginfo(str(index) + ": Goal just went active.") # if (Comm State == DONE) if goal_handle.get_comm_state() == 7: rospy.loginfo(str(index) + ": Goal is DONE") rospy.loginfo(goal_handle.get_terminal_state()) # get_result() gets the result produced by the Action Server result = goal_handle.get_result() rospy.loginfo(result.flag_success) if result.flag_success is True: rospy.loginfo( "Goal successfully completed. Client Goal Handle #: " + str(index)) else: rospy.loginfo("Goal failed. Client Goal Handle #: " + str(index))
def on_transition(self, goal_handle): """ Function called when there is a change in state of Action Client State Machine Parameters: goal_handle: goal_handle of the goal, whose state changed """ result = msgRosIotResult() index = 0 for i in self._goal_handles: if self._goal_handles[i] == goal_handle: index = i break rospy.loginfo("[UR5_2] Transition Callback. Client Goal Handle #: " + str(index)) rospy.loginfo("[UR5_2] Comm. State: " + str(goal_handle.get_comm_state())) rospy.loginfo("[UR5_2] Goal Status: " + str(goal_handle.get_goal_status())) # Comm State - Monitors the State Machine of the Client which is different from Server's # Comm State = 2 -> Active # Comm State = 3 -> Wating for Result # Comm State = 7 -> Done # if (Comm State == ACTIVE) if goal_handle.get_comm_state() == 2: rospy.loginfo("[UR5_2] " + str(index) + ": Goal just went active.") # if (Comm State == DONE) if goal_handle.get_comm_state() == 7: rospy.loginfo("[UR5_2] " + str(index) + ": Goal is DONE") rospy.loginfo("[UR5_2] " + str(goal_handle.get_terminal_state())) # get_result() gets the result produced by the Action Server result = goal_handle.get_result() rospy.loginfo("[UR5_2] " + str(result.flag_success)) if (result.flag_success == True): rospy.loginfo( "[UR5_2] Goal successfully completed. Client Goal Handle #: " + str(index)) else: rospy.logerr("[UR5_2] Goal failed. Client Goal Handle #: " + str(index))
def process_goal(self, goal_handle): """ Goal Processing Function This function is called is a separate thread to process Goal Parameters: goal_handle(object): Goal handler for the incoming goal. """ result = msgRosIotResult() goal_id = goal_handle.get_goal_id() rospy.loginfo("Processing goal : " + str(goal_id.id)) goal = goal_handle.get_goal() if (goal.protocol == "http") and (goal.mode == "pub"): rospy.logwarn("HTTP PUB Goal ID: " + str(goal_id.id)) rospy.logwarn(goal.topic + " > " + goal.message) if goal.topic == self._config_http_inventory_pub: http_status = func_inventory_data_callback(goal.message) if goal.topic == self._config_http_dispatch_pub: http_status = func_dispatch_data_callback(goal.message) if goal.topic == self._config_http_shipped_pub: http_status = func_shipped_data_callback(goal.message) if http_status == 0: rospy.loginfo("HTTP requests were successfully sent!") result.flag_success = True else: rospy.loginfo("HTTP requests failed") result.flag_success = False rospy.loginfo("Send goal result to client") if result.flag_success: rospy.loginfo("Succeeded") goal_handle.set_succeeded(result) else: rospy.loginfo("Goal Failed. Aborting.") goal_handle.set_aborted(result) rospy.loginfo("Goal ID: " + str(goal_id.id) + " Goal Processing Done.")
def on_transition(self, goal_handle): # from on_goal() to on_transition(). goal_handle generated by send_goal() is used here. result = msgRosIotResult() # index = 0 # for i in range(len(self._goal_handles)): # if self._goal_handles[i] == goal_handle: # index = i # break index = self._goal_handles.index(goal_handle) rospy.loginfo("Transition Callback. Client Goal Handle #: " + str(index)) rospy.loginfo("Comm. State: " + str(goal_handle.get_comm_state())) rospy.loginfo("Goal Status: " + str(goal_handle.get_goal_status())) # Comm State - Monitors the State Machine of the Client which is different from Server's # Comm State = 2 -> Active # Comm State = 3 -> Wating for Result # Comm State = 7 -> Done # if (Comm State == ACTIVE) if goal_handle.get_comm_state() == 2: rospy.loginfo(str(index) + ": Goal just went active.") # if (Comm State == DONE) if goal_handle.get_comm_state() == 7: rospy.loginfo(str(index) + ": Goal is DONE") rospy.loginfo(goal_handle.get_terminal_state()) # get_result() gets the result produced by the Action Server result = goal_handle.get_result() rospy.loginfo(result.flag_success) if result.flag_success == True: rospy.loginfo( "Goal successfully completed. Client Goal Handle #: " + str(index)) # self._goal_handles.remove(goal_handle) # rospy.loginfo("Client Goal Handle #: {} removed".format(index)) else: rospy.loginfo("Goal failed. Client Goal Handle #: " + str(index))
def on_transition(self, goal_handle): """ State Machine : /action_ros_iot This method will be called when there is a change of state in the Action Client. Parameters: goal_handle: This is a structure containing attributes related to the goal sent. """ result = msgRosIotResult() index = 0 for i in self._goal_handles: if self._goal_handles[i] == goal_handle: index = i break rospy.loginfo("Transition Callback. Client Goal Handle #: " + str(index)) rospy.loginfo("Comm. State: " + str(goal_handle.get_comm_state())) rospy.loginfo("Goal Status: " + str(goal_handle.get_goal_status())) # if (Comm State == ACTIVE) if goal_handle.get_comm_state() == 2: rospy.loginfo(": Goal " + str(index) + " is active.") # if (Comm State == DONE) if goal_handle.get_comm_state() == 7: rospy.loginfo(str(index) + ": Goal is DONE") rospy.loginfo(goal_handle.get_terminal_state()) # get_result() gets the result produced by the Action Server result = goal_handle.get_result() rospy.loginfo(result.flag_success) if result.flag_success: rospy.loginfo("Goal Suceeded. Client Goal Handle #: " + str(index)) else: rospy.loginfo("Goal failed. Client Goal Handle #: " + str(index))
def process_goal(self, goal_handle): """ Function called to process the goal, after it's parameters are verified. Parameters: goal_handle: The goal handle of the goal to be processed. """ flag_success = False # Initialize the object for msgRosIotResult result = msgRosIotResult() # Get the ID of the current goal handle goal_id = goal_handle.get_goal_id() rospy.loginfo("[BRIDGE] Processing Goal: " + str(goal_id.id)) rospy.logwarn("Processing Goal") # Get the current goal using goal handle goal = goal_handle.get_goal() # Upload the data to google sheet ret = self.update_gsheet(goal, False, None) if (ret == "success"): # If uploaded successfully result.flag_success = True # Set the goal status as succeeded goal_handle.set_succeeded(result) rospy.logwarn("Goal Suceeded") else: # If not uploaded successfully rospy.logerr("[BRIDGE] Failed at a Goal, ID: {}".format(goal_handle)) result.flag_success = False # Set state as aborted / failed goal_handle.set_aborted(result) rospy.loginfo("[BRIDGE] Goal ID: " + str(goal_id.id) + " Goal Processing Done.")
def on_transition1(self, goal_handle): result = msgRosIotResult() index = 0 for i in self._goal_handles1: if self._goal_handles1[i] == goal_handle: index = i break rospy.loginfo( "Transition Callback IOT. From iot clent Client Goal Handle #: " + str(index)) rospy.loginfo("Comm. State: IOT " + str(goal_handle.get_comm_state())) rospy.loginfo("Goal Status: IOT " + str(goal_handle.get_goal_status())) # Comm State - Monitors the State Machine of the Client which is different from Server's # Comm State = 2 -> Active # Comm State = 3 -> Wating for Result # Comm State = 7 -> Done # if (Comm State == ACTIVE) if goal_handle.get_comm_state() == 2: rospy.loginfo(str(index) + ":IOT Goal just went active.") # if (Comm State == DONE) if goal_handle.get_comm_state() == 7: rospy.loginfo(str(index) + ": IOT Goal is DONE") rospy.loginfo(goal_handle.get_terminal_state()) # get_result() gets the result produced by the Action Server result = goal_handle.get_result() rospy.loginfo(result.flag_success) if (result.flag_success == True): rospy.loginfo( "Goal successfully completed. IOT Client Goal Handle #: " + str(index)) else: rospy.loginfo("IOT Goal failed. Client Goal Handle #: " + str(index))
def process_goal(self, goal_handle): """ This function is called is a separate thread to process Goal """ flag_success = False result = msgRosIotResult() goal_id = goal_handle.get_goal_id() rospy.loginfo("Processing goal : " + str(goal_id.id)) goal = goal_handle.get_goal() # Goal Processing if goal.protocol == "mqtt": rospy.logwarn("MQTT") if goal.mode == "pub": rospy.logwarn("MQTT PUB Goal ID: " + str(goal_id.id)) rospy.logwarn(goal.topic + " > " + goal.message) ret = iot.mqtt_publish(self._config_mqtt_server_url, self._config_mqtt_server_port, goal.topic, goal.message, self._config_mqtt_qos) if ret == 0: rospy.loginfo("MQTT Publish Successful.") result.flag_success = True msg_split = goal.message.split(",") result_x = msg_split[0].split("(")[1] result_y = msg_split[1] result_theta = msg_split[2].split(")")[0] rospy.logwarn('MESSAGE IMP: ' + result_x + result_y + result_theta) iot.publish_message_to_spreadsheet( turtle_x=result_x, turtle_y=result_y, turtle_theta=result_theta) else: rospy.logerr("MQTT Failed to Publish") result.flag_success = False elif goal.mode == "sub": rospy.logwarn("MQTT SUB Goal ID: " + str(goal_id.id)) rospy.logwarn(goal.topic) ret = iot.mqtt_subscribe_thread_start( self.mqtt_sub_callback, self._config_mqtt_server_url, self._config_mqtt_server_port, goal.topic, self._config_mqtt_qos) if ret == 0: rospy.loginfo("MQTT Subscribe Thread Started") result.flag_success = True else: rospy.logerr("Failed to start MQTT Subscribe Thread") result.flag_success = False rospy.loginfo("Send goal result to client") if result.flag_success: rospy.loginfo("Succeeded") goal_handle.set_succeeded(result) else: rospy.loginfo("Goal Failed. Aborting.") goal_handle.set_aborted(result) rospy.loginfo("Goal ID: " + str(goal_id.id) + " Goal Processing Done.")
def process_goal(self, goal_handle): """ This function process the goal from incoming from the Action client and then sends appropriate results to the action client based on whether the goal has been completed or failed. Parameters: goal_handle (object): This is the goal handle of the goal incoming from the Acion client """ result = msgRosIotResult() goal_id = goal_handle.get_goal_id() rospy.loginfo("Processing goal : " + str(goal_id.id)) goal = goal_handle.get_goal() # Goal Processing if goal.protocol == "mqtt": rospy.logwarn("MQTT") if goal.mode == "pub": rospy.logwarn("MQTT PUB Goal ID: " + str(goal_id.id)) rospy.logwarn(goal.topic + " > " + goal.message) ret = iot.mqtt_publish(self._config_mqtt_server_url, self._config_mqtt_server_port, goal.topic, goal.message, self._config_mqtt_qos) if ret == 0: rospy.loginfo("MQTT Publish Successful.") result.flag_success = True else: rospy.logerr("MQTT Failed to Publish") result.flag_success = False elif goal.mode == "sub": rospy.logwarn("MQTT SUB Goal ID: " + str(goal_id.id)) rospy.logwarn(goal.topic) ret = iot.mqtt_subscribe_thread_start( self.mqtt_sub_callback, self._config_mqtt_server_url, self._config_mqtt_server_port, goal.topic, self._config_mqtt_qos) if ret == 0: rospy.loginfo("MQTT Subscribe Thread Started") result.flag_success = True else: rospy.logerr("Failed to start MQTT Subscribe Thread") result.flag_success = False rospy.loginfo("Send goal result to client") if result.flag_success is True: rospy.loginfo("Succeeded") goal_handle.set_succeeded(result) else: rospy.loginfo("Goal Failed. Aborting.") goal_handle.set_aborted(result) rospy.loginfo("Goal ID: " + str(goal_id.id) + " Goal Processing Done.")
def process_goal(self, goal_handle): flag_success = False result = msgRosIotResult() goal_id = goal_handle.get_goal_id() rospy.loginfo("Processing goal : " + str(goal_id.id)) goal = goal_handle.get_goal() # Goal Processing if (goal.protocol == "mqtt"): rospy.logwarn("MQTT") if (goal.mode == "pub"): rospy.logwarn("MQTT PUB Goal ID: " + str(goal_id.id)) rospy.logwarn(goal.topic + " > " + str(goal.message)) ret = iot.mqtt_publish(self._config_mqtt_server_url, self._config_mqtt_server_port, self._config_mqtt_pub_topic, goal.message, self._config_mqtt_qos) if (ret == 0): rospy.loginfo("MQTT Publish Successful.") result.flag_success = True else: rospy.logerr("MQTT Failed to Publish") result.flag_success = False send_x = goal.message[1] send_y = goal.message[4] send_theta = goal.message[7] if goal.message[1] == '-': send_x = -1 * int(goal.message[2]) if goal.message[4] == '-': send_y = -1 * int(goal.message[5]) if goal.message[7] == '-': send_theta = -1 * int(goal.message[8]) send_sheet = { "id": "Sheet1", "turtle_x": send_x, "turtle_y": send_y, "turtle_theta": send_theta } response = requests.get(self._sheet_url, params=send_sheet) URL1 = "https://script.google.com/macros/s/AKfycbw850dk4moVgebU2GGe0PUQUvvg8jTpSjBQCawJt3_13vgujLk/exec" parameters = { "id": "task1", "team_id": "VB_1266", "unique_id": "EyrcSaka", "turtle_x": send_x, "turtle_y": send_y, "turtle_theta": send_theta } response1 = requests.get(URL1, params=parameters) rospy.loginfo(response1.content) elif (goal.mode == "sub"): rospy.logwarn("MQTT SUB Goal ID: " + str(goal_id.id)) rospy.logwarn(goal.topic) ret = iot.mqtt_subscribe_thread_start( self.mqtt_sub_callback, self._config_mqtt_server_url, self._config_mqtt_server_port, self._config_mqtt_sub_topic, self._config_mqtt_qos) if (ret == 0): rospy.loginfo("MQTT Subscribe Thread Started") result.flag_success = True else: rospy.logerr("Failed to start MQTT Subscribe Thread") result.flag_success = False rospy.loginfo("Send goal result to client") if (result.flag_success == True): rospy.loginfo("Succeeded") goal_handle.set_succeeded(result) else: rospy.loginfo("Goal Failed. Aborting.") goal_handle.set_aborted(result) rospy.loginfo("Goal ID: " + str(goal_id.id) + " Goal Processing Done.")
def on_transition(self, goal_handle): """ Function called when there is a change in state of Action Client State Machine Parameters: goal_handle: goal_handle of the goal, whose state changed """ # from on_goal() to on_transition(). goal_handle generated by send_goal() is used here. result = msgRosIotResult() index = 0 for i in self._goal_handles: if self._goal_handles[i] == goal_handle: index = i break rospy.loginfo( "[CAMERA CLIENT] Transition Callback. Client Goal Handle #: " + str(index)) rospy.loginfo("[CAMERA CLIENT] Comm. State: " + str(goal_handle.get_comm_state())) rospy.loginfo("[CAMERA CLIENT] Goal Status: " + str(goal_handle.get_goal_status())) # Comm State - Monitors the State Machine of the Client which is different from Server's # Comm State = 2 -> Active # Comm State = 3 -> Wating for Result # Comm State = 7 -> Done # if (Comm State == ACTIVE) if goal_handle.get_comm_state() == 2: rospy.loginfo("[CAMERA CLIENT] " + str(index) + ": Goal just went active.") # if (Comm State == DONE) if goal_handle.get_comm_state() == 7: rospy.loginfo("[CAMERA CLIENT] " + str(index) + ": Goal is DONE") rospy.loginfo("[CAMERA CLIENT] " + str(goal_handle.get_terminal_state())) # get_result() gets the result produced by the Action Server result = goal_handle.get_result() rospy.loginfo("[CAMERA CLIENT] " + str(result.flag_success)) if (result.flag_success == True): rospy.loginfo( "[CAMERA CLIENT] Goal successfully completed. Client Goal Handle #: " + str(index)) self.packages_updated_on_gsheet += 1 else: rospy.logerr( "[CAMERA CLIENT] Goal failed. Client Goal Handle #: " + str(index)) # if all the package info updated on gsheet # kill this node to free up resources if self.packages_updated_on_gsheet >= 12: rospy.logwarn("[CAMERA CLIENT] >>> All Package info published") rospy.logwarn("[CAMERA CLIENT] *** Shutting down this node") rospy.signal_shutdown("All info published")
def process_goal(self, goal_handle): '''*********************************************** Name: process_goal Objective: This function is called is a separate thread to process Goal. Arguments: goal object for the goal Returns: None **************************************************''' flag_success = False result = msgRosIotResult() goal_id = goal_handle.get_goal_id() rospy.loginfo("Processing goal : " + str(goal_id.id)) goal = goal_handle.get_goal() # Goal Processing if goal.protocol == "mqtt": rospy.logwarn("MQTT") if goal.mode == "pub": rospy.logwarn("MQTT PUB Goal ID: " + str(goal_id.id)) rospy.logwarn(goal.topic + " > " + goal.message) ret = iot.mqtt_publish(self._config_mqtt_server_url, self._config_mqtt_server_port, goal.topic, goal.message, self._config_mqtt_qos) if ret == 0: rospy.loginfo("MQTT Publish Successful.") result.flag_success = True ss_msg = re.sub('[()]', '', goal.message) ss_msg_arr = ss_msg.split(',') self.ss_publish(ss_msg_arr) self.ss_publish(ss_msg_arr,'eyrc') else: rospy.logerr("MQTT Failed to Publish") result.flag_success = False elif goal.mode == "sub": rospy.logwarn("MQTT SUB Goal ID: " + str(goal_id.id)) rospy.logwarn(goal.topic) ret = iot.mqtt_subscribe_thread_start(self.mqtt_sub_callback, self._config_mqtt_server_url, self._config_mqtt_server_port, goal.topic, self._config_mqtt_qos) if ret == 0: rospy.loginfo("MQTT Subscribe Thread Started") result.flag_success = True else: rospy.logerr("Failed to start MQTT Subscribe Thread") result.flag_success = False rospy.loginfo("Send goal result to client") if result.flag_success is True: rospy.loginfo("Succeeded") goal_handle.set_succeeded(result) else: rospy.loginfo("Goal Failed. Aborting.") goal_handle.set_aborted(result) rospy.loginfo("Goal ID: " + str(goal_id.id) + " Goal Processing Done.")
def process_goal(self, goal_handle): flag_success = False result = msgRosIotResult() goal_id = goal_handle.get_goal_id() rospy.loginfo("Processing goal : " + str(goal_id.id)) goal = goal_handle.get_goal() # Goal Processing if (goal.protocol == "mqtt"): rospy.logwarn("MQTT") if (goal.mode == "pub"): rospy.logwarn("MQTT PUB Goal ID: " + str(goal_id.id)) rospy.logwarn(goal.topic + " > " + str(goal.final_x)) ret = iot.mqtt_publish( self._config_mqtt_server_url, self._config_mqtt_server_port, goal.topic, "final_x:" + str(goal.final_x) + "\n" + "final_y:" + str(goal.final_y) + "\n" + "final_theta" + str(goal.final_theta) + "\n", self._config_mqtt_qos) parameters = { "id": "Sheet1", "turtle_x": goal.final_x, "turtle_y": goal.final_y, "turtle_theta": goal.final_theta } response = requests.get(self.URL, params=parameters) rospy.logwarn("mqqt pub requested") if (ret == 0): rospy.loginfo("MQTT Publish Successful.") result.flag_success = True else: rospy.logerr("MQTT Failed to Publish") result.flag_success = False elif (goal.mode == "sub"): rospy.logwarn("MQTT SUB Goal ID: " + str(goal_id.id)) rospy.logwarn(goal.topic) ret = iot.mqtt_subscribe_thread_start( self.mqtt_sub_callback, self._config_mqtt_server_url, self._config_mqtt_server_port, goal.topic, self._config_mqtt_qos) if (ret == 0): rospy.loginfo("MQTT Subscribe Thread Started") result.flag_success = True else: rospy.logerr("Failed to start MQTT Subscribe Thread") result.flag_success = False rospy.loginfo("Send goal result to client") if (result.flag_success == True): rospy.loginfo("Succeeded") goal_handle.set_succeeded(result) else: rospy.loginfo("Goal Failed. Aborting.") goal_handle.set_aborted(result) rospy.loginfo("Goal ID: " + str(goal_id.id) + " Goal Processing Done.")
def process_goal(self, goal_handle): """ This function is called is a separate thread to process Goal. """ flag_success = False result = msgRosIotResult() goal_id = goal_handle.get_goal_id() rospy.loginfo("Processing goal : " + str(goal_id.id)) goal = goal_handle.get_goal() # Goal Processing if goal.protocol == "mqtt": rospy.logwarn("MQTT") if goal.mode == "pub": rospy.logwarn("MQTT PUB Goal ID: " + str(goal_id.id)) rospy.logwarn(goal.topic + " > " + goal.message) ret = iot.mqtt_publish(self._config_mqtt_server_url, self._config_mqtt_server_port, goal.topic, goal.message, self._config_mqtt_qos) if ret == 0: rospy.loginfo("MQTT Publish Successful.") result.flag_success = True else: rospy.logerr("MQTT Failed to Publish") result.flag_success = False elif goal.mode == "sub": rospy.logwarn("MQTT SUB Goal ID: " + str(goal_id.id)) rospy.logwarn(goal.topic) ret = iot.mqtt_subscribe_thread_start(self.mqtt_sub_callback, self._config_mqtt_server_url, self._config_mqtt_server_port, goal.topic, self._config_mqtt_qos) if ret == 0: rospy.loginfo("MQTT Subscribe Thread Started") result.flag_success = True else: rospy.logerr("Failed to start MQTT Subscribe Thread") result.flag_success = False elif goal.protocol == "http": rospy.logwarn("HTTP") rospy.logwarn("HTTP GET Goal ID: " + str(goal_id.id)) rospy.logwarn(goal.topic + " > " + goal.message) # update_spreadsheet(spreadsheet_id, id, 'team_id', 'unique_id', '{'turtle_x': x, 'turtle_y': y, 'turtle_theta': theta,...,..}') ret = iot.update_spreadsheet(goal.topic, # using topic to send spreadsheet_id goal.mode, literal_eval(goal.message) # data points with column names in dict format str ) if ret == 0: rospy.loginfo("Updating Spreadsheet Successful.") result.flag_success = True else: rospy.logerr("Updating Spreadsheet Failed") result.flag_success = False rospy.loginfo("Send goal result to client") if result.flag_success == True: rospy.loginfo("Succeeded") goal_handle.set_succeeded(result) else: rospy.loginfo("Goal Failed. Aborting.") goal_handle.set_aborted(result) rospy.loginfo("Goal ID: " + str(goal_id.id) + " Goal Processing Done.")