mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
68 lines
1.7 KiB
Python
Executable file
68 lines
1.7 KiB
Python
Executable file
#!/usr/bin/env python2
|
|
# license removed for brevity
|
|
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 = '/joint1'
|
|
marker_.ns = 'basic_cube'
|
|
|
|
print('publishing ...')
|
|
while not rospy.is_shutdown():
|
|
now = rospy.Time.now() - rospy.Duration(0.1)
|
|
# now = rospy.Time.now()
|
|
# print(now)
|
|
|
|
try:
|
|
trans, rot = listener.lookupTransform('joint1', '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] - 0.02
|
|
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
|