Fixed startup publishing issue

tonybaltovski-forks · May 6, 2015 · 22388eb · 22388eb
1 parent d31adbf
commit 22388eb
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 9 deletions.
diff --git a/src/LMS1xx.cpp b/src/LMS1xx.cpp
@@ -21,6 +21,7 @@
  *                                                                         *
  ***************************************************************************/
 
+#include <sys/time.h>
 #include <sys/types.h>
 #include <sys/socket.h>
 #include <netinet/in.h>
@@ -30,6 +31,7 @@
 #include <cstring>
 #include <unistd.h>
 
+#include "ros/ros.h"
 #include "LMS1xx/LMS1xx.h"
 #include "console_bridge/console.h"
 
@@ -50,7 +52,6 @@ void LMS1xx::connect(std::string host, int port) {
 			stSockAddr.sin_family = PF_INET;
 			stSockAddr.sin_port = htons(port);
 			Res = inet_pton(AF_INET, host.c_str(), &stSockAddr.sin_addr);
-
 			int ret = ::connect(sockDesc, (struct sockaddr *) &stSockAddr,
 					sizeof stSockAddr);
 			if (ret == 0) {
@@ -117,9 +118,24 @@ status_t LMS1xx::queryStatus() {
 
 void LMS1xx::login() {
 	char buf[100];
+	int result;
 	sprintf(buf, "%c%s%c", 0x02, "sMN SetAccessMode 03 F4724744", 0x03);
+
+	fd_set readset;
+	struct timeval timeout;
+	timeout.tv_sec = 1;
+	timeout.tv_usec = 0;
 
-	write(sockDesc, buf, strlen(buf));
+	do { //loop until data is available to read
+	  ros::Duration(1.0).sleep();
+
+	  write(sockDesc, buf, strlen(buf));
+
+	  FD_ZERO(&readset);
+	  FD_SET(sockDesc, &readset);
+	  result = select(sockDesc + 1, &readset, NULL, NULL, &timeout);
+
+	} while (result <= 0);
 
 	int len = read(sockDesc, buf, 100);
 	if (buf[0] != 0x02)

diff --git a/src/LMS1xx_node.cpp b/src/LMS1xx_node.cpp
@@ -34,15 +34,27 @@ int main(int argc, char **argv)
     ROS_INFO("Connecting to laser at : %s", host.c_str());
 
     // initialize hardware
-    laser.connect(host);
+    do {
+      laser.connect(host);
+
+      if (laser.isConnected())
+      {	
+	laser.login();
+	cfg = laser.getScanCfg();
+	outputRange = laser.getScanOutputRange();
+      }
 
-    if (laser.isConnected())
-    {
-      ROS_INFO("Connected to laser.");
+      //check if laser is fully initialized, else reconnect
+      //assuming fully initialized => scaningFrequency=5000
+      if (cfg.scaningFrequency != 5000) {
+	laser.disconnect();
+	ROS_INFO("Waiting for laser to initialize...");
+      }
+
+    } while (!laser.isConnected() || cfg.scaningFrequency != 5000);
 
-      laser.login();
-      cfg = laser.getScanCfg();
-      outputRange = laser.getScanOutputRange();
+    if (laser.isConnected()) {
+      ROS_INFO("Connected to laser.");
 
       ROS_DEBUG("Laser configuration: scaningFrequency %d, angleResolution %d, startAngle %d, stopAngle %d",
                 cfg.scaningFrequency, cfg.angleResolution, cfg.startAngle, cfg.stopAngle);