def on_event(self, message):
#process the message
#replace JSON Null values in float32 types with infinity datatype (changed according to the error for LaserScan values)
message = message.replace("null", "Infinity")
msg_dict = json.loads(message)
topic_name = msg_dict['topic_name']
topic_type = msg_dict['topic_type']
del msg_dict['topic_name']
del msg_dict['topic_type']
#get the topic type
msg_class = ros_loader.get_message_class(topic_type)
# Create a message instance
inst = msg_class()
# Populate the instance, propagating any exceptions that may be thrown
message_conversion.populate_instance(msg_dict, inst)
if not topic_name in self.pub_objects:
self.pub_objects[topic_name] = rospy.Publisher(topic_name,
msg_class,
latch=True,
queue_size=10)
self.pub_objects[topic_name].publish(inst)
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 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 args_to_service_request_instance(service, inst, args):
""" Populate a service request instance with the provided args
args can be a dictionary of values, or a list, or None
Propagates any exceptions that may be raised. """
msg = {}
if isinstance(args, list):
msg = dict(zip(inst.__slots__, args))
elif isinstance(args, dict):
msg = args
# Populate the provided instance, propagating any exceptions
populate_instance(msg, inst)
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 service_response(self, message):
# check for the service
service_name = message["service"]
if service_name in self.protocol.external_service_list:
service_handler = self.protocol.external_service_list[service_name]
# parse the message
request_id = message["id"]
values = message["values"]
# create a message instance
resp = ros_loader.get_service_response_instance(service_handler.service_type)
message_conversion.populate_instance(values, resp)
# pass along the response
service_handler.responses[request_id] = resp
else:
self.protocol.log("error", "Service %s has no been advertised externally." % service_name)
def start_activity(self, activity_type, json_goal, desired_activity_id):
# find the python class that implements this activity
activity_class = self._activity_loader.get_activity_class(
activity_type)
activity_goal_class = self._activity_loader.get_activity_goal_class(
activity_type)
# convert the goal from JSON to a ROS message
activity_goal_dict = json.loads(json_goal)
activity_goal = message_conversion.populate_instance(
activity_goal_dict, activity_goal_class)
rospy.loginfo('Starting activity with class: %s',
str(type(activity_class)))
with self._activity_lock:
# create a unique id for this activity
activity_id = generate_unique_id(self._running_activities.keys(),
desired_activity_id)
# create an instance of this activity class
activity_handle = ActivityHandle(activity_id)
activity_object = activity_class(activity_handle, activity_goal)
self._running_activities[activity_id] = (activity_handle,
activity_object,
activity_type)
# start the activity in a separate thread
activity_object.start()
rospy.loginfo('Done starting activity')
return activity_id
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 service_response(self, message):
# convert the message into according response instance
# then just put response into singleton list for responses.. the requesting process will find it there and clean up after delivery to client
# get information about the request with the same id as the incoming response
# ..this information gets written into "request_list" by advertise_service.py within "handle_service_request()"
request = self.protocol.request_list[message["request_id"]]
# get module and type
service_module = request["service_module"]
service_type = request["service_type"]
## Create a message instance
inst = ros_loader.get_service_response_instance(service_module+"/"+service_type)
# Populate the instance, propagating any exceptions that may be thrown
message_conversion.populate_instance(message["data"], inst)
# add response instance to response_list
self.response_list[message["request_id"]] = inst
def publish(self, msg):
""" Publish a message using this publisher.
Keyword arguments:
msg -- the dict (json) message to publish
Throws:
Exception -- propagates exceptions from message conversion if the
provided msg does not properly conform to the message type of this
publisher
"""
# Create a message instance
inst = self.msg_class()
# Populate the instance, propagating any exceptions that may be thrown
message_conversion.populate_instance(msg, inst)
# Publish the message
self.publisher.publish(inst)
def service_response(self, message):
# convert the message into according response instance
# then just put response into singleton list for responses.. the requesting process will find it there and clean up after delivery to client
# get information about the request with the same id as the incoming response
# ..this information gets written into "request_list" by advertise_service.py within "handle_service_request()"
request = self.protocol.request_list[message["request_id"]]
# get module and type
service_module = request["service_module"]
service_type = request["service_type"]
## Create a message instance
inst = ros_loader.get_service_response_instance(service_module + "/" +
service_type)
# Populate the instance, propagating any exceptions that may be thrown
message_conversion.populate_instance(message["data"], inst)
# add response instance to response_list
self.response_list[message["request_id"]] = inst
def publish(self, msg):
""" Publish a message using this publisher.
Keyword arguments:
msg -- the dict (json) message to publish
Throws:
Exception -- propagates exceptions from message conversion if the
provided msg does not properly conform to the message type of this
publisher
"""
# First, check the publisher consistency listener to see if it's done
if self.listener.attached and self.listener.timed_out():
self.listener.detach()
# Create a message instance
inst = self.msg_class()
# Populate the instance, propagating any exceptions that may be thrown
message_conversion.populate_instance(msg, inst)
# Publish the message
self.publisher.publish(inst)
def publish(self, msg):
""" Publish a message using this publisher.
Keyword arguments:
msg -- the dict (json) message to publish
Throws:
Exception -- propagates exceptions from message conversion if the
provided msg does not properly conform to the message type of this
publisher
"""
# First, check the publisher consistency listener to see if it's done
if self.listener.attached and self.listener.timed_out():
self.listener.detach()
# Create a message instance
inst = self.msg_class()
# Populate the instance, propagating any exceptions that may be thrown
message_conversion.populate_instance(msg, inst)
# Publish the message
self.publisher.publish(inst)
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 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 start_activity(self, activity_type, json_goal, desired_activity_id):
# find the python class that implements this activity
activity_class = self._activity_loader.get_activity_class(activity_type)
activity_goal_class = self._activity_loader.get_activity_goal_class(activity_type)
# convert the goal from JSON to a ROS message
activity_goal_dict = json.loads(json_goal)
activity_goal = message_conversion.populate_instance(activity_goal_dict, activity_goal_class)
rospy.loginfo('Starting activity with class: %s', str(type(activity_class)))
with self._activity_lock:
# create a unique id for this activity
activity_id = generate_unique_id(self._running_activities.keys(), desired_activity_id)
# create an instance of this activity class
activity_handle = ActivityHandle(activity_id)
activity_object = activity_class(activity_handle, activity_goal)
self._running_activities[activity_id] = (activity_handle, activity_object, activity_type)
# start the activity in a separate thread
activity_object.start()
rospy.loginfo('Done starting activity')
return activity_id
def test_int8_msg(rostype, data):
msg = {"data": data}
inst = ros_loader.get_message_instance(rostype)
c.populate_instance(msg, inst)
self.validate_instance(inst)
return inst.data
def from_json(json_str, message_class):
json_val = json.loads(json_str)
if message_class in simple_types:
return json_val
return message_conversion.populate_instance(json_val, message_class)
def test_int8_msg(rostype, data):
msg = {"data": data}
inst = ros_loader.get_message_instance(rostype)
c.populate_instance(msg, inst)
self.validate_instance(inst)
return inst.data
def from_json(json_str, message_class):
json_val = json.loads(json_str)
if message_class in simple_types:
return json_val
return message_conversion.populate_instance(json_val, message_class)