Commit 942d6a25 authored by Matteo Saveriano's avatar Matteo Saveriano
Browse files

OImplemented simple readLine() for serial stream

parent d1040f75
......@@ -111,6 +111,22 @@ namespace lss_4dof_hw_interface
ROS_INFO_STREAM_NAMED(name_, "Hardware Interface Ready.");
}
std::string LSS4DOFHWInterface::readLine(LibSerial::SerialStream sStream, char lineTerminator)
{
// ToDO Add a timeout
std::string line_ = "";
char nextChar = 0;
while(nextChar != lineTerminator)
{
sStream >> nextChar;
line_ += nextChar;
}
return line_;
}
void LSS4DOFHWInterface::read(ros::Duration &elapsed_time)
{
// ToDo current position from Arm
......@@ -122,15 +138,19 @@ namespace lss_4dof_hw_interface
// This should compile with libserial-dev (Version >=1.0. Note: Ubuntu 18.04 uses libserial-dev 0.6)
// This is an untest code stub just to get going
/*
serial << "#0QD" << std::endl ;
myposstr << serial.Readline() ;
//myposstr << serial.Readline() ;
myposstr = readLine(serial);
// Diveded by 10 because we get 1/10 degree and converted tor rad
pos_double = std::stod(myposstr.substr(4))/10*(M_PI/180) ;
myposstr="";
joint_position_.push_back(pos_double);
serial << "#1QD" << std::endl ;
// For debug
ROS_INFO("First joint angle: %d", pos_double);
/*serial << "#1QD" << std::endl ;
myposstr << serial.Readline() ;
// Diveded by 10 because we get 1/10 degree and converted tor rad
pos_double = std::stod(myposstr.substr(4))/10*(M_PI/180);
......
......@@ -96,6 +96,8 @@ namespace lss_4dof_hw_interface
// Serial connection
LibSerial::SerialStream serial;
std::string readLine(LibSerial::SerialStream sStream, const char lineTerminator="\n");
protected:
/** \brief LSS 4DOF interface */
LSS4DOFHWInterface *lss_4dof;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment