Crazyflie-Ros2 connection

[ntukenmez3]

Hi all, my problem is basically about tf_fransformations package. I installed and changed all dependencies according to https://index.ros.org/p/tf_transformations/, but it is not working. I used transforms3d to import the related things. Could you help us about that problem. I added the related code in our python files below, If needs I can share more, thanks.

terminal output:

ntukenmez3@ae-icps-407120:~/Documents/2023-crazy-flie/crazyflie_ros2_experimental/crazyflie_ros2$ ros2 launch crazyflie_ros2 crazyflie_publisher_launch.py
[INFO] [launch]: All log files can be found below /home/ntukenmez3/.ros/log/2023-10-01-16-07-47-625647-ae-icps-407120-3378000
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [crazyflie_publisher-1]: process started with pid [3378004]
[crazyflie_publisher-1] Traceback (most recent call last):
[crazyflie_publisher-1] File "/home/ntukenmez3/Documents/2023-crazy-flie/crazyflie_ros2_experimental/crazyflie_ros2/install/crazyflie_ros2/lib/crazyflie_ros2/crazyflie_publisher", line 33, in
[crazyflie_publisher-1] sys.exit(load_entry_point('crazyflie-ros2==0.0.0', 'console_scripts', 'crazyflie_publisher')())
[crazyflie_publisher-1] File "/home/ntukenmez3/Documents/2023-crazy-flie/crazyflie_ros2_experimental/crazyflie_ros2/install/crazyflie_ros2/lib/crazyflie_ros2/crazyflie_publisher", line 25, in importlib_load_entry_point
[crazyflie_publisher-1] return next(matches).load()
[crazyflie_publisher-1] File "/usr/lib/python3.10/importlib/metadata/init.py", line 171, in load
[crazyflie_publisher-1] module = import_module(match.group('module'))
[crazyflie_publisher-1] File "/usr/lib/python3.10/importlib/init.py", line 126, in import_module
[crazyflie_publisher-1] return _bootstrap._gcd_import(name[level:], package, level)
[crazyflie_publisher-1] File "", line 1050, in _gcd_import
[crazyflie_publisher-1] File "", line 1027, in _find_and_load
[crazyflie_publisher-1] File "", line 1006, in _find_and_load_unlocked
[crazyflie_publisher-1] File "", line 688, in _load_unlocked
[crazyflie_publisher-1] File "", line 883, in exec_module
[crazyflie_publisher-1] File "", line 241, in _call_with_frames_removed
[crazyflie_publisher-1] File "/home/ntukenmez3/Documents/2023-crazy-flie/crazyflie_ros2_experimental/crazyflie_ros2/install/crazyflie_ros2/lib/python3.10/site-packages/crazyflie_ros2/crazyflie_publisher.py", line 6, in
[crazyflie_publisher-1] from transforms3d import *
[crazyflie_publisher-1] ModuleNotFoundError: No module named 'transforms3d'
[ERROR] [crazyflie_publisher-1]: process has died [pid 3378004, exit code 1, cmd '/home/ntukenmez3/Documents/2023-crazy-flie/crazyflie_ros2_experimental/crazyflie_ros2/install/crazyflie_ros2/lib/crazyflie_ros2/crazyflie_publisher --ros-args'].

Python files:

1.crazyflie_publisher.py:

import rclpy
from rclpy.node import Node

from std_msgs.msg import String
from geometry_msgs.msg import Pose
#from tf_transformations import euler_from_quaternion
from transforms3d import *
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped
from sensor_msgs.msg import Range
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist

import cflib.crtp  # noqa
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.utils import uri_helper
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.positioning.motion_commander import MotionCommander

import math

from math import pi

URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
FLYING = True
def radians(degrees):  
    return degrees * math.pi / 180.0


class CrazyfliePublisher(Node):

    def __init__(self, link_uri):
        super().__init__('crazyflie_publisher')
        #self.pose_publisher = self.create_publisher(Pose, 'pose', 10)
        self.range_publisher = self.create_publisher(Range, 'zrange', 10)
        self.laser_publisher = self.create_publisher(LaserScan, 'scan', 10)
        self.odom_publisher = self.create_publisher(Odometry, 'odom', 10)
    
        self.create_subscription(Twist, 'cmd_vel', self.cmd_vel_callback, 1)

        self.tfbr = TransformBroadcaster(self)

        self._cf = Crazyflie(rw_cache='./cache')
        self._cf.connected.add_callback(self._connected)
        self._cf.disconnected.add_callback(self._disconnected)
        self._cf.connection_failed.add_callback(self._connection_failed)
        self._cf.connection_lost.add_callback(self._connection_lost)
        self._cf.open_link(link_uri)


        self.ranges= [0.0, 0.0, 0.0, 0.0, 0.0]
        self.create_timer(1.0/30.0, self.publish_laserscan_data)
        if FLYING:
            timer_period = 0.1  # seconds
            self.create_timer(timer_period, self.sendHoverCommand)


            self.hover = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'height': 0.3}


            self._cf.commander.send_hover_setpoint(
                self.hover['x'], self.hover['y'], self.hover['yaw'],
                self.hover['height'])

    def publish_laserscan_data(self):

        msg = LaserScan()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'base_link'
        msg.range_min = 0.01
        msg.range_max = 3.49
        msg.ranges = self.ranges
        msg.angle_min = 0.5 * 2*pi
        msg.angle_max =  -0.5 * 2*pi
        msg.angle_increment = -1.0*pi/2
        self.laser_publisher.publish(msg)


    def sendHoverCommand(self):
        hover_height =  self.hover['height'] + self.hover['z']*0.1
        self._cf.commander.send_hover_setpoint(
            self.hover['x'], self.hover['y'], self.hover['yaw'],
            hover_height)
        self.hover['height'] = hover_height


    def _connected(self, link_uri):

        self.get_logger().info('Connected!')
        self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=100)
        self._lg_stab.add_variable('stateEstimate.x', 'float')
        self._lg_stab.add_variable('stateEstimate.y', 'float')
        self._lg_stab.add_variable('stateEstimate.z', 'float')
        self._lg_stab.add_variable('stabilizer.roll', 'float')
        self._lg_stab.add_variable('stabilizer.pitch', 'float')
        self._lg_stab.add_variable('stabilizer.yaw', 'float')

        self._lg_range = LogConfig(name='Range', period_in_ms=100)
        self._lg_range.add_variable('range.zrange', 'uint16_t')
        self._lg_range.add_variable('range.front', 'uint16_t')
        self._lg_range.add_variable('range.right', 'uint16_t')
        self._lg_range.add_variable('range.left', 'uint16_t')
        self._lg_range.add_variable('range.back', 'uint16_t')

        try:
            self._cf.log.add_config(self._lg_stab)
            self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
            self._lg_stab.error_cb.add_callback(self._stab_log_error)
            self._lg_stab.start()
            self._cf.log.add_config(self._lg_range)
            self._lg_range.data_received_cb.add_callback(self._range_log_data)
            self._lg_range.error_cb.add_callback(self._range_log_error)
            self._lg_range.start()

        except KeyError as e:
            print('Could not start log configuration,'
                  '{} not found in TOC'.format(str(e)))
        except AttributeError:
            print('Could not add Stabilizer log config, bad configuration.')


    def _disconnected():
        print('disconnected')

    def _connection_failed(self, link_uri, msg):
        print('connection_failed')

    def _connection_lost(self, link_uri, msg):
        print('connection_lost')

    def _range_log_error(self, logconf, msg):
        """Callback from the log API when an error occurs"""
        print('Error when logging %s: %s' % (logconf.name, msg))

    def _range_log_data(self, timestamp, data, logconf):
        """Callback from a the log API when data arrives"""
        for name, value in data.items():
            print(f'{name}: {value:3.3f} ', end='')
        print()

        t_range = TransformStamped()
        q = tf_transformations.quaternion_from_euler(0, radians(90), 0)
        t_range.header.stamp = self.get_clock().now().to_msg()
        t_range.header.frame_id = 'base_link'
        t_range.child_frame_id = 'crazyflie_flowdeck'
        t_range.transform.rotation.x = q[0]
        t_range.transform.rotation.y = q[1]
        t_range.transform.rotation.z = q[2]
        t_range.transform.rotation.w = q[3]
        self.tfbr.sendTransform(t_range)

        zrange = float(data.get('range.zrange'))/1000.0
        msg = Range()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'crazyflie_flowdeck'
        msg.field_of_view = radians(4.7)
        msg.radiation_type = Range().INFRARED
        msg.min_range = 0.01
        msg.max_range = 3.5
        msg.range = zrange
        ##self.range_publisher.publish(msg)

        max_range = 3.49
        front_range = float(data.get('range.front'))/1000.0
        left_range = float(data.get('range.left'))/1000.0
        back_range = float(data.get('range.back'))/1000.0
        right_range = float(data.get('range.right'))/1000.0
        if front_range > max_range:
            front_range = float("inf")
        if left_range > max_range:
            left_range = float("inf")
        if right_range > max_range:
            right_range = float("inf")
        if back_range > max_range:
            back_range = float("inf")  
        self.ranges = [back_range, left_range, front_range, right_range, back_range]


    def cmd_vel_callback(self, twist):
        self.target_twist = twist

        self.hover['x'] = twist.linear.x
        self.hover['y'] = twist.linear.y
        self.hover['z'] = twist.linear.z
        self.hover['yaw'] = -1* math.degrees(twist.angular.z)
        


    def _stab_log_error(self, logconf, msg):
        """Callback from the log API when an error occurs"""
        print('Error when logging %s: %s' % (logconf.name, msg))

    def _stab_log_data(self, timestamp, data, logconf):
        """Callback from a the log API when data arrives"""

        """for name, value in data.items():
            print(f'{name}: {value:3.3f} ', end='')
        print()"""

        msg = Odometry()

        x = data.get('stateEstimate.x')
        y = data.get('stateEstimate.y')
        z = data.get('stateEstimate.z')
        roll = radians(data.get('stabilizer.roll'))
        pitch = radians(-1.0 * data.get('stabilizer.pitch'))
        yaw = radians(data.get('stabilizer.yaw'))
        msg.pose.pose.position.x = x
        msg.pose.pose.position.y = y
        msg.pose.pose.position.z = z
        q = tf_transformations.quaternion_from_euler(roll, pitch, yaw)
        msg.pose.pose.orientation.x = q[0]
        msg.pose.pose.orientation.y = q[1]
        msg.pose.pose.orientation.z = q[2]
        msg.pose.pose.orientation.w = q[3]
        self.odom_publisher.publish(msg)


        q_base = tf_transformations.quaternion_from_euler(0, 0, yaw)crazyflie_publisher.py:
        t_base = TransformStamped()
        t_base.header.stamp = self.get_clock().now().to_msg()
        t_base.header.frame_id = 'odom'
        t_base.child_frame_id = 'base_footprint'
        t_base.transform.translation.x = x
        t_base.transform.translation.y = y
        t_base.transform.translation.z = 0.0
        t_base.transform.rotation.x = q_base[0]
        t_base.transform.rotation.y = q_base[1]
        t_base.transform.rotation.z = q_base[2]
        t_base.transform.rotation.w = q_base[3]
        self.tfbr.sendTransform(t_base)

        t_odom = TransformStamped()
        t_odom.header.stamp = self.get_clock().now().to_msg()
        t_odom.header.frame_id = 'odom'
        t_odom.child_frame_id = 'base_footprint'
        q_odom = tf_transformations.quaternion_from_euler(0, 0, 0)
        t_odom.transform.translation.x = 0.0
        t_odom.transform.translation.y = 0.0
        t_odom.transform.translation.z = 0.1
        t_odom.transform.rotation.x = q_odom[0]
        t_odom.transform.rotation.y = q_odom[1]
        t_odom.transform.rotation.z = q_odom[2]
        t_odom.transform.rotation.w = q_odom[3]
    
        #self.tfbr.sendTransform(t_odom)

        t_cf = TransformStamped()
        q_cf = tf_transformations.quaternion_from_euler(roll, pitch, 0)
        t_cf.header.stamp = self.get_clock().now().to_msg()
        t_cf.header.frame_id = 'base_footprint'
        t_cf.child_frame_id = 'base_link'
        t_cf.transform.translation.x = 0.0
        t_cf.transform.translation.y = 0.0
        t_cf.transform.translation.z = z
        t_cf.transform.rotation.x = q_cf[0]
        t_cf.transform.rotation.y = q_cf[1]
        t_cf.transform.rotation.z = q_cf[2]
        t_cf.transform.rotation.w = q_cf[3]
        self.tfbr.sendTransform(t_cf)

        

