publish_world_models.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Copyright (c) 2016 The UUV Simulator Authors.
3 # All rights reserved.
4 #
5 # Licensed under the Apache License, Version 2.0 (the "License");
6 # you may not use this file except in compliance with the License.
7 # You may obtain a copy of the License at
8 #
9 # http://www.apache.org/licenses/LICENSE-2.0
10 #
11 # Unless required by applicable law or agreed to in writing, software
12 # distributed under the License is distributed on an "AS IS" BASIS,
13 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 # See the License for the specific language governing permissions and
15 # limitations under the License.
16 
17 import rospy
18 import sys
19 from tf_quaternion.transformations import quaternion_from_euler
20 from visualization_msgs.msg import Marker, MarkerArray
21 from gazebo_msgs.srv import GetModelState
22 
23 
25  def __init__(self):
26  self._model_paths = dict()
27 
28  try:
29  # Handle for retrieving model properties
30  rospy.wait_for_service('/gazebo/get_model_state', 100)
31  self._get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
32  except rospy.ROSException:
33  print('/gazebo/get_model_state service is unavailable')
34  sys.exit()
35 
36  if rospy.has_param('~meshes'):
37  meshes = rospy.get_param('~meshes')
38  if type(meshes) != dict:
39  raise rospy.ROSException('A list of mesh filenames is required')
40 
41  self.add_meshes(meshes)
42 
43  self._mesh_topic = rospy.Publisher('/world_models', MarkerArray, queue_size=1)
44 
45  rate = rospy.Rate(0.1)
46  while not rospy.is_shutdown():
47  self.publish_meshes()
48  rate.sleep()
49 
50  def add_meshes(self, models):
51  for model in models:
52  if model in self._model_paths:
53  print('Model %s already exists' % model)
54  continue
55 
56  new_model = dict()
57 
58  new_model['position'] = [0, 0, 0]
59  new_model['orientation'] = [0, 0, 0, 1]
60  new_model['scale'] = [1, 1, 1]
61 
62  if 'pose' in models[model]:
63  if 'position' in models[model]['pose']:
64  if len(models[model]['pose']['position']) == 3:
65  new_model['position'] = models[model]['pose']['position']
66  if 'orientation' in models[model]['pose']:
67  if len(models[model]['pose']['orientation']) == 3:
68  new_model['orientation'] = quaternion_from_euler(*models[model]['pose']['orientation'])
69 
70  if 'scale' in models[model]:
71  if len(models[model]['scale']) == 3:
72  new_model['scale'] = models[model]['scale']
73 
74  if 'mesh' in models[model]:
75  new_model['mesh'] = models[model]['mesh']
76 
77  if 'model' in models[model]:
78  model_name = models[model]['model']
79  prop = self._get_model_state(model_name, '')
80  if prop.success:
81  new_model['position'] = [prop.pose.position.x,
82  prop.pose.position.y,
83  prop.pose.position.z]
84  new_model['orientation'] = [prop.pose.orientation.x,
85  prop.pose.orientation.y,
86  prop.pose.orientation.z,
87  prop.pose.orientation.w]
88  else:
89  print('Model %s not found in the current Gazebo scenario' % model)
90  else:
91  print('Information about the model %s for the mesh %s could not be '
92  'retrieved' % (model, models[model]['mesh']))
93  elif 'plane' in models[model]:
94  new_model['plane'] = [1, 1, 1]
95  if 'plane' in models[model]:
96  if len(models[model]['plane']) == 3:
97  new_model['plane'] = models[model]['plane']
98  else:
99  print('Invalid scale vector for ' + model)
100  else:
101  continue
102 
103  self._model_paths[model] = new_model
104  print('New model being published: %s' % model)
105  print('\t Position: ' + str(self._model_paths[model]['position']))
106  print('\t Orientation: ' + str(self._model_paths[model]['orientation']))
107  print('\t Scale: ' + str(self._model_paths[model]['scale']))
108 
109  def publish_meshes(self):
110  markers = MarkerArray()
111  i = 0
112  total_models = len(self._model_paths.keys())
113  for model in self._model_paths:
114  marker = Marker()
115 
116  if 'mesh' in self._model_paths[model]:
117  marker.type = Marker.MESH_RESOURCE
118  marker.mesh_resource = self._model_paths[model]['mesh']
119  marker.scale.x = self._model_paths[model]['scale'][0]
120  marker.scale.y = self._model_paths[model]['scale'][1]
121  marker.scale.z = self._model_paths[model]['scale'][2]
122  elif 'plane' in self._model_paths[model]:
123  marker.type = Marker.CUBE
124  marker.scale.x = self._model_paths[model]['plane'][0]
125  marker.scale.y = self._model_paths[model]['plane'][1]
126  marker.scale.z = self._model_paths[model]['plane'][2]
127 
128  marker.header.frame_id = 'world'
129  marker.header.stamp = rospy.get_rostime()
130  marker.ns = ''
131  marker.id = i
132  marker.action = Marker.ADD
133  marker.pose.position.x = self._model_paths[model]['position'][0]
134  marker.pose.position.y = self._model_paths[model]['position'][1]
135  marker.pose.position.z = self._model_paths[model]['position'][2]
136  marker.pose.orientation.x = self._model_paths[model]['orientation'][0]
137  marker.pose.orientation.y = self._model_paths[model]['orientation'][1]
138  marker.pose.orientation.z = self._model_paths[model]['orientation'][2]
139  marker.pose.orientation.w = self._model_paths[model]['orientation'][3]
140  marker.color.a = 0.2
141  marker.color.r = 0.0
142  marker.color.g = 0.0
143  marker.color.b = 1.0 - float(i) / total_models
144 
145  markers.markers.append(marker)
146  i += 1
147 
148  self._mesh_topic.publish(markers)
149 
150 
151 if __name__ == '__main__':
152  print('Start publishing vehicle footprints to RViz')
153  rospy.init_node('publish_world_models')
154 
155  try:
156  world_pub = WorldPublisher()
157  rospy.spin()
158  except rospy.ROSInterruptException:
159  print('caught exception')
160  print('exiting')
def quaternion_from_euler(ai, aj, ak, axes='sxyz')


uuv_assistants
Author(s): Musa Morena Marcusso Manhaes , Sebastian Scherer , Luiz Ricardo Douat
autogenerated on Thu Jun 18 2020 03:28:20