knmcguire / crazyflie_ros2_experimental Goto Github PK

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()

<?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>

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',
        ],
    },
)

$ ros2 launch crazyflie_ros2_simple_mapper simple_mapper_simulation_launch.py

$ ros2 run teleop_twist_keyboard teleop_twist_keyboard

[INFO] [launch]: All log files can be found below /home/kiko/.ros/log/2022-07-07-18-21-08-874676-kiko-HP-ZBook-103229
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [webots-1]: process started with pid [103232]
[INFO] [driver-2]: process started with pid [103234]
[INFO] [robot_state_publisher-3]: process started with pid [103237]
[driver-2] Cannot open file: /tmp/webots-103232-lxyt4b/WEBOTS_SERVER (retry count 1)
[robot_state_publisher-3] Parsing robot urdf xml string.
[robot_state_publisher-3] [INFO] [1657210868.967780395] [robot_state_publisher]: got segment 
[driver-2] Cannot open file: /tmp/webots-103232-lxyt4b/WEBOTS_SERVER (retry count 2)
[driver-2] Cannot open file: /tmp/webots-103232-lxyt4b/WEBOTS_SERVER (retry count 3)
[robot_state_publisher-3] Parsing robot urdf xml string.
[robot_state_publisher-3] Link body had 0 children
[robot_state_publisher-3] Link camera had 0 children
[robot_state_publisher-3] Link gps had 0 children
[robot_state_publisher-3] Link gyro had 0 children
[robot_state_publisher-3] Link inertial unit had 0 children
[robot_state_publisher-3] Link range_back had 0 children
[robot_state_publisher-3] Link range_front had 0 children
[robot_state_publisher-3] Link range_left had 0 children
[robot_state_publisher-3] Link range_right had 0 children
[robot_state_publisher-3] Link solid_0 had 0 children
[robot_state_publisher-3] Link solid_1 had 0 children
[robot_state_publisher-3] Link solid_2 had 0 children
[robot_state_publisher-3] Link solid had 0 children
[robot_state_publisher-3] [INFO] [1657210872.379969651] [robot_state_publisher]: got segment base_link
[robot_state_publisher-3] [INFO] [1657210872.379988398] [robot_state_publisher]: got segment body
[robot_state_publisher-3] [INFO] [1657210872.379991251] [robot_state_publisher]: got segment camera
[robot_state_publisher-3] [INFO] [1657210872.379993656] [robot_state_publisher]: got segment gps
[robot_state_publisher-3] [INFO] [1657210872.379996054] [robot_state_publisher]: got segment gyro
[robot_state_publisher-3] [INFO] [1657210872.379999465] [robot_state_publisher]: got segment inertial unit
[robot_state_publisher-3] [INFO] [1657210872.380002079] [robot_state_publisher]: got segment range_back
[robot_state_publisher-3] [INFO] [1657210872.380004431] [robot_state_publisher]: got segment range_front
[robot_state_publisher-3] [INFO] [1657210872.380006740] [robot_state_publisher]: got segment range_left
[robot_state_publisher-3] [INFO] [1657210872.380009063] [robot_state_publisher]: got segment range_right
[robot_state_publisher-3] [INFO] [1657210872.380011337] [robot_state_publisher]: got segment solid
[robot_state_publisher-3] [INFO] [1657210872.380013574] [robot_state_publisher]: got segment solid_0
[robot_state_publisher-3] [INFO] [1657210872.380015837] [robot_state_publisher]: got segment solid_1
[robot_state_publisher-3] [INFO] [1657210872.380018087] [robot_state_publisher]: got segment solid_2
[driver-2] Traceback (most recent call last):
[driver-2]   File "/home/kiko/roboticpark_ws/build/crazyflie_ros2_simulation/crazyflie_ros2_simulation/crazyflie_webots_driver.py", line 62, in init
[driver-2]     cffirmware.controllerPidInit()
[driver-2] AttributeError: module 'cffirmware' has no attribute 'controllerPidInit'
[driver-2] Traceback (most recent call last):
[driver-2]   File "/home/kiko/roboticpark_ws/build/crazyflie_ros2_simulation/crazyflie_ros2_simulation/crazyflie_webots_driver.py", line 76, in step
[driver-2]     rclpy.spin_once(self.node, timeout_sec=0)
[driver-2] AttributeError: 'CrazyflieWebotsDriver' object has no attribute 'node'