def roundtrip(m):
yaml_text = strify_message(m)
print yaml_text
loaded = yaml.load(yaml_text)
print "loaded", loaded
new_inst = m.__class__()
if loaded is not None:
fill_message_args(new_inst, [loaded])
else:
fill_message_args(new_inst, [])
return new_inst
def roundtrip(m):
yaml_text = strify_message(m)
print yaml_text
loaded = yaml.load(yaml_text)
print "loaded", loaded
new_inst = m.__class__()
if loaded is not None:
fill_message_args(new_inst, [loaded])
else:
fill_message_args(new_inst, [])
return new_inst
def test_fill_message_args_embed_time(self):
from roslib.rostime import Time, Duration
from roslib.message import fill_message_args
from test_roslib_comm.msg import FillEmbedTime
# test fill_message_args with embeds and time vals
# time t
# duration d
# std_msgs/String str_msg
# std_msgs/String[] str_msg_array
# int32 i32
tests = [
]
m = FillEmbedTime()
fill_message_args(m, [{}])
self.assertEquals(m.t, Time())
self.assertEquals(m.d, Duration())
self.assertEquals(m.str_msg.data, '')
self.assertEquals(m.str_msg_array, [])
self.assertEquals(m.i32, 0)
# list tests
# - these should be equivalent
equiv = [
[[10, 20], [30, 40], ['foo'], [['bar'], ['baz']], 32],
[{'secs': 10, 'nsecs': 20}, {'secs': 30, 'nsecs': 40}, ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], {'data': 'foo'}, [['bar'], ['baz']], 32],
[[10, 20], [30, 40], ['foo'], [{'data': 'bar'}, {'data': 'baz'}], 32],
[{'t': [10, 20], 'd': [30, 40], 'str_msg': {'data': 'foo'}, 'str_msg_array': [{'data': 'bar'}, {'data': 'baz'}], 'i32': 32}],
[{'t': {'secs': 10, 'nsecs': 20}, 'd': [30, 40], 'str_msg': {'data': 'foo'}, 'str_msg_array': [{'data': 'bar'}, {'data': 'baz'}], 'i32': 32}],
]
for test in equiv:
m = FillEmbedTime()
try:
fill_message_args(m, test)
except Exception, e:
self.fail("failed to fill with : %s\n%s"%(str(test), traceback.format_exc()))
self.assertEquals(m.t, Time(10, 20))
self.assertEquals(m.d, Duration(30, 40))
self.assertEquals(m.str_msg.data, 'foo')
self.assertEquals(len(m.str_msg_array), 2, m.str_msg_array)
self.assertEquals(m.str_msg_array[0].data, 'bar')
self.assertEquals(m.str_msg_array[1].data, 'baz')
self.assertEquals(m.i32, 32)
def rave_env_to_ros(env):
pub = make_planning_scene_pub()
ps=mm.PlanningScene()
with open(osp.join(pbc.miscdata_dir,"planning_scene_prototype.yaml"),"r") as fh:
d = yaml.load(fh)
rm.fill_message_args(ps, d)
cos = ps.world.collision_objects
for body in env.GetBodies():
if body.IsRobot():
rstate = ps.robot_state
rave_joint_vals = body.GetDOFValues()
rave_joint_names = [joint.GetName() for joint in body.GetJoints()]
ros_joint_names = []
ros_joint_values = []
for name in ROS_JOINT_NAMES:
if name in rave_joint_names:
i = rave_joint_names.index(name)
ros_joint_values.append(rave_joint_vals[i])
ros_joint_names.append(name)
rstate.joint_state.position = ros_joint_values
rstate.joint_state.name = ros_joint_names
rstate.multi_dof_joint_state.header.frame_id = 'odom_combined'
rstate.multi_dof_joint_state.header.stamp = rospy.Time.now()
rstate.multi_dof_joint_state.joint_names = ['world_joint']
rstate.multi_dof_joint_state.joint_transforms = [ RosTransformFromRaveMatrix(body.GetTransform())]
else:
for link in body.GetLinks():
co = mm.CollisionObject()
co.operation = co.ADD
co.id = body.GetName()
co.header.frame_id = "odom_combined"
co.header.stamp = rospy.Time.now()
for geom in link.GetGeometries():
trans = RosPoseFromRaveMatrix(body.GetTransform().dot(link.GetTransform().dot(geom.GetTransform())))
if geom.GetType() == openravepy.GeometryType.Trimesh:
mesh = sm.Mesh()
rave_mesh = geom.GetCollisionMesh()
for pt in rave_mesh.vertices:
mesh.vertices.append(gm.Point(*pt))
for tri in rave_mesh.indices:
mt = sm.MeshTriangle()
mt.vertex_indices = tri
mesh.triangles.append(mt)
co.meshes.append(mesh)
co.mesh_poses.append(trans)
else:
shape = sm.SolidPrimitive()
shape.type = shape.BOX
shape.dimensions = geom.GetBoxExtents()*2
co.primitives.append(shape)
co.primitive_poses.append(trans)
cos.append(co)
pub.publish(ps)
return ps
def test_fill_message_args_simple(self):
from roslib.message import fill_message_args
from test_roslib_comm.msg import FillSimple
#int32 i32
#string str
#int32[] i32_array
#bool b
for v in [[], {}]:
try:
fill_message_args(object(), v)
self.fail("should have raised ValueError")
except ValueError: pass
try:
m = FillSimple()
# call underlying routine as the branch is not reachable from above
roslib.message._fill_message_args(m, 1, {}, '')
self.fail("should have raised ValueError for bad msg_args")
except ValueError: pass
simple_tests = [
[1, 'foo', [], True],
[1, 'foo', [1, 2, 3, 4], False],
]
for test in simple_tests:
m = FillSimple()
fill_message_args(m, test)
self.assertEquals(m.i32, test[0])
self.assertEquals(m.str, test[1])
self.assertEquals(m.i32_array, test[2])
self.assertEquals(m.b, test[3])
# test with dictionaries
m = FillSimple()
fill_message_args(m, [{}])
self.assertEquals(m.i32, 0)
self.assertEquals(m.str, '')
self.assertEquals(m.i32_array, [])
self.assertEquals(m.b, False)
m = FillSimple()
fill_message_args(m, [{'i32': 10}])
self.assertEquals(m.i32, 10)
self.assertEquals(m.str, '')
self.assertEquals(m.i32_array, [])
self.assertEquals(m.b, False)
m = FillSimple()
fill_message_args(m, [{'str': 'hello', 'i32_array': [1, 2, 3]}])
self.assertEquals(m.i32, 0)
self.assertEquals(m.str, 'hello')
self.assertEquals(m.i32_array, [1, 2, 3])
self.assertEquals(m.b, False)
# fill_message_args currently does not type check
bad = [
# extra key
[{'bad': 1, 'str': 'hello', 'i32_array': [1, 2, 3]}],
# underfill
[1, 'foo', [1, 2, 3]],
# overfill
[1, 'foo', [1, 2, 3], True, 1],
# non-list value for list field
[1, 'foo', 1, True],
]
for b in bad:
failed = True
try:
m = FillSimple()
fill_message_args(m, b)
except roslib.message.ROSMessageException:
failed = False
self.failIf(failed, "fill_message_args should have failed: %s"%str(b))
except Exception, e:
self.fail("failed to fill with : %s\n%s"%(str(test), traceback.format_exc()))
self.assertEquals(m.t, Time(10, 20))
self.assertEquals(m.d, Duration(30, 40))
self.assertEquals(m.str_msg.data, 'foo')
self.assertEquals(len(m.str_msg_array), 2, m.str_msg_array)
self.assertEquals(m.str_msg_array[0].data, 'bar')
self.assertEquals(m.str_msg_array[1].data, 'baz')
self.assertEquals(m.i32, 32)
# test creation of Time/Duration from single number representation, which is necessary for
# yaml single-number support
# - cannot include in tests above as conversion from integer is lossy
m = FillEmbedTime()
fill_message_args(m, [10000000020, 30000000040, ['foo'], [['bar'], ['baz']], 32])
self.assertEquals(10, m.t.secs)
self.assert_(abs(20 - m.t.nsecs) < 2)
self.assertEquals(30, m.d.secs)
self.assert_(abs(40 - m.d.nsecs) < 2)
self.assertEquals(len(m.str_msg_array), 2, m.str_msg_array)
self.assertEquals(m.str_msg_array[0].data, 'bar')
self.assertEquals(m.str_msg_array[1].data, 'baz')
self.assertEquals(m.i32, 32)
bad = [
# underfill in sub-args
[[10, 20], [30, 40], ['foo'], [['bar'], ['baz']]],
[[10], [30, 40], ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30], ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], [], [['bar'], ['baz']], 32],
def test_fill_message_args_simple(self):
from roslib.message import fill_message_args
from test_roslib_comm.msg import FillSimple
#int32 i32
#string str
#int32[] i32_array
#bool b
for v in [[], {}]:
try:
fill_message_args(object(), v)
self.fail("should have raised ValueError")
except ValueError:
pass
try:
m = FillSimple()
# call underlying routine as the branch is not reachable from above
roslib.message._fill_message_args(m, 1, {}, '')
self.fail("should have raised ValueError for bad msg_args")
except ValueError:
pass
simple_tests = [
[1, 'foo', [], True],
[1, 'foo', [1, 2, 3, 4], False],
]
for test in simple_tests:
m = FillSimple()
fill_message_args(m, test)
self.assertEquals(m.i32, test[0])
self.assertEquals(m.str, test[1])
self.assertEquals(m.i32_array, test[2])
self.assertEquals(m.b, test[3])
# test with dictionaries
m = FillSimple()
fill_message_args(m, [{}])
self.assertEquals(m.i32, 0)
self.assertEquals(m.str, '')
self.assertEquals(m.i32_array, [])
self.assertEquals(m.b, False)
m = FillSimple()
fill_message_args(m, [{'i32': 10}])
self.assertEquals(m.i32, 10)
self.assertEquals(m.str, '')
self.assertEquals(m.i32_array, [])
self.assertEquals(m.b, False)
m = FillSimple()
fill_message_args(m, [{'str': 'hello', 'i32_array': [1, 2, 3]}])
self.assertEquals(m.i32, 0)
self.assertEquals(m.str, 'hello')
self.assertEquals(m.i32_array, [1, 2, 3])
self.assertEquals(m.b, False)
# fill_message_args currently does not type check
bad = [
# extra key
[{
'bad': 1,
'str': 'hello',
'i32_array': [1, 2, 3]
}],
# underfill
[1, 'foo', [1, 2, 3]],
# overfill
[1, 'foo', [1, 2, 3], True, 1],
# non-list value for list field
[1, 'foo', 1, True],
]
for b in bad:
failed = True
try:
m = FillSimple()
fill_message_args(m, b)
except roslib.message.ROSMessageException:
failed = False
self.failIf(failed,
"fill_message_args should have failed: %s" % str(b))
def test_fill_message_args_embed_time(self):
from roslib.rostime import Time, Duration
from roslib.message import fill_message_args
from test_roslib_comm.msg import FillEmbedTime
# test fill_message_args with embeds and time vals
# time t
# duration d
# std_msgs/String str_msg
# std_msgs/String[] str_msg_array
# int32 i32
tests = []
m = FillEmbedTime()
fill_message_args(m, [{}])
self.assertEquals(m.t, Time())
self.assertEquals(m.d, Duration())
self.assertEquals(m.str_msg.data, '')
self.assertEquals(m.str_msg_array, [])
self.assertEquals(m.i32, 0)
# list tests
# - these should be equivalent
equiv = [
[[10, 20], [30, 40], ['foo'], [['bar'], ['baz']], 32],
[{
'secs': 10,
'nsecs': 20
}, {
'secs': 30,
'nsecs': 40
}, ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], {
'data': 'foo'
}, [['bar'], ['baz']], 32],
[[10, 20], [30, 40], ['foo'], [{
'data': 'bar'
}, {
'data': 'baz'
}], 32],
[{
't': [10, 20],
'd': [30, 40],
'str_msg': {
'data': 'foo'
},
'str_msg_array': [{
'data': 'bar'
}, {
'data': 'baz'
}],
'i32': 32
}],
[{
't': {
'secs': 10,
'nsecs': 20
},
'd': [30, 40],
'str_msg': {
'data': 'foo'
},
'str_msg_array': [{
'data': 'bar'
}, {
'data': 'baz'
}],
'i32': 32
}],
]
for test in equiv:
m = FillEmbedTime()
try:
fill_message_args(m, test)
except Exception, e:
self.fail("failed to fill with : %s\n%s" %
(str(test), traceback.format_exc()))
self.assertEquals(m.t, Time(10, 20))
self.assertEquals(m.d, Duration(30, 40))
self.assertEquals(m.str_msg.data, 'foo')
self.assertEquals(len(m.str_msg_array), 2, m.str_msg_array)
self.assertEquals(m.str_msg_array[0].data, 'bar')
self.assertEquals(m.str_msg_array[1].data, 'baz')
self.assertEquals(m.i32, 32)
self.fail("failed to fill with : %s\n%s" %
(str(test), traceback.format_exc()))
self.assertEquals(m.t, Time(10, 20))
self.assertEquals(m.d, Duration(30, 40))
self.assertEquals(m.str_msg.data, 'foo')
self.assertEquals(len(m.str_msg_array), 2, m.str_msg_array)
self.assertEquals(m.str_msg_array[0].data, 'bar')
self.assertEquals(m.str_msg_array[1].data, 'baz')
self.assertEquals(m.i32, 32)
# test creation of Time/Duration from single number representation, which is necessary for
# yaml single-number support
# - cannot include in tests above as conversion from integer is lossy
m = FillEmbedTime()
fill_message_args(
m, [10000000020, 30000000040, ['foo'], [['bar'], ['baz']], 32])
self.assertEquals(10, m.t.secs)
self.assert_(abs(20 - m.t.nsecs) < 2)
self.assertEquals(30, m.d.secs)
self.assert_(abs(40 - m.d.nsecs) < 2)
self.assertEquals(len(m.str_msg_array), 2, m.str_msg_array)
self.assertEquals(m.str_msg_array[0].data, 'bar')
self.assertEquals(m.str_msg_array[1].data, 'baz')
self.assertEquals(m.i32, 32)
bad = [
# underfill in sub-args
[[10, 20], [30, 40], ['foo'], [['bar'], ['baz']]],
[[10], [30, 40], ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30], ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], [], [['bar'], ['baz']], 32],
def rave_env_to_ros(env):
pub = make_planning_scene_pub()
ps = mm.PlanningScene()
with open(osp.join(pbc.miscdata_dir, "planning_scene_prototype.yaml"),
"r") as fh:
d = yaml.load(fh)
rm.fill_message_args(ps, d)
cos = ps.world.collision_objects
for body in env.GetBodies():
if body.IsRobot():
rstate = ps.robot_state
rave_joint_vals = body.GetDOFValues()
rave_joint_names = [joint.GetName() for joint in body.GetJoints()]
ros_joint_names = []
ros_joint_values = []
for name in ROS_JOINT_NAMES:
if name in rave_joint_names:
i = rave_joint_names.index(name)
ros_joint_values.append(rave_joint_vals[i])
ros_joint_names.append(name)
rstate.joint_state.position = ros_joint_values
rstate.joint_state.name = ros_joint_names
rstate.multi_dof_joint_state.header.frame_id = 'odom_combined'
rstate.multi_dof_joint_state.header.stamp = rospy.Time.now()
rstate.multi_dof_joint_state.joint_names = ['world_joint']
rstate.multi_dof_joint_state.joint_transforms = [
RosTransformFromRaveMatrix(body.GetTransform())
]
else:
for link in body.GetLinks():
co = mm.CollisionObject()
co.operation = co.ADD
co.id = body.GetName()
co.header.frame_id = "odom_combined"
co.header.stamp = rospy.Time.now()
for geom in link.GetGeometries():
trans = RosPoseFromRaveMatrix(body.GetTransform().dot(
link.GetTransform().dot(geom.GetTransform())))
if geom.GetType() == openravepy.GeometryType.Trimesh:
mesh = sm.Mesh()
rave_mesh = geom.GetCollisionMesh()
for pt in rave_mesh.vertices:
mesh.vertices.append(gm.Point(*pt))
for tri in rave_mesh.indices:
mt = sm.MeshTriangle()
mt.vertex_indices = tri
mesh.triangles.append(mt)
co.meshes.append(mesh)
co.mesh_poses.append(trans)
else:
shape = sm.SolidPrimitive()
shape.type = shape.BOX
shape.dimensions = geom.GetBoxExtents() * 2
co.primitives.append(shape)
co.primitive_poses.append(trans)
cos.append(co)
pub.publish(ps)
return ps
