dock.cpp

#include "dock.h"
#include <ros/ros.h>

#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>


#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::curr_error = 0;
double RobotDocking::int_error = 0;
double RobotDocking::diff = 0;

double RobotDocking::p_term = 0;
double RobotDocking::i_term = 0;
double RobotDocking::d_term = 0;

double RobotDocking::prop_gain = 0.0001;
double RobotDocking::integ_gain = 0.01;
double RobotDocking::deriv_gain = 0;

double RobotDocking::control_signal = 0;

//constructor declaration
RobotDocking::RobotDocking() : ac("Robot_move", true)
{
        keepMoving = true;
        
        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("/base_controller/odometry",1,&RobotDocking::RobCurrPose,this);
	
}


void RobotDocking::ProgStart()
{
        ros::Rate rate(100);
        
        while (ros::ok() && keepMoving)
        {
                //dock(FORWARD_SPEED_MPS);
		Controller(refVal,robPose,.01);

                ros::spinOnce();
                rate.sleep();