PID files

MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~

@@ -92,18 +92,19 @@ double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::
 //const double PID4Docking::RefPose[4] = {-.0957, 0.00638817 /* Y_ref*/ ,  0.308857 /* X_ref*/ , 0.17 /* theta_ref*/}; 
 
 // ---- Ref. Values for Logitech Camera ---- //
-const double PID4Docking::RefPose[4] = {-.0957, -0.0310121 /* Y_ref*/ ,  0.219607 /* X_ref*/ , -0.618508 /* theta_ref*/}; 
+const double PID4Docking::RefPose[4] = {-.0957, 0.00903123 /* Y_ref*/ , 0.199791 /* X_ref*/ , -0.56 /* theta_ref*/}; 
 
 // ----------------  PID gains---------------- //
-double PID4Docking::Kp_y = .49; //.55
-double PID4Docking::Ki_y = 0 ;//.002
-double PID4Docking::Kd_y = 0; //.1
+double PID4Docking::Kp_y = .44; //.55
+double PID4Docking::Ki_y = .0005 ;//.002
+double PID4Docking::Kd_y = .15; //.1
 
-double PID4Docking::Kp_theta = .07;// .34 * Kp_y;  .//35 * Kp_y (u can't put that gain less than certain value since the left joint on the robot would hit the docking platform!)
+double PID4Docking::Kp_theta = .08;// .11
 double PID4Docking::Ki_theta = 0; //* Ki_y; // .15 * Ki_y
 double PID4Docking::Kd_theta = 0; //* Kd_y; // .0008
 // ----------------  PID gains---------------- //
 
+
 double PID4Docking::TT_S,PID4Docking::TT_E;
 // random pose initialized
 const double PID4Docking::y_up = .3; 
@@ -119,8 +120,8 @@ double PID4Docking::speed_reducer_Y = 1;
 double PID4Docking::speed_reducer_theta = 1;
 
 // ------ offsets X, Y, theta for Docking ---------
-double PID4Docking::X_dock_thresh = .001;
-double PID4Docking::y_dock_thresh = .0025;
+double PID4Docking::x_dock_thresh = .001;
+double PID4Docking::y_dock_thresh = .002; //.0015
 double PID4Docking::theta_dock_thresh = (CV_PI/180) * 1; // 1 deg.
 
 double PID4Docking::safety_margin_X = .15; // safety margin X axis in docking process : 18 cm
@@ -143,9 +144,9 @@ PID4Docking::PID4Docking()
 
     	// Publish pose message and buffer up to 100 messages
     	MarPose_pub = node_vis.advertise<geometry_msgs::PoseStamped>("/marker_pose", 100);
-    	commandPub = node_cont.advertise<geometry_msgs::Twist>("/base_controller/command",1);
+    	commandPub = node_cont.advertise<geometry_msgs::Twist>("/base_controller/command",100);
     
-    	MarPose_Sub = node_vis.subscribe("/marker_pose",1,&PID4Docking::camCB,this);
+    	MarPose_Sub = node_vis.subscribe("/marker_pose",100,&PID4Docking::camCB,this);
 }
 
   PID4Docking::~PID4Docking()
