publish_footprints.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 from __future__ import print_function
17 import rospy
18 import rostopic
19 import rosgraph
20 import numpy as np
21 from copy import deepcopy
22 from nav_msgs.msg import Odometry
23 from geometry_msgs.msg import PolygonStamped, Point32
24 from tf_quaternion.transformations import euler_from_quaternion
25 from gazebo_msgs.srv import GetWorldProperties, GetModelProperties
26 
27 
28 vehicle_pub = dict()
29 
30 odom_sub = dict()
31 
32 get_world_props = None
33 
34 get_model_props = None
35 
36 marker = np.array([[0, 0.75], [-0.5, -0.25], [0.5, -0.25]])
37 
38 def rot(alpha):
39  return np.array([[np.cos(alpha), -np.sin(alpha)],
40  [np.sin(alpha), np.cos(alpha)]])
41 
42 def odometry_callback(msg, name):
43  global vehicle_pub
44  if name not in vehicle_pub:
45  return
46  x = msg.pose.pose.position.x
47  y = msg.pose.pose.position.y
48 
49  orientation = np.array([msg.pose.pose.orientation.x,
50  msg.pose.pose.orientation.y,
51  msg.pose.pose.orientation.z,
52  msg.pose.pose.orientation.w])
53 
54  yaw = euler_from_quaternion(orientation)[2]
55 
56  new_marker = deepcopy(marker)
57  points = list()
58  for i in range(new_marker.shape[0]):
59  new_marker[i, :] = np.dot(rot(yaw-np.pi/2), new_marker[i, :])
60  new_marker[i, 0] += x
61  new_marker[i, 1] += y
62 
63  p = Point32()
64  p.x = new_marker[i, 0]
65  p.y = new_marker[i, 1]
66  points.append(p)
67 
68  new_poly = PolygonStamped()
69  new_poly.header.stamp = rospy.Time.now()
70  new_poly.header.frame_id = 'world'
71  new_poly.polygon.points = points
72 
73  vehicle_pub[name].publish(new_poly)
74 
75 def get_topics(name):
76  return [t for t, _ in rosgraph.Master('/{}/'.format(name)).getPublishedTopics('') if not t.startswith('/rosout') and name in t]
77 
79  odom_topic_sub = None
80  for t in get_topics(name):
81  msg_class, _, _ = rostopic.get_topic_class(t)
82  if msg_class == Odometry:
83  odom_topic_sub = rospy.Subscriber(
84  t, Odometry, lambda msg: odometry_callback(msg, name))
85  return odom_topic_sub
86 
88  """Call list of models in the Gazebo simulation and filter out the
89  marine crafts.
90  """
91  global get_world_props
92  if get_world_props is None:
93  try:
94  # Handle for world properties update function
95  rospy.wait_for_service('/gazebo/get_world_properties', timeout=2)
96  get_world_props = rospy.ServiceProxy('/gazebo/get_world_properties',
97  GetWorldProperties)
98  except rospy.ROSException:
99  print('/gazebo/get_world_properties service is unavailable')
100 
101  global get_model_props
102  if get_model_props is None:
103  try:
104  # Handle for retrieving model properties
105  rospy.wait_for_service('/gazebo/get_model_properties')
106  get_model_props = rospy.ServiceProxy('/gazebo/get_model_properties',
107  GetModelProperties)
108  except rospy.ROSException:
109  print('/gazebo/get_model_properties service is unavailable')
110  try:
111  global vehicle_pub
112  global odom_sub
113  msg = get_world_props()
114  for model in msg.model_names:
115  model_properties = get_model_props(model)
116  if not model_properties.is_static and \
117  rospy.has_param('/{}/robot_description'.format(model)) and \
118  model not in vehicle_pub:
119  odom_sub[model] = sub_odometry_topic(model)
120  vehicle_pub[model] = rospy.Publisher('/{}/footprint'.format(model),
121  PolygonStamped,
122  queue_size=1)
123  except rospy.ServiceException as e:
124  print('Service call failed: {}'.format(e))
125 
126 def main():
127  update_timer = rospy.Timer(rospy.Duration(10), update_vehicle_list)
128 
129 if __name__ == '__main__':
130  print('Start publishing vehicle footprints to RViz')
131  rospy.init_node('publish_footprints')
132 
133  try:
134  main()
135  rospy.spin()
136  except rospy.ROSInterruptException:
137  print('caught exception')
138  print('exiting')
def euler_from_quaternion(quaternion, axes='sxyz')
def odometry_callback(msg, name)
def update_vehicle_list(event)


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