md

#include <ros/ros.h>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>
#include <math.h>
#include "md/vel_msg.h"
#include "md/monitor_msg.h"

//mskim
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Empty.h>
#include <std_srvs/Empty.h>

#include <std_msgs/Int32.h>
#define DEG2RAD(x) (x * 0.01745329252) // *PI/180
#define RAD2DEG(x) (x * 57.2957795131) // *180/PI

typedef unsigned char  BYTE;

#define LEFT           	  0
#define RIGHT             1

#define BIT0              0x01
#define BIT1              0x02
#define BIT2              0x04
#define BIT3              0x08
#define BIT4              0x10
#define BIT5              0x20
#define BIT6              0x40
#define BIT7              0x80



//mskim
float odom_pose[3];
float odom_vel[3];
double global_yaw;
bool is_nomotionupdate;

typedef struct {
    long lPosiX;
    long lPosiY, lExPosiY;
    short sTheta;
    float fTheta;
    short sVoltIn;
    short sCurrent[2];
    BYTE byUS1;
    BYTE byUS2;
    BYTE byUS3;
    BYTE byUS4;
    BYTE byPlatStatus;
    BYTE byDocStatus;
    BYTE byMotStatus[2];
    BYTE byRun;
    BYTE fgAlarm[2], fgCtrlFail[2], fgOverVolt[2], fgOverTemp[2];
    BYTE fgOverLoad[2], fgHallFail[2], fgInvVel[2], fgStall[2];

    BYTE fgEmerON, fgBatChargeON, fgRccState;

    short sCmdLinearVel, sCmdAngularVel;
    short sRealLinearVel, sRealAngularVel;

    float fRealLinearVel, fRealAngularVel;

    int nAngleResolution;

}MotorDriver;
MotorDriver Md;

