diff --git a/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/CXX.includecache b/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/CXX.includecache
index c12350d480fc2501fbe2fadfbebbeb181e835b2d..5a97d95687c4e807c505142b266294f36c86b5b9 100644
--- a/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/CXX.includecache
+++ b/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/CXX.includecache
@@ -71,6 +71,8 @@ fstream
-
sstream
-
+array
+-
math.h
-
unistd.h
diff --git a/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/flags.make b/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/flags.make
index 51ba30e6c555e137f51c86aacbb7883bc625e205..8863ce1bf389b159986e8ce50eb800ce5973470f 100644
--- a/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/flags.make
+++ b/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/flags.make
@@ -2,7 +2,7 @@
# Generated by "Unix Makefiles" Generator, CMake Version 2.8
# compile CXX with /usr/bin/c++
-CXX_FLAGS = -I/usr/local/include -I/home/faridalijani/thesis/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/ros_aruco -I/home/faridalijani/thesis/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/include -I/home/faridalijani/thesis/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/ros_aruco.h -I/opt/ros/hydro/include -I/opt/ros/hydro/include/opencv -I/usr/local/include/aruco
+CXX_FLAGS = -std=c++0x -I/usr/local/include -I/home/faridalijani/thesis/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/ros_aruco -I/home/faridalijani/thesis/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/include -I/home/faridalijani/thesis/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/ros_aruco.h -I/opt/ros/hydro/include -I/opt/ros/hydro/include/opencv -I/usr/local/include/aruco
CXX_DEFINES = -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\"ros_aruco\" -DROSCONSOLE_BACKEND_LOG4CXX
diff --git a/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/link.txt b/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/link.txt
index 7e41271cd4744600dbab2e446593fe708eabc8de..0357e4a90cd4a527ef729f2e75e86004d1581e75 100644
--- a/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/link.txt
+++ b/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/link.txt
@@ -1 +1 @@
-/usr/bin/c++ CMakeFiles/ros_aruco.dir/src/ros_aruco.cpp.o CMakeFiles/ros_aruco.dir/src/run.cpp.o -o ../../devel/lib/ros_aruco/ros_aruco -rdynamic -L/usr/local/lib /opt/ros/hydro/lib/libtf.so /opt/ros/hydro/lib/libtf2_ros.so /opt/ros/hydro/lib/libactionlib.so /opt/ros/hydro/lib/libtf2.so /opt/ros/hydro/lib/libimage_transport.so /opt/ros/hydro/lib/libmessage_filters.so -ltinyxml /opt/ros/hydro/lib/libclass_loader.so -lPocoFoundation -ldl /opt/ros/hydro/lib/libroscpp.so -lboost_signals-mt -lboost_filesystem-mt /opt/ros/hydro/lib/libxmlrpcpp.so /opt/ros/hydro/lib/libroslib.so /opt/ros/hydro/lib/libcv_bridge.so /opt/ros/hydro/lib/libopencv_videostab.so.2.4.9 /opt/ros/hydro/lib/libopencv_video.so.2.4.9 /opt/ros/hydro/lib/libopencv_superres.so.2.4.9 /opt/ros/hydro/lib/libopencv_stitching.so.2.4.9 /opt/ros/hydro/lib/libopencv_photo.so.2.4.9 /opt/ros/hydro/lib/libopencv_ocl.so.2.4.9 /opt/ros/hydro/lib/libopencv_objdetect.so.2.4.9 /opt/ros/hydro/lib/libopencv_nonfree.so.2.4.9 /opt/ros/hydro/lib/libopencv_ml.so.2.4.9 /opt/ros/hydro/lib/libopencv_legacy.so.2.4.9 /opt/ros/hydro/lib/libopencv_imgproc.so.2.4.9 /opt/ros/hydro/lib/libopencv_highgui.so.2.4.9 /opt/ros/hydro/lib/libopencv_gpu.so.2.4.9 /opt/ros/hydro/lib/libopencv_flann.so.2.4.9 /opt/ros/hydro/lib/libopencv_features2d.so.2.4.9 /opt/ros/hydro/lib/libopencv_core.so.2.4.9 /opt/ros/hydro/lib/libopencv_contrib.so.2.4.9 /opt/ros/hydro/lib/libopencv_calib3d.so.2.4.9 /opt/ros/hydro/lib/librosconsole.so /opt/ros/hydro/lib/librosconsole_log4cxx.so /opt/ros/hydro/lib/librosconsole_backend_interface.so -llog4cxx -lboost_regex-mt /opt/ros/hydro/lib/libroscpp_serialization.so /opt/ros/hydro/lib/librostime.so -lboost_date_time-mt -lboost_system-mt -lboost_thread-mt -lpthread /opt/ros/hydro/lib/libcpp_common.so /opt/ros/hydro/lib/libconsole_bridge.so /opt/ros/hydro/lib/libopencv_videostab.so.2.4.9 /opt/ros/hydro/lib/libopencv_video.so.2.4.9 /opt/ros/hydro/lib/libopencv_ts.a /opt/ros/hydro/lib/libopencv_superres.so.2.4.9 /opt/ros/hydro/lib/libopencv_stitching.so.2.4.9 /opt/ros/hydro/lib/libopencv_photo.so.2.4.9 /opt/ros/hydro/lib/libopencv_ocl.so.2.4.9 /opt/ros/hydro/lib/libopencv_objdetect.so.2.4.9 /opt/ros/hydro/lib/libopencv_nonfree.so.2.4.9 /opt/ros/hydro/lib/libopencv_ml.so.2.4.9 /opt/ros/hydro/lib/libopencv_legacy.so.2.4.9 /opt/ros/hydro/lib/libopencv_imgproc.so.2.4.9 /opt/ros/hydro/lib/libopencv_highgui.so.2.4.9 /opt/ros/hydro/lib/libopencv_gpu.so.2.4.9 /opt/ros/hydro/lib/libopencv_flann.so.2.4.9 /opt/ros/hydro/lib/libopencv_features2d.so.2.4.9 /opt/ros/hydro/lib/libopencv_core.so.2.4.9 /opt/ros/hydro/lib/libopencv_contrib.so.2.4.9 /opt/ros/hydro/lib/libopencv_calib3d.so.2.4.9 -laruco -ldl -lm -lpthread -lrt /opt/ros/hydro/lib/libopencv_nonfree.so.2.4.9 /opt/ros/hydro/lib/libopencv_ocl.so.2.4.9 /opt/ros/hydro/lib/libopencv_gpu.so.2.4.9 /opt/ros/hydro/lib/libopencv_photo.so.2.4.9 /opt/ros/hydro/lib/libopencv_objdetect.so.2.4.9 /opt/ros/hydro/lib/libopencv_legacy.so.2.4.9 /opt/ros/hydro/lib/libopencv_video.so.2.4.9 /opt/ros/hydro/lib/libopencv_ml.so.2.4.9 /opt/ros/hydro/lib/libopencv_calib3d.so.2.4.9 /opt/ros/hydro/lib/libopencv_features2d.so.2.4.9 /opt/ros/hydro/lib/libopencv_highgui.so.2.4.9 /opt/ros/hydro/lib/libopencv_imgproc.so.2.4.9 /opt/ros/hydro/lib/libopencv_flann.so.2.4.9 /opt/ros/hydro/lib/libopencv_core.so.2.4.9 -Wl,-rpath,/opt/ros/hydro/lib:/usr/local/lib
+/usr/bin/c++ -std=c++0x CMakeFiles/ros_aruco.dir/src/ros_aruco.cpp.o CMakeFiles/ros_aruco.dir/src/run.cpp.o -o ../../devel/lib/ros_aruco/ros_aruco -rdynamic -L/usr/local/lib /opt/ros/hydro/lib/libtf.so /opt/ros/hydro/lib/libtf2_ros.so /opt/ros/hydro/lib/libactionlib.so /opt/ros/hydro/lib/libtf2.so /opt/ros/hydro/lib/libimage_transport.so /opt/ros/hydro/lib/libmessage_filters.so -ltinyxml /opt/ros/hydro/lib/libclass_loader.so -lPocoFoundation -ldl /opt/ros/hydro/lib/libroscpp.so -lboost_signals-mt -lboost_filesystem-mt /opt/ros/hydro/lib/libxmlrpcpp.so /opt/ros/hydro/lib/libroslib.so /opt/ros/hydro/lib/libcv_bridge.so /opt/ros/hydro/lib/libopencv_videostab.so.2.4.9 /opt/ros/hydro/lib/libopencv_video.so.2.4.9 /opt/ros/hydro/lib/libopencv_superres.so.2.4.9 /opt/ros/hydro/lib/libopencv_stitching.so.2.4.9 /opt/ros/hydro/lib/libopencv_photo.so.2.4.9 /opt/ros/hydro/lib/libopencv_ocl.so.2.4.9 /opt/ros/hydro/lib/libopencv_objdetect.so.2.4.9 /opt/ros/hydro/lib/libopencv_nonfree.so.2.4.9 /opt/ros/hydro/lib/libopencv_ml.so.2.4.9 /opt/ros/hydro/lib/libopencv_legacy.so.2.4.9 /opt/ros/hydro/lib/libopencv_imgproc.so.2.4.9 /opt/ros/hydro/lib/libopencv_highgui.so.2.4.9 /opt/ros/hydro/lib/libopencv_gpu.so.2.4.9 /opt/ros/hydro/lib/libopencv_flann.so.2.4.9 /opt/ros/hydro/lib/libopencv_features2d.so.2.4.9 /opt/ros/hydro/lib/libopencv_core.so.2.4.9 /opt/ros/hydro/lib/libopencv_contrib.so.2.4.9 /opt/ros/hydro/lib/libopencv_calib3d.so.2.4.9 /opt/ros/hydro/lib/librosconsole.so /opt/ros/hydro/lib/librosconsole_log4cxx.so /opt/ros/hydro/lib/librosconsole_backend_interface.so -llog4cxx -lboost_regex-mt /opt/ros/hydro/lib/libroscpp_serialization.so /opt/ros/hydro/lib/librostime.so -lboost_date_time-mt -lboost_system-mt -lboost_thread-mt -lpthread /opt/ros/hydro/lib/libcpp_common.so /opt/ros/hydro/lib/libconsole_bridge.so /opt/ros/hydro/lib/libopencv_videostab.so.2.4.9 /opt/ros/hydro/lib/libopencv_video.so.2.4.9 /opt/ros/hydro/lib/libopencv_ts.a /opt/ros/hydro/lib/libopencv_superres.so.2.4.9 /opt/ros/hydro/lib/libopencv_stitching.so.2.4.9 /opt/ros/hydro/lib/libopencv_photo.so.2.4.9 /opt/ros/hydro/lib/libopencv_ocl.so.2.4.9 /opt/ros/hydro/lib/libopencv_objdetect.so.2.4.9 /opt/ros/hydro/lib/libopencv_nonfree.so.2.4.9 /opt/ros/hydro/lib/libopencv_ml.so.2.4.9 /opt/ros/hydro/lib/libopencv_legacy.so.2.4.9 /opt/ros/hydro/lib/libopencv_imgproc.so.2.4.9 /opt/ros/hydro/lib/libopencv_highgui.so.2.4.9 /opt/ros/hydro/lib/libopencv_gpu.so.2.4.9 /opt/ros/hydro/lib/libopencv_flann.so.2.4.9 /opt/ros/hydro/lib/libopencv_features2d.so.2.4.9 /opt/ros/hydro/lib/libopencv_core.so.2.4.9 /opt/ros/hydro/lib/libopencv_contrib.so.2.4.9 /opt/ros/hydro/lib/libopencv_calib3d.so.2.4.9 -laruco -ldl -lm -lpthread -lrt /opt/ros/hydro/lib/libopencv_nonfree.so.2.4.9 /opt/ros/hydro/lib/libopencv_ocl.so.2.4.9 /opt/ros/hydro/lib/libopencv_gpu.so.2.4.9 /opt/ros/hydro/lib/libopencv_photo.so.2.4.9 /opt/ros/hydro/lib/libopencv_objdetect.so.2.4.9 /opt/ros/hydro/lib/libopencv_legacy.so.2.4.9 /opt/ros/hydro/lib/libopencv_video.so.2.4.9 /opt/ros/hydro/lib/libopencv_ml.so.2.4.9 /opt/ros/hydro/lib/libopencv_calib3d.so.2.4.9 /opt/ros/hydro/lib/libopencv_features2d.so.2.4.9 /opt/ros/hydro/lib/libopencv_highgui.so.2.4.9 /opt/ros/hydro/lib/libopencv_imgproc.so.2.4.9 /opt/ros/hydro/lib/libopencv_flann.so.2.4.9 /opt/ros/hydro/lib/libopencv_core.so.2.4.9 -Wl,-rpath,/opt/ros/hydro/lib:/usr/local/lib
diff --git a/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/src/ros_aruco.cpp.o b/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/src/ros_aruco.cpp.o
index dec31813742a48c860fade08348f0252f4891d1d..cb823190b62bb7649d754fcdf8b54a71e102abdc 100644
Binary files a/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/src/ros_aruco.cpp.o and b/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/src/ros_aruco.cpp.o differ
diff --git a/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/src/run.cpp.o b/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/src/run.cpp.o
index 7f9f12b55bc65f3907c6fae187c82d948499df83..d83dab231b310e912bb8e6abf3c480b8cde97cff 100644
Binary files a/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/src/run.cpp.o and b/MobileRobot/AugReaMarker/ROS_ArUco/build/ros_aruco/CMakeFiles/ros_aruco.dir/src/run.cpp.o differ
diff --git a/MobileRobot/AugReaMarker/ROS_ArUco/devel/lib/ros_aruco/ros_aruco b/MobileRobot/AugReaMarker/ROS_ArUco/devel/lib/ros_aruco/ros_aruco
index 72d92810ae69652166e024d0a01e901ba12ebc88..967b9d61217705211de521fa55ab6828bbd44486 100755
Binary files a/MobileRobot/AugReaMarker/ROS_ArUco/devel/lib/ros_aruco/ros_aruco and b/MobileRobot/AugReaMarker/ROS_ArUco/devel/lib/ros_aruco/ros_aruco differ
diff --git a/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/CMakeLists.txt b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/CMakeLists.txt
index acd2731ea6d12b84361bda75ce1afb6a03c073d5..37fb74cf00eea1e2f12521dad5f40dc12524644d 100644
--- a/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/CMakeLists.txt
+++ b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/CMakeLists.txt
@@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS
SET(CMAKE_MODULE_PATH ${ARUCO_PATH}/lib/cmake )
SET(ARUCO_INCLUDE_DIRS ${ARUCO_PATH}/include/aruco )
+set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}")
## System dependencies are found with CMake's conventions
find_package(aruco REQUIRED )
diff --git a/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/CMakeLists.txt~ b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/CMakeLists.txt~
index d922a79391e958b15d387f848ade9199ac08a90d..acd2731ea6d12b84361bda75ce1afb6a03c073d5 100644
--- a/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/CMakeLists.txt~
+++ b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/CMakeLists.txt~
@@ -125,7 +125,7 @@ catkin_package(
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
-include_directories(
+include_directories(ros_aruco include/ ros_aruco.h
${catkin_INCLUDE_DIRS}
${ARUCO_INCLUDE_DIRS}
)
diff --git a/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/include/ros_aruco.h b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/include/ros_aruco.h
index ef8e4891311c3e52b3cc50717cc7daa22cfd1941..a70b83196566c50471b5e4c0aa99e00d09bdf2c4 100644
--- a/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/include/ros_aruco.h
+++ b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/include/ros_aruco.h
@@ -85,15 +85,15 @@ private:
static int ThePyrDownLevel;
static float TheMarkerSize;
- static double poseX,poseY,poseZ,orienX,orienY,orienZ,orienW;
-
- static bool update_images,stream;
+ void ContStart();
+ static bool update_images,MarKerPoseObtained,found;
bool readArguments ( int argc,char **argv );
static const string trackbarWindowName;
void Controller(double Reference, double RobPose, double dt, int index);
+ //void Controller(double Reference[6], double RobPose[6], double dt);
void dock(double VelX, double VelY, double VelZ, double omegaX, double omegaY, double omegaZ, double omegaW);
//void dock(double speed[6]);
@@ -134,23 +134,22 @@ public:
static double control_signal;
+
static double prop_gain;
static double integ_gain;
static double deriv_gain;
+ static double zeroMin,zeroMax;
double marpose[6];
double robpose[6];
-double getXobjPos();
-double getYobjPos();
-double getZobjPos();
-
-double getXobjOrien();
-double getYobjOrien();
-double getZobjOrien();
-double getWobjOrien();
-
-double getMarpose();
-double getRobPose();
-
+ /*double prev_error[6];
+ double int_error[6];
+ double curr_error[6];
+ double diff[6];
+ double p_term[6];
+ double d_term[6];
+ double i_term[6];
+ double control_signal[6];*/
+
};
diff --git a/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/include/ros_aruco.h~ b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/include/ros_aruco.h~
index ef8e4891311c3e52b3cc50717cc7daa22cfd1941..7822fecd7db1b9628508a3a43f548ec14f116361 100644
--- a/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/include/ros_aruco.h~
+++ b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/include/ros_aruco.h~
@@ -85,15 +85,15 @@ private:
static int ThePyrDownLevel;
static float TheMarkerSize;
- static double poseX,poseY,poseZ,orienX,orienY,orienZ,orienW;
-
- static bool update_images,stream;
+ void ContStart();
+ static bool update_images,MarKerPoseObtained,found;
bool readArguments ( int argc,char **argv );
static const string trackbarWindowName;
void Controller(double Reference, double RobPose, double dt, int index);
+ //void Controller(double Reference[6], double RobPose[6], double dt);
void dock(double VelX, double VelY, double VelZ, double omegaX, double omegaY, double omegaZ, double omegaW);
//void dock(double speed[6]);
@@ -134,23 +134,22 @@ public:
static double control_signal;
+
static double prop_gain;
static double integ_gain;
static double deriv_gain;
+
double marpose[6];
double robpose[6];
-double getXobjPos();
-double getYobjPos();
-double getZobjPos();
-
-double getXobjOrien();
-double getYobjOrien();
-double getZobjOrien();
-double getWobjOrien();
-
-double getMarpose();
-double getRobPose();
-
+ /*double prev_error[6];
+ double int_error[6];
+ double curr_error[6];
+ double diff[6];
+ double p_term[6];
+ double d_term[6];
+ double i_term[6];
+ double control_signal[6];*/
+
};
diff --git a/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/src/ros_aruco.cpp b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/src/ros_aruco.cpp
index b15640fe561bfd2fa0c3f12596376fd279b82a85..c85fd29678c3a470249681163cc4d2890ca65847 100644
--- a/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/src/ros_aruco.cpp
+++ b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/src/ros_aruco.cpp
@@ -1,6 +1,7 @@
#include <iostream>
#include <fstream>
#include <sstream>
+#include <array>
#include <math.h>
#include <unistd.h>
@@ -55,19 +56,12 @@ int ImageConverter::ThePyrDownLevel = 0;
bool ImageConverter::update_images = true;
-bool ImageConverter::stream = false;
+bool ImageConverter::MarKerPoseObtained = false;
+bool ImageConverter::found = false;
int ImageConverter::ThresParam1 = 0;
int ImageConverter::ThresParam2 = 0;
-double ImageConverter::poseX = 0;
-double ImageConverter::poseY = 0;
-double ImageConverter::poseZ = 0;
-
-double ImageConverter::orienX = 0;
-double ImageConverter::orienY = 0;
-double ImageConverter::orienZ = 0;
-double ImageConverter::orienW = 0;
double ImageConverter::prev_error;
double ImageConverter::curr_error;
@@ -77,12 +71,16 @@ double ImageConverter::diff;
double ImageConverter::p_term = 0;
double ImageConverter::i_term = 0;
double ImageConverter::d_term = 0;
+double ImageConverter::control_signal;
double ImageConverter::prop_gain = 0.01;
double ImageConverter::integ_gain = 0.01;
double ImageConverter::deriv_gain = 0;
-double ImageConverter::control_signal;
+double ImageConverter::zeroMax = .0000000000000000001;
+
+double ImageConverter::zeroMin = -.0000000000000000001;
+
ImageConverter::ImageConverter() :
it_(nh_)
@@ -111,58 +109,6 @@ double ImageConverter::control_signal;
{
return img;
}
-
-double ImageConverter::getXobjPos()
-{
- return poseX;
-}
-
-
-double ImageConverter::getYobjPos()
-{
- return poseY;
-}
-
-
-double ImageConverter::getZobjPos()
-{
- return poseZ;
-}
-
-double ImageConverter::getXobjOrien()
-{
- return orienX;
-}
-
-
-double ImageConverter::getYobjOrien()
-{
- return orienY;
-}
-
-
-double ImageConverter::getZobjOrien()
-{
- return orienZ;
-}
-
-
-double ImageConverter::getWobjOrien()
-{
- return orienW;
-}
-
-double ImageConverter::getMarpose()
-{
- return marpose[6];
-}
-
-
-double ImageConverter::getRobPose()
-{
- return robpose[6];
-}
-
void ImageConverter::cvTackBarEvents(int value,void* ptr)
{
ImageConverter* ic = (ImageConverter*)(ptr);
@@ -272,9 +218,6 @@ void ImageConverter::ProgStart(int argc,char** argv)
{
cerr<<"Could not open video!!"<<endl;
nh_.shutdown();
- } else
- {
- stream = true;
}
// Read first image to get the dimensions
@@ -312,18 +255,26 @@ void ImageConverter::ProgStart(int argc,char** argv)
float roll,yaw,pitch;
float rollE,yawE,pitchE;
- bool found = (TheMarkers.size()>0)?true:false;
-
- if (found)
+ if (TheMarkers.size()>0)
{
- x_t = -TheMarkers[0].Tvec.at<Vec3f>(0,0)[0];
+ found = true;
+ ROS_INFO("MARKER FOUND!!! ... \n");
+ }
+ else
+ {
+ found = false;
+ ROS_INFO("MARKER NOT FOUND ! \n");
+ }
+ //bool found = (TheMarkers.size()>0)?true:false;
+
+ if (ros::ok() && found)
+ {
+ x_t = -TheMarkers[0].Tvec.at<Vec3f>(0,0)[0];
y_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[1];
z_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[2];
//printf("\n X = %4.2f Y = %4.2f Z = %4.2f\n",x_t,y_t,z_t);
- //printf("\n rotX = %4.2f rotY = %4.2f rotZ = %4.2f\n",rotX,rotY,rotZ);
-
cv::Mat R(3,3,cv::DataType<float>::type);
// You need to apply cv::Rodrigues() in order to obatain angles wrt to camera coords
cv::Rodrigues(TheMarkers[0].Rvec,R);
@@ -364,19 +315,7 @@ void ImageConverter::ProgStart(int argc,char** argv)
pitch = -atan2(R.at<float>(2,0), R.at<float>(2,1));
yaw = acos(R.at<float>(2,2));
roll = -atan2(R.at<float>(0,2), R.at<float>(1,2));
-
- } else
- {
- // x_t = 0.0;
- // y_t = 0.0;
- // z_t = -10.0;
- // yaw = 0.0;
- // roll = 0.0;
- // pitch = 0.0;
- ROS_INFO("MARKER NOT FOUND \n!");
- }
-
// Marker rotation should be initially zero (just for convenience)
float p_off = CV_PI;
float r_off = CV_PI/2;
@@ -384,9 +323,7 @@ void ImageConverter::ProgStart(int argc,char** argv)
// See: http://en.wikipedia.org/wiki/Flight_dynamics
-
- if (ros::ok() && found && stream)
- {
+
ros::Time time = ros::Time::now();
// Camera Frame ---
transformCAM.setOrigin( tf::Vector3(.25, 0, .5) );
@@ -405,6 +342,18 @@ void ImageConverter::ProgStart(int argc,char** argv)
"/camera",
"marker"));
+ // Now publish the pose message, remember the offsets
+ msg.position.x = x_t;
+ msg.position.y = y_t;
+ msg.position.z = z_t;
+
+ geometry_msgs::Quaternion p_quat = tf::createQuaternionMsgFromRollPitchYaw(roll-r_off, pitch+p_off, yaw-y_off);
+ msg.orientation = p_quat;
+
+ pose_pub.publish(msg);
+ ros::spinOnce();
+
+
tf::StampedTransform ObjPose;
tf::StampedTransform RobPose;
@@ -421,7 +370,6 @@ void ImageConverter::ProgStart(int argc,char** argv)
{
ROS_WARN("%s",ex.what());
}
- ROS_INFO("MARKER FOUND!!! ROBOT IS MOVING ... ");
/*printf("\nMarker Pose in Global Coordinate : Xobj = %5.4f, Yobj = %5.4f, Zobj = %5.4f \n ",
ObjPose.getOrigin().x(),
ObjPose.getOrigin().y(),
@@ -440,47 +388,36 @@ void ImageConverter::ProgStart(int argc,char** argv)
marpose[0] = ObjPose.getOrigin().x();
marpose[1] = ObjPose.getOrigin().y();
marpose[2] = ObjPose.getOrigin().z();
-
marpose[3] = ObjPose.getRotation().x();
marpose[4] = ObjPose.getRotation().y();
marpose[5] = ObjPose.getRotation().z();
marpose[6] = ObjPose.getRotation().w();
- getMarpose();
robpose[0] = RobPose.getOrigin().x();
robpose[1] = RobPose.getOrigin().y();
robpose[2] = RobPose.getOrigin().z();
-
robpose[3] = RobPose.getRotation().x();
robpose[4] = RobPose.getRotation().y();
robpose[5] = RobPose.getRotation().z();
robpose[6] = RobPose.getRotation().w();
- getRobPose();
-
-
-
- poseX = ObjPose.getOrigin().x();
- getXobjPos();
-
- poseY = ObjPose.getOrigin().y();
- getYobjPos();
-
- poseZ = ObjPose.getOrigin().z();
- getZobjPos();
-
- orienX = ObjPose.getRotation().x();
- getXobjOrien();
-
- orienY = ObjPose.getRotation().y();
- getYobjOrien();
-
- orienZ = ObjPose.getRotation().z();
- getZobjOrien();
-
- orienW = ObjPose.getRotation().w();
- getWobjOrien();
+ for (int i = 0; i<7; ++i)
+ {
+ ROS_INFO_STREAM("ROBOT element " << i << " = " << robpose[i] << " . \n");
+ ROS_INFO_STREAM("MARKER element " << i << " = " << marpose[i] << " . \n");
+
+ if (marpose[i] >= zeroMax || marpose[i] <= zeroMin)
+ {
+ MarKerPoseObtained = true;
+ ROS_INFO("MARKER POSE ACHIEVED, MOVE TO CONTROLLER ... ! \n");
+
+ } else
+ {
+ MarKerPoseObtained = false;
+ ROS_INFO("MARKER POSE HAS NOT ACHIEVED ! \n");
+ }
+ }
//printf( "\nAngles (deg.) with offset : roll = %5.2f pitch = %5.2f yaw = %5.2f \n", (roll-r_off)*(180.0/CV_PI), (pitch+p_off)*(180.0/CV_PI), (yaw-y_off)*(180.0/CV_PI));
@@ -496,17 +433,8 @@ void ImageConverter::ProgStart(int argc,char** argv)
printf( "\nMarker distance (m.): x_d = %5.4f y_d = %5.4f z_d = %5.4f \n", x_t, y_t, z_t);*/
- // Now publish the pose message, remember the offsets
- msg.position.x = x_t;
- msg.position.y = y_t;
- msg.position.z = z_t;
- geometry_msgs::Quaternion p_quat = tf::createQuaternionMsgFromRollPitchYaw(roll-r_off, pitch+p_off, yaw-y_off);
- msg.orientation = p_quat;
-
- pose_pub.publish(msg);
-
- ros::spinOnce();
+ //ContStart();
}
/*// Print other rectangles that contains no valid markers
for (unsigned int i=0;i<MDetector.getCandidates().size();i++) {
@@ -546,9 +474,22 @@ void ImageConverter::ProgStart(int argc,char** argv)
}
// ---- Controller part ----------- START -------
+
+void ImageConverter::ContStart()
+{
+ ros::Rate rate(100);
+
+ while (ros::ok() && keepMoving)
+ {
+ ros::spinOnce();
+ rate.sleep();
+ }
+
+}
+
void ImageConverter::robCB(const geometry_msgs::PoseStamped::ConstPtr& RobFB)
{
- ROS_INFO_STREAM(" Xrob = " << robpose[0] << " m. \n");
+ /*ROS_INFO_STREAM(" Xrob = " << robpose[0] << " m. \n");
ROS_INFO_STREAM(" ObjX = " << marpose[0] << " m. \n");
ROS_INFO_STREAM(" Yrob = " << robpose[1] << " m. \n");
@@ -567,10 +508,64 @@ void ImageConverter::robCB(const geometry_msgs::PoseStamped::ConstPtr& RobFB)
ROS_INFO_STREAM(" ObjOrienZ = " << marpose[5] << " rad. \n");
ROS_INFO_STREAM(" WorienRob = " << robpose[6] << " rad. \n");
- ROS_INFO_STREAM(" ObjOrienW = " << marpose[6] << " rad. \n");
+ ROS_INFO_STREAM(" ObjOrienW = " << marpose[6] << " rad. \n");*/
+
- for (int i = 0; i<7; i++)
+if (MarKerPoseObtained && keepMoving)
+{
+ if ((robpose[0] >= marpose[0]) &&
+ (robpose[1] >= marpose[1]) /*&&
+ (robpose[2] >= marpose[2])*/)
+ {
+ if (/*(robpose[3] >= marpose[3]) &&
+ (robpose[4] >= marpose[4]) &&*/
+ (robpose[5] >= marpose[5]) /*&&
+ (robpose[6] >= marpose[6])*/)
+ {
+ ROS_INFO(" Dock is completed! \n ");
+ keepMoving = false;
+ }/*else if (robpose[3] <= marpose[3])
+ {
+ ROS_INFO(" Calculating control signal For ROLL! \n ");
+ Controller(marpose[3],robpose[3],.01,4);
+
+ } else if (robpose[4] <= marpose[4])
+ {
+ ROS_INFO(" Calculating control signal For PITCH! \n ");
+ Controller(marpose[4],robpose[4],.01,5);
+
+ }*/ else if ((robpose[5] <= marpose[5]))
+ {
+ ROS_INFO(" Calculating control signal For YAW! \n ");
+ Controller(marpose[5],robpose[5],.01,6);
+ } /*else if ((robpose[6] <= marpose[6]))
+ {
+ ROS_INFO(" Calculating control signal For W! \n ");
+ Controller(marpose[6],robpose[6],.01,7);
+ }*/
+
+
+ } else if ((robpose[0] <= marpose[0]))
+ {
+ ROS_INFO(" Calculating control signal For X-direction! \n ");
+ Controller(marpose[0],robpose[0],.01,1);
+ } else if ((robpose[1] <= marpose[1]))
+ {
+ ROS_INFO(" Calculating control signal For Y-direction! \n ");
+ Controller(marpose[1],robpose[1],.01,2);
+ } /*else if ((robpose[2] <= marpose[2]))
+ {
+ ROS_INFO(" Calculating control signal For Z-direction! \n ");
+ Controller(marpose[2],robpose[2],.01,3);
+ }*/
+
+
+
+
+
+
+ /*for (int i = 0; i<7; i++)
{
if (marpose[i] >= robpose[i])
{
@@ -579,10 +574,24 @@ void ImageConverter::robCB(const geometry_msgs::PoseStamped::ConstPtr& RobFB)
{
Controller(marpose[i],robpose[i],.1,i);
}
- }
+ }*/
+
+ /*std::array<double,7> MarkerPos {marpose[0],marpose[1],marpose[2],marpose[3],marpose[4],marpose[5],marpose[6]};
+ std::array<double,7> RobotPos {robpose[0],robpose[1],robpose[2],robpose[3],robpose[4],robpose[5],robpose[6]};
+ if (MarkerPos >= RobotPos)
+ {
+ ROS_INFO_STREAM("Dock is completed!");
+ } else
+ {
+ Controller(marpose,robpose,.01);
+ }*/
+
+} // end if;
}
+
+//void ImageConverter::Controller(double Reference[i], double RobPose[i], double dt)
void ImageConverter::Controller(double Reference, double RobPose, double dt, int index)
{
// e(t) = setpoint - actual value;
@@ -590,8 +599,6 @@ void ImageConverter::Controller(double Reference, double RobPose, double dt, int
// Integrated error
int_error += curr_error * dt;
-
-
/*
// -- windup gaurd --
if (int_error < )
@@ -639,11 +646,8 @@ void ImageConverter::Controller(double Reference, double RobPose, double dt, int
{
dock(0, 0, 0, 0, 0, 0, control_signal);
}*/
-
-
-
-
}
+
//void ImageConverter::dock(double speed[6])
void ImageConverter::dock(double VelX, double VelY, double VelZ, double omegaX, double omegaY, double omegaZ, double omegaW)
{
diff --git a/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/src/ros_aruco.cpp~ b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/src/ros_aruco.cpp~
index 2d20a73763ceb289ea9111d75ca959a8560b8b1e..61c10d8d4c7f9cb465b26126d6de7df657eb7f78 100644
--- a/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/src/ros_aruco.cpp~
+++ b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/src/ros_aruco.cpp~
@@ -1,6 +1,7 @@
#include <iostream>
#include <fstream>
#include <sstream>
+#include <array>
#include <math.h>
#include <unistd.h>
@@ -55,19 +56,12 @@ int ImageConverter::ThePyrDownLevel = 0;
bool ImageConverter::update_images = true;
-bool ImageConverter::stream = false;
+bool ImageConverter::MarKerPoseObtained = false;
+bool ImageConverter::found = false;
int ImageConverter::ThresParam1 = 0;
int ImageConverter::ThresParam2 = 0;
-double ImageConverter::poseX = 0;
-double ImageConverter::poseY = 0;
-double ImageConverter::poseZ = 0;
-
-double ImageConverter::orienX = 0;
-double ImageConverter::orienY = 0;
-double ImageConverter::orienZ = 0;
-double ImageConverter::orienW = 0;
double ImageConverter::prev_error;
double ImageConverter::curr_error;
@@ -77,12 +71,16 @@ double ImageConverter::diff;
double ImageConverter::p_term = 0;
double ImageConverter::i_term = 0;
double ImageConverter::d_term = 0;
+double ImageConverter::control_signal;
-double ImageConverter::prop_gain = 0.01;
-double ImageConverter::integ_gain = 0.01;
+double ImageConverter::prop_gain = 0.001;
+double ImageConverter::integ_gain = 0.001;
double ImageConverter::deriv_gain = 0;
-double ImageConverter::control_signal;
+double ImageConverter::zeroMax = .0000000000000000001;
+
+double ImageConverter::zeroMin = -.0000000000000000001;
+
ImageConverter::ImageConverter() :
it_(nh_)
@@ -111,58 +109,6 @@ double ImageConverter::control_signal;
{
return img;
}
-
-double ImageConverter::getXobjPos()
-{
- return poseX;
-}
-
-
-double ImageConverter::getYobjPos()
-{
- return poseY;
-}
-
-
-double ImageConverter::getZobjPos()
-{
- return poseZ;
-}
-
-double ImageConverter::getXobjOrien()
-{
- return orienX;
-}
-
-
-double ImageConverter::getYobjOrien()
-{
- return orienY;
-}
-
-
-double ImageConverter::getZobjOrien()
-{
- return orienZ;
-}
-
-
-double ImageConverter::getWobjOrien()
-{
- return orienW;
-}
-
-double ImageConverter::getMarpose()
-{
- return marpose[6];
-}
-
-
-double ImageConverter::getRobPose()
-{
- return robpose[6];
-}
-
void ImageConverter::cvTackBarEvents(int value,void* ptr)
{
ImageConverter* ic = (ImageConverter*)(ptr);
@@ -272,9 +218,6 @@ void ImageConverter::ProgStart(int argc,char** argv)
{
cerr<<"Could not open video!!"<<endl;
nh_.shutdown();
- } else
- {
- stream = true;
}
// Read first image to get the dimensions
@@ -312,18 +255,26 @@ void ImageConverter::ProgStart(int argc,char** argv)
float roll,yaw,pitch;
float rollE,yawE,pitchE;
- bool found = (TheMarkers.size()>0)?true:false;
-
- if (found)
+ if (TheMarkers.size()>0)
{
- x_t = -TheMarkers[0].Tvec.at<Vec3f>(0,0)[0];
+ found = true;
+ ROS_INFO("MARKER FOUND!!! ... \n");
+ }
+ else
+ {
+ found = false;
+ ROS_INFO("MARKER NOT FOUND ! \n");
+ }
+ //bool found = (TheMarkers.size()>0)?true:false;
+
+ if (ros::ok() && found)
+ {
+ x_t = -TheMarkers[0].Tvec.at<Vec3f>(0,0)[0];
y_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[1];
z_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[2];
//printf("\n X = %4.2f Y = %4.2f Z = %4.2f\n",x_t,y_t,z_t);
- //printf("\n rotX = %4.2f rotY = %4.2f rotZ = %4.2f\n",rotX,rotY,rotZ);
-
cv::Mat R(3,3,cv::DataType<float>::type);
// You need to apply cv::Rodrigues() in order to obatain angles wrt to camera coords
cv::Rodrigues(TheMarkers[0].Rvec,R);
@@ -364,19 +315,7 @@ void ImageConverter::ProgStart(int argc,char** argv)
pitch = -atan2(R.at<float>(2,0), R.at<float>(2,1));
yaw = acos(R.at<float>(2,2));
roll = -atan2(R.at<float>(0,2), R.at<float>(1,2));
-
- } else
- {
- // x_t = 0.0;
- // y_t = 0.0;
- // z_t = -10.0;
- // yaw = 0.0;
- // roll = 0.0;
- // pitch = 0.0;
- ROS_INFO("MARKER NOT FOUND \n!");
- }
-
// Marker rotation should be initially zero (just for convenience)
float p_off = CV_PI;
float r_off = CV_PI/2;
@@ -384,9 +323,7 @@ void ImageConverter::ProgStart(int argc,char** argv)
// See: http://en.wikipedia.org/wiki/Flight_dynamics
-
- if (ros::ok() && found && stream)
- {
+
ros::Time time = ros::Time::now();
// Camera Frame ---
transformCAM.setOrigin( tf::Vector3(.25, 0, .5) );
@@ -405,6 +342,18 @@ void ImageConverter::ProgStart(int argc,char** argv)
"/camera",
"marker"));
+ // Now publish the pose message, remember the offsets
+ msg.position.x = x_t;
+ msg.position.y = y_t;
+ msg.position.z = z_t;
+
+ geometry_msgs::Quaternion p_quat = tf::createQuaternionMsgFromRollPitchYaw(roll-r_off, pitch+p_off, yaw-y_off);
+ msg.orientation = p_quat;
+
+ pose_pub.publish(msg);
+ ros::spinOnce();
+
+
tf::StampedTransform ObjPose;
tf::StampedTransform RobPose;
@@ -421,7 +370,6 @@ void ImageConverter::ProgStart(int argc,char** argv)
{
ROS_WARN("%s",ex.what());
}
- ROS_INFO("MARKER FOUND!!! ROBOT IS MOVING ... ");
/*printf("\nMarker Pose in Global Coordinate : Xobj = %5.4f, Yobj = %5.4f, Zobj = %5.4f \n ",
ObjPose.getOrigin().x(),
ObjPose.getOrigin().y(),
@@ -440,47 +388,36 @@ void ImageConverter::ProgStart(int argc,char** argv)
marpose[0] = ObjPose.getOrigin().x();
marpose[1] = ObjPose.getOrigin().y();
marpose[2] = ObjPose.getOrigin().z();
-
marpose[3] = ObjPose.getRotation().x();
marpose[4] = ObjPose.getRotation().y();
marpose[5] = ObjPose.getRotation().z();
marpose[6] = ObjPose.getRotation().w();
- getMarpose();
robpose[0] = RobPose.getOrigin().x();
robpose[1] = RobPose.getOrigin().y();
robpose[2] = RobPose.getOrigin().z();
-
robpose[3] = RobPose.getRotation().x();
robpose[4] = RobPose.getRotation().y();
robpose[5] = RobPose.getRotation().z();
robpose[6] = RobPose.getRotation().w();
- getRobPose();
-
-
-
- poseX = ObjPose.getOrigin().x();
- getXobjPos();
-
- poseY = ObjPose.getOrigin().y();
- getYobjPos();
-
- poseZ = ObjPose.getOrigin().z();
- getZobjPos();
-
- orienX = ObjPose.getRotation().x();
- getXobjOrien();
-
- orienY = ObjPose.getRotation().y();
- getYobjOrien();
-
- orienZ = ObjPose.getRotation().z();
- getZobjOrien();
-
- orienW = ObjPose.getRotation().w();
- getWobjOrien();
+ for (int i = 0; i<7; ++i)
+ {
+ ROS_INFO_STREAM("ROBOT element " << i << " = " << robpose[i] << " . \n");
+ ROS_INFO_STREAM("MARKER element " << i << " = " << marpose[i] << " . \n");
+
+ if (marpose[i] >= zeroMax || marpose[i] <= zeroMin)
+ {
+ MarKerPoseObtained = true;
+ ROS_INFO("MARKER POSE ACHIEVED, MOVE TO CONTROLLER ... ! \n");
+
+ } else
+ {
+ MarKerPoseObtained = false;
+ ROS_INFO("MARKER POSE HAS NOT ACHIEVED ! \n");
+ }
+ }
//printf( "\nAngles (deg.) with offset : roll = %5.2f pitch = %5.2f yaw = %5.2f \n", (roll-r_off)*(180.0/CV_PI), (pitch+p_off)*(180.0/CV_PI), (yaw-y_off)*(180.0/CV_PI));
@@ -496,17 +433,8 @@ void ImageConverter::ProgStart(int argc,char** argv)
printf( "\nMarker distance (m.): x_d = %5.4f y_d = %5.4f z_d = %5.4f \n", x_t, y_t, z_t);*/
- // Now publish the pose message, remember the offsets
- msg.position.x = x_t;
- msg.position.y = y_t;
- msg.position.z = z_t;
- geometry_msgs::Quaternion p_quat = tf::createQuaternionMsgFromRollPitchYaw(roll-r_off, pitch+p_off, yaw-y_off);
- msg.orientation = p_quat;
-
- pose_pub.publish(msg);
-
- ros::spinOnce();
+ //ContStart();
}
/*// Print other rectangles that contains no valid markers
for (unsigned int i=0;i<MDetector.getCandidates().size();i++) {
@@ -546,9 +474,22 @@ void ImageConverter::ProgStart(int argc,char** argv)
}
// ---- Controller part ----------- START -------
+
+void ImageConverter::ContStart()
+{
+ ros::Rate rate(100);
+
+ while (ros::ok() && keepMoving)
+ {
+ ros::spinOnce();
+ rate.sleep();
+ }
+
+}
+
void ImageConverter::robCB(const geometry_msgs::PoseStamped::ConstPtr& RobFB)
{
- ROS_INFO_STREAM(" Xrob = " << robpose[0] << " m. \n");
+ /*ROS_INFO_STREAM(" Xrob = " << robpose[0] << " m. \n");
ROS_INFO_STREAM(" ObjX = " << marpose[0] << " m. \n");
ROS_INFO_STREAM(" Yrob = " << robpose[1] << " m. \n");
@@ -567,22 +508,90 @@ void ImageConverter::robCB(const geometry_msgs::PoseStamped::ConstPtr& RobFB)
ROS_INFO_STREAM(" ObjOrienZ = " << marpose[5] << " rad. \n");
ROS_INFO_STREAM(" WorienRob = " << robpose[6] << " rad. \n");
- ROS_INFO_STREAM(" ObjOrienW = " << marpose[6] << " rad. \n");
+ ROS_INFO_STREAM(" ObjOrienW = " << marpose[6] << " rad. \n");*/
+
- for (int i = 0; i<7; i++)
+if (MarKerPoseObtained && keepMoving)
+{
+ if ((robpose[0] >= marpose[0]) &&
+ (robpose[1] >= marpose[1]) /*&&
+ (robpose[2] >= marpose[2])*/)
+ {
+ if (/*(robpose[3] >= marpose[3]) &&
+ (robpose[4] >= marpose[4]) &&*/
+ (robpose[5] >= marpose[5]) /*&&
+ (robpose[6] >= marpose[6])*/)
+ {
+ ROS_INFO(" Dock is completed! \n ");
+ keepMoving = false;
+ }/*else if (robpose[3] <= marpose[3])
+ {
+ ROS_INFO(" Calculating control signal For ROLL! \n ");
+ Controller(marpose[3],robpose[3],.01,4);
+
+ } else if (robpose[4] <= marpose[4])
+ {
+ ROS_INFO(" Calculating control signal For PITCH! \n ");
+ Controller(marpose[4],robpose[4],.01,5);
+
+ }*/ else if ((robpose[5] <= marpose[5]))
+ {
+ ROS_INFO(" Calculating control signal For YAW! \n ");
+ Controller(marpose[5],robpose[5],.01,6);
+ } /*else if ((robpose[6] <= marpose[6]))
+ {
+ ROS_INFO(" Calculating control signal For W! \n ");
+ Controller(marpose[6],robpose[6],.01,7);
+ }*/
+
+
+ } else if ((robpose[0] <= marpose[0]))
+ {
+ ROS_INFO(" Calculating control signal For X-direction! \n ");
+ Controller(marpose[0],robpose[0],.01,1);
+ } else if ((robpose[1] <= marpose[1]))
+ {
+ ROS_INFO(" Calculating control signal For Y-direction! \n ");
+ Controller(marpose[1],robpose[1],.01,2);
+ } /*else if ((robpose[2] <= marpose[2]))
+ {
+ ROS_INFO(" Calculating control signal For Z-direction! \n ");
+ Controller(marpose[2],robpose[2],.01,3);
+ }*/
+
+
+
+
+
+
+ /*for (int i = 0; i<7; i++)
{
if (marpose[i] >= robpose[i])
{
- ROS_INFO_STREAM("The " << i << " th. element of marker = " << marpose[i] << " and robot = " << robpose[i] << "are matched! \n" );
+ ROS_INFO_STREAM("The " << i << " th. element of marker = " << marpose[i] << " and robot = " << robpose[i] << " are matched! \n" );
} else
{
Controller(marpose[i],robpose[i],.1,i);
}
- }
+ }*/
+
+ /*std::array<double,7> MarkerPos {marpose[0],marpose[1],marpose[2],marpose[3],marpose[4],marpose[5],marpose[6]};
+ std::array<double,7> RobotPos {robpose[0],robpose[1],robpose[2],robpose[3],robpose[4],robpose[5],robpose[6]};
+ if (MarkerPos >= RobotPos)
+ {
+ ROS_INFO_STREAM("Dock is completed!");
+ } else
+ {
+ Controller(marpose,robpose,.01);
+ }*/
+
+} // end if;
}
+
+//void ImageConverter::Controller(double Reference[i], double RobPose[i], double dt)
void ImageConverter::Controller(double Reference, double RobPose, double dt, int index)
{
// e(t) = setpoint - actual value;
@@ -590,8 +599,6 @@ void ImageConverter::Controller(double Reference, double RobPose, double dt, int
// Integrated error
int_error += curr_error * dt;
-
-
/*
// -- windup gaurd --
if (int_error < )
@@ -639,11 +646,8 @@ void ImageConverter::Controller(double Reference, double RobPose, double dt, int
{
dock(0, 0, 0, 0, 0, 0, control_signal);
}*/
-
-
-
-
}
+
//void ImageConverter::dock(double speed[6])
void ImageConverter::dock(double VelX, double VelY, double VelZ, double omegaX, double omegaY, double omegaZ, double omegaW)
{
diff --git a/MobileRobot/ObjectTrack/a.out b/MobileRobot/ObjectTrack/a.out
index 2dae1d3d2f62538391e9b5575c19e11e04244d58..042b114a2a126dbd727d9c9ca4d602ea294684bb 100755
Binary files a/MobileRobot/ObjectTrack/a.out and b/MobileRobot/ObjectTrack/a.out differ
diff --git a/copy_2_robot.txt b/copy_2_robot.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5fd823facf81d605e3244f97992eccc68b02b750
--- /dev/null
+++ b/copy_2_robot.txt
@@ -0,0 +1,8 @@
+ ---- How to copy a file from prace to robot via ssh connection ----------
+
+ 1. copy a normal file :
+ $ scp /home/faridalijani/thesis/MobileRobot/CameraCalibration/build/bin/Android_calibrated_data.yml robot@192.168.0.146:Farid/ROS_ArUco
+
+ 2. copy a whole folder :
+ $ scp -r ROS_ArUco/ robot@192.168.0.146:Farid/
+