0
$\begingroup$

my problem is basically about tf_fransformations package. I installed and changed all dependencies according to enter link description here, 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="[email protected]">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='[email protected]',
    description='Publishing Crazyflie Logging Variable',
    license='MIT',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
                'crazyflie_publisher = crazyflie_ros2.crazyflie_publisher:main',
        ],
    },
)
$\endgroup$

1 Answer 1

1
$\begingroup$

The key is

'[crazyflie_publisher-1] ModuleNotFoundError: No module named 'transforms3d'

This is a common python error, which means that the module 'transform3d' is missing in your python environment.

You will need to do

pip3 install transforms3d

This could have perhaps also been solved by running ROSdep, as that package had that as a dependency.

Edit: Also make sure that you are using the right Python version for ROS the package was installed for. This can happen if you have multiple installs of Python on your machine (see this post as well)

P.S. Just a reference for the others watching this, this is the repository of question with the same issue it is referring too:

https://github.com/knmcguire/crazyflie_ros2_experimental/issues/9

$\endgroup$
9
  • $\begingroup$ I'm pretty sure that I installed that library. However, I'm getting same feedback on the terminal. $\endgroup$ Oct 2, 2023 at 14:26
  • $\begingroup$ that's odd... perhaps a quick way to check that is to just try in terminal with python directly. Just do 'pip3 install transforms3d' and then in the same terminal (assuming you have ubuntu), just write python3 and in terminal editor opens up. In there, write the same thing, namely 'from transforms3d import *'. Are you also getting an error there? $\endgroup$ Oct 2, 2023 at 14:32
  • $\begingroup$ Also, perhaps good to check out those rosdep instructions I referred too... maybe your ROS environment is using a different python install? It could happen that it's therefore not installing it for the right version (see this as well robotics.stackexchange.com/questions/94176/…) $\endgroup$ Oct 2, 2023 at 14:41
  • $\begingroup$ I'm checking it now. $\endgroup$ Oct 2, 2023 at 17:13
  • $\begingroup$ I have not gotten same error on python3, it works on the pyhon3 $\endgroup$ Oct 2, 2023 at 17:21

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.