motor_controller/main.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <ros/subscriber.h>
#include <ros/publisher.h>
#include <string>
#include <igvc_msgs/velocity_pair.h>
#include <igvc/SerialPort.h>
#include <std_msgs/Bool.h>

using namespace std;

igvc_msgs::velocity_pair current_motor_command;

bool enabled = false;

void cmdCallback(const igvc_msgs::velocity_pair::ConstPtr& msg)
{
    current_motor_command = *msg;
}

void enabledCallback(const std_msgs::BoolConstPtr& msg)
{
    enabled = msg->data;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "motor_controller");
    ros::NodeHandle nh;
    ros::NodeHandle nhp("~");

    ros::Subscriber cmd_sub = nh.subscribe("/motors", 1, cmdCallback);

    ros::Subscriber enabled_sub = nh.subscribe("/robot_enabled", 1, enabledCallback);

    ros::Publisher enc_pub = nh.advertise<igvc_msgs::velocity_pair>("/encoders", 1000);
    
    std::string device_path;
    nhp.param(string("device"), device_path, string("/dev/igvc_motor_arduino"));
    
    int baud_rate;
    nhp.param(string("baud_rate"), baud_rate, 9600);
    
    SerialPort port(device_path, baud_rate);

    if(!port.isOpen())
    {
        ROS_ERROR_STREAM("Motor Controller serial port failed to open.");
        return -1;
    }
    
    auto line = port.readln();
    while(ros::ok() && line.compare("Ready"))
    {
        ROS_INFO_STREAM("Waiting for arduino. " << line);
        line = port.readln();
        usleep(250);
    }

    ROS_INFO_STREAM("Motor Controller ready.");
    
    ros::Rate rate(10);
    while(ros::ok() && port.isOpen())
    {
        ros::spinOnce();
        
        string msg = "$" + to_string(enabled?current_motor_command.left_velocity:0.0) + "," + to_string(enabled?current_motor_command.right_velocity:0.0) + "\n";
        
        ROS_INFO_STREAM(msg);
        
        port.write(msg);
        
        string ret = port.readln();
        
        try {
            if(!ret.empty())
            {
                size_t dollar = ret.find('$');
                size_t comma = ret.find(',');
                size_t end = ret.find('\n');
                string leftStr = ret.substr(dollar+1, comma-dollar-1);
                string rightStr = ret.substr(comma+1, end-comma-1);
                igvc_msgs::velocity_pair enc_msg;
                enc_msg.left_velocity = atof(leftStr.c_str());
                enc_msg.right_velocity = atof(rightStr.c_str());
                enc_pub.publish(enc_msg);
            } else {
                ROS_ERROR_STREAM("Empty return from arduino.\t" << ret);
            }
        } catch (std::out_of_range) { }
        
        rate.sleep();
    }
}