Skip to content
GitLab
Commit b6de0f5a authored by Farid Alijani's avatar Farid Alijani
Browse files

Merge branch 'master' of https://gitlab.control.lth.se/farid/thesis

parents 5e36e068 32686fad
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.
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
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment