OImplemented simple readLine() for serial stream

catkin_ws/src/lss_4dof/lss_4dof_hw/src/lss_4dof_hw_interface.cpp

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

catkin_ws/src/lss_4dof/lss_4dof_hw/src/lss_4dof_hw_interface.h

@@ -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;