def pose_msg(pos, rot):
# Build pose message
pose_goal = PoseMsg()
rot_arr = rot.as_quat()
pose_goal.orientation.x = rot_arr[0]
pose_goal.orientation.y = rot_arr[1]
pose_goal.orientation.z = rot_arr[2]
pose_goal.orientation.w = rot_arr[3]
pose_goal.position.x = pos[0] / 1_000
pose_goal.position.y = pos[1] / 1_000
pose_goal.position.z = pos[2] / 1_000
return pose_goal
def build_marker_array(map_obj):
# type: (MapDocument) -> MarkerArray
assert isinstance(map_obj, MapDocument)
marker_array = MarkerArray()
marker_array.markers.append(Marker(type=Marker.DELETEALL))
for zone in map_obj.zones:
marker_array.markers += build_zone_markers(zone=zone)
nodes = {}
for node in map_obj.nodes:
nodes[node.id] = node
marker_array.markers.append(
Marker(type=Marker.SPHERE,
pose=PoseMsg(position=PointMsg(x=node.point.x,
y=node.point.y,
z=0),
orientation=QuaternionMsg(w=1)),
frame_locked=True,
scale=Vector3Msg(0.06, 0.06, 0.06),
color=ColorRGBA(0.0, 0.0, 1.0, 1.0)))
for path in map_obj.paths:
for i in range(len(path.nodes) - 1):
start_node = nodes[path.nodes[i]]
end_node = nodes[path.nodes[i + 1]]
marker_array.markers.append(
build_cylinder_marker(
start_point=Vector3(start_node.point.x, start_node.point.y,
0),
end_point=Vector3(end_node.point.x, end_node.point.y, 0),
color=ColorRGBA(0.0, 0.0, 1.0, 1.0),
radius=0.02))
print marker_array
for marker in map_obj.markers:
marker_array.markers += build_pose_markers(pose=marker.pose.get_msg(),
size=0.2)
now = rospy.Time(0)
for i, marker in enumerate(marker_array.markers):
assert isinstance(marker, Marker)
marker.id = i
marker.header.frame_id = 'map'
marker.header.stamp = now
return marker_array
def auto_encode(data):
t = type(data)
if t in ts.matrix_types:
m = data if t in ts.symengine_matrix_types else data.to_sym_matrix()
if m.shape == (4, 4):
return encode_pose(m)
elif m.shape == (3, 3):
return encode_rotation(m)
elif m.shape == (3, 1):
return encode_point(m)
elif m.shape == (4, 1):
return encode_vector(m) if m[3] == 0 else encode_point(m)
else:
raise Exception(
'No encoding known for matrices of shape {}'.format(m.shape))
elif t == list or t == tuple:
if len(data) == 0:
raise Exception('Can not encode empty list.')
s_t = type(data[0])
if s_t is float or s_t is int or s_t is list or s_t is tuple:
return auto_encode(se.Matrix(data))
raise Exception('Can not encode list with inner type {}'.format(s_t))
elif t == pb.Transform:
out = PoseMsg()
out.position.x = data.origin.x
out.position.y = data.origin.y
out.position.z = data.origin.z
out.orientation.x = data.rotation.x
out.orientation.y = data.rotation.y
out.orientation.z = data.rotation.z
out.orientation.w = data.rotation.w
return out
elif t == pb.Quaternion:
out = QuaternionMsg()
out.x = data.x
out.y = data.y
out.z = data.z
out.w = data.w
return out
elif t == pb.Vector3:
out = PointMsg()
out.x = data.x
out.y = data.y
out.z = data.z
return out
else:
raise Exception('Can not encode data of type {}'.format(t))
def build_cylinder_marker(start_point, end_point, color, radius):
# type: (Vector3, Vector3, ColorRGBA, float) -> Marker
vec = Vector3(end_point - start_point)
center = (start_point + end_point) * (1 / 2.0)
axis_vec = end_point - center
axis_vec.normalize()
up_vec = Vector3(0, 0, 1)
right_axis_vector = axis_vec.cross(up_vec)
right_axis_vector.normalize()
theta = axis_vec * up_vec
angle_rotation = -1.0 * math.acos(theta)
qt = Quaternion.from_axis_angle(axis=right_axis_vector,
angle=angle_rotation)
return Marker(type=Marker.CYLINDER,
pose=PoseMsg(position=PointMsg(*center.data()),
orientation=QuaternionMsg(
qt.x, qt.y, qt.z, qt.w)),
frame_locked=True,
scale=Vector3Msg(radius, radius, vec.length()),
color=color)
def encode_pose(data):
out = PoseMsg()
out.position = encode_point(data[:, 3:])
out.orientation = encode_rotation(data[:3, :3])
return out
def to_pose_msg(self):
msg = PoseMsg()
(msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w) = self.quat.to_xyzw()
(msg.position.x, msg.position.y, msg.position.z) = self.tvec
return msg
def _generate_pose_constraints(self, str_path, model):
with self.lock:
if str_path in self.tracked_poses:
align_rotation = '{} align rotation'.format(str_path)
align_position = '{} align position'.format(str_path)
if model is not None:
m_free_symbols = cm.free_symbols(model.pose)
if len(m_free_symbols) > 0:
te = self.tracked_poses[str_path]
self.joints |= m_free_symbols
self.joint_aliases = {s: str(s) for s in self.joints}
r_dist = norm(
cm.rot_of(model.pose) - cm.rot_of(te.pose))
self.soft_constraints[align_rotation] = SC(
-r_dist, -r_dist, 1, r_dist)
# print(align_position)
# print(model.pose)
dist = norm(cm.pos_of(model.pose) - cm.pos_of(te.pose))
# print('Distance expression:\n{}'.format(dist))
# print('Distance expression symbol overlap:\n{}'.format(m_free_symbols.intersection(cm.free_symbols(dist))))
# for s in m_free_symbols:
# print('Diff w.r.t {}:\n{}'.format(s, cm.diff(dist, s)))
self.soft_constraints[align_position] = SC(
-dist, -dist, 1, dist)
self.generate_opt_problem()
# Avoid crashes due to insufficient perception data. This is not fully correct.
if str_path in self._unintialized_poses:
state = self.integrator.state.copy(
) if self.integrator is not None else {}
state.update({
s: 0.0
for s in m_free_symbols if s not in state
})
null_pose = cm.subs(model.pose, state)
pos = cm.pos_of(null_pose)
quat = real_quat_from_matrix(cm.rot_of(null_pose))
msg = PoseMsg()
msg.position.x = pos[0]
msg.position.y = pos[1]
msg.position.z = pos[2]
msg.orientation.x = quat[0]
msg.orientation.y = quat[1]
msg.orientation.z = quat[2]
msg.orientation.w = quat[3]
te.update_state(msg, self.integrator.state)
else:
regenerate_problem = False
if align_position in self.soft_constraints:
del self.soft_constraints[align_position]
regenerate_problem = True
if align_rotation in self.soft_constraints:
del self.soft_constraints[align_rotation]
regenerate_problem = True
if regenerate_problem:
self.generate_opt_problem()
def get_msg(self):
# type: () -> PoseMsg
return PoseMsg(position=self.position.get_msg(),
orientation=self.quaternion.get_msg())
from interactive_markers.interactive_marker_server import *
from iai_bullet_sim.basic_simulator import BasicSimulator
from iai_bullet_sim.full_state_node import FullStatePublishingNode
from iai_bullet_sim.multibody import MultiBody
from iai_bullet_sim.srv import *
from iai_bullet_sim.utils import Frame
from geometry_msgs.msg import Pose as PoseMsg
from std_msgs.msg import String as StringMsg
from visualization_msgs.msg import Marker as MarkerMsg
from visualization_msgs.msg import InteractiveMarkerControl as InteractiveMarkerControlMsg
from visualization_msgs.msg import InteractiveMarkerFeedback as InteractiveMarkerFeedbackMsg
zero_pose = PoseMsg()
class FullStateInteractiveNode(FullStatePublishingNode):
"""This node publishes the state of the simulation and makes objects interactable through interactive markers."""
def __init__(self,
server_name='iai_bullet_sim',
simulator_class=BasicSimulator):
"""Instantiates the node.
:param server_name: Name for the marker server.
:type server_name: str
:param simulator_class: Simulator class to use
:type simulator_class: type
"""
super(FullStateInteractiveNode, self).__init__(simulator_class)
def generate_cartographer_map(cartographer_configuration_directory,
world_sdf_path,
resolution=0.02,
map_origin=(-30, -10),
map_size=(64, 64),
submap_locations=((30, 22, 64, 32), (46, 22, 64,
32))):
assert os.path.isdir(cartographer_configuration_directory)
assert os.path.isfile(world_sdf_path)
temp_png_f = tempfile.NamedTemporaryFile()
temp_pb_f = tempfile.NamedTemporaryFile()
cmd = [
'rosrun',
'cartographer_ros',
'sdf_to_pbstream',
'--configuration_directory',
cartographer_configuration_directory,
'--pbstream',
temp_pb_f.name,
'--map_png',
temp_png_f.name,
'--world_sdf',
world_sdf_path,
'--map_origin', # bottom left corner position
'{},{}'.format(*map_origin),
'--map_size',
'{},{}'.format(*map_size)
]
for s in submap_locations:
cmd += ['--submap_location', ','.join([str(_s) for _s in s])]
str_cmd = ' '.join(cmd)
subprocess.check_call(str_cmd, shell=True, stderr=subprocess.STDOUT)
temp_pb_f.seek(0)
pb_bytes = temp_pb_f.read()
temp_png_f.seek(0)
im_bytes = temp_png_f.read()
xml_file = open(world_sdf_path, 'r')
sdf_xml = xml_file.read()
parser = etree.XMLParser(remove_blank_text=True)
original_xml_element = etree.XML(sdf_xml, parser=parser)
world = original_xml_element.find('world')
#
# Extract the zones
#
zones = []
for model in world.findall('model'):
links = model.findall('link')
for link in links:
layer = (link.attrib['layer']
if 'layer' in link.attrib else '').lower()
zone_type = get_zone_type(layer)
if zone_type != Zone.UNKNOWN:
collision = link.find('visual')
geometry = collision.find('geometry')
polyline = geometry.find('polyline')
points = [[float(t) for t in point.text.split(' ')]
for point in polyline.findall('point')]
zones.append(
Zone(name=link.attrib['name']
if 'name' in link.attrib else layer,
zone_type=zone_type,
polygon=PolygonMsg(points=[
Point32Msg(point[0], point[1], 0)
for point in points
])))
#
# Extract the path nodes
#
nodes = []
for model in world.findall('model'):
for node in model.findall('node'):
position = node.find('position')
node_id = node.attrib['id']
points = [float(t) for t in position.text.split(' ')]
nodes.append(Node(id=node_id, x=points[0], y=points[1]))
#
# Extract the paths
#
paths = []
for model in world.findall('model'):
for path in model.findall('path'):
node_ids = [ni.text for ni in path.findall('node')]
paths.append(Path(
name='',
nodes=node_ids,
))
#
# Extract the markers
#
markers = []
for model in world.findall('model'):
links = model.findall('link')
for link in links:
layer = (link.attrib['layer']
if 'layer' in link.attrib else '').lower()
if layer == 'marker':
pose = link.find('pose')
values = [float(f) for f in pose.text.split(' ')]
if len(values) != 6:
raise Exception('Invalid Pose: {}'.format(values))
qt = Quaternion.from_euler_extrinsic(*values[3:])
markers.append(
Marker(name=link.attrib['name']
if 'name' in link.attrib else layer,
marker_type=0,
pose=PoseMsg(position=PointMsg(
values[0], values[1], values[2]),
orientation=QuaternionMsg(x=qt.x,
y=qt.y,
z=qt.z,
w=qt.w))))
#
# Save the map
#
add_map_srv = rospy.ServiceProxy(name='/map_manager/add_map',
service_class=AddMap)
add_map_srv.wait_for_service(timeout=10)
add_response = add_map_srv.call(
AddMapRequest(map=Map(info=MapInfo(
name='sim_map',
meta_data=MapMetaData(
resolution=resolution,
width=map_size[0] / resolution,
height=map_size[1] / resolution,
origin=PoseMsg(
position=PointMsg(x=map_origin[0], y=map_origin[1])))),
markers=markers,
zones=zones,
paths=paths,
nodes=nodes,
default_zone=Zone.EXCLUSION_ZONE),
occupancy_grid=CompressedImage(format='png',
data=im_bytes),
map_data=pb_bytes)) # type: AddMapResponse
if not add_response.success:
raise Exception('Failed to save map: {}'.format(add_response.message))