Example #1
0
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
Example #2
0
    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)
Example #3
0
    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)
Example #4
0
    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)