publish_vehicle_footprint.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 from copy import deepcopy
19 from tf_quaternion.transformations import euler_from_quaternion
20 from nav_msgs.msg import Odometry
21 from geometry_msgs.msg import PolygonStamped, Point32
22 from visualization_msgs.msg import Marker
23 import numpy as np
24 
25 
27  MARKER = np.array([[0, 0.75], [-0.5, -0.25], [0.5, -0.25]])
28 
29  def __init__(self):
30  self._namespace = rospy.get_namespace().replace('/', '')
31 
32  self._scale_footprint = 10
33 
34  if rospy.has_param('~scale_footprint'):
35  scale = rospy.get_param('~scale_footprint')
36  if scale > 0:
37  self._scale_footprint = scale
38  else:
39  print('Scale factor should be greater than zero')
40 
41  print('Footprint marker scale factor= ', self._scale_footprint)
42 
43  self._scale_label = 10
44 
45  if rospy.has_param('~_scale_label'):
46  scale = rospy.get_param('~_scale_label')
47  if scale > 0:
48  self._scale_label = scale
49  else:
50  print('Scale factor should be greater than zero')
51 
52  print('Label marker scale factor = ', self._scale_label)
53 
54  self._label_x_offset = 60
55  if rospy.get_param('~label_x_offset'):
56  self._label_x_offset = rospy.get_param('~label_x_offset')
57 
58  self._label_marker = Marker()
59  self._label_marker.header.frame_id = 'world'
60  self._label_marker.header.stamp = rospy.Time.now()
61  self._label_marker.ns = self._namespace
62  self._label_marker.type = Marker.TEXT_VIEW_FACING
63  self._label_marker.text = self._namespace
64  self._label_marker.action = Marker.ADD
65  self._label_marker.pose.orientation.x = 0.0
66  self._label_marker.pose.orientation.y = 0.0
67  self._label_marker.pose.orientation.z = 0.0
68  self._label_marker.pose.orientation.w = 1.0
69  self._label_marker.scale.x = 0.0
70  self._label_marker.scale.y = 0.0
71  self._label_marker.scale.z = self._scale_label
72  self._label_marker.color.a = 1.0
73  self._label_marker.color.r = 0.0
74  self._label_marker.color.g = 1.0
75  self._label_marker.color.b = 0.0
76 
77  # Odometry subscriber (remap this topic in the launch file if necessary)
78  self._odom_sub = rospy.Subscriber('odom', Odometry, self.odometry_callback)
79  # Footprint marker publisher (remap this topic in the launch file if necessary)
80  self._footprint_pub = rospy.Publisher('footprint', PolygonStamped, queue_size=1)
81  # Vehicle label marker (remap this topic in the launch file if necessary)
82  self._label_pub = rospy.Publisher('label', Marker, queue_size=1)
83 
84  @staticmethod
85  def rot(alpha):
86  return np.array([[np.cos(alpha), -np.sin(alpha)],
87  [np.sin(alpha), np.cos(alpha)]])
88 
89  def odometry_callback(self, msg):
90  x = msg.pose.pose.position.x
91  y = msg.pose.pose.position.y
92 
93  orientation = np.array([msg.pose.pose.orientation.x,
94  msg.pose.pose.orientation.y,
95  msg.pose.pose.orientation.z,
96  msg.pose.pose.orientation.w])
97 
98  yaw = euler_from_quaternion(orientation)[2]
99 
100  # Generating the vehicle footprint marker
101  new_marker = self._scale_footprint * deepcopy(self.MARKER)
102 
103  points = list()
104  for i in range(new_marker.shape[0]):
105  new_marker[i, :] = np.dot(self.rot(yaw - np.pi / 2), new_marker[i, :])
106  new_marker[i, 0] += x
107  new_marker[i, 1] += y
108 
109  p = Point32()
110  p.x = new_marker[i, 0]
111  p.y = new_marker[i, 1]
112  points.append(p)
113 
114  new_poly = PolygonStamped()
115  new_poly.header.stamp = rospy.Time.now()
116  new_poly.header.frame_id = 'world'
117  new_poly.polygon.points = points
118 
119  self._footprint_pub.publish(new_poly)
120 
121  # Generating the label marker
122  self._label_marker.pose.position.x = msg.pose.pose.position.x + self._label_x_offset
123  self._label_marker.pose.position.y = msg.pose.pose.position.y
124  self._label_marker.pose.position.z = msg.pose.pose.position.z
125 
126  self._label_pub.publish(self._label_marker)
127 
128 if __name__ == '__main__':
129  print('Generate RViz footprint and markers for 2D visualization')
130  rospy.init_node('generate_vehicle_footprint')
131 
132  try:
134  rospy.spin()
135  except rospy.ROSInterruptException:
136  print('caught exception')
137  print('exiting')
def euler_from_quaternion(quaternion, axes='sxyz')


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