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 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 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 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 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 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 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 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(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])
def serialize(self, data):
return json.dumps(message_conversion.extract_values(data))
