mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
64 lines
1.6 KiB
Python
Executable file
64 lines
1.6 KiB
Python
Executable file
#!/usr/bin/env python2
|
|
import time
|
|
|
|
import rospy
|
|
from sensor_msgs.msg import JointState
|
|
from std_msgs.msg import Header
|
|
from visualization_msgs.msg import Marker
|
|
import tf
|
|
|
|
|
|
def talker():
|
|
rospy.init_node("following_marker", anonymous=True)
|
|
|
|
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
|
|
rate = rospy.Rate(20)
|
|
|
|
listener = tf.TransformListener()
|
|
|
|
marker_ = Marker()
|
|
marker_.header.frame_id = "/base"
|
|
marker_.ns = "basic_cube"
|
|
|
|
print("publishing ...")
|
|
while not rospy.is_shutdown():
|
|
now = rospy.Time.now() - rospy.Duration(0.1)
|
|
|
|
try:
|
|
trans, rot = listener.lookupTransform("base", "basic_shapes", now)
|
|
except Exception as e:
|
|
print(e)
|
|
continue
|
|
|
|
print(type(trans), trans)
|
|
print(type(rot), rot)
|
|
|
|
# marker
|
|
marker_.header.stamp = now
|
|
marker_.type = marker_.CUBE
|
|
marker_.action = marker_.ADD
|
|
marker_.scale.x = 0.04
|
|
marker_.scale.y = 0.04
|
|
marker_.scale.z = 0.04
|
|
|
|
# marker position initial,标记位置初始化
|
|
marker_.pose.position.x = trans[0]
|
|
marker_.pose.position.y = trans[1]
|
|
marker_.pose.position.z = trans[2]
|
|
marker_.pose.orientation.x = rot[0]
|
|
marker_.pose.orientation.y = rot[1]
|
|
marker_.pose.orientation.z = rot[2]
|
|
marker_.pose.orientation.w = rot[3]
|
|
|
|
marker_.color.a = 1.0
|
|
marker_.color.g = 1.0
|
|
pub_marker.publish(marker_)
|
|
|
|
rate.sleep()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
try:
|
|
talker()
|
|
except rospy.ROSInterruptException:
|
|
pass
|