00001
00002
00003 import roslib; roslib.load_manifest( 'marker_rviz_plugin' )
00004 from math import cos, sin, pi
00005 from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped, Pose
00006 from marker_msgs.msg import Marker, MarkerDetection, MarkerWithCovariance, MarkerWithCovarianceArray, MarkerWithCovarianceStamped
00007 import rospy
00008 import tf
00009 import tf_conversions
00010
00011 publisher_detection = rospy.Publisher( 'marker_detection', MarkerDetection, queue_size=5 )
00012 publisher_cov = rospy.Publisher( 'marker_with_cov', MarkerWithCovarianceStamped, queue_size=5 )
00013
00014 rospy.init_node( 'test_marker_covariance' )
00015
00016 br = tf.TransformBroadcaster()
00017
00018
00019 rate = rospy.Rate(100)
00020 angle = 0
00021
00022 r = pi/2
00023 p = pi/3
00024 y = 0
00025
00026
00027 while not rospy.is_shutdown():
00028 stamp = rospy.Time.now()
00029
00030
00031 marker_detection = MarkerDetection()
00032 marker_detection.header.frame_id = "/base_link"
00033 marker_detection.header.stamp = stamp
00034 marker_detection.type = "bch"
00035
00036 marker = Marker()
00037 marker.ids.append(1)
00038 marker.ids_confidence.append(1)
00039 marker.pose = Pose()
00040 marker.pose.position.x = 4.5
00041 marker.pose.position.y = 4.0
00042 marker.pose.position.z = 0.0
00043 ori = marker.pose.orientation
00044 ori.x, ori.y, ori.z, ori.w = tf.transformations.quaternion_from_euler(r, p, y + cos(10 * angle))
00045
00046
00047 marker_detection.markers.append(marker)
00048
00049 publisher_detection.publish( marker_detection )
00050
00051
00052 markerc = Marker()
00053 markerc.ids.append(1)
00054 markerc.ids_confidence.append(1)
00055 markerc.pose = Pose()
00056 markerc.pose.position.x = 6.03287422834
00057 markerc.pose.position.y = 4.93379116194
00058 markerc.pose.position.z = 0.0
00059 ori = markerc.pose.orientation
00060 ori.x = 0.0
00061 ori.y = 0.0
00062 ori.z = 0.16781934864
00063 ori.w = 0.985817765219
00064
00065 marker_with_cov_s = MarkerWithCovarianceStamped()
00066 marker_with_cov_s.header.frame_id = "/base_link"
00067 marker_with_cov_s.header.stamp = stamp
00068
00069 marker_with_cov_s.marker = MarkerWithCovariance()
00070 marker_with_cov_s.marker.marker = markerc
00071 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]
00072
00073 publisher_cov.publish( marker_with_cov_s )
00074
00075
00076 br.sendTransform((marker.pose.position.x, marker.pose.position.y, marker.pose.position.z),
00077 (ori.x, ori.y, ori.z, ori.w),
00078 stamp,
00079 "pose",
00080 "base_link")
00081
00082 angle += .0005
00083 rate.sleep()
00084
00085
00086 '''
00087 ---
00088 header:
00089 seq: 419
00090 stamp:
00091 secs: 1472036917
00092 nsecs: 157325029
00093 frame_id: /base_link
00094 child_frame_id: /odom_msg
00095 pose:
00096 pose:
00097 position:
00098 x: 0.0
00099 y: 1.98
00100 z: 0.0
00101 orientation:
00102 x: -0.0
00103 y: 0.0
00104 z: 0.99695882639
00105 w: -0.0779300871451
00106 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]
00107
00108 '''
00109
00110
00111 '''
00112 header:
00113 seq: 2917
00114 stamp:
00115 secs: 292
00116 nsecs: 800000000
00117 frame_id: map
00118 markers:
00119 -
00120 marker:
00121 ids: [1]
00122 ids_confidence: [1.0]
00123 pose:
00124 position:
00125 x: 2.99259999116
00126 y: 1.99296511443
00127 z: 0.0
00128 orientation:
00129 x: 0.0
00130 y: 0.0
00131 z: 0.642837187793
00132 w: 0.766002839414
00133 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]
00134 -
00135 marker:
00136 ids: [2]
00137 ids_confidence: [1.0]
00138 pose:
00139 position:
00140 x: -2.97632781247
00141 y: 2.00375800444
00142 z: 0.0
00143 orientation:
00144 x: 0.0
00145 y: 0.0
00146 z: -0.000658593183246
00147 w: 0.999999783127
00148 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]
00149 -
00150 marker:
00151 ids: [3]
00152 ids_confidence: [1.0]
00153 pose:
00154 position:
00155 x: 0.0140226204194
00156 y: -1.00262787094
00157 z: 0.0
00158 orientation:
00159 x: 0.0
00160 y: 0.0
00161 z: -0.000983026938579
00162 w: 0.999999516829
00163 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]
00164 -
00165 marker:
00166 ids: [4]
00167 ids_confidence: [1.0]
00168 pose:
00169 position:
00170 x: 6.03287422834
00171 y: 4.93379116194
00172 z: 0.0
00173 orientation:
00174 x: 0.0
00175 y: 0.0
00176 z: 0.16781934864
00177 w: 0.985817765219
00178 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]
00179
00180 [
00181 0.25, 0.0, 0.0, 0.0, 0.0, 0.0,
00182 0.0, 0.001, 0.0, 0.0, 0.0, 0.0,
00183 0.0, 0.0, 0.001, 0.0, 0.0, 0.0,
00184 0.0, 0.0, 0.0, 0.030461741978670857, 0.0, 0.0,
00185 0.0, 0.0, 0.0, 0.0, 0.030461741978670857, 0.0,
00186 0.0, 0.0, 0.0, 0.0, 0.0, 0.2741556778080377
00187 ]
00188 '''
00189
00190