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

camera has changed

parent 3d2ef225
Branches
Tags
No related merge requests found
Showing
with 460 additions and 543 deletions
...@@ -77,6 +77,14 @@ math.h ...@@ -77,6 +77,14 @@ math.h
- -
unistd.h unistd.h
- -
cstdlib
-
stdio.h
-
stdlib.h
-
time.h
-
aruco/aruco.h aruco/aruco.h
- -
aruco/cvdrawingutils.h aruco/cvdrawingutils.h
......
No preview for this file type
No preview for this file type
...@@ -36,20 +36,18 @@ using namespace cv; ...@@ -36,20 +36,18 @@ using namespace cv;
using namespace aruco; using namespace aruco;
using namespace std; using namespace std;
class ImageConverter class VisCont
{ {
private: private:
ros::NodeHandle nh_; ros::NodeHandle node_vis;
image_transport::ImageTransport it_; ros::NodeHandle node_cont;
image_transport::Subscriber image_sub_;
ros::Publisher commandPub;
ros::Subscriber Sub; ros::Publisher commandPub;
ros::Subscriber marSub;
ros::Subscriber MarPose_Sub;
ros::Publisher MarPose_pub; ros::Publisher MarPose_pub;
ros::Publisher CamPose_pub;
Mat img; Mat img;
string TheInputVideo; string TheInputVideo;
...@@ -85,7 +83,6 @@ private: ...@@ -85,7 +83,6 @@ private:
static int ThePyrDownLevel; static int ThePyrDownLevel;
static float TheMarkerSize; static float TheMarkerSize;
void ContStart();
static bool update_images,found,Go2RandomPose; static bool update_images,found,Go2RandomPose;
bool readArguments ( int argc,char **argv ); bool readArguments ( int argc,char **argv );
...@@ -101,9 +98,9 @@ private: ...@@ -101,9 +98,9 @@ private:
public: public:
ImageConverter(); VisCont();
~ImageConverter(); ~VisCont();
Mat getCurrentImage(); Mat getCurrentImage();
...@@ -122,6 +119,8 @@ public: ...@@ -122,6 +119,8 @@ public:
void GenerateRandomVal(); void GenerateRandomVal();
void RandomPose(double X_rand, double Y_rand, double theta_rand); void RandomPose(double X_rand, double Y_rand, double theta_rand);
void ContStart();
static double Kp_x,Ki_x,Kd_x; static double Kp_x,Ki_x,Kd_x;
static double Kp_theta,Ki_theta,Kd_theta; static double Kp_theta,Ki_theta,Kd_theta;
...@@ -199,6 +198,8 @@ public: ...@@ -199,6 +198,8 @@ public:
static double docking_counter; static double docking_counter;
static double dock_started,dock_finished,docking_duration; static double dock_started,dock_finished,docking_duration;
static double TT_S,TT_E;
static double zeroMin,zeroMax; static double zeroMin,zeroMax;
static double Py_eps,Pz_eps,A_eps; static double Py_eps,Pz_eps,A_eps;
static double x_thresh_undock,y_thresh_undock,theta_thresh_undock; static double x_thresh_undock,y_thresh_undock,theta_thresh_undock;
......
...@@ -175,8 +175,8 @@ public: ...@@ -175,8 +175,8 @@ public:
static double alpha; static double alpha;
static double R_step; static double R_step;
static double R[5][5]; static double R[7][7];
static double Q[5][5]; static double Q[7][7];
static double Q_curr_state,Q_next_state; static double Q_curr_state,Q_next_state;
static double reward; static double reward;
......
...@@ -3,12 +3,13 @@ ...@@ -3,12 +3,13 @@
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
ros::init(argc, argv, "aruco_tf_publisher"); ros::init(argc, argv, "aruco_tf_publisher");
ImageConverter imgConv; VisCont imgConv;
imgConv.ProgStart(argc,argv); imgConv.ProgStart(argc,argv);
return 0; return 0;
......
#include <iostream>
#include<VisionControl.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "aruco_tf_publisher");
VisCont imgConv;
imgConv.ProgStart(argc,argv);
return 0;
}
No preview for this file type
...@@ -13,10 +13,11 @@ ...@@ -13,10 +13,11 @@
<!-- The input to use for calibration. <!-- The input to use for calibration.
To use an input camera -> give the ID of the camera, like "1" To use an input camera -> give the ID of the camera, like "1"
To use an input camera with IP address -> give the ID of the camera, like "http://192.168.0.101:8080/video?x.mjpeg"
To use an input video -> give the path of the input video, like "/tmp/x.avi" To use an input video -> give the path of the input video, like "/tmp/x.avi"
To use an image list -> give the path to the XML or YAML file containing the list of the images, like "/tmp/circles_list.xml" To use an image list -> give the path to the XML or YAML file containing the list of the images, like "/tmp/circles_list.xml"
--> -->
<Input>"http://192.168.0.101:8080/video?x.mjpeg"</Input> <Input>"0"</Input>
<!-- <Input>"images/CameraCalibraation/VID5/VID5.xml"</Input> --> <!-- <Input>"images/CameraCalibraation/VID5/VID5.xml"</Input> -->
<!-- If true (non-zero) we flip the input images around the horizontal axis.--> <!-- If true (non-zero) we flip the input images around the horizontal axis.-->
...@@ -36,7 +37,7 @@ ...@@ -36,7 +37,7 @@
<Calibrate_FixPrincipalPointAtTheCenter> 1 </Calibrate_FixPrincipalPointAtTheCenter> <Calibrate_FixPrincipalPointAtTheCenter> 1 </Calibrate_FixPrincipalPointAtTheCenter>
<!-- The name of the output log file. --> <!-- The name of the output log file. -->
<Write_outputFileName>"Xperia_calibrated_data.yml"</Write_outputFileName> <Write_outputFileName>"Logitech_C920_Calibration_file.yml"</Write_outputFileName>
<!-- If true (non-zero) we write to the output file the feature points.--> <!-- If true (non-zero) we write to the output file the feature points.-->
<Write_DetectedFeaturePoints>1</Write_DetectedFeaturePoints> <Write_DetectedFeaturePoints>1</Write_DetectedFeaturePoints>
<!-- If true (non-zero) we write to the output file the extrinsic camera parameters.--> <!-- If true (non-zero) we write to the output file the extrinsic camera parameters.-->
......
...@@ -13,10 +13,11 @@ ...@@ -13,10 +13,11 @@
<!-- The input to use for calibration. <!-- The input to use for calibration.
To use an input camera -> give the ID of the camera, like "1" To use an input camera -> give the ID of the camera, like "1"
To use an input camera with IP address -> give the ID of the camera, like "http://192.168.0.101:8080/video?x.mjpeg"
To use an input video -> give the path of the input video, like "/tmp/x.avi" To use an input video -> give the path of the input video, like "/tmp/x.avi"
To use an image list -> give the path to the XML or YAML file containing the list of the images, like "/tmp/circles_list.xml" To use an image list -> give the path to the XML or YAML file containing the list of the images, like "/tmp/circles_list.xml"
--> -->
<Input>"http://192.168.0.101:8080/video?x.mjpeg"</Input> <Input>"0"</Input>
<!-- <Input>"images/CameraCalibraation/VID5/VID5.xml"</Input> --> <!-- <Input>"images/CameraCalibraation/VID5/VID5.xml"</Input> -->
<!-- If true (non-zero) we flip the input images around the horizontal axis.--> <!-- If true (non-zero) we flip the input images around the horizontal axis.-->
...@@ -36,7 +37,7 @@ ...@@ -36,7 +37,7 @@
<Calibrate_FixPrincipalPointAtTheCenter> 1 </Calibrate_FixPrincipalPointAtTheCenter> <Calibrate_FixPrincipalPointAtTheCenter> 1 </Calibrate_FixPrincipalPointAtTheCenter>
<!-- The name of the output log file. --> <!-- The name of the output log file. -->
<Write_outputFileName>"Android_calibrated_data.yml"</Write_outputFileName> <Write_outputFileName>"Logitech_C920_Calibration_file.yml"</Write_outputFileName>
<!-- If true (non-zero) we write to the output file the feature points.--> <!-- If true (non-zero) we write to the output file the feature points.-->
<Write_DetectedFeaturePoints>1</Write_DetectedFeaturePoints> <Write_DetectedFeaturePoints>1</Write_DetectedFeaturePoints>
<!-- If true (non-zero) we write to the output file the extrinsic camera parameters.--> <!-- If true (non-zero) we write to the output file the extrinsic camera parameters.-->
......
...@@ -3,6 +3,8 @@ ...@@ -3,6 +3,8 @@
#include <actionlib/client/simple_action_client.h> #include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> Client; typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> Client;
...@@ -19,7 +21,7 @@ public: ...@@ -19,7 +21,7 @@ public:
static double FORWARD_SPEED_MPS; static double FORWARD_SPEED_MPS;
static double refVal; static double refVal;
static double robPose;
static double prev_error; static double prev_error;
static double int_error; static double int_error;
...@@ -45,6 +47,8 @@ private: ...@@ -45,6 +47,8 @@ private:
ros::NodeHandle node; ros::NodeHandle node;
ros::Publisher commandPub; ros::Publisher commandPub;
ros::Publisher OdomPub;
ros::Subscriber PosSub; ros::Subscriber PosSub;
Client ac; Client ac;
...@@ -53,7 +57,7 @@ private: ...@@ -53,7 +57,7 @@ private:
void dock(double speed); void dock(double speed);
void RobCurrPose(const geometry_msgs::PoseStamped::ConstPtr& feedback); void RobCurrPose(const nav_msgs::Odometry::ConstPtr& feedback);
void Controller(double Reference, double RobPose, double dt); void Controller(double Reference, double RobPose, double dt);
......
...@@ -18,8 +18,6 @@ public: ...@@ -18,8 +18,6 @@ public:
static double FORWARD_SPEED_MPS; static double FORWARD_SPEED_MPS;
static double robpose;
static double refVal; static double refVal;
static double prev_error; static double prev_error;
......
...@@ -6,10 +6,27 @@ ...@@ -6,10 +6,27 @@
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
double RobotDocking::refVal = .81; // add a ref. value
double RobotDocking::FORWARD_SPEED_MPS = .05; #include "opencv2/core/core_c.h"
#include "opencv2/core/core.hpp"
#include "opencv2/flann/miniflann.hpp"
#include "opencv2/imgproc/imgproc_c.h"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/video/video.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/ml/ml.hpp"
#include "opencv2/highgui/highgui_c.h"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/contrib/contrib.hpp"
double RobotDocking::refVal = .52/*(CV_PI/180) * 45*/; // 1 deg. for ref. value
double RobotDocking::robPose /*= (CV_PI/180) * 1*/; // 1 deg.; // Robot Current Position
double RobotDocking::FORWARD_SPEED_MPS = .01;
double RobotDocking::prev_error = 0; double RobotDocking::prev_error = 0;
double RobotDocking::curr_error = 0; double RobotDocking::curr_error = 0;
...@@ -20,8 +37,8 @@ double RobotDocking::p_term = 0; ...@@ -20,8 +37,8 @@ double RobotDocking::p_term = 0;
double RobotDocking::i_term = 0; double RobotDocking::i_term = 0;
double RobotDocking::d_term = 0; double RobotDocking::d_term = 0;
double RobotDocking::prop_gain = 0.1; double RobotDocking::prop_gain = 0.0001;
double RobotDocking::integ_gain = 0.1; double RobotDocking::integ_gain = 0.01;
double RobotDocking::deriv_gain = 0; double RobotDocking::deriv_gain = 0;
double RobotDocking::control_signal = 0; double RobotDocking::control_signal = 0;
...@@ -32,8 +49,10 @@ RobotDocking::RobotDocking() : ac("Robot_move", true) ...@@ -32,8 +49,10 @@ RobotDocking::RobotDocking() : ac("Robot_move", true)
keepMoving = true; keepMoving = true;
commandPub = node.advertise<geometry_msgs::Twist>("/base_controller/command",1); commandPub = node.advertise<geometry_msgs::Twist>("/base_controller/command",1);
OdomPub = node.advertise<nav_msgs::Odometry>("/base_controller/odometry",1);
//PosSub = node.subscribe("/visualize_pose_ekf",1,&RobotDocking::RobCurrPose,this); //PosSub = node.subscribe("/visualize_pose_ekf",1,&RobotDocking::RobCurrPose,this);
PosSub = node.subscribe("/base_controller/odometry",1,&RobotDocking::RobCurrPose,this);
} }
...@@ -44,7 +63,9 @@ void RobotDocking::ProgStart() ...@@ -44,7 +63,9 @@ void RobotDocking::ProgStart()
while (ros::ok() && keepMoving) while (ros::ok() && keepMoving)
{ {
dock(FORWARD_SPEED_MPS); //dock(FORWARD_SPEED_MPS);
Controller(refVal,robPose,.01);
ros::spinOnce(); ros::spinOnce();
rate.sleep(); rate.sleep();
} }
...@@ -82,41 +103,63 @@ void RobotDocking::Controller(double Reference, double RobPose, double dt) ...@@ -82,41 +103,63 @@ void RobotDocking::Controller(double Reference, double RobPose, double dt)
// save the current error as the previous one // save the current error as the previous one
// for the next iteration. // for the next iteration.
prev_error = curr_error; prev_error = curr_error;
//ROS_INFO_STREAM("RAW Control signalYAW = "<< control_signal <<". \n");
ROS_INFO_STREAM(" ------------------------------------------------------------- \n");
dock(control_signal); dock(control_signal);
} }
void RobotDocking::dock(double speed) void RobotDocking::dock(double speed)
{ {
geometry_msgs::Twist msg; geometry_msgs::Twist msg;
msg.linear.x = speed; msg.linear.x = speed;
//msg.angular.z = speed; //msg.angular.z = speed;
commandPub.publish(msg); commandPub.publish(msg);
ROS_INFO_STREAM(" Current speed of robot: " << speed << " m/s \n"); ROS_INFO_STREAM(" Current speed of robot: " << msg << " m/s \n");
/*if(robPose >= refVal) // it moves forever.. cuz robot pose is a cte value that never achieved!!!!
{
ROS_INFO(" Dock is completed! \n ");
keepMoving = false;
} else
{
geometry_msgs::Twist msg;
msg.linear.x = speed;
//msg.angular.z = speed;
commandPub.publish(msg);
ROS_INFO_STREAM(" Current speed of robot: " << msg << " m/s \n");
}*/
} }
void RobotDocking::RobCurrPose(const geometry_msgs::PoseStamped::ConstPtr& feedback) void RobotDocking::RobCurrPose(const nav_msgs::Odometry::ConstPtr& feedback)
{ {
ROS_INFO_STREAM(" X = " << feedback->pose.position.x << " m. \n"); ROS_INFO_STREAM(" X = " << feedback->pose.pose.position.x << " m. \n");
ROS_INFO_STREAM(" Y = " << feedback->pose.position.y << " m. \n"); /*ROS_INFO_STREAM(" Y = " << feedback->pose.position.y << " m. \n");
ROS_INFO_STREAM(" Z = " << feedback->pose.position.z << " m. \n"); ROS_INFO_STREAM(" Z = " << feedback->pose.position.z << " m. \n");
ROS_INFO_STREAM(" Xorien = " << feedback->pose.orientation.x << " rad. \n"); ROS_INFO_STREAM(" Xorien = " << feedback->pose.orientation.x << " rad. \n");
ROS_INFO_STREAM(" Yorien = " << feedback->pose.orientation.y << " rad. \n"); ROS_INFO_STREAM(" Yorien = " << feedback->pose.orientation.y << " rad. \n");
ROS_INFO_STREAM(" Zorien = " << feedback->pose.orientation.z << " rad. \n"); ROS_INFO_STREAM(" Zorien = " << feedback->pose.orientation.z << " rad. \n");
ROS_INFO_STREAM(" Worien = " << feedback->pose.orientation.w << " rad. \n"); ROS_INFO_STREAM(" Worien = " << feedback->pose.orientation.w << " rad. \n");
*/
if (feedback->pose.pose.position.x >= refVal)
if (feedback->pose.orientation.z >= refVal)
{ {
ROS_INFO(" Dock is completed! \n "); ROS_INFO(" Dock is completed! \n ");
keepMoving = false; keepMoving = false;
} }
else else
{ {
Controller(refVal,feedback->pose.orientation.z,.01); Controller(refVal,feedback->pose.pose.position.x,.01);
//ROS_INFO(" Calculating control signal! \n "); //ROS_INFO(" Calculating control signal! \n ");
} }
......
...@@ -35,7 +35,6 @@ RobotDocking::RobotDocking() : ac("Robot_move", true) ...@@ -35,7 +35,6 @@ RobotDocking::RobotDocking() : ac("Robot_move", true)
//PosSub = node.subscribe("/visualize_pose_ekf",1,&RobotDocking::RobCurrPose,this); //PosSub = node.subscribe("/visualize_pose_ekf",1,&RobotDocking::RobCurrPose,this);
} }
......
No preview for this file type
...@@ -6,8 +6,8 @@ ...@@ -6,8 +6,8 @@
using namespace std; using namespace std;
const int row = 13; const int row = 7;
const int col = 13; const int col = 7;
double gamma = .8; double gamma = .8;
...@@ -27,8 +27,8 @@ double Q_curr_state = Q[i][j]; ...@@ -27,8 +27,8 @@ double Q_curr_state = Q[i][j];
double reward; double reward;
int R_indx_i = row - row; int R_indx_i = 5/*row - row*/;
int R_indx_j = .5 * col; int R_indx_j = 3/*.5 * col*/;
int P_indx_i = row - 2; int P_indx_i = row - 2;
int P_indx_j = col - 1; int P_indx_j = col - 1;
......
...@@ -6,8 +6,8 @@ ...@@ -6,8 +6,8 @@
using namespace std; using namespace std;
const int row = 48; const int row = 7;
const int col = 13; const int col = 7;
double gamma = .8; double gamma = .8;
......
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject> <!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 3.5.1, 2016-01-12T21:53:35. --> <!-- Written by QtCreator 3.5.1, 2016-04-13T19:04:29. -->
<qtcreator> <qtcreator>
<data> <data>
<variable>EnvironmentId</variable> <variable>EnvironmentId</variable>
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment