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();
        }

}

void RobotDocking::Controller(double Reference, double RobPose, double dt)
{
        // e(t) = setpoint - actual value;
        curr_error = Reference - RobPose;
        
        // Integrated error
        int_error +=  curr_error * dt;
        
        /*
        // -- windup gaurd -- 
        if (int_error < )
        {}
        else if ()
        {}
        */
        
        // differentiation
        diff = ((curr_error - prev_error) / dt);
        
        // scalling
        p_term = prop_gain  * curr_error;
        i_term = integ_gain * int_error;
        d_term = deriv_gain * diff;
        
        // control signal
        control_signal = p_term + i_term + d_term;
        
        
        // save the current error as the previous one
        // for the next iteration.
        prev_error = curr_error;

	//ROS_INFO_STREAM("RAW Control signalYAW = "<< control_signal <<". \n");
	ROS_INFO_STREAM(" ------------------------------------------------------------- \n");	
	
        dock(control_signal);
}

void RobotDocking::dock(double speed)
{


		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");

        
	/*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 nav_msgs::Odometry::ConstPtr& feedback)
{
        
        
        ROS_INFO_STREAM(" X = " << feedback->pose.pose.position.x << " 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(" Xorien = " << feedback->pose.orientation.x << " 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(" Worien = " << feedback->pose.orientation.w << " rad. \n");
       */ 
        
        if (feedback->pose.pose.position.x >= refVal)
        {
                ROS_INFO(" Dock is completed! \n ");
                keepMoving = false;
        }
        else
        {
                Controller(refVal,feedback->pose.pose.position.x,.01);
                //ROS_INFO(" Calculating control signal! \n ");
        }
        
}