From 83dd6bd11423c7881be49c32224ae1bfc2bbce76 Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com>
Date: Tue, 10 May 2022 17:09:18 +0200
Subject: [PATCH] Positioning fixed
---
CMakeLists.txt | 2 +-
README.md | 5 +-
config/omniwheels_config.yaml | 2 +
package.xml | 2 -
...ogger_node.py => crazyflie_logger_node.py} | 118 +++++----
scripts/omni_wheel_controller_node.py | 230 +++++++++++-------
6 files changed, 223 insertions(+), 136 deletions(-)
rename scripts/{crazzyflie_logger_node.py => crazyflie_logger_node.py} (70%)
mode change 100644 => 100755
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 96da5be..3bdb17f 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 tf2)
+find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg geometry_msgs message_generation)
add_message_files(
FILES
motorCommand.msg
diff --git a/README.md b/README.md
index d45d20f..2d97c02 100644
--- a/README.md
+++ b/README.md
@@ -11,4 +11,7 @@ Doubts that occured during coding
make sure that the most main computer is the ros master and and source the
configure_ros_master.bash file
-the file is in the utills important
\ No newline at end of file
+the file is in the utills important
+
+issue with device permission usb crazyflie
+https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/installation/usb_permissions/
diff --git a/config/omniwheels_config.yaml b/config/omniwheels_config.yaml
index 5f12b34..ac92cd5 100644
--- a/config/omniwheels_config.yaml
+++ b/config/omniwheels_config.yaml
@@ -4,4 +4,6 @@ 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
+R: 0.0165
+
diff --git a/package.xml b/package.xml
index 4e73a0d..ecce13d 100644
--- a/package.xml
+++ b/package.xml
@@ -27,7 +27,6 @@
<build_depend>roscpp_serialization</build_depend>
<build_depend>rostime</build_depend>
<build_depend>std_msgs</build_depend>
- <build_depend>tf2</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>
@@ -41,6 +40,5 @@
<run_depend>roscpp_serialization</run_depend>
<run_depend>rostime</run_depend>
<run_depend>std_msgs</run_depend>
- <run_depend>tf2</run_depend>
</package>
diff --git a/scripts/crazzyflie_logger_node.py b/scripts/crazyflie_logger_node.py
old mode 100644
new mode 100755
similarity index 70%
rename from scripts/crazzyflie_logger_node.py
rename to scripts/crazyflie_logger_node.py
index 1345a4c..68da397
--- a/scripts/crazzyflie_logger_node.py
+++ b/scripts/crazyflie_logger_node.py
@@ -1,9 +1,9 @@
+#!/usr/bin/env python3
import logging
-import time
-from threading import Timer
-from typing import Set
import numpy as np
-import matplotlib.pyplot as plt
+from threading import Timer
+
+
import cflib.crtp # noqa
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
@@ -12,6 +12,7 @@ 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
+logging.basicConfig(level=logging.ERROR)
# ros stuff
import rospy
@@ -21,8 +22,6 @@ from omniwheels_controller.msg import motorCommand
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Pose
-# numpy stuff
-import numpy as np
uri = uri_helper.uri_from_env(default='usb://0') # Connection-uri for crazyflie via USB
@@ -51,47 +50,56 @@ class ParameterLogging:
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
- period_in_ms = 20
+ period_in_ms = 100
- def __init__(self, link_uri,cf):
+ def __init__(self, link_uri):
# 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)
+ 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")
+ self._position_offset = rospy.get_param("/position_offset")
+ #
print("Started " + robot_name + " Logging")
- # self._cf = Crazyflie(rw_cache='./cache')
- self._cf = cf
+ self._cf = Crazyflie(rw_cache='./cache')
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)
+ self.R = rospy.get_param("/R")
+
+ #try to connected to the crazyflie
+ self._cf.open_link(link_uri)
def _connected(self, link_uri):
- self.reset_estimator(cf)
+ print("Connected")
+ #self.reset_estimator(self._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')
+ self._lg_stab = LogConfig(name='formation_test_position', period_in_ms=self.period_in_ms)
+ self._lg_stab.add_variable('stateEstimate.yaw')
+ self._lg_stab.add_variable('stateEstimateZ.x')
+ self._lg_stab.add_variable('stateEstimateZ.y')
+ self._lg_stab.add_variable('stateEstimateZ.z')
+ print("Position configuraion finished")
+
+
+ self._lg_est_vel = LogConfig(name='formation_test_velocity', period_in_ms=self.period_in_ms)
+ self._lg_est_vel.add_variable('stateEstimateZ.vx')
+ self._lg_est_vel.add_variable('stateEstimateZ.vy')
+ self._lg_est_vel.add_variable('stateEstimateZ.rateYaw')
+
+ print("velocity configuraion finished")
try:
self._cf.log.add_config(self._lg_stab)
@@ -108,15 +116,19 @@ class ParameterLogging:
# Start the logging
self._lg_stab.start()
self._lg_est_vel.start()
+
+ print("Completed configuration")
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()
+ #t = Timer(5, self._cf.close_link)
+ #t.start()
+ #print("Timer started")
def reset_estimator(self):
@@ -180,22 +192,35 @@ class ParameterLogging:
print('Error when logging %s: %s' % (logconf.name, msg))
def _pos_log_data(self, timestamp, data, logconf):
+ x = data['stateEstimateZ.x']/1000.0
+ y = data['stateEstimateZ.y']/1000.0
+ z = data['stateEstimateZ.z']/1000.0
+ yaw = (data['stateEstimate.yaw'])
+ yaw_ = np.mod(yaw-30, 360);
+
+
+ quat_val = get_quaternion_from_euler(0,0,np.deg2rad(yaw_))
+
+
- 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
- quat_val = get_quaternion_from_euler(0,0,data['stateEstimate.yaw']);
+ self._pose_data_.orientation.x = quat_val[0]
+ self._pose_data_.orientation.y =quat_val[1]
+ self._pose_data_.orientation.z = quat_val[2]
+ self._pose_data_.orientation.w = quat_val[3]
+ self._pose_data_.position.x =x+ np.sin(np.deg2rad(yaw))*self._position_offset;
+ self._pose_data_.position.y = y- np.cos(np.deg2rad(yaw))*self._position_offset
+ self._pose_data_.position.z = z;
- 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)
+
+ self._position_pub.publish(self._pose_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)
+ self._vel_data_.linear.x = data['stateEstimateZ.vx']/1000.0
+ self._vel_data_.linear.y = data['stateEstimateZ.vy']/1000.0
+ self._vel_data_.angular.z = data['stateEstimateZ.rateYaw']/1000.0
+ self._velocity_pub.publish(self._vel_data_)
+
+
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
@@ -211,21 +236,20 @@ class ParameterLogging:
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
+
+ def close(self):
+ self._cf.close_link()
if __name__ == '__main__':
- rospy.init_node("crazyflie_logger_node")
+ 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:
+ le = ParameterLogging(uri)
+
+ except rospy.ROSInterruptException:
pass
-
diff --git a/scripts/omni_wheel_controller_node.py b/scripts/omni_wheel_controller_node.py
index 633c916..4d726ec 100755
--- a/scripts/omni_wheel_controller_node.py
+++ b/scripts/omni_wheel_controller_node.py
@@ -4,101 +4,161 @@ 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
+import math
+import numpy as np
# Dynamixel packages
from dynamixel.model.xm430_w210_t_r import XM430_W210_T_R
import dynamixel.channel
import time
-
+def euler_from_quaternion(x, y, z, w):
+ """
+ Convert a quaternion into euler angles (roll, pitch, yaw)
+ roll is rotation around x in radians (counterclockwise)
+ pitch is rotation around y in radians (counterclockwise)
+ yaw is rotation around z in radians (counterclockwise)
+ """
+ t0 = +2.0 * (w * x + y * z)
+ t1 = +1.0 - 2.0 * (x * x + y * y)
+ roll_x = math.atan2(t0, t1)
+
+ t2 = +2.0 * (w * y - z * x)
+ t2 = +1.0 if t2 > +1.0 else t2
+ t2 = -1.0 if t2 < -1.0 else t2
+ pitch_y = math.asin(t2)
+
+ t3 = +2.0 * (w * z + x * y)
+ t4 = +1.0 - 2.0 * (y * y + z * z)
+ yaw_z = math.atan2(t3, t4)
+
+ return roll_x, pitch_y, yaw_z # in radians
class omniWheelvelocityController:
- def __init__(self):
- # Load the servos
- self.servos = self.load_servos()
- self.motor_command = [0,0,0];
- self.send_command = False;
- self.vel_cmd_arrived = False;
- self.motor_command[0] = 0;
- self.motor_command[1] = 0;
- self.motor_command[2] = 0;
- self.vel_cmd = Twist()
- self.vel_cmd.linear.x = 0;
- self.vel_cmd.linear.y = 0;
- self.vel_cmd.angular.z = 0;
-
- # get ros param value
- robot_name = rospy.get_param("/name");
-
-
-
-
- # subscriber
- #rospy.Subscriber("motor_command", motorCommand, self.motorVelocityCallback)
- # todo add grounp ns
- rospy.Subscriber("vel_cmd", Twist, self.velocityCommandCallback)
-
-
- time.sleep(1) # Wait for connection to work
- rospy.loginfo("Starting controller: " + robot_name)
-
- # main loop
- while not rospy.is_shutdown():
-
-
- if self.vel_cmd_arrived == True:
- # convert velocity command to motor command
- self.motor_command[0] = self.vel_cmd.linear.x
- self.motor_command[1] = self.vel_cmd.linear.y
- self.motor_command[2] = self.vel_cmd.angular.z
- self.vel_cmd_arrived = False
-
-
- else:
+ def __init__(self):
+ # Load the servos
+ self.servos = self.load_servos()
+ self.motor_command = [0,0,0];
+ self.send_command = False;
+ self.vel_cmd_arrived = False;
+ self.motor_command[0] = 0;
+ self.motor_command[1] = 0;
+ self.motor_command[2] = 0;
+ self.vel_cmd = Twist()
+ self.vel_cmd.linear.x = 0;
+ self.vel_cmd.linear.y = 0;
+ self.vel_cmd.angular.z = 0;
+ self._current_vel = Twist()
+ self._current_pose = Pose()
+ self.vel_callback_initiated = False;
+ self.pose_callback_initiated = False;
+ self.R = 0.165 # Distance between center of robot and wheels
+ self.a1 = 2*np.pi/3 # Angle between x axis and first wheel
+ self.a2 = 4*np.pi/3 # Angle between x axis and second wheel
+ self.r = 0.027 # Wheel radius. Has been fudge-factored because the actual velocity of the wheels did not align with the set-points.
+
+
+ # get ros param value
+ robot_name = rospy.get_param("/name");
+
+
+
+
+ # subscriber
+ #rospy.Subscriber("motor_command", motorCommand, self.motorVelocityCallback)
+ # Velocity command node
+ rospy.Subscriber("vel_cmd", Twist, self.velocityCommandCallback)
+ # pose and velocity values
+ ss = rospy.get_param("/name")
+ ss_pose = "/" + ss+"/pose";
+ ss_vel= "/" + ss+"/vel";
+ rospy.Subscriber(ss_pose, Pose, self.poseCallback)
+ rospy.Subscriber(ss_vel, Twist, self.velCallback)
+
+
+
+
+
+ time.sleep(1) # Wait for connection to work
+ rospy.loginfo("Starting controller: " + robot_name)
self.motor_command[0] = 0
self.motor_command[1] = 0
self.motor_command[2] = 0
-
- # Send command to the motors
- for (j,s) in enumerate(self.servos):
- s.goal_velocity.write(round(self.motor_command[j]))
-
+ print("Starting main loop")
+
+ # main loop
+ while not rospy.is_shutdown():
+
+
+ if self.vel_cmd_arrived == True and self.vel_callback_initiated == True and self.pose_callback_initiated==True:
+ # compute motor values from velocity command
+ roll, pitch,yaw = euler_from_quaternion(self._current_pose.orientation.x,self._current_pose.orientation.y,self._current_pose.orientation.z,self._current_pose.orientation.w)
+ xdot = np.array([1,0,0]);#self._current_vel.linear.x, self._current_vel.linear.y, self._current_vel.angular.z])
+ ph = self.vel2motor(xdot, 0);
+ #print(ph)
+ print(yaw)
+ self.motor_command[0] = ph[0]
+ self.motor_command[1] = ph[1]
+ self.motor_command[2] = ph[2]
+ self.vel_cmd_arrived = False
+
+ else:
+ self.motor_command[0] = 0
+ self.motor_command[1] = 0
+ self.motor_command[2] = 0
+
+ # Send command to the motors
+ for (j,s) in enumerate(self.servos):
+ s.goal_velocity.write(round(self.motor_command[j]))
- def load_servos(self):
- """
- Servo load function, based on the dynamixel implementation of Anders Blomdell.
- """
- # The speed and channel numbers can be found and checked via the dynamixel software "dynamixel wizard".
- # The device can be found by e.g. lsusb, and is currently adapted for the three-color robots.
-
- channel = dynamixel.channel.Channel(speed=57600,device='/dev/ttyACM0')
- servos = [ XM430_W210_T_R(channel, 1),
- XM430_W210_T_R(channel, 2),
- XM430_W210_T_R(channel, 3) ]
- for s in servos:
- s.torque_enable.write(0)
- print(s.model_number.read(), s.id.read())
- s.operating_mode.write(1)
- s.bus_watchdog.write(0) # Clear old watchdog error
- s.bus_watchdog.write(100) # 2 second timeout
- s.torque_enable.write(1)
-
- return servos
-
- def motorVelocityCallback(self, data):
- self.send_command = True
- self.motor_command[0] = data.data[0];
- self.motor_command[1] = data.data[1];
- self.motor_command[2] = data.data[2];
- def velocityCommandCallback(self,data):
- self.vel_cmd_arrived = True
- self.vel_cmd = data
-
-
+
+ def load_servos(self):
+ """
+ Servo load function, based on the dynamixel implementation of Anders Blomdell.
+ """
+ # The speed and channel numbers can be found and checked via the dynamixel software "dynamixel wizard".
+ # The device can be found by e.g. lsusb, and is currently adapted for the three-color robots.
+
+ channel = dynamixel.channel.Channel(speed=57600,device='/dev/ttyACM0')
+ servos = [ XM430_W210_T_R(channel, 1),
+ XM430_W210_T_R(channel, 2),
+ XM430_W210_T_R(channel, 3) ]
+ for s in servos:
+ s.torque_enable.write(0)
+ print(s.model_number.read(), s.id.read())
+ s.operating_mode.write(1)
+ s.bus_watchdog.write(0) # Clear old watchdog error
+ s.bus_watchdog.write(100) # 2 second timeout
+ s.torque_enable.write(1)
+
+ return servos
+
+ def motorVelocityCallback(self, data):
+ self.send_command = True
+ self.motor_command[0] = data.data[0];
+ self.motor_command[1] = data.data[1];
+ self.motor_command[2] = data.data[2];
+ def velocityCommandCallback(self,data):
+ self.vel_cmd_arrived = True
+ self.vel_cmd = data
+
+ def velCallback(self, data):
+ self._current_vel = data;
+ self.vel_callback_initiated = True;
+
+ def poseCallback(self, data):
+ self._current_pose = data;
+ self.pose_callback_initiated = True;
+ def vel2motor(self,xdot, ang):
+ M = -1/self.r*np.array([[-np.sin(ang), np.cos(ang), self.R ],[-np.sin(ang+self.a1), np.cos(ang+self.a1), self.R],[-np.sin(ang+self.a2), np.cos(ang+self.a2), self.R]])
+ return M.dot(xdot)
+ #return M.dot(xdot)
if __name__ == '__main__':
- rospy.init_node("omni_wheel_controller_node")
- try:
- omniwheel1 = omniWheelvelocityController()
- except rospy.ROSInterruptException:
- pass
+ rospy.init_node("omni_wheel_controller_node")
+ try:
+ omniwheel1 = omniWheelvelocityController()
+ except rospy.ROSInterruptException:
+ pass
+
+
--
GitLab