Skip to content
Snippets Groups Projects
Commit 63871bca authored by Farid Alijani's avatar Farid Alijani
Browse files

controller

parent 35594315
Branches
No related tags found
No related merge requests found
Showing
with 234 additions and 350 deletions
No preview for this file type
No preview for this file type
No preview for this file type
......@@ -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,6 +138,8 @@ public:
static double integ_gain;
static double deriv_gain;
double marpose[6];
double robpose[6];
double getXobjPos();
double getYobjPos();
......@@ -158,4 +150,7 @@ double getYobjOrien();
double getZobjOrien();
double getWobjOrien();
double getMarpose();
double getRobPose();
};
......@@ -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 marpose[6];
double robpose[6];
double getXobjPos();
double getXobjPos();
double getXobjPos();
double getYobjPos();
double getZobjPos();
double getXobjOrien();
double getYobjOrien();
double getZobjOrien();
double getWobjOrien();
double getMarpose();
double getRobPose();
};
......@@ -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::robCB(const geometry_msgs::PoseStamped::ConstPtr& RobFB)
{
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");
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");
void ImageConverter::ContStart(const tf::Transform& ObjFB,
const tf::Transform& RobFB)
{
ros::Rate rate(100);
while (ros::ok() && keepMoving)
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 --------
......@@ -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::robCB(const geometry_msgs::PoseStamped::ConstPtr& RobFB)
{
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");
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");
void ImageConverter::ContStart(const tf::Transform& ObjFB,
const tf::Transform& RobFB)
{
ros::Rate rate(100);
while (ros::ok() && keepMoving)
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 --------
......@@ -66,7 +66,6 @@ void RobotDocking::Controller(double Reference, double RobPose, double dt)
else if ()
{}
*/
// differentiation
diff = ((curr_error - prev_error) / dt);
......
......@@ -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);
}
......
No preview for this file type
No preview for this file type
......@@ -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 ");
}
......
......@@ -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;
......@@ -35,6 +35,7 @@ RobotDocking::RobotDocking() : ac("Robot_move", true)
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 ");
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment