From f5044c7ec8d3691a7bf18945d750b906d8195668 Mon Sep 17 00:00:00 2001
From: Jacob Perron <jacobmperron@gmail.com>
Date: Tue, 27 Aug 2019 00:11:29 -0700
Subject: [PATCH] Disconnect from serial cleanly on SIGINT

Send the STOP opcode before exiting the program to ensure the robot is not left in a state that could potentially drain the battery.

Signed-off-by: Jacob Perron <jacobmperron@gmail.com>
---
 include/create/serial.h |  3 ++-
 src/create.cpp          |  2 --
 src/serial.cpp          | 15 +++++++++++++++
 3 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/include/create/serial.h b/include/create/serial.h
index 28b5f69..6ae9eba 100644
--- a/include/create/serial.h
+++ b/include/create/serial.h
@@ -51,6 +51,7 @@ namespace create {
 
     protected:
       boost::asio::io_service io;
+      boost::asio::signal_set signals;
       boost::asio::serial_port port;
 
     private:
@@ -62,7 +63,6 @@ namespace create {
       bool firstRead;
       uint8_t byteRead;
 
-
       // Callback executed when data arrives from Create
       void onData(const boost::system::error_code& e, const std::size_t& size);
       // Callback to execute once data arrives
@@ -80,6 +80,7 @@ namespace create {
       virtual bool startSensorStream() = 0;
       virtual void processByte(uint8_t byteRead) = 0;
 
+      void signalHandler(const boost::system::error_code& error, int signal_number);
       // Notifies main thread that data is fresh and makes the user callback
       void notifyDataReady();
 
diff --git a/src/create.cpp b/src/create.cpp
index e71a556..f908661 100644
--- a/src/create.cpp
+++ b/src/create.cpp
@@ -15,8 +15,6 @@ namespace create {
 
   namespace ublas = boost::numeric::ublas;
 
-  // TODO: Handle SIGINT to do clean disconnect
-
   void Create::init() {
     mainMotorPower = 0;
     sideMotorPower = 0;
diff --git a/src/serial.cpp b/src/serial.cpp
index 9eba401..8cf27eb 100644
--- a/src/serial.cpp
+++ b/src/serial.cpp
@@ -8,6 +8,7 @@ namespace create {
   Serial::Serial(boost::shared_ptr<Data> d) :
     data(d),
     port(io),
+    signals(io, SIGINT, SIGTERM),
     isReading(false),
     dataReady(false),
     corruptPackets(0),
@@ -18,6 +19,18 @@ namespace create {
     disconnect();
   }
 
+  void Serial::signalHandler(const boost::system::error_code& error, int signal_number) {
+    if (!error) {
+      if (connected()) {
+        // Ensure not in Safe/Full modes
+        sendOpcode(OC_START);
+        // Stop OI
+        sendOpcode(OC_STOP);
+        exit(signal_number);
+      }
+    }
+  }
+
   bool Serial::connect(const std::string& portName, const int& baud, boost::function<void()> cb) {
     using namespace boost::asio;
     port.open(portName);
@@ -27,6 +40,8 @@ namespace create {
     port.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
     port.set_option(serial_port::flow_control(serial_port::flow_control::none));
 
+    signals.async_wait(boost::bind(&Serial::signalHandler, this, _1, _2));
+
     usleep(1000000);
 
     if (port.is_open()) {