Roboteq_Driver.cpp

Go to the documentation of this file.

#include <string>
#include <string.h>
#include "RoboteqDevice.h"
#include "ErrorCodes.h"
#include "Constants.h"
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "komodo_rover/StampedEncoders.h"
#include "nav_msgs/Odometry.h"
#include "tf/tf.h"
#include <tf/transform_broadcaster.h>
#include <cmath>
#include <iostream>
#include <stdio.h>
#include <std_srvs/Empty.h>
#include "sensor_msgs/Imu.h"
#include "tf/transform_datatypes.h"
#include "std_msgs/Float32.h"

using namespace std;

ros::Publisher rover_bat_pub;

RoboteqDevice device;

#define MAX_ERRORS 5

bool rp_from_imu = true;
bool y_from_imu = true;
int enc_loop_count = 0, enc_loop_div;
int encoder_cpr, encoder_ppr;
int status = -1;
int prev_l_encoder = 0, prev_r_encoder = 0;
int l_encoder_init = 0, r_encoder_init = 0;
bool first_enc_read = true;
double Left_Motor_command = 0;
double Right_Motor_command = 0;
ros::Publisher odom_pub;
ros::Publisher encoder_pub;
tf::TransformBroadcaster *odom_broadcaster;

//ros::Subscriber imu_sub;
//double imu_yaw=0;
geometry_msgs::Quaternion q_imu = tf::createQuaternionMsgFromYaw(0);

int l_encoder=0;
int r_encoder=0;

double wheel_circumference = 0.0;
double wheel_base_length = 0.0;
double wheel_diameter = 0.0;
double motors_loop_rate;
double encoders_loop_rate;
double tf_loop_rate;
std::string odom_frame_id;
int enc_error_count = 0;
int mot_error_count = 0;
double target_speed = 0.0;
double target_direction = 0.0;

double rot_cov = 0.0;
double pos_cov = 0.0;

int rpm_limit;

// Persistent variables
double xx = 0, yy = 0, tt = 0;
ros::Time prev_time, mot_loop_time, enc_loop_time, bat_loop_time, tf_loop_time;

void updtae_state();

double wrapToPi(double angle) {
        angle += M_PI;
        bool is_neg = (angle < 0);
        angle = fmod(angle, (2.0 * M_PI));
        if (is_neg) {
                angle += (2.0 * M_PI);
        }
        angle -= M_PI;
        return angle;
}

void cmd_velCallback(const geometry_msgs::Twist::ConstPtr& msg) {

        //double Left_Motor_v = msg->linear.x - msg->angular.z * (wheel_base_length / 2.0);
        //double Right_Motor_v = msg->linear.x + msg->angular.z * (wheel_base_length / 2.0);
        double Right_Motor_v = msg->linear.x;
        double Left_Motor_v = msg->angular.z * (wheel_base_length / 2.0);

        double Left_Motor_rpm = Left_Motor_v * (60.0 / (M_PI * wheel_diameter));
        double Right_Motor_rpm = Right_Motor_v * (60.0 / (M_PI * wheel_diameter));

        if (Left_Motor_rpm > rpm_limit)
                Left_Motor_rpm = rpm_limit;
        if (Left_Motor_rpm < -rpm_limit)
                Left_Motor_rpm = -rpm_limit;
        if (Right_Motor_rpm > rpm_limit)
                Right_Motor_rpm = rpm_limit;
        if (Right_Motor_rpm < -rpm_limit)
                Right_Motor_rpm = -rpm_limit;

        //ROS_INFO("Left rpm: %d, Right rpm: %d", (int)Left_Motor_rpm, (int)Right_Motor_rpm);
        Left_Motor_command = Left_Motor_rpm*1000.0/(double)rpm_limit;
        Right_Motor_command = Right_Motor_rpm*1000.0/(double)rpm_limit;

        //ROS_INFO("Left command: %d, Right commandm: %d", (int)Left_Motor_command, (int)Right_Motor_command);

}

