From 37ae486c376da8678417f306107366e580bd30a9 Mon Sep 17 00:00:00 2001
From: Farid Alijani <farid.alijani@student.lut.fi>
Date: Tue, 23 Feb 2016 23:32:47 +0100
Subject: [PATCH] y direction wrongggg
---
.../CamMark/camtomar/src/VisionControl.cpp | 55 ++--
.../CamMark/camtomar/src/VisionControl.cpp~ | 246 ++++++++++--------
2 files changed, 175 insertions(+), 126 deletions(-)
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index 38414113..99a12017 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -42,13 +42,13 @@ using namespace cv;
using namespace aruco;
using namespace std;
-double ImageConverter::Pos_P = .0000001;
-double ImageConverter::Pos_I = .02;
+double ImageConverter::Pos_P = .0001;
+double ImageConverter::Pos_I = 0.002;
double ImageConverter::Pos_D = 0;
-double ImageConverter::Ang_P = 0.05 * ImageConverter::Pos_P;
-double ImageConverter::Ang_I = 0.05 * ImageConverter::Pos_I;
-double ImageConverter::Ang_D = 0.05 * ImageConverter::Pos_D;
+double ImageConverter::Ang_P = 0.01 * ImageConverter::Pos_P;
+double ImageConverter::Ang_I = 0.01 * ImageConverter::Pos_I;
+double ImageConverter::Ang_D = 0.01 * ImageConverter::Pos_D;
float ImageConverter::TheMarkerSize = -1;
@@ -100,10 +100,9 @@ double ImageConverter::control_signalYAW;
double ImageConverter::zeroMax = .0000000000000000001;
double ImageConverter::zeroMin = -.0000000000000000001;
-
-double ImageConverter::Pz_eps = .01;
-double ImageConverter::Py_eps = .001;
-double ImageConverter::A_eps = .1;
+double ImageConverter::Pz_eps = .011;
+double ImageConverter::Py_eps = .0001;
+double ImageConverter::A_eps = .05;
ImageConverter::ImageConverter() :
@@ -216,7 +215,6 @@ void ImageConverter::ProgStart(int argc,char** argv)
cerr<<"check the authenticity of your file again!"<<endl;
nh_.shutdown();
}
-
createTrackbars();
// IP address for raw3_lund
@@ -255,7 +253,7 @@ void ImageConverter::ProgStart(int argc,char** argv)
char key=0;
int index=0;
// Capture until press ESC or until the end of the video
- while ((key != 'x') && (key!=27) && TheVideoCapturer.grab() && ros::ok()){
+ while ((key != 'x') && (key!=27) && TheVideoCapturer.grab() && ros::ok() && keepMoving){
ros::spinOnce();
@@ -281,7 +279,14 @@ void ImageConverter::ProgStart(int argc,char** argv)
}else
{
found = false;
- move2docking(-control_signalX, -control_signalY, control_signalYAW);
+
+ if(curr_errorYAW > RefPose[3])
+ {
+ move2docking(-control_signalX, -control_signalY, control_signalYAW); // CW (---|| \\)
+ } else
+ {
+ move2docking(-control_signalX, -control_signalY, -control_signalYAW); // CCW (// || ---)
+ }
}
if (ros::ok() && found)
@@ -423,9 +428,9 @@ void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB)
{
RefPose[0] = -.0957;
-RefPose[1] = .006;
-RefPose[2] = .35;
-RefPose[3] = -.68952;
+RefPose[1] = 0.00521064;
+RefPose[2] = 0.35;
+RefPose[3] = -0.679162;
camPose[0] = CamFB->pose.position.x;
camPose[1] = CamFB->pose.position.y;
@@ -462,9 +467,8 @@ camPose[3] = CamFB->pose.orientation.x;
)
{
- ROS_INFO("--------/*******//----- Dock is completed ! -----/*********---- \n ");
- //keepMoving = false;
- nh_.shutdown();
+ ROS_INFO("--------/*******//----- Dock is completed ! -----/*********---- \n ");
+ keepMoving = false;
}else
{
@@ -581,11 +585,18 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
control_signalYAW = 0;
}
- ROS_INFO_STREAM("Control signalX = " << control_signalX <<" . \n");
- ROS_INFO_STREAM("Control signaly = " << control_signalY << ". \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");
- dock(-control_signalX, -control_signalY, control_signalYAW);
+
+ if(curr_errorYAW > RefYAW)
+ {
+ dock(-control_signalX, -control_signalY, control_signalYAW); // CW (---|| \\)
+ } else
+ {
+ dock(-control_signalX, -control_signalY, -control_signalYAW); // CCW (// || ---)
+ }
}
void ImageConverter::dock(double VelX, double VelY, double omegaZ)
@@ -599,7 +610,7 @@ void ImageConverter::dock(double VelX, double VelY, double omegaZ)
commandPub.publish(msg);
- ROS_INFO_STREAM(" Current speed of robot: " << msg << ".\n");
+ ROS_INFO_STREAM(" Current speed of robot: \n" << msg << ".\n");
}
void ImageConverter::move2docking(double VelX_est, double VelY_est, double omegaZ_est)
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index 10a803ba..38414113 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -33,7 +33,6 @@
#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"
@@ -43,18 +42,18 @@ using namespace cv;
using namespace aruco;
using namespace std;
-double ImageConverter::Pos_P = .01;
-double ImageConverter::Pos_I = .5;
+double ImageConverter::Pos_P = .0000001;
+double ImageConverter::Pos_I = .02;
double ImageConverter::Pos_D = 0;
-double ImageConverter::Ang_P = .001;
-double ImageConverter::Ang_I = .05;
-double ImageConverter::Ang_D = 0;
+double ImageConverter::Ang_P = 0.05 * ImageConverter::Pos_P;
+double ImageConverter::Ang_I = 0.05 * ImageConverter::Pos_I;
+double ImageConverter::Ang_D = 0.05 * ImageConverter::Pos_D;
float ImageConverter::TheMarkerSize = -1;
-int ImageConverter::Thresh1_min = 5;
-int ImageConverter::Thresh2_min = 5;
+int ImageConverter::Thresh1_min = 20;
+int ImageConverter::Thresh2_min = 20;
int ImageConverter::Thresh1_max = 300;
int ImageConverter::Thresh2_max = 300;
@@ -102,8 +101,8 @@ double ImageConverter::control_signalYAW;
double ImageConverter::zeroMax = .0000000000000000001;
double ImageConverter::zeroMin = -.0000000000000000001;
-double ImageConverter::Pz_eps = .09;
-double ImageConverter::Py_eps = .00001;
+double ImageConverter::Pz_eps = .01;
+double ImageConverter::Py_eps = .001;
double ImageConverter::A_eps = .1;
@@ -224,9 +223,9 @@ void ImageConverter::ProgStart(int argc,char** argv)
const std::string vsa = "http://192.168.0.101:8080/video?x.mjpeg";
// -- publishing video stream with Android Camera--
- //TheVideoCapturer.open(vsa);
+ TheVideoCapturer.open(vsa);
- TheVideoCapturer.open(1);
+ //TheVideoCapturer.open(0);
// Check video is open
if (!TheVideoCapturer.isOpened())
@@ -373,7 +372,7 @@ void ImageConverter::ProgStart(int argc,char** argv)
ros::spinOnce();
// -------------------------Removed----------------------------- //
- }
+ }
// Print other rectangles that contains no valid markers
for (unsigned int i=0;i<MDetector.getCandidates().size();i++)
{
@@ -427,11 +426,15 @@ RefPose[0] = -.0957;
RefPose[1] = .006;
RefPose[2] = .35;
RefPose[3] = -.68952;
-
+
+camPose[0] = CamFB->pose.position.x;
+camPose[1] = CamFB->pose.position.y;
+camPose[2] = CamFB->pose.position.z;
+camPose[3] = CamFB->pose.orientation.x;
+
//ROS_INFO_STREAM(" Xmar = " << CamFB->pose.position.x << " m. \n");
//ROS_INFO_STREAM(" Xref = " << RefPose[0] << " m. \n");
-
// in Marker coordinate sys.
// z => X robot (thrust)
@@ -451,114 +454,137 @@ RefPose[3] = -.68952;
ROS_INFO_STREAM(" rollref = " << RefPose[3] << " rad. \n");
ROS_INFO_STREAM(" ------------------------------------------------------------- \n");
-
-
- if (
- (abs(RefPose[1] - CamFB->pose.position.y) <= Py_eps) && // Y
- (abs(RefPose[2] - CamFB->pose.position.z) <= Pz_eps) && // Z
- (abs(RefPose[3] - CamFB->pose.orientation.x) <= A_eps) // Yaw
- )
- {
+
+ if (
+ (abs(RefPose[1] - camPose[1]) <= Py_eps) && // Y
+ (abs(RefPose[2] - camPose[2]) <= Pz_eps) && // Z
+ (abs(RefPose[3] - camPose[3]) <= A_eps) // Yaw
+ )
+ {
- ROS_INFO("------------------------ Dock is completed ! ---------------------- \n ");
- keepMoving = false;
+ ROS_INFO("--------/*******//----- Dock is completed ! -----/*********---- \n ");
+ //keepMoving = false;
+ nh_.shutdown();
- } else
- {
- Controller(RefPose[2], CamFB->pose.position.z, RefPose[1], CamFB->pose.position.y, RefPose[3], CamFB->pose.orientation.x,.01);
- }
+ }else
+ {
+ Controller(RefPose[2], camPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.01);
+ }
}
void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt)
{
// -----------------X--------------------- //
- // e(t) = setpoint - actual value;
- curr_errorX = RefX - MarPoseX;
- // Integrated error
- int_errorX += curr_errorX * dt;
- /*
- // -- windup gaurd --
- if (int_error < )
- {}
- else if ()
- {}*/
+ if(abs(RefX - MarPoseX) > Pz_eps)
+ {
+ // e(t) = setpoint - actual value;
+ curr_errorX = RefX - MarPoseX;
- // differentiation
- diffX = ((curr_errorX - prev_errorX) / dt);
+ // Integrated error
+ int_errorX += curr_errorX * dt;
+ /*
+ // -- windup gaurd --
+ if (int_error < )
+ {}
+ else if ()
+ {}*/
- // scalling
- p_termX = Pos_P * curr_errorX;
- i_termX = Pos_I * int_errorX;
- d_termX = Pos_D * diffX;
+ // differentiation
+ diffX = ((curr_errorX - prev_errorX) / dt);
- // control signal
- control_signalX = p_termX + i_termX + d_termX;
+ // scalling
+ p_termX = Pos_P * curr_errorX;
+ i_termX = Pos_I * int_errorX;
+ d_termX = Pos_D * diffX;
+ // control signal
+ control_signalX = p_termX + i_termX + d_termX;
- // save the current error as the previous one
- // for the next iteration.
- prev_errorX = curr_errorX;
+ // save the current error as the previous one
+ // for the next iteration.
+ prev_errorX = curr_errorX;
+ } else
+ {
+ control_signalX = 0;
+ }
// -----------------Y--------------------- //
- // e(t) = setpoint - actual value;
- curr_errorY = RefY - MarPoseY;
-
- // Integrated error
- int_errorY += curr_errorY * dt;
- /*
- // -- windup gaurd --
- if (int_error < )
- {}
- else if ()
- {}*/
-
- // differentiation
- diffY = ((curr_errorY - prev_errorY) / dt);
-
- // scalling
- p_termY = Pos_P * curr_errorY;
- i_termY = Pos_I * int_errorY;
- d_termY = Pos_D * diffY;
-
- // control signal
- control_signalY = p_termY + i_termY + d_termY;
-
-
- // save the current error as the previous one
- // for the next iteration.
- prev_errorY = curr_errorY;
-
+
+ if(abs(RefY - MarPoseY) > Py_eps)
+ {
+ // e(t) = setpoint - actual value;
+ curr_errorY = RefY - MarPoseY;
+
+ // Integrated error
+ int_errorY += curr_errorY * dt;
+ /*
+ // -- windup gaurd --
+ if (int_error < )
+ {}
+ else if ()
+ {}*/
+
+ // differentiation
+ diffY = ((curr_errorY - prev_errorY) / dt);
+
+ // scalling
+ p_termY = Pos_P * curr_errorY;
+ i_termY = Pos_I * int_errorY;
+ d_termY = Pos_D * diffY;
+
+ // control signal
+ control_signalY = p_termY + i_termY + d_termY;
+
+
+ // save the current error as the previous one
+ // for the next iteration.
+ prev_errorY = curr_errorY;
+ } else
+ {
+ control_signalY = 0;
+ }
// -------------------YAW--------------------------//
- // e(t) = setpoint - actual value;
- curr_errorYAW = RefYAW - MarPoseYAW;
- // Integrated error
- int_errorYAW += curr_errorYAW * dt;
- /*
- // -- windup gaurd --
- if (int_error < )
- {}
- else if ()
- {}*/
+
+ if(abs(RefYAW - MarPoseYAW) > A_eps)
+ {
+ // e(t) = setpoint - actual value;
+ curr_errorYAW = RefYAW - MarPoseYAW;
- // differentiation
- diffYAW = ((curr_errorYAW - prev_errorYAW) / dt);
+ // Integrated error
+ int_errorYAW += curr_errorYAW * dt;
+ /*
+ // -- windup gaurd --
+ if (int_error < )
+ {}
+ else if ()
+ {}*/
- // scalling
- p_termYAW = Ang_P * curr_errorYAW;
- i_termYAW = Ang_I * int_errorYAW;
- d_termYAW = Ang_D * diffYAW;
+ // differentiation
+ diffYAW = ((curr_errorYAW - prev_errorYAW) / dt);
- // control signal
- control_signalYAW = p_termYAW + i_termYAW + d_termYAW;
+ // scalling
+ p_termYAW = Ang_P * curr_errorYAW;
+ i_termYAW = Ang_I * int_errorYAW;
+ d_termYAW = Ang_D * diffYAW;
+ // control signal
+ control_signalYAW = p_termYAW + i_termYAW + d_termYAW;
- // save the current error as the previous one
- // for the next iteration.
- prev_errorYAW = curr_errorYAW;
- ROS_INFO(" Calculating control signals ... ! \n ");
+ // save the current error as the previous one
+ // for the next iteration.
+ prev_errorYAW = curr_errorYAW;
+ } else
+ {
+ 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");
+
dock(-control_signalX, -control_signalY, control_signalYAW);
}
@@ -578,14 +604,26 @@ void ImageConverter::dock(double VelX, double VelY, double omegaZ)
void ImageConverter::move2docking(double VelX_est, double VelY_est, double omegaZ_est)
{
- ROS_INFO(".... ESTIMATION .... !");
+
+ ROS_INFO_STREAM(" Zmar = " << camPose[2] << " m. \n");
+ ROS_INFO_STREAM(" Zref = " << RefPose[2] << " m. \n");
+
+ ROS_INFO_STREAM(" Ymar = " << camPose[1] << " m. \n");
+ ROS_INFO_STREAM(" Yref = " << RefPose[1] << " m. \n");
+
+ ROS_INFO_STREAM(" rollmar = " << camPose[3] << " rad. \n");
+ ROS_INFO_STREAM(" rollref = " << RefPose[3] << " rad. \n");
+
+ //ROS_INFO(" ------- /\/\/\/\ --------- \n");
+
+ ROS_INFO(".... ESTIMATION .... !\n");
geometry_msgs::Twist msg;
if (VelX_est == 0 && VelY_est == 0 && omegaZ_est == 0)
{
- VelX_est = .000001;
- VelX_est = .000001;
- omegaZ_est = .000001;
+ VelX_est = .0001;
+ VelX_est = .0001;
+ omegaZ_est = 0;
}
msg.linear.x = VelX_est;
--
GitLab