crazyflie_demo: publish_pose_teleop.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import tf
00005 from geometry_msgs.msg import PoseStamped
00006 from sensor_msgs.msg import Joy
00007 from math import fabs
00008 
00009 lastData = None
00010 
00011 def joyChanged(data):
00012     global lastData
00013     lastData = data
00014     # print(data)
00015 
00016 if __name__ == '__main__':
00017     rospy.init_node('publish_pose', anonymous=True)
00018     worldFrame = rospy.get_param("~worldFrame", "/world")
00019     name = rospy.get_param("~name")
00020     r = rospy.get_param("~rate")
00021     joy_topic = rospy.get_param("~joy_topic", "joy")
00022     x = rospy.get_param("~x")
00023     y = rospy.get_param("~y")
00024     z = rospy.get_param("~z")
00025 
00026     rate = rospy.Rate(r)
00027 
00028     msg = PoseStamped()
00029     msg.header.seq = 0
00030     msg.header.stamp = rospy.Time.now()
00031     msg.header.frame_id = worldFrame
00032     msg.pose.position.x = x
00033     msg.pose.position.y = y
00034     msg.pose.position.z = z
00035     yaw = 0
00036     quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
00037     msg.pose.orientation.x = quaternion[0]
00038     msg.pose.orientation.y = quaternion[1]
00039     msg.pose.orientation.z = quaternion[2]
00040     msg.pose.orientation.w = quaternion[3]
00041 
00042     pub = rospy.Publisher(name, PoseStamped, queue_size=1)
00043     rospy.Subscriber(joy_topic, Joy, joyChanged)
00044 
00045     while not rospy.is_shutdown():
00046         global lastData
00047         if lastData != None:
00048             if fabs(lastData.axes[1]) > 0.1:
00049                 msg.pose.position.z += lastData.axes[1] / r / 2
00050             if fabs(lastData.axes[4]) > 0.1:
00051                 msg.pose.position.x += lastData.axes[4] / r * 1
00052             if fabs(lastData.axes[3]) > 0.1:
00053                 msg.pose.position.y += lastData.axes[3] / r * 1
00054             if fabs(lastData.axes[0]) > 0.1:
00055                 yaw += lastData.axes[0] / r * 2
00056             quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
00057             msg.pose.orientation.x = quaternion[0]
00058             msg.pose.orientation.y = quaternion[1]
00059             msg.pose.orientation.z = quaternion[2]
00060             msg.pose.orientation.w = quaternion[3]
00061             # print(pose)
00062         msg.header.seq += 1
00063         msg.header.stamp = rospy.Time.now()
00064         pub.publish(msg)
00065         rate.sleep()