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.")
