sample_snapit_pose_publisher.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 from geometry_msgs.msg import PoseStamped
4 import numpy as np
5 import rospy
6 
7 
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))
13 
14  while not rospy.is_shutdown():
15  now = rospy.Time.now().to_sec()
16  theta = now % (2 * np.pi)
17 
18  msg = PoseStamped()
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
28  pub.publish(msg)
29 
30  r.sleep()


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:47