def read(filename): yaml_data = None with open(filename) as f: yaml_data = yaml.load(f) anns_list = [] data_list = [] for t in yaml_data: ann = Annotation() ann.timestamp = rospy.Time.now() ann.world_id = unique_id.toMsg(uuid.UUID('urn:uuid:' + world_id)) ann.data_id = unique_id.toMsg(unique_id.fromRandom()) ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.name = t['name'] ann.type = 'yocs_msgs/Wall' for i in range(0, random.randint(0,11)): ann.keywords.append('kw'+str(random.randint(1,11))) if 'prev_id' in vars(): ann.relationships.append(prev_id) prev_id = ann.id ann.shape = 1 # CUBE ann.color.r = 0.8 ann.color.g = 0.2 ann.color.b = 0.2 ann.color.a = 0.4 ann.size.x = float(t['width']) ann.size.y = float(t['length']) ann.size.z = float(t['height']) ann.pose.header.frame_id = t['frame_id'] ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']) # walls are assumed to lay on the floor, so z coordinate is zero; # but WCF assumes that the annotation pose is the center of the object ann.pose.pose.pose.position.z += ann.size.z/2.0 anns_list.append(ann) object = Wall() object.name = t['name'] object.length = float(t['length']) object.width = float(t['width']) object.height = float(t['height']) object.pose.header.frame_id = t['frame_id'] object.pose.header.stamp = rospy.Time.now() object.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']) data = AnnotationData() data.id = ann.data_id data.data = pickle.dumps(object) data_list.append(data) print ann, object, data return anns_list, data_list
def read(file): yaml_data = None with open(file) as f: yaml_data = yaml.load(f) anns_list = [] data_list = [] for t in yaml_data: ann = Annotation() ann.timestamp = rospy.Time.now() ann.data_id = unique_id.toMsg(unique_id.fromRandom()) ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.world = world ann.name = t['name'] ann.type = 'yocs_msgs/Table' for i in range(0, random.randint(0,11)): ann.keywords.append('kw'+str(random.randint(1,11))) if 'prev_id' in vars(): ann.relationships.append(prev_id) prev_id = ann.id ann.shape = 3 #CYLINDER ann.color.r = 0.2 ann.color.g = 0.2 ann.color.b = 0.8 ann.color.a = 0.5 ann.size.x = float(t['radius'])*2 ann.size.y = float(t['radius'])*2 ann.size.z = float(t['height']) ann.pose.header.frame_id = t['frame_id'] ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']) # tables are assumed to lay on the floor, so z coordinate is zero; # but WCF assumes that the annotation pose is the center of the object ann.pose.pose.pose.position.z += ann.size.z/2.0 anns_list.append(ann) object = Table() object.name = t['name'] object.radius = float(t['radius']) object.height = float(t['height']) object.pose.header.frame_id = t['frame_id'] object.pose.header.stamp = rospy.Time.now() object.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']) data = AnnotationData() data.id = ann.data_id data.type = ann.type data.data = serialize_msg(object) data_list.append(data) return anns_list, data_list
def read(filename): yaml_data = None with open(filename) as f: yaml_data = yaml.load(f) anns_list = [] data_list = [] for t in yaml_data: ann = Annotation() ann.timestamp = rospy.Time.now() ann.world_id = unique_id.toMsg(uuid.UUID('urn:uuid:' + world_id)) ann.data_id = unique_id.toMsg(unique_id.fromRandom()) ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.name = t['name'] ann.type = 'ar_track_alvar/AlvarMarker' for i in range(0, random.randint(0,11)): ann.keywords.append('kw'+str(random.randint(1,11))) # if 'prev_id' in vars(): # ann.relationships.append(prev_id) # prev_id = ann.id ann.shape = 1 # CUBE ann.color.r = 1.0 ann.color.g = 1.0 ann.color.b = 1.0 ann.color.a = 1.0 ann.size.x = 0.18 ann.size.y = 0.18 ann.size.z = 0.01 ann.pose.header.frame_id = t['frame_id'] ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']) anns_list.append(ann) object = AlvarMarker() object.id = t['id'] object.confidence = t['confidence'] object.pose.header.frame_id = t['frame_id'] object.pose.header.stamp = rospy.Time.now() object.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']) data = AnnotationData() data.id = ann.data_id data.data = pickle.dumps(object) data_list.append(data) print ann, object, data return anns_list, data_list
def test_dictionary_with_empty_service(self): from std_srvs.srv import Empty, EmptyRequest, EmptyResponse expected_req = EmptyRequest() expected_res = EmptyResponse() dictionary_req = {} dictionary_res = {} message = message_converter.convert_dictionary_to_ros_message('std_srvs/Empty', dictionary_req, 'request') expected_req = serialize_deserialize(expected_req) self.assertEqual(message, expected_req) message = message_converter.convert_dictionary_to_ros_message('std_srvs/Empty', dictionary_res, 'response') expected_res = serialize_deserialize(expected_res) self.assertEqual(message, expected_res)
def test_dictionary_with_trigger_service(self): from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse expected_req = TriggerRequest() expected_res = TriggerResponse(success=True, message='Success!') dictionary_req = {} dictionary_res = { 'success': True, 'message': 'Success!' } message = message_converter.convert_dictionary_to_ros_message('std_srvs/Trigger', dictionary_req, 'request') expected_req = serialize_deserialize(expected_req) self.assertEqual(message, expected_req) message = message_converter.convert_dictionary_to_ros_message('std_srvs/Trigger', dictionary_res, 'response') expected_res = serialize_deserialize(expected_res) self.assertEqual(message, expected_res)
def test_dictionary_with_setbool_service(self): from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse expected_req = SetBoolRequest(data=True) expected_res = SetBoolResponse(success=True, message='Success!') dictionary_req = { 'data': True } dictionary_res = { 'success': True, 'message': 'Success!' } message = message_converter.convert_dictionary_to_ros_message('std_srvs/SetBool', dictionary_req, 'request') expected_req = serialize_deserialize(expected_req) self.assertEqual(message, expected_req) message = message_converter.convert_dictionary_to_ros_message('std_srvs/SetBool', dictionary_res, 'response') expected_res = serialize_deserialize(expected_res) self.assertEqual(message, expected_res)
def see(self, depth, width, height): q = {'detect': {'pose': ''}} self.print_with_prefix('sending: {}'.format(q)) req = RSQueryServiceRequest() req.query = json.dumps(q) rospy.sleep(0.4) objects = [] expected_volume = self.volume(depth, width, height) while len(objects) < 5: result = self.robosherlock_service.call(req) answers = [json.loads(x) for x in result.answer] if not answers: continue real_object = min( answers, key=lambda x: abs(self.answer_volume(x) - expected_volume)) if abs( 1 - self.answer_front_area(real_object) / (width * height)) > 0.3: #175: # reject if more than x% diff continue try: pose = message_converter.convert_dictionary_to_ros_message( 'geometry_msgs/PoseStamped', real_object['poses'][0]['pose_stamped']) except: continue pose.pose.position.z += real_object['boundingbox'][ 'dimensions-3D']['height'] / 2. objects.append(pose) self.print_with_prefix('found pose number {}'.format(len(objects))) objects = self.filter_outlier(objects) p = self.avg_pose(objects) # p.pose.position.z += depth/2. return transform_pose('map', p)
def read_pose_files(pose_files): return [ tf_conversions.fromMsg( message_converter.convert_dictionary_to_ros_message( 'geometry_msgs/Pose', json.loads(read_file(p)))) for p in pose_files ]
def load_target_packing_list(file_path): print "load target list: %s" % "/".join(__file__.split("/")[:-1]) + "/" + file_path f = open("/".join(saphari_task_requirements_list.__file__.split("/")[:-1]) + "/res/" + file_path, 'r+') json_str = f.read() msg_dict = json.loads(json_str) return message_converter.convert_dictionary_to_ros_message('saphari_task_requirements_list/TaskList', msg_dict)
def stop_floor_detection(self, shelf_id): shelf_frame = self.knowrob.get_perceived_frame_id(shelf_id) if self.robosherlock and self.floor_detection: req = RSQueryServiceRequest() q = { 'scan': { 'type': 'shelf', 'location': shelf_frame, 'command': 'stop' } } req.query = json.dumps(q) result = self.robosherlock_service.call(req) floors = [] for floor in result.answer: p = message_converter.convert_dictionary_to_ros_message( 'geometry_msgs/PoseStamped', json.loads(floor)['poses'][0]['pose_stamped']) p = self.tf.transform_pose(shelf_frame, p) floors.append([0, p.pose.position.y, p.pose.position.z]) floors = list(sorted(floors, key=lambda x: x[-1])) floors = [x for x in floors if x[-1] > 0.3] floors = [FLOORS[0][0]] + floors print('detected shelfs at heights: {}'.format(floors)) # TODO remove this if floor detection works shelf_pose = self.tf.lookup_transform(MAP, shelf_frame) floors = FLOORS[int(shelf_pose.pose.position.x)] else: shelf_pose = self.tf.lookup_transform(MAP, shelf_frame) floors = FLOORS[int(shelf_pose.pose.position.x)] return floors
def test_dictionary_with_uint64(self): from std_msgs.msg import UInt64 expected_message = UInt64(data=0xFFFFFFFFFFFFFFFF) dictionary = {'data': expected_message.data} message = message_converter.convert_dictionary_to_ros_message( 'std_msgs/UInt64', dictionary) self.assertEqual(message, expected_message)
def test_dictionary_with_int32(self): from std_msgs.msg import Int32 expected_message = Int32(data=-0x7FFFFFFF) dictionary = {'data': expected_message.data} message = message_converter.convert_dictionary_to_ros_message( 'std_msgs/Int32', dictionary) self.assertEqual(message, expected_message)
def test_dictionary_with_empty(self): from std_msgs.msg import Empty expected_message = Empty() dictionary = {} message = message_converter.convert_dictionary_to_ros_message( 'std_msgs/Empty', dictionary) self.assertEqual(message, expected_message)
def test_dictionary_with_char(self): from std_msgs.msg import Char expected_message = Char(data='c') dictionary = {'data': expected_message.data} message = message_converter.convert_dictionary_to_ros_message( 'std_msgs/Char', dictionary) self.assertEqual(message, expected_message)
def test_dictionary_with_byte(self): from std_msgs.msg import Byte expected_message = Byte(data=3) dictionary = {'data': expected_message.data} message = message_converter.convert_dictionary_to_ros_message( 'std_msgs/Byte', dictionary) self.assertEqual(message, expected_message)
def test_dictionary_with_bool(self): from std_msgs.msg import Bool expected_message = Bool(data=True) dictionary = {'data': expected_message.data} message = message_converter.convert_dictionary_to_ros_message( 'std_msgs/Bool', dictionary) self.assertEqual(message, expected_message)
def test_dictionary_with_string(self): from std_msgs.msg import String expected_message = String(data='Hello') dictionary = {'data': expected_message.data} message = message_converter.convert_dictionary_to_ros_message( 'std_msgs/String', dictionary) self.assertEqual(message, expected_message)
def test_dictionary_with_float64(self): from std_msgs.msg import Float64 expected_message = Float64(data = struct.unpack('<d', '\x7F\xEF\xFF\xFF\xFF\xFF\xFF\xFD')[0]) dictionary = { 'data': expected_message.data } message = message_converter.convert_dictionary_to_ros_message('std_msgs/Float64', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def publish(self, msg): if isinstance(msg, str) or isinstance(msg, unicode): # Message is a JSON string self._pub.publish(json_message_converter.convert_json_to_ros_message(self.topic.type, msg)) elif isinstance(msg, dict): # Message is Python dictionary self._pub.publish(message_converter.convert_dictionary_to_ros_message(self.topic.type, msg))
def test_dictionary_with_child_message(self): from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension expected_message = Float64MultiArray(layout=MultiArrayLayout( dim=[ MultiArrayDimension(label='Dimension1', size=12, stride=7), MultiArrayDimension(label='Dimension2', size=90, stride=8) ], data_offset=1), data=[1.1, 2.2, 3.3]) dictionary = { 'layout': { 'dim': [{ 'label': expected_message.layout.dim[0].label, 'size': expected_message.layout.dim[0].size, 'stride': expected_message.layout.dim[0].stride }, { 'label': expected_message.layout.dim[1].label, 'size': expected_message.layout.dim[1].size, 'stride': expected_message.layout.dim[1].stride }], 'data_offset': expected_message.layout.data_offset }, 'data': expected_message.data } message = message_converter.convert_dictionary_to_ros_message( 'std_msgs/Float64MultiArray', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def test_dictionary_with_nested_additional_args_strict_mode(self): from base64 import b64encode expected_data = bytes(bytearray([97, 98, 99])) dictionary = { "arrays": [{ "data": b64encode(expected_data), "additional_args": "should raise value error" }] } with self.assertRaises(ValueError) as context: message_converter.convert_dictionary_to_ros_message( 'rospy_message_converter/NestedUint8ArrayTestMessage', dictionary) self.assertEqual( 'ROS message type "rospy_message_converter/Uint8ArrayTestMessage" has no field named "additional_args"', context.exception.args[0])
def test_dictionary_with_char(self): from std_msgs.msg import Char expected_message = Char(data = 99) dictionary = { 'data': expected_message.data } message = message_converter.convert_dictionary_to_ros_message('std_msgs/Char', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def test_dictionary_with_child_message(self): from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension expected_message = Float64MultiArray( layout = MultiArrayLayout( dim = [ MultiArrayDimension(label = 'Dimension1', size = 12, stride = 7), MultiArrayDimension(label = 'Dimension2', size = 90, stride = 8) ], data_offset = 1 ), data = [1.1, 2.2, 3.3] ) dictionary = { 'layout': { 'dim': [ { 'label' : expected_message.layout.dim[0].label, 'size' : expected_message.layout.dim[0].size, 'stride' : expected_message.layout.dim[0].stride }, { 'label' : expected_message.layout.dim[1].label, 'size' : expected_message.layout.dim[1].size, 'stride' : expected_message.layout.dim[1].stride } ], 'data_offset': expected_message.layout.data_offset }, 'data': expected_message.data } message = message_converter.convert_dictionary_to_ros_message('std_msgs/Float64MultiArray', dictionary) self.assertEqual(message, expected_message)
def test_dictionary_with_string(self): from std_msgs.msg import String expected_message = String(data = 'Hello') dictionary = { 'data': expected_message.data } message = message_converter.convert_dictionary_to_ros_message('std_msgs/String', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def test_dictionary_with_int32(self): from std_msgs.msg import Int32 expected_message = Int32(data = -0x7FFFFFFF) dictionary = { 'data': expected_message.data } message = message_converter.convert_dictionary_to_ros_message('std_msgs/Int32', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def test_dictionary_with_uint64(self): from std_msgs.msg import UInt64 expected_message = UInt64(data = 0xFFFFFFFFFFFFFFFF) dictionary = { 'data': expected_message.data } message = message_converter.convert_dictionary_to_ros_message('std_msgs/UInt64', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def test_dictionary_with_array(self): from rospy_message_converter.msg import TestArray expected_message = TestArray(data=[1.1, 2.2, 3.3, 4.4]) dictionary = {'data': expected_message.data} message = message_converter.convert_dictionary_to_ros_message( 'rospy_message_converter/TestArray', dictionary) self.assertEqual(message, expected_message)
def load_target_packing_list(self, file_path): print "load target list: %s" % "/".join(__file__.split("/")[:-1]) + "/" + file_path f = open("/".join(saphari_task_requirements_list.__file__.split("/")[:-1]) + "/res/" + file_path, 'r+') json_str = f.read() msg_dict = json.loads(json_str) return message_converter.convert_dictionary_to_ros_message('saphari_task_requirements_list/TaskList', msg_dict)
def test_dictionary_with_empty(self): from std_msgs.msg import Empty expected_message = Empty() dictionary = {} message = message_converter.convert_dictionary_to_ros_message('std_msgs/Empty', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def test_dictionary_with_byte(self): from std_msgs.msg import Byte expected_message = Byte(data = 3) dictionary = { 'data': expected_message.data } message = message_converter.convert_dictionary_to_ros_message('std_msgs/Byte', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def test_dictionary_with_array(self): from rospy_message_converter.msg import TestArray expected_message = TestArray(data = [1.1, 2.2, 3.3, 4.4]) dictionary = { 'data': expected_message.data } message = message_converter.convert_dictionary_to_ros_message('rospy_message_converter/TestArray', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def create_msg(self, msg_dict, msg_type): namespace, msg_name = msg_type.split("/") mod = __import__(namespace + ".msg") msg_cls = getattr(mod.msg, msg_name) msg = message_converter.convert_dictionary_to_ros_message( msg_type, msg_dict) return msg, msg_cls
def callback(self, msg_dict): msg_dict = _prepend_tf_prefix_dict_filter(msg_dict) if self.topic_config.dict_filter is not None: msg_dict = self.topic_config.dict_filter(msg_dict) msg = message_converter.convert_dictionary_to_ros_message( self.topic_config.topic_type._type, msg_dict) self.pub.publish(msg)
def test_dictionary_with_uint8(self): from std_msgs.msg import UInt8 expected_message = UInt8(data=0xFF) dictionary = {'data': expected_message.data} message = message_converter.convert_dictionary_to_ros_message( 'std_msgs/UInt8', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def test_dictionary_with_duration(self): from std_msgs.msg import Duration duration = rospy.rostime.Duration(33, 25) expected_message = Duration(data=duration) dictionary = {'data': {'secs': duration.secs, 'nsecs': duration.nsecs}} message = message_converter.convert_dictionary_to_ros_message( 'std_msgs/Duration', dictionary) self.assertEqual(message, expected_message)
def publish_transform(self): transformation = self.tmap["transformation"] trans = message_converter.convert_dictionary_to_ros_message( "geometry_msgs/Vector3", transformation["translation"]) rot = message_converter.convert_dictionary_to_ros_message( "geometry_msgs/Quaternion", transformation["rotation"]) msg = TransformStamped() msg.header.stamp = rospy.Time.now() msg.header.frame_id = transformation["parent"] msg.child_frame_id = transformation["child"] msg.transform.translation = trans msg.transform.rotation = rot self.broadcaster.sendTransform(msg)
def test_dictionary_with_missing_field_unchecked(self): from std_msgs.msg import Bool expected_message = Bool(data=False) dictionary = {} message = message_converter.convert_dictionary_to_ros_message( 'std_msgs/Bool', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def load_message(self): data = load(open(self.filepath)) if self.message_type in ACCEPTED_MESSAGES: self.message = message_converter.convert_dictionary_to_ros_message(self.message_type, data) return True else: print "Error!", self.message_type , " not in list of accepted messages" return False
def test_dictionary_with_3uint8_array_list(self): from rospy_message_converter.msg import Uint8Array3TestMessage expected_message = Uint8Array3TestMessage(data=[97, 98, 99]) dictionary = {'data': expected_message.data} message = message_converter.convert_dictionary_to_ros_message( 'rospy_message_converter/Uint8Array3TestMessage', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def test_dictionary_with_time(self): from std_msgs.msg import Time now_time = rospy.Time.now() expected_message = Time(data=now_time) dictionary = {'data': {'secs': now_time.secs, 'nsecs': now_time.nsecs}} message = message_converter.convert_dictionary_to_ros_message( 'std_msgs/Time', dictionary) self.assertEqual(message, expected_message)
def test_dictionary_with_unicode(self): from std_msgs.msg import String expected_message = String(data = 'Hello') dictionary = { 'data': u'Hello' } message = message_converter.convert_dictionary_to_ros_message('std_msgs/String', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message.data,expected_message.data) self.assertEqual(type(message.data),type(expected_message.data))
def test_dictionary_with_empty_additional_args_forgiving(self): from std_msgs.msg import Empty expected_message = Empty() dictionary = {"additional_args": "should be ignored"} message = message_converter.convert_dictionary_to_ros_message( 'std_msgs/Empty', dictionary, strict_mode=False) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def make_goal(self,location): goal = MoveBaseGoal() pose = deepcopy(self.a_locations[location]) del pose['covariance'] goal_pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',pose) goal.target_pose.pose = goal_pose goal.target_pose.header.frame_id = self.a_frame goal.target_pose.header.stamp = rospy.Time.now() return goal
def test_dictionary_with_nested_missing_field_unchecked(self): from rospy_message_converter.msg import NestedUint8ArrayTestMessage, Uint8ArrayTestMessage expected_message = NestedUint8ArrayTestMessage( arrays=[Uint8ArrayTestMessage(data=[])]) dictionary = {"arrays": [{}]} message = message_converter.convert_dictionary_to_ros_message( 'rospy_message_converter/NestedUint8ArrayTestMessage', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def insert_site(site): req = AddSemanticRegionRequest() req.name = site['name'] req.pose_stamped.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',site['pose']) req.pose_stamped.header.frame_id = site['frame_id'] req.region.radius = float(site['radius']) return req
def test_dictionary_with_float64(self): from std_msgs.msg import Float64 expected_message = Float64( data=struct.unpack('<d', '\x7F\xEF\xFF\xFF\xFF\xFF\xFF\xFD')[0]) dictionary = {'data': expected_message.data} message = message_converter.convert_dictionary_to_ros_message( 'std_msgs/Float64', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def load_example_services(self): filename = rospy.get_param('~example_services') with open(filename) as f: profile = yaml.load(f) msg = message_converter.convert_dictionary_to_ros_message('concert_msgs/Services', profile) self.profile = profile self.services = msg
def read_transform_stamped_files(pose_files): transforms = [ message_converter.convert_dictionary_to_ros_message( 'geometry_msgs/TransformStamped', json.loads(read_file(p))).transform for p in pose_files ] return [ tf_conversions.fromMsg(Pose(tf.translation, tf.rotation)) for tf in transforms ]
def test_dictionary_with_3uint8_array_bytes(self): from rospy_message_converter.msg import Uint8Array3TestMessage from base64 import b64encode expected_message = Uint8Array3TestMessage( data=bytes(bytearray([97, 98, 99]))) dictionary = {'data': b64encode(expected_message.data)} message = message_converter.convert_dictionary_to_ros_message( 'rospy_message_converter/Uint8Array3TestMessage', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def test_dictionary_with_duration(self): from std_msgs.msg import Duration duration = rospy.rostime.Duration(33, 25) expected_message = Duration(data = duration) dictionary = { 'data': { 'secs' : duration.secs, 'nsecs' : duration.nsecs } } message = message_converter.convert_dictionary_to_ros_message('std_msgs/Duration', dictionary) self.assertEqual(message, expected_message)
def convert_json_to_ros_message(message_type, json_message): """ Takes in the message type and a JSON-formatted string and returns a ROS message. Example: message_type = "std_msgs/String" json_message = '{"data": "Hello, Robot"}' ros_message = convert_json_to_ros_message(message_type, json_message) """ dictionary = json.loads(json_message) return message_converter.convert_dictionary_to_ros_message(message_type, dictionary)
def test_dictionary_with_time(self): from std_msgs.msg import Time now_time = rospy.Time.now() expected_message = Time(data=now_time) dictionary = { 'data': { 'secs' : now_time.secs, 'nsecs' : now_time.nsecs } } message = message_converter.convert_dictionary_to_ros_message('std_msgs/Time', dictionary) self.assertEqual(message, expected_message)
def test_dictionary_with_header(self): from std_msgs.msg import Header rospy.init_node('time_node') now_time = rospy.Time.now() expected_message = Header( stamp = now_time, frame_id = 'my_frame', seq = 12 ) dictionary = { 'stamp': { 'secs': now_time.secs, 'nsecs': now_time.nsecs }, 'frame_id' : expected_message.frame_id, 'seq': expected_message.seq } message = message_converter.convert_dictionary_to_ros_message('std_msgs/Header', dictionary) self.assertEqual(message, expected_message)
def test_dictionary_with_header_with_no_prefix(self): from std_msgs.msg import Header from time import time now_time = rospy.Time(time()) expected_message = Header( stamp = now_time, frame_id = 'my_frame', seq = 12 ) dictionary = { 'stamp': { 'secs': now_time.secs, 'nsecs': now_time.nsecs }, 'frame_id' : expected_message.frame_id, 'seq': expected_message.seq } message = message_converter.convert_dictionary_to_ros_message('Header', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def publish(filename): yaml_data = None with open(filename) as f: yaml_data = yaml.load(f) wall_list = WallList() # Markers marker_list = MarkerArray() marker_id = 1 for t in yaml_data: object = Wall() object.name = t['name'] object.length = float(t['length']) object.width = float(t['width']) object.height = float(t['height']) object.pose.header.frame_id = t['frame_id'] object.pose.header.stamp = rospy.Time.now() object.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']) wall_list.obstacles.append(object) marker = Marker() marker.id = marker_id marker.header = object.pose.header marker.type = Marker.CUBE marker.ns = "wall_obstacles" marker.action = Marker.ADD marker.lifetime = rospy.Duration.from_sec(0) marker.pose = copy.deepcopy(object.pose.pose.pose) marker.pose.position.z += object.height/2.0 marker.scale.x = object.width marker.scale.y = object.length marker.scale.z = object.height marker.color.r = 0.2 marker.color.g = 0.4 marker.color.b = 0.4 marker.color.a = 0.5 marker_list.markers.append(marker) marker_id = marker_id + 1 wall_pub.publish(wall_list) marker_pub.publish(marker_list) return
def process_enable_service(self, req): en = 'enabled' if req.enable else 'disabled' self.loginfo(str(req.name) + ' : ' + en) service_found = False for service in self.profile['services']: if req.name == service['name']: service['enabled'] = req.enable service_found = True if service_found: msg = message_converter.convert_dictionary_to_ros_message('concert_msgs/Services', self.profile) self.services = msg self.pub['service_list'].publish(self.services) #self.running = req.enable #if self.running: # threading.Thread(target=self.moving).start() return concert_srvs.EnableServiceResponse(True, '') else: return concert_srvs.EnableServiceResponse(False,'Service not found')
def read(filename): ''' Parse a yaml file containing a single map message @param filename Target file path ''' yaml_data = None with open(filename) as f: yaml_data = yaml.load(f) ann = Annotation() ann.timestamp = rospy.Time.now() ann.world_id = unique_id.toMsg(uuid.UUID('urn:uuid:' + world_id)) ann.data_id = unique_id.toMsg(unique_id.fromRandom()) ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.name = map_name ann.type = 'nav_msgs/OccupancyGrid' ann.keywords.append(map_name) ann.shape = 1 # CUBE ann.color.r = 0.2 ann.color.g = 0.2 ann.color.b = 0.2 ann.color.a = 1.0 ann.size.x = yaml_data['info']['width'] * yaml_data['info']['resolution'] ann.size.y = yaml_data['info']['height'] * yaml_data['info']['resolution'] ann.size.z = 0.000001 ann.pose.header.frame_id = '/map' ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose', yaml_data['info']['origin']) object = OccupancyGrid() genpy.message.fill_message_args(object, yaml_data) map = AnnotationData() map.id = ann.data_id map.data = pickle.dumps(object) print ann return [ann], [map] # return as lists, as is what expects save_annotations_data service
def test_dictionary_with_float64(self): from std_msgs.msg import Float64 expected_message = Float64(data = 0x7FEFFFFFFFFFFFF) dictionary = { 'data': expected_message.data } message = message_converter.convert_dictionary_to_ros_message('std_msgs/Float64', dictionary) self.assertEqual(message, expected_message)
def convert_json_to_ros_message(message_type, json_message): dictionary = json.loads(json_message) return message_converter.convert_dictionary_to_ros_message(message_type, dictionary)
def test_dictionary_with_uint8(self): from std_msgs.msg import UInt8 expected_message = UInt8(data = 0xFF) dictionary = { 'data': expected_message.data } message = message_converter.convert_dictionary_to_ros_message('std_msgs/UInt8', dictionary) self.assertEqual(message, expected_message)