Skip to content
Snippets Groups Projects
Commit f7227d56 authored by Tommy Persson's avatar Tommy Persson
Browse files

Adding signal handling

parent d76619d4
No related branches found
No related tags found
No related merge requests found
......@@ -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
......
#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;
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment