def publish_coordinates():
# Firstly we setup a ros node, so that we can communicate with the other packages
rospy.init_node('ardrone_desired_coordinates')
# Initialize a class to hold the coordinate information
coordinates = DesiredCoordinates()
# Publish the coordinates to the desired_coordinates topic
pub = rospy.Publisher('path_coordinates',StateData)
pub_data = StateData()
# Continue to publish until shutdown
while not rospy.is_shutdown():
# Get current values
coordinates.updatePosition()
# Set values for publishing
#pub_data.header.stamp = rospy.get_rostime()
pub_data.x = coordinates.x
pub_data.y = coordinates.y
pub_data.z = coordinates.z
pub_data.roll = coordinates.roll
pub_data.pitch = coordinates.pitch
pub_data.yaw = coordinates.yaw
pub_data.vx = coordinates.vx
pub_data.vy = coordinates.vy
pub_data.vz = coordinates.vz
pub_data.ax = coordinates.ax
pub_data.ay = coordinates.ay
pub_data.az = coordinates.az
# Publish the data
pub.publish(pub_data)
# Pause
rospy.sleep(1.0/70.0) # publish at 70Hz
def process_request(self, unneeded):
# Publish state
state = StateData()
state.x = self.x
state.y = self.y
state.z = self.z
state.vx = self.vx
state.vy = self.vy
state.vz = self.vz
state.ax = self.ax
state.ay = self.ay
state.az = self.az
state.roll = self.roll
state.pitch = self.pitch
state.yaw = self.yaw
self.pub_desired.publish(state)
def process_request(self, unneeded):
# Remove any unnecessary data points
self.clean_buffers()
# If buffers are empty, go to the default position
if len(self.time_buffer) == 0:
self.x = 0.0
self.y = 0.0
self.z = 1.0
self.vx = 0.0
self.vy = 0.0
self.vz = 0.0
self.ax = 0.0
self.ay = 0.0
self.az = 0.0
self.roll = 0.0
self.pitch = 0.0
self.yaw = 0.0
# Else if buffers only have one data point, hold steady
elif len(self.time_buffer) == 1:
self.x = 0.0
self.y = 0.0
self.z = 1.0 + self.point_buffer[0]*scale
self.vx = 0.0
self.vy = 0.0
self.vz = 0.0
self.ax = 0.0
self.ay = 0.0
self.az = 0.0
self.roll = 0.0
self.pitch = 0.0
self.yaw = 0.0
# Else create the trajectory to move to the next point
else:
total_time = self.time_buffer[1] - self.time_buffer[0]
step_time = 0.75 * total_time * (1.0 - 2.0*math.fabs(0.5 - k))
p_initial = 1.0 + self.point_buffer[0]*scale
p_final = 1.0 + self.point_buffer[1]*scale
v_initial = 0.0
v_final = 0.0
self.determine_state(total_time, step_time, p_initial, p_final, v_initial, v_final, self.cur_time - self.time_buffer[0])
# Publish state
state = StateData()
state.x = self.x
state.y = self.y
state.z = self.z
state.vx = self.vx
state.vy = self.vy
state.vz = self.vz
state.ax = self.ax
state.ay = self.ay
state.az = self.az
state.roll = self.roll
state.pitch = self.pitch
state.yaw = self.yaw
self.pub_desired.publish(state)
def publishVicon(self):
# Get the most recent VICON data
vicon_data = StateData()
vicon_data.x = self.x
vicon_data.y = self.y
vicon_data.z = self.z
vicon_data.vx = self.vx
vicon_data.vy = self.vy
vicon_data.vz = self.vz
vicon_data.ax = self.ax
vicon_data.ay = self.ay
vicon_data.az = self.az
vicon_data.roll = self.roll
vicon_data.pitch = self.pitch
vicon_data.yaw = self.yaw
# Publish the most recent data
self.pub_state.publish(vicon_data)
