8 if __name__ ==
"__main__":
9 rospy.init_node(
"sample_snapit_pose_publisher")
10 frame_id = rospy.get_param(
'~frame_id',
'map')
11 pub = rospy.Publisher(
"~output", PoseStamped, queue_size=1)
12 r = rospy.Rate(rospy.get_param(
'~rate', 1.0))
14 while not rospy.is_shutdown():
15 now = rospy.Time.now().to_sec()
16 theta = now % (2 * np.pi)
19 msg.header.stamp = rospy.Time.now()
20 msg.header.frame_id = frame_id
21 msg.pose.position.x = 4 * np.cos(theta)
22 msg.pose.position.y = 0
23 msg.pose.position.z = 2 * np.sin(theta) + 1
24 msg.pose.orientation.x = 0
25 msg.pose.orientation.y = 0
26 msg.pose.orientation.z = 0
27 msg.pose.orientation.w = 1