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)