void controlLoop() {
        // ROS_INFO("Relative move commands: %f %f", target_speed, target_direction);
        int status1 = -1;
        int status2 = -1;
        //ROS_INFO("Left: %d, Right: %d", (int)Left_Motor_command, (int)Right_Motor_command);
        status1 = device.SetCommand(_GO, 1, (int) Right_Motor_command);
        status2 = device.SetCommand(_GO, 2, (int) Left_Motor_command);
        if ((status1 != RQ_SUCCESS) || (status2 != RQ_SUCCESS)) { 
                ROS_ERROR("Error commanding motors. error codes: m1=%d m2=%d", status1,status2);
                mot_error_count = mot_error_count + 1;
                if (mot_error_count >= MAX_ERRORS) {
                        ROS_ERROR("Error commanding motors -> Resarting connection...");
                        device.Disconnect();
                }
                return;
        }
        mot_error_count = 0;
        Left_Motor_command = 0.0;
        Right_Motor_command = 0.0;

}

void queryBat() {
        

                
        
        int status3 = -1;
        //
                int bat;
                status3 = device.GetValue(_VOLTS, 2, bat);
                if (status3 == RQ_SUCCESS) {
                        //ROS_INFO("V=%d", bat);
                        std_msgs::Float32 v_bat;
                        v_bat.data=(float)bat/10;
                        rover_bat_pub.publish(v_bat);
                } 
                else {
                        ROS_ERROR("Error reading battary voltage");
                }
}

void queryEncoders() {

        int status1 = -1, status2 = -1;
        

        if (!ros::ok() || !device.IsConnected())
                return;

        ros::Time now = ros::Time::now();
double delta_time = (now - prev_time).toSec();
        if (((status1 = device.GetValue(_C, 1, r_encoder)) != RQ_SUCCESS)
                        || ((status2 = device.GetValue(_C, 2, l_encoder)) != RQ_SUCCESS)) {
                ROS_ERROR("Error reading encoders. error codes: e1=%d e2=%d", status1,
                                status2);
                enc_error_count = enc_error_count + 1;

                if (enc_error_count >= MAX_ERRORS) {
                        ROS_ERROR("Error reading encoders -> Resarting connection...");
                        device.Disconnect();
                }
                return;
        }
        enc_error_count = 0;

        l_encoder = l_encoder - l_encoder_init;
        r_encoder = r_encoder - r_encoder_init;

        komodo_rover::StampedEncoders encoder_msg;
        encoder_msg.header.stamp = now;
        encoder_msg.header.frame_id = "base_link";
        encoder_msg.encoders.time_delta = delta_time;
        encoder_msg.encoders.left_wheel = l_encoder;
        encoder_msg.encoders.right_wheel = r_encoder;

        encoder_pub.publish(encoder_msg);
}

void updtae_state() {

        ros::Time now = ros::Time::now();
        double delta_time = (now - prev_time).toSec();

        prev_time = now;

        
        if (first_enc_read) {
        prev_l_encoder = l_encoder;
        prev_r_encoder = r_encoder;
}
        int diff_l_encoder = l_encoder-prev_l_encoder ;
        int diff_r_encoder = r_encoder-prev_r_encoder ;
        prev_l_encoder = l_encoder;
        prev_r_encoder = r_encoder;
        // Convert to rad/s for each wheel from delta encoder ticks
        double l_w = diff_l_encoder * 2.0 * M_PI / (double) encoder_cpr
                        / delta_time;

        double r_w = diff_r_encoder * 2.0 * M_PI / (double) encoder_cpr
                        / delta_time;

        //cout << "l_e: " << l_encoder << "  r_e: " << r_encoder <<"     l_w: " << l_w << "  r_w: " << r_w << endl;

        double l_v = l_w * wheel_diameter / 2.0;
        double r_v = r_w * wheel_diameter / 2.0;

        double v = (l_v + r_v) / 2.0;

        double w = (r_v - l_v) / wheel_base_length;

        // Update the states based on model and input
        geometry_msgs::Quaternion quat;
        double roll = 0, pitch = 0, yaw = 0;
        tf::Quaternion q;
        tf::quaternionMsgToTF(q_imu, q);
        tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

        if (first_enc_read) {

                xx = 0;
                yy = 0;
                tt = 0;
                first_enc_read = false;
        }
        else {

                if (!rp_from_imu) {
                        roll = 0;
                        pitch = 0;
                }

                //      tt=wrapToPi(yaw);
                //}

                if (y_from_imu) {
                        tt = wrapToPi(yaw);
                }

                else {
                        tt = tt + w * delta_time;
                        tt = wrapToPi(tt);

                }
        }

        quat = tf::createQuaternionMsgFromRollPitchYaw(wrapToPi(roll),
                        wrapToPi(pitch), tt);
        //ROS_INFO("RPY = (%lf, %lf, %lf)",  wrapToPi(roll), wrapToPi(pitch), wrapToPi(tt));
        //tt = wrapToPi(tt);
        xx = xx + (v * cos(tt))*delta_time;
        yy = yy + (v * sin(tt))*delta_time;

        //geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(0);
        // Populate the msg
        nav_msgs::Odometry odom_msg;

        odom_msg.header.stamp = now;
        odom_msg.header.frame_id = odom_frame_id;
        odom_msg.pose.pose.position.x = xx;
        odom_msg.pose.pose.position.y = yy;
        odom_msg.pose.pose.position.z = 0;
        odom_msg.pose.pose.orientation = quat;
        odom_msg.child_frame_id = "base_link";
        odom_msg.twist.twist.linear.x = v;
        odom_msg.twist.twist.linear.y = 0;
        odom_msg.twist.twist.angular.z = w;

        odom_msg.pose.covariance[0] = pos_cov;
        odom_msg.pose.covariance[7] = pos_cov;
        odom_msg.pose.covariance[14] = 1e100;
        odom_msg.pose.covariance[21] = 1e100;
        odom_msg.pose.covariance[28] = 1e100;
        odom_msg.pose.covariance[35] = rot_cov;
        odom_msg.twist.covariance = odom_msg.pose.covariance;

        odom_pub.publish(odom_msg);

        //Add TF broadcaster
        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = now;
        odom_trans.header.frame_id = odom_frame_id;
        odom_trans.child_frame_id = "base_link";
        odom_trans.transform.translation.x = xx;
        odom_trans.transform.translation.y = yy;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = quat;
        odom_broadcaster->sendTransform(odom_trans);
}

bool zero_enc_callback(std_srvs::Empty::Request& request,
                std_srvs::Empty::Response& response) {
        ROS_INFO("Zeroing odometry");

        first_enc_read = true;
        return true;
}

void imu_Callback(const sensor_msgs::Imu::ConstPtr& msg) {
        //ROS_INFO("imu yaw:%f",imu_yaw);
        q_imu = msg->orientation;
//imu_yaw=tf::getYaw(msg->orientation);
        //ROS_INFO("imu yaw:%f",tf::getYaw(msg->orientation));
}




