def validate(self, equality_function): if hasattr(self, "exc"): print self.exc print self.exc.message raise self.exc equality_function(self.input, c.extract_values(self.req)) equality_function(self.output, self.rsp)
def callback(self, msg, callbacks=None): """ Callback for incoming messages on the rospy.Subscriber Converts the incoming msg to JSON, then passes the JSON to the registered subscriber callbacks. Keyword Arguments: msg - the ROS message coming from the subscriber callbacks - subscriber callbacks to invoke """ # Try to convert the msg to JSON json = None try: json = message_conversion.extract_values(msg) except Exception as exc: logerr("Exception while converting messages in subscriber callback : %s", exc) return # Get the callbacks to call if not callbacks: with self.lock: callbacks = self.subscriptions.values() # Pass the JSON to each of the callbacks for callback in callbacks: try: callback(json) except Exception as exc: # Do nothing if one particular callback fails except log it logerr("Exception calling subscribe callback: %s", exc) pass
def start(self): req = self.srvClass._request_class() gen = c.extract_values(req) gen = populate_random_args(gen) self.input = gen services.ServiceCaller(self.name, gen, self.success, self.error).start()
def validate(self, equality_function): if hasattr(self, "exc"): print(self.exc) print(self.exc.message) raise self.exc equality_function(self.input, c.extract_values(self.req)) equality_function(self.output, self.rsp)
def callback(self, msg): """ Callback for incoming messages on the rospy.Subscriber Converts the incoming msg to JSON, then passes the JSON to the registered subscriber callbacks. Keyword Arguments: msg - the ROS message coming from the subscriber """ # Try to convert the msg to JSON json = None try: json = message_conversion.extract_values(msg) except: return # Get the callbacks to call with self.lock: callbacks = self.subscriptions.values() # Pass the JSON to each of the callbacks for callback in callbacks: try: callback(json) except Exception as exc: # Do nothing if one particular callback fails except log it logerr("Exception calling subscribe callback: %s", exc) pass
def handle_request(self, req): with self.lock: self.active_requests += 1 # generate a unique ID request_id = "service_request:" + self.service_name + ":" + str(self.next_id()) # build a request to send to the external client request_message = { "op": "call_service", "id": request_id, "service": self.service_name, "args": message_conversion.extract_values(req) } self.protocol.send(request_message) # wait for a response while request_id not in self.responses.keys(): with self.lock: if self.shutdown_requested: break time.sleep(0) with self.lock: self.active_requests -= 1 if self.shutdown_requested: self.protocol.log( "warning", "Service %s was unadvertised with a service call in progress, " "aborting service call with request ID %s" % (self.service_name, request_id)) return None resp = self.responses[request_id] del self.responses[request_id] return resp
def call_service(node_handle, service, args=None): # Given the service name, fetch the type and class of the service, # and a request instance # This should be equivalent to rospy.resolve_name. service = expand_topic_name(service, node_handle.get_name(), node_handle.get_namespace()) service_names_and_types = dict(node_handle.get_service_names_and_types()) service_type = service_names_and_types.get(service) if service_type is None: raise InvalidServiceException(service) # service_type is a tuple of types at this point; only one type is supported. if len(service_type) > 1: node_handle.get_logger().warning( 'More than one service type detected: {}'.format(service_type)) service_type = service_type[0] service_class = get_service_class(service_type) inst = get_service_request_instance(service_type) # Populate the instance with the provided args args_to_service_request_instance(service, inst, args) client = node_handle.create_client(service_class, service) future = client.call_async(inst) spin_until_future_complete(node_handle, future) if future.result() is not None: # Turn the response into JSON and pass to the callback json_response = extract_values(future.result()) else: raise Exception(future.exception()) return json_response
def do_test(self, orig_msg, msgtype): for msg in [orig_msg, loads(dumps(orig_msg))]: inst = ros_loader.get_message_instance(msgtype) c.populate_instance(msg, inst) self.validate_instance(inst) extracted = c.extract_values(inst) for msg2 in [extracted, loads(dumps(extracted))]: self.msgs_equal(msg, msg2)
def handle_service_request(self, req): # stay in this loop until (this) instance is not waiting for response for an "old" request # (.. probably not best solution so far) while not self.spawned or self.busy: # if stop_service was called.. if self.finish_flag: # kill unsent requests to that service return None time.sleep(self.wait_for_busy_service_provider) self.busy = True # generate request_id # ..service_name avoids having same id's for different service-requests request_id = "service:" + self.service_name + "_count:" + str(self.request_counter) + "_time:" + datetime.now().strftime("%H:%M:%f") # increment request_counter self.request_counter = (self.request_counter + 1) % self.max_requests req_extracted = message_conversion.extract_values(req) request_message_object = {"op":"service_request", "request_id": request_id, "args": req_extracted } # add request to request_list if request_id not in self.protocol.request_list.keys(): # put information about request into request_list, we need this later to create a response instance with service-module and -type self.protocol.request_list[request_id] = { "service_name" : self.service_name, "service_module" : self.service_module, "service_type" : self.service_type } # answer will be passed to client that requested service answer = None try: # send JSON-service request to client that is providing the service (via protocol) self.client_callback( request_message_object ) begin = datetime.now() duration = datetime.now() - begin # wait for service response by checking response_list # ..if stop_service was called.. stop waiting for response while not self.finish_flag and request_id not in self.response_list.keys() and duration.total_seconds() < self.service_request_timeout: time.sleep(self.check_response_delay) duration = datetime.now() - begin # if response found.. if request_id in self.response_list: answer = self.response_list[request_id] # remove response from response_list del self.response_list[request_id] else: # request failed due to timeout (or some other reason?!) print "request timed out!" answer = None # remove request from request_list del self.protocol.request_list[request_id] # TODO: more detailed exception handling except Exception, e: print e
def CBMap(msg) : json_msg = message_conversion.extract_values(msg) outgoing_msg = {"op": "publish", "topic": "/map", "msg": json_msg}#.data} #print('recv ros map msg:', msg.data) print("will deal, Callback thread=",threading.current_thread().name, threading.currentThread()) outgoing_msg_dumped = dumps(outgoing_msg) outgoing_msg = {"op": "png", "data": encode(outgoing_msg_dumped)} #print ('map png:[', outgoing_msg,']') map_pub.publish(String(dumps(outgoing_msg)))
def CBMap(msg): json_msg = message_conversion.extract_values(msg) outgoing_msg = {"op": "publish", "topic": "/map", "msg": json_msg} #.data} #print('recv ros map msg:', msg.data) print("will deal, Callback thread=", threading.current_thread().name, threading.currentThread()) outgoing_msg_dumped = dumps(outgoing_msg) outgoing_msg = {"op": "png", "data": encode(outgoing_msg_dumped)} #print ('map png:[', outgoing_msg,']') map_pub.publish(String(dumps(outgoing_msg)))
def do_primitive_test(self, data_value, msgtype): for msg in [{"data": data_value}, loads(dumps({"data": data_value}))]: inst = ros_loader.get_message_instance(msgtype) c.populate_instance(msg, inst) self.assertEqual(inst.data, data_value) self.validate_instance(inst) extracted = c.extract_values(inst) for msg2 in [extracted, loads(dumps(extracted))]: self.msgs_equal(msg, msg2) self.assertEqual(msg["data"], msg2["data"]) self.assertEqual(msg2["data"], inst.data)
def test_assorted_msgs(self): assortedmsgs = ["geometry_msgs/Pose", "actionlib_msgs/GoalStatus", "geometry_msgs/WrenchStamped", "stereo_msgs/DisparityImage", "nav_msgs/OccupancyGrid", "geometry_msgs/Point32", "std_msgs/String", "trajectory_msgs/JointTrajectoryPoint", "diagnostic_msgs/KeyValue", "visualization_msgs/InteractiveMarkerUpdate", "nav_msgs/GridCells", "sensor_msgs/PointCloud2"] for rostype in assortedmsgs: inst = ros_loader.get_message_instance(rostype) msg = c.extract_values(inst) self.do_test(msg, rostype) l = loads(dumps(msg)) inst2 = ros_loader.get_message_instance(rostype) c.populate_instance(msg, inst2) self.assertEqual(inst, inst2)
def callback(self, req): self.req = req time.sleep(0.5) rsp = self.srvClass._response_class() gen = c.extract_values(rsp) gen = populate_random_args(gen) try: rsp = c.populate_instance(gen, rsp) except: print "populating instance" print rsp print "populating with" print gen raise self.output = gen return rsp
def test_time_msg_now(self): msg = {"data": "now"} msgtype = "std_msgs/Time" inst = ros_loader.get_message_instance(msgtype) c.populate_instance(msg, inst) currenttime = rospy.get_rostime() self.validate_instance(inst) extracted = c.extract_values(inst) print(extracted) self.assertIn("data", extracted) self.assertIn("secs", extracted["data"]) self.assertIn("nsecs", extracted["data"]) self.assertNotEqual(extracted["data"]["secs"], 0) self.assertLessEqual(extracted["data"]["secs"], currenttime.secs) self.assertGreaterEqual(currenttime.secs, extracted["data"]["secs"])
def test_time_msg_now(self): msg = {"data": "now"} msgtype = "std_msgs/Time" inst = ros_loader.get_message_instance(msgtype) c.populate_instance(msg, inst) currenttime = rospy.get_rostime() self.validate_instance(inst) extracted = c.extract_values(inst) print extracted self.assertIn("data", extracted) self.assertIn("secs", extracted["data"]) self.assertIn("nsecs", extracted["data"]) self.assertNotEqual(extracted["data"]["secs"], 0) self.assertLessEqual(extracted["data"]["secs"], currenttime.secs) self.assertGreaterEqual(currenttime.secs, extracted["data"]["secs"])
def callback(self, req): self.req = req time.sleep(0.5) rsp = self.srvClass._response_class() gen = c.extract_values(rsp) gen = populate_random_args(gen) try: rsp = c.populate_instance(gen, rsp) except: print("populating instance") print(rsp) print("populating with") print(gen) raise self.output = gen return rsp
def call_service(service, args=None): # Given the service name, fetch the type and class of the service, # and a request instance service_type = get_service_type(str(service)) if service_type is None: raise InvalidServiceException(service) service_class = get_service_class(service_type) inst = get_service_request_instance(service_type) # Populate the instance with the provided args args_to_service_request_instance(service, inst, args) # Call the service proxy = ServiceProxy(service, service_class) response = proxy.call(inst) # Turn the response into JSON and pass to the callback json_response = extract_values(response) return json_response
def handle_request(self, req): # generate a unique ID request_id = "service_request:" + self.service_name + ":" + str(self.next_id()) # build a request to send to the external client request_message = { "op": "call_service", "id": request_id, "service": self.service_name, "args": message_conversion.extract_values(req) } self.protocol.send(request_message) # wait for a response while request_id not in self.responses.keys(): time.sleep(0) resp = self.responses[request_id] del self.responses[request_id] return resp
def handle_request(self, req): # generate a unique ID request_id = "service_request:" + \ self.service_name + ":" + str(self.next_id()) # build a request to send to the external client request_message = { "op": "call_service", "id": request_id, "service": self.service_name, "args": message_conversion.extract_values(req) } self.protocol.send(request_message) # wait for a response while request_id not in self.responses.keys(): time.sleep(0) resp = self.responses[request_id] del self.responses[request_id] return resp
def _joystick_triggered(self, joystick): if joystick.buttons[self.trigger_button] == 1: rospy.loginfo("Data collection triggered. Writing data to file...") self.id += 1 self.data_list += [{'id': self.id, 'data': extract_values(self.curr_data)}]
def to_json(msg): if msg.__class__ in simple_types: return json.dumps(msg) print msg return json.dumps(message_conversion.extract_values(msg))
def handle_service_request(self, req): # stay in this loop until (this) instance is not waiting for response for an "old" request # (.. probably not best solution so far) while not self.spawned or self.busy: # if stop_service was called.. if self.finish_flag: # kill unsent requests to that service return None time.sleep(self.wait_for_busy_service_provider) self.busy = True # generate request_id # ..service_name avoids having same id's for different service-requests request_id = "service:" + self.service_name + "_count:" + str( self.request_counter) + "_time:" + datetime.now().strftime( "%H:%M:%f") # increment request_counter self.request_counter = (self.request_counter + 1) % self.max_requests req_extracted = message_conversion.extract_values(req) request_message_object = { "op": "service_request", "request_id": request_id, "args": req_extracted } # add request to request_list if request_id not in self.protocol.request_list.keys(): # put information about request into request_list, we need this later to create a response instance with service-module and -type self.protocol.request_list[request_id] = { "service_name": self.service_name, "service_module": self.service_module, "service_type": self.service_type } # answer will be passed to client that requested service answer = None try: # send JSON-service request to client that is providing the service (via protocol) self.client_callback(request_message_object) begin = datetime.now() duration = datetime.now() - begin # wait for service response by checking response_list # ..if stop_service was called.. stop waiting for response while not self.finish_flag and request_id not in self.response_list.keys( ) and duration.total_seconds() < self.service_request_timeout: time.sleep(self.check_response_delay) duration = datetime.now() - begin # if response found.. if request_id in self.response_list: answer = self.response_list[request_id] # remove response from response_list del self.response_list[request_id] else: # request failed due to timeout (or some other reason?!) print "request timed out!" answer = None # remove request from request_list del self.protocol.request_list[request_id] # TODO: more detailed exception handling except Exception, e: print e
def to_json(data_list): return json.dumps([message_conversion.extract_values(data.msg) for data in data_list])
def serialize(self, data): return json.dumps(message_conversion.extract_values(data))