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)
Пример #2
0
    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)
Пример #5
0
    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
Пример #6
0
    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 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
Пример #8
0
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)
Пример #10
0
 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)
Пример #11
0
    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
Пример #12
0
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)))
Пример #13
0
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)))
Пример #14
0
 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 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)
Пример #17
0
 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
Пример #19
0
    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
Пример #22
0
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
Пример #23
0
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
Пример #25
0
    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 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()
Пример #29
0
    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
Пример #30
0
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 to_json(data_list):
     return json.dumps([message_conversion.extract_values(data.msg) for data in data_list])
Пример #32
0
 def serialize(self, data):
     return json.dumps(message_conversion.extract_values(data))