int main(int argc, char **argv) {

        // Node setup

        ros::init(argc, argv, "Roboteq_Driver");
        ros::NodeHandle n("~");
        prev_time = ros::Time::now();
        mot_loop_time = ros::Time::now();
        enc_loop_time = ros::Time::now();
        bat_loop_time = ros::Time::now();
        tf_loop_time = ros::Time::now();
        std::string port;
        n.param("serial_port", port, std::string("/dev/Roboteq"));
        n.param("wheel_diameter", wheel_diameter, 0.2);
        wheel_circumference = wheel_diameter * M_PI;
        n.param("wheel_base_length", wheel_base_length, 0.45);
        n.param("encoder_ppr", encoder_ppr, 512);
        encoder_cpr = encoder_ppr * 4;
        n.param("odom_frame_id", odom_frame_id, std::string("/odom"));
        n.param("rpm_limit", rpm_limit, 500);
        n.param("rotation_covariance", rot_cov, 1.0);
        n.param("position_covariance", pos_cov, 1.0);
        n.param("motors_loop_rate", motors_loop_rate, 20.0);
        //ros::Rate loop_rate1(loop_rate);
        n.param("encoders_loop_rate", encoders_loop_rate, 5.0);
        n.param("tf_loop_rate", tf_loop_rate, 10.0);
        n.param("roll_pitch_from_imu", rp_from_imu, true);
        n.param("yaw_from_imu", y_from_imu, true);

        ros::Subscriber imu_sub = n.subscribe("/imu/data", 10, imu_Callback);

        // Odometry Publisher
        odom_pub = n.advertise < nav_msgs::Odometry > ("/odom", 10);
        // Encoder Publisher
        encoder_pub = n.advertise < komodo_rover::StampedEncoders > ("/encoders", 10);
        // TF Broadcaster
        odom_broadcaster = new tf::TransformBroadcaster;
        // cmd_vel Subscriber
        ros::Subscriber sub = n.subscribe("/cmd_vel", 1, cmd_velCallback);

        ros::ServiceServer service = n.advertiseService("zero_odometry",
                        zero_enc_callback);

        
        rover_bat_pub = n.advertise<std_msgs::Float32>("/rover_bat", 10);
        
        // Spinner
        ros::AsyncSpinner spinner(1);
        spinner.start();

        while (ros::ok()) {
                ROS_INFO("Connecting to port %s", port.c_str());
                status = device.Connect(port);
                if (status != RQ_SUCCESS) {
                        ROS_ERROR(
                                        "Error connecting to device. Error code: %d. Trying again in 5 seconds...",
                                        status);
                        for (int i = 0; i < 100; ++i) {
                                updtae_state();
                                ros::Duration(5.0 / 100.0).sleep();
                                if (!ros::ok())
                                        break;
                                
                                ros::Time now_time0 = ros::Time::now();
                                if ((now_time0 - tf_loop_time).toSec()
                                        >= 1.0 / tf_loop_rate) {
                                
                                updtae_state ();
                                tf_loop_time = now_time0;
                        }

                        }
                        continue;
                }
                
                //int s1 = -1;
                //      if ((s1 = device.SetCommand(_HOME, 1)) != RQ_SUCCESS)
                //      ROS_ERROR("HOME 1 failed!");
                //if ((s1 = device.SetCommand(_HOME, 2)) != RQ_SUCCESS)
                //      ROS_ERROR("HOME 2 failed!");

                while ((device.IsConnected()) && (ros::ok())) {

                        ros::Time now_time = ros::Time::now();

                        if ((now_time - mot_loop_time).toSec() >= 1.0 / motors_loop_rate) {
                                controlLoop();
                                //      cout<<"loop"<<endl;
                                mot_loop_time = now_time;
                        }
                        if ((now_time - enc_loop_time).toSec()
                                        >= 1.0 / encoders_loop_rate) {
                                queryEncoders();
                                //cout<<"enc"<<endl;;
                                enc_loop_time = now_time;
                        }
                        if ((now_time - tf_loop_time).toSec()
                                        >= 1.0 / tf_loop_rate) {
                                
                                updtae_state ();
                                tf_loop_time = now_time;
                        }

                        if ((now_time - bat_loop_time).toSec() >= 1.0) { //1Hz read battery
                                queryBat();
                                //cout<<"enc"<<endl;
                                bat_loop_time = now_time;
                        }

                        //loop_rate1.sleep();

                }
                if ((ros::ok()) && (!device.IsConnected())) {
                        ROS_ERROR("Connection failed!");
                        continue;
                }

        }

        spinner.stop();
        device.Disconnect();
        ROS_INFO("Disconnected");
        return 0;
}