demo.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import math
00005 import tf
00006 import numpy as np
00007 import time
00008 from tf import TransformListener
00009 from geometry_msgs.msg import PoseStamped
00010 
00011 class Demo():
00012     def __init__(self, goals):
00013         rospy.init_node('demo', anonymous=True)
00014         self.worldFrame = rospy.get_param("~worldFrame", "/world")
00015         self.frame = rospy.get_param("~frame")
00016         self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1)
00017         self.listener = TransformListener()
00018         self.goals = goals
00019         self.goalIndex = 0
00020 
00021     def run(self):
00022         self.listener.waitForTransform(self.worldFrame, self.frame, rospy.Time(), rospy.Duration(5.0))
00023         goal = PoseStamped()
00024         goal.header.seq = 0
00025         goal.header.frame_id = self.worldFrame
00026         while not rospy.is_shutdown():
00027             goal.header.seq += 1
00028             goal.header.stamp = rospy.Time.now()
00029             goal.pose.position.x = self.goals[self.goalIndex][0]
00030             goal.pose.position.y = self.goals[self.goalIndex][1]
00031             goal.pose.position.z = self.goals[self.goalIndex][2]
00032             quaternion = tf.transformations.quaternion_from_euler(0, 0, self.goals[self.goalIndex][3])
00033             goal.pose.orientation.x = quaternion[0]
00034             goal.pose.orientation.y = quaternion[1]
00035             goal.pose.orientation.z = quaternion[2]
00036             goal.pose.orientation.w = quaternion[3]
00037 
00038             self.pubGoal.publish(goal)
00039 
00040             t = self.listener.getLatestCommonTime(self.worldFrame, self.frame)
00041             if self.listener.canTransform(self.worldFrame, self.frame, t):
00042                 position, quaternion = self.listener.lookupTransform(self.worldFrame, self.frame, t)
00043                 rpy = tf.transformations.euler_from_quaternion(quaternion)
00044                 if     math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.3 \
00045                    and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.3 \
00046                    and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.3 \
00047                    and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(10) \
00048                    and self.goalIndex < len(self.goals) - 1:
00049                         rospy.sleep(self.goals[self.goalIndex][4])
00050                         self.goalIndex += 1


crazyflie_demo
Author(s): Wolfgang Hoenig
autogenerated on Wed Jun 12 2019 19:20:46