def __init__(self): # Initialize the Action Server self._as = actionlib.ActionServer('/action_ros_iot', msgRosIotAction, self.on_goal, self.on_cancel, auto_start=False) ''' * self.on_goal - It is the fuction pointer which points to a function which will be called when the Action Server receives a Goal. * self.on_cancel - It is the fuction pointer which points to a function which will be called when the Action Server receives a Cancel Request. ''' # Read and Store IoT Configuration data from Parameter Server param_config_iot = rospy.get_param('config_pyiot') self._config_mqtt_server_url = param_config_iot['mqtt']['server_url'] self._config_mqtt_server_port = param_config_iot['mqtt']['server_port'] self._config_mqtt_sub_topic = param_config_iot['mqtt']['topic_sub'] self._config_mqtt_pub_topic = param_config_iot['mqtt']['topic_pub'] self._config_mqtt_qos = param_config_iot['mqtt']['qos'] self._config_mqtt_sub_cb_ros_topic = param_config_iot['mqtt'][ 'sub_cb_ros_topic'] self._config_google_apps_spread_sheet_id = param_config_iot[ 'google_apps']['spread_sheet_id'] print(param_config_iot) # Initialize ROS Topic Publication # Incoming message from MQTT Subscription will be published on a ROS Topic (/ros_iot_bridge/mqtt/sub). # ROS Nodes can subscribe to this ROS Topic (/ros_iot_bridge/mqtt/sub) to get messages from MQTT Subscription. self._handle_ros_pub = rospy.Publisher( self._config_mqtt_sub_cb_ros_topic, msgMqttSub, queue_size=10) # Subscribe to MQTT Topic (eyrc/xYzqLm/iot_to_ros) which is defined in 'config_iot_ros.yaml'. # self.mqtt_sub_callback() function will be called when there is a message from MQTT Subscription. ret = iot.mqtt_subscribe_thread_start(self.mqtt_sub_callback, self._config_mqtt_server_url, self._config_mqtt_server_port, self._config_mqtt_pub_topic, self._config_mqtt_qos) if (ret == 0): rospy.loginfo("MQTT Subscribe Thread Started") else: rospy.logerr("Failed to start MQTT Subscribe Thread") ret2 = iot.mqtt_subscribe_thread_start(self.mqtt_sub_callback_start, self._config_mqtt_server_url, self._config_mqtt_server_port, self._config_mqtt_sub_topic, self._config_mqtt_qos) if (ret2 == 0): rospy.loginfo("MQTT Subscribe Thread Started") else: rospy.logerr("Failed to start MQTT Subscribe Thread") # Start the Action Server self._as.start() rospy.loginfo("Started ROS-IoT Bridge Action Server.")
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 __init__(self): self._as = actionlib.ActionServer('/action_ros_iot', msgRosIotAction, self.on_goal, on_cancel, auto_start=False) param_config_iot = rospy.get_param('config_iot') self._config_mqtt_unique_id = param_config_iot['mqtt_unique_id'] self._config_mqtt_server_url = param_config_iot['mqtt']['server_url'] self._config_mqtt_server_port = param_config_iot['mqtt']['server_port'] self._config_mqtt_qos = param_config_iot['mqtt']['qos'] self._config_mqtt_incoming_orders = param_config_iot['mqtt'][ 'incoming_orders'] self._config_mqtt_sub_cb_ros_topic = param_config_iot['mqtt'][ 'sub_cb_ros_topic'] self._config_http_inventory_pub = param_config_iot['http']['inventory'] self._config_http_dispatch_pub = param_config_iot['http']['dispatch'] self._config_http_shipped_pub = param_config_iot['http']['shipped'] self._handle_incoming_orders = rospy.Publisher( self._config_mqtt_sub_cb_ros_topic, incomingMsg, queue_size=10) try: iot.mqtt_subscribe_thread_start(self.func_incoming_order_callback, self._config_mqtt_server_url, self._config_mqtt_server_port, self._config_mqtt_incoming_orders, self._config_mqtt_qos) rospy.loginfo("MQTT Subscribe Thread Started") except: # pylint: disable=bare-except rospy.logerr("Failed to start MQTT Subscribe Thread") self._as.start() rospy.loginfo("Started ROS-IoT Bridge Action Server")
def __init__(self): # Initialize the Action Server self._as = actionlib.ActionServer('/action_ros_iot', msgRosIotAction, self.on_goal, self.on_cancel, auto_start=False) # Read and Store IoT Configuration data from Parameter Server param_config_iot = rospy.get_param('config_iot') self._config_mqtt_server_url = param_config_iot['mqtt']['server_url'] self._config_mqtt_server_port = param_config_iot['mqtt']['server_port'] self._config_mqtt_sub_topic = param_config_iot['mqtt']['topic_sub'] self._config_mqtt_pub_topic = param_config_iot['mqtt']['topic_pub'] self._config_mqtt_qos = param_config_iot['mqtt']['qos'] self._config_mqtt_sub_cb_ros_topic = param_config_iot['mqtt'][ 'sub_cb_ros_topic'] self._config_mqtt_spread_sheet_id = param_config_iot['mqtt'][ 'spread_sheet_id'] print("\n\nMQTT PUB: " + self._config_mqtt_sub_topic) print("MQTT SUB: " + self._config_mqtt_pub_topic + "\n\n") print("Publish message START to start the turtle\n\n") print param_config_iot # Initialize ROS Topic Publication self._handle_ros_pub = rospy.Publisher( self._config_mqtt_sub_cb_ros_topic, msgMqttSub, queue_size=10) # Subscribe to MQTT 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") else: rospy.logerr("Failed to start MQTT Subscribe Thread") # Start the Action Server self._as.start() rospy.loginfo("Started ROS-IoT Bridge Action Server.")
def main(): global packages_priority_map global incoming_order_list packages_priority_map["Clothes"] = list() packages_priority_map["Food"] = list() packages_priority_map["Medicine"] = list() intial_joint_angles = [ math.radians(7.7848814851), math.radians(-138.015566396), math.radians(-58.0424957695), math.radians(-71.7379508013), math.radians(90.3676987576), math.radians(-84.2097855739) ] rospy.init_node('node_task5') obj_task5 = task5() obj_color = colorMsg() var_handle_pub = rospy.Publisher('my_topic', colorMsg, queue_size=10) image_sub = rospy.Subscriber("/eyrc/vb/camera_1/image_raw", Image, obj_task5.camera2D_callback_func) rospy.sleep(2) image_sub.unregister() obj_task5.hard_set_joint_angles(intial_joint_angles, 10) try: #getting incoming orders ret = iot.mqtt_subscribe_thread_start( obj_task5.on_recv_msg_mqtt, obj_task5.config_server_url, obj_task5.config_server_port, obj_task5.config_mqtt_topic_to_sub, obj_task5.config_qos) if ret == 0: rospy.loginfo('MQTT subscribe thread started') else: rospy.loginfo('MQTT subscribe failed') # print(color_map) #updating inventory sheet for val in color_map: obj_task5.update_inventory_sheet(val[1] + "0221", val[2], val[3], val[4], val[5]) # print("Done") rospy.sleep(0.5) package_picked = 1 # print(obj_task5.get_time_str()) # to_pick="" # new_order=dict() while package_picked <= 9: if len(incoming_order_list) > 0: new_order = incoming_order_list[0] to_pick = new_order["item"] # print(incoming_order_list) # print(type(incoming_order_list)) tnow = obj_task5.get_time_str() if to_pick == "Clothes": obj_task5.pick_place(packages_priority_map["Clothes"][0]) obj_task5.update_dispatch_orders_sheet( new_order["order_id"], new_order["city"], new_order["item"], "LP", new_order["qty"], 150, "Yes", tnow) obj_color.box_color = "green " + str( new_order["order_id"]) + " " + str( new_order["city"]) + " " + str( new_order["item"]) + " " + "LP" + " " + str( new_order["qty"]) + " " + str(150) var_handle_pub.publish(obj_color) packages_priority_map["Clothes"].pop(0) elif to_pick == "Food": obj_task5.pick_place(packages_priority_map["Food"][0]) obj_task5.update_dispatch_orders_sheet( new_order["order_id"], new_order["city"], new_order["item"], "MP", new_order["qty"], 250, "Yes", tnow) obj_color.box_color = "yellow " + str( new_order["order_id"]) + " " + str( new_order["city"]) + " " + str( new_order["item"]) + " " + "MP" + " " + str( new_order["qty"]) + " " + str(250) var_handle_pub.publish(obj_color) packages_priority_map["Food"].pop(0) elif to_pick == "Medicine": obj_task5.pick_place(packages_priority_map["Medicine"][0]) obj_task5.update_dispatch_orders_sheet( new_order["order_id"], new_order["city"], new_order["item"], "HP", new_order["qty"], 450, "Yes", tnow) obj_color.box_color = "red " + str( new_order["order_id"]) + " " + str( new_order["city"]) + " " + str( new_order["item"]) + " " + "HP" + " " + str( new_order["qty"]) + " " + str(450) var_handle_pub.publish(obj_color) packages_priority_map["Medicine"].pop(0) incoming_order_list.pop(0) package_picked += 1 rospy.spin() except KeyboardInterrupt: rospy.loginfo("Shutting down") cv2.destroyAllWindows()
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): """ 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.")
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 __init__(self): """Initialize the Action Server""" self._as = actionlib.ActionServer('/action_ros_iot', msgRosIotAction, self.on_goal, self.on_cancel, auto_start=False) ''' self.on_goal - It is the fuction pointer which points to a function which will be called when the Action Server receives a Goal. self.on_cancel - It is the fuction pointer which points to a function which will be called when the Action Server receives a Cancel Request. ''' # Read and Store IoT Configuration data from Parameter Server param_config_iot = rospy.get_param('config_pyiot') self._config_mqtt_server_url = param_config_iot['mqtt']['server_url'] self._config_mqtt_server_port = param_config_iot['mqtt']['server_port'] self._config_mqtt_sub_topic = param_config_iot['mqtt']['topic_sub'] self._config_mqtt_pub_topic = param_config_iot['mqtt']['topic_pub'] self._config_mqtt_qos = param_config_iot['mqtt']['qos'] self._config_mqtt_sub_cb_ros_topic = param_config_iot['mqtt'][ 'sub_cb_ros_topic'] self._config_google_apps_spread_sheet_id = param_config_iot[ 'google_apps']['spread_sheet_id'] self._config_google_apps_submission_spread_sheet_id = param_config_iot[ 'google_apps']['submission_spread_sheet_id'] self._config_google_apps_team_id = "VB#1321" self._config_google_apps_unique_id = "eyrcRAVS" print param_config_iot self.url_spread_sheet = "https://script.google.com/macros/s/" + self._config_google_apps_spread_sheet_id + "/exec" self.url_sumission_spread_sheet = "https://script.google.com/macros/s/" + self._config_google_apps_submission_spread_sheet_id + "/exec" # Call inventorydatas function self.inventorydatas(True) # Subscribe ROSTOPIC eyrc/publish/dispatchedorder rospy.Subscriber("eyrc/publish/dispatchedorder", msgDispatchedOrder, self.dispatchedorders) # Subscribe ROSTOPIC eyrc/publish/shippedorder rospy.Subscriber("eyrc/publish/shippedorder", msgShippedOrder, self.shippedorders) self.pub_client = mqtt.Client() self.pub_client.connect(self._config_mqtt_server_url, self._config_mqtt_server_port) # Initialize ROS Topic Publication # Incoming message from MQTT Subscription will be published on a ROS Topic (/ros_iot_bridge/mqtt/sub). # ROS Nodes can subscribe to this ROS Topic (/ros_iot_bridge/mqtt/sub) to get messages from MQTT Subscription. self._handle_ros_pub = rospy.Publisher( self._config_mqtt_sub_cb_ros_topic, msgMqttSub, queue_size=10) # Subscribe to MQTT Topic (eyrc/xYzqLm/iot_to_ros) which is defined in 'config_ros_iot.yaml'. # self.mqtt_sub_callback() function will be called when there is a message from MQTT Subscription. 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") else: rospy.logerr("Failed to start MQTT Subscribe Thread") # Start the Action Server self._as.start() rospy.loginfo("Started ROS-IoT Bridge Action Server.")
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 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 __init__(self): """ The constructor for the RosIotBridgeActionServer class. This function will initialize this node as an action server. We will also load various paramter values from the ROS parameter server, and start a seprate thread to read the data coming from MQTT client """ # Initialize the action server self._as = actionlib.ActionServer('/action_ros_iot', msgRosIotAction, self.on_goal, auto_start = False) # * self.on_goal - Pointer of the function to be called # when a goal is received # * self.on_cancel - Pointer of the function to be called # when a cancel req is received # Read and Store the IOT Configuration from parameter server param_config_iot = rospy.get_param('config_pyiot') # Loading the MQTT Parameters self._config_mqtt_server_url = param_config_iot['mqtt']['server_url'] self._config_mqtt_server_port = param_config_iot['mqtt']['server_port'] self._config_mqtt_sub_topic = param_config_iot['mqtt']['topic_sub'] self._config_mqtt_pub_topic = param_config_iot['mqtt']['topic_pub'] self._config_mqtt_qos = param_config_iot['mqtt']['qos'] self._config_mqtt_sub_cb_ros_topic = param_config_iot['mqtt']['sub_cb_ros_topic'] # Loading the Google Sheet Parameter self._config_sheet_id = param_config_iot['google_apps']['spread_sheet_id'] self._config_sheet_url = param_config_iot['google_apps']['spread_sheet_url'] self._config_email_id = param_config_iot['google_apps']['email_id'] rospy.logwarn("email_id {}".format(self._config_email_id)) # Overwriting for Eyantra's Gsheet # self._config_sheet_url = "https://script.google.com/macros/s/AKfycbw5xylppoda-8HPjt2Tzq4ShU_Xef-Ik-hEtBPcPk0gdGw8095j4RZ7/exec" # Loading the dictionary for each sheets self.dict_IncomingOrders = {"id": "IncomingOrders", "Team Id": "VB#1083", "Unique Id": "iOeCqZLI", "Order ID": "NA", "Order Date and Time": "NA", "Item": "NA", "Priority": "NA", "Order Quantity": "NA", "City": "NA", "Longitude": "NA", "Latitude": "NA", "Cost": "NA"} self.dict_Inventory = {"id": "Inventory", "Team Id": "VB#1083", "Unique Id": "iOeCqZLI", "SKU": "NA", "Item": "NA", "Priority": "NA", "Storage Number": "NA", "Cost": "NA", "Quantity": "NA", } self.dict_OrdersDispatched = {"id": "OrdersDispatched", "Team Id": "VB#1083", "Unique Id": "iOeCqZLI", "Order ID":"NA", "City":"NA", "Item":"NA", "Priority":"NA", "Dispatch Quantity":"NA", "Cost":"NA", "Dispatch Status":"NA", "Dispatch Date and Time":"NA", "email_id": self._config_email_id} self.dict_OrdersShipped = {"id":"OrdersShipped", "Team Id":"VB#1083", "Unique Id":"iOeCqZLI", "Order ID":"NA", "City":"NA", "Item":"NA", "Priority":"NA", "Shipped Quantity":"NA", "Cost":"NA", "Shipped Status":"NA", "Shipped Date and Time":"NA", "Estimated Time of Delivery":"NA", "email_id": self._config_email_id} # Initialize the ros topic '/ros_iot_bridge/mqtt/sub' so that # other ROS nodes can listen to MQTT messages self._handle_ros_pub = rospy.Publisher(self._config_mqtt_sub_cb_ros_topic, msgMqttSub, queue_size = 10) # Subscribe to MQTT topic 'eyrc/iOeCqZLl/iot_to_ros' so that # it can later on publish the message to '/ros_iot_bridge/mqtt/sub' # for other ROS Nodes to listen 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) # * mqtt_sub_callback - Function which will be called when this node # receives the message from MQTT # * other arguments - probably required to make connection to # appropiate server (assumption) if (ret == 0): rospy.loginfo("[BRIDGE] MQTT Subscribe Thread Started") else: rospy.logerr("[BRIDGE] Failed to start MQTT Subscribe Thread") # Start the Action Server self._as.start() rospy.loginfo("[BRIDGE] Started ROS IOT Bridge")