from fake_sensor import FakeSensor import rospy import tf from geometry_msgs.msg import Quaternion from ch15.srv import FakeSensor, FakeSensorResponse # modified def make_quaternion(angle): q = tf.transformations.quaternion_from_euler(0, 0, angle) return Quaternion(*q) def callback(request): # <1> angle = sensor.value() * 2 * pi / 100.0 q = make_quaternion(angle) return FakeSensorResponse(q) if __name__ == '__main__': sensor = FakeSensor() rospy.init_node('fake_sensor') service = rospy.Service('angle', FakeSensor, callback) # <2> rospy.spin() # added
from fake_sensor import FakeSensor # <1> import rospy import tf from geometry_msgs.msg import Quaternion # <2> def make_quaternion(angle): # <3> q = tf.transformations.quaternion_from_euler(0, 0, angle) return Quaternion(*q) if __name__ == '__main__': sensor = FakeSensor() # <4> rospy.init_node('fake_sensor') pub = rospy.Publisher('angle', Quaternion, queue_size=10) rate = rospy.Rate(10.0) # <5> while not rospy.is_shutdown(): # <6> angle = sensor.value() * 2 * pi / 100.0 q = make_quaternion(angle) pub.publish(q) rate.sleep()