@@ -316,7 +317,8 @@ if (TheVideoCapturer.retrieve(TheInputImage))
 			
 		if (node_vis.ok() && found)
 		{
-		        y_t = -TheMarkers[0].Tvec.at<Vec3f>(0,0)[0];
+		        /*y_t = -TheMarkers[0].Tvec.at<Vec3f>(0,0)[0]; // changed !!! */ 
+			y_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[0];
 			x_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[1];
 			z_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[2];
 		
@@ -446,7 +448,8 @@ camPose[3] = CamFB->pose.orientation.x; //  theta orientation
         
 	
 	ROS_INFO_STREAM("--------- PID gains in trial no. " << docking_counter << " : ---------\n");
-	ROS_INFO_STREAM(" Kp = " << Kp_y << " ,  Ki = " << Ki_y << " , Kd = " << Kd_y << "\n");
+	ROS_INFO_STREAM(" Kp_y = " << Kp_y << " ,  Ki_y = " << Ki_y << " , Kd_y = " << Kd_y << "\n");        
+	ROS_INFO_STREAM(" Kp_theta = " << Kp_theta << " ,  Ki_theta = " << Ki_theta << " , Kd_theta = " << Kd_theta << "\n");
 
 	ROS_INFO_STREAM(" --------------------- Pose estimation ------------------ \n");
 	ROS_INFO_STREAM(" X_mar = " << camPose[2] << " vs. X_ref = " << RefPose[2] << " \n");
@@ -460,7 +463,7 @@ camPose[3] = CamFB->pose.orientation.x; //  theta orientation
 	{
 		ROS_INFO_STREAM("---------- MOVING TOWARDS DOCKING PLATFORM ---------  \n ");
 		if (
-            		(abs(RefPose[2] - camPose[2]) <= X_dock_thresh) //&& // Z
+            		(abs(RefPose[2] - camPose[2]) <= x_dock_thresh) //&& // Z
             		//(abs(RefPose[1] - camPose[1]) <= y_dock_thresh) && // Y
             		//(abs(RefPose[3] - camPose[3]) <= theta_dock_thresh) // Yaw
             	)
@@ -505,7 +508,7 @@ camPose[3] = CamFB->pose.orientation.x; //  theta orientation
 	} else
 	{
   		ROS_INFO("---------- MOVING TOWARDS RANDOM POSE ---------\n");		
-		RandomPose(x_new,y_new,theta_new);
+		Undocking(x_new,y_new,theta_new);
 	}
 }
 
@@ -514,7 +517,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
 	ROS_INFO_STREAM("--------------------- Controller started ----------------------\n "); 
         
 	// -----------------X--------------------- //        
-	if(abs(RefX - MarPoseX) > X_dock_thresh)
+	if(abs(RefX - MarPoseX) > x_dock_thresh)
 	{			
 		/*// e(t) = setpoint - actual value;
         	curr_errorX = RefX - MarPoseX;
@@ -533,7 +536,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
 		control_signalX = speed_reducer_X * 0.1;        
         } else
 	{
-		control_signalX = 5e-5;	
+		control_signalX = 0;	// 5e-5
 	}
         // -----------------Y--------------------- // 
 	 
@@ -560,11 +563,22 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
         	d_termY = Kd_y * diffY;
 
 		ROS_INFO_STREAM("pY = " << p_termY << ", iY = " << i_termY << " dY = " << d_termY<< " \n");	      
-        	// control signal
+        	
         	control_signalY = p_termY + i_termY + d_termY;
         	
-		// robot & marker coordinates conversion
+		
+		//control_signalY = speed_reducer_Y * control_signalY;
+
+		/* -- MARKER IN CAMERA COORDINATATE FRAME */
+		if(MarPoseY < 0)
+		{
 			control_signalY = - speed_reducer_Y * control_signalY;
+			ROS_INFO("marker pose < 0 => robot is going to the RIGHT \n");
+		}else
+		{
+			control_signalY = speed_reducer_Y * control_signalY;
+			ROS_INFO("marker pose > 0 => robot is going to the LEFT \n");
+		}
 
         	prev_errorY = curr_errorY;
 	
@@ -590,7 +604,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
        		i_termYAW = Ki_theta * int_errorYAW;
        		d_termYAW = Kd_theta * diffYAW;
         	
-		ROS_INFO_STREAM("p_theta = " << p_termYAW << ", i_theta = " << i_termYAW << " d_theta = " << d_termYAW << " \n");	      
+		//ROS_INFO_STREAM("p_theta = " << p_termYAW << ", i_theta = " << i_termYAW << " d_theta = " << d_termYAW << " \n");	      
 		// control signal
         	control_signalYAW = p_termYAW + i_termYAW + d_termYAW;
         	
@@ -606,13 +620,20 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
 	
 		} else if (MarPoseYAW < 0 && abs(RefYAW) >= abs(MarPoseYAW))
 		{
-			ROS_INFO("abs(RefYAW) >= abs(orientation) && orientation < 0 => CW rotation  \n"); // correct
-			control_signalYAW = - speed_reducer_theta * control_signalYAW;
+			/*ROS_INFO("abs(RefYAW) >= abs(orientation) && orientation < 0 => CW rotation  \n"); // correct (changed)
+			control_signalYAW = - speed_reducer_theta * control_signalYAW;*/
+
+			ROS_INFO("abs(RefYAW) >= abs(orientation) && orientation < 0 => CCW rotation  \n");
+			control_signalYAW = speed_reducer_theta * control_signalYAW;
 		
 		} else if (MarPoseYAW < 0 && abs(RefYAW) < abs(MarPoseYAW))
 		{
-			ROS_INFO("abs(RefYAW) < abs(orientation) && orientation < 0 => CCW rotation \n"); // correct ?
-			control_signalYAW = - speed_reducer_theta * control_signalYAW;
+			/*ROS_INFO("abs(RefYAW) < abs(orientation) && orientation < 0 => CCW rotation \n"); // correct ? (changed)
+			control_signalYAW = - speed_reducer_theta * control_signalYAW;*/
+
+			ROS_INFO("abs(RefYAW) < abs(orientation) && orientation < 0 => CW rotation \n");
+			control_signalYAW =  speed_reducer_theta * control_signalYAW;
+			
 		} else 
 		{
 			ROS_INFO("New Condition should be added! \n");
@@ -627,11 +648,13 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
 		control_signalYAW = 0;	
 	}
 
