Commit b6de0f5a authored by Farid Alijani's avatar Farid Alijani
Browse files
parents 5e36e068 32686fad
......@@ -380,19 +380,24 @@ void PID4Docking::ProgStart(int argc,char** argv)
} else
{
// yaw has an offset of 180 deg.
if (yaw > 0)
{
theta_with_offset = yaw - y_off;
} else
{
theta_with_offset = yaw + y_off;
}
x_robINmar_coor = z_t * cos(yaw) - y_t * sin(yaw);
y_robINmar_coor = -z_t * sin(yaw) + y_t * cos(yaw);
// yaw has an offset of 180 deg.
if (yaw > 0)
{
msg_t.pose.orientation.z = yaw_prev;
theta_with_offset = yaw - y_off;
} else
{
yaw_prev = yaw;
msg_t.pose.orientation.z = yaw;
theta_with_offset = yaw + y_off;
}
x_robINmar_coor = z_t * cos(yaw) - y_t * sin(yaw);
y_robINmar_coor = -z_t * sin(yaw) + y_t * cos(yaw);
/*//if ((yaw - (CV_PI - RefPose[3])) > RefPose[3])
if (yaw > 0)
{
......@@ -511,13 +516,15 @@ void PID4Docking::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // su
ROS_INFO_STREAM(" --------------------- Pose estimation ------------------ \n");
ROS_INFO_STREAM(" theta = " << camPose[5] << " rad =~ " << camPose[5]*(180.0/CV_PI) << " deg\n");
ROS_INFO_STREAM(" theta_ref = " << RefPose[3] << " rad =~ " << (180/CV_PI) * RefPose[3] << " deg\n");
ROS_INFO_STREAM(" X_mar = " << camPose[2] << " vs. X_ref = " << RefPose[2] << " \n");
ROS_INFO_STREAM(" Y_mar = " << camPose[1] << " vs. Y_ref = " << RefPose[1] << " \n");
ROS_INFO_STREAM(" yaw = " << yaw << " rad =~ " << yaw*(180.0/CV_PI) << " deg \n");
ROS_INFO_STREAM(" yaw_init = " << yaw_1 << " rad =~ " << yaw_1*(180.0/CV_PI) << " deg \n");
ROS_INFO_STREAM(" X_mar = " << camPose[2] << " vs. X_ref = " << RefPose[2] << " \n");
ROS_INFO_STREAM(" Y_mar = " << camPose[1] << " vs. Y_ref = " << RefPose[1] << " \n");
ROS_INFO_STREAM(" y_t = " << y_t << " , x_t = " << z_t << "\n");
ROS_INFO_STREAM("------------------------------------------------------ \n ");
ROS_INFO_STREAM(" yaw_prev = " << yaw_prev << " rad =~ " << yaw_prev*(180.0/CV_PI) << " deg \n");
ROS_INFO_STREAM(" y_t = " << y_t << " , x_t = " << z_t << "\n");
ROS_INFO_STREAM(" ------------------------------------------------------\n");
if(Go2RandomPose == false)
{
......@@ -528,7 +535,7 @@ void PID4Docking::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // su
{
dock_finished = ros::Time::now().toSec();
docking_duration = dock_finished - dock_started;
ROS_INFO_STREAM("docking No. " << docking_counter << " is finished in "<< docking_duration <<" sec, moving to new Random Pose\n");
ROS_INFO_STREAM("docking No. " << docking_counter << " is finished in "<< docking_duration <<" sec\n");
keepMoving = false;
GenerateRandomVal();
docking_counter ++;
......@@ -595,7 +602,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
//control_signalX = speed_reducer_X * 0.1;
control_signalX = speed_reducer_X * 0.15;
//control_signalX = speed_reducer_X * 0.16;
//control_signalX = speed_reducer_X * 0.16;
} else
{
control_signalX = 0; // 5e-5
......
......@@ -380,19 +380,24 @@ void PID4Docking::ProgStart(int argc,char** argv)
} else
{
// yaw has an offset of 180 deg.
if (yaw > 0)
{
theta_with_offset = yaw - y_off;
} else
{
theta_with_offset = yaw + y_off;
}
x_robINmar_coor = z_t * cos(yaw) - y_t * sin(yaw);
y_robINmar_coor = -z_t * sin(yaw) + y_t * cos(yaw);
// yaw has an offset of 180 deg.
if (yaw > 0)
{
msg_t.pose.orientation.z = yaw_prev;
theta_with_offset = yaw - y_off;
} else
{
yaw_prev = yaw;
msg_t.pose.orientation.z = yaw;
theta_with_offset = yaw + y_off;
}
x_robINmar_coor = z_t * cos(yaw) - y_t * sin(yaw);
y_robINmar_coor = -z_t * sin(yaw) + y_t * cos(yaw);
/*//if ((yaw - (CV_PI - RefPose[3])) > RefPose[3])
if (yaw > 0)
{
......@@ -511,13 +516,15 @@ void PID4Docking::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // su
ROS_INFO_STREAM(" --------------------- Pose estimation ------------------ \n");
ROS_INFO_STREAM(" theta = " << camPose[5] << " rad =~ " << camPose[5]*(180.0/CV_PI) << " deg\n");
ROS_INFO_STREAM(" theta_ref = " << RefPose[3] << " rad =~ " << (180/CV_PI) * RefPose[3] << " deg\n");
ROS_INFO_STREAM(" X_mar = " << camPose[2] << " vs. X_ref = " << RefPose[2] << " \n");
ROS_INFO_STREAM(" Y_mar = " << camPose[1] << " vs. Y_ref = " << RefPose[1] << " \n");
ROS_INFO_STREAM(" yaw = " << yaw << " rad =~ " << yaw*(180.0/CV_PI) << " deg \n");
ROS_INFO_STREAM(" yaw_init = " << yaw_1 << " rad =~ " << yaw_1*(180.0/CV_PI) << " deg \n");
ROS_INFO_STREAM(" X_mar = " << camPose[2] << " vs. X_ref = " << RefPose[2] << " \n");
ROS_INFO_STREAM(" Y_mar = " << camPose[1] << " vs. Y_ref = " << RefPose[1] << " \n");
ROS_INFO_STREAM(" y_t = " << y_t << " , x_t = " << z_t << "\n");
ROS_INFO_STREAM("------------------------------------------------------ \n ");
ROS_INFO_STREAM(" yaw_prev = " << yaw_prev << " rad =~ " << yaw_prev*(180.0/CV_PI) << " deg \n");
ROS_INFO_STREAM(" y_t = " << y_t << " , x_t = " << z_t << "\n");
ROS_INFO_STREAM(" ------------------------------------------------------\n");
if(Go2RandomPose == false)
{
......@@ -528,7 +535,7 @@ void PID4Docking::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // su
{
dock_finished = ros::Time::now().toSec();
docking_duration = dock_finished - dock_started;
ROS_INFO_STREAM("docking No. " << docking_counter << " is finished in "<< docking_duration <<" sec, moving to new Random Pose\n");
ROS_INFO_STREAM("docking No. " << docking_counter << " is finished in "<< docking_duration <<" sec\n");
keepMoving = false;
GenerateRandomVal();
docking_counter ++;
......@@ -554,8 +561,8 @@ void PID4Docking::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // su
ROS_INFO("y & theta fixed successfully, MOVING STRAIGHT AHEAD ... \n");
//speed_reducer_X = .06; // for x_dot = .1 =>.06
//speed_reducer_X = .04; // for x_dot = .15 =>.04 ---- optimal docking time and Y-axis offset ----
speed_reducer_X = .0375; // for x_dot = .16 =>.0375
speed_reducer_X = .04; // for x_dot = .15 =>.04 ---- optimal docking time and Y-axis offset ----
//speed_reducer_X = .0375; // for x_dot = .16 =>.0375
Controller(RefPose[2], camPose[2], RefPose[1], RefPose[1], RefPose[3], RefPose[3],.1);
}
......@@ -594,8 +601,8 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
prev_errorX = curr_errorX;*/
//control_signalX = speed_reducer_X * 0.1;
//control_signalX = speed_reducer_X * 0.15;
control_signalX = speed_reducer_X * 0.16;
control_signalX = speed_reducer_X * 0.15;
//control_signalX = speed_reducer_X * 0.16;
} else
{
control_signalX = 0; // 5e-5
......
#include <iostream>
#include <fstream>
#include <sstream>
#include <array>
#include <math.h>
#include <unistd.h>
#include <cstdlib>
#include <stdio.h> /* printf, scanf, puts, NULL */
#include <stdlib.h> /* srand, rand */
#include <time.h> /* time */
#include <aruco/aruco.h>
#include <aruco/cvdrawingutils.h>
#include "ros/ros.h"
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf/tf.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include "opencv2/core/core_c.h"
#include "opencv2/core/core.hpp"
#include "opencv2/flann/miniflann.hpp"
#include "opencv2/imgproc/imgproc_c.h"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/video/video.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/ml/ml.hpp"
#include "opencv2/highgui/highgui_c.h"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/contrib/contrib.hpp"
#include<VisionControl.h>
using namespace cv;
using namespace aruco;
using namespace std;
float PID4Docking::TheMarkerSize = .1; // Default marker size
int PID4Docking::Thresh1_min = 75;
int PID4Docking::Thresh2_min = 10;
int PID4Docking::Thresh1_max = 300;
int PID4Docking::Thresh2_max = 300;
const string PID4Docking::trackbarWindowName = "Trackbars";
int PID4Docking::ThePyrDownLevel = 0;
bool PID4Docking::update_images = true;
bool PID4Docking::found = false;
bool PID4Docking::Go2RandomPose = false;
int PID4Docking::ThresParam1 = 0;
int PID4Docking::ThresParam2 = 0;
// ---- CONTROLL PARAMETERS ------ //
double PID4Docking::prev_errorX, PID4Docking::curr_errorX, PID4Docking::int_errorX, PID4Docking::diffX;
double PID4Docking::p_termX = 0;
double PID4Docking::i_termX = 0;
double PID4Docking::d_termX = 0;
double PID4Docking::prev_errorY, PID4Docking::curr_errorY, PID4Docking::int_errorY, PID4Docking::diffY;
double PID4Docking::p_termY = 0;
double PID4Docking::i_termY = 0;
double PID4Docking::d_termY = 0;
double PID4Docking::prev_errorYAW,PID4Docking::curr_errorYAW,PID4Docking::int_errorYAW,PID4Docking::diffYAW;
double PID4Docking::p_termYAW = 0;
double PID4Docking::i_termYAW = 0;
double PID4Docking::d_termYAW = 0;
double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::control_signalYAW;
// ---- CONTROLL PARAMETERS ------ //
// ---- Ref. Values for Android Camera ---- //
//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.0188 /* Y_ref*/ , -0.2025 /* X_ref*/ , -0.05 /* theta_ref*/};
const double PID4Docking::RefPose[4] = {-.0957, 0.0188 /* Y_ref*/ , 0.2025 /* X_ref*/ , 0 /* theta_ref*/};
// ---------------- PID gains---------------- //
double PID4Docking::Kp_y = .25; //.4
double PID4Docking::Ki_y = 0 ;// 0
double PID4Docking::Kd_y = 0; //.1
double PID4Docking::Kp_theta = .25;// .18
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;
const double PID4Docking::y_dwn = -.1;
const double PID4Docking::theta_dwn = -.7 /*-RefPose[3]*/;
const double PID4Docking::theta_up = .7 /*RefPose[3]*/;
double PID4Docking::x_new,PID4Docking::y_new,PID4Docking::theta_new;
double PID4Docking::dock_started,PID4Docking::dock_finished,PID4Docking::docking_duration;
double PID4Docking::speed_reducer_X = 1;
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 = .002; //.002
double PID4Docking::theta_dock_thresh = (CV_PI/180) * .5; // .5 deg.
double PID4Docking::safety_margin_X = .16; // safety margin X axis in docking process : 18 cm
// ------ offsets X, Y, theta for Undocking ---------
double PID4Docking::x_thresh_undock = .02;
double PID4Docking::y_thresh_undock = .015;
double PID4Docking::theta_thresh_undock = (CV_PI/180) * 3;
double PID4Docking::docking_counter = 1;
// ---- offsets for Roll, Pitch, Yaw ----//
float PID4Docking::p_off = CV_PI;
float PID4Docking::r_off = CV_PI/2;
float PID4Docking::y_off = CV_PI;
float PID4Docking::roll,PID4Docking::yaw,PID4Docking::pitch;
float PID4Docking::yaw_1,PID4Docking::yaw_prev;
float PID4Docking::theta_prev,PID4Docking::theta_init;
double PID4Docking::theta_with_offset;
double PID4Docking::x_robCen2cam = -.95/2; // x_cam from the center of robot
double PID4Docking::y_robCen2cam = 0; // y_cam from the center of robot
double PID4Docking::x_robINmar_coor,PID4Docking::y_robINmar_coor;
float PID4Docking::x_t, PID4Docking::y_t, PID4Docking::z_t;
PID4Docking::PID4Docking()
{
keepMoving = true;
/* initialize random seed: */
srand (time(NULL));
x_rand_SM = RefPose[2] + .55; // 55 cm spreading domain in the x-axis while moving towards the random pose
// Publish pose message and buffer up to 100 messages
MarPose_pub = node_vis.advertise<geometry_msgs::PoseStamped>("/marker_pose", 100);
Transpose_pub = node_vis.advertise<geometry_msgs::PoseStamped>("/transposed_axis", 100);
commandPub = node_cont.advertise<geometry_msgs::Twist>("/base_controller/command",100);
MarPose_Sub = node_vis.subscribe("/marker_pose",100,&PID4Docking::camCB,this);
}
PID4Docking::~PID4Docking()
{
}
Mat PID4Docking::getCurrentImage()
{
return img;
}
void PID4Docking::cvTackBarEvents(int value,void* ptr)
{
PID4Docking* ic = (PID4Docking*)(ptr);
ic-> myhandler(value);
}
void PID4Docking::myhandler(int value)
{
if (Thresh1_min<3) Thresh1_min=3;
if (Thresh1_min%2!=1) Thresh1_min++;
if (ThresParam2<1) ThresParam2=1;
ThresParam1 = Thresh1_min;
ThresParam2 = Thresh2_min;
MDetector.setThresholdParams(ThresParam1,ThresParam2);
// Recompute
MDetector.detect(TheInputImage,TheMarkers,TheCameraParameters);
// TheInputImageCopy is the output image for TheInputImage
TheInputImage.copyTo(TheInputImageCopy);
for (unsigned int i=0;i<TheMarkers.size();i++)
{
TheMarkers[i].draw(TheInputImageCopy,Scalar(205,0,0),1);
}
imshow("INPUT IMAGE",TheInputImageCopy);
//imshow("THRESHOLD IMAGE",MDetector.getThresholdedImage());
}
void PID4Docking::createTrackbars()
{
namedWindow(trackbarWindowName, 0);
createTrackbar("ThresParam 1", trackbarWindowName, &Thresh1_min, Thresh1_max, cvTackBarEvents, this);
createTrackbar("ThresParam 2", trackbarWindowName, &Thresh2_min, Thresh2_max, cvTackBarEvents, this);
}
void PID4Docking::imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
}
img = cv_ptr->image;
}
bool PID4Docking::readArguments ( int argc,char **argv )
{
if (argc<2) {
cerr<< "Invalid number of arguments!\n" <<endl;
cerr<< "Usage: (in.avi|live|copy) [intrinsics.yml] [marker's width/height]" <<endl;
return false;
}
TheInputVideo=argv[1];
if (argc>=3)
TheIntrinsicFile=argv[2];
if (argc>=4)
TheMarkerSize=atof(argv[3]);
if (argc==3)
cerr<< "NOTE: You need makersize to see 3d info!" <<endl;
return true;
}
void PID4Docking::ProgStart(int argc,char** argv)
{
// Show images, press "SPACE" to diable image
// rendering to save CPU time
if (readArguments(argc,argv)==false)
{
cerr<<"check the authenticity of your file again!"<<endl;
node_vis.shutdown();
}
createTrackbars();
// IP address for raw3_lund
const std::string vsa = "http://192.168.0.101:8080/video?x.mjpeg";
// -- publishing video stream with Android Camera--
//TheVideoCapturer.open(vsa);
TheVideoCapturer.open(0);
// Check video is open
if (!TheVideoCapturer.isOpened())
{
cerr<<"Could not open video!!"<<endl;
node_vis.shutdown();
}
dock_started = ros::Time::now().toSec();
// Read first image to get the dimensions
TheVideoCapturer>>TheInputImage;
// Read camera parameters if passed
if (TheIntrinsicFile!="") {
TheCameraParameters.readFromXMLFile(TheIntrinsicFile);
TheCameraParameters.resize(TheInputImage.size());
}
// Configure other parameters
if (ThePyrDownLevel>0)
{
MDetector.pyrDown(ThePyrDownLevel);
}
MDetector.setCornerRefinementMethod(MarkerDetector::LINES);
char key=0;
int index=0;
//ros::Rate rate(10);
// Capture until press ESC or until the end of the video
// Detection of markers in the image passed
MDetector.detect(TheInputImage,TheMarkers,TheCameraParameters,TheMarkerSize);
TheInputImage.copyTo(TheInputImageCopy);
Mat R_init(3,3,cv::DataType<float>::type);
Rodrigues(TheMarkers[0].Rvec,R_init);
yaw_1 = atan2(R_init.at<float>(2,1), R_init.at<float>(2,2)); // yaw_init
// theta_init with an offset of 180 deg.
if (yaw_1 > 0)
{
theta_init = yaw_1 - y_off;
} else
{
theta_init = yaw_1 + y_off;
}
while ((key != 'x') && (key!=27) && TheVideoCapturer.grab() && node_vis.ok() && keepMoving)
{
TT_S = ros::Time::now().toSec();
if (TheVideoCapturer.retrieve(TheInputImage))
{
// Detection of markers in the image passed
MDetector.detect(TheInputImage,TheMarkers,TheCameraParameters,TheMarkerSize);
TheInputImage.copyTo(TheInputImageCopy);
geometry_msgs::PoseStamped msg;
geometry_msgs::PoseStamped msg_t;
if (TheMarkers.size()>0)
{
found = true;
//ROS_INFO("MARKER FOUND!!! ... \n");
}else
{
found = false;
keepMoving = false;
ROS_INFO_STREAM("Marker is lost, successful docking trials : " << (docking_counter - 1) << "\n");
}
if (node_vis.ok() && found)
{
Mat R(3,3,cv::DataType<float>::type);
Rodrigues(TheMarkers[0].Rvec,R);
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];
roll = atan2(R.at<float>(1,0), R.at<float>(0,0));
pitch = atan2(-R.at<float>(2,0),pow((pow(R.at<float>(2,1),2)+pow(R.at<float>(2,2),2)),.5));
yaw = atan2(R.at<float>(2,1), R.at<float>(2,2)); // useful
// yaw has an offset of 180 deg.
if (yaw > 0)
{
theta_with_offset = yaw - y_off;
} else
{
theta_with_offset = yaw + y_off;
}
// just to publish the transpose values
// Publish Position
msg_t.pose.position.x = x_t;
msg_t.pose.position.y = y_t;
msg_t.pose.position.z = z_t;
// Publish Orientation
msg_t.pose.orientation.x = roll;
msg_t.pose.orientation.y = pitch;
msg_t.pose.orientation.z = yaw;
Transpose_pub.publish(msg_t);
if(theta_init > 0)
{
if (theta_with_offset > 0)
{
msg.pose.orientation.z = theta_with_offset;
theta_prev = theta_with_offset;
x_robINmar_coor = z_t * cos(theta_with_offset) - y_t * sin(theta_with_offset);
y_robINmar_coor = -z_t * sin(theta_with_offset) + y_t * cos(theta_with_offset);
} else
{
msg.pose.orientation.z = theta_prev;
x_robINmar_coor = z_t * cos(theta_prev) - y_t * sin(theta_prev);
y_robINmar_coor = -z_t * sin(theta_prev) + y_t * cos(theta_prev);
}
} else if (theta_init < 0)
{
if (theta_with_offset < 0)
{
msg.pose.orientation.z = theta_with_offset;
theta_prev = theta_with_offset;
x_robINmar_coor = z_t * cos(theta_with_offset) - y_t * sin(theta_with_offset);
y_robINmar_coor = -z_t * sin(theta_with_offset) + y_t * cos(theta_with_offset);
} else
{
msg.pose.orientation.z = theta_prev;
x_robINmar_coor = z_t * cos(theta_prev) - y_t * sin(theta_prev);
y_robINmar_coor = -z_t * sin(theta_prev) + y_t * cos(theta_prev);
}
} else
{
ROS_INFO("New Condition");
keepMoving = false;
}
/* // yaw has an offset of 180 deg.
if (yaw > 0)
{
theta_with_offset = yaw - y_off;
} else
{
theta_with_offset = yaw + y_off;
}
*/
/*x_robINmar_coor = z_t * cos(yaw) - y_t * sin(yaw);
y_robINmar_coor = -z_t * sin(yaw) + y_t * cos(yaw);*/