def main(args=None):

    cflib.crtp.init_drivers()

    rclpy.init(args=args)

    crazyflie_publisher = CrazyfliePublisher(URI)

    rclpy.spin(crazyflie_publisher)

    crazyflie_publisher.destroy_node()
    rclpy.shutdown()
    



if __name__ == '__main__':
    main()

2.package.xml:

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>crazyflie_ros2</name>
  <version>0.0.0</version>
  <description>Publishing Crazyflie Logging Variables</description>
  <maintainer email="kimberly@bitcraze.io">knmcguire</maintainer>
  <license>MIT</license>

  <exec_depend>python3-cflib</exec_depend>
  <exec_depend>rclpy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>transforms3d</exec_depend>
  <exec_depend>python-transforms3d-pip</exec_depend>
  <exec_depend>tf2_ros</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>nav_msgs</exec_depend>


  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>

  <export>
    <build_type>ament_python</build_type>
  </export>
</package>

3.setup.py:

from setuptools import setup
import os
from glob import glob

package_name = 'crazyflie_ros2'
data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ]

def package_files(data_files, directory_list):
    paths_dict = {}
    for directory in directory_list:
        for (path, directories, filenames) in os.walk(directory):
            for filename in filenames:
                file_path = os.path.join(path, filename)
                install_path = os.path.join('share', package_name, path)
                if install_path in paths_dict.keys():
                    paths_dict[install_path].append(file_path)
                else:
                    paths_dict[install_path] = [file_path]

    for key in paths_dict.keys():
        data_files.append((key, paths_dict[key]))

    return data_files

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=package_files(data_files, [ 'resource/', 'launch/']),
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='knmcguire',
    maintainer_email='kimberly@bitcraze.io',
    description='Publishing Crazyflie Logging Variable',
    license='MIT',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
                'crazyflie_publisher = crazyflie_ros2.crazyflie_publisher:main',
        ],
    },
)