-        //ROS_INFO_STREAM("Control signalX = " << control_signalX <<" . \n");
-	//ROS_INFO_STREAM("Control signalY = " << control_signalY << ". \n");
-	//ROS_INFO_STREAM("Control signalYAW = "<< control_signalYAW <<". \n");
-	ROS_INFO_STREAM(" ---------------------- Controller ended ----------------------- \n");	
+        /* ---   
+	ROS_INFO_STREAM("Control signalX = " << control_signalX <<"\n");
+	ROS_INFO_STREAM("Control signalY = " << control_signalY << "\n");
+	ROS_INFO_STREAM("Control signalYAW = "<< control_signalYAW <<"\n");
+	*/
 
+	ROS_INFO_STREAM(" ---------------------- Controller ended ----------------------- \n");	
 	dock(control_signalX, control_signalY, control_signalYAW);
 }
 
@@ -696,7 +719,7 @@ void PID4Docking::GenerateRandomVal()
 	theta_new = ((double) rand() / (RAND_MAX)) * (theta_up - theta_dwn) + theta_dwn; // will be used for Q_Learning
 }
 
-void PID4Docking::RandomPose(double X_rand, double Y_rand, double theta_rand)
+void PID4Docking::Undocking(double X_rand, double Y_rand, double theta_rand)
 {
 	ROS_INFO_STREAM(" Xr = " << X_rand << ", Yr = " << Y_rand << ", Thetar = " << theta_rand << " rad ~ " << theta_rand * (180/CV_PI) << " deg\n");
 	ROS_INFO_STREAM(" -------------------------------------------------------------- \n");

MobileRobot/ej6.cpp

@@ -78,7 +78,7 @@ int main (int argc, char * const argv[])
 	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)
+	float markerSize = (float)0.08;	// size of ARUCO marker (meters)
 	aruco::BoardConfiguration boardInfo;	// ARUCO board info
 	aruco::CameraParameters cameraParameters; //ARUCO class for camera parameters
 

MobileRobot/ej6.cpp~

+//*************************************************************************
+// 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;
+}