//It is a message callback function.
//It is a function that oprates when a topic message named 'monitor_topic' is received.
//The input message is to receive the 'monitor_msg' message from 'md' package in msg directory
void monitorCallBack(const md::monitor_msg::ConstPtr& monitor)
{
    int nGap;
    static int nExsTheta;

    Md.lPosiX             = monitor->lPosiX;
    Md.lPosiY             = monitor->lPosiY;
    Md.sTheta             = monitor->sTheta;
    Md.fTheta             = (float)(monitor->sTheta)/10;
    Md.sRealLinearVel     = monitor->sRealLinearVel;
    Md.fRealAngularVel    = (float)(monitor->sRealAngularVel)/10;
    Md.sVoltIn            = monitor->sVoltIn;
    Md.sCurrent[LEFT]     = monitor->sLeftMotCur;
    Md.sCurrent[RIGHT]    = monitor->sRightMotCur;
    Md.byUS1              = monitor->byUS1;
    Md.byUS2              = monitor->byUS2;
    Md.byUS3              = monitor->byUS3;
    Md.byUS4              = monitor->byUS4;
    Md.byPlatStatus       = monitor->byPlatStatus;
    Md.byDocStatus        = monitor->byDocStatus;
    Md.byMotStatus[LEFT]  = monitor->byLeftMotStatus;
    Md.byMotStatus[RIGHT] = monitor->byRightMotStatus;
    Md.byRun              = monitor->byRun;

    Md.fgEmerON      = Md.byPlatStatus & BIT0;
    Md.fgBatChargeON = Md.byDocStatus & BIT0;
    Md.fgRccState    = (Md.byDocStatus & BIT7) >> 7;

    Md.fgAlarm[LEFT]    = Md.byMotStatus[LEFT] & BIT0;
    Md.fgCtrlFail[LEFT] = Md.byMotStatus[LEFT] & BIT1;
    Md.fgOverVolt[LEFT] = Md.byMotStatus[LEFT] & BIT2;
    Md.fgOverTemp[LEFT] = Md.byMotStatus[LEFT] & BIT3;
    Md.fgOverLoad[LEFT] = Md.byMotStatus[LEFT] & BIT4;
    Md.fgHallFail[LEFT] = Md.byMotStatus[LEFT] & BIT5;
    Md.fgInvVel[LEFT]   = Md.byMotStatus[LEFT] & BIT6;
    Md.fgStall[LEFT]    = Md.byMotStatus[LEFT] & BIT7;

    Md.fgAlarm[RIGHT]    = Md.byMotStatus[RIGHT] & BIT0;
    Md.fgCtrlFail[RIGHT] = Md.byMotStatus[RIGHT] & BIT1;
    Md.fgOverVolt[RIGHT] = Md.byMotStatus[RIGHT] & BIT2;
    Md.fgOverTemp[RIGHT] = Md.byMotStatus[RIGHT] & BIT3;
    Md.fgOverLoad[RIGHT] = Md.byMotStatus[RIGHT] & BIT4;
    Md.fgHallFail[RIGHT] = Md.byMotStatus[RIGHT] & BIT5;
    Md.fgInvVel[RIGHT]   = Md.byMotStatus[RIGHT] & BIT6;
    Md.fgStall[RIGHT]    = Md.byMotStatus[RIGHT] & BIT7;

    //nGap = Md.sTheta - nExsTheta;
    //ROS_INFO("%4d %4d", nGap, Md.sTheta);
    //nExsTheta = Md.sTheta;
    //ROS_INFO("%d, %d, %d, %d", Md.sCmdLinearVel, Md.sRealLinearVel, Md.sCmdAngularVel, Md.sRealAngularVel);
    /*printf("sub-> x: %ld  y: %ld  theta: %3.1f  linearVel: %d  angularVel: %4.1f  L_Cur: %d  R_Cur: %d  US1:%d  US2:%d  US3:%d  "
           "volt:%d  Emergecy:%d  Charge:%d  DocState:%d  LeftMotStatu:%d  RightMotStatu:%d\n",
           Md.lPosiX, Md.lPosiY, Md.fTheta, Md.sRealLinearVel, Md.fRealAngularVel, Md.sCurrent[LEFT], Md.sCurrent[RIGHT], Md.byUS1, Md.byUS2, Md.byUS3, Md.sVoltIn,
           Md.fgEmerON, Md.fgBatChargeON, Md.fgRccState, Md.byMotStatus[LEFT], Md.byMotStatus[RIGHT]);*/
    //printf("encoder x: %ld  y: %ld  theta: %3.1f\n", Md.lPosiX, Md.lPosiY, Md.fTheta);
    //printf("L_Cur: %d  R_Cur: %d Cur: %d\n", Md.sCurrent[LEFT], Md.sCurrent[RIGHT]+Md.sCurrent[LEFT], Md.sCurrent[RIGHT]);
    if(Md.fgAlarm[RIGHT] != 0){
      printf("RIGHT\n");
      printf("ALARM = %d\n", Md.fgAlarm[RIGHT]);
      printf("Ctrl Fail= %d\n", Md.fgCtrlFail[RIGHT]);
      printf("Over Volt = %d\n", Md.fgOverVolt[RIGHT]);
      printf("Over Temp = %d\n", Md.fgOverTemp[RIGHT]);
      printf("Over Load = %d\n", Md.fgOverLoad[RIGHT]);
      printf("Hall Fail = %d\n", Md.fgHallFail[RIGHT]);
      printf("Inv Vel = %d\n", Md.fgInvVel[RIGHT]);
      printf("Stall = %d\n", Md.fgStall[RIGHT]);
      printf("Current = %d\n", Md.sCurrent[RIGHT]);
      // 21.01.15 sykim add - auto motor fault reset
      system("rostopic pub /motor_reset_topic std_msgs/Empty -1 &");
    }

    if(Md.fgAlarm[LEFT] != 0){
      printf("LEFT\n");
      printf("ALARM = %d\n", Md.fgAlarm[LEFT]);
      printf("Ctrl Fail= %d\n", Md.fgCtrlFail[LEFT]);
      printf("Over Volt = %d\n", Md.fgOverVolt[LEFT]);
      printf("Over Temp = %d\n", Md.fgOverTemp[LEFT]);
      printf("Over Load = %d\n", Md.fgOverLoad[LEFT]);
      printf("Hall Fail = %d\n", Md.fgHallFail[LEFT]);
      printf("Inv Vel = %d\n", Md.fgInvVel[LEFT]);
      printf("Stall = %d\n", Md.fgStall[LEFT]);
      printf("Current = %d\n", Md.sCurrent[LEFT]);
      // 21.01.15 sykim add - auto motor fault reset
      system("rostopic pub /motor_reset_topic std_msgs/Empty -1 &");
    }

    //mskim, odometry pose set
    //odom_pose[0] = Md.lPosiY*0.001;
    //odom_pose[1] = 0;
    //odom_pose[2] = DEG2RAD(Md.fRealAngularVel);
    //printf("Md.lPosiY [0] = %f,\t Md.lPosiX [1] = %f,\t RAD = %f\n", odom_pose[0], odom_pose[1], odom_pose[2]);
    //printf("sRealAngularVel = %f\n", odom_pose[2]);

}

