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
import pickle
import matplotlib.pyplot as plt
from operator import itemgetter
from pykalman import KalmanFilter
from imutils.video import VideoStream
#PARAMS
MARKER_SIZE = 200 #size in mm
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
# WIDTH = 640
......@@ -133,6 +139,14 @@ cfcamera_pub = rospy.Publisher(node_name, CameraData, queue_size=10)
# 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
fontface = cv2.FONT_HERSHEY_SIMPLEX
fontscale = 0.5
......@@ -142,13 +156,13 @@ framecount = 0
data_out = np.array([])
#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]],
observation_matrices = np.eye(6),
#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),
#initial_state_mean = [3.14,0,0,0,0,1500],
#initial_state_covariance = [0,0,0,0,0,0],
#observation_covariance=1,
#transition_covariance=.05
)
#)
#numpy arrays as data storage for the kalman filter
data = np.array([[0,0,0,0,0,0]])
......@@ -215,7 +229,18 @@ while(cam.isOpened()):
outputt = 't: '+str(tvec[0])
cv2.putText(QueryImg, outputr, (0,20), 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:
print('Wrong Marker detected at Frame: ' + str(framecount))
......@@ -227,7 +252,7 @@ while(cam.isOpened()):
#kalman filter-----------------------------------------------------------------------------------------------------------
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
output = 'flt' + str(np.round(data_out, decimals = 2))
#cv2.putText(QueryImg, output, (0,50), fontface, fontscale, fontcolor)
......@@ -241,7 +266,12 @@ while(cam.isOpened()):
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;
// data" array is expected to match the name in
// "m_context", (negative numbers indicate unknown)
int m_poseDataIndex = -1;
int m_poseDataMothershipIndex = -1;
// > Boolen indicating if the Mocap data is availble
bool m_isAvailable_mocap_data = false;
// > Boolen indicating if the object is "long term" occuled
......
......@@ -120,7 +120,7 @@ integratorGain_forTauYaw : 0.05
# 2 - Point Mass Per Dimension Method
# Uses a 2nd order random walk estimator independently for
# each of (x,y,z,roll,pitch,yaw)
estimator_method : 1
estimator_method : 2
# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
# > 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)
// Initilise a variable for the pose data of this agent
CrazyflieData poseDataForThisAgent;
CrazyflieData poseDataMothership;
// Extract the pose data from the full motion capture array
// NOTE: that if the return index is a negative then this
// indicates that the pose data was not found.
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
// > Update the flag
......@@ -173,8 +174,7 @@ void viconCallback(const ViconData& viconData)
// Fill in the pose data for this agent
controllerCall.request.ownCrazyflie = poseDataForThisAgent;
controllerCall.request.otherCrazyflies.push_back(poseDataMothership);
// PERFORM THE SAFTY CHECK (IF NOT THE DEFAULT CONTROLLER)
......
......@@ -184,22 +184,40 @@ float ydiff;
float zdiff;
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
// computation here, or you may find it convienient to create functions
// 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
float stateErrorInertial[9];
// Fill in the (x,y,z) position error
stateErrorInertial[0] = request.ownCrazyflie.x - m_setpoint[0] + xdiff/1000;
stateErrorInertial[1] = request.ownCrazyflie.y - m_setpoint[1] + ydiff/1000;
stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2] + zdiff/1000;
stateErrorInertial[0] = request.ownCrazyflie.x - m_setpoint[0];
stateErrorInertial[1] = request.ownCrazyflie.y - m_setpoint[1];
stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2];
// Compute an estimate of the velocity
// > As simply the derivative between the current and previous position
......@@ -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)
{
......@@ -805,6 +831,33 @@ void CrazyCameraCallback(const CameraData & msg)
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);
......@@ -812,7 +865,18 @@ void CrazyCameraCallback(const CameraData & msg)
zdiff=(z-dist)*sin(pitch)*sin(yaw)-y*cos(pitch)*sin(yaw)+x*cos(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;
......
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