diff --git a/Drone/copter_client.py b/Drone/copter_client.py index b093424..19e7220 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -2,6 +2,7 @@ import os import time import math import rospy +import datetime import logging from FlightLib import FlightLib @@ -66,7 +67,7 @@ class CopterClient(client.Client): def start(self, task_manager_instance): client.logger.info("Init ROS node") - rospy.init_node('Swarm_client') + rospy.init_node('clever_show_client') if self.USE_LEDS: LedLib.init_led(self.LED_PIN) task_manager_instance.start() @@ -417,7 +418,8 @@ def _copter_flip(*args, **kwargs): @messaging.message_callback("takeoff") def _command_takeoff(*args, **kwargs): - task_manager.add_task(time.time(), 0, animation.takeoff, + print("Takeoff at {}".format(datetime.datetime.now())) + task_manager.add_task(0, 0, animation.takeoff, task_kwargs={ "z": client.active_client.TAKEOFF_HEIGHT, "timeout": client.active_client.TAKEOFF_TIME, @@ -426,6 +428,23 @@ def _command_takeoff(*args, **kwargs): } ) +@messaging.message_callback("takeoff_z") +def _command_takeoff_z(*args, **kwargs): + z_str = kwargs.get("z", None) + if z_str is not None: + telem = FlightLib.get_telemetry(client.active_client.FRAME_ID) + print("Takeoff to z = {} at {}".format(z_str, datetime.datetime.now())) + task_manager.add_task(0, 0, FlightLib.reach_point, + task_kwargs={ + "x": telem.x, + "y": telem.y, + "z": float(z_str), + "frame_id": client.active_client.FRAME_ID, + "timeout": client.active_client.TAKEOFF_TIME, + "auto_arm": True, + } + ) + @messaging.message_callback("land") def _command_land(*args, **kwargs):