diff --git a/CMakeLists.txt b/CMakeLists.txt index 737581e9db0334f5eae8a321a9c9c043c272077a..96da5bec6bdfefcfdc1640d3d4733580e4b86843 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 tf) +find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg geometry_msgs message_generation tf2) add_message_files( FILES motorCommand.msg diff --git a/package.xml b/package.xml index 450f1d78051556e86d325c73d54cf9b8b71baf63..4e73a0d96f397cf078ffa4c6d1a2c45ae6cedf8b 100644 --- a/package.xml +++ b/package.xml @@ -27,7 +27,7 @@ <build_depend>roscpp_serialization</build_depend> <build_depend>rostime</build_depend> <build_depend>std_msgs</build_depend> - <build_depend>tf</build_depend> + <build_depend>tf2</build_depend> <build_depend>message_generation</build_depend> <run_depend>message_runtime</run_depend> @@ -41,6 +41,6 @@ <run_depend>roscpp_serialization</run_depend> <run_depend>rostime</run_depend> <run_depend>std_msgs</run_depend> - <run_depend>tf</run_depend> + <run_depend>tf2</run_depend> </package> diff --git a/scripts/crazzyflie_logger_node.py b/scripts/crazzyflie_logger_node.py index 8cd09eb7850b48e3de417d81fd3f0e3da6ccd2f6..1345a4c3c9a7f075c8c771374e5510cab8ebdfe3 100644 --- a/scripts/crazzyflie_logger_node.py +++ b/scripts/crazzyflie_logger_node.py @@ -20,11 +20,31 @@ 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 + +# numpy stuff +import numpy as np uri = uri_helper.uri_from_env(default='usb://0') # Connection-uri for crazyflie via USB +def get_quaternion_from_euler(roll, pitch, yaw): + """ + Convert an Euler angle to a quaternion. + + Input + :param roll: The roll (rotation around x-axis) angle in radians. + :param pitch: The pitch (rotation around y-axis) angle in radians. + :param yaw: The yaw (rotation around z-axis) angle in radians. + + Output + :return qx, qy, qz, qw: The orientation in quaternion [x,y,z,w] format + """ + qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) + qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) + qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) + + return [qx, qy, qz, qw] class ParameterLogging: """ @@ -164,7 +184,12 @@ class ParameterLogging: 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']) + quat_val = get_quaternion_from_euler(0,0,data['stateEstimate.yaw']); + + self._pos_log_data.orientaton.x = quat_val[0] + self._pos_log_data.orientaton.y =quat_val[1] + self._pos_log_data.orientaton.z = quat_val[2] + self._pos_log_data.orientaton.w = quat_val[3] 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