/////////////////////////////////////for example to get the 'Md.sCmdAngularVel'////////////////
void keyboardCallBack(const geometry_msgs::Twist& keyVel)   //from turtlebot3_teleop_key node
{
    Md.sCmdLinearVel  = keyVel.linear.x * 1000;// mm/s
    if(Md.nAngleResolution)
        Md.sCmdAngularVel = keyVel.angular.z * 10;// 0.1 deg/s
    else
        Md.sCmdAngularVel = keyVel.angular.z;// 1 deg/s
}
///////////////////////////////////////////////////////////////////////////////////////////////


//mskim
void cmdVelCallBack(const geometry_msgs::Twist& cmdVel)   //from turtlebot3_teleop_key node
{
    //keyboard or navigation value
    odom_vel[0] = cmdVel.linear.x;
    odom_vel[1] = cmdVel.linear.y;
    odom_vel[2] = cmdVel.angular.z;
    
    Md.sCmdLinearVel  = cmdVel.linear.x * 1000;// mm/s
    if(Md.nAngleResolution)
        Md.sCmdAngularVel = RAD2DEG(cmdVel.angular.z) * 10;// 0.1 deg/s
    else
        Md.sCmdAngularVel = RAD2DEG(cmdVel.angular.z);// 1 deg/s

    //printf("\ncmdVel[0] = %f,\t cmdVel[1] = %f,\t cmdVel[2] = %f\t\n", odom_vel[0], odom_vel[1], odom_vel[2]);	
}


void imuCallBack(const sensor_msgs::Imu& imu_vel)   //from turtlebot3_teleop_key node
{
    double roll=0;
    double pitch=0;
    double yaw=0;

    tf::Quaternion q(imu_vel.orientation.x, imu_vel.orientation.y, imu_vel.orientation.z, imu_vel.orientation.w);
    tf::Matrix3x3 m(q);
    m.getRPY(roll, pitch, yaw);

    global_yaw = yaw;
    //printf("sub-> yaw: %3.1f\n", global_yaw);
}

void nomotionCallBcak(const std_msgs::Empty::ConstPtr & _is_nomotionupdate)
{
    is_nomotionupdate = true;
}


