Add wait for service

crazyflie/scripts/vel_mux.py

@@ -16,8 +16,6 @@
 from crazyflie_interfaces.msg import Hover
 import time
 
-HEIGHT = 0.3
-
 class VelMux(Node):
  def __init__(self):
  super().__init__('vel_mux')
@@ -29,11 +27,6 @@ def __init__(self):
  robot_prefix = self.get_parameter('robot_prefix').value
  incoming_twist_topic = self.get_parameter('incoming_twist_topic').value
 
- self.get_logger().info(f"Velocity Multiplexer set for {robot_prefix}"+
- f" with height {self.hover_height} m using the {incoming_twist_topic} topic")
-
- print("PARAMETERS", self.hover_height, robot_prefix, incoming_twist_topic)
-
  self.subscription = self.create_subscription(
  Twist,
  incoming_twist_topic,
@@ -43,26 +36,31 @@ def __init__(self):
  self.received_first_cmd_vel = False
  timer_period = 0.1
  self.timer = self.create_timer(timer_period, self.timer_callback)
- self.take_off_client = self.create_client(Takeoff, robot_prefix + '/takeoff')
+ self.takeoff_client = self.create_client(Takeoff, robot_prefix + '/takeoff')
  self.publisher_hover = self.create_publisher(Hover, robot_prefix + '/cmd_hover', 10)
  self.land_client = self.create_client(Land, robot_prefix + '/land')
  self.cf_has_taken_off = False
 
+ self.takeoff_client.wait_for_service()
+ self.land_client.wait_for_service()
+
+ self.get_logger().info(f"Velocity Multiplexer set for {robot_prefix}"+
+ f" with height {self.hover_height} m using the {incoming_twist_topic} topic")
+
  def cmd_vel_callback(self, msg):
  self.msg_cmd_vel = msg
  # This is to handle the zero twist messages from teleop twist keyboard closing
  # or else the crazyflie would constantly take off again.
  msg_is_zero = msg.linear.x == 0.0 and msg.linear.y == 0.0 and msg.angular.z == 0.0 and msg.linear.z == 0.0
  if msg_is_zero is False and self.received_first_cmd_vel is False and msg.linear.z >= 0.0:
  self.received_first_cmd_vel = True
- print('takeoff')
 
  def timer_callback(self):
  if self.received_first_cmd_vel and self.cf_has_taken_off is False:
  req = Takeoff.Request()
  req.height = self.hover_height
  req.duration = rclpy.duration.Duration(seconds=2.0).to_msg()
- self.take_off_client.call_async(req)
+ self.takeoff_client.call_async(req)
  self.cf_has_taken_off = True
  time.sleep(2.0) 
  if self.received_first_cmd_vel and self.cf_has_taken_off:
@@ -71,10 +69,9 @@ def timer_callback(self):
  msg.vx = self.msg_cmd_vel.linear.x
  msg.vy = self.msg_cmd_vel.linear.y
  msg.yaw_rate = self.msg_cmd_vel.angular.z
- msg.z_distance = HEIGHT
+ msg.z_distance = self.hover_height
  self.publisher_hover.publish(msg)
  else:
- print('land')
  req = Land.Request()
  req.height = 0.1
  req.duration = rclpy.duration.Duration(seconds=2.0).to_msg()
@@ -84,7 +81,6 @@ def timer_callback(self):
  self.received_first_cmd_vel = False
 
 
-
 def main(args=None):
  rclpy.init(args=args)