From dca74cc262ef23b41280fef1dbba908a7d2105fb Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com>
Date: Mon, 9 May 2022 15:51:43 +0200
Subject: [PATCH] Code not working
---
CMakeLists.txt | 4 +-
config/omniwheels_config.yaml | 1 +
launch/launch_controller.launch | 2 +
package.xml | 4 +
scripts/crazzyflie_logger.py | 0
scripts/crazzyflie_logger_node.py | 206 ++++++++++++++
scripts/omni_wheel_controller_node.py | 118 +-------
scripts/test.py | 371 ++++++++++++++++++++++++++
8 files changed, 587 insertions(+), 119 deletions(-)
delete mode 100644 scripts/crazzyflie_logger.py
create mode 100644 scripts/crazzyflie_logger_node.py
create mode 100644 scripts/test.py
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8c87e83..737581e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
project(omniwheels_controller)
## Find catkin and any catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg geometry_msgs message_generation)
+find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg geometry_msgs message_generation tf)
add_message_files(
FILES
motorCommand.msg
@@ -22,7 +22,7 @@ include_directories(include ${catkin_INCLUDE_DIRS})
#add_executable(talker src/talker.cpp)
#target_link_libraries(talker ${catkin_LIBRARIES})
-catkin_install_python(PROGRAMS scripts/omni_wheel_controller_node.py
+catkin_install_python(PROGRAMS scripts/omni_wheel_controller_node.py scripts/crazyflie_logger_node.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
diff --git a/config/omniwheels_config.yaml b/config/omniwheels_config.yaml
index 0bdf958..5f12b34 100644
--- a/config/omniwheels_config.yaml
+++ b/config/omniwheels_config.yaml
@@ -3,4 +3,5 @@ name: "omniwheel1"
id: 1
channel: 90 # change to the correct one
type: default
+position_offset: 0.11 # ofsset from crazyflie module to the cenyter of the omniwheels
diff --git a/launch/launch_controller.launch b/launch/launch_controller.launch
index b6446ab..db8dd3e 100644
--- a/launch/launch_controller.launch
+++ b/launch/launch_controller.launch
@@ -2,5 +2,7 @@
<rosparam command="load" file="$(find omniwheels_controller)/config/omniwheels_config.yaml"/>
<group ns = "omniwheel1">
<node name="omniwheel_controller" pkg="omniwheels_controller" type="omni_wheel_controller_node.py" output="screen"/>
+ <node name="crazyflie_logger" pkg="omniwheels_controller" type="crazyflie_logger_node.py" output="screen"/>
+
</group>
</launch>
diff --git a/package.xml b/package.xml
index ac21299..450f1d7 100644
--- a/package.xml
+++ b/package.xml
@@ -27,6 +27,8 @@
<build_depend>roscpp_serialization</build_depend>
<build_depend>rostime</build_depend>
<build_depend>std_msgs</build_depend>
+ <build_depend>tf</build_depend>
+
<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>
@@ -39,4 +41,6 @@
<run_depend>roscpp_serialization</run_depend>
<run_depend>rostime</run_depend>
<run_depend>std_msgs</run_depend>
+ <run_depend>tf</run_depend>
+
</package>
diff --git a/scripts/crazzyflie_logger.py b/scripts/crazzyflie_logger.py
deleted file mode 100644
index e69de29..0000000
diff --git a/scripts/crazzyflie_logger_node.py b/scripts/crazzyflie_logger_node.py
new file mode 100644
index 0000000..8cd09eb
--- /dev/null
+++ b/scripts/crazzyflie_logger_node.py
@@ -0,0 +1,206 @@
+import logging
+import time
+from threading import Timer
+from typing import Set
+import numpy as np
+import matplotlib.pyplot as plt
+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.crazyflie.syncLogger import SyncLogger
+from cflib.positioning.position_hl_commander import PositionHlCommander
+from cflib.positioning.motion_commander import MotionCommander
+
+# ros stuff
+import rospy
+import time
+from std_msgs.msg import String
+from omniwheels_controller.msg import motorCommand
+from geometry_msgs.msg import Twist
+from geometry_msgs.msg import Pose
+from tf import quaternion_from_euler
+
+
+uri = uri_helper.uri_from_env(default='usb://0') # Connection-uri for crazyflie via USB
+
+
+class ParameterLogging:
+ """
+ Simple logging example class that logs the Stabilizer from a supplied
+ link uri and disconnects after 5s.
+ """
+ period_in_ms = 20
+
+ def __init__(self, link_uri,cf):
+
+ # ros stuff
+ self._pose_data_ = Pose()
+ self._vel_data_ = Twist()
+ # create publishers
+ self._position_pub = rospy.Publisher("/pose", Pose,queue_size=100)
+ self._velocity_pub = rospy.Publisher("/vel", Twist,queue_size=100)
+ robot_name = rospy.get_param("/name")
+ print("Started " + robot_name + " Logging")
+ # self._cf = Crazyflie(rw_cache='./cache')
+ self._cf = cf
+ print("Connectiong callbacks")
+ # Connect some callbacks from the Crazyflie API
+ 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)
+
+
+
+
+
+ def _connected(self, link_uri):
+ self.reset_estimator(cf)
+ print("Reset estimation complete")
+
+ # The definition of the logconfig can be made before connecting
+ self._lg_stab = LogConfig(name='Position value', period_in_ms=self.period_in_ms)
+ self._lg_stab.add_variable('stateEstimateZ.x', 'int16')
+ self._lg_stab.add_variable('stateEstimateZ.y', 'int16')
+ self._lg_stab.add_variable('stateEstimateZ.z', 'int16')
+ self._lg_stab.add_variable('stabilizer.yaw', 'float')
+
+
+ self._lg_est_vel = LogConfig(name='Velocity value', period_in_ms=self.period_in_ms)
+ self._lg_est_vel.add_variable('stateEstimateZ.vx'.'float')
+ self._lg_est_vel.add_variable('stateEstimateZ.vy', 'float')
+ self._lg_est_vel.add_variable('stateEstimatorZ.rateYaw', 'float')
+
+ try:
+ self._cf.log.add_config(self._lg_stab)
+ self._cf.log.add_config(self._lg_est_vel)
+ # This callback will receive the data
+ self._lg_stab.data_received_cb.add_callback(self._pos_log_data)
+ self._lg_est_vel.data_received_cb.add_callback(self._vel_log_data)
+
+
+ # This callback will be called on errors
+ self._lg_stab.error_cb.add_callback(self._log_error)
+ self._lg_est_vel.error_cb.add_callback(self._log_error)
+
+ # Start the logging
+ self._lg_stab.start()
+ self._lg_est_vel.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.')
+
+ # Start a timer to disconnect in 10s
+ # t = Timer(5, self._cf.close_link)
+ # t.start()
+
+
+ def reset_estimator(self):
+ self._cf.param.set_value('kalman.resetEstimation','1')
+ time.sleep(0.1)
+ self._cf.param.set_value('kalman.resetEstimation', '1')
+ print("Waiting for position estimate")
+ # self.wait_for_position_estimator()
+
+ def wait_for_position_estimator(self):
+ print('Waiting for estimator to find position...')
+ log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
+ log_config.add_variable('kalman.varPX', 'float')
+ log_config.add_variable('kalman.varPY', 'float')
+ log_config.add_variable('kalman.varPZ', 'float')
+
+ var_y_history = [1000] * 10
+ var_x_history = [1000] * 10
+ var_z_history = [1000] * 10
+
+ threshold = 0.001
+
+ with SyncLogger(self._cf, log_config) as logger:
+ for log_entry in logger:
+ data = log_entry[1]
+ var_y_history = [1000] * 10
+ var_x_history = [1000] * 10
+ var_z_history = [1000] * 10
+
+ threshold = 0.001
+ # with SyncLogger(self._cf, self._lg_stab) as logger:
+ while True:
+
+ var_x_history.append(data['kalman.varPX'])
+ var_x_history.pop(0)
+ var_y_history.append(data['kalman.varPY'])
+ var_y_history.pop(0)
+ var_z_history.append(data['kalman.varPZ'])
+ var_z_history.pop(0)
+
+
+ min_x = min(var_x_history)
+ max_x = max(var_x_history)
+ min_y = min(var_y_history)
+ max_y = max(var_y_history)
+ min_z = min(var_z_history)
+ max_z = max(var_z_history)
+
+ # print("{} {} {}".
+ # format(max_x - min_x, max_y - min_y, max_z - min_z))
+
+ if (max_x - min_x) < threshold and (
+ max_y - min_y) < threshold and (
+ max_z - min_z) < threshold:
+ break
+
+
+
+ def _log_error(self, logconf, msg):
+ """Callback from the log API when an error occurs"""
+ print('Error when logging %s: %s' % (logconf.name, msg))
+
+ def _pos_log_data(self, timestamp, data, logconf):
+
+ self._pos_log_data.position.x = data['stateEstimateZ.x']/1000.0
+ self._pos_log_data.position.y = data['stateEstimateZ.y']/1000.0
+ self._pos_log_data.position.z = data['stateEstimateZ.z']/1000.0
+ self._pos_log_data.orientaton = quaternion_from_euler(0,, data['stateEstimate.yaw'])
+ self.position_pub.publish(self._pos_log_data)
+ def _vel_log_data(self, timestamp,data, logconf):
+ self._pos_log_data.linear.x = data['stateEstimateZ.vx']/1000.0
+ self._vel_log_data.linear.y = data['stateEstimateZ.vy']/1000.0
+ self._vel_log_data.angular.z = data['stateEstimateZ.rateYaw']/1000.0
+ self.velocity_pub.publish(self._vel_log_data)
+ def _connection_failed(self, link_uri, msg):
+ """Callback when connection initial connection fails (i.e no Crazyflie
+ at the specified address)"""
+ print('Connection to %s failed: %s' % (link_uri, msg))
+ self.is_connected = False
+
+ def _connection_lost(self, link_uri, msg):
+ """Callback when disconnected after a connection has been made (i.e
+ Crazyflie moves out of range)"""
+ print('Connection to %s lost: %s' % (link_uri, msg))
+
+ def _disconnected(self, link_uri):
+ """Callback when the Crazyflie is disconnected (called in all cases)"""
+ print('Disconnected from %s' % link_uri)
+ self.is_connected = False
+
+
+
+if __name__ == '__main__':
+ rospy.init_node("crazyflie_logger_node")
+ # Initialize the low-level drivers
+ cflib.crtp.init_drivers()
+ cf=Crazyflie(rw_cache='./cache')
+ le = ParameterLogging(uri, cf)
+ cf=Crazyflie(rw_cache='./cache')
+
+ try:
+ le = ParameterLogging(uri, cf)
+ except rospy.ROSInterruptException:
+ pass
+
+
+
diff --git a/scripts/omni_wheel_controller_node.py b/scripts/omni_wheel_controller_node.py
index 3691c1a..633c916 100755
--- a/scripts/omni_wheel_controller_node.py
+++ b/scripts/omni_wheel_controller_node.py
@@ -10,123 +10,7 @@ from geometry_msgs.msg import Twist
from dynamixel.model.xm430_w210_t_r import XM430_W210_T_R
import dynamixel.channel
import time
-
-
-
-
-class CrazyLogger:
-
- """
- Simple logging class that logs the Crazyflie Stabilizer from a supplied
- link uri and disconnects after 60s.
- """
- # Define which states to log:
- namelink = {'stateEstimate.x': 0, 'stateEstimate.y': 1,'stateEstimate.z': 2, 'stabilizer.yaw':3}
- #_Offset for crazyflie position on robot to center
- position_offset = 0.11
-
- def __init__(self, link_uri):
- """ Initialize and run the example with the specified link_uri """
-
- self._cf = Crazyflie(rw_cache='./cache')
- self._state = np.array([0.0,0.0,0.0,0.0])
-
- # Connect some callbacks from the Crazyflie API
- 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)
-
- print('Connecting to %s' % link_uri)
-
- # Try to connect to the Crazyflie
- self._cf.open_link(link_uri)
-
- # Variable used to keep main loop occupied until disconnect
- self.is_connected = True
-
- def _connected(self, link_uri):
- """ This callback is called form the Crazyflie API when a Crazyflie
- has been connected and the TOCs have been downloaded."""
- print('Connected to %s' % link_uri)
-
- # The definition of the logconfig can be made before connecting
- 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.yaw', 'float')
-
- # The fetch-as argument can be set to FP16 to save space in the log packet
- #self._lg_stab.add_variable('pm.vbat', 'FP16')
-
- # Adding the configuration cannot be done until a Crazyflie is
- # connected, since we need to check that the variables we
- # would like to log are in the TOC.
- try:
- self._cf.log.add_config(self._lg_stab)
- # This callback will receive the data
- self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
- # This callback will be called on errors
- self._lg_stab.error_cb.add_callback(self._stab_log_error)
- # # Start a timer to disconnect in 10s
- t = Timer(60, self._cf.close_link)
- t.start()
-
- 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"""
- #print(f'[{timestamp}][{logconf.name}]: ', end='')
- for name, value in data.items():
- self._state[self._namelink[name]] = value
- #print(f'{name}: {value:3.3f} ', end='\n')
- #print()
-
- def _connection_failed(self, link_uri, msg):
- """Callback when connection initial connection fails (i.e no Crazyflie
- at the specified address)"""
- print('Connection to %s failed: %s' % (link_uri, msg))
- self.is_connected = False
-
- def _connection_lost(self, link_uri, msg):
- """Callback when disconnected after a connection has been made (i.e
- Crazyflie moves out of range)"""
- print('Connection to %s lost: %s' % (link_uri, msg))
-
- def _disconnected(self, link_uri):
- """Callback when the Crazyflie is disconnected (called in all cases)"""
- print('Disconnected from %s' % link_uri)
- self.is_connected = False
-
- def x(self):
- """ Return x-position of robot center"""
- # Note that the offset of the crazyflie mount position is included
- return self._state[0] - np.sin(self._state[3])*self._position_offset
-
- def y(self):
- """ Return y-position of robot center"""
- # Note that the offset of the crazyflie mount position is included
- return self._state[1] + np.cos(self._state[3])*self._position_offset
-
- def z(self):
- """ Return z-position of robot center"""
- return self._state[2]
-
- def theta(self):
- """ Return direction of robot center, measured in radians between -pi and pi."""
- # Note that the offset of the crazyflie mount position is included
- return np.mod(self._state[3]*np.pi/180 + 11*np.pi/6,2*np.pi)-np.pi
-
- def states(self):
- return self._state
-
- def close(self):
- self._cf.close_link()
-
-
+
class omniWheelvelocityController:
def __init__(self):
diff --git a/scripts/test.py b/scripts/test.py
new file mode 100644
index 0000000..10074cc
--- /dev/null
+++ b/scripts/test.py
@@ -0,0 +1,371 @@
+import logging
+import time
+from threading import Timer
+from typing import Set
+import numpy as np
+import matplotlib.pyplot as plt
+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.crazyflie.syncLogger import SyncLogger
+from cflib.positioning.position_hl_commander import PositionHlCommander
+from cflib.positioning.motion_commander import MotionCommander
+uri = uri_helper.uri_from_env(default='usb://0') # Connection-uri for crazyflie via USB
+
+
+drone_connected = False
+
+# Only output errors from the logging framework
+logging.basicConfig(level=logging.ERROR)
+
+sequence = [
+ (0.0, 0.3,(1.0-0.5),0.0),
+ (0.0, 0.0,(1.0-0.5),0.0),
+ (0.0,-0.3,(1.0-0.5),0.0),
+ (0.0, -0.3,(1.5-0.5),0.0),
+ (0.0, 0.0,(1.5-0.5),0.0),
+ (0.0, 0.3,(1.5-0.5),0.0),
+ (0.0, 0.3,(2.0-0.5),0.0),
+ (0.0, 0.0,(2.0-0.5),0.0),
+ (0.0,-0.3,(2.0-0.5),0.0),
+ (0.0, 0.0,(1.0-0.5),0.0),
+ (0.0, 0.0,0.4,0.0)
+
+]
+
+
+current_setpoint = [0,0,0,0]
+setpoint_list = np.array([[0,0,0,0]])
+current_pose_list = np.array([[0,0,0,0]])
+stop = False
+
+
+class ParameterLogging:
+ """
+ Simple logging example class that logs the Stabilizer from a supplied
+ link uri and disconnects after 5s.
+ """
+ period_in_ms = 20
+
+ def __init__(self, link_uri,cf):
+ """ Initialize and run the example with the specified link_uri """
+
+ # self._cf = Crazyflie(rw_cache='./cache')
+ self._cf = cf
+ print("Connectiong callbacks")
+ # Connect some callbacks from the Crazyflie API
+ 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)
+
+
+
+ def _connected(self, link_uri):
+ global drone_connected
+ """ This callback is called form the Crazyflie API when a Crazyflie
+ has been connected and the TOCs have been downloaded."""
+ print('Connected to %s' % link_uri)
+ drone_connected = True
+
+ self.reset_estimator(cf)
+ print("Reset estimation complete")
+
+ # The definition of the logconfig can be made before connecting
+ self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=self.period_in_ms)
+ 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')
+ # The fetch-as argument can be set to FP16 to save space in the log packet
+ # self._lg_stab.add_variable('pm.vbat', 'FP16')
+
+ self._lg_kal_pos = LogConfig(name='Kalman Position', period_in_ms=self.period_in_ms)
+ self._lg_kal_pos.add_variable('kalman.stateX', 'float')
+ self._lg_kal_pos.add_variable('kalman.stateY', 'float')
+ self._lg_kal_pos.add_variable('kalman.stateZ', 'float')
+
+ # self._lg_kal_vel = LogConfig(name='Kalman Velocity', period_in_ms=self.period_in_ms)
+ # self._lg_kal_vel.add_variable('kalman.statePX', 'float')
+ # self._lg_kal_vel.add_variable('kalman.statePY', 'float')
+ # self._lg_kal_vel.add_variable('kalman.statePZ', 'float')
+
+ self._lg_kal_alt = LogConfig(name='Kalman Altitude', period_in_ms=self.period_in_ms)
+ self._lg_kal_alt.add_variable('kalman.q0', 'float')
+ self._lg_kal_alt.add_variable('kalman.q1', 'float')
+ self._lg_kal_alt.add_variable('kalman.q2', 'float')
+ self._lg_kal_alt.add_variable('kalman.q3', 'float')
+
+ # Adding the configuration cannot be done until a Crazyflie is
+ # connected, since we need to check that the variables we
+ # would like to log are in the TOC.
+ try:
+ self._cf.log.add_config(self._lg_stab)
+ self._cf.log.add_config(self._lg_kal_pos)
+ # self._cf.log.add_config(self._lg_kal_vel)
+ self._cf.log.add_config(self._lg_kal_alt)
+
+
+ # This callback will receive the data
+ self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
+ self._lg_kal_pos.data_received_cb.add_callback(self._pos_log_data)
+ # self._lg_kal_vel.data_received_cb.add_callback(self._vel_log_data)
+ self._lg_kal_alt.data_received_cb.add_callback(self._att_log_data)
+
+ # This callback will be called on errors
+ self._lg_stab.error_cb.add_callback(self._log_error)
+ self._lg_kal_pos.error_cb.add_callback(self._log_error)
+ # self._lg_kal_vel.error_cb.add_callback(self._log_error)
+ self._lg_kal_alt.error_cb.add_callback(self._log_error)
+
+ # Start the logging
+ self._lg_stab.start()
+ self._lg_kal_pos.start()
+ # self._lg_kal_vel.start()
+ self._lg_kal_alt.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.')
+
+ self.reset_estimator()
+ print("Finished reset estimator")
+ # Start a timer to disconnect in 10s
+ # t = Timer(5, self._cf.close_link)
+ # t.start()
+
+
+ def reset_estimator(self):
+ self._cf.param.set_value('kalman.resetEstimation','1')
+ time.sleep(0.1)
+ self._cf.param.set_value('kalman.resetEstimation', '1')
+ print("Waiting for position estimate")
+ # self.wait_for_position_estimator()
+
+ def wait_for_position_estimator(self):
+ print('Waiting for estimator to find position...')
+ log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
+ log_config.add_variable('kalman.varPX', 'float')
+ log_config.add_variable('kalman.varPY', 'float')
+ log_config.add_variable('kalman.varPZ', 'float')
+
+ var_y_history = [1000] * 10
+ var_x_history = [1000] * 10
+ var_z_history = [1000] * 10
+
+ threshold = 0.001
+
+ with SyncLogger(self._cf, log_config) as logger:
+ for log_entry in logger:
+ data = log_entry[1]
+ var_y_history = [1000] * 10
+ var_x_history = [1000] * 10
+ var_z_history = [1000] * 10
+
+ threshold = 0.001
+ # with SyncLogger(self._cf, self._lg_stab) as logger:
+ while True:
+
+ var_x_history.append(data['kalman.varPX'])
+ var_x_history.pop(0)
+ var_y_history.append(data['kalman.varPY'])
+ var_y_history.pop(0)
+ var_z_history.append(data['kalman.varPZ'])
+ var_z_history.pop(0)
+
+
+ min_x = min(var_x_history)
+ max_x = max(var_x_history)
+ min_y = min(var_y_history)
+ max_y = max(var_y_history)
+ min_z = min(var_z_history)
+ max_z = max(var_z_history)
+
+ # print("{} {} {}".
+ # format(max_x - min_x, max_y - min_y, max_z - min_z))
+
+ if (max_x - min_x) < threshold and (
+ max_y - min_y) < threshold and (
+ max_z - min_z) < threshold:
+ break
+
+
+
+ def _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):
+ global current_setpoint
+ global current_pose_list
+ global setpoint_list
+ global stop
+ self.stab_att = np.r_[data['stateEstimate.x'],
+ data['stateEstimate.y'],
+ data['stateEstimate.z']]
+
+ current_pose_list =np.append(current_pose_list,[[self.stab_att[0],self.stab_att[1],self.stab_att[2],0]], axis=0)
+ setpoint_list = np.append(setpoint_list,[current_setpoint], axis=0)
+ print(current_setpoint)
+ # """Callback from a the log API when data arrives"""
+ # print(f'[{timestamp}][{logconf.name}]: ', end='')
+ # for name, value in data.items():
+ # print(f'{name}: {value:3.3f} ', end='')
+ # print()
+ def _pos_log_data(self, timestamp,data, logconf):
+ # global current_setpoint
+ # global current_pose_list
+ # global setpoint_list
+ # global stop
+ self.pos = np.r_[data['kalman.stateX'],
+ data['kalman.stateY'],
+ data['kalman.stateZ']]
+
+ # if not stop:
+ # current_pose_list =np.append(current_pose_list,[[self.pos[0],self.pos[1],self.pos[2],0]], axis=0)
+ # setpoint_list = np.append(setpoint_list,[current_setpoint], axis=0)
+ # print(current_setpoint)
+
+
+
+ # def _vel_log_data(self, timestamp,data, logconf):
+ # self.vel = np.r_[data[]]
+ def _att_log_data(self, timestamp,data, logconf):
+ self.attq = np.r_[data['kalman.q0'],
+ data['kalman.q1'],
+ data['kalman.q2'],
+ data['kalman.q3']]
+ # self.R = trans.quarternion_matrix(self.attq)[:3, :3]
+
+
+ def _connection_failed(self, link_uri, msg):
+ """Callback when connection initial connection fails (i.e no Crazyflie
+ at the specified address)"""
+ print('Connection to %s failed: %s' % (link_uri, msg))
+ self.is_connected = False
+
+ def _connection_lost(self, link_uri, msg):
+ """Callback when disconnected after a connection has been made (i.e
+ Crazyflie moves out of range)"""
+ print('Connection to %s lost: %s' % (link_uri, msg))
+
+ def _disconnected(self, link_uri):
+ """Callback when the Crazyflie is disconnected (called in all cases)"""
+ print('Disconnected from %s' % link_uri)
+ self.is_connected = False
+
+def simple_sequence(cf):
+ with SyncCrazyflie(uri, cf=cf) as scf:
+ with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc:
+ pc.forward(0.5)
+ pc.left(0.5)
+ pc.back(0.5)
+ pc.go_to(0.0, 0.0, 0.5)
+def motion_commander_test(cf):
+ with SyncCrazyflie(uri, cf=cf) as scf:
+ # We take off when the commander is created
+ with MotionCommander(scf) as mc:
+ time.sleep(1)
+
+ # There is a set of functions that move a specific distance
+ # We can move in all directions
+ mc.forward(0.8)
+ mc.back(0.8)
+ time.sleep(1)
+
+ mc.up(0.5)
+ mc.down(0.5)
+ time.sleep(1)
+
+ # We can also set the velocity
+ mc.right(0.5, velocity=0.8)
+ time.sleep(1)
+ mc.left(0.5, velocity=0.4)
+ time.sleep(1)
+
+ # We can do circles or parts of circles
+ mc.circle_right(0.5, velocity=0.5, angle_degrees=180)
+
+ # Or turn
+ mc.turn_left(90)
+ time.sleep(1)
+
+ # We can move along a line in 3D space
+ mc.move_distance(-1, 0.0, 0.5, velocity=0.6)
+ time.sleep(1)
+
+ # There is also a set of functions that start a motion. The
+ # Crazyflie will keep on going until it gets a new command.
+
+ mc.start_left(velocity=0.5)
+ # The motion is started and we can do other stuff, printing for
+ # instance
+ for _ in range(5):
+ print('Doing other work')
+ time.sleep(0.2)
+
+ # And we can stop
+ mc.stop()
+
+ # We land when the MotionCommander goes out of scope
+def send_position_test(cf, sequence):
+ global current_setpoint
+ global setpoint_list
+ global stop
+ global drone_connected
+ print("Waiting for drone to connect")
+ while drone_connected == False:
+ ...
+ print("Drone connected: position command starting")
+ for posi in sequence:
+ print('Setting position {}'.format(posi))
+ for i in range(20):
+ for i in range(4):
+ current_setpoint[i] = posi[i]
+ # print(current_setpoint)
+ cf.commander.send_position_setpoint(posi[0],posi[1],posi[2],posi[3])
+ time.sleep(0.1)
+
+ cf.commander.send_stop_setpoint()
+ time.sleep(0.1)
+ # print(len(setpoint_list))
+ # print(len(setpoint_list[0]))
+ # stpt_array = np.array(setpoint_list)
+ # print(setpoint_list)
+ stop = True
+ #plotting data
+ fig = plt.figure()
+
+ ax = fig.add_subplot(111, projection='3d')
+
+ ax.plot(setpoint_list[:,0],setpoint_list[:,1],setpoint_list[:,2])
+ ax.plot(current_pose_list[:,0],current_pose_list[:,1],current_pose_list[:,2])
+
+
+ plt.show()
+ print("Savin data to file")
+ np.savetxt("setpoint.csv",setpoint_list, delimiter="," )
+ np.savetxt("sensordata.csv",current_pose_list, delimiter="," )
+
+ # print(current_pose_list)
+
+
+
+
+if __name__ == '__main__':
+ # Initialize the low-level drivers
+ cflib.crtp.init_drivers()
+ cf=Crazyflie(rw_cache='./cache')
+ le = ParameterLogging(uri, cf)
+ # simple_sequence(cf)
+ # motion_commander_test(cf)
+ cf.open_link(uri)
+ send_position_test(cf, sequence)
+
+
+
--
GitLab