From c8d91019df84bff302d374aa881633d8182eb23b Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 30 Oct 2020 19:42:35 -0700 Subject: [PATCH 1/2] Replace boost features with C++11 equivalents Signed-off-by: Jacob Perron --- include/create/create.h | 6 +++--- include/create/data.h | 7 +++---- include/create/packet.h | 6 +++--- include/create/serial.h | 27 ++++++++++++++------------- include/create/serial_query.h | 8 +++----- include/create/serial_stream.h | 8 ++------ src/create.cpp | 11 +++++------ src/data.cpp | 10 +++++----- src/packet.cpp | 10 ++++++---- src/serial.cpp | 30 ++++++++++++++++++++---------- src/serial_query.cpp | 6 ++++-- src/serial_stream.cpp | 3 ++- tests/test_create.cpp | 2 -- tests/test_data.cpp | 18 +++++++++--------- tests/test_serial_query.cpp | 14 +++++++------- tests/test_serial_stream.cpp | 14 +++++++------- 16 files changed, 93 insertions(+), 87 deletions(-) diff --git a/include/create/create.h b/include/create/create.h index 14ede01..6ca2fe9 100644 --- a/include/create/create.h +++ b/include/create/create.h @@ -32,9 +32,9 @@ POSSIBILITY OF SUCH DAMAGE. #ifndef CREATE_H #define CREATE_H -#include #include #include +#include #include #include @@ -96,8 +96,8 @@ namespace create { bool updateLEDs(); protected: - boost::shared_ptr data; - boost::shared_ptr serial; + std::shared_ptr data; + std::shared_ptr serial; public: /** diff --git a/include/create/data.h b/include/create/data.h index 345bd1a..c4392e8 100644 --- a/include/create/data.h +++ b/include/create/data.h @@ -32,9 +32,8 @@ POSSIBILITY OF SUCH DAMAGE. #ifndef CREATE_DATA_H #define CREATE_DATA_H -#include -#include #include +#include #include #include "create/packet.h" @@ -43,7 +42,7 @@ POSSIBILITY OF SUCH DAMAGE. namespace create { class Data { private: - std::map > packets; + std::map > packets; uint32_t totalDataBytes; std::vector ids; @@ -52,7 +51,7 @@ namespace create { ~Data(); bool isValidPacketID(const uint8_t id) const; - boost::shared_ptr getPacket(const uint8_t id); + std::shared_ptr getPacket(const uint8_t id); void validateAll(); uint32_t getTotalDataBytes() const; uint8_t getNumPackets() const; diff --git a/include/create/packet.h b/include/create/packet.h index da0a6e2..d1e7928 100644 --- a/include/create/packet.h +++ b/include/create/packet.h @@ -31,15 +31,15 @@ POSSIBILITY OF SUCH DAMAGE. #ifndef CREATE_PACKET_H #define CREATE_PACKET_H -#include +#include namespace create { class Packet { private: uint16_t data; uint16_t tmpData; - mutable boost::mutex dataMutex; - mutable boost::mutex tmpDataMutex; + mutable std::mutex dataMutex; + mutable std::mutex tmpDataMutex; protected: // Thread safe diff --git a/include/create/serial.h b/include/create/serial.h index 6ae9eba..5280040 100644 --- a/include/create/serial.h +++ b/include/create/serial.h @@ -35,19 +35,20 @@ POSSIBILITY OF SUCH DAMAGE. #ifndef CREATE_SERIAL_H #define CREATE_SERIAL_H +#include +#include +#include +#include +#include + #include -#include -#include -#include -#include -#include #include "create/data.h" #include "create/types.h" #include "create/util.h" namespace create { - class Serial : public boost::enable_shared_from_this { + class Serial : public std::enable_shared_from_this { protected: boost::asio::io_service io; @@ -55,9 +56,9 @@ namespace create { boost::asio::serial_port port; private: - boost::thread ioThread; - boost::condition_variable dataReadyCond; - boost::mutex dataReadyMut; + std::thread ioThread; + std::condition_variable dataReadyCond; + std::mutex dataReadyMut; bool dataReady; bool isReading; bool firstRead; @@ -66,13 +67,13 @@ namespace create { // 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 - boost::function callback; + std::function callback; // Start and stop reading data from Create bool startReading(); void stopReading(); protected: - boost::shared_ptr data; + std::shared_ptr data; // These are for possible diagnostics uint64_t corruptPackets; uint64_t totalPackets; @@ -85,9 +86,9 @@ namespace create { void notifyDataReady(); public: - Serial(boost::shared_ptr data); + Serial(std::shared_ptr data); ~Serial(); - bool connect(const std::string& port, const int& baud = 115200, boost::function cb = 0); + bool connect(const std::string& port, const int& baud = 115200, std::function cb = 0); void disconnect(); inline bool connected() const { return port.is_open(); }; bool send(const uint8_t* bytes, const uint32_t numBytes); diff --git a/include/create/serial_query.h b/include/create/serial_query.h index 033785a..a10b34e 100644 --- a/include/create/serial_query.h +++ b/include/create/serial_query.h @@ -36,11 +36,9 @@ POSSIBILITY OF SUCH DAMAGE. #ifndef CREATE_SERIAL_QUERY_H #define CREATE_SERIAL_QUERY_H +#include + #include -#include -#include -#include -#include #include "create/data.h" #include "create/types.h" @@ -69,7 +67,7 @@ namespace create { void processByte(uint8_t byteRead); public: - SerialQuery(boost::shared_ptr data); + SerialQuery(std::shared_ptr data); }; } // namespace create diff --git a/include/create/serial_stream.h b/include/create/serial_stream.h index d59d126..07d4c05 100644 --- a/include/create/serial_stream.h +++ b/include/create/serial_stream.h @@ -35,11 +35,7 @@ POSSIBILITY OF SUCH DAMAGE. #ifndef CREATE_SERIAL_STREAM_H #define CREATE_SERIAL_STREAM_H -#include -#include -#include -#include -#include +#include #include "create/data.h" #include "create/types.h" @@ -73,7 +69,7 @@ namespace create { void processByte(uint8_t byteRead); public: - SerialStream(boost::shared_ptr data, const uint8_t& header = create::util::STREAM_HEADER); + SerialStream(std::shared_ptr data, const uint8_t& header = create::util::STREAM_HEADER); }; } // namespace create diff --git a/src/create.cpp b/src/create.cpp index a0f9da6..35d7f39 100644 --- a/src/create.cpp +++ b/src/create.cpp @@ -1,8 +1,7 @@ -#include -#include #include #include #include +#include #include #include "create/create.h" @@ -42,11 +41,11 @@ namespace create { poseCovar = Matrix(3, 3, 0.0); requestedLeftVel = 0; requestedRightVel = 0; - data = boost::shared_ptr(new Data(model.getVersion())); + data = std::shared_ptr(new Data(model.getVersion())); if (model.getVersion() == V_1) { - serial = boost::make_shared(data); + serial = std::make_shared(data); } else { - serial = boost::make_shared(data); + serial = std::make_shared(data); } } @@ -273,7 +272,7 @@ namespace create { float maxWait = 30; // seconds float retryInterval = 5; //seconds time(&start); - while (!serial->connect(port, baud, boost::bind(&Create::onData, this)) && !timeout) { + while (!serial->connect(port, baud, std::bind(&Create::onData, this)) && !timeout) { time(&now); if (difftime(now, start) > maxWait) { timeout = true; diff --git a/src/data.cpp b/src/data.cpp index f8eab92..3bc7f38 100644 --- a/src/data.cpp +++ b/src/data.cpp @@ -1,6 +1,6 @@ #include "create/data.h" -#define ADD_PACKET(id,nbytes,info,enabledVersion) if ((enabledVersion) & version) packets[id]=boost::make_shared(nbytes,info) +#define ADD_PACKET(id,nbytes,info,enabledVersion) if ((enabledVersion) & version) packets[id]=std::make_shared(nbytes,info) namespace create { @@ -45,7 +45,7 @@ namespace create { ADD_PACKET(ID_STASIS, 1, "stasis", V_3); totalDataBytes = 0; - for (std::map >::iterator it = packets.begin(); + for (std::map >::iterator it = packets.begin(); it != packets.end(); ++it) { ids.push_back(it->first); @@ -62,15 +62,15 @@ namespace create { return false; } - boost::shared_ptr Data::getPacket(uint8_t id) { + std::shared_ptr Data::getPacket(uint8_t id) { if (isValidPacketID(id)) { return packets[id]; } - return boost::shared_ptr(); + return std::shared_ptr(); } void Data::validateAll() { - for (std::map >::iterator it = packets.begin(); + for (std::map >::iterator it = packets.begin(); it != packets.end(); ++it) { it->second->validate(); diff --git a/src/packet.cpp b/src/packet.cpp index d408f75..517e90d 100644 --- a/src/packet.cpp +++ b/src/packet.cpp @@ -1,3 +1,5 @@ +#include + #include "create/packet.h" namespace create { @@ -11,22 +13,22 @@ namespace create { Packet::~Packet() { } void Packet::setDataToValidate(const uint16_t& tmp) { - boost::mutex::scoped_lock lock(tmpDataMutex); + std::lock_guard lock(tmpDataMutex); tmpData = tmp; } void Packet::validate() { - boost::mutex::scoped_lock lock(tmpDataMutex); + std::lock_guard lock(tmpDataMutex); setData(tmpData); } void Packet::setData(const uint16_t& d) { - boost::mutex::scoped_lock lock(dataMutex); + std::lock_guard lock(dataMutex); data = d; } uint16_t Packet::getData() const { - boost::mutex::scoped_lock lock(dataMutex); + std::lock_guard lock(dataMutex); return data; } diff --git a/src/serial.cpp b/src/serial.cpp index ad9f7fc..2a87a8a 100644 --- a/src/serial.cpp +++ b/src/serial.cpp @@ -1,3 +1,5 @@ +#include +#include #include #include "create/serial.h" @@ -5,7 +7,7 @@ namespace create { - Serial::Serial(boost::shared_ptr d) : + Serial::Serial(std::shared_ptr d) : signals(io, SIGINT, SIGTERM), port(io), dataReady(false), @@ -31,7 +33,7 @@ namespace create { } } - bool Serial::connect(const std::string& portName, const int& baud, boost::function cb) { + bool Serial::connect(const std::string& portName, const int& baud, std::function cb) { using namespace boost::asio; port.open(portName); port.set_option(serial_port::baud_rate(baud)); @@ -40,7 +42,7 @@ 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)); + signals.async_wait(std::bind(&Serial::signalHandler, this, std::placeholders::_1, std::placeholders::_2)); usleep(1000000); @@ -90,17 +92,22 @@ namespace create { // Start continuously reading one byte at a time boost::asio::async_read(port, boost::asio::buffer(&byteRead, 1), - boost::bind(&Serial::onData, shared_from_this(), _1, _2)); + std::bind(&Serial::onData, + shared_from_this(), + std::placeholders::_1, + std::placeholders::_2)); - ioThread = boost::thread(boost::bind(&boost::asio::io_service::run, &io)); + ioThread = std::thread(std::bind( + static_cast( + &boost::asio::io_service::run), &io)); // Wait for first complete read to finish - boost::unique_lock lock(dataReadyMut); + std::unique_lock lock(dataReadyMut); int attempts = 1; int maxAttempts = 10; while (!dataReady) { - if (!dataReadyCond.timed_wait(lock, boost::get_system_time() + boost::posix_time::milliseconds(500))) { + if (dataReadyCond.wait_for(lock, std::chrono::milliseconds(500)) == std::cv_status::timeout) { if (attempts >= maxAttempts) { CERR("[create::Serial] ", "failed to receive data from Create. Check if robot is powered!"); io.stop(); @@ -125,7 +132,7 @@ namespace create { ioThread.join(); isReading = false; { - boost::lock_guard lock(dataReadyMut); + std::lock_guard lock(dataReadyMut); dataReady = false; } } @@ -138,7 +145,7 @@ namespace create { // Notify first data packets ready { - boost::lock_guard lock(dataReadyMut); + std::lock_guard lock(dataReadyMut); if (!dataReady) { dataReady = true; dataReadyCond.notify_one(); @@ -163,7 +170,10 @@ namespace create { // Read the next byte boost::asio::async_read(port, boost::asio::buffer(&byteRead, 1), - boost::bind(&Serial::onData, shared_from_this(), _1, _2)); + std::bind(&Serial::onData, + shared_from_this(), + std::placeholders::_1, + std::placeholders::_2)); } bool Serial::send(const uint8_t* bytes, unsigned int numBytes) { diff --git a/src/serial_query.cpp b/src/serial_query.cpp index 987cd26..2d3997b 100644 --- a/src/serial_query.cpp +++ b/src/serial_query.cpp @@ -1,4 +1,5 @@ #include +#include #include "create/serial_query.h" #include "create/types.h" @@ -7,7 +8,7 @@ namespace create { - SerialQuery::SerialQuery(boost::shared_ptr d) : Serial(d), + SerialQuery::SerialQuery(std::shared_ptr d) : Serial(d), streamRecoveryTimer(io), packetID(ID_BUMP_WHEELDROP), packetByte(0), @@ -30,7 +31,8 @@ namespace create { send(requestPacket, 2); // Automatically resend request if no response is received streamRecoveryTimer.expires_from_now(boost::posix_time::milliseconds(50)); - streamRecoveryTimer.async_wait(boost::bind(&SerialQuery::restartSensorStream, this, _1)); + streamRecoveryTimer.async_wait( + std::bind(&SerialQuery::restartSensorStream, this, std::placeholders::_1)); } void SerialQuery::restartSensorStream(const boost::system::error_code& err) { diff --git a/src/serial_stream.cpp b/src/serial_stream.cpp index 6cd8ea3..b22a07e 100644 --- a/src/serial_stream.cpp +++ b/src/serial_stream.cpp @@ -1,11 +1,12 @@ #include +#include #include "create/serial_stream.h" #include "create/types.h" namespace create { - SerialStream::SerialStream(boost::shared_ptr d, const uint8_t& header) : Serial(d), readState(READ_HEADER), headerByte(header) { + SerialStream::SerialStream(std::shared_ptr d, const uint8_t& header) : Serial(d), readState(READ_HEADER), headerByte(header) { } bool SerialStream::startSensorStream() { diff --git a/tests/test_create.cpp b/tests/test_create.cpp index 57e76e8..aaa8a26 100644 --- a/tests/test_create.cpp +++ b/tests/test_create.cpp @@ -33,8 +33,6 @@ POSSIBILITY OF SUCH DAMAGE. #include "gtest/gtest.h" -#include - TEST(CreateTest, ConstructorSingleParam) { create::Create create_default; diff --git a/tests/test_data.cpp b/tests/test_data.cpp index 8f893fe..fd7f79b 100644 --- a/tests/test_data.cpp +++ b/tests/test_data.cpp @@ -34,7 +34,7 @@ POSSIBILITY OF SUCH DAMAGE. #include "gtest/gtest.h" -#include +#include TEST(DataTest, Constructor) { @@ -76,31 +76,31 @@ TEST(DataTest, GetPacket) { // Get a packet exclusive to V_1 create::Data data_v_1(create::V_1); - boost::shared_ptr v_1_packet_ptr = data_v_1.getPacket(create::ID_OVERCURRENTS); - EXPECT_NE(v_1_packet_ptr, boost::shared_ptr()) + std::shared_ptr v_1_packet_ptr = data_v_1.getPacket(create::ID_OVERCURRENTS); + EXPECT_NE(v_1_packet_ptr, std::shared_ptr()) << "ID_OVERCURRENTS packet not found for protocol V_1"; EXPECT_EQ(static_cast(v_1_packet_ptr->nbytes), 1); EXPECT_EQ(v_1_packet_ptr->info, std::string("overcurrents")); // Get a packet for V_2 create::Data data_v_2(create::V_2); - boost::shared_ptr v_2_packet_ptr = data_v_2.getPacket(create::ID_DISTANCE); - EXPECT_NE(v_2_packet_ptr, boost::shared_ptr()) + std::shared_ptr v_2_packet_ptr = data_v_2.getPacket(create::ID_DISTANCE); + EXPECT_NE(v_2_packet_ptr, std::shared_ptr()) << "ID_DISTANCE packet not found for protocol V_2"; EXPECT_EQ(static_cast(v_2_packet_ptr->nbytes), 2); EXPECT_EQ(v_2_packet_ptr->info, std::string("distance")); // Get a packet exclusive to V_3 create::Data data_v_3(create::V_3); - boost::shared_ptr v_3_packet_ptr = data_v_3.getPacket(create::ID_LIGHT_FRONT_RIGHT); - EXPECT_NE(v_3_packet_ptr, boost::shared_ptr()) + std::shared_ptr v_3_packet_ptr = data_v_3.getPacket(create::ID_LIGHT_FRONT_RIGHT); + EXPECT_NE(v_3_packet_ptr, std::shared_ptr()) << "ID_LIGHT_FRONT_RIGHT packet not found for protocol V_3"; EXPECT_EQ(static_cast(v_3_packet_ptr->nbytes), 2); EXPECT_EQ(v_3_packet_ptr->info, std::string("light_bumper_front_right")); // Get a non-existent packet - boost::shared_ptr not_a_packet_ptr = data_v_3.getPacket(60); - EXPECT_EQ(not_a_packet_ptr, boost::shared_ptr()); + std::shared_ptr not_a_packet_ptr = data_v_3.getPacket(60); + EXPECT_EQ(not_a_packet_ptr, std::shared_ptr()); } TEST(DataTest, GetPacketIDs) diff --git a/tests/test_serial_query.cpp b/tests/test_serial_query.cpp index 3527bb9..b88b101 100644 --- a/tests/test_serial_query.cpp +++ b/tests/test_serial_query.cpp @@ -33,17 +33,17 @@ POSSIBILITY OF SUCH DAMAGE. #include "gtest/gtest.h" -#include +#include TEST(SerialQueryTest, Constructor) { - boost::shared_ptr data_ptr = boost::make_shared(); + std::shared_ptr data_ptr = std::make_shared(); create::SerialQuery serial_query(data_ptr); } TEST(SerialQueryTest, Connected) { - boost::shared_ptr data_ptr = boost::make_shared(); + std::shared_ptr data_ptr = std::make_shared(); create::SerialQuery serial_query(data_ptr); // Did not call connect and nothing to connect to, so expect false @@ -52,7 +52,7 @@ TEST(SerialQueryTest, Connected) TEST(SerialQueryTest, Disconnect) { - boost::shared_ptr data_ptr = boost::make_shared(); + std::shared_ptr data_ptr = std::make_shared(); create::SerialQuery serial_query(data_ptr); // Not connected, but should not fail @@ -61,7 +61,7 @@ TEST(SerialQueryTest, Disconnect) TEST(SerialQueryTest, NumPackets) { - boost::shared_ptr data_ptr = boost::make_shared(); + std::shared_ptr data_ptr = std::make_shared(); create::SerialQuery serial_query(data_ptr); // Not connected, so zero packets should have been received @@ -71,7 +71,7 @@ TEST(SerialQueryTest, NumPackets) TEST(SerialQueryTest, Send) { - boost::shared_ptr data_ptr = boost::make_shared(); + std::shared_ptr data_ptr = std::make_shared(); create::SerialQuery serial_query(data_ptr); // Some bytes to send (to set date) @@ -82,7 +82,7 @@ TEST(SerialQueryTest, Send) TEST(SerialQueryTest, SendOpcode) { - boost::shared_ptr data_ptr = boost::make_shared(); + std::shared_ptr data_ptr = std::make_shared(); create::SerialQuery serial_query(data_ptr); // Not connected, so failure expected diff --git a/tests/test_serial_stream.cpp b/tests/test_serial_stream.cpp index 7445939..9ae789c 100644 --- a/tests/test_serial_stream.cpp +++ b/tests/test_serial_stream.cpp @@ -33,17 +33,17 @@ POSSIBILITY OF SUCH DAMAGE. #include "gtest/gtest.h" -#include +#include TEST(SerialStreamTest, Constructor) { - boost::shared_ptr data_ptr = boost::make_shared(); + std::shared_ptr data_ptr = std::make_shared(); create::SerialStream serial_stream(data_ptr); } TEST(SerialStreamTest, Connected) { - boost::shared_ptr data_ptr = boost::make_shared(); + std::shared_ptr data_ptr = std::make_shared(); create::SerialStream serial_stream(data_ptr); // Did not call connect and nothing to connect to, so expect false @@ -52,7 +52,7 @@ TEST(SerialStreamTest, Connected) TEST(SerialStreamTest, Disconnect) { - boost::shared_ptr data_ptr = boost::make_shared(); + std::shared_ptr data_ptr = std::make_shared(); create::SerialStream serial_stream(data_ptr); // Not connected, but should not fail @@ -61,7 +61,7 @@ TEST(SerialStreamTest, Disconnect) TEST(SerialStreamTest, NumPackets) { - boost::shared_ptr data_ptr = boost::make_shared(); + std::shared_ptr data_ptr = std::make_shared(); create::SerialStream serial_stream(data_ptr); // Not connected, so zero packets should have been received @@ -71,7 +71,7 @@ TEST(SerialStreamTest, NumPackets) TEST(SerialStreamTest, Send) { - boost::shared_ptr data_ptr = boost::make_shared(); + std::shared_ptr data_ptr = std::make_shared(); create::SerialStream serial_stream(data_ptr); // Some bytes to send (to set date) @@ -82,7 +82,7 @@ TEST(SerialStreamTest, Send) TEST(SerialStreamTest, SendOpcode) { - boost::shared_ptr data_ptr = boost::make_shared(); + std::shared_ptr data_ptr = std::make_shared(); create::SerialStream serial_stream(data_ptr); // Not connected, so failure expected From 74603ba1f0fca3c27019722c8aaa1d04fde22607 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Sun, 1 Nov 2020 17:26:17 -0800 Subject: [PATCH 2/2] Replace Boost serial with alternative serial library This still needs testing. The following feature have been disabled and need to be reimplemented: - signal handling - recovery timer in SerialQuery Signed-off-by: Jacob Perron --- CMakeLists.txt | 4 ++ include/create/serial.h | 17 +++--- include/create/serial_query.h | 6 +- package.xml | 2 +- src/serial.cpp | 104 +++++++++++++++------------------- src/serial_query.cpp | 28 ++++----- src/serial_stream.cpp | 1 + 7 files changed, 75 insertions(+), 87 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b406f58..e868dd0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,7 @@ set(PACKAGE_VERSION 1.6.1) option(LIBCREATE_BUILD_TESTS "Enable the build of tests." ON) find_package(Boost REQUIRED COMPONENTS system thread) +find_package(serial REQUIRED) find_package(Threads REQUIRED) # Default to C++11 @@ -31,6 +32,7 @@ set(LIBRARY_NAME create) # Specify locations of header files include_directories( include + ${serial_INCLUDE_DIRS} ) # Declare cpp library @@ -56,6 +58,7 @@ endif() # Link to Boost target_link_libraries(${LIBRARY_NAME} + ${serial_LIBRARIES} ${Boost_LIBRARIES} ) @@ -75,6 +78,7 @@ foreach(EXAMPLE ${EXAMPLES}) add_executable(${EXAMPLE} examples/${EXAMPLE}.cpp) target_link_libraries(${EXAMPLE} + ${serial_LIBRARIES} ${Boost_LIBRARIES} ${LIBRARY_NAME} ) diff --git a/include/create/serial.h b/include/create/serial.h index 5280040..56e4740 100644 --- a/include/create/serial.h +++ b/include/create/serial.h @@ -29,9 +29,6 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -// Based on example from: -// https://github.com/labust/labust-ros-pkg/wiki/Create-a-Serial-Port-application - #ifndef CREATE_SERIAL_H #define CREATE_SERIAL_H @@ -41,7 +38,7 @@ POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include #include "create/data.h" #include "create/types.h" @@ -51,9 +48,7 @@ namespace create { class Serial : public std::enable_shared_from_this { protected: - boost::asio::io_service io; - boost::asio::signal_set signals; - boost::asio::serial_port port; + serial::Serial serial; private: std::thread ioThread; @@ -65,7 +60,7 @@ namespace create { uint8_t byteRead; // Callback executed when data arrives from Create - void onData(const boost::system::error_code& e, const std::size_t& size); + void onData(); // Callback to execute once data arrives std::function callback; // Start and stop reading data from Create @@ -81,7 +76,9 @@ namespace create { virtual bool startSensorStream() = 0; virtual void processByte(uint8_t byteRead) = 0; - void signalHandler(const boost::system::error_code& error, int signal_number); + // TODO + // 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(); @@ -90,7 +87,7 @@ namespace create { ~Serial(); bool connect(const std::string& port, const int& baud = 115200, std::function cb = 0); void disconnect(); - inline bool connected() const { return port.is_open(); }; + inline bool connected() const { return serial.isOpen(); }; bool send(const uint8_t* bytes, const uint32_t numBytes); bool sendOpcode(const Opcode& code); uint64_t getNumCorruptPackets() const; diff --git a/include/create/serial_query.h b/include/create/serial_query.h index a10b34e..a11e3f8 100644 --- a/include/create/serial_query.h +++ b/include/create/serial_query.h @@ -38,8 +38,6 @@ POSSIBILITY OF SUCH DAMAGE. #include -#include - #include "create/data.h" #include "create/types.h" #include "create/util.h" @@ -49,7 +47,7 @@ namespace create { class SerialQuery : public Serial { private: - boost::asio::deadline_timer streamRecoveryTimer; + // boost::asio::deadline_timer streamRecoveryTimer; uint8_t packetID; int8_t packetByte; uint16_t packetData; @@ -58,7 +56,7 @@ namespace create { bool started; void requestSensorData(); - void restartSensorStream(const boost::system::error_code& err); + // void restartSensorStream(const boost::system::error_code& err); void flushInput(); diff --git a/package.xml b/package.xml index aa633ad..ef8a8c8 100644 --- a/package.xml +++ b/package.xml @@ -16,7 +16,7 @@ cmake - boost + serial catkin diff --git a/src/serial.cpp b/src/serial.cpp index 2a87a8a..562c19b 100644 --- a/src/serial.cpp +++ b/src/serial.cpp @@ -1,6 +1,9 @@ #include #include #include +#include + +#include #include "create/serial.h" #include "create/types.h" @@ -8,8 +11,7 @@ namespace create { Serial::Serial(std::shared_ptr d) : - signals(io, SIGINT, SIGTERM), - port(io), + serial(), dataReady(false), isReading(false), data(d), @@ -21,36 +23,35 @@ 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); - } - } - } + // TODO + // 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, std::function cb) { - using namespace boost::asio; - port.open(portName); - port.set_option(serial_port::baud_rate(baud)); - port.set_option(serial_port::character_size(8)); - port.set_option(serial_port::parity(serial_port::parity::none)); - port.set_option(serial_port::stop_bits(serial_port::stop_bits::one)); - port.set_option(serial_port::flow_control(serial_port::flow_control::none)); + // using namespace boost::asio; + serial.setPort(portName); + serial.setBaudrate(baud); + serial.open(); - signals.async_wait(std::bind(&Serial::signalHandler, this, std::placeholders::_1, std::placeholders::_2)); + // TODO + // signals.async_wait(std::bind(&Serial::signalHandler, this, std::placeholders::_1, std::placeholders::_2)); - usleep(1000000); + std::this_thread::sleep_for(std::chrono::seconds(1)); - if (port.is_open()) { + if (serial.isOpen()) { callback = cb; bool startReadSuccess = startReading(); if (!startReadSuccess) { - port.close(); + serial.close(); } return startReadSuccess; } @@ -67,7 +68,7 @@ namespace create { sendOpcode(OC_START); // Stop OI sendOpcode(OC_STOP); - port.close(); + serial.close(); } } @@ -87,21 +88,11 @@ namespace create { if (!startSensorStream()) return false; - io.reset(); - // Start continuously reading one byte at a time - boost::asio::async_read(port, - boost::asio::buffer(&byteRead, 1), - std::bind(&Serial::onData, - shared_from_this(), - std::placeholders::_1, - std::placeholders::_2)); - - ioThread = std::thread(std::bind( - static_cast( - &boost::asio::io_service::run), &io)); + ioThread = std::thread(std::bind(&Serial::onData, this)); // Wait for first complete read to finish + // TODO: remove this mutex and use atomic variable? std::unique_lock lock(dataReadyMut); int attempts = 1; @@ -110,7 +101,7 @@ namespace create { if (dataReadyCond.wait_for(lock, std::chrono::milliseconds(500)) == std::cv_status::timeout) { if (attempts >= maxAttempts) { CERR("[create::Serial] ", "failed to receive data from Create. Check if robot is powered!"); - io.stop(); + serial.close(); ioThread.join(); return false; } @@ -128,7 +119,7 @@ namespace create { void Serial::stopReading() { if (isReading) { - io.stop(); + serial.close(); ioThread.join(); isReading = false; { @@ -156,24 +147,22 @@ namespace create { callback(); } - void Serial::onData(const boost::system::error_code& e, const std::size_t& size) { - if (e) { - CERR("[create::Serial] ", "serial error - " << e.message()); - return; - } + void Serial::onData() { + // TODO check for program interrupt + while (true) { + size_t size = 0u; + try { + size = serial.read(&byteRead, 1); + } catch (const std::runtime_error & e) { + CERR("[create::Serial] ", "serial error - " << e.what()); + continue; + } - // Should have read exactly one byte - if (size == 1) { - processByte(byteRead); - } // end if (size == 1) - - // Read the next byte - boost::asio::async_read(port, - boost::asio::buffer(&byteRead, 1), - std::bind(&Serial::onData, - shared_from_this(), - std::placeholders::_1, - std::placeholders::_2)); + // Should have read exactly one byte + if (size == 1) { + processByte(byteRead); + } + } } bool Serial::send(const uint8_t* bytes, unsigned int numBytes) { @@ -181,8 +170,7 @@ namespace create { CERR("[create::Serial] ", "send failed, not connected."); return false; } - // TODO: catch boost exceptions - boost::asio::write(port, boost::asio::buffer(bytes, numBytes)); + serial.write(bytes, static_cast(numBytes)); return true; } diff --git a/src/serial_query.cpp b/src/serial_query.cpp index 2d3997b..078ebe8 100644 --- a/src/serial_query.cpp +++ b/src/serial_query.cpp @@ -9,7 +9,7 @@ namespace create { SerialQuery::SerialQuery(std::shared_ptr d) : Serial(d), - streamRecoveryTimer(io), + // streamRecoveryTimer(io), packetID(ID_BUMP_WHEELDROP), packetByte(0), packetData(0), @@ -30,23 +30,23 @@ namespace create { flushInput(); send(requestPacket, 2); // Automatically resend request if no response is received - streamRecoveryTimer.expires_from_now(boost::posix_time::milliseconds(50)); - streamRecoveryTimer.async_wait( - std::bind(&SerialQuery::restartSensorStream, this, std::placeholders::_1)); + // TODO: implement recovery timer + // streamRecoveryTimer.expires_from_now(boost::posix_time::milliseconds(50)); + // streamRecoveryTimer.async_wait( + // std::bind(&SerialQuery::restartSensorStream, this, std::placeholders::_1)); } - void SerialQuery::restartSensorStream(const boost::system::error_code& err) { - if (err != boost::asio::error::operation_aborted) { - if (packetID != ID_BUMP_WHEELDROP) { - ++corruptPackets; - } - requestSensorData(); - } - } + // void SerialQuery::restartSensorStream(const boost::system::error_code& err) { + // if (err != boost::asio::error::operation_aborted) { + // if (packetID != ID_BUMP_WHEELDROP) { + // ++corruptPackets; + // } + // requestSensorData(); + // } + // } void SerialQuery::flushInput() { - // Only works with POSIX support - tcflush(port.lowest_layer().native_handle(), TCIFLUSH); + serial.flushInput(); } void SerialQuery::processByte(uint8_t byteRead) { diff --git a/src/serial_stream.cpp b/src/serial_stream.cpp index b22a07e..4475e94 100644 --- a/src/serial_stream.cpp +++ b/src/serial_stream.cpp @@ -1,3 +1,4 @@ +#include #include #include