Skip to content
Snippets Groups Projects
Commit 36dda2ab authored by Farid Alijani's avatar Farid Alijani
Browse files

a new AR added

parent 564c96df
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Files_4_thesis/docking_1.asv deleted 100644 → 0
+ 0
− 405
View file @ 564c96df
clc;
close all;
clear all;
%% Import data and time conversion
% data 1: 556 ros messages for geometry_msgs/Twist
% 557 ros messages for geometry_msgs/PoseStamped
% y-axis gains:
% (280716_2024) better!!!
% P = .44; I = 0; D = 0;
% theta-axis gains:
% P = .08; I = 0; D = 0;
% y-axis gains:
% (280716_2002)
% P = .46; I = 0; D = 0;
% theta-axis gains:
% P = .08; I = 0; D = 0;
Pose_Matrix_data_1 = csvread('Pose28_7_16_2024.txt',1,0);
Vel_Matrix_data_1 = csvread('Velocity28_7_16_2024.txt',1,0);
% time in position
TimeP_ros_data_1 = Pose_Matrix_data_1(:,1); % ros time, needs to be converted to sec...
% duriation = end_time - start_time
durationP_data_1 = (TimeP_ros_data_1(size(TimeP_ros_data_1,1),:) - TimeP_ros_data_1(1,:))*10^(-9);
t_P_sec_data_1 = 0:durationP_data_1/size(TimeP_ros_data_1,1):durationP_data_1;
t_P_sec_data_1(:,size(t_P_sec_data_1,2)) = [];
% time in vel
TimeV_ros_data_1 = Vel_Matrix_data_1(:,1); % ros time, needs to be converted to sec...
% duriation = end_time - start_time
durationV_data_1 = (TimeV_ros_data_1(size(TimeV_ros_data_1,1),:) - TimeV_ros_data_1(1,:))*10^(-9);
t_V_sec_data_1 = 0:durationV_data_1/size(TimeV_ros_data_1,1):durationV_data_1;
t_V_sec_data_1(:,size(t_V_sec_data_1,2)) = [];
% -----------------------------------------------------------------------------------------------------
% data 2: 588 ros messages for geometry_msgs/Twist
% 589 ros messages for geometry_msgs/PoseStamped
% y-axis gains:
% P = .5; I = 0.001; D = 0.03;
% theta-axis gains:
% P = .28; I = 0.1; D = 0.15;
Pose_Matrix_data_2 = csvread('Pose18_7_16_1815.txt',1,0);
Vel_Matrix_data_2 = csvread('Velocity18_7_16_1815.txt',1,0);
% time in position
TimeP_ros_data_2 = Pose_Matrix_data_2(:,1); % ros time, needs to be converted to sec...
% duriation = end_time - start_time
durationP_data_2 = (TimeP_ros_data_2(size(TimeP_ros_data_2,1),:) - TimeP_ros_data_2(1,:))*10^(-9);
t_P_sec_data_2 = 0:durationP_data_2/size(TimeP_ros_data_2,1):durationP_data_2;
t_P_sec_data_2(:,size(t_P_sec_data_2,2)) = [];
% time in vel
TimeV_ros_data_2 = Vel_Matrix_data_2(:,1); % ros time, needs to be converted to sec...
% duriation = end_time - start_time
durationV_data_2 = (TimeV_ros_data_2(size(TimeV_ros_data_2,1),:) - TimeV_ros_data_2(1,:))*10^(-9);
t_V_sec_data_2 = 0:durationV_data_2/size(TimeV_ros_data_2,1):durationV_data_2;
t_V_sec_data_2(:,size(t_V_sec_data_2,2)) = [];
% -----------------------------------------------------------------------------------------------------
% data 3: 628 ros messages for geometry_msgs/Twist
% 629 ros messages for geometry_msgs/PoseStamped
% y-axis gains:
% P = .44; I = 0; D = 0.07;
% theta-axis gains:
% P = .08; I = 0; D = 0;
Pose_Matrix_data_3 = csvread('Pose28_7_16_2052.txt',1,0);
Vel_Matrix_data_3 = csvread('Velocity18_7_16_2052.txt',1,0);
% time in position
TimeP_ros_data_3 = Pose_Matrix_data_3(:,1); % ros time, needs to be converted to sec...
% duriation = end_time - start_time
durationP_data_3 = (TimeP_ros_data_3(size(TimeP_ros_data_3,1),:) - TimeP_ros_data_3(1,:))*10^(-9);
t_P_sec_data_3 = 0:durationP_data_3/size(TimeP_ros_data_3,1):durationP_data_3;
t_P_sec_data_3(:,size(t_P_sec_data_3,2)) = [];
% time in vel
TimeV_ros_data_3 = Vel_Matrix_data_3(:,1); % ros time, needs to be converted to sec...
% duriation = end_time - start_time
durationV_data_3 = (TimeV_ros_data_3(size(TimeV_ros_data_3,1),:) - TimeV_ros_data_3(1,:))*10^(-9);
t_V_sec_data_3 = 0:durationV_data_3/size(TimeV_ros_data_3,1):durationV_data_3;
t_V_sec_data_3(:,size(t_V_sec_data_3,2)) = [];
%% Pose estimation
%data 1:
% when using marker pose ,,,,
Pose_X_data_1 = Pose_Matrix_data_1(:,4);
Pose_Y_data_1 = Pose_Matrix_data_1(:,3);
Theta_data_1 = Pose_Matrix_data_1(:,5);
% when using robot odometry ,,,,
% Pose_X = Pose_Matrix(:,2);
% Pose_Y = Pose_Matrix(:,3);
% Theta = Pose_Matrix(:,7);
% Extracting reference values when the robot is manually docked!
ref_X_data_1 = Pose_X_data_1(size(Pose_X_data_1,1));
ref_Y_data_1 = Pose_Y_data_1(size(Pose_Y_data_1,1));
ref_Theta_data_1 = Theta_data_1(size(Theta_data_1,1));
ref_Pose_data_1 =[ref_X_data_1;ref_Y_data_1];
%data 2:
% when using marker pose ,,,,
Pose_X_data_2 = Pose_Matrix_data_2(:,4);
Pose_Y_data_2 = Pose_Matrix_data_2(:,3);
Theta_data_2 = Pose_Matrix_data_2(:,5);
% when using robot odometry ,,,,
% Pose_X = Pose_Matrix(:,2);
% Pose_Y = Pose_Matrix(:,3);
% Theta = Pose_Matrix(:,7);
% Extracting reference values when the robot is manually docked!
ref_X_data_2 = Pose_X_data_2(size(Pose_X_data_2,1));
ref_Y_data_2 = Pose_Y_data_2(size(Pose_Y_data_2,1));
ref_Theta_data_2 = Theta_data_2(size(Theta_data_2,1));
ref_Pose_data_2 =[ref_X_data_2;ref_Y_data_2];
%data 3:
% when using marker pose ,,,,
Pose_X_data_3 = Pose_Matrix_data_3(:,4);
Pose_Y_data_3 = Pose_Matrix_data_3(:,3);
Theta_data_3 = Pose_Matrix_data_3(:,5);
% when using robot odometry ,,,,
% Pose_X = Pose_Matrix(:,2);
% Pose_Y = Pose_Matrix(:,3);
% Theta = Pose_Matrix(:,7);
% Extracting reference values when the robot is manually docked!
ref_X_data_3 = Pose_X_data_3(size(Pose_X_data_3,1));
ref_Y_data_3 = Pose_Y_data_3(size(Pose_Y_data_3,1));
ref_Theta_data_3 = Theta_data_3(size(Theta_data_3,1));
ref_Pose_data_3 =[ref_X_data_3;ref_Y_data_3];
thresh_X = .001;
theta = 0:.001:2*pi;
% needs to be adjusted manually if docking platform is replaced!
ref_x = .19979;
ref_y = .00903;
x_circle = thresh_X*cos(theta) + ref_x; % ref_X needs to be recorded
y_circle = thresh_X*sin(theta) + ref_y; % ref_Y needs to be recorded
%% Velocity estimation
% data 1:
Vel_X_data_1 = Vel_Matrix_data_1(:,2);
Vel_Y_data_1 = Vel_Matrix_data_1(:,3);
Omega_Z_data_1 = Vel_Matrix_data_1(:,7);
% data 2:
Vel_X_data_2 = Vel_Matrix_data_2(:,2);
Vel_Y_data_2 = Vel_Matrix_data_2(:,3);
Omega_Z_data_2 = Vel_Matrix_data_2(:,7);
% data 3:
Vel_X_data_3 = Vel_Matrix_data_3(:,2);
Vel_Y_data_3 = Vel_Matrix_data_3(:,3);
Omega_Z_data_3 = Vel_Matrix_data_3(:,7);
%% Plots
% for the presenation all pose estimation(x,y,theta) in one plot
% figure;
% plot(t_P_sec,Pose_X,'b',t_P_sec,Pose_Y,'r',t_P_sec,Theta,'g');
%
% title('Marker Pose');
% xlabel('Time [sec]');
% ylabel('Pose');
% legend('X [m]','Y [m]','\theta [rad]');
% grid on
% marker position
figure;
subplot(3,1,1);
plot(t_P_sec_data_1,Pose_X_data_1);
hold on
plot(t_P_sec_data_2,Pose_X_data_2,'r');
hold on
plot(t_P_sec_data_3,Pose_X_data_3,'g');
title('Marker X-axis');
% xlabel('Time [sec]');
ylabel('X [m]');
grid on
subplot(3,1,2);
plot(t_P_sec_data_1,Pose_Y_data_1);
hold on
plot(t_P_sec_data_2,Pose_Y_data_2,'r');
hold on
plot(t_P_sec_data_3,Pose_Y_data_3,'g');
title('Marker Y-axis');
% xlabel('Time [sec]');
ylabel('Y [m]');
grid on
subplot(3,1,3);
plot(t_P_sec_data_1,Theta_data_1);
hold on
plot(t_P_sec_data_2,Theta_data_2,'r');
hold on
plot(t_P_sec_data_3,Theta_data_3,'g');
title('Marker \theta-axis');
xlabel('Time [sec]');
ylabel('\theta [rad]');
grid on
% control signals
figure;
subplot(3,1,1);
plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',1.2);
hold on
plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.2);
hold on
plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.2);
title('Control Signal X-axis');
% xlabel('Time [sec]');
ylabel('V_x [m/s]');
grid on
% axis([0 46 -.01 .11]);
subplot(3,1,2);
plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',1.2);
hold on
plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.2);
hold on
plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.2);
title('Control Signal Y-axis');
% xlabel('Time [sec]');
ylabel('V_y [m/s]');
grid on
% axis([0 46 -.2 .1]);
subplot(3,1,3);
plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',1.2);
hold on
plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.2);
hold on
plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.2);
title('Control Signal \theta-axis');
xlabel('Time [sec]');
ylabel('\omega [rad/s]');
grid on
% axis([0 46 -.1 .15]);
% Trajectory
% figure;
% plot(Pose_Y_data_1,Pose_X_data_1,'b',ref_Y_data_1,ref_X_data_1,'k*');
% hold on
% plot(Pose_Y_data_2,Pose_X_data_2,'r',ref_Y_data_2,ref_X_data_2,'k*');
%
% hold on
% plot(Pose_Y_data_3,Pose_X_data_3,'g',ref_Y_data_3,ref_X_data_3,'k*');
%
%
% for k = Pose_X_data_1(1):Pose_X_data_1(size(Pose_X_data_1,1))
% x_arrow(k) =
%
% end
%
% x_arrow = linspace(Pose_X_data_1(1),Pose_X_data_1(size(Pose_X_data_1,1)),150);
% y_arrow = linspace(Pose_Y_data_1(1),Pose_Y_data_1(size(Pose_Y_data_1,1)),150);
%
% Vx = gradient(x_arrow,.01);
% Vy = gradient(y_arrow,.01);
%
% figure;
% plot(y_arrow,x_arrow,'b--','LineWidth',.5);
% % hold on
% % quiver(y_arrow,x_arrow,Vy,Vx,'r--','LineWidth',1.5);
%
figure;
subplot(1,3,1);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
hold on
plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4)
title('Docking Trajectroy (Whole area)');
xlabel('Y [m]');
ylabel('X [m]');
axis([-.3 .2 .18 1.3]);
% legend('Trajectory 1','Reference Position 1', 'Trajectory 2','Reference Position 2','Trajectory 3','Reference Position 3');
grid on
subplot(1,3,2);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
hold on
plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
title('Docking Trajectroy (SM zone)');
xlabel('Y [m]');
ylabel('X [m]');
% axis([-.08 0 .265 .355]);
grid on
subplot(1,3,3);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
hold on
plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
title('Docking Trajectroy (reference area)');
xlabel('Y [m]');
ylabel('X [m]');
% axis([-.04 -.03 .266 .269]);
grid on
figure;
subplot(2,1,1);
plot(diff(TimeP_ros_data_1)/1e9,'g')
hold on
plot (diff(t_P_sec_data_1),'k--')
title('Position sampling time');
ylabel('Sampling time');
legend('ROS calc.','Real time calc. [sec]','Orientation','horizontal');
% grid on
% axis([0 428 .05 .15]);
subplot(2,1,2);
plot(diff(TimeV_ros_data_1)/1e9,'g');
hold on
plot (diff(t_V_sec_data_1),'k--')
title('Velocity sampling time');
xlabel('samples');
ylabel('Sampling time');
% legend('ROS calc.','Ordinary calc.','Orientation','horizontal');
% grid on
% axis([0 428 -.005 .025]);
%
% subplot(3,1,3);
% plot(diff(TimeV_ros_data_1)/1e9,'g')
% hold on
% plot (diff(t_V_sec_data_1),'k--')
% xlabel('samples');
% ylabel('time step');
% axis([150 250 0 .02]);
figure;
subplot(3,1,1);
plot(diff(Pose_X_data_1));
title('X-Pose diff');
xlabel('samples');
ylabel('X difference [m]');
grid on
subplot(3,1,2);
plot(diff(Pose_X_data_1));
title('Y-Pose diff');
xlabel('samples');
ylabel('Y difference [m]');
grid on
subplot(3,1,3);
plot(diff(Theta_data_1));
title('\theta-Pose diff');
xlabel('samples');
ylabel('\theta difference [rad]');
grid on
\ No newline at end of file
MobileRobot/ej6.cpp 0 → 100644
+ 172
− 0
View file @ 36dda2ab
//*************************************************************************
// Example program using OpenCV library
//
// Luis M. Jimnez
//
// Course: Computer Vision (1782)
// Dpo. of Systems Engineering and Automation
// Automation, Robotics and Computer Vision Lab (ARVC)
// http://arvc.umh.es
// University Miguel Hernndez
//
// Description:
// - Loads camera calibration data from file (YAML type)
// - Capture images from a camera (decrease resolution)
// - Detects markers using ARUCO library
// - Calculates POSE from camera calibration file
// - Draws 3D cube on each maker over captured image
//
//*************************************************************************
#pragma warning( disable : 4290 ) // avoids warning 4290
#include <cassert> // debugging
#include <iostream> // c++ standar input/output
#include <sstream> // string to number conversion
#include <string> // handling strings
#include <vector> // handling vectors
#include <math.h> // math functions
#include <stdio.h> // standar C input/ouput
#include <opencv2/opencv.hpp> // OpenCV library headers
#include <opencv2/nonfree/nonfree.hpp> // Nonfree opencv library headers
// ARUCO Library (Markers)
#include <aruco/aruco.h>
//*************************************************************************
// C++ namespaces C++ (avoid using prefix for starndar and openCV classes)
//*************************************************************************
using namespace std;
using namespace cv;
//*************************************************************************
// Function prototypes
//*************************************************************************
//*************************************************************************
// Constants
//*************************************************************************
const char * WINDOW_CAMERA1 = "(W1) Camera 1 (Marker Detector)"; // windows id
//*************************************************************************
// Variables Globales
//*************************************************************************
int CAMERA_ID = 0; // default camera
//*************************************************************************
// Funciones
//*************************************************************************
int main (int argc, char * const argv[])
{
int key;
VideoCapture camera; // Cameras
Mat capture; // Images
Mat gray_image;
locale::global(locale("spanish")); // Character set (so you can use accents)
Size camSize(640, 480); // capture resolution
bool markerWasFound = false;
aruco::MarkerDetector MDetector; // handler for marker detector
vector<aruco::Marker> Markers; // storage for detected markers
float markerSize = (float)0.076; // size of ARUCO marker (meters)
aruco::BoardConfiguration boardInfo; // ARUCO board info
aruco::CameraParameters cameraParameters; //ARUCO class for camera parameters
Mat cameraMatrix, distCoeffs; // Intrinsic Camera Calibration parameters
// Load ARUCO board info
boardInfo.readFromFile("ARUCO_board.yaml");
// Load camera calibration data
FileStorage fs("camera.yaml", FileStorage::READ);
if (fs.isOpened())
{
fs["camera_matrix"] >> cameraMatrix;
fs["distortion_coefficients"] >> distCoeffs;
cout << "Camera Calibration Matrix A: " << endl << cameraMatrix << endl;
cout << "Distortion Coefs: " << distCoeffs << endl;
// configure internal ARUCO cameraParameters object
cameraParameters.setParams(cameraMatrix, distCoeffs, camSize);
}
else
{
cout << "you need a camera calibration file, sorry.\n";
getchar(); // wait for a keystroke and exits
return -1;
}
// check command line parameters (camera id)
if(argc>2 && string(argv[1])=="-c")
CAMERA_ID = atoi(argv[2]);
// Configure cameras
camera.open(CAMERA_ID); // open camera
if (!camera.isOpened())
{
cout << "you need to connect a camera, sorry.\n";
getchar(); // wait for a keystroke and exits
return -1;
}
//set capture properties (low resolution to speed up detection)
camera.set(CV_CAP_PROP_FRAME_WIDTH, camSize.width);
camera.set(CV_CAP_PROP_FRAME_HEIGHT, camSize.height);
// Create the visualization windows
namedWindow (WINDOW_CAMERA1, WINDOW_AUTOSIZE);
cout << "Capturing images.\n Hit q/Q to exit.\n";
// while there are images ...
while (camera.read(capture))
{
if(capture.empty())
continue; // capture has failed, continue
cvtColor( capture, gray_image, CV_BGR2GRAY ); // transforms to gray level
MDetector.detect(gray_image, Markers); // detects markers on image
markerWasFound = (Markers.size()>0); // checks if some marker was found
if(markerWasFound)
{
//for each marker calculates POSE and draw 3D cube
for (unsigned int i=0; i<Markers.size(); i++)
{
Markers[i].calculateExtrinsics(markerSize, cameraParameters, false);
cout << "Marker id:" << Markers[i].id << " Rvec: " << Markers[i].Rvec << " Tvec: " << Markers[i].Tvec << endl;
Markers[i].draw(capture, Scalar(0,0,255), 1);
aruco::CvDrawingUtils::draw3dCube(capture, Markers[i], cameraParameters);
}
// Draws coordinate axis for first marker
aruco::CvDrawingUtils::draw3dAxis(capture, Markers[0], cameraParameters);
}
imshow(WINDOW_CAMERA1, capture); // show image in a window
// wait 10ms for a keystroke to exit (image window must be on focus)
key = waitKey (10);
if (key == 'q' || key == 'Q')
break;
}
// free windows and camera resources
destroyAllWindows();
if (camera.isOpened()) camera.release();
// programm exits with no errors
return 0;
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment