def publish_next_waypoint(waypoint_cmd_publisher, waypoint_data_publisher):
print "PUBLISHING WAYPOINT"
wp_command = WaypointCommand()
wp_command.cmd = ">*>wg"
wp_command.header.stamp = rospy.get_rostime()
waypoint_cmd_publisher.publish(wp_command)
current_wp = waypointList.pop(0)
wp = Waypoint(current_wp['X'], current_wp['Y'], current_wp['Z'], 0)
wp.get_Waypoint(0, 500)
#print wp.generateWPforROSPublish()
waypoint_data = wp.getWaypointStruct()
waypoint_data.header.stamp = rospy.get_rostime()
waypoint_data_publisher.publish(waypoint_data)
print "WAYPOINT PUBLISHED", wp_command, waypoint_data
def gotoWaypoint(self, currentWp):
#Before the command was set to send before the waypoint is ready. Was moved to here in the code.
wpCommand = WaypointCommand()
wpCommand.cmd = ">*>ws"
wpCommand.header.stamp = rospy.get_rostime()
self.waypointCmdPublisher.publish(wpCommand)
if currentWp['height'] == None:
wp = Waypoint(currentWp['lng'], currentWp['lat'], self.height, 0,
self.velocity, self.waypointTime)
else:
wp = Waypoint(currentWp['lng'], currentWp['lat'],
currentWp['height'], 0, self.velocity,
self.waypointTime)
self.height = currentWp['height']
wp.getWaypoint(
) #usado para calcular chksum e parametros restantes nao inicializados
waypointData = wp.getWaypointStruct()
waypointData.header.stamp = rospy.get_rostime()
self.waypointDataPublisher.publish(waypointData)
wpCommand = WaypointCommand()
wpCommand.cmd = ">*>wg"
wpCommand.header.stamp = rospy.get_rostime()
self.waypointCmdPublisher.publish(wpCommand)
###################################################################################################
def publish_waypoint_by_index(waypoint_cmd_publisher, waypoint_data_publisher, waypoint_index):
global hummBaseName, roslaunch_height
print "PUBLISHING WAYPOINT"
#Before the command was set to send before the waypoint is ready. Was moved to here in the code.
wp_command = WaypointCommand()
wp_command.cmd = ">*>wg"
wp_command.header.stamp = rospy.get_rostime()
waypoint_cmd_publisher.publish(wp_command)
current_wp = waypointList[waypoint_index]
wp = Waypoint(current_wp['X'], current_wp['Y'], roslaunch_height, 0)
wp.get_Waypoint() #usado para calcular chksum e parametros restantes nao inicializados
#print wp.generateWPforROSPublish()
waypoint_data = wp.getWaypointStruct()
waypoint_data.header.stamp = rospy.get_rostime()
waypoint_data_publisher.publish(waypoint_data)
#print "WAYPOINT PUBLISHED",wp_command, waypoint_data
print "WAYPOINT PUBLISHED"
def comeHomeQuadrotor(self):
wpCommand = WaypointCommand()
wpCommand.cmd = ">*>wh"
wpCommand.header.stamp = rospy.get_rostime()
self.waypointCmdPublisher.publish(wpCommand)