diff --git a/data/Makefile b/data/Makefile index 3e55543cef107dfe19873d4ef6a71ac0010f0ca8..3484a560ea5fb54acfe1589b5b5d329ddc672c5a 100644 --- a/data/Makefile +++ b/data/Makefile @@ -2,6 +2,9 @@ terra8: tcprewrite --infile=udp_dump_port_5000.pcap --outfile=changed.pcap --dstipmap=0.0.0.0/0:192.168.7.160/32 --fixcsum +laptop: + tcprewrite --infile=mypcap.pcap --outfile=changed.pcap --dstipmap=0.0.0.0/0:192.168.1.136/32 --fixcsum + replay-terra8: udpreplay -i ens1 -c 1000 changed.pcap diff --git a/src/axis_imu.cc b/src/axis_imu.cc index 3a3291ae540b7dc916845da837878dbaf08be75a..15b9d06ef224a6a1bd5b84c9d546898a9adb24d0 100644 --- a/src/axis_imu.cc +++ b/src/axis_imu.cc @@ -1,5 +1,7 @@ #include "ros/ros.h" +#include <boost/thread.hpp> + #include "sensor_msgs/Imu.h" #include "sensor_msgs/MagneticField.h" @@ -25,6 +27,19 @@ ros::Publisher imu_pub; 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 }; @@ -37,13 +52,12 @@ float bytesToFloat(unsigned char b0, unsigned char b1, unsigned char b2, unsign 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)) - { + 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; + start_receive(); } private: @@ -125,7 +139,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); @@ -152,6 +167,10 @@ 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) { @@ -173,13 +192,11 @@ int main(int argc, char *argv[]) { imu_pub = nh.advertise<sensor_msgs::Imu>("imu",1); - ROS_ERROR("Before runUDPServer"); + boost::thread sthread (spin_thread); + + ROS_ERROR("runUDPServer"); runUDPServer(port); - - ROS_ERROR("Before Spin"); - - ros::spin(); return 0; }