From 2b9f7cc61630643a881ca750f3309ff157da82ae Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Wed, 17 Jun 2020 11:38:29 +0800 Subject: [PATCH] more code cleanup --- README.md | 1 + apps/hunter_demo/hunter_demo.cpp | 2 +- apps/scout_demo/scout_demo.cpp | 2 +- apps/scout_demo/scout_discharge.cpp | 2 +- apps/scout_monitor/app/app_scout_monitor.cpp | 2 +- .../scout_monitor/include/monitor/ncolors.hpp | 4 +- .../scout_monitor/include/monitor/nshapes.hpp | 4 +- .../include/monitor/scout_monitor.hpp | 4 +- apps/scout_monitor/src/ncolors.cpp | 4 +- apps/scout_monitor/src/nshapes.cpp | 4 +- apps/scout_monitor/src/scout_monitor.cpp | 4 +- apps/scout_monitor/tests/test_ncolor2.cpp | 2 +- .../tests/test_scout_monitor.cpp | 2 +- .../tests/test_scout_monitor_virtual.cpp | 2 +- apps/tracer_demo/tracer_demo.cpp | 2 +- .../{async_io => asyncio}/async_can.hpp | 4 +- .../{async_io => asyncio}/async_serial.hpp | 8 +- .../{async_io => asyncio}/device_error.hpp | 2 +- .../{async_io => asyncio}/msg_buffer.hpp | 4 +- .../wrp_sdk/platforms/common/mobile_base.hpp | 96 +++++ .../wrp_sdk/platforms/hunter/hunter_base.hpp | 122 +++--- .../wrp_sdk/platforms/hunter/hunter_types.hpp | 4 +- .../wrp_sdk/platforms/scout/scout_base.hpp | 8 +- .../wrp_sdk/platforms/scout/scout_types.hpp | 4 +- .../wrp_sdk/platforms/tracer/tracer_base.hpp | 4 +- .../wrp_sdk/platforms/tracer/tracer_types.hpp | 4 +- src/async_can.cpp | 4 +- src/async_serial.cpp | 4 +- src/asyncio_utils.cpp | 4 +- src/asyncio_utils.hpp | 4 +- src/platforms/hunter_base.cpp | 4 +- src/platforms/mobile_base.cpp | 350 ++++++++++++++++++ src/platforms/scout_base.cpp | 2 +- src/platforms/tracer_base.cpp | 4 +- src/stopwatch.h | 4 +- tests/test_acan.cpp | 4 +- tests/test_aserial.cpp | 4 +- tests/test_aserial_comm.cpp | 4 +- tests/test_can_msg.cpp | 2 +- tests/test_hunter_base.cpp | 2 +- tests/test_scout_base.cpp | 2 +- tests/test_scout_serial.cpp | 4 +- tests/test_serial_parser.cpp | 2 +- tests/test_tracer_base.cpp | 2 +- unit_tests/gtests/scout_can_protocol_test.cpp | 2 +- 45 files changed, 580 insertions(+), 129 deletions(-) rename include/wrp_sdk/{async_io => asyncio}/async_can.hpp (98%) rename include/wrp_sdk/{async_io => asyncio}/async_serial.hpp (97%) rename include/wrp_sdk/{async_io => asyncio}/device_error.hpp (98%) rename include/wrp_sdk/{async_io => asyncio}/msg_buffer.hpp (96%) create mode 100644 include/wrp_sdk/platforms/common/mobile_base.hpp create mode 100644 src/platforms/mobile_base.cpp diff --git a/README.md b/README.md index 7544112..75e3ee4 100755 --- a/README.md +++ b/README.md @@ -5,6 +5,7 @@ Copyright (c) 2020 [WestonRobot](https://www.westonrobot.com/) ## Introduction Supported platforms + * **Scout**: skid-steer mobile base * **Hunter**: ackermann mobile base diff --git a/apps/hunter_demo/hunter_demo.cpp b/apps/hunter_demo/hunter_demo.cpp index d98d73a..694ab45 100644 --- a/apps/hunter_demo/hunter_demo.cpp +++ b/apps/hunter_demo/hunter_demo.cpp @@ -9,7 +9,7 @@ #include "hunter_base/hunter_base.hpp" -using namespace wescore; +using namespace westonrobot; int main(int argc, char **argv) { diff --git a/apps/scout_demo/scout_demo.cpp b/apps/scout_demo/scout_demo.cpp index 3d7c284..01a3a9a 100644 --- a/apps/scout_demo/scout_demo.cpp +++ b/apps/scout_demo/scout_demo.cpp @@ -9,7 +9,7 @@ #include "scout_base/scout_base.hpp" -using namespace wescore; +using namespace westonrobot; int main(int argc, char **argv) { diff --git a/apps/scout_demo/scout_discharge.cpp b/apps/scout_demo/scout_discharge.cpp index 781899f..03e5728 100644 --- a/apps/scout_demo/scout_discharge.cpp +++ b/apps/scout_demo/scout_discharge.cpp @@ -9,7 +9,7 @@ #include "scout_base/scout_base.hpp" -using namespace wescore; +using namespace westonrobot; int main(int argc, char **argv) { diff --git a/apps/scout_monitor/app/app_scout_monitor.cpp b/apps/scout_monitor/app/app_scout_monitor.cpp index 9ed4fb7..cde67b0 100644 --- a/apps/scout_monitor/app/app_scout_monitor.cpp +++ b/apps/scout_monitor/app/app_scout_monitor.cpp @@ -7,7 +7,7 @@ #include "monitor/scout_monitor.hpp" -using namespace wescore; +using namespace westonrobot; int main(int argc, char **argv) { diff --git a/apps/scout_monitor/include/monitor/ncolors.hpp b/apps/scout_monitor/include/monitor/ncolors.hpp index c7f0913..4af8925 100644 --- a/apps/scout_monitor/include/monitor/ncolors.hpp +++ b/apps/scout_monitor/include/monitor/ncolors.hpp @@ -17,7 +17,7 @@ #include -namespace wescore +namespace westonrobot { struct NColors { @@ -62,6 +62,6 @@ struct NColors static void WSetColor(WINDOW *win, int fg, int bg = BLACK); static void WUnsetColor(WINDOW *win, int fg, int bg = BLACK); }; -} // namespace wescore +} // namespace westonrobot #endif /* NCOLORS_HPP */ diff --git a/apps/scout_monitor/include/monitor/nshapes.hpp b/apps/scout_monitor/include/monitor/nshapes.hpp index 88c5f60..5f71a29 100644 --- a/apps/scout_monitor/include/monitor/nshapes.hpp +++ b/apps/scout_monitor/include/monitor/nshapes.hpp @@ -12,13 +12,13 @@ #include -namespace wescore +namespace westonrobot { struct NShapes { static void DrawRectangle(int tl_y, int tl_x, int br_y, int br_x); static void WDrawRectangle(WINDOW *win, int tl_y, int tl_x, int br_y, int br_x); }; -} // namespace wescore +} // namespace westonrobot #endif /* NSHAPES_HPP */ diff --git a/apps/scout_monitor/include/monitor/scout_monitor.hpp b/apps/scout_monitor/include/monitor/scout_monitor.hpp index bb06e60..e7e58e0 100644 --- a/apps/scout_monitor/include/monitor/scout_monitor.hpp +++ b/apps/scout_monitor/include/monitor/scout_monitor.hpp @@ -14,7 +14,7 @@ #include -namespace wescore +namespace westonrobot { class ScoutMonitor { @@ -73,6 +73,6 @@ private: void UpdateScoutSystemInfo(); void UpdateScoutCmdWindow(); }; -} // namespace wescore +} // namespace westonrobot #endif /* SCOUT_MONITOR_HPP */ diff --git a/apps/scout_monitor/src/ncolors.cpp b/apps/scout_monitor/src/ncolors.cpp index 6904c6b..a3ea543 100644 --- a/apps/scout_monitor/src/ncolors.cpp +++ b/apps/scout_monitor/src/ncolors.cpp @@ -58,7 +58,7 @@ short CursorColor(int fg) } } // namespace -namespace wescore +namespace westonrobot { void NColors::InitColors() { @@ -111,4 +111,4 @@ void NColors::WUnsetColor(WINDOW *win, int fg, int bg) if (IsBold(fg)) wattroff(win, A_BOLD); } -} // namespace wescore \ No newline at end of file +} // namespace westonrobot \ No newline at end of file diff --git a/apps/scout_monitor/src/nshapes.cpp b/apps/scout_monitor/src/nshapes.cpp index fdca15c..0fc2e6e 100644 --- a/apps/scout_monitor/src/nshapes.cpp +++ b/apps/scout_monitor/src/nshapes.cpp @@ -9,7 +9,7 @@ #include "monitor/nshapes.hpp" -namespace wescore +namespace westonrobot { void NShapes::DrawRectangle(int tl_y, int tl_x, int br_y, int br_x) { @@ -38,4 +38,4 @@ void NShapes::WDrawRectangle(WINDOW *win, int tl_y, int tl_x, int br_y, int br_x mvwprintw(win, br_y, i, "-"); } } -} // namespace wescore \ No newline at end of file +} // namespace westonrobot \ No newline at end of file diff --git a/apps/scout_monitor/src/scout_monitor.cpp b/apps/scout_monitor/src/scout_monitor.cpp index 4e4e2f9..d11baf5 100644 --- a/apps/scout_monitor/src/scout_monitor.cpp +++ b/apps/scout_monitor/src/scout_monitor.cpp @@ -111,7 +111,7 @@ struct StopWatch }; } // namespace -namespace wescore +namespace westonrobot { ScoutMonitor::ScoutMonitor() { @@ -655,4 +655,4 @@ void ScoutMonitor::ShowMotorInfo(int y, int x, double cur, int rpm, int temp, bo std::string temp_str = "TMP:" + ConvertFloatToString(temp, 0); mvwprintw(body_info_win_, y + 3, col_title, temp_str.c_str()); } -} // namespace wescore +} // namespace westonrobot diff --git a/apps/scout_monitor/tests/test_ncolor2.cpp b/apps/scout_monitor/tests/test_ncolor2.cpp index 6d8ab2d..643d045 100644 --- a/apps/scout_monitor/tests/test_ncolor2.cpp +++ b/apps/scout_monitor/tests/test_ncolor2.cpp @@ -1,6 +1,6 @@ #include "monitor/ncolors.hpp" -using namespace wescore; +using namespace westonrobot; int main(void) { diff --git a/apps/scout_monitor/tests/test_scout_monitor.cpp b/apps/scout_monitor/tests/test_scout_monitor.cpp index 0d0dc9e..7747122 100644 --- a/apps/scout_monitor/tests/test_scout_monitor.cpp +++ b/apps/scout_monitor/tests/test_scout_monitor.cpp @@ -7,7 +7,7 @@ #include "monitor/scout_monitor.hpp" -using namespace wescore; +using namespace westonrobot; ScoutMonitor monitor; diff --git a/apps/scout_monitor/tests/test_scout_monitor_virtual.cpp b/apps/scout_monitor/tests/test_scout_monitor_virtual.cpp index e66eacf..5bc0e07 100644 --- a/apps/scout_monitor/tests/test_scout_monitor_virtual.cpp +++ b/apps/scout_monitor/tests/test_scout_monitor_virtual.cpp @@ -7,7 +7,7 @@ #include "monitor/scout_monitor.hpp" -using namespace wescore; +using namespace westonrobot; ScoutMonitor monitor; diff --git a/apps/tracer_demo/tracer_demo.cpp b/apps/tracer_demo/tracer_demo.cpp index 30e3548..33aad3c 100644 --- a/apps/tracer_demo/tracer_demo.cpp +++ b/apps/tracer_demo/tracer_demo.cpp @@ -9,7 +9,7 @@ #include "tracer_base/tracer_base.hpp" -using namespace wescore; +using namespace westonrobot; int main(int argc, char **argv) { diff --git a/include/wrp_sdk/async_io/async_can.hpp b/include/wrp_sdk/asyncio/async_can.hpp similarity index 98% rename from include/wrp_sdk/async_io/async_can.hpp rename to include/wrp_sdk/asyncio/async_can.hpp index ae7a016..dd712cc 100644 --- a/include/wrp_sdk/async_io/async_can.hpp +++ b/include/wrp_sdk/asyncio/async_can.hpp @@ -42,7 +42,7 @@ // #include "async_io/device_error.hpp" // #include "async_io/msg_buffer.hpp" -namespace wescore +namespace westonrobot { using steady_clock = std::chrono::steady_clock; using lock_guard = std::lock_guard; @@ -125,6 +125,6 @@ private: void iostat_tx_add(std::size_t bytes); void iostat_rx_add(std::size_t bytes); }; -} // namespace wescore +} // namespace westonrobot #endif /* ASYNC_CAN_HPP */ diff --git a/include/wrp_sdk/async_io/async_serial.hpp b/include/wrp_sdk/asyncio/async_serial.hpp similarity index 97% rename from include/wrp_sdk/async_io/async_serial.hpp rename to include/wrp_sdk/asyncio/async_serial.hpp index 827459c..c179101 100644 --- a/include/wrp_sdk/async_io/async_serial.hpp +++ b/include/wrp_sdk/asyncio/async_serial.hpp @@ -40,10 +40,10 @@ #include "asio.hpp" -#include "wrp_sdk/async_io/device_error.hpp" -#include "wrp_sdk/async_io/msg_buffer.hpp" +#include "wrp_sdk/asyncio/device_error.hpp" +#include "wrp_sdk/asyncio/msg_buffer.hpp" -namespace wescore +namespace westonrobot { using steady_clock = std::chrono::steady_clock; using lock_guard = std::lock_guard; @@ -140,6 +140,6 @@ private: void iostat_tx_add(std::size_t bytes); void iostat_rx_add(std::size_t bytes); }; -} // namespace wescore +} // namespace westonrobot #endif /* ASYNC_SERIAL_HPP */ diff --git a/include/wrp_sdk/async_io/device_error.hpp b/include/wrp_sdk/asyncio/device_error.hpp similarity index 98% rename from include/wrp_sdk/async_io/device_error.hpp rename to include/wrp_sdk/asyncio/device_error.hpp index 4362a96..b56f111 100644 --- a/include/wrp_sdk/async_io/device_error.hpp +++ b/include/wrp_sdk/asyncio/device_error.hpp @@ -19,7 +19,7 @@ #include #include -namespace wescore +namespace westonrobot { class DeviceError : public std::runtime_error { diff --git a/include/wrp_sdk/async_io/msg_buffer.hpp b/include/wrp_sdk/asyncio/msg_buffer.hpp similarity index 96% rename from include/wrp_sdk/async_io/msg_buffer.hpp rename to include/wrp_sdk/asyncio/msg_buffer.hpp index 4caab51..da41da9 100644 --- a/include/wrp_sdk/async_io/msg_buffer.hpp +++ b/include/wrp_sdk/asyncio/msg_buffer.hpp @@ -19,7 +19,7 @@ #include #include -namespace wescore +namespace westonrobot { /** @@ -66,4 +66,4 @@ struct MsgBuffer return len - pos; } }; -} // namespace wescore +} // namespace westonrobot diff --git a/include/wrp_sdk/platforms/common/mobile_base.hpp b/include/wrp_sdk/platforms/common/mobile_base.hpp new file mode 100644 index 0000000..0f06999 --- /dev/null +++ b/include/wrp_sdk/platforms/common/mobile_base.hpp @@ -0,0 +1,96 @@ +/* + * mobile_base.hpp + * + * Created on: Jun 17, 2020 11:23 + * Description: + * + * Generic mobile base: this class handles the communication + * logic that is similar across different mobile platforms + * + * Copyright (c) 2020 Ruixiang Du (rdu) + */ + +#ifndef MOBILE_BASE_HPP +#define MOBILE_BASE_HPP + +#include +#include +#include +#include +#include + +#include "wrp_sdk/asyncio/async_can.hpp" +#include "wrp_sdk/asyncio/async_serial.hpp" + +namespace westonrobot { +class MobileBase { + public: + MobileBase() = default; + virtual ~MobileBase(); + + // do not allow copy or assignment + MobileBase(const MobileBase &hunter) = delete; + MobileBase &operator=(const MobileBase &hunter) = delete; + + // connect to roboot from CAN or serial + void Connect(std::string dev_name, int32_t baud_rate = 0); + + // disconnect from roboot, only valid for serial port + void Disconnect(); + + // cmd thread runs at 100Hz (10ms) by default + void SetCmdThreadPeriodMs(int32_t period_ms) { + cmd_thread_period_ms_ = period_ms; + }; + + // motion control + void SetMotionCommand(double linear_vel, double steering_angle, + HunterMotionCmd::FaultClearFlag fault_clr_flag = + HunterMotionCmd::FaultClearFlag::NO_FAULT); + + // get robot state + HunterState GetHunterState(); + + protected: + // hardware communication interface + std::shared_ptr can_if_; + std::shared_ptr serial_if_; + + // CAN priority higher than serial if both connected + bool can_connected_ = false; + bool serial_connected_ = false; + + // serial port related variables + uint8_t tx_cmd_len_; + uint8_t tx_buffer_[HUNTER_CMD_BUF_LEN]; + + // cmd/status update related variables + std::thread cmd_thread_; + std::mutex hunter_state_mutex_; + std::mutex motion_cmd_mutex_; + + HunterState hunter_state_; + HunterMotionCmd current_motion_cmd_; + + int32_t cmd_thread_period_ms_ = 10; + bool cmd_thread_started_ = false; + + // internal functions + void ConfigureCANBus(const std::string &can_if_name = "can1"); + void ConfigureSerial(const std::string uart_name = "/dev/ttyUSB0", + int32_t baud_rate = 115200); + + void StartCmdThread(); + void ControlLoop(int32_t period_ms); + + void SendMotionCmd(uint8_t count); + + void ParseCANFrame(can_frame *rx_frame); + void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, + size_t bytes_received); + + void NewStatusMsgReceivedCallback(const HunterMessage &msg); +}; +} // namespace westonrobot + +#endif /* MOBILE_BASE_HPP */ diff --git a/include/wrp_sdk/platforms/hunter/hunter_base.hpp b/include/wrp_sdk/platforms/hunter/hunter_base.hpp index c6094ff..1fd654b 100644 --- a/include/wrp_sdk/platforms/hunter/hunter_base.hpp +++ b/include/wrp_sdk/platforms/hunter/hunter_base.hpp @@ -1,11 +1,11 @@ -/* +/* * hunter_base.hpp - * + * * Created on: Apr 01, 2020 09:43 - * Description: - * + * Description: + * * Copyright (c) 2019 Ruixiang Du (rdu) - */ + */ #ifndef HUNTER_BASE_HPP #define HUNTER_BASE_HPP @@ -16,84 +16,88 @@ #include #include -#include "wrp_sdk/async_io/async_can.hpp" -#include "wrp_sdk/async_io/async_serial.hpp" +#include "wrp_sdk/asyncio/async_can.hpp" +#include "wrp_sdk/asyncio/async_serial.hpp" #include "wrp_sdk/platforms/hunter/hunter_protocol.h" #include "wrp_sdk/platforms/hunter/hunter_can_parser.h" #include "wrp_sdk/platforms/hunter/hunter_types.hpp" -namespace wescore -{ -class HunterBase -{ -public: - HunterBase() = default; - ~HunterBase(); +namespace westonrobot { +class HunterBase { + public: + HunterBase() = default; + ~HunterBase(); - // do not allow copy - HunterBase(const HunterBase &hunter) = delete; - HunterBase &operator=(const HunterBase &hunter) = delete; + // do not allow copy + HunterBase(const HunterBase &hunter) = delete; + HunterBase &operator=(const HunterBase &hunter) = delete; -public: - // connect to roboot from CAN - void Connect(std::string dev_name); + public: + // connect to roboot from CAN + void Connect(std::string dev_name); - // disconnect from roboot, only valid for serial port - void Disconnect(); + // disconnect from roboot, only valid for serial port + void Disconnect(); - // cmd thread runs at 100Hz (10ms) by default - void SetCmdThreadPeriodMs(int32_t period_ms) { cmd_thread_period_ms_ = period_ms; }; + // cmd thread runs at 100Hz (10ms) by default + void SetCmdThreadPeriodMs(int32_t period_ms) { + cmd_thread_period_ms_ = period_ms; + }; - // motion control - void SetMotionCommand(double linear_vel, double steering_angle, - HunterMotionCmd::FaultClearFlag fault_clr_flag = HunterMotionCmd::FaultClearFlag::NO_FAULT); + // motion control + void SetMotionCommand(double linear_vel, double steering_angle, + HunterMotionCmd::FaultClearFlag fault_clr_flag = + HunterMotionCmd::FaultClearFlag::NO_FAULT); - // get robot state - HunterState GetHunterState(); + // get robot state + HunterState GetHunterState(); -private: - // hardware communication interface - std::shared_ptr can_if_; - std::shared_ptr serial_if_; + private: + // hardware communication interface + std::shared_ptr can_if_; + std::shared_ptr serial_if_; - // CAN priority higher than serial if both connected - bool can_connected_ = false; - bool serial_connected_ = false; + // CAN priority higher than serial if both connected + bool can_connected_ = false; + bool serial_connected_ = false; - // serial port related variables - uint8_t tx_cmd_len_; - uint8_t tx_buffer_[HUNTER_CMD_BUF_LEN]; + // serial port related variables + uint8_t tx_cmd_len_; + uint8_t tx_buffer_[HUNTER_CMD_BUF_LEN]; - // cmd/status update related variables - std::thread cmd_thread_; - std::mutex hunter_state_mutex_; - std::mutex motion_cmd_mutex_; + // cmd/status update related variables + std::thread cmd_thread_; + std::mutex hunter_state_mutex_; + std::mutex motion_cmd_mutex_; - HunterState hunter_state_; - HunterMotionCmd current_motion_cmd_; + HunterState hunter_state_; + HunterMotionCmd current_motion_cmd_; - int32_t cmd_thread_period_ms_ = 10; - bool cmd_thread_started_ = false; + int32_t cmd_thread_period_ms_ = 10; + bool cmd_thread_started_ = false; - // internal functions - void ConfigureCANBus(const std::string &can_if_name = "can1"); - void ConfigureSerial(const std::string uart_name = "/dev/ttyUSB0", int32_t baud_rate = 115200); + // internal functions + void ConfigureCANBus(const std::string &can_if_name = "can1"); + void ConfigureSerial(const std::string uart_name = "/dev/ttyUSB0", + int32_t baud_rate = 115200); - void StartCmdThread(); - void ControlLoop(int32_t period_ms); + void StartCmdThread(); + void ControlLoop(int32_t period_ms); - void SendMotionCmd(uint8_t count); + void SendMotionCmd(uint8_t count); - void ParseCANFrame(can_frame *rx_frame); - void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received); + void ParseCANFrame(can_frame *rx_frame); + void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, + size_t bytes_received); - void NewStatusMsgReceivedCallback(const HunterMessage &msg); + void NewStatusMsgReceivedCallback(const HunterMessage &msg); -public: - static void UpdateHunterState(const HunterMessage &status_msg, HunterState &state); + public: + static void UpdateHunterState(const HunterMessage &status_msg, + HunterState &state); }; -} // namespace wescore +} // namespace westonrobot #endif /* HUNTER_BASE_HPP */ diff --git a/include/wrp_sdk/platforms/hunter/hunter_types.hpp b/include/wrp_sdk/platforms/hunter/hunter_types.hpp index c35af4f..ac2ce7c 100644 --- a/include/wrp_sdk/platforms/hunter/hunter_types.hpp +++ b/include/wrp_sdk/platforms/hunter/hunter_types.hpp @@ -13,7 +13,7 @@ #include #include -namespace wescore +namespace westonrobot { struct HunterState { @@ -69,6 +69,6 @@ struct HunterMotionCmd static constexpr double max_steering_angle = 0.4622; // 0.4622 rad ~= 26.5 degree static constexpr double min_steering_angle = -0.4622; // -0.4622 rad }; -} // namespace wescore +} // namespace westonrobot #endif /* HUNTER_TYPES_HPP */ diff --git a/include/wrp_sdk/platforms/scout/scout_base.hpp b/include/wrp_sdk/platforms/scout/scout_base.hpp index 3c71611..60d58cd 100644 --- a/include/wrp_sdk/platforms/scout/scout_base.hpp +++ b/include/wrp_sdk/platforms/scout/scout_base.hpp @@ -16,8 +16,8 @@ #include #include -#include "wrp_sdk/async_io/async_can.hpp" -#include "wrp_sdk/async_io/async_serial.hpp" +#include "wrp_sdk/asyncio/async_can.hpp" +#include "wrp_sdk/asyncio/async_serial.hpp" #include "wrp_sdk/platforms/scout/scout_protocol.h" #include "wrp_sdk/platforms/scout/scout_can_parser.h" @@ -25,7 +25,7 @@ #include "wrp_sdk/platforms/scout/scout_types.hpp" -namespace wescore +namespace westonrobot { class ScoutBase { @@ -105,6 +105,6 @@ private: public: static void UpdateScoutState(const ScoutMessage &status_msg, ScoutState &state); }; -} // namespace wescore +} // namespace westonrobot #endif /* SCOUT_BASE_HPP */ diff --git a/include/wrp_sdk/platforms/scout/scout_types.hpp b/include/wrp_sdk/platforms/scout/scout_types.hpp index 6767a0e..b56c571 100644 --- a/include/wrp_sdk/platforms/scout/scout_types.hpp +++ b/include/wrp_sdk/platforms/scout/scout_types.hpp @@ -13,7 +13,7 @@ #include #include -namespace wescore +namespace westonrobot { struct ScoutState { @@ -106,6 +106,6 @@ struct ScoutLightCmd LightMode rear_mode; uint8_t rear_custom_value; }; -} // namespace wescore +} // namespace westonrobot #endif /* SCOUT_STATE_HPP */ diff --git a/include/wrp_sdk/platforms/tracer/tracer_base.hpp b/include/wrp_sdk/platforms/tracer/tracer_base.hpp index 9da51e6..93df36f 100644 --- a/include/wrp_sdk/platforms/tracer/tracer_base.hpp +++ b/include/wrp_sdk/platforms/tracer/tracer_base.hpp @@ -24,7 +24,7 @@ #include "tracer_base/tracer_types.hpp" -namespace wescore +namespace westonrobot { class TracerBase { @@ -104,6 +104,6 @@ private: public: static void UpdateTracerState(const TracerMessage &status_msg, TracerState &state); }; -} // namespace wescore +} // namespace westonrobot #endif /* TRACER_BASE_HPP */ diff --git a/include/wrp_sdk/platforms/tracer/tracer_types.hpp b/include/wrp_sdk/platforms/tracer/tracer_types.hpp index 43945a0..964c860 100644 --- a/include/wrp_sdk/platforms/tracer/tracer_types.hpp +++ b/include/wrp_sdk/platforms/tracer/tracer_types.hpp @@ -13,7 +13,7 @@ #include #include -namespace wescore +namespace westonrobot { struct TracerState { @@ -106,6 +106,6 @@ struct TracerLightCmd LightMode rear_mode; uint8_t rear_custom_value; }; -} // namespace wescore +} // namespace westonrobot #endif /* TRACER_TYPES_HPP */ diff --git a/src/async_can.cpp b/src/async_can.cpp index a696241..8c74453 100644 --- a/src/async_can.cpp +++ b/src/async_can.cpp @@ -13,7 +13,7 @@ #define _GNU_SOURCE #endif -#include "wrp_sdk/async_io/async_can.hpp" +#include "wrp_sdk/asyncio/async_can.hpp" #include #include @@ -26,7 +26,7 @@ #include "asyncio_utils.hpp" -using namespace wescore; +using namespace westonrobot; using asio::buffer; using asio::io_service; diff --git a/src/async_serial.cpp b/src/async_serial.cpp index b187a31..eef4708 100644 --- a/src/async_serial.cpp +++ b/src/async_serial.cpp @@ -26,14 +26,14 @@ #include #include -#include "wrp_sdk/async_io/async_serial.hpp" +#include "wrp_sdk/asyncio/async_serial.hpp" #include "asyncio_utils.hpp" #if defined(__linux__) #include #endif -using namespace wescore; +using namespace westonrobot; using asio::buffer; using asio::io_service; diff --git a/src/asyncio_utils.cpp b/src/asyncio_utils.cpp index d01a608..35ed8a0 100644 --- a/src/asyncio_utils.cpp +++ b/src/asyncio_utils.cpp @@ -9,7 +9,7 @@ #include "asyncio_utils.hpp" -namespace wescore +namespace westonrobot { void url_parse_host(std::string host, std::string &host_out, int &port_out, @@ -79,4 +79,4 @@ void url_parse_query(std::string query) sys.assign(ids_it, comma_it); comp.assign(comma_it + 1, query.end()); } -} // namespace wescore \ No newline at end of file +} // namespace westonrobot \ No newline at end of file diff --git a/src/asyncio_utils.hpp b/src/asyncio_utils.hpp index 7083081..915f1e1 100644 --- a/src/asyncio_utils.hpp +++ b/src/asyncio_utils.hpp @@ -19,7 +19,7 @@ #include #include -namespace wescore +namespace westonrobot { template std::string format(const std::string &fmt, Args... args) @@ -58,6 +58,6 @@ void url_parse_host(std::string host, * Parse ?ids=sid,cid */ void url_parse_query(std::string query); -} // namespace wescore +} // namespace westonrobot #endif /* ASYNCIO_UTILS_HPP */ diff --git a/src/platforms/hunter_base.cpp b/src/platforms/hunter_base.cpp index b469573..98397d0 100644 --- a/src/platforms/hunter_base.cpp +++ b/src/platforms/hunter_base.cpp @@ -12,7 +12,7 @@ #include "stopwatch.h" -namespace wescore { +namespace westonrobot { HunterBase::~HunterBase() { if (serial_connected_) serial_if_->close(); @@ -253,4 +253,4 @@ void HunterBase::UpdateHunterState(const HunterMessage &status_msg, break; } } -} // namespace wescore +} // namespace westonrobot diff --git a/src/platforms/mobile_base.cpp b/src/platforms/mobile_base.cpp new file mode 100644 index 0000000..9ee03f0 --- /dev/null +++ b/src/platforms/mobile_base.cpp @@ -0,0 +1,350 @@ +/* + * mobile_base.cpp + * + * Created on: Jun 17, 2020 11:26 + * Description: + * + * Copyright (c) 2020 Ruixiang Du (rdu) + */ + +#include "wrp_sdk/platforms/mobile_base.hpp" + +#include +#include +#include + +#include "stopwatch.h" + +namespace westonrobot { +MobileBase::~MobileBase() { + if (serial_connected_) serial_if_->close(); + if (cmd_thread_.joinable()) cmd_thread_.join(); +} + +void MobileBase::Connect(std::string dev_name, int32_t baud_rate) { + if (baud_rate == 0) { + ConfigureCANBus(dev_name); + } else { + ConfigureSerial(dev_name, baud_rate); + + if (!serial_connected_) + std::cerr << "ERROR: Failed to connect to serial port" << std::endl; + } +} + +void MobileBase::Disconnect() { + if (serial_connected_) { + if (serial_if_->is_open()) serial_if_->close(); + } +} + +void MobileBase::ConfigureCANBus(const std::string &can_if_name) { + can_if_ = std::make_shared(can_if_name); + + can_if_->set_receive_callback( + std::bind(&MobileBase::ParseCANFrame, this, std::placeholders::_1)); + + can_connected_ = true; +} + +void MobileBase::ConfigureSerial(const std::string uart_name, + int32_t baud_rate) { + serial_if_ = std::make_shared(uart_name, baud_rate); + serial_if_->open(); + + if (serial_if_->is_open()) serial_connected_ = true; + + serial_if_->set_receive_callback( + std::bind(&MobileBase::ParseUARTBuffer, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); +} + +void MobileBase::StartCmdThread() { + current_motion_cmd_.linear_velocity = 0; + current_motion_cmd_.angular_velocity = 0; + current_motion_cmd_.fault_clear_flag = + ScoutMotionCmd::FaultClearFlag::NO_FAULT; + + cmd_thread_ = std::thread( + std::bind(&MobileBase::ControlLoop, this, cmd_thread_period_ms_)); + cmd_thread_started_ = true; +} + +void MobileBase::SendMotionCmd(uint8_t count) { + // motion control message + ScoutMessage m_msg; + m_msg.type = ScoutMotionControlMsg; + + if (can_connected_) + m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN; + else if (serial_connected_) + m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART; + + motion_cmd_mutex_.lock(); + m_msg.body.motion_control_msg.data.cmd.fault_clear_flag = + static_cast(current_motion_cmd_.fault_clear_flag); + m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd = + current_motion_cmd_.linear_velocity; + m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd = + current_motion_cmd_.angular_velocity; + motion_cmd_mutex_.unlock(); + + m_msg.body.motion_control_msg.data.cmd.reserved0 = 0; + m_msg.body.motion_control_msg.data.cmd.reserved1 = 0; + m_msg.body.motion_control_msg.data.cmd.count = count; + + if (can_connected_) + m_msg.body.motion_control_msg.data.cmd.checksum = + CalcScoutCANChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID, + m_msg.body.motion_control_msg.data.raw, 8); + // serial_connected_: checksum will be calculated later when packed into a + // complete serial frame + + if (can_connected_) { + // send to can bus + can_frame m_frame; + EncodeScoutMsgToCAN(&m_msg, &m_frame); + can_if_->send_frame(m_frame); + } else { + // send to serial port + EncodeScoutMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_); + serial_if_->send_bytes(tx_buffer_, tx_cmd_len_); + } +} + +void MobileBase::SendLightCmd(uint8_t count) { + ScoutMessage l_msg; + l_msg.type = ScoutLightControlMsg; + + light_cmd_mutex_.lock(); + if (light_ctrl_enabled_) { + l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_ENABLE_CTRL; + + l_msg.body.light_control_msg.data.cmd.front_light_mode = + static_cast(current_light_cmd_.front_mode); + l_msg.body.light_control_msg.data.cmd.front_light_custom = + current_light_cmd_.front_custom_value; + l_msg.body.light_control_msg.data.cmd.rear_light_mode = + static_cast(current_light_cmd_.rear_mode); + l_msg.body.light_control_msg.data.cmd.rear_light_custom = + current_light_cmd_.rear_custom_value; + + // std::cout << "cmd: " << l_msg.data.cmd.front_light_mode << " , " << + // l_msg.data.cmd.front_light_custom << " , " + // << l_msg.data.cmd.rear_light_mode << " , " << + // l_msg.data.cmd.rear_light_custom << std::endl; + // std::cout << "light cmd generated" << std::endl; + } else { + l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = + LIGHT_DISABLE_CTRL; + + l_msg.body.light_control_msg.data.cmd.front_light_mode = + LIGHT_MODE_CONST_OFF; + l_msg.body.light_control_msg.data.cmd.front_light_custom = 0; + l_msg.body.light_control_msg.data.cmd.rear_light_mode = + LIGHT_MODE_CONST_OFF; + l_msg.body.light_control_msg.data.cmd.rear_light_custom = 0; + } + light_ctrl_requested_ = false; + light_cmd_mutex_.unlock(); + + l_msg.body.light_control_msg.data.cmd.reserved0 = 0; + l_msg.body.light_control_msg.data.cmd.count = count; + + if (can_connected_) + l_msg.body.light_control_msg.data.cmd.checksum = CalcScoutCANChecksum( + CAN_MSG_LIGHT_CONTROL_CMD_ID, l_msg.body.light_control_msg.data.raw, 8); + // serial_connected_: checksum will be calculated later when packed into a + // complete serial frame + + if (can_connected_) { + // send to can bus + can_frame l_frame; + EncodeScoutMsgToCAN(&l_msg, &l_frame); + + can_if_->send_frame(l_frame); + } else { + // send to serial port + EncodeScoutMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_); + serial_if_->send_bytes(tx_buffer_, tx_cmd_len_); + } + + // std::cout << "cmd: " << static_cast(l_msg.data.cmd.front_light_mode) + // << " , " << static_cast(l_msg.data.cmd.front_light_custom) << " , " + // << static_cast(l_msg.data.cmd.rear_light_mode) << " , " << + // static_cast(l_msg.data.cmd.rear_light_custom) << std::endl; + // std::cout << "can: "; + // for (int i = 0; i < 8; ++i) + // std::cout << static_cast(l_frame.data[i]) << " "; + // std::cout << "uart: "; + // for (int i = 0; i < tx_cmd_len_; ++i) + // std::cout << static_cast(tx_buffer_[i]) << " "; + // std::cout << std::endl; +} + +void MobileBase::ControlLoop(int32_t period_ms) { + StopWatch ctrl_sw; + uint8_t cmd_count = 0; + uint8_t light_cmd_count = 0; + while (true) { + ctrl_sw.tic(); + + // motion control message + SendMotionCmd(cmd_count++); + + // check if there is request for light control + if (light_ctrl_requested_) SendLightCmd(light_cmd_count++); + + ctrl_sw.sleep_until_ms(period_ms); + // std::cout << "control loop update frequency: " << 1.0 / ctrl_sw.toc() << + // std::endl; + } +} + +ScoutState MobileBase::GetScoutState() { + std::lock_guard guard(scout_state_mutex_); + return scout_state_; +} + +void MobileBase::SetMotionCommand( + double linear_vel, double angular_vel, + ScoutMotionCmd::FaultClearFlag fault_clr_flag) { + // make sure cmd thread is started before attempting to send commands + if (!cmd_thread_started_) StartCmdThread(); + + if (linear_vel < ScoutMotionCmd::min_linear_velocity) + linear_vel = ScoutMotionCmd::min_linear_velocity; + if (linear_vel > ScoutMotionCmd::max_linear_velocity) + linear_vel = ScoutMotionCmd::max_linear_velocity; + if (angular_vel < ScoutMotionCmd::min_angular_velocity) + angular_vel = ScoutMotionCmd::min_angular_velocity; + if (angular_vel > ScoutMotionCmd::max_angular_velocity) + angular_vel = ScoutMotionCmd::max_angular_velocity; + + std::lock_guard guard(motion_cmd_mutex_); + current_motion_cmd_.linear_velocity = static_cast( + linear_vel / ScoutMotionCmd::max_linear_velocity * 100.0); + current_motion_cmd_.angular_velocity = static_cast( + angular_vel / ScoutMotionCmd::max_angular_velocity * 100.0); + current_motion_cmd_.fault_clear_flag = fault_clr_flag; +} + +void MobileBase::SetLightCommand(ScoutLightCmd cmd) { + if (!cmd_thread_started_) StartCmdThread(); + + std::lock_guard guard(light_cmd_mutex_); + current_light_cmd_ = cmd; + light_ctrl_enabled_ = true; + light_ctrl_requested_ = true; +} + +void MobileBase::DisableLightCmdControl() { + std::lock_guard guard(light_cmd_mutex_); + light_ctrl_enabled_ = false; + light_ctrl_requested_ = true; +} + +void MobileBase::ParseCANFrame(can_frame *rx_frame) { + // validate checksum, discard frame if fails + if (!rx_frame->data[7] == CalcScoutCANChecksum(rx_frame->can_id, + rx_frame->data, + rx_frame->can_dlc)) { + std::cerr << "ERROR: checksum mismatch, discard frame with id " + << rx_frame->can_id << std::endl; + return; + } + + // otherwise, update robot state with new frame + ScoutMessage status_msg; + DecodeScoutMsgFromCAN(rx_frame, &status_msg); + NewStatusMsgReceivedCallback(status_msg); +} + +void MobileBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize, + size_t bytes_received) { + // std::cout << "bytes received from serial: " << bytes_received << std::endl; + // serial_parser_.PrintStatistics(); + // serial_parser_.ParseBuffer(buf, bytes_received); + ScoutMessage status_msg; + for (int i = 0; i < bytes_received; ++i) { + if (DecodeScoutMsgFromUART(buf[i], &status_msg)) + NewStatusMsgReceivedCallback(status_msg); + } +} + +void MobileBase::NewStatusMsgReceivedCallback(const ScoutMessage &msg) { + // std::cout << "new status msg received" << std::endl; + std::lock_guard guard(scout_state_mutex_); + UpdateScoutState(msg, scout_state_); +} + +void MobileBase::UpdateScoutState(const ScoutMessage &status_msg, + ScoutState &state) { + switch (status_msg.type) { + case ScoutMotionStatusMsg: { + // std::cout << "motion control feedback received" << std::endl; + const MotionStatusMessage &msg = status_msg.body.motion_status_msg; + state.linear_velocity = + static_cast( + static_cast(msg.data.status.linear_velocity.low_byte) | + static_cast(msg.data.status.linear_velocity.high_byte) + << 8) / + 1000.0; + state.angular_velocity = + static_cast( + static_cast(msg.data.status.angular_velocity.low_byte) | + static_cast(msg.data.status.angular_velocity.high_byte) + << 8) / + 1000.0; + break; + } + case ScoutLightStatusMsg: { + // std::cout << "light control feedback received" << std::endl; + const LightStatusMessage &msg = status_msg.body.light_status_msg; + if (msg.data.status.light_ctrl_enable == LIGHT_DISABLE_CTRL) + state.light_control_enabled = false; + else + state.light_control_enabled = true; + state.front_light_state.mode = msg.data.status.front_light_mode; + state.front_light_state.custom_value = msg.data.status.front_light_custom; + state.rear_light_state.mode = msg.data.status.rear_light_mode; + state.rear_light_state.custom_value = msg.data.status.rear_light_custom; + break; + } + case ScoutSystemStatusMsg: { + // std::cout << "system status feedback received" << std::endl; + const SystemStatusMessage &msg = status_msg.body.system_status_msg; + state.control_mode = msg.data.status.control_mode; + state.base_state = msg.data.status.base_state; + state.battery_voltage = + (static_cast(msg.data.status.battery_voltage.low_byte) | + static_cast(msg.data.status.battery_voltage.high_byte) + << 8) / + 10.0; + state.fault_code = + (static_cast(msg.data.status.fault_code.low_byte) | + static_cast(msg.data.status.fault_code.high_byte) << 8); + break; + } + case ScoutMotorDriverStatusMsg: { + // std::cout << "motor 1 driver feedback received" << std::endl; + const MotorDriverStatusMessage &msg = + status_msg.body.motor_driver_status_msg; + for (int i = 0; i < 4; ++i) { + state.motor_states[status_msg.body.motor_driver_status_msg.motor_id] + .current = + (static_cast(msg.data.status.current.low_byte) | + static_cast(msg.data.status.current.high_byte) << 8) / + 10.0; + state.motor_states[status_msg.body.motor_driver_status_msg.motor_id] + .rpm = static_cast( + static_cast(msg.data.status.rpm.low_byte) | + static_cast(msg.data.status.rpm.high_byte) << 8); + state.motor_states[status_msg.body.motor_driver_status_msg.motor_id] + .temperature = msg.data.status.temperature; + } + break; + } + } +} +} // namespace westonrobot diff --git a/src/platforms/scout_base.cpp b/src/platforms/scout_base.cpp index 76fb586..ac924df 100644 --- a/src/platforms/scout_base.cpp +++ b/src/platforms/scout_base.cpp @@ -12,7 +12,7 @@ #include "stopwatch.h" -namespace wescore +namespace westonrobot { ScoutBase::~ScoutBase() { diff --git a/src/platforms/tracer_base.cpp b/src/platforms/tracer_base.cpp index de1680b..6e1fe63 100644 --- a/src/platforms/tracer_base.cpp +++ b/src/platforms/tracer_base.cpp @@ -12,7 +12,7 @@ #include "stopwatch/stopwatch.h" -namespace wescore +namespace westonrobot { TracerBase::~TracerBase() { @@ -335,4 +335,4 @@ void TracerBase::UpdateTracerState(const TracerMessage &status_msg, TracerState } } } -} // namespace wescore +} // namespace westonrobot diff --git a/src/stopwatch.h b/src/stopwatch.h index f30b1c5..df76d4a 100644 --- a/src/stopwatch.h +++ b/src/stopwatch.h @@ -35,7 +35,7 @@ For more information, please refer to #include -namespace wescore { +namespace westonrobot { // only supported on x86 processors #if (defined __x86_64__) || (defined __i386) // An implementation of the 'TrivialClock' concept using the rdtscp instruction. @@ -157,6 +157,6 @@ struct StopWatch { }; }; -} // namespace wescore +} // namespace westonrobot #endif // STOPWATCH_H diff --git a/tests/test_acan.cpp b/tests/test_acan.cpp index e454f87..783fe14 100644 --- a/tests/test_acan.cpp +++ b/tests/test_acan.cpp @@ -1,7 +1,7 @@ #include -#include "wrp_sdk/async_io/async_can.hpp" +#include "wrp_sdk/asyncio/async_can.hpp" -using namespace wescore; +using namespace westonrobot; void parse_buffer(uint8_t *buf, const size_t bufsize, size_t bytes_received) { diff --git a/tests/test_aserial.cpp b/tests/test_aserial.cpp index 9214e03..bc956e5 100644 --- a/tests/test_aserial.cpp +++ b/tests/test_aserial.cpp @@ -7,9 +7,9 @@ #include -#include "wrp_sdk/async_io/async_serial.hpp" +#include "wrp_sdk/asyncio/async_serial.hpp" -using namespace wescore; +using namespace westonrobot; void parse_buffer(uint8_t *buf, const size_t bufsize, size_t bytes_received) { diff --git a/tests/test_aserial_comm.cpp b/tests/test_aserial_comm.cpp index 82bef37..a7d7231 100644 --- a/tests/test_aserial_comm.cpp +++ b/tests/test_aserial_comm.cpp @@ -7,9 +7,9 @@ #include -#include "wrp_sdk/async_io/async_serial.hpp" +#include "wrp_sdk/asyncio/async_serial.hpp" -using namespace wescore; +using namespace westonrobot; void parse_buffer(uint8_t *buf, const size_t bufsize, size_t bytes_received) { diff --git a/tests/test_can_msg.cpp b/tests/test_can_msg.cpp index a86648b..ef9ef2f 100644 --- a/tests/test_can_msg.cpp +++ b/tests/test_can_msg.cpp @@ -2,7 +2,7 @@ #include "scout_base/details/scout_can_parser.hpp" -using namespace wescore; +using namespace westonrobot; void print_msg(uint8_t data[8]) { diff --git a/tests/test_hunter_base.cpp b/tests/test_hunter_base.cpp index d829b1b..22f9fb7 100644 --- a/tests/test_hunter_base.cpp +++ b/tests/test_hunter_base.cpp @@ -12,7 +12,7 @@ #define TEST_WITHOUT_SERIAL_HARDWARE -using namespace wescore; +using namespace westonrobot; int main(int argc, char **argv) { std::string device_name; diff --git a/tests/test_scout_base.cpp b/tests/test_scout_base.cpp index c5d1d26..49db34b 100644 --- a/tests/test_scout_base.cpp +++ b/tests/test_scout_base.cpp @@ -12,7 +12,7 @@ #define TEST_WITHOUT_SERIAL_HARDWARE -using namespace wescore; +using namespace westonrobot; int main(int argc, char **argv) { diff --git a/tests/test_scout_serial.cpp b/tests/test_scout_serial.cpp index bb1441f..c5b9af4 100644 --- a/tests/test_scout_serial.cpp +++ b/tests/test_scout_serial.cpp @@ -2,13 +2,13 @@ #include "scout_base/details/scout_serial_parser.hpp" -using namespace wescore; +using namespace westonrobot; #include "scout_base/scout_base.hpp" #define TEST_WITHOUT_SERIAL_HARDWARE -using namespace wescore; +using namespace westonrobot; int main(int argc, char **argv) { diff --git a/tests/test_serial_parser.cpp b/tests/test_serial_parser.cpp index 0a97de1..1b56905 100644 --- a/tests/test_serial_parser.cpp +++ b/tests/test_serial_parser.cpp @@ -2,7 +2,7 @@ #include "scout_base/details/scout_serial_parser.hpp" -using namespace wescore; +using namespace westonrobot; void print_msg(uint8_t data[8]) { diff --git a/tests/test_tracer_base.cpp b/tests/test_tracer_base.cpp index 4a0a617..99e2403 100644 --- a/tests/test_tracer_base.cpp +++ b/tests/test_tracer_base.cpp @@ -12,7 +12,7 @@ #define TEST_WITHOUT_SERIAL_HARDWARE -using namespace wescore; +using namespace westonrobot; int main(int argc, char **argv) { diff --git a/unit_tests/gtests/scout_can_protocol_test.cpp b/unit_tests/gtests/scout_can_protocol_test.cpp index 104efc5..b34de4b 100644 --- a/unit_tests/gtests/scout_can_protocol_test.cpp +++ b/unit_tests/gtests/scout_can_protocol_test.cpp @@ -17,7 +17,7 @@ #include "wrp_sdk/platforms/scout/scout_base.hpp" -using namespace wescore; +using namespace westonrobot; struct ScoutCANProtocolTest : testing::Test {