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 3a1dfb844acfe88edec5caa22c12112b51e70a7b..dec31813742a48c860fade08348f0252f4891d1d 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 7d13b0f73daaf54a1437f94ee1d1d2fd67834794..7f9f12b55bc65f3907c6fae187c82d948499df83 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 21377700039f881b7b83cac4773fcef37d5be792..72d92810ae69652166e024d0a01e901ba12ebc88 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/include/ros_aruco.h b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/include/ros_aruco.h
index 98f4d0eb6374efa2f48dca1422e2bd3018ddf52d..ef8e4891311c3e52b3cc50717cc7daa22cfd1941 100644
--- a/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/include/ros_aruco.h
+++ b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/include/ros_aruco.h
@@ -96,10 +96,7 @@ private:
void Controller(double Reference, double RobPose, double dt, int index);
void dock(double VelX, double VelY, double VelZ, double omegaX, double omegaY, double omegaZ, double omegaW);
-
-
- void ContStart(const tf::Transform& ObjFB, const tf::Transform& RobFB);
-
+ //void dock(double speed[6]);
public:
ImageConverter();
@@ -112,13 +109,6 @@ public:
void robCB(const geometry_msgs::PoseStamped::ConstPtr& RobFB);
- void marCB(const tf::Transform& ObjFB);
-
- //void marCB(const geometry_msgs::PoseStamped::ConstPtr& ObjFB);
-
-
- void ErrCal(const tf::Transform& ObjFB, const tf::Transform& RobFB);
-
static void cvTackBarEvents(int value,void* ptr);
void ProgStart(int argc,char** argv);
@@ -148,7 +138,9 @@ public:
static double integ_gain;
static double deriv_gain;
-
+ double marpose[6];
+ double robpose[6];
+
double getXobjPos();
double getYobjPos();
double getZobjPos();
@@ -158,4 +150,7 @@ double getYobjOrien();
double getZobjOrien();
double getWobjOrien();
+double getMarpose();
+double getRobPose();
+
};
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 e7240834752a7ba6425755f7019ba91e734a5168..ef8e4891311c3e52b3cc50717cc7daa22cfd1941 100644
--- a/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/include/ros_aruco.h~
+++ b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/include/ros_aruco.h~
@@ -96,10 +96,7 @@ private:
void Controller(double Reference, double RobPose, double dt, int index);
void dock(double VelX, double VelY, double VelZ, double omegaX, double omegaY, double omegaZ, double omegaW);
-
-
- void ContStart(const tf::Transform& ObjFB, const tf::Transform& RobFB);
-
+ //void dock(double speed[6]);
public:
ImageConverter();
@@ -112,13 +109,6 @@ public:
void robCB(const geometry_msgs::PoseStamped::ConstPtr& RobFB);
- void marCB(const tf::Transform& ObjFB);
-
- //void marCB(const geometry_msgs::PoseStamped::ConstPtr& ObjFB);
-
-
- void ErrCal(const tf::Transform& ObjFB, const tf::Transform& RobFB);
-
static void cvTackBarEvents(int value,void* ptr);
void ProgStart(int argc,char** argv);
@@ -148,14 +138,19 @@ public:
static double integ_gain;
static double deriv_gain;
-
-double getXobjPos();
-double getXobjPos();
+ double marpose[6];
+ double robpose[6];
+
double getXobjPos();
+double getYobjPos();
+double getZobjPos();
double getXobjOrien();
double getYobjOrien();
double getZobjOrien();
double getWobjOrien();
+double getMarpose();
+double getRobPose();
+
};
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 a384933bc4882143e0b8d6e5082eb2edca64aeef..b15640fe561bfd2fa0c3f12596376fd279b82a85 100644
--- a/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/src/ros_aruco.cpp
+++ b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/src/ros_aruco.cpp
@@ -69,23 +69,20 @@ double ImageConverter::orienY = 0;
double ImageConverter::orienZ = 0;
double ImageConverter::orienW = 0;
-
-
-
-double ImageConverter::prev_error = 0;
-double ImageConverter::curr_error = 0;
-double ImageConverter::int_error = 0;
-double ImageConverter::diff = 0;
+double ImageConverter::prev_error;
+double ImageConverter::curr_error;
+double ImageConverter::int_error;
+double ImageConverter::diff;
double ImageConverter::p_term = 0;
double ImageConverter::i_term = 0;
double ImageConverter::d_term = 0;
-double ImageConverter::prop_gain = 0.001;
-double ImageConverter::integ_gain = 0.001;
+double ImageConverter::prop_gain = 0.01;
+double ImageConverter::integ_gain = 0.01;
double ImageConverter::deriv_gain = 0;
-double ImageConverter::control_signal = 0;
+double ImageConverter::control_signal;
ImageConverter::ImageConverter() :
it_(nh_)
@@ -106,7 +103,7 @@ double ImageConverter::control_signal = 0;
ImageConverter::~ImageConverter()
{
- nh_.shutdown();
+ //nh_.shutdown();
//destroyWindow("Preview");
}
@@ -155,6 +152,16 @@ double ImageConverter::getWobjOrien()
return orienW;
}
+double ImageConverter::getMarpose()
+{
+ return marpose[6];
+}
+
+
+double ImageConverter::getRobPose()
+{
+ return robpose[6];
+}
void ImageConverter::cvTackBarEvents(int value,void* ptr)
{
@@ -198,9 +205,6 @@ void ImageConverter::myhandler(int value)
imshow("INPUT IMAGE",TheInputImageCopy);
imshow("THRESHOLD IMAGE",MDetector.getThresholdedImage());
}
-
-
-// ----------------------------------------------------
void ImageConverter::createTrackbars()
{
namedWindow(trackbarWindowName, 0);
@@ -208,8 +212,6 @@ void ImageConverter::createTrackbars()
createTrackbar("ThresParam 2", trackbarWindowName, &Thresh2_min, Thresh2_max, cvTackBarEvents, this);
MDetector.setCornerRefinementMethod(MarkerDetector::LINES);
}
-// ----------------------------------------------------
-
void ImageConverter::imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
@@ -225,8 +227,6 @@ void ImageConverter::imageCb(const sensor_msgs::ImageConstPtr& msg)
img = cv_ptr->image;
}
-
-
bool ImageConverter::readArguments ( int argc,char **argv )
{
if (argc<2) {
@@ -294,8 +294,6 @@ 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()){
@@ -335,8 +333,6 @@ void ImageConverter::ProgStart(int argc,char** argv)
//cout << "\nR = " << R << endl;
-
-
// ----------- Euler angle -----------------//
float roll1 = -asin(R.at<float>(2,0));
@@ -378,7 +374,7 @@ void ImageConverter::ProgStart(int argc,char** argv)
// yaw = 0.0;
// roll = 0.0;
// pitch = 0.0;
- printf("\nMarker _NOT_ found\n");
+ ROS_INFO("MARKER NOT FOUND \n!");
}
// Marker rotation should be initially zero (just for convenience)
@@ -391,13 +387,12 @@ void ImageConverter::ProgStart(int argc,char** argv)
if (ros::ok() && found && stream)
{
-
ros::Time time = ros::Time::now();
// Camera Frame ---
transformCAM.setOrigin( tf::Vector3(.25, 0, .5) );
transformCAM.setRotation( tf::Quaternion(0, 0, 0 , 1) );
CAMbr.sendTransform(tf::StampedTransform(transformCAM,
- (ros::Time::now() - ros::Duration(10)),
+ (ros::Time::now()/* - ros::Duration(35)*/),
"/base_link",
"/camera"));
@@ -406,7 +401,7 @@ void ImageConverter::ProgStart(int argc,char** argv)
tf::Quaternion quat = tf::createQuaternionFromRPY(roll-r_off, pitch+p_off, yaw-y_off);
broadcaster.sendTransform(tf::StampedTransform(tf::Transform(quat,
tf::Vector3(x_t, y_t, z_t)),
- (ros::Time::now() - ros::Duration(0)),
+ (ros::Time::now()/* - ros::Duration(35)*/),
"/camera",
"marker"));
@@ -415,20 +410,18 @@ void ImageConverter::ProgStart(int argc,char** argv)
try
{
- ObjListener.waitForTransform("/map", "/marker", ros::Time(0), ros::Duration(1/60));
- ObjListener.lookupTransform("/map", "/marker", ros::Time(0), ObjPose);
-
RobListener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(1/60));
RobListener.lookupTransform("/map", "/base_link", ros::Time(0), RobPose);
+ ObjListener.waitForTransform("/map", "/marker", ros::Time(0), ros::Duration(1/60));
+ ObjListener.lookupTransform("/map", "/marker", ros::Time(0), ObjPose);
+
}
catch (tf::TransformException &ex)
{
ROS_WARN("%s",ex.what());
}
ROS_INFO("MARKER FOUND!!! ROBOT IS MOVING ... ");
- ContStart(ObjPose,RobPose);
- //ErrCal(ObjPose,RobPose);
/*printf("\nMarker Pose in Global Coordinate : Xobj = %5.4f, Yobj = %5.4f, Zobj = %5.4f \n ",
ObjPose.getOrigin().x(),
ObjPose.getOrigin().y(),
@@ -441,6 +434,33 @@ void ImageConverter::ProgStart(int argc,char** argv)
ObjPose.getRotation().w());*/
+ /*double marpose[6] = {ObjPose.getOrigin().x(),ObjPose.getOrigin().y(),ObjPose.getOrigin().z(),
+ ObjPose.getRotation().x(),ObjPose.getRotation().y(),ObjPose.getRotation().z(),ObjPose.getRotation().w()};*/
+
+ 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();
@@ -526,24 +546,43 @@ void ImageConverter::ProgStart(int argc,char** argv)
}
// ---- Controller part ----------- START -------
-
-
-
-void ImageConverter::ContStart(const tf::Transform& ObjFB,
- const tf::Transform& RobFB)
+void ImageConverter::robCB(const geometry_msgs::PoseStamped::ConstPtr& RobFB)
{
- ros::Rate rate(100);
+ ROS_INFO_STREAM(" Xrob = " << robpose[0] << " m. \n");
+ ROS_INFO_STREAM(" ObjX = " << marpose[0] << " m. \n");
+
+ ROS_INFO_STREAM(" Yrob = " << robpose[1] << " m. \n");
+ ROS_INFO_STREAM(" ObjY = " << marpose[1] << " m. \n");
+
+ ROS_INFO_STREAM(" Zrob = " << robpose[2] << " m. \n");
+ ROS_INFO_STREAM(" ObjZ = " << marpose[2] << " m. \n");
+
+ ROS_INFO_STREAM(" XorienRob = " << robpose[3] << " rad. \n");
+ ROS_INFO_STREAM(" ObjOrienX = " << marpose[3] << " rad. \n");
- while (ros::ok() && keepMoving)
+ ROS_INFO_STREAM(" YorienRob = " << robpose[4] << " rad. \n");
+ ROS_INFO_STREAM(" ObjOrienY = " << marpose[4] << " rad. \n");
+
+ ROS_INFO_STREAM(" ZorienRob = " << robpose[5] << " rad. \n");
+ ROS_INFO_STREAM(" ObjOrienZ = " << marpose[5] << " rad. \n");
+
+ ROS_INFO_STREAM(" WorienRob = " << robpose[6] << " rad. \n");
+ ROS_INFO_STREAM(" ObjOrienW = " << marpose[6] << " rad. \n");
+
+
+ for (int i = 0; i<7; i++)
{
- ros::spinOnce();
- rate.sleep();
+ if (marpose[i] >= robpose[i])
+ {
+ 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);
+ }
}
-
+
+
}
-
-
-
void ImageConverter::Controller(double Reference, double RobPose, double dt, int index)
{
// e(t) = setpoint - actual value;
@@ -576,36 +615,48 @@ void ImageConverter::Controller(double Reference, double RobPose, double dt, int
// for the next iteration.
prev_error = curr_error;
- if (index = 1)
+ //dock(control_signal);
+
+ if (index == 1)
{
dock(control_signal, 0, 0, 0, 0, 0, 0);
- } else if (index = 2)
+ } else if (index == 2)
{
dock(0, control_signal, 0, 0, 0, 0, 0);
- } else if (index = 3)
+ } else if (index == 3)
{
dock(0, 0, control_signal, 0, 0, 0, 0);
- }else if (index = 4)
+ }else if (index == 4)
{
dock(0, 0, 0, control_signal, 0, 0, 0);
- } else if (index = 5)
+ } else if (index == 5)
{
dock(0, 0, 0, 0, control_signal, 0, 0);
- } else if (index = 6)
+ } else if (index == 6)
{
dock(0, 0, 0, 0, 0, control_signal, 0);
- } /*else if (index = 7)
+ } /*else if (index == 7)
{
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)
{
geometry_msgs::Twist msg;
+ /*msg.linear.x = speed[0];
+ msg.linear.y = speed[1];
+ msg.linear.z = speed[2];
+
+ msg.angular.x = speed[3];
+ msg.angular.y = speed[4];
+ msg.angular.z = speed[5];*/
+
msg.linear.x = VelX;
msg.linear.y = VelY;
msg.linear.z = VelZ;
@@ -618,109 +669,4 @@ void ImageConverter::dock(double VelX, double VelY, double VelZ, double omegaX,
ROS_INFO_STREAM(" Current speed of robot: " << msg << " m/s \n");
}
-
-
-
-
-void ImageConverter::robCB(const geometry_msgs::PoseStamped::ConstPtr& RobFB)
-{
- ROS_INFO_STREAM(" Xrob = " << RobFB->pose.position.x << " m. \n");
- ROS_INFO_STREAM(" Yrob = " << RobFB->pose.position.y << " m. \n");
- ROS_INFO_STREAM(" Zrob = " << RobFB->pose.position.z << " m. \n");
-
- ROS_INFO_STREAM(" XorienRob = " << RobFB->pose.orientation.x << " rad. \n");
- ROS_INFO_STREAM(" YorienRob = " << RobFB->pose.orientation.y << " rad. \n");
- ROS_INFO_STREAM(" ZorienRob = " << RobFB->pose.orientation.z << " rad. \n");
- ROS_INFO_STREAM(" WorienRob = " << RobFB->pose.orientation.w << " rad. \n");
-
- if ((RobFB->pose.orientation.x >= orienX) &&
- (RobFB->pose.orientation.y >= orienY) &&
- (RobFB->pose.orientation.z >= orienZ) /*&&
- (RobFB->pose.orientation.w >= orienW)*/)
- {
- if ((RobFB->pose.position.x >= poseX) &&
- (RobFB->pose.position.y >= poseY) /*&&
- (RobFB->pose.position.z >= poseZ)*/)
- {
- ROS_INFO(" Dock is completed! \n ");
- keepMoving = false;
-
- } else if ((RobFB->pose.position.x <= poseX))
- {
- Controller(poseX,RobFB->pose.position.x,.01,1);
- ROS_INFO(" Calculating control signal For X-direction! \n ");
- } else if ((RobFB->pose.position.y <= poseY))
- {
- Controller(poseY,RobFB->pose.position.y,.01,2);
- ROS_INFO(" Calculating control signal For Y-direction! \n ");
- } /*else if ((RobFB->pose.position.x <= poseZ))
- {
- Controller(poseZ,RobFB->pose.position.z,.01,3);
- ROS_INFO(" Calculating control signal For Z-direction! \n ");
- }*/
-
- } else if (RobFB->pose.orientation.x <= orienX)
- {
- Controller(orienX,RobFB->pose.orientation.x,.01,4);
- ROS_INFO(" Calculating control signal For ROLL! \n ");
-
- } else if (RobFB->pose.orientation.y <= orienY)
- {
- Controller(orienY,RobFB->pose.orientation.y,.01,5);
- ROS_INFO(" Calculating control signal For PITCH! \n ");
-
- } else if ((RobFB->pose.orientation.z <= orienZ))
- {
- Controller(orienZ,RobFB->pose.orientation.z,.01,6);
- ROS_INFO(" Calculating control signal For YAW! \n ");
- } /*else if ((RobFB->pose.orientation.w <= orienW))
- {
- Controller(orienW,RobFB->pose.orientation.w,.01,7);
- ROS_INFO(" Calculating control signal For W! \n ");
- }*/
-}
-//void ImageConverter::marCB(const geometry_msgs::PoseStamped::ConstPtr& ObjFB)
-void ImageConverter::marCB(const tf::Transform& ObjFB)
-{
-
- /*ROS_INFO_STREAM(" Xmar = " << ObjFB->pose.position.x << " m. \n");
- ROS_INFO_STREAM(" Ymar = " << ObjFB->pose.position.y << " m. \n");
- ROS_INFO_STREAM(" Zmar = " << ObjFB->pose.position.z << " m. \n");
-
- ROS_INFO_STREAM(" XorienMar = " << ObjFB->pose.orientation.x << " rad. \n");
- ROS_INFO_STREAM(" YorienMar = " << ObjFB->pose.orientation.y << " rad. \n");
- ROS_INFO_STREAM(" ZorienMar = " << ObjFB->pose.orientation.z << " rad. \n");
- ROS_INFO_STREAM(" WorienMar = " << ObjFB->pose.orientation.w << " rad. \n");*/
-
-
-
- /*printf("\nMarker Pose in Global Coordinate : Xobj = %5.4f, Yobj = %5.4f, Zobj = %5.4f \n ",
- ObjFB.getOrigin().x(),
- ObjFB.getOrigin().y(),
- ObjFB.getOrigin().z());
-
- printf("\nMarker Orientation in Global Coordinate : RxObj = %5.4f, RyObj = %5.4f, RzObj = %5.4f, wObj = %5.4f \n",
- ObjFB.getRotation().x(),
- ObjFB.getRotation().y(),
- ObjFB.getRotation().z(),
- ObjFB.getRotation().w());*/
-}
-
-void ImageConverter::ErrCal(const tf::Transform& ObjFB,
- const tf::Transform& RobFB)
-{
- /*printf("\nPosition Error : Xobj - Xrob = %5.4f, Yobj - Yrob = %5.4f, Zobj - Zrob = %5.4f \n ",
- ObjFB.getOrigin().x() - RobFB.getOrigin().x(),
- ObjFB.getOrigin().y() - RobFB.getOrigin().y(),
- ObjFB.getOrigin().z() - RobFB.getOrigin().z());
-
- printf("\nOrientation Error : RxObj - RxRob= %5.4f, RyObj - RyRob = %5.4f, RzObj - RzRob= %5.4f, wObj - wRob = %5.4f \n",
- ObjFB.getRotation().x() - RobFB.getRotation().x(),
- ObjFB.getRotation().y() - RobFB.getRotation().y(),
- ObjFB.getRotation().z() - RobFB.getRotation().z(),
- ObjFB.getRotation().w() - RobFB.getRotation().w());*/
-
-}
-
-
// ---- Controller part ----------- END --------
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 16f2efc855c93f87c255440e3331d27842b89e64..2d20a73763ceb289ea9111d75ca959a8560b8b1e 100644
--- a/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/src/ros_aruco.cpp~
+++ b/MobileRobot/AugReaMarker/ROS_ArUco/ros_aruco/src/ros_aruco.cpp~
@@ -69,13 +69,10 @@ double ImageConverter::orienY = 0;
double ImageConverter::orienZ = 0;
double ImageConverter::orienW = 0;
-
-
-
-double ImageConverter::prev_error = 0;
-double ImageConverter::curr_error = 0;
-double ImageConverter::int_error = 0;
-double ImageConverter::diff = 0;
+double ImageConverter::prev_error;
+double ImageConverter::curr_error;
+double ImageConverter::int_error;
+double ImageConverter::diff;
double ImageConverter::p_term = 0;
double ImageConverter::i_term = 0;
@@ -85,7 +82,7 @@ double ImageConverter::prop_gain = 0.01;
double ImageConverter::integ_gain = 0.01;
double ImageConverter::deriv_gain = 0;
-double ImageConverter::control_signal = 0;
+double ImageConverter::control_signal;
ImageConverter::ImageConverter() :
it_(nh_)
@@ -106,7 +103,7 @@ double ImageConverter::control_signal = 0;
ImageConverter::~ImageConverter()
{
- nh_.shutdown();
+ //nh_.shutdown();
//destroyWindow("Preview");
}
@@ -155,6 +152,16 @@ double ImageConverter::getWobjOrien()
return orienW;
}
+double ImageConverter::getMarpose()
+{
+ return marpose[6];
+}
+
+
+double ImageConverter::getRobPose()
+{
+ return robpose[6];
+}
void ImageConverter::cvTackBarEvents(int value,void* ptr)
{
@@ -198,9 +205,6 @@ void ImageConverter::myhandler(int value)
imshow("INPUT IMAGE",TheInputImageCopy);
imshow("THRESHOLD IMAGE",MDetector.getThresholdedImage());
}
-
-
-// ----------------------------------------------------
void ImageConverter::createTrackbars()
{
namedWindow(trackbarWindowName, 0);
@@ -208,8 +212,6 @@ void ImageConverter::createTrackbars()
createTrackbar("ThresParam 2", trackbarWindowName, &Thresh2_min, Thresh2_max, cvTackBarEvents, this);
MDetector.setCornerRefinementMethod(MarkerDetector::LINES);
}
-// ----------------------------------------------------
-
void ImageConverter::imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
@@ -225,8 +227,6 @@ void ImageConverter::imageCb(const sensor_msgs::ImageConstPtr& msg)
img = cv_ptr->image;
}
-
-
bool ImageConverter::readArguments ( int argc,char **argv )
{
if (argc<2) {
@@ -294,8 +294,6 @@ 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()){
@@ -335,8 +333,6 @@ void ImageConverter::ProgStart(int argc,char** argv)
//cout << "\nR = " << R << endl;
-
-
// ----------- Euler angle -----------------//
float roll1 = -asin(R.at<float>(2,0));
@@ -378,7 +374,7 @@ void ImageConverter::ProgStart(int argc,char** argv)
// yaw = 0.0;
// roll = 0.0;
// pitch = 0.0;
- printf("\nMarker _NOT_ found\n");
+ ROS_INFO("MARKER NOT FOUND \n!");
}
// Marker rotation should be initially zero (just for convenience)
@@ -391,13 +387,12 @@ void ImageConverter::ProgStart(int argc,char** argv)
if (ros::ok() && found && stream)
{
-
ros::Time time = ros::Time::now();
// Camera Frame ---
transformCAM.setOrigin( tf::Vector3(.25, 0, .5) );
transformCAM.setRotation( tf::Quaternion(0, 0, 0 , 1) );
CAMbr.sendTransform(tf::StampedTransform(transformCAM,
- (ros::Time::now() - ros::Duration(10)),
+ (ros::Time::now()/* - ros::Duration(35)*/),
"/base_link",
"/camera"));
@@ -406,7 +401,7 @@ void ImageConverter::ProgStart(int argc,char** argv)
tf::Quaternion quat = tf::createQuaternionFromRPY(roll-r_off, pitch+p_off, yaw-y_off);
broadcaster.sendTransform(tf::StampedTransform(tf::Transform(quat,
tf::Vector3(x_t, y_t, z_t)),
- (ros::Time::now() - ros::Duration(0)),
+ (ros::Time::now()/* - ros::Duration(35)*/),
"/camera",
"marker"));
@@ -415,20 +410,18 @@ void ImageConverter::ProgStart(int argc,char** argv)
try
{
- ObjListener.waitForTransform("/map", "/marker", ros::Time(0), ros::Duration(1/60));
- ObjListener.lookupTransform("/map", "/marker", ros::Time(0), ObjPose);
-
RobListener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(1/60));
RobListener.lookupTransform("/map", "/base_link", ros::Time(0), RobPose);
+ ObjListener.waitForTransform("/map", "/marker", ros::Time(0), ros::Duration(1/60));
+ ObjListener.lookupTransform("/map", "/marker", ros::Time(0), ObjPose);
+
}
catch (tf::TransformException &ex)
{
ROS_WARN("%s",ex.what());
}
ROS_INFO("MARKER FOUND!!! ROBOT IS MOVING ... ");
- ContStart(ObjPose,RobPose);
- //ErrCal(ObjPose,RobPose);
/*printf("\nMarker Pose in Global Coordinate : Xobj = %5.4f, Yobj = %5.4f, Zobj = %5.4f \n ",
ObjPose.getOrigin().x(),
ObjPose.getOrigin().y(),
@@ -441,6 +434,33 @@ void ImageConverter::ProgStart(int argc,char** argv)
ObjPose.getRotation().w());*/
+ /*double marpose[6] = {ObjPose.getOrigin().x(),ObjPose.getOrigin().y(),ObjPose.getOrigin().z(),
+ ObjPose.getRotation().x(),ObjPose.getRotation().y(),ObjPose.getRotation().z(),ObjPose.getRotation().w()};*/
+
+ 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();
@@ -526,24 +546,43 @@ void ImageConverter::ProgStart(int argc,char** argv)
}
// ---- Controller part ----------- START -------
-
-
-
-void ImageConverter::ContStart(const tf::Transform& ObjFB,
- const tf::Transform& RobFB)
+void ImageConverter::robCB(const geometry_msgs::PoseStamped::ConstPtr& RobFB)
{
- ros::Rate rate(100);
+ ROS_INFO_STREAM(" Xrob = " << robpose[0] << " m. \n");
+ ROS_INFO_STREAM(" ObjX = " << marpose[0] << " m. \n");
+
+ ROS_INFO_STREAM(" Yrob = " << robpose[1] << " m. \n");
+ ROS_INFO_STREAM(" ObjY = " << marpose[1] << " m. \n");
+
+ ROS_INFO_STREAM(" Zrob = " << robpose[2] << " m. \n");
+ ROS_INFO_STREAM(" ObjZ = " << marpose[2] << " m. \n");
+
+ ROS_INFO_STREAM(" XorienRob = " << robpose[3] << " rad. \n");
+ ROS_INFO_STREAM(" ObjOrienX = " << marpose[3] << " rad. \n");
- while (ros::ok() && keepMoving)
+ ROS_INFO_STREAM(" YorienRob = " << robpose[4] << " rad. \n");
+ ROS_INFO_STREAM(" ObjOrienY = " << marpose[4] << " rad. \n");
+
+ ROS_INFO_STREAM(" ZorienRob = " << robpose[5] << " rad. \n");
+ ROS_INFO_STREAM(" ObjOrienZ = " << marpose[5] << " rad. \n");
+
+ ROS_INFO_STREAM(" WorienRob = " << robpose[6] << " rad. \n");
+ ROS_INFO_STREAM(" ObjOrienW = " << marpose[6] << " rad. \n");
+
+
+ for (int i = 0; i<7; i++)
{
- ros::spinOnce();
- rate.sleep();
+ if (marpose[i] >= robpose[i])
+ {
+ 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);
+ }
}
-
+
+
}
-
-
-
void ImageConverter::Controller(double Reference, double RobPose, double dt, int index)
{
// e(t) = setpoint - actual value;
@@ -576,36 +615,48 @@ void ImageConverter::Controller(double Reference, double RobPose, double dt, int
// for the next iteration.
prev_error = curr_error;
- if (index = 1)
+ //dock(control_signal);
+
+ if (index == 1)
{
dock(control_signal, 0, 0, 0, 0, 0, 0);
- } else if (index = 2)
+ } else if (index == 2)
{
dock(0, control_signal, 0, 0, 0, 0, 0);
- } else if (index = 3)
+ } else if (index == 3)
{
dock(0, 0, control_signal, 0, 0, 0, 0);
- }else if (index = 4)
+ }else if (index == 4)
{
dock(0, 0, 0, control_signal, 0, 0, 0);
- } else if (index = 5)
+ } else if (index == 5)
{
dock(0, 0, 0, 0, control_signal, 0, 0);
- } else if (index = 6)
+ } else if (index == 6)
{
dock(0, 0, 0, 0, 0, control_signal, 0);
- } /*else if (index = 7)
+ } /*else if (index == 7)
{
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)
{
geometry_msgs::Twist msg;
+ /*msg.linear.x = speed[0];
+ msg.linear.y = speed[1];
+ msg.linear.z = speed[2];
+
+ msg.angular.x = speed[3];
+ msg.angular.y = speed[4];
+ msg.angular.z = speed[5];*/
+
msg.linear.x = VelX;
msg.linear.y = VelY;
msg.linear.z = VelZ;
@@ -618,109 +669,4 @@ void ImageConverter::dock(double VelX, double VelY, double VelZ, double omegaX,
ROS_INFO_STREAM(" Current speed of robot: " << msg << " m/s \n");
}
-
-
-
-
-void ImageConverter::robCB(const geometry_msgs::PoseStamped::ConstPtr& RobFB)
-{
- ROS_INFO_STREAM(" Xrob = " << RobFB->pose.position.x << " m. \n");
- ROS_INFO_STREAM(" Yrob = " << RobFB->pose.position.y << " m. \n");
- ROS_INFO_STREAM(" Zrob = " << RobFB->pose.position.z << " m. \n");
-
- ROS_INFO_STREAM(" XorienRob = " << RobFB->pose.orientation.x << " rad. \n");
- ROS_INFO_STREAM(" YorienRob = " << RobFB->pose.orientation.y << " rad. \n");
- ROS_INFO_STREAM(" ZorienRob = " << RobFB->pose.orientation.z << " rad. \n");
- ROS_INFO_STREAM(" WorienRob = " << RobFB->pose.orientation.w << " rad. \n");
-
- if ((RobFB->pose.orientation.x >= orienX) &&
- (RobFB->pose.orientation.y >= orienY) &&
- (RobFB->pose.orientation.z >= orienZ) /*&&
- (RobFB->pose.orientation.w >= orienW)*/)
- {
- if ((RobFB->pose.position.x >= poseX) &&
- (RobFB->pose.position.y >= poseY) /*&&
- (RobFB->pose.position.z >= poseZ)*/)
- {
- ROS_INFO(" Dock is completed! \n ");
- keepMoving = false;
-
- } else if ((RobFB->pose.position.x <= poseX))
- {
- Controller(poseX,RobFB->pose.position.x,.01,1);
- ROS_INFO(" Calculating control signal For X-direction! \n ");
- } else if ((RobFB->pose.position.y <= poseY))
- {
- Controller(poseY,RobFB->pose.position.y,.01,2);
- ROS_INFO(" Calculating control signal For Y-direction! \n ");
- } /*else if ((RobFB->pose.position.x <= poseZ))
- {
- Controller(poseZ,RobFB->pose.position.z,.01,3);
- ROS_INFO(" Calculating control signal For Z-direction! \n ");
- }*/
-
- } else if (RobFB->pose.orientation.x <= orienX)
- {
- Controller(orienX,RobFB->pose.orientation.x,.01,4);
- ROS_INFO(" Calculating control signal For ROLL! \n ");
-
- } else if (RobFB->pose.orientation.y <= orienY)
- {
- Controller(orienY,RobFB->pose.orientation.y,.01,5);
- ROS_INFO(" Calculating control signal For PITCH! \n ");
-
- } else if ((RobFB->pose.orientation.z <= orienZ))
- {
- Controller(orienZ,RobFB->pose.orientation.z,.01,6);
- ROS_INFO(" Calculating control signal For YAW! \n ");
- } /*else if ((RobFB->pose.orientation.w <= orienW))
- {
- Controller(orienW,RobFB->pose.orientation.w,.01,7);
- ROS_INFO(" Calculating control signal For W! \n ");
- }*/
-}
-//void ImageConverter::marCB(const geometry_msgs::PoseStamped::ConstPtr& ObjFB)
-void ImageConverter::marCB(const tf::Transform& ObjFB)
-{
-
- /*ROS_INFO_STREAM(" Xmar = " << ObjFB->pose.position.x << " m. \n");
- ROS_INFO_STREAM(" Ymar = " << ObjFB->pose.position.y << " m. \n");
- ROS_INFO_STREAM(" Zmar = " << ObjFB->pose.position.z << " m. \n");
-
- ROS_INFO_STREAM(" XorienMar = " << ObjFB->pose.orientation.x << " rad. \n");
- ROS_INFO_STREAM(" YorienMar = " << ObjFB->pose.orientation.y << " rad. \n");
- ROS_INFO_STREAM(" ZorienMar = " << ObjFB->pose.orientation.z << " rad. \n");
- ROS_INFO_STREAM(" WorienMar = " << ObjFB->pose.orientation.w << " rad. \n");*/
-
-
-
- /*printf("\nMarker Pose in Global Coordinate : Xobj = %5.4f, Yobj = %5.4f, Zobj = %5.4f \n ",
- ObjFB.getOrigin().x(),
- ObjFB.getOrigin().y(),
- ObjFB.getOrigin().z());
-
- printf("\nMarker Orientation in Global Coordinate : RxObj = %5.4f, RyObj = %5.4f, RzObj = %5.4f, wObj = %5.4f \n",
- ObjFB.getRotation().x(),
- ObjFB.getRotation().y(),
- ObjFB.getRotation().z(),
- ObjFB.getRotation().w());*/
-}
-
-void ImageConverter::ErrCal(const tf::Transform& ObjFB,
- const tf::Transform& RobFB)
-{
- /*printf("\nPosition Error : Xobj - Xrob = %5.4f, Yobj - Yrob = %5.4f, Zobj - Zrob = %5.4f \n ",
- ObjFB.getOrigin().x() - RobFB.getOrigin().x(),
- ObjFB.getOrigin().y() - RobFB.getOrigin().y(),
- ObjFB.getOrigin().z() - RobFB.getOrigin().z());
-
- printf("\nOrientation Error : RxObj - RxRob= %5.4f, RyObj - RyRob = %5.4f, RzObj - RzRob= %5.4f, wObj - wRob = %5.4f \n",
- ObjFB.getRotation().x() - RobFB.getRotation().x(),
- ObjFB.getRotation().y() - RobFB.getRotation().y(),
- ObjFB.getRotation().z() - RobFB.getRotation().z(),
- ObjFB.getRotation().w() - RobFB.getRotation().w());*/
-
-}
-
-
// ---- Controller part ----------- END --------
diff --git a/MobileRobot/Controller/ARController/ar_with_controller/src/dock_with_ar.cpp b/MobileRobot/Controller/ARController/ar_with_controller/src/dock_with_ar.cpp
index a42035994d1e89daec11670db78f3b566c6299e7..7a70092012af0d6e4a130a556a9b862cf62df979 100644
--- a/MobileRobot/Controller/ARController/ar_with_controller/src/dock_with_ar.cpp
+++ b/MobileRobot/Controller/ARController/ar_with_controller/src/dock_with_ar.cpp
@@ -66,7 +66,6 @@ void RobotDocking::Controller(double Reference, double RobPose, double dt)
else if ()
{}
*/
-
// differentiation
diff = ((curr_error - prev_error) / dt);
diff --git a/MobileRobot/Controller/ARController/ar_with_controller/src/dock_with_ar.cpp~ b/MobileRobot/Controller/ARController/ar_with_controller/src/dock_with_ar.cpp~
index 1037fbde8ace10463a463bc1073f3428bd835fc0..a42035994d1e89daec11670db78f3b566c6299e7 100644
--- a/MobileRobot/Controller/ARController/ar_with_controller/src/dock_with_ar.cpp~
+++ b/MobileRobot/Controller/ARController/ar_with_controller/src/dock_with_ar.cpp~
@@ -34,7 +34,7 @@ RobotDocking::RobotDocking() : ac("Robot_move", true)
commandPub = node.advertise<geometry_msgs::Twist>("/base_controller/command",1);
RobSub = node.subscribe("/visualize_pose_ekf",1,&RobotDocking::robCB,this);
- marSub = node.subscriber("/tf",1,&RobotDocking::marCB,this)
+ marSub = node.subscriber("/tf",1,&RobotDocking::marCB,this);
}
diff --git a/MobileRobot/Controller/PID/build/dock/CMakeFiles/dock.dir/src/dock.cpp.o b/MobileRobot/Controller/PID/build/dock/CMakeFiles/dock.dir/src/dock.cpp.o
index 5d38e63773cb3073898812efcb3ed3a1abdc4bb5..31ac88b67f528d495e42b531e574dc483d2d90e9 100644
Binary files a/MobileRobot/Controller/PID/build/dock/CMakeFiles/dock.dir/src/dock.cpp.o and b/MobileRobot/Controller/PID/build/dock/CMakeFiles/dock.dir/src/dock.cpp.o differ
diff --git a/MobileRobot/Controller/PID/devel/lib/dock/dock b/MobileRobot/Controller/PID/devel/lib/dock/dock
index 875071042493781e89a61aff2314913bc9cfdd1c..10e309c19704a2c2f8da5500623f67c287702575 100755
Binary files a/MobileRobot/Controller/PID/devel/lib/dock/dock and b/MobileRobot/Controller/PID/devel/lib/dock/dock differ
diff --git a/MobileRobot/Controller/PID/dock/src/dock.cpp b/MobileRobot/Controller/PID/dock/src/dock.cpp
index e82a44d2c7650e0a664c9a952f3789b57274fae5..0f13003ee24701b7e86e24735e7e63ce382613cd 100644
--- a/MobileRobot/Controller/PID/dock/src/dock.cpp
+++ b/MobileRobot/Controller/PID/dock/src/dock.cpp
@@ -7,7 +7,7 @@
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
-double RobotDocking::refVal = 3; // add a ref. value
+double RobotDocking::refVal = .81; // add a ref. value
double RobotDocking::FORWARD_SPEED_MPS = .05;
@@ -89,7 +89,8 @@ void RobotDocking::Controller(double Reference, double RobPose, double dt)
void RobotDocking::dock(double speed)
{
geometry_msgs::Twist msg;
- msg.linear.x = speed;
+ //msg.linear.x = speed;
+ msg.angular.z = speed;
commandPub.publish(msg);
ROS_INFO_STREAM(" Current speed of robot: " << speed << " m/s \n");
@@ -109,14 +110,14 @@ void RobotDocking::RobCurrPose(const geometry_msgs::PoseStamped::ConstPtr& feedb
ROS_INFO_STREAM(" Worien = " << feedback->pose.orientation.w << " rad. \n");
- if (feedback->pose.position.x >= refVal)
+ if (feedback->pose.orientation.z >= refVal)
{
ROS_INFO(" Dock is completed! \n ");
keepMoving = false;
}
else
{
- Controller(refVal,feedback->pose.position.x,.01);
+ Controller(refVal,feedback->pose.orientation.z,.01);
//ROS_INFO(" Calculating control signal! \n ");
}
diff --git a/MobileRobot/Controller/PID/dock/src/dock.cpp~ b/MobileRobot/Controller/PID/dock/src/dock.cpp~
index 673eaf24a98bc6801bcb6ac2f9bf0e3a1b630b57..0f13003ee24701b7e86e24735e7e63ce382613cd 100644
--- a/MobileRobot/Controller/PID/dock/src/dock.cpp~
+++ b/MobileRobot/Controller/PID/dock/src/dock.cpp~
@@ -7,7 +7,7 @@
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
-double RobotDocking::refVal = 3; // add a ref. value
+double RobotDocking::refVal = .81; // add a ref. value
double RobotDocking::FORWARD_SPEED_MPS = .05;
@@ -34,6 +34,7 @@ RobotDocking::RobotDocking() : ac("Robot_move", true)
commandPub = node.advertise<geometry_msgs::Twist>("/base_controller/command",1);
PosSub = node.subscribe("/visualize_pose_ekf",1,&RobotDocking::RobCurrPose,this);
+
}
@@ -88,7 +89,8 @@ void RobotDocking::Controller(double Reference, double RobPose, double dt)
void RobotDocking::dock(double speed)
{
geometry_msgs::Twist msg;
- msg.linear.x = speed;
+ //msg.linear.x = speed;
+ msg.angular.z = speed;
commandPub.publish(msg);
ROS_INFO_STREAM(" Current speed of robot: " << speed << " m/s \n");
@@ -108,14 +110,14 @@ void RobotDocking::RobCurrPose(const geometry_msgs::PoseStamped::ConstPtr& feedb
ROS_INFO_STREAM(" Worien = " << feedback->pose.orientation.w << " rad. \n");
- if (feedback->pose.position.x >= refVal)
+ if (feedback->pose.orientation.z >= refVal)
{
ROS_INFO(" Dock is completed! \n ");
keepMoving = false;
}
else
{
- Controller(refVal,feedback->pose.position.x,.01);
+ Controller(refVal,feedback->pose.orientation.z,.01);
//ROS_INFO(" Calculating control signal! \n ");
}