Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
farid
thesis
Commits
b6de0f5a
Commit
b6de0f5a
authored
Oct 13, 2016
by
Farid Alijani
Browse files
Merge branch 'master' of
https://gitlab.control.lth.se/farid/thesis
parents
5e36e068
32686fad
Changes
4
Expand all
Hide whitespace changes
Inline
Side-by-side
MobileRobot/docking_data/back_up_recording.cpp
View file @
b6de0f5a
...
...
@@ -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
...
...
MobileRobot/docking_data/back_up_recording.cpp~
View file @
b6de0f5a
...
...
@@ -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
...
...
MobileRobot/docking_data/recording_with_theta_zero.cpp
0 → 100644
View file @
b6de0f5a
This diff is collapsed.
Click to expand it.
import_catkin_in_Qt.txt
0 → 100644
View file @
b6de0f5a
/* ---- How to import a catkin package into qt creator ---- */
1. run Qt from the terminal NOT desktop $ faridalijani@prace:~/Qt/Tools/QtCreator/bin$
2. Edit CMakeLists.txt
To be able to modify all the files in the workspace add those lines in "CMakeLists.txt" :
#Add custom (non compiling) targets so launch scripts and python files show up in QT Creator's project view.
file(GLOB_RECURSE EXTRA_FILES */*)
add_custom_target(DockingWithQt ${PROJECT_NAME}_OTHER_FILES ALL WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} SOURCES ${EXTRA_FILES})
3. "Open File or Project"
4. Go to package and select CMakeLists.txt
5. run CMake
6. Have everything in Qt
7. if more than one argument in runnig file :
project -> run -> add arguments in command line arguments
exampple : live /home/faridalijani/thesis/MobileRobot/Calibration_Files/Logitech_C920_Calibration_file.yml .08
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment