axis_imu.cc

#include "ros/ros.h"

#include <boost/thread.hpp>

#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"

#include <iostream>
#include <exception>
#include <boost/array.hpp>
#include <boost/asio.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/enable_shared_from_this.hpp>
#include <boost/asio.hpp>
#include <boost/bind.hpp>

std::string frame_id;

ros::Publisher imu_pub;

// 1 byte header ('A' eller 'G') | float64 timestamp | float64 x | float64 y | float64 z
//
// Headern 'A' och 'G' står för accelerometer respektive gyro.
// timestamp är sekunder sedan kamerans uppstart
// x, y och z är i grader/s för gyrot och G för accelerometern.
//
// Data sent in network byte order
//


using namespace std;

void handler(const boost::system::error_code& error,
             int signal_number) {
  if (!error) {
    // A signal occurred.
    exit(0);
  }
}

void spin_thread () {
  ROS_ERROR("Before Spin in spin_thread");
  ros::spin();
}

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;
  std::copy(reinterpret_cast<const char*>(&byte_array[0]),
            reinterpret_cast<const char*>(&byte_array[4]),
            reinterpret_cast<char*>(&result));
  return result;
} 


class udp_server {
public:
  udp_server(boost::asio::io_service& io_service, int port_number)
    : socket_(io_service, boost::asio::ip::udp::udp::endpoint(boost::asio::ip::udp::udp::v4(), port_number)) {
    std::cout << "UDP server listening on " << port_number << std::endl;
    have_gyro_flag = false;
    have_acc_flag = false;
    start_receive();
  }

private:

  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],
                                   recv_buffer_[i+5],
                                   recv_buffer_[i+4],
                                   recv_buffer_[i+3],
                                   recv_buffer_[i+2],
                                   recv_buffer_[i+1],
                                   recv_buffer_[i+0]}; //netw byte order
    double result;
    std::copy(reinterpret_cast<const char*>(&byte_array[0]),
              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,
                                           boost::asio::placeholders::error,
                                           boost::asio::placeholders::bytes_transferred));
  }

  void handle_receive(const boost::system::error_code& error,
                      std::size_t bytes_transferred) {
    if (!error || error == boost::asio::error::message_size) {
      // cerr << "Got message: " << bytes_transferred << " - " << recv_buffer_[0] << endl;
      //      float time = bytesToFloat(recv_buffer_[1], recv_buffer_[2], recv_buffer_[3], recv_buffer_[4]);
      double gtime = bytesToDouble(1);
      //float ax, ay, az, gx, gy, gz;
      double ax, ay, az, gx, gy, gz;
      gx = bytesToDouble(9);
      gy = bytesToDouble(17);
      gz = bytesToDouble(25);

      double atime = bytesToDouble(33);
      ax = bytesToDouble(41);
      ay = bytesToDouble(49);
      az = bytesToDouble(57);
      if (recv_buffer_[0] == 'G') {
        have_gyro_flag = true;
        ROS_ERROR("         GYRO: %f - %f %f %f", time, gx, gy, gz);        
      }
      if (recv_buffer_[32] == 'A') {
        have_acc_flag = true;
        ROS_ERROR("ACCELEROMETER: %f - %f %f %f", time, ax, ay, az);
      }

      if (!have_gyro_flag) {
        start_receive();        
        return;
      }
      
      if (!have_acc_flag) {
        start_receive();        
        return;
      }

      sensor_msgs::Imu imu_msg;

      /* 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();

      imu_msg.header.stamp = ros::Time::now();
      imu_msg.header.frame_id = frame_id;

      // Fill orientation quaternion
      //      imu_msg.orientation.w = data.q[0];
      //      imu_msg.orientation.x = -data.q[1];
      //      imu_msg.orientation.y = -data.q[2];
      //      imu_msg.orientation.z = -data.q[3];

      // Fill angular velocity data
      // - scale from deg/s to rad/s
      imu_msg.angular_velocity.x = gx*3.1415926/180;
      imu_msg.angular_velocity.y = gy*3.1415926/180;
      imu_msg.angular_velocity.z = gz*3.1415926/180;

      // Fill linear acceleration data
      imu_msg.linear_acceleration.x = -ax*9.81;
      imu_msg.linear_acceleration.y = -ay*9.81;
      imu_msg.linear_acceleration.z = -az*9.81;
      
      // \TODO: Fill covariance matrices
      // msg.orientation_covariance = ...
      // msg.angular_velocity_covariance = ...
      // msg linear_acceleration_covariance = ...
      
      /* 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;
      //      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();
    }
  }

  void handle_send(boost::shared_ptr<std::string> /*message*/,
                   const boost::system::error_code& /*error*/,
                   std::size_t bytes_transferred) {
    //    ROS_ERROR("SENT: %zu", bytes_transferred);
  }

  boost::asio::ip::udp::udp::socket socket_;
  boost::asio::ip::udp::udp::endpoint remote_endpoint_;
  boost::array<char, 128> recv_buffer_;

  bool have_gyro_flag;
  bool have_acc_flag;
};
  
void runUDPServer(int port) {
  try {
    boost::asio::io_service io_service;
    udp_server server(io_service, 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) {
    std::cerr << e.what() << std::endl;
  }
}

int main(int argc, char *argv[]) {

  ros::init(argc, argv, "axis_imu");
  ros::NodeHandle nh, private_nh;

  int port;
  int rate;

  private_nh.param<int>("port", port, 5000);
  private_nh.param<std::string>("frame_id", frame_id, "imu");
  private_nh.param("rate", rate, 200);

  imu_pub = nh.advertise<sensor_msgs::Imu>("imu",1);

  boost::thread sthread (spin_thread);  
  
  ROS_ERROR("runUDPServer");

  runUDPServer(port);
  
  return 0;
}