diff --git a/data/Makefile b/data/Makefile
index 58b384c1d3bcfdcc8b87db1adb709c0473a2db62..3e55543cef107dfe19873d4ef6a71ac0010f0ca8 100644
--- a/data/Makefile
+++ b/data/Makefile
@@ -3,7 +3,7 @@ terra8:
tcprewrite --infile=udp_dump_port_5000.pcap --outfile=changed.pcap --dstipmap=0.0.0.0/0:192.168.7.160/32 --fixcsum
replay-terra8:
- udpreplay -l -i ens1 changed.pcap
+ udpreplay -i ens1 -c 1000 changed.pcap
replay:
- udpreplay -l -i wlp2s0 changed.pcap
+ udpreplay -i wlp2s0 -c 1000 changed.pcap
diff --git a/src/axis_imu.cc b/src/axis_imu.cc
new file mode 100644
index 0000000000000000000000000000000000000000..3a3291ae540b7dc916845da837878dbaf08be75a
--- /dev/null
+++ b/src/axis_imu.cc
@@ -0,0 +1,185 @@
+#include "ros/ros.h"
+
+#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') | float32 timestamp | float32 x | float32 y | float32 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.
+
+
+using namespace std;
+
+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;
+ start_receive();
+ have_gyro_flag = false;
+ have_acc_flag = false;
+ }
+
+private:
+ 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]);
+ float ax, ay, az, gx, gy, gz;
+ if (recv_buffer_[0] == 'A') {
+ have_acc_flag = true;
+ ax = bytesToFloat(recv_buffer_[5], recv_buffer_[6], recv_buffer_[7], recv_buffer_[8]);
+ ay = bytesToFloat(recv_buffer_[9], recv_buffer_[10], recv_buffer_[11], recv_buffer_[12]);
+ az = bytesToFloat(recv_buffer_[13], recv_buffer_[14], recv_buffer_[15], recv_buffer_[16]);
+ ROS_ERROR("ACCELEROMETER: %f - %f %f %f", time, ax, ay, az);
+ }
+ if (recv_buffer_[0] == 'G') {
+ have_gyro_flag = true;
+ gx = bytesToFloat(recv_buffer_[5], recv_buffer_[6], recv_buffer_[7], recv_buffer_[8]);
+ gy = bytesToFloat(recv_buffer_[9], recv_buffer_[10], recv_buffer_[11], recv_buffer_[12]);
+ gz = bytesToFloat(recv_buffer_[13], recv_buffer_[14], recv_buffer_[15], recv_buffer_[16]);
+ ROS_ERROR(" GYRO: %f - %f %f %f", time, gx, gy, gz);
+ }
+
+ 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;
+
+ // 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);
+ 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);
+
+ ROS_ERROR("Before runUDPServer");
+
+ runUDPServer(port);
+
+ ROS_ERROR("Before Spin");
+
+ ros::spin();
+
+ return 0;
+}