jsk_pcl_ros: pose_with_covariance

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 from geometry_msgs.msg import PoseWithCovarianceStamped
00005 from math import cos, sin, pi
00006 
00007 if __name__ == "__main__":
00008     rospy.init_node("pose_with_covariance_sample")
00009     pub = rospy.Publisher("~output", PoseWithCovarianceStamped)
00010     r = rospy.Rate(20)
00011     counter = 0
00012     rad = 2.0
00013     var = 0.3
00014     while not rospy.is_shutdown():
00015         counter = counter + 10
00016         theta = counter / 180.0 * pi
00017         if counter > 360:
00018             counter = 0
00019         var = abs(cos(theta / 2))
00020         x = rad * cos(theta)
00021         y = rad * sin(theta)
00022         msg = PoseWithCovarianceStamped()
00023         msg.header.stamp = rospy.Time.now()
00024         msg.header.frame_id = "odom"
00025         msg.pose.pose.orientation.w = 1.0
00026         msg.pose.pose.position.x = x
00027         msg.pose.pose.position.y = y
00028         msg.pose.covariance[0] = var
00029         msg.pose.covariance[7] = var
00030         msg.pose.covariance[14] = var
00031         msg.pose.covariance[21] = var
00032         msg.pose.covariance[28] = var
00033         msg.pose.covariance[35] = var
00034         pub.publish(msg)
00035         r.sleep()