dxl_rotation_tf.py

#!/usr/bin/env python

import rospy
from math import fabs, pi
from dynamixel_msgs.msg import JointState
from std_msgs.msg import Float64
import tf

goal_pos = 0;
pub = rospy.Publisher('/tilt_controller/command', Float64)
br = tf.TransformBroadcaster()

def transform_callback(data):
	global goal_pos
	rospy.loginfo('Current motor angle {0}'.format(data.current_pos))

	br.sendTransform((0.0, 0.0, 0.0),
					  tf.transformations.quaternion_from_euler(0.0, -90.0*pi/180.0, data.current_pos, 'sxyz'),
					  rospy.Time.now(),
					  "laser",
					  "base_link")

	# If the motor has reached its limit, publish a new command.
	if fabs(goal_pos-data.current_pos) < 0.01:
		if goal_pos == 0:
			goal_pos = 3.141592
		else:
			goal_pos = 0

		str = "Moving motor to {0}" .format(goal_pos)
		rospy.loginfo(str)
		pub.publish(Float64(goal_pos))


def dxl_control():
	rospy.init_node('dxl_control', anonymous=True)
	rospy.Subscriber('/tilt_controller/state', JointState, transform_callback)
	# Initial movement.
	pub.publish(Float64(goal_pos))
	rospy.spin()


if __name__ == '__main__':
	try:
		dxl_control()
	except rospy.ROSInterruptException:
		pass