Merge pull request IMRCLab#78 from IMRCLab/vel_mux

Velocity hover handler

.github/workflows/ci-docs2.yml

@@ -16,6 +16,7 @@ jobs:
 
  - name: Install dependencies
  run: |
+ sudo apt update
  sudo apt install texlive-latex-extra dvipng
  sudo pip3 install -U setuptools
  sudo pip3 install -U -r docs2/requirements.txt

crazyflie/CMakeLists.txt

@@ -102,6 +102,7 @@ install(TARGETS
 install(PROGRAMS
  scripts/crazyflie_server.py
  scripts/chooser.py
+ scripts/vel_mux.py
  DESTINATION lib/${PROJECT_NAME}
 )
 

crazyflie/package.xml

@@ -17,6 +17,8 @@
  <depend>geometry_msgs</depend>
  <depend>motion_capture_tracking_interfaces</depend>
 
+ <exec_depend>tf_transformations</exec_depend>
+
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
 

crazyflie/scripts/crazyflie_server.py

@@ -21,6 +21,7 @@
 from crazyflie_interfaces.srv import Takeoff, Land, GoTo, RemoveLogging, AddLogging
 from crazyflie_interfaces.srv import UploadTrajectory, StartTrajectory, NotifySetpointsStop
 from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult, ParameterType
+from crazyflie_interfaces.msg import Hover
 
 from std_srvs.srv import Empty
 from geometry_msgs.msg import Twist
@@ -33,9 +34,7 @@
 from tf2_ros import TransformBroadcaster
 
 from functools import partial
-from math import radians, pi, cos, sin
-
-FIX_HEIGHT_CMD_VEL_2D = False
+from math import degrees, radians, pi, cos, sin
 
 cf_log_to_ros_topic = {
  "uint8_t": UInt8,
@@ -110,13 +109,6 @@ def __init__(self):
 
  # Initialize logging, services and parameters for each crazyflie
  for link_uri in self.uris:
-
- # Only used for the pos hold mode
- self.swarm._cfs[link_uri].cmd_vel_2d = {}
- self.swarm._cfs[link_uri].cmd_vel_2d['hold_pos'] = True
- self.swarm._cfs[link_uri].cmd_vel_2d['current_height'] = 0.0
- self.swarm._cfs[link_uri].cmd_vel_2d['timestamp'] = 0.0
- self.swarm._cfs[link_uri].cmd_vel_2d['msg'] = Twist
 
  # Connect callbacks for different connection states of the crazyflie
  self.swarm._cfs[link_uri].cf.fully_connected.add_callback(
@@ -247,38 +239,9 @@ def __init__(self):
  "/cmd_vel_legacy", partial(self._cmd_vel_legacy_changed, uri=uri), 10
  )
  self.create_subscription(
- Twist, name +
- "/cmd_vel_2d", partial(self._cmd_vel_2d_changed, uri=uri), 10
+ Hover, name +
+ "/cmd_hover", partial(self._cmd_hover_changed, uri=uri), 10
  )
- if FIX_HEIGHT_CMD_VEL_2D:
- timer_period = 0.1
- self.create_timer(timer_period, self.send_hover_command)
-
- def send_hover_command(self):
- for uri in self.cf_dict:
- height = self.swarm._cfs[uri].cmd_vel_2d['current_height']
- if height<0.2:
- return
- timestamp = self.swarm._cfs[uri].cmd_vel_2d['timestamp']
-
- if time.time() < timestamp + 0.5:
- self.get_logger().info(f'{uri}: Received 2d twist message')
-
- msg = self.swarm._cfs[uri].cmd_vel_2d['msg']
-
- vx = msg.linear.y
- vy = msg.linear.x
- yawrate = msg.angular.z
- self.swarm._cfs[uri].cf.commander.send_hover_setpoint(
- vx, vy, yawrate, height)
- self.swarm._cfs[uri].cmd_vel_2d['hold_pos'] = False
- else:
- if self.swarm._cfs[uri].cmd_vel_2d['hold_pos'] == False:
- self.get_logger().info(f'{uri}: Sent Position hold')
-
- self.swarm._cfs[uri].cf.high_level_commander.go_to(0,0,0,0,0,relative=1)
- self.swarm._cfs[uri].cmd_vel_2d['hold_pos'] = True
-
 
  def _init_default_logblocks(self, prefix, link_uri, list_logvar, global_logging_enabled, topic_type):
  """
@@ -682,14 +645,10 @@ def _takeoff_callback(self, request, response, uri="all"):
  self.swarm._cfs[link_uri].cf.high_level_commander.takeoff(
  request.height, duration
  )
- self.swarm._cfs[link_uri].cmd_vel_2d['current_height'] = request.height
-
  else:
  self.swarm._cfs[uri].cf.high_level_commander.takeoff(
  request.height, duration
  )
- self.swarm._cfs[uri].cmd_vel_2d['current_height'] = request.height
-
 
  return response
 
@@ -710,13 +669,10 @@ def _land_callback(self, request, response, uri="all"):
  self.swarm._cfs[link_uri].cf.high_level_commander.land(
  request.height, duration, group_mask=request.group_mask
  )
- self.swarm._cfs[link_uri].cmd_vel_2d['current_height'] = request.height
  else:
  self.swarm._cfs[uri].cf.high_level_commander.land(
  request.height, duration, group_mask=request.group_mask
  )
- self.swarm._cfs[uri].cmd_vel_2d['current_height'] = request.height
-
 
  return response
 
@@ -787,21 +743,17 @@ def _cmd_vel_legacy_changed(self, msg, uri=""):
  self.swarm._cfs[uri].cf.commander.send_setpoint(
  roll, pitch, yawrate, thrust)
 
- def _cmd_vel_2d_changed(self, msg, uri=""):
+ def _cmd_hover_changed(self, msg, uri=""):
  """
- Topic update callback to control the 2d velocity and thrust
- of the crazyflie with teleop
+ Topic update callback to control the hover command
+ of the crazyflie from the velocity multiplexer (vel_mux)
  """
- self.swarm._cfs[uri].cmd_vel_2d['timestamp'] = time.time()
- self.swarm._cfs[uri].cmd_vel_2d['msg'] = msg
-
- if FIX_HEIGHT_CMD_VEL_2D is False:
- vx = msg.linear.y
- vy = msg.linear.x
- z = msg.linear.z
- yawrate = msg.angular.z
- self.swarm._cfs[uri].cf.commander.send_hover_setpoint(
- vx, vy, yawrate, z)
+ vx = msg.vx
+ vy = msg.vy
+ z = msg.z_distance
+ yawrate = -1.0*degrees(msg.yaw_rate)
+ self.swarm._cfs[uri].cf.commander.send_hover_setpoint(vx, vy, yawrate, z)
+ self.get_logger().info(f"{uri}: Received hover topic {vx} {vy} {yawrate} {z}")
 
  def _remove_logging(self, request, response, uri="all"):
  """
@@ -847,7 +799,6 @@ def _add_logging(self, request, response, uri="all"):
  topic_name = request.topic_name
  frequency = request.frequency
  variables = request.vars
- print(self.cf_dict[uri] + "/logs/pose/frequency/", frequency)
  if topic_name in self.default_log_type.keys():
  try:
  self.declare_parameter(

crazyflie/scripts/vel_mux.py

@@ -0,0 +1,95 @@
+#!/usr/bin/env python3
+
+"""
+A Twist message handler that get incoming twist messages from 
+ external packages and handles proper takeoff, landing and
+ hover commands of connected crazyflie in the crazyflie_server
+ node
+
+ 2022 - K. N. McGuire (Bitcraze AB)
+"""
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import Twist
+from crazyflie_interfaces.srv import Takeoff, Land
+from crazyflie_interfaces.msg import Hover
+import time
+
+class VelMux(Node):
+ def __init__(self):
+ super().__init__('vel_mux')
+ self.declare_parameter('hover_height', 0.5)
+ self.declare_parameter('robot_prefix', '/cf1')
+ self.declare_parameter('incoming_twist_topic', '/cmd_vel')
+
+ self.hover_height = self.get_parameter('hover_height').value
+ robot_prefix = self.get_parameter('robot_prefix').value
+ incoming_twist_topic = self.get_parameter('incoming_twist_topic').value
+
+ self.subscription = self.create_subscription(
+ Twist,
+ incoming_twist_topic,
+ self.cmd_vel_callback,
+ 10)
+ self.msg_cmd_vel = Twist()
+ self.received_first_cmd_vel = False
+ timer_period = 0.1
+ self.timer = self.create_timer(timer_period, self.timer_callback)
+ 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
+
+ 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.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:
+ if self.msg_cmd_vel.linear.z >= 0:
+ msg = Hover()
+ 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 = self.hover_height
+ self.publisher_hover.publish(msg)
+ else:
+ req = Land.Request()
+ req.height = 0.1
+ req.duration = rclpy.duration.Duration(seconds=2.0).to_msg()
+ self.land_client.call_async(req)
+ time.sleep(2.0) 
+ self.cf_has_taken_off = False
+ self.received_first_cmd_vel = False
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ vel_mux = VelMux()
+
+ rclpy.spin(vel_mux)
+
+ vel_mux.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()

crazyflie_examples/launch/keyboard_velmux_launch.py

@@ -0,0 +1,36 @@
+import os
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ # load crazyflies
+ crazyflies_yaml = os.path.join(
+ get_package_share_directory('crazyflie'),
+ 'config',
+ 'crazyflies.yaml')
+
+ with open(crazyflies_yaml, 'r') as ymlfile:
+ crazyflies = yaml.safe_load(ymlfile)
+
+ server_params = crazyflies
+
+ return LaunchDescription([
+ Node(
+ package='crazyflie',
+ executable='crazyflie_server.py',
+ name='crazyflie_server',
+ output='screen',
+ parameters=[server_params]
+ ),
+ Node(
+ package='crazyflie',
+ executable='vel_mux.py',
+ name='vel_mux',
+ output='screen',
+ parameters=[{"hover_height": 0.3},
+ {"incoming_twist_topic": "/cmd_vel"},
+ {"robot_prefix": "/cf1"}]
+ ),
+ ])

crazyflie_examples/setup.py

@@ -1,4 +1,6 @@
 from setuptools import setup
+import os
+from glob import glob
 
 package_name = 'crazyflie_examples'
 
@@ -11,6 +13,7 @@
  ('share/ament_index/resource_index/packages',
  ['resource/' + package_name]),
  ('share/' + package_name, ['package.xml']),
+ (os.path.join('share', package_name), glob('launch/*launch.py'))
  ],
  install_requires=['setuptools'],
  zip_safe=True,

docs2/usage.rst

@@ -48,6 +48,7 @@ Enabling Logblocks
 ~~~~~~~~~~~~~~~~~~
 
 In one terminal run
+
 .. code-block:: bash
 
  ros2 launch crazyflie launch.py backend:=cflib
@@ -69,8 +70,8 @@ To close the logblocks again, run:
  ros2 service call /cf2/remove_logging crazyflie_interfaces/srv/RemoveLogging "{topic_name: 'pose'}"
 
 
-Teleoperation
-~~~~~~~~~~~~~
+Teleoperation controller
+~~~~~~~~~~~~~~~~~~~~~~~~
 
 We currently assume an XBox controller (the button mapping can be changed in teleop.yaml). It is possible to fly in different modes, including attitude-control and position-control (in which case any localization system can assist.)
 
@@ -79,6 +80,27 @@ We currently assume an XBox controller (the button mapping can be changed in tel
  ros2 launch crazyflie launch.py
 
 
+Teleoperation keyboard
+~~~~~~~~~~~~~~~~~~~~~~
+We have an example of the telop_twist_keyboard package working together with the crazyflie
+
+First make sure that the crazyflies.yaml has the right URI and if you are using the flowdeck, 
+set the controller to 1 (PID)
+
+Then, run the following launch file to start up the crazyflie server (CFlib):
+
+.. code-block:: bash
+
+ ros2 launch crazyflie_examples keyboard_velmux_launch.py
+
+in another terminal run:
+
+.. code-block:: bash
+
+ ros2 run teleop_twist_keyboard telop_twist_keyboard
+
+Use 't' to take off, and 'b' to land. For the rest, use the instructions of the telop package. 
+
 Python scripts
 ~~~~~~~~~~~~~~