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)
