UNTESTED: issue stop command before calling start

Make sure to shut down the data transmitting from the camera before starting a new instance. Otherwise the new port and other settings will not be applied. Also disable position argument, since it is not relevant to IMU data.

Change-Id: I4e0d239dc2c566932eaf6495f06eb4c98c5b53c2

src/axis_imu.cc

@@ -45,7 +45,7 @@ void spin_thread () {
   ros::spin();
 }
 
-float bytesToFloat(unsigned char b0, unsigned char b1, unsigned char b2,  unsigned char b3) { 
+float bytesToFloat(unsigned char b0, unsigned char b1, unsigned char b2,  unsigned char b3) {
   // unsigned char byte_array[] = { b3, b2, b1, b0 };
   unsigned char byte_array[] = { b0, b1, b2, b3 };
   float result;
@@ -53,7 +53,7 @@ float bytesToFloat(unsigned char b0, unsigned char b1, unsigned char b2,  unsign
             reinterpret_cast<const char*>(&byte_array[4]),
             reinterpret_cast<char*>(&result));
   return result;
-} 
+}
 
 
 class udp_server {
@@ -69,7 +69,7 @@ public:
 
 private:
 
-  double bytesToDouble(unsigned int i) { 
+  double bytesToDouble(unsigned int i) {
   //unsigned char byte_array[] = { b0, b1, b2, b3, b4, b5, b6, b7 };
     unsigned char byte_array[] = { recv_buffer_[i+7],
                                    recv_buffer_[i+6],
@@ -84,9 +84,9 @@ private:
               reinterpret_cast<const char*>(&byte_array[8]),
               reinterpret_cast<char*>(&result));
     return result;
-  } 
+  }
+
 
-  
   void start_receive() {
     socket_.async_receive_from(boost::asio::buffer(recv_buffer_), remote_endpoint_,
                                boost::bind(&udp_server::handle_receive, this,
@@ -99,7 +99,7 @@ private:
     if (!error || error == boost::asio::error::message_size) {
       // MESSAGES SIZE: 91 bytes
       // cerr << "Got message: " << bytes_transferred << endl;
-      
+
       //      cerr << "Got message: " << bytes_transferred << " - " << recv_buffer_[0] << " - " << recv_buffer_[33] << endl;
       //      float time = bytesToFloat(recv_buffer_[1], recv_buffer_[2], recv_buffer_[3], recv_buffer_[4]);
       double gtime = bytesToDouble(1);
@@ -117,7 +117,7 @@ private:
       az = bytesToDouble(58);
       if (recv_buffer_[0] == 'G') {
         have_gyro_flag = true;
-        ROS_INFO("         GYRO: %f - %f %f %f", gtime, gx, gy, gz);        
+        ROS_INFO("         GYRO: %f - %f %f %f", gtime, gx, gy, gz);
       }
       if (recv_buffer_[33] == 'A') {
         have_acc_flag = true;
@@ -137,20 +137,20 @@ private:
       }
 
       if (!have_gyro_flag) {
-        start_receive();        
+        start_receive();
         return;
       }
-      
+
       if (!have_acc_flag) {
-        start_receive();        
+        start_receive();
         return;
       }
 
       {
           boost::mutex::scoped_lock lock(mutex);
-          
+
       /* Fill the IMU message */
-      
+
       // Fill the header
       // imu_msg.header.stamp = enable_Tsync ? ros::Time(pstampSynchronizer->sync(data.timeStamp, ros::Time::now().toSec(), data.frameCount)) : ros::Time::now();
 
@@ -161,7 +161,7 @@ private:
       imu_msg.orientation.y = 0.0;
       imu_msg.orientation.z = 0.0;
       imu_msg.orientation.w = 1.0;
-      
+
       imu_msg.orientation_covariance[0] = -1.0;
 
       imu_msg.angular_velocity.x = gx*M_PI/180.0;
@@ -171,11 +171,11 @@ private:
       imu_msg.angular_velocity_covariance[0] = 0.0;
       imu_msg.angular_velocity_covariance[1] = 0.0;
       imu_msg.angular_velocity_covariance[2] = 0.0;
-      
+
       imu_msg.angular_velocity_covariance[3] = 0.0;
       imu_msg.angular_velocity_covariance[4] = 0.0;
       imu_msg.angular_velocity_covariance[5] = 0.0;
-      
+
       imu_msg.angular_velocity_covariance[6] = 0.0;
       imu_msg.angular_velocity_covariance[7] = 0.0;
       imu_msg.angular_velocity_covariance[8] = 0.0;
@@ -197,7 +197,7 @@ private:
       /* Fill the magnetometer message */
       //      mag_msg.header.stamp = imu_msg.header.stamp;
       //      mag_msg.header.frame_id = frame_id;
-      
+
       // Units are microTesla in the LPMS library, Tesla in ROS.
       //      mag_msg.magnetic_field.x = data.b[0]*1e-6;
       //      mag_msg.magnetic_field.y = data.b[1]*1e-6;
@@ -205,7 +205,7 @@ private:
       }
 
       //      mag_pub.publish(mag_msg);
-      
+
       start_receive();
     }
   }
@@ -224,7 +224,7 @@ private:
   bool have_acc_flag;
   bool have_pos_flag;
 };
-  
+
 void runUDPServer(int port) {
   try {
     boost::asio::io_service io_service;
@@ -232,7 +232,7 @@ void runUDPServer(int port) {
     // Construct a signal set registered for process termination.
     boost::asio::signal_set signals(io_service, SIGINT, SIGTERM);
     signals.async_wait(handler);
-    
+
     io_service.run();
   }
   catch (std::exception& e) {
@@ -270,7 +270,7 @@ int main(int argc, char *argv[]) {
   ROS_INFO("     Port: %d", port);
   ROS_INFO(" Frame Id: %s", frame_id.c_str());
   ROS_INFO("     Rate: %d", rate);
-  ROS_INFO("Root pass: %s", root_passwd.c_str());  
+  ROS_INFO("Root pass: %s", root_passwd.c_str());
 
   int gyro_div = 2000/rate;
   int accel_div = 1000/rate;
@@ -284,18 +284,18 @@ int main(int argc, char *argv[]) {
   std::ostringstream osstop;
 
   osstop << "http://" << camera_ip << "/local/motionloggerudp/control.cgi?cmd=stop";
-  
+
 
   os << "http://" << camera_ip
      << "/local/motionloggerudp/control.cgi?cmd=start&target_ip=" << driver_ip
      << "&target_port=" << port
-     << "&accel_enabled=1&accel_rate_div=" << accel_div << "&gyro_enabled=1&gyro_rate_div=" << gyro_div << "&pos_enabled=1"
+     << "&accel_enabled=1&accel_rate_div=" << accel_div << "&gyro_enabled=1&gyro_rate_div=" << gyro_div
     ;
 
   std::string stopurl = osstop.str();
   std::string url = os.str();
 
-  // ROS_INFO("STOPCOMMAND URL: %s", stopurl.c_str());
+  ROS_INFO("STOPCOMMAND URL: %s", stopurl.c_str());
   ROS_INFO("COMMAND URL: %s", url.c_str());
 
   CURL *curl;
@@ -304,17 +304,27 @@ int main(int argc, char *argv[]) {
   curl = curl_easy_init();
 
   if (curl) {
-    curl_easy_setopt(curl, CURLOPT_URL, url.c_str());
-    /* example.com is redirected, so we tell libcurl to follow redirection */ 
     curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1L);
     //    curl_easy_setopt(curl, CURLOPT_HTTPAUTH, CURLAUTH_DIGEST);
     curl_easy_setopt(curl, CURLOPT_HTTPAUTH, CURLAUTH_ANY);
     curl_easy_setopt(curl, CURLOPT_USERNAME, "root");
     curl_easy_setopt(curl, CURLOPT_PASSWORD, root_passwd.c_str());
- 
-    /* Perform the request, res will get the return code */ 
+
+    curl_easy_setopt(curl, CURLOPT_URL, stopurl.c_str());
+    /* Perform the request, res will get the return code */
+    res = curl_easy_perform(curl);
+    /* Check for errors */
+    if(res != CURLE_OK) {
+      fprintf(stderr, "curl_easy_perform() failed: %s\n",
+              curl_easy_strerror(res));
+    } else {
+      ROS_INFO("Curl call succeeded");
+    }
+
+    curl_easy_setopt(curl, CURLOPT_URL, url.c_str());
+    /* Perform the request, res will get the return code */
     res = curl_easy_perform(curl);
-    /* Check for errors */ 
+    /* Check for errors */
     if(res != CURLE_OK) {
       fprintf(stderr, "curl_easy_perform() failed: %s\n",
               curl_easy_strerror(res));
@@ -322,18 +332,19 @@ int main(int argc, char *argv[]) {
       ROS_INFO("Curl call succeeded");
     }
 
+
 #if 0
     sleep(3);
     curl_easy_setopt(curl, CURLOPT_URL, url.c_str());
-    /* Check for errors */ 
+    /* Check for errors */
     if(res != CURLE_OK) {
       fprintf(stderr, "curl_easy_perform() failed: %s\n",
               curl_easy_strerror(res));
     } else {
       ROS_INFO("Curl call succeeded");
     }
-#endif    
-    /* always cleanup */ 
+#endif
+    /* always cleanup */
     curl_easy_cleanup(curl);
   } else {
     ROS_ERROR("Could not create curl object, exiting");
@@ -345,13 +356,13 @@ int main(int argc, char *argv[]) {
 
   ros::Timer timer = nh.createTimer(ros::Duration(1.0/rate), timer_callback);
 
-  boost::thread sthread (spin_thread);  
-  
+  boost::thread sthread (spin_thread);
+
   ROS_INFO("runUDPServer");
 
   //runUDPServer(port);
 
   runUDPServer(port);
-  
+
   return 0;
 }