send_demo_msgs.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import roslib; roslib.load_manifest( 'marker_rviz_plugin' )
4 from math import cos, sin, pi
5 from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped, Pose
6 from marker_msgs.msg import Marker, MarkerDetection, MarkerWithCovariance, MarkerWithCovarianceArray, MarkerWithCovarianceStamped
7 import rospy
8 import tf
9 import tf_conversions
10 
11 publisher_detection = rospy.Publisher( 'marker_detection', MarkerDetection, queue_size=5 )
12 publisher_cov = rospy.Publisher( 'marker_with_cov', MarkerWithCovarianceStamped, queue_size=5 )
13 
14 rospy.init_node( 'test_marker_covariance' )
15 
16 br = tf.TransformBroadcaster()
17 
18 
19 rate = rospy.Rate(100)
20 angle = 0
21 
22 r = pi/2 # 90 deg
23 p = pi/3 # 60 deg
24 y = 0
25 
26 
27 while not rospy.is_shutdown():
28  stamp = rospy.Time.now()
29 
30  # MarkerDetection
31  marker_detection = MarkerDetection()
32  marker_detection.header.frame_id = "/base_link"
33  marker_detection.header.stamp = stamp
34  marker_detection.type = "bch"
35 
36  marker = Marker()
37  marker.ids.append(1)
38  marker.ids_confidence.append(1)
39  marker.pose = Pose()
40  marker.pose.position.x = 4.5
41  marker.pose.position.y = 4.0
42  marker.pose.position.z = 0.0
43  ori = marker.pose.orientation
44  ori.x, ori.y, ori.z, ori.w = tf.transformations.quaternion_from_euler(r, p, y + cos(10 * angle))
45  #ori.x, ori.y, ori.z, ori.w = tf.transformations.quaternion_from_euler(0, 0, 0)
46 
47  marker_detection.markers.append(marker)
48 
49  publisher_detection.publish( marker_detection )
50 
51  # MarkerWithCovarianceStamped
52  markerc = Marker()
53  markerc.ids.append(1)
54  markerc.ids_confidence.append(1)
55  markerc.pose = Pose()
56  markerc.pose.position.x = 6.03287422834
57  markerc.pose.position.y = 4.93379116194
58  markerc.pose.position.z = 0.0
59  ori = markerc.pose.orientation
60  ori.x = 0.0
61  ori.y = 0.0
62  ori.z = 0.16781934864
63  ori.w = 0.985817765219
64 
65  marker_with_cov_s = MarkerWithCovarianceStamped()
66  marker_with_cov_s.header.frame_id = "/base_link"
67  marker_with_cov_s.header.stamp = stamp
68 
69  marker_with_cov_s.marker = MarkerWithCovariance()
70  marker_with_cov_s.marker.marker = markerc
71  marker_with_cov_s.marker.covariance = [0.011241161991539792, -0.011615365449532622, 0.0, 0.0, 0.0, -0.002759911553295745, -0.011615365449532622, 0.01903682230368378, 0.0, 0.0, 0.0, 0.00390837573140233, 0.0, 0.0, 0.00203078279, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00203078279, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00203078279, 0.0, -0.002759911553295745, 0.00390837573140233, 0.0, 0.0, 0.0, 0.01827704518]
72 
73  publisher_cov.publish( marker_with_cov_s )
74 
75  # Send base_link transform
76  br.sendTransform((marker.pose.position.x, marker.pose.position.y, marker.pose.position.z),
77  (ori.x, ori.y, ori.z, ori.w),
78  stamp,
79  "pose",
80  "base_link")
81 
82  angle += .0005
83  rate.sleep()
84 
85 
86 '''
87 ---
88 header:
89  seq: 419
90  stamp:
91  secs: 1472036917
92  nsecs: 157325029
93  frame_id: /base_link
94 child_frame_id: /odom_msg
95 pose:
96  pose:
97  position:
98  x: 0.0
99  y: 1.98
100  z: 0.0
101  orientation:
102  x: -0.0
103  y: 0.0
104  z: 0.99695882639
105  w: -0.0779300871451
106  covariance: [0.9919999999999933, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.030461741978670857, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.030461741978670857, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2741556778080377]
107 
108 '''
109 
110 
111 '''
112 header:
113  seq: 2917
114  stamp:
115  secs: 292
116  nsecs: 800000000
117  frame_id: map
118 markers:
119  -
120  marker:
121  ids: [1]
122  ids_confidence: [1.0]
123  pose:
124  position:
125  x: 2.99259999116
126  y: 1.99296511443
127  z: 0.0
128  orientation:
129  x: 0.0
130  y: 0.0
131  z: 0.642837187793
132  w: 0.766002839414
133  covariance: [5.145585936373751e-06, 3.5465074498143265e-06, 0.0, 0.0, 0.0, 7.763297496781882e-09, 3.5465074498143265e-06, 2.7218408354024943e-06, 0.0, 0.0, 0.0, 6.08589337627961e-09, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.763297496781882e-09, 6.08589337627961e-09, 0.0, 0.0, 0.0, 2.1112777137113703e-08]
134  -
135  marker:
136  ids: [2]
137  ids_confidence: [1.0]
138  pose:
139  position:
140  x: -2.97632781247
141  y: 2.00375800444
142  z: 0.0
143  orientation:
144  x: 0.0
145  y: 0.0
146  z: -0.000658593183246
147  w: 0.999999783127
148  covariance: [9.593501938212036e-05, -2.22641412848119e-05, 0.0, 0.0, 0.0, 4.916769263738688e-07, -2.22641412848119e-05, 5.104665830981123e-05, 0.0, 0.0, 0.0, -2.248132132389405e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.916769263738688e-07, -2.248132132389405e-06, 0.0, 0.0, 0.0, 1.7013588091485252e-06]
149  -
150  marker:
151  ids: [3]
152  ids_confidence: [1.0]
153  pose:
154  position:
155  x: 0.0140226204194
156  y: -1.00262787094
157  z: 0.0
158  orientation:
159  x: 0.0
160  y: 0.0
161  z: -0.000983026938579
162  w: 0.999999516829
163  covariance: [0.00013515838629420734, 9.920123763170366e-07, 0.0, 0.0, 0.0, 6.020879714999367e-06, 9.920123763170366e-07, 1.3787290943207972e-05, 0.0, 0.0, 0.0, 3.510932706789802e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 6.020879714999367e-06, 3.510932706789802e-06, 0.0, 0.0, 0.0, 2.10976565686069e-06]
164  -
165  marker:
166  ids: [4]
167  ids_confidence: [1.0]
168  pose:
169  position:
170  x: 6.03287422834
171  y: 4.93379116194
172  z: 0.0
173  orientation:
174  x: 0.0
175  y: 0.0
176  z: 0.16781934864
177  w: 0.985817765219
178  covariance: [0.011241161991539792, -0.011615365449532622, 0.0, 0.0, 0.0, -0.002759911553295745, -0.011615365449532622, 0.01903682230368378, 0.0, 0.0, 0.0, 0.00390837573140233, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.002759911553295745, 0.00390837573140233, 0.0, 0.0, 0.0, 0.0008458541363815106]
179 
180 [
181 0.25, 0.0, 0.0, 0.0, 0.0, 0.0,
182 0.0, 0.001, 0.0, 0.0, 0.0, 0.0,
183 0.0, 0.0, 0.001, 0.0, 0.0, 0.0,
184 0.0, 0.0, 0.0, 0.030461741978670857, 0.0, 0.0,
185 0.0, 0.0, 0.0, 0.0, 0.030461741978670857, 0.0,
186 0.0, 0.0, 0.0, 0.0, 0.0, 0.2741556778080377
187 ]
188 '''
189 
190 

marker_rviz_plugin
Author(s): Markus Bader, Lukas Pfeifhofer, Markus Macsek
autogenerated on Mon Jun 10 2019 13:54:22