Adding parameters.

src/axis_imu.cc

@@ -19,6 +19,9 @@
 std::string frame_id;
 
 ros::Publisher imu_pub;
+sensor_msgs::Imu imu_msg;
+boost::mutex mutex;
+
 
 // 1 byte header ('A' eller 'G') | float64 timestamp | float64 x | float64 y | float64 z
 //
@@ -36,6 +39,7 @@ ros::Publisher imu_pub;
 //
 // Max sample rate för gyrot är 2kHz och för accelerometern 1kHz, och detta styrs med gyro_rate_div och accel_rate_div i ovanstående http-anrop.
 //
+// Fix rate ut, 1kHz
 
 
 
@@ -137,8 +141,9 @@ private:
         return;
       }
 
-      sensor_msgs::Imu imu_msg;
-
+      {
+          boost::mutex::scoped_lock lock(mutex);
+          
       /* Fill the IMU message */
       
       // Fill the header
@@ -177,10 +182,8 @@ private:
       //      mag_msg.magnetic_field.x = data.b[0]*1e-6;
       //      mag_msg.magnetic_field.y = data.b[1]*1e-6;
       //      mag_msg.magnetic_field.z = data.b[2]*1e-6;
+      }
 
-      // ROS_ERROR("publish imu");
-      // Publish the messages
-      imu_pub.publish(imu_msg);
       //      mag_pub.publish(mag_msg);
       
       start_receive();
@@ -216,6 +219,13 @@ void runUDPServer(int port) {
   }
 }
 
+void timer_callback(const ros::TimerEvent&) {
+  boost::mutex::scoped_lock lock(mutex);
+  //  ROS_ERROR("publish imu");
+  // Publish the messages
+  imu_pub.publish(imu_msg);
+}
+
 int main(int argc, char *argv[]) {
 
   ros::init(argc, argv, "axis_imu");
@@ -244,14 +254,20 @@ int main(int argc, char *argv[]) {
   int gyro_div = 2000/rate;
   int accel_div = 1000/rate;
 
+  // currently fixed rate out so set div to 0
+
+  gyro_div = 0;
+  accel_div = 0;
+
   std::ostringstream os;
 
-  //  os << "http://" << "root:" << root_passwd << "@" << camera_ip
   os << "http://" << camera_ip
      << "/local/motionloggerudp/control.cgi?cmd=start&target_ip=" << driver_ip
      << "&accel_enabled=1&accel_rate_div=" << accel_div << "&gyro_enabled=1&gyro_rate_div=" << gyro_div;
   std::string url = os.str();
 
+  ROS_INFO("COMMAND URL: %s", url.c_str());
+
   CURL *curl;
   CURLcode res;
 
@@ -261,6 +277,10 @@ int main(int argc, char *argv[]) {
     curl_easy_setopt(curl, CURLOPT_URL, url);
     /* 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);
  
     /* Perform the request, res will get the return code */ 
     res = curl_easy_perform(curl);
@@ -279,10 +299,11 @@ int main(int argc, char *argv[]) {
     exit(1);
   }
 
-  ROS_INFO("COMMAND URL: %s", url.c_str());
 
   imu_pub = nh.advertise<sensor_msgs::Imu>("imu",1);
 
+  ros::Timer timer = nh.createTimer(ros::Duration(1.0/rate), timer_callback);
+
   boost::thread sthread (spin_thread);  
   
   ROS_ERROR("runUDPServer");