//Node main function
int main(int argc, char** argv)
{
    static int nResolution, nCnt1, nOperMode, nResetOdometry, nResetAngle;

    ros::init(argc, argv, "vel_cmd_node");                                                //Node name initialization.
    ros::NodeHandle nh;                                                                   //Node handle declaration for communication with ROS system.
    ros::Publisher cmd_vel_pub = nh.advertise<md::vel_msg>("vel_topic", 10);             //Publisher declaration.
    ros::Subscriber monitor_sub = nh.subscribe("monitor_topic", 10, monitorCallBack);    //Subscriber declaration.
    //ros::Subscriber keyboard_sub = nh.subscribe("cmd_vel", 10, keyboardCallBack); //original
    ros::Subscriber imu_sub = nh.subscribe("imu", 10, imuCallBack);    //mskim add
    ros::Subscriber keyboard_sub = nh.subscribe("cmd_vel", 10, cmdVelCallBack); //mskim add
    ros::Subscriber nomotion_sub = nh.subscribe("need_nomotionupdate", 10, nomotionCallBcak);

    //mskim add, for call rosservice
    ros::ServiceClient nomotionUpdateClient = nh.serviceClient<std_srvs::Empty>("/request_nomotion_update");
    std_srvs::Empty srv;
    //mskim, add odometery publisher, tf publisher
    ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 50);

    ros::Publisher is_move_pub = nh.advertise<std_msgs::Int32>("is_move", 50);
    
    tf::TransformBroadcaster tf_broadcaster;

    //original code
    ros::Rate r(20);                                                                      //Set the loop period -> 50ms.

    nOperMode = 0;
    md::vel_msg vel;          //vel_msg declares message 'vel' as message file.
    nh.getParam("md_node/angleresolution", Md.nAngleResolution);
    /////////////////////////////////////////////////////////////////

    //mskim add
    std_srvs::Empty::Request req;
    std_srvs::Empty::Response resp;
    std_msgs::Int32 move_data;
    // Odometry Initialize
    nav_msgs::Odometry odom;
    odom.pose.pose.position.x = 0.0;
    odom.pose.pose.position.y = 0.0;
    odom.pose.pose.position.z = 0.0;

    odom.pose.pose.orientation.x = 0.0;
    odom.pose.pose.orientation.y = 0.0;
    odom.pose.pose.orientation.z = 0.0;
    odom.pose.pose.orientation.w = 0.0;

    odom.twist.twist.linear.x = 0.0;
    odom.twist.twist.linear.y = 0.0;
    odom.twist.twist.angular.z = 0.0;

    for (int index = 0; index < 3; index++)
    {
      odom_pose[index] = 0.0;
      odom_vel[index] = 0.0;
    }

    double x = 0.0;
    double y = 0.0;
    double th = 0.0; 
    int is_move_count = 0;
    int nomotionupdate_count = 0;

    ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();

    

    while(ros::ok())
    {
        if(nCnt1++ == 10)     //Store the value of the parameter in the variable once per 500mS.
        {
            nCnt1 = 0;
            nh.getParam("vel_cmd_node/reset_odometry", nResetOdometry);
            nh.getParam("vel_cmd_node/reset_angle", nResetAngle);
        }
	    if(Md.byRun)
	    {
            move_data.data = 1;
	    }
        else{
            move_data.data = 0;
        }
        if(is_move_count++ == 20)
        {
            is_move_count = 0;
            is_move_pub.publish(move_data);
        }
        if(is_nomotionupdate)
        {
            if(nomotionupdate_count<1000){
                nomotionUpdateClient.call(srv);
                nomotionupdate_count++;
        }
            else
            {
                is_nomotionupdate = false;
                nomotionupdate_count = 0;
            }
        }
        vel.nLinear         = Md.sCmdLinearVel;
        vel.nAngular        = Md.sCmdAngularVel;
        vel.byResetOdometry = nResetOdometry;     //Put the value of 'nResetOdometry' in the child byResetOdometry message of 'vel'
        vel.byResetAngle    = nResetAngle;
        cmd_vel_pub.publish(vel);                 //Publish the message 'vel'

        ros::spinOnce();
	current_time = ros::Time::now();

        double dt = (current_time - last_time).toSec();
        double delta_x = (odom_vel[0] * cos(th) - odom_vel[1] * sin(th)) * dt;
        double delta_y = (odom_vel[0] * sin(th) + odom_vel[1] * cos(th)) * dt;
        double delta_th = odom_pose[2] * dt; //odom_vel[2] * dt;

        x = (Md.lPosiX/1000.0)*(-1);
        y = (Md.lPosiY/1000.0)*(-1);
        th = Md.fTheta-90;
        //x += delta_x;
        //y += delta_y;
        //th += delta_th;
	//ROS_INFO("x:%f y:%f th:%f\n",x ,y ,th);
        //ROS_INFO("dt:%f\n",dt);
        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(DEG2RAD(th));
        // temp blocked, mskim, test odom_combined
/*        geometry_msgs::TransformStamped odom_tf;
        odom_tf.header.stamp = current_time;
        odom_tf.header.frame_id = "odom";
        //odom_tf.child_frame_id = "base_link";
   	//odom_tf.child_frame_id = "base_footprint";
        odom_tf.transform.translation.x = x;
        odom_tf.transform.translation.y = y;
        odom_tf.transform.translation.z = 0.0;
        odom_tf.transform.rotation = odom_quat;

        //send the transform
        tf_broadcaster.sendTransform(odom_tf);
*/  
        //next, we'll publish the odometry message over ROS
        odom.header.stamp = current_time;
        odom.header.frame_id = "odom";
   
        //set the position
        odom.pose.pose.position.x = x;
        odom.pose.pose.position.y = y;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;

        //set the velocity
        //odom.child_frame_id = "base_link"
        odom.twist.twist.linear.x = (Md.sRealLinearVel)/1000.0;
        odom.twist.twist.linear.y = 0;
        odom.twist.twist.angular.z = DEG2RAD(Md.fRealAngularVel);

	//jmlee add covariance 210607
	//odom.pose.covariance[0] = 0.01;
	//odom.pose.covariance[7] = 0.01;
	//odom.pose.covariance[14] = 0.01;
	//odom.pose.covariance[21] = 0.1;
	//odom.pose.covariance[28] = 0.1;
	//odom.pose.covariance[35] = 0.1;
	//odom.twist.covariance = odom.pose.covariance;
    //sykim add covariance
	 odom.pose.covariance[0] = 0.0001;
	 odom.pose.covariance[7] = 0.0001;
	 odom.pose.covariance[14] = 10000000000.0;
	 odom.pose.covariance[21] = 10000000000.0;
	 odom.pose.covariance[28] = 10000000000.0;
	 odom.pose.covariance[35] = 0.001;
	 odom.twist.covariance = odom.pose.covariance;
        /*
	//sykim add covariance
	odom.pose.covariance = [0.000001, 0, 0, 0, 0, 0,
				0, 0.000001, 0, 0, 0, 0,
				0, 0, 0.000001, 0, 0, 0,
				0, 0, 0, 0.000001, 0, 0,
				0, 0, 0, 0, 0.000001, 0,
				0, 0, 0, 0, 0, 0.000001]
	odom.twist.covariance = [99999, 0, 0, 0, 0, 0,
				 0, 99999, 0, 0, 0, 0,
				 0, 0, 99999, 0, 0, 0,
				 0, 0, 0, 99999, 0, 0,
				 0, 0, 0, 0, 99999, 0,
				 0, 0, 0, 0, 0, 99999]
*/
	//printf("\ntrans.x = %f\t, trans.y = %f\t,\n", odom_tf.transform.translation.x,odom_tf.transform.translation.y);
        //printf("linear.x = %f\t, angular.z = %f\n\n", Md.sRealLinearVel, Md.fRealAngularVel);
	//printf("linear.x = %f\t, linear.y = %f\t, angular.z = %f\n\n", odom.twist.twist.linear.x, odom.twist.twist.linear.y, odom.twist.twist.angular.z);

        odom_pub.publish(odom);
        last_time = current_time;

        r.sleep();
    }
}