To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 4de68738 authored by Lars's avatar Lars
Browse files

changed stuff

parent b46d8fc5
...@@ -76,13 +76,19 @@ import os ...@@ -76,13 +76,19 @@ import os
import pickle import pickle
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from operator import itemgetter from operator import itemgetter
from pykalman import KalmanFilter
from imutils.video import VideoStream from imutils.video import VideoStream
#PARAMS #PARAMS
MARKER_SIZE = 200 #size in mm MARKER_SIZE = 200 #size in mm
CAMERA = 1 CAMERA = 1
KALMAN = True
# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
# > For the (x,y,z) position
#PMKF_Ahat = np.matrix([[0.6723, 0.0034],[-12.9648, 0.9352]])
#PMKF_Kinf = np.arange([ 0.3277,12.9648])
#OLD_STUFF #OLD_STUFF
# WIDTH = 640 # WIDTH = 640
...@@ -133,6 +139,14 @@ cfcamera_pub = rospy.Publisher(node_name, CameraData, queue_size=10) ...@@ -133,6 +139,14 @@ cfcamera_pub = rospy.Publisher(node_name, CameraData, queue_size=10)
# return max_diff > abs(v1-v2) # return max_diff > abs(v1-v2)
outt0 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/t0.txt', 'w')
outt1 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/t1.txt', 'w')
outt2 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/t2.txt', 'w')
outr0 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/r0.txt', 'w')
outr1 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/r1.txt', 'w')
outr2 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/r2.txt', 'w')
#initialize font #initialize font
fontface = cv2.FONT_HERSHEY_SIMPLEX fontface = cv2.FONT_HERSHEY_SIMPLEX
fontscale = 0.5 fontscale = 0.5
...@@ -142,13 +156,13 @@ framecount = 0 ...@@ -142,13 +156,13 @@ framecount = 0
data_out = np.array([]) data_out = np.array([])
#define filter #define filter
kf = KalmanFilter(transition_matrices = [[1,0,0.033,0,0,0.0055],[0,1,0,0.033,0,0],[0,0,1,0,0.033,0],[0,0,0,1,0,0.033],[0,0,0,0,1,0],[0,0,0,0,0,1]], #kf = KalmanFilter(transition_matrices = [[1,0,0.033,0,0,0.0055],[0,1,0,0.033,0,0],[0,0,1,0,0.033,0],[0,0,0,1,0,0.033],[0,0,0,0,1,0],[0,0,0,0,0,1]],
observation_matrices = np.eye(6), #observation_matrices = np.eye(6),
#initial_state_mean = [3.14,0,0,0,0,1500], #initial_state_mean = [3.14,0,0,0,0,1500],
#initial_state_covariance = [0,0,0,0,0,0], #initial_state_covariance = [0,0,0,0,0,0],
#observation_covariance=1, #observation_covariance=1,
#transition_covariance=.05 #transition_covariance=.05
) #)
#numpy arrays as data storage for the kalman filter #numpy arrays as data storage for the kalman filter
data = np.array([[0,0,0,0,0,0]]) data = np.array([[0,0,0,0,0,0]])
...@@ -215,7 +229,18 @@ while(cam.isOpened()): ...@@ -215,7 +229,18 @@ while(cam.isOpened()):
outputt = 't: '+str(tvec[0]) outputt = 't: '+str(tvec[0])
cv2.putText(QueryImg, outputr, (0,20), fontface, fontscale, fontcolor) cv2.putText(QueryImg, outputr, (0,20), fontface, fontscale, fontcolor)
cv2.putText(QueryImg, outputt, (0,50), fontface, fontscale, fontcolor) cv2.putText(QueryImg, outputt, (0,50), fontface, fontscale, fontcolor)
outt0.write(str(tvec[0,0]))
outt1.write(str(tvec[0,1]))
outt2.write(str(tvec[0,2]))
outr0.write(str(rvec[0,0]))
outr1.write(str(rvec[0,1]))
outr2.write(str(rvec[0,2]))
outt0.write('\n')
outt1.write('\n')
outt2.write('\n')
outr0.write('\n')
outr1.write('\n')
outr2.write('\n')
else: else:
print('Wrong Marker detected at Frame: ' + str(framecount)) print('Wrong Marker detected at Frame: ' + str(framecount))
...@@ -227,7 +252,7 @@ while(cam.isOpened()): ...@@ -227,7 +252,7 @@ while(cam.isOpened()):
#kalman filter----------------------------------------------------------------------------------------------------------- #kalman filter-----------------------------------------------------------------------------------------------------------
masked_data = np.ma.masked_array(data, mask) #mask missing measurements masked_data = np.ma.masked_array(data, mask) #mask missing measurements
filtered, covariance = (kf.filter_update(filtered, covariance, masked_data[0])) #filtered, covariance = (kf.filter_update(filtered, covariance, masked_data[0]))
data_out = filtered data_out = filtered
output = 'flt' + str(np.round(data_out, decimals = 2)) output = 'flt' + str(np.round(data_out, decimals = 2))
#cv2.putText(QueryImg, output, (0,50), fontface, fontscale, fontcolor) #cv2.putText(QueryImg, output, (0,50), fontface, fontscale, fontcolor)
...@@ -241,7 +266,12 @@ while(cam.isOpened()): ...@@ -241,7 +266,12 @@ while(cam.isOpened()):
cv2.destroyAllWindows() cv2.destroyAllWindows()
outt0.close()
outt1.close()
outt2.close()
outr0.close()
outr1.close()
outr2.close()
......
...@@ -129,6 +129,7 @@ std::string m_namespace_to_coordinator_parameter_service; ...@@ -129,6 +129,7 @@ std::string m_namespace_to_coordinator_parameter_service;
// data" array is expected to match the name in // data" array is expected to match the name in
// "m_context", (negative numbers indicate unknown) // "m_context", (negative numbers indicate unknown)
int m_poseDataIndex = -1; int m_poseDataIndex = -1;
int m_poseDataMothershipIndex = -1;
// > Boolen indicating if the Mocap data is availble // > Boolen indicating if the Mocap data is availble
bool m_isAvailable_mocap_data = false; bool m_isAvailable_mocap_data = false;
// > Boolen indicating if the object is "long term" occuled // > Boolen indicating if the object is "long term" occuled
......
...@@ -120,7 +120,7 @@ integratorGain_forTauYaw : 0.05 ...@@ -120,7 +120,7 @@ integratorGain_forTauYaw : 0.05
# 2 - Point Mass Per Dimension Method # 2 - Point Mass Per Dimension Method
# Uses a 2nd order random walk estimator independently for # Uses a 2nd order random walk estimator independently for
# each of (x,y,z,roll,pitch,yaw) # each of (x,y,z,roll,pitch,yaw)
estimator_method : 1 estimator_method : 2
# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION # THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
# > For the (x,y,z) position # > For the (x,y,z) position
......
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
...@@ -87,12 +87,13 @@ void viconCallback(const ViconData& viconData) ...@@ -87,12 +87,13 @@ void viconCallback(const ViconData& viconData)
// Initilise a variable for the pose data of this agent // Initilise a variable for the pose data of this agent
CrazyflieData poseDataForThisAgent; CrazyflieData poseDataForThisAgent;
CrazyflieData poseDataMothership;
// Extract the pose data from the full motion capture array // Extract the pose data from the full motion capture array
// NOTE: that if the return index is a negative then this // NOTE: that if the return index is a negative then this
// indicates that the pose data was not found. // indicates that the pose data was not found.
m_poseDataIndex = getPoseDataForObjectNameWithExpectedIndex( viconData, m_context.crazyflieName , m_poseDataIndex , poseDataForThisAgent ); m_poseDataIndex = getPoseDataForObjectNameWithExpectedIndex( viconData, m_context.crazyflieName , m_poseDataIndex , poseDataForThisAgent );
m_poseDataMothershipIndex = getPoseDataForObjectNameWithExpectedIndex( viconData, "Mothership" , m_poseDataMothershipIndex , poseDataMothership );
// Detecting time-out of the motion capture data // Detecting time-out of the motion capture data
// > Update the flag // > Update the flag
...@@ -173,8 +174,7 @@ void viconCallback(const ViconData& viconData) ...@@ -173,8 +174,7 @@ void viconCallback(const ViconData& viconData)
// Fill in the pose data for this agent // Fill in the pose data for this agent
controllerCall.request.ownCrazyflie = poseDataForThisAgent; controllerCall.request.ownCrazyflie = poseDataForThisAgent;
controllerCall.request.otherCrazyflies.push_back(poseDataMothership);
// PERFORM THE SAFTY CHECK (IF NOT THE DEFAULT CONTROLLER) // PERFORM THE SAFTY CHECK (IF NOT THE DEFAULT CONTROLLER)
......
...@@ -184,22 +184,40 @@ float ydiff; ...@@ -184,22 +184,40 @@ float ydiff;
float zdiff; float zdiff;
float yawdiff; float yawdiff;
float actualPos[4] = {0,0,0,0};
float markerPos[6] = {0,0,0,0,0,0};
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) bool calculateControlOutput(Controller::Request &request, Controller::Response
{ &response) {
// This is the "start" of the outer loop controller, add all your control // This is the "start" of the outer loop controller, add all your control
// computation here, or you may find it convienient to create functions // computation here, or you may find it convienient to create functions
// to keep you code cleaner // to keep you code cleaner
//FILL IN ACTUAL POSITION
actualPos[0]=request.ownCrazyflie.x;
actualPos[1]=request.ownCrazyflie.y;
actualPos[2]=request.ownCrazyflie.z;
actualPos[3]=request.ownCrazyflie.yaw;
markerPos[0]=request.otherCrazyflies[0].x;
markerPos[0]=request.otherCrazyflies[0].y;
markerPos[0]=request.otherCrazyflies[0].z;
markerPos[0]=request.otherCrazyflies[0].yaw;
markerPos[0]=request.otherCrazyflies[0].pitch;
markerPos[0]=request.otherCrazyflies[0].roll;
ROS_INFO_STREAM("Marker Position: " << markerPos[0]);
// Define a local array to fill in with the state error // Define a local array to fill in with the state error
float stateErrorInertial[9]; float stateErrorInertial[9];
// Fill in the (x,y,z) position error // Fill in the (x,y,z) position error
stateErrorInertial[0] = request.ownCrazyflie.x - m_setpoint[0] + xdiff/1000; stateErrorInertial[0] = request.ownCrazyflie.x - m_setpoint[0];
stateErrorInertial[1] = request.ownCrazyflie.y - m_setpoint[1] + ydiff/1000; stateErrorInertial[1] = request.ownCrazyflie.y - m_setpoint[1];
stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2] + zdiff/1000; stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2];
// Compute an estimate of the velocity // Compute an estimate of the velocity
// > As simply the derivative between the current and previous position // > As simply the derivative between the current and previous position
...@@ -789,7 +807,15 @@ void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle) ...@@ -789,7 +807,15 @@ void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
// ---------------------------------------------------------------------------------- // ----------------------------------------------------------------------------------
float difflimiter(float diff, float max)
{
if (diff > max)
return max;
else if (diff < -max)
return -max;
else return diff;
}
void CrazyCameraCallback(const CameraData & msg) void CrazyCameraCallback(const CameraData & msg)
{ {
...@@ -805,6 +831,33 @@ void CrazyCameraCallback(const CameraData & msg) ...@@ -805,6 +831,33 @@ void CrazyCameraCallback(const CameraData & msg)
float dist=800; float dist=800;
//construct rotationmatrix from Vicon frame to camera frame
//define alpha, betha and gamma as yaw pitch and roll angle
float a=CrazyflieData.yaw;
float b=CrazyflieData.pitch;
float c=CrazyflieData.roll;
Float32 R_vicon_to_frame[3][3];
R_vicon_to_frame[0][0]=cos(b)*cos(a);
R_vicon_to_frame[0][1]=cos(b)*sin(a);
R_vicon_to_frame[0][2]=sin(b);
R_vicon_to_frame[1][0]=-cos(c)*sin(a)+sin(c)sin(b)cos(a);
R_vicon_to_frame[1][1]=cos(b)*cos(a)+sin(c)sin(b)sin(a);
R_vicon_to_frame[1][2]=sin(c)*cos(b);
R_vicon_to_frame[2][0]=sin(c)*sin(a)+cos(c)sin(b)cos(a);
R_vicon_to_frame[2][1]=-sin(c)*cos(a)+cos(c)sin(b)sin(a);
R_vicon_to_frame[2][2]=cos(c)*cos(b);
Float32 R_aruco_to_camera[3][3];
Float32 rvec[3]={pitch,roll,yaw};
R_aruco_to_camera= cv2.rodrigues(rvec);
xdiff=(z-dist)*sin(pitch)*cos(yaw)-y*cos(pitch)*cos(yaw)-x*sin(yaw); xdiff=(z-dist)*sin(pitch)*cos(yaw)-y*cos(pitch)*cos(yaw)-x*sin(yaw);
...@@ -812,9 +865,20 @@ void CrazyCameraCallback(const CameraData & msg) ...@@ -812,9 +865,20 @@ void CrazyCameraCallback(const CameraData & msg)
zdiff=(z-dist)*sin(pitch)*sin(yaw)-y*cos(pitch)*sin(yaw)+x*cos(yaw); zdiff=(z-dist)*sin(pitch)*sin(yaw)-y*cos(pitch)*sin(yaw)+x*cos(yaw);
yawdiff=yaw; yawdiff=yaw;
ROS_INFO_STREAM("CrazyCameraCallback received " << " "<< xdiff<< " " << " "<<" "<< ydiff <<" "<<zdiff <<" "<< yawdiff); xdiff = difflimiter(xdiff, 100);
ydiff = difflimiter(ydiff, 100);
zdiff = difflimiter(zdiff, 100);
ROS_INFO_STREAM("CrazyCameraCallback received " <<" " << x << " "<< xdiff << " "<< ydiff <<" "<<zdiff <<" "<< yawdiff);
//Set constant distant, WORKS FINE
//setNewSetpoint( actualPos[0]-ydiff*0.001, actualPos[1]-zdiff*0.001, actualPos[2]-xdiff*0.001, m_setpoint[3] );
//setNewSetpoint( m_setpoint[0], m_setpoint[1], actualPos[2]-xdiff*0.001, m_setpoint[3] );
setNewSetpoint( actualPos[0]-ydiff*0.001, m_setpoint[1], m_setpoint[2] , m_setpoint[3]);
//m_setpoint[0] += xdiff/1000; //m_setpoint[0] += xdiff/1000;
//m_setpoint[1] += ydiff/1000; //m_setpoint[1] += ydiff/1000;
//m_setpoint[2] += zdiff/1000; //m_setpoint[2] += zdiff/1000;
......
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment