diff --git a/CMakeLists.txt b/CMakeLists.txt index f7949ba..eab878e 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -71,21 +71,26 @@ find_package(Threads REQUIRED) # add sdk main library add_library(${PROJECT_NAME} - # async_port from weston robot + ######################## + ## async_port from weston robot src/async_port/async_serial.cpp src/async_port/async_can.cpp - # agx common - src/agilex_base.cpp - src/agx_msg_parser.c - # robot support - src/scout_base.cpp - # src/hunter_base.cpp - src/tracer_base.cpp - # src/bunker_base.cpp - src/ranger_base.cpp ######################## - # legacy support - src/legacy/scout_can_parser_v1.c + ## public interface to access robot + # src/mobile_base/scout_robot.cpp + ######################## + ## protocol v2 support + # parser + # src/protocol_v2/agilex_base.cpp + src/protocol_v2/agx_msg_parser.c + src/protocol_v2/protocol_v2_parser.cpp + # robots + src/protocol_v2/scout_base.cpp + # src/protocol_v2/tracer_base.cpp + # src/protocol_v2/ranger_base.cpp + ######################## + ## legacy protocol v1 support + src/protocol_v1/scout_can_parser_v1.c ) target_link_libraries(${PROJECT_NAME} PUBLIC Threads::Threads) target_compile_definitions(${PROJECT_NAME} PUBLIC ASIO_ENABLE_OLD_SERVICES) @@ -108,7 +113,7 @@ if(BUILD_WITHOUT_ROS) endif() # add app source directory - add_subdirectory(apps) + add_subdirectory(demo) endif() # Build tests diff --git a/README.md b/README.md index 6948130..daa0e44 100755 --- a/README.md +++ b/README.md @@ -32,12 +32,17 @@ It should also work in other similar Linux environments but only the above liste **Communication protocol** -| Robot | Protocol Version | -| :----: | :--------------: | -| Scout | V1, V2 | -| Hunter | V1, V2 | -| Tracer | V2 | -| Bunker | V1 | +| Robot | Protocol V1 | Protocol V2 | UART | CAN | Support Status | +| :---------------: | :---------: | :---------: | :---: | :---: | :------------: | +| Scout 1.0 | Y | - | Y | Y | Discontinued | +| Scout 2.0 | Y | Y | Y | Y | Active | +| Scout Mini (Skid) | Y | Y | - | Y | Active | +| Scout Mini (Omni) | Y | Y | - | Y | Active | +| Hunter 1.0 | Y | Y | - | Y | Active | +| Hunter 2.0 | - | Y | - | Y | Active | +| Bunker | Y | Y | - | Y | Active | +| Tracer | - | Y | Y | Y | Active | +| Ranger Mini | - | Y | - | Y | Active | Generally, you only need to instantiate an object of the robot base class (such as ScoutBase), then use the object to programmatically control the robot. Internally, the base class manages two background threads, one to process CAN/UART messages of the robot state and accordingly update state variables in the robot state data structure, and the other to maintain a 50Hz loop and send the latest command to the robot base. User can iteratively perform tasks in the main thread and check the robot state or set control commands. Advanced users may also use this setup as a reference and maintain the control and monitoring loops in a different way. diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt deleted file mode 100755 index eec58c8..0000000 --- a/apps/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -# Dependency libraries -#find_package(LIBRARY_NAME REQUIRED) - -# tests -# add_executable(app_scout_demo scout_demo/scout_demo.cpp) -# target_link_libraries(app_scout_demo ugv_sdk) - -# add_executable(app_scout_discharge scout_demo/scout_discharge.cpp) -# target_link_libraries(app_scout_discharge ugv_sdk) - -# add_executable(app_hunter_demo hunter_demo/hunter_demo.cpp) -# target_link_libraries(app_hunter_demo ugv_sdk) - -add_executable(app_tracer_demo tracer_demo/tracer_demo.cpp) -target_link_libraries(app_tracer_demo ugv_sdk) - -add_executable(app_ranger_demo ranger_demo/ranger_demo.cpp) -target_link_libraries(app_ranger_demo ugv_sdk) - -# if(BUILD_MONITOR) -# add_subdirectory(scout_monitor) -# endif() diff --git a/apps/scout_demo/scout_demo.cpp b/apps/scout_demo/scout_demo.cpp deleted file mode 100644 index 8a16a8c..0000000 --- a/apps/scout_demo/scout_demo.cpp +++ /dev/null @@ -1,117 +0,0 @@ -/* - * demo_scout_can.cpp - * - * Created on: Jun 12, 2019 05:03 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#include "ugv_sdk/scout_base.hpp" - -using namespace westonrobot; - -int main(int argc, char **argv) { - std::string device_name; - - if (argc == 2) { - device_name = {argv[1]}; - std::cout << "Specified CAN: " << device_name << std::endl; - } else { - std::cout << "Usage: app_scout_demo " << std::endl - << "Example 1: ./app_scout_demo can0" << std::endl; - return -1; - } - - ScoutBase scout; - scout.Connect(device_name); - - scout.EnableCommandedMode(); - -// // light control - std::cout << "Light: const off" << std::endl; - scout.SetLightCommand({CONST_OFF, 0, - CONST_OFF, 0}); - sleep(3); - std::cout << "Light: const on" << std::endl; - scout.SetLightCommand({CONST_ON, 0, - CONST_ON, 0}); - sleep(3); - std::cout << "Light: breath" << std::endl; - scout.SetLightCommand({BREATH, 0, - BREATH, 0}); - sleep(3); - std::cout << "Light: custom 90-80" << std::endl; - scout.SetLightCommand({CUSTOM, 90, - CUSTOM, 80}); - sleep(3); - std::cout << "Light: diabled cmd control" << std::endl; - scout.DisableLightControl(); - - int count = 0; - while (true) { - // motion control - // if (count < 5) - // { - // std::cout << "Motor: 0.2, 0" << std::endl; - // scout.SetMotionCommand(0.2, 0.0); - // } - // else if (count < 10) - // { - // std::cout << "Motor: 0.8, 0" << std::endl; - // scout.SetMotionCommand(0.8, 0.0); - // } - // else if (count < 15) - // { - // std::cout << "Motor: 1.5, 0" << std::endl; - // scout.SetMotionCommand(1.5, 0.0); - // } - // else if (count < 20) - // { - // std::cout << "Motor: 1.0, 0.5" << std::endl; - // scout.SetMotionCommand(1.0, 0.5); - // } - // else if (count < 25) - // { - // std::cout << "Motor: 0.0, 0" << std::endl; - // scout.SetMotionCommand(0.0, 0.0); - // } - // else if (count < 30) - // { - // std::cout << "Motor: -0.5, 0" << std::endl; - // scout.SetMotionCommand(-0.5, 0.0); - // } - // else if (count < 35) - // { - // std::cout << "Motor: -1.0, -0.5" << std::endl; - // scout.SetMotionCommand(-1.0, -0.5); - // } - // else if (count < 40) - // { - // std::cout << "Motor: 0.0, 0, Light: breath" << std::endl; - // scout.SetMotionCommand(0.0, 0.0); - // scout.SetLightCommand({BREATH, 0, - // BREATH, 0}); - // } - if (count < 100) { - std::cout << "Motor: 0.2, 0" << std::endl; - scout.SetMotionCommand(0.2, 0.0); - } - - auto state = scout.GetScoutState(); - std::cout << "-------------------------------" << std::endl; - std::cout << "count: " << count << std::endl; - std::cout << "control mode: " << static_cast(state.system_state.control_mode) - << " , vehicle state: " << static_cast(state.system_state.vehicle_state) - << std::endl; - std::cout << "battery voltage: " << state.system_state.battery_voltage << std::endl; - std::cout << "velocity (linear, angular): " << state.motion_state.linear_velocity << ", " - << state.motion_state.angular_velocity << std::endl; - std::cout << "-------------------------------" << std::endl; - - usleep(20000); - ++count; - } - - return 0; -} \ No newline at end of file diff --git a/apps/scout_demo/scout_discharge.cpp b/apps/scout_demo/scout_discharge.cpp deleted file mode 100644 index 185b460..0000000 --- a/apps/scout_demo/scout_discharge.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/* - * demo_scout_can.cpp - * - * Created on: Jun 12, 2019 05:03 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#include "ugv_sdk/scout_base.hpp" - -using namespace westonrobot; - -int main(int argc, char **argv) -{ - std::string device_name; - - if (argc == 2) - { - device_name = {argv[1]}; - std::cout << "Specified CAN: " << device_name << std::endl; - } - else - { - std::cout << "Usage: app_scout_monitor " << std::endl - << "Example 1: ./app_scout_demo can0" << std::endl - << "Example 2: ./app_scout_demo /dev/ttyUSB0 115200" << std::endl; - return -1; - } - - ScoutBase scout; - scout.Connect(device_name); - - scout.EnableCommandedMode(); - - // light control - std::cout << "Light: const on" << std::endl; - scout.SetLightCommand({CONST_ON, 0, CONST_ON, 0}); - - int count = 0; - while (true) - { - auto state = scout.GetScoutState(); - - if (state.system_state.battery_voltage >= 22.5) - { - scout.SetMotionCommand(1.35, 0); - - std::cout << "-------------------------------" << std::endl; - std::cout << "-------> discharging <--------" << std::endl; - std::cout << "elapsed time: " << count / 60 << " minutes " << count % 60 << " seconds" << std::endl; - std::cout << "control mode: " << static_cast(state.system_state.control_mode) << " , vehicle state: " << static_cast(state.system_state.vehicle_state) << std::endl; - std::cout << "battery voltage: " << state.system_state.battery_voltage << std::endl; - std::cout << "velocity (linear, angular): " << state.motion_state.linear_velocity << ", " << state.motion_state.angular_velocity << std::endl; - std::cout << "-------------------------------" << std::endl; - } - else - { - scout.SetMotionCommand(0, 0); - std::cout << "-------------------------------" << std::endl; - std::cout << "discharge stopped at: " << state.system_state.battery_voltage << " V" << std::endl; - std::cout << "-------------------------------" << std::endl; - } - sleep(1); - ++count; - } - - return 0; -} \ No newline at end of file diff --git a/apps/scout_monitor/CMakeLists.txt b/apps/scout_monitor/CMakeLists.txt deleted file mode 100644 index 8776490..0000000 --- a/apps/scout_monitor/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -# Dependency libraries -set(CURSES_NEED_NCURSES TRUE) -find_package(Curses REQUIRED) - -# Add libraries -set(SCOUT_MONITOR_SRC - src/nshapes.cpp - src/ncolors.cpp - src/scout_monitor.cpp -) -add_library(monitor STATIC ${SCOUT_MONITOR_SRC}) -target_link_libraries(monitor ugv_sdk ${CURSES_LIBRARIES}) -target_include_directories(monitor PUBLIC - $ - $ - $ - PRIVATE src) - -add_subdirectory(app) - -# Add executables -if(BUILD_TESTS) - add_subdirectory(tests) -endif() \ No newline at end of file diff --git a/apps/scout_monitor/app/CMakeLists.txt b/apps/scout_monitor/app/CMakeLists.txt deleted file mode 100644 index 373a576..0000000 --- a/apps/scout_monitor/app/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -# Add executables -add_executable(app_scout_monitor app_scout_monitor.cpp) -target_link_libraries(app_scout_monitor monitor) \ No newline at end of file diff --git a/apps/scout_monitor/app/app_scout_monitor.cpp b/apps/scout_monitor/app/app_scout_monitor.cpp deleted file mode 100644 index cde67b0..0000000 --- a/apps/scout_monitor/app/app_scout_monitor.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include "monitor/scout_monitor.hpp" - -using namespace westonrobot; - -int main(int argc, char **argv) -{ - std::string device_name; - int32_t baud_rate = 0; - - if (argc == 2) - { - device_name = {argv[1]}; - std::cout << "Specified CAN: " << device_name << std::endl; - } - else if (argc == 3) - { - device_name = {argv[1]}; - baud_rate = std::stol(argv[2]); - std::cout << "Specified serial: " << device_name << "@" << baud_rate << std::endl; - } - else - { - std::cout << "Usage: app_scout_monitor " << std::endl - << "Example 1: ./app_scout_monitor can1" << std::endl - << "Example 2: ./app_scout_monitor /dev/ttyUSB0 115200" << std::endl; - return -1; - } - - ScoutMonitor monitor; - monitor.Run(device_name, baud_rate); - - return 0; -} \ No newline at end of file diff --git a/apps/scout_monitor/color_demo.png b/apps/scout_monitor/color_demo.png deleted file mode 100644 index 92f0331..0000000 Binary files a/apps/scout_monitor/color_demo.png and /dev/null differ diff --git a/apps/scout_monitor/include/monitor/ncolors.hpp b/apps/scout_monitor/include/monitor/ncolors.hpp deleted file mode 100644 index 4af8925..0000000 --- a/apps/scout_monitor/include/monitor/ncolors.hpp +++ /dev/null @@ -1,67 +0,0 @@ -/* - * ncolors.hpp - * - * Created on: Jun 20, 2019 06:22 - * Description: - * - * Original source: https://www.linuxjournal.com/content/about-ncurses-colors-0 - * This copy is based on the original implementation, modified and - * maintained by Ruixiang. - * - * Copyright (c) 2018 Jim Hall - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#ifndef NCOLORS_HPP -#define NCOLORS_HPP - -#include - -namespace westonrobot -{ -struct NColors -{ - enum BackgroundColor - { - BLACK = 0, - BLUE, - GREEN, - CYAN, - RED, - MAGENTA, - YELLOW, - WHITE - }; - - enum ForegroundColor - { - /* - BLACK = 0, - BLUE, - GREEN, - CYAN, - RED, - MAGENTA, - YELLOW, - WHITE,*/ - BRIGHT_BLACK = 8, - BRIGHT_BLUE, - BRIGHT_GREEN, - BRIGHT_CYAN, - BRIGHT_RED, - BRIGHT_MAGENTA, - BRIGHT_YELLOW, - BRIGHT_WHITE - }; - - static void InitColors(); - - static void SetColor(int fg, int bg = BLACK); - static void UnsetColor(int fg, int bg = BLACK); - - static void WSetColor(WINDOW *win, int fg, int bg = BLACK); - static void WUnsetColor(WINDOW *win, int fg, int bg = BLACK); -}; -} // namespace westonrobot - -#endif /* NCOLORS_HPP */ diff --git a/apps/scout_monitor/include/monitor/nshapes.hpp b/apps/scout_monitor/include/monitor/nshapes.hpp deleted file mode 100644 index 5f71a29..0000000 --- a/apps/scout_monitor/include/monitor/nshapes.hpp +++ /dev/null @@ -1,24 +0,0 @@ -/* - * nshapes.hpp - * - * Created on: Jun 20, 2019 06:21 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#ifndef NSHAPES_HPP -#define NSHAPES_HPP - -#include - -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 westonrobot - -#endif /* NSHAPES_HPP */ diff --git a/apps/scout_monitor/include/monitor/scout_monitor.hpp b/apps/scout_monitor/include/monitor/scout_monitor.hpp deleted file mode 100644 index 0916a33..0000000 --- a/apps/scout_monitor/include/monitor/scout_monitor.hpp +++ /dev/null @@ -1,78 +0,0 @@ -/* - * scout_monitor.hpp - * - * Created on: Jun 12, 2019 01:19 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#ifndef SCOUT_MONITOR_HPP -#define SCOUT_MONITOR_HPP - -#include "ugv_sdk/scout_base.hpp" - -#include - -namespace westonrobot -{ -class ScoutMonitor -{ -public: - ScoutMonitor(); - ~ScoutMonitor(); - - void Run(std::string device_name = "", int32_t baud_rate = 0); - void Terminate() { keep_running_ = false; } - -private: - bool keep_running_ = true; - bool test_mode_ = true; - - int term_sx_ = -1; - int term_sy_ = -1; - - WINDOW *body_info_win_; - int bi_win_sx_; - int bi_win_sy_; - int bi_origin_x_; - int bi_origin_y_; - - WINDOW *system_info_win_; - int si_win_sx_; - int si_win_sy_; - int si_origin_x_; - int si_origin_y_; - - WINDOW *scout_cmd_win_; - - ScoutBase scout_base_; - ScoutState scout_state_; - - const int linear_axis_length_ = 5; - const int angular_axis_length_ = 5; - - const int vehicle_fp_offset_x_ = 9; - const int vehicle_fp_offset_y_ = 9; - - bool resizing_detected_; - - void UpdateAll(); - void ClearAll(); - - void CalcDimensions(); - void HandleResizing(); - - void SetTestStateData(); - void ShowVehicleState(int y, int x); - void ShowStatusItemName(int y, int x, std::string name); - void ShowFault(int y, int x, bool no_fault); - void ShowMotorInfo(int y, int x, double cur, int rpm, int temp, bool is_right); - - void UpdateScoutBodyInfo(); - void UpdateScoutSystemInfo(); - void UpdateScoutCmdWindow(); -}; -} // namespace westonrobot - -#endif /* SCOUT_MONITOR_HPP */ diff --git a/apps/scout_monitor/src/ncolors.cpp b/apps/scout_monitor/src/ncolors.cpp deleted file mode 100644 index 2d76168..0000000 --- a/apps/scout_monitor/src/ncolors.cpp +++ /dev/null @@ -1,116 +0,0 @@ -/* - * ncolors.cpp - * - * Created on: Jun 20, 2019 06:22 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#include "monitor/ncolors.hpp" - -#include - -namespace -{ -int IsBold(int fg) -{ - /* return the intensity bit */ - - int i; - - i = 1 << 3; - return (i & fg); -} - -int ColorNum(int fg, int bg) -{ - int B, bbb, ffff; - - B = 1 << 7; - bbb = (7 & bg) << 4; - ffff = 7 & fg; - - return (B | bbb | ffff); -} - -short CursorColor(int fg) -{ - switch (7 & fg) - { /* RGB */ - case 0: /* 000 */ - return (COLOR_BLACK); - case 1: /* 001 */ - return (COLOR_BLUE); - case 2: /* 010 */ - return (COLOR_GREEN); - case 3: /* 011 */ - return (COLOR_CYAN); - case 4: /* 100 */ - return (COLOR_RED); - case 5: /* 101 */ - return (COLOR_MAGENTA); - case 6: /* 110 */ - return (COLOR_YELLOW); - case 7: /* 111 */ - return (COLOR_WHITE); - default: - return COLOR_BLACK; - } -} -} // namespace - -namespace westonrobot -{ -void NColors::InitColors() -{ - if (has_colors() != FALSE) - start_color(); - else - std::cerr << "Your terminal does not support color" << std::endl; - - int fg, bg; - int colorpair; - - for (bg = 0; bg <= 7; bg++) - { - for (fg = 0; fg <= 7; fg++) - { - colorpair = ColorNum(fg, bg); - init_pair(colorpair, CursorColor(fg), CursorColor(bg)); - } - } -} - -void NColors::SetColor(int fg, int bg) -{ - // set the color pair (ColorNum) and bold/bright (A_BOLD) - attron(COLOR_PAIR(ColorNum(fg, bg))); - if (IsBold(fg)) - attron(A_BOLD); -} - -void NColors::UnsetColor(int fg, int bg) -{ - // unset the color pair (ColorNum) and bold/bright (A_BOLD) - attroff(COLOR_PAIR(ColorNum(fg, bg))); - if (IsBold(fg)) - attroff(A_BOLD); -} - -void NColors::WSetColor(WINDOW *win, int fg, int bg) -{ - // set the color pair (ColorNum) and bold/bright (A_BOLD) - wattron(win, COLOR_PAIR(ColorNum(fg, bg))); - if (IsBold(fg)) - wattron(win, A_BOLD); -} - -void NColors::WUnsetColor(WINDOW *win, int fg, int bg) -{ - // unset the color pair (ColorNum) and bold/bright (A_BOLD) - wattroff(win, COLOR_PAIR(ColorNum(fg, bg))); - if (IsBold(fg)) - wattroff(win, A_BOLD); -} -} // namespace westonrobot \ No newline at end of file diff --git a/apps/scout_monitor/src/nshapes.cpp b/apps/scout_monitor/src/nshapes.cpp deleted file mode 100644 index 0fc2e6e..0000000 --- a/apps/scout_monitor/src/nshapes.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/* - * nshapers.cpp - * - * Created on: Jun 20, 2019 06:21 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#include "monitor/nshapes.hpp" - -namespace westonrobot -{ -void NShapes::DrawRectangle(int tl_y, int tl_x, int br_y, int br_x) -{ - for (int i = tl_y; i <= br_y; ++i) - { - mvprintw(i, tl_x, "|"); - mvprintw(i, br_x, "|"); - } - for (int i = tl_x; i <= br_x; ++i) - { - mvprintw(tl_y, i, "-"); - mvprintw(br_y, i, "-"); - } -} - -void NShapes::WDrawRectangle(WINDOW *win, int tl_y, int tl_x, int br_y, int br_x) -{ - for (int i = tl_y; i <= br_y; ++i) - { - mvwprintw(win, i, tl_x, "|"); - mvwprintw(win, i, br_x, "|"); - } - for (int i = tl_x; i <= br_x; ++i) - { - mvwprintw(win, tl_y, i, "-"); - mvwprintw(win, br_y, i, "-"); - } -} -} // 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 deleted file mode 100644 index d11baf5..0000000 --- a/apps/scout_monitor/src/scout_monitor.cpp +++ /dev/null @@ -1,658 +0,0 @@ -/* - * scout_monitor.cpp - * - * Created on: Jun 12, 2019 01:19 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -/* - * Coordinate System: - * - * o --------------------> x - * | - * | - * | - * | - * | - * | - * v - * y - */ - -#include "monitor/scout_monitor.hpp" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "monitor/nshapes.hpp" -#include "monitor/ncolors.hpp" - -namespace -{ -// reference: https://thispointer.com/c-convert-double-to-string-and-manage-precision-scientific-notation/ -std::string ConvertFloatToString(double vel, int digit_num = 3) -{ - std::ostringstream streamObj; - streamObj << std::fixed; - streamObj << std::setprecision(digit_num); - streamObj << vel; - return streamObj.str(); -} - -// source: https://github.com/rxdu/stopwatch -struct StopWatch -{ - using Clock = std::chrono::high_resolution_clock; - using time_point = typename Clock::time_point; - using duration = typename Clock::duration; - - StopWatch() { tic_point = Clock::now(); }; - - time_point tic_point; - - void tic() - { - tic_point = Clock::now(); - }; - - double toc() - { - return std::chrono::duration_cast(Clock::now() - tic_point).count() / 1000000.0; - }; - - // for different precisions - double stoc() - { - return std::chrono::duration_cast(Clock::now() - tic_point).count(); - }; - - double mtoc() - { - return std::chrono::duration_cast(Clock::now() - tic_point).count(); - }; - - double utoc() - { - return std::chrono::duration_cast(Clock::now() - tic_point).count(); - }; - - double ntoc() - { - return std::chrono::duration_cast(Clock::now() - tic_point).count(); - }; - - // you have to call tic() before calling this function - void sleep_until_ms(int64_t period_ms) - { - int64_t duration = period_ms - std::chrono::duration_cast(Clock::now() - tic_point).count(); - - if (duration > 0) - std::this_thread::sleep_for(std::chrono::milliseconds(duration)); - }; - - void sleep_until_us(int64_t period_us) - { - int64_t duration = period_us - std::chrono::duration_cast(Clock::now() - tic_point).count(); - - if (duration > 0) - std::this_thread::sleep_for(std::chrono::microseconds(duration)); - }; -}; -} // namespace - -namespace westonrobot -{ -ScoutMonitor::ScoutMonitor() -{ - // init ncurses - initscr(); - // raw(); - cbreak(); - noecho(); - nonl(); - curs_set(FALSE); - intrflush(stdscr, FALSE); - keypad(stdscr, TRUE); - - CalcDimensions(); - - // setup sub-windows - body_info_win_ = newwin(bi_win_sy_, bi_win_sx_, bi_origin_y_, bi_origin_x_); - system_info_win_ = newwin(si_win_sy_, si_win_sx_, si_origin_y_, si_origin_x_); - - scout_state_.linear_velocity = 0; - scout_state_.angular_velocity = 0; - - NColors::InitColors(); -} - -ScoutMonitor::~ScoutMonitor() -{ - delwin(body_info_win_); - delwin(system_info_win_); - endwin(); -} - -void ScoutMonitor::UpdateAll() -{ - ClearAll(); - - CalcDimensions(); - if (resizing_detected_) - HandleResizing(); - - UpdateScoutBodyInfo(); - UpdateScoutSystemInfo(); -} - -void ScoutMonitor::SetTestStateData() -{ - scout_state_.base_state = BASE_STATE_NORMAL; - scout_state_.battery_voltage = 28.5; - - scout_state_.linear_velocity = 1.234; - scout_state_.angular_velocity = -0.6853; - - // scout_state_.fault_code |= FAULT_MOTOR_DRV_OVERHEAT_W; - // scout_state_.fault_code |= FAULT_MOTOR_OVERCURRENT_W; - // scout_state_.fault_code |= FAULT_MOTOR_DRV_OVERHEAT_F; - // scout_state_.fault_code |= FAULT_MOTOR_OVERCURRENT_F; - // scout_state_.fault_code |= FAULT_BAT_UNDER_VOL_W; - // scout_state_.fault_code |= FAULT_BAT_UNDER_VOL_F; - scout_state_.fault_code = 0x0000; - // scout_state_.fault_code = 0xffff; - - // scout_state_.front_light_state.mode = CONST_ON; - scout_state_.front_light_state.mode = LIGHT_MODE_CUSTOM; - scout_state_.front_light_state.custom_value = 50; - - scout_state_.rear_light_state.mode = LIGHT_MODE_CONST_ON; - - scout_state_.motor_states[0].current = 10.1; - scout_state_.motor_states[0].rpm = 2000; - scout_state_.motor_states[0].temperature = 35; - - scout_state_.motor_states[1].current = 10.1; - scout_state_.motor_states[1].rpm = 2000; - scout_state_.motor_states[1].temperature = 35; - - scout_state_.motor_states[2].current = 10.1; - scout_state_.motor_states[2].rpm = 2000; - scout_state_.motor_states[2].temperature = 35; - - scout_state_.motor_states[3].current = 10.1; - scout_state_.motor_states[3].rpm = 2000; - scout_state_.motor_states[3].temperature = 35; -} - -void ScoutMonitor::Run(std::string device_name, int32_t baud_rate) -{ - if (device_name != "") - test_mode_ = false; - - if (test_mode_) - SetTestStateData(); - else - scout_base_.Connect(device_name, baud_rate); - - StopWatch sw; - while (keep_running_) - { - // label starting point of iteration - sw.tic(); - - // query for latest robot state - if (!test_mode_) - scout_state_ = scout_base_.GetScoutState(); - - // update window contents - UpdateAll(); - - // manage window refresh rate - sw.sleep_until_ms(100); - } -} - -void ScoutMonitor::CalcDimensions() -{ - int sy, sx; - getmaxyx(stdscr, sy, sx); - - if (sy != term_sy_ || sx != term_sx_) - { - resizing_detected_ = true; - - term_sy_ = sy; - term_sx_ = sx; - - bi_win_sy_ = term_sy_; - bi_win_sx_ = term_sx_ * 15 / 24; - bi_origin_y_ = 0; - bi_origin_x_ = 0; - - si_win_sy_ = term_sy_; - si_win_sx_ = term_sx_ * 9 / 24; - si_origin_y_ = 0; - si_origin_x_ = bi_win_sx_; - } -} - -void ScoutMonitor::HandleResizing() -{ - delwin(body_info_win_); - delwin(system_info_win_); - - body_info_win_ = newwin(bi_win_sy_, bi_win_sx_, bi_origin_y_, bi_origin_x_); - system_info_win_ = newwin(si_win_sy_, si_win_sx_, si_origin_y_, si_origin_x_); - - resizing_detected_ = false; -} - -void ScoutMonitor::ClearAll() -{ - wclear(body_info_win_); - wclear(system_info_win_); -} - -void ScoutMonitor::ShowVehicleState(int y, int x) -{ - // show linear velocity - const int linear_axis_x = x + vehicle_fp_offset_x_; - const int linear_axis_tip_y = y + 2; - const int linear_axis_origin_y = linear_axis_tip_y + linear_axis_length_; - const int linear_axis_negative_y = linear_axis_origin_y + linear_axis_length_ + 1; - mvwprintw(body_info_win_, linear_axis_tip_y - 1, linear_axis_x, "^"); - for (int i = linear_axis_tip_y; i < linear_axis_origin_y; ++i) - mvwprintw(body_info_win_, i, linear_axis_x, "-"); - mvwprintw(body_info_win_, linear_axis_origin_y, linear_axis_x, "x"); - for (int i = linear_axis_origin_y + 1; i < linear_axis_negative_y; ++i) - mvwprintw(body_info_win_, i, linear_axis_x, "-"); - mvwprintw(body_info_win_, linear_axis_negative_y, linear_axis_x, "v"); - double linear_percentage = scout_state_.linear_velocity / ScoutMotionCmd::max_linear_velocity; - int linear_bars = std::abs(static_cast(linear_percentage * 5)) + 1; - if (std::abs(scout_state_.linear_velocity) < 0.001) - linear_bars = 0; - if (linear_bars > 5) - linear_bars = 5; - if (scout_state_.linear_velocity > 0) - { - for (int i = linear_axis_origin_y - linear_bars; i < linear_axis_origin_y; ++i) - { - NColors::WSetColor(body_info_win_, NColors::BLACK, NColors::CYAN); - mvwprintw(body_info_win_, i, linear_axis_x, "-"); - NColors::WUnsetColor(body_info_win_, NColors::BLACK, NColors::CYAN); - } - } - else if (scout_state_.linear_velocity < 0) - { - for (int i = linear_axis_origin_y + linear_bars; i > linear_axis_origin_y; --i) - { - NColors::WSetColor(body_info_win_, NColors::BLACK, NColors::CYAN); - mvwprintw(body_info_win_, i, linear_axis_x, "-"); - NColors::WUnsetColor(body_info_win_, NColors::BLACK, NColors::CYAN); - } - } - - // show angular velocity - const int angular_axis_y = linear_axis_origin_y; - const int angular_axis_origin_x = linear_axis_x; - const int angular_axis_positive_x = angular_axis_origin_x + angular_axis_length_ + 1; - const int angular_axis_negative_x = angular_axis_origin_x - angular_axis_length_; - mvwprintw(body_info_win_, angular_axis_y, angular_axis_negative_x - 1, "<"); - for (int i = angular_axis_negative_x; i < angular_axis_origin_x; ++i) - mvwprintw(body_info_win_, angular_axis_y, i, "-"); - mvwprintw(body_info_win_, linear_axis_origin_y, linear_axis_x, "x"); - for (int i = angular_axis_origin_x + 1; i < angular_axis_positive_x; ++i) - mvwprintw(body_info_win_, angular_axis_y, i, "-"); - mvwprintw(body_info_win_, angular_axis_y, angular_axis_positive_x, ">"); - - double angular_percentage = scout_state_.angular_velocity / ScoutMotionCmd::max_angular_velocity; - int angular_bars = std::abs(static_cast(angular_percentage * 5)) + 1; - if (std::abs(scout_state_.angular_velocity) < 0.001) - angular_bars = 0; - if (angular_bars > 5) - angular_bars = 5; - if (scout_state_.angular_velocity < 0) - { - for (int i = angular_axis_origin_x + angular_bars; i > angular_axis_origin_x; --i) - { - NColors::WSetColor(body_info_win_, NColors::BLACK, NColors::MAGENTA); - mvwprintw(body_info_win_, angular_axis_y, i, "-"); - NColors::WUnsetColor(body_info_win_, NColors::BLACK, NColors::MAGENTA); - } - } - else if (scout_state_.angular_velocity > 0) - { - for (int i = angular_axis_origin_x - angular_bars; i < angular_axis_origin_x; ++i) - { - NColors::WSetColor(body_info_win_, NColors::BLACK, NColors::MAGENTA); - mvwprintw(body_info_win_, angular_axis_y, i, "-"); - NColors::WUnsetColor(body_info_win_, NColors::BLACK, NColors::MAGENTA); - } - } - - // show velocity values - std::string linear_vel_str = "linear : " + ConvertFloatToString(scout_state_.linear_velocity); - mvwprintw(body_info_win_, linear_axis_negative_y + 2, angular_axis_negative_x - 2, linear_vel_str.c_str()); - - std::string angular_vel_str = "angular: " + ConvertFloatToString(scout_state_.angular_velocity); - mvwprintw(body_info_win_, linear_axis_negative_y + 3, angular_axis_negative_x - 2, angular_vel_str.c_str()); - - // show vehicle base - NShapes::WDrawRectangle(body_info_win_, linear_axis_tip_y - 2, angular_axis_negative_x - 4, - linear_axis_negative_y + 4, angular_axis_positive_x + 3); - - // show vehicle wheels - NShapes::WDrawRectangle(body_info_win_, linear_axis_tip_y - 1, angular_axis_negative_x - 9, - linear_axis_tip_y + 4, angular_axis_negative_x - 5); - NShapes::WDrawRectangle(body_info_win_, linear_axis_negative_y - 2, angular_axis_negative_x - 9, - linear_axis_negative_y + 3, angular_axis_negative_x - 5); - NShapes::WDrawRectangle(body_info_win_, linear_axis_tip_y - 1, angular_axis_positive_x + 4, - linear_axis_tip_y + 4, angular_axis_positive_x + 8); - NShapes::WDrawRectangle(body_info_win_, linear_axis_negative_y - 2, angular_axis_positive_x + 4, - linear_axis_negative_y + 3, angular_axis_positive_x + 8); - - // front right motor - ShowMotorInfo(linear_axis_tip_y - 1, angular_axis_positive_x + 4, scout_state_.motor_states[0].current, - scout_state_.motor_states[0].rpm, scout_state_.motor_states[0].temperature, true); - // front left motor - ShowMotorInfo(linear_axis_tip_y - 1, angular_axis_negative_x - 9, scout_state_.motor_states[1].current, - scout_state_.motor_states[1].rpm, scout_state_.motor_states[1].temperature, false); - // rear left motor - ShowMotorInfo(linear_axis_negative_y - 2, angular_axis_negative_x - 9, scout_state_.motor_states[2].current, - scout_state_.motor_states[2].rpm, scout_state_.motor_states[2].temperature, false); - // rear right motor - ShowMotorInfo(linear_axis_negative_y - 2, angular_axis_positive_x + 4, scout_state_.motor_states[3].current, - scout_state_.motor_states[3].rpm, scout_state_.motor_states[3].temperature, true); - - // show vehicle lights - std::string front_mode_str = "Mode: "; - if (scout_state_.front_light_state.mode == LIGHT_MODE_CONST_ON) - front_mode_str += "ON"; - else if (scout_state_.front_light_state.mode == LIGHT_MODE_CONST_OFF) - front_mode_str += "OFF"; - else if (scout_state_.front_light_state.mode == LIGHT_MODE_BREATH) - front_mode_str += "BREATH"; - else if (scout_state_.front_light_state.mode == LIGHT_MODE_CUSTOM) - front_mode_str += "CUSTOM"; - mvwprintw(body_info_win_, linear_axis_tip_y - 4, angular_axis_origin_x - 13, front_mode_str.c_str()); - std::string front_custom_str = "Custom: " + ConvertFloatToString(scout_state_.front_light_state.custom_value, 0); - mvwprintw(body_info_win_, linear_axis_tip_y - 4, angular_axis_origin_x + 3, front_custom_str.c_str()); - if (scout_state_.front_light_state.mode != LIGHT_MODE_CONST_OFF && - !(scout_state_.front_light_state.mode == LIGHT_MODE_CUSTOM && scout_state_.front_light_state.custom_value == 0)) - { - NColors::WSetColor(body_info_win_, NColors::BRIGHT_YELLOW); - for (int i = angular_axis_origin_x - 5; i < angular_axis_origin_x - 1; ++i) - mvwprintw(body_info_win_, linear_axis_tip_y - 3, i, "v"); - mvwprintw(body_info_win_, linear_axis_tip_y - 3, angular_axis_origin_x, "v"); - for (int i = angular_axis_origin_x + 2; i <= angular_axis_origin_x + 5; ++i) - mvwprintw(body_info_win_, linear_axis_tip_y - 3, i, "v"); - NColors::WUnsetColor(body_info_win_, NColors::BRIGHT_YELLOW); - } - - std::string rear_mode_str = "Mode: "; - if (scout_state_.rear_light_state.mode == LIGHT_MODE_CONST_ON) - rear_mode_str += "ON"; - else if (scout_state_.rear_light_state.mode == LIGHT_MODE_CONST_OFF) - rear_mode_str += "OFF"; - else if (scout_state_.rear_light_state.mode == LIGHT_MODE_BREATH) - rear_mode_str += "BREATH"; - else if (scout_state_.rear_light_state.mode == LIGHT_MODE_CUSTOM) - rear_mode_str += "CUSTOM"; - mvwprintw(body_info_win_, linear_axis_negative_y + 6, angular_axis_origin_x - 13, rear_mode_str.c_str()); - std::string rear_custom_str = "Custom: " + ConvertFloatToString(scout_state_.rear_light_state.custom_value, 0); - mvwprintw(body_info_win_, linear_axis_negative_y + 6, angular_axis_origin_x + 3, rear_custom_str.c_str()); - if (scout_state_.rear_light_state.mode != LIGHT_MODE_CONST_OFF && - !(scout_state_.rear_light_state.mode == LIGHT_MODE_CUSTOM && scout_state_.rear_light_state.custom_value == 0)) - { - NColors::WSetColor(body_info_win_, NColors::BRIGHT_RED); - for (int i = angular_axis_origin_x - 5; i < angular_axis_origin_x - 1; ++i) - mvwprintw(body_info_win_, linear_axis_negative_y + 5, i, "^"); - mvwprintw(body_info_win_, linear_axis_negative_y + 5, angular_axis_origin_x, "^"); - for (int i = angular_axis_origin_x + 2; i <= angular_axis_origin_x + 5; ++i) - mvwprintw(body_info_win_, linear_axis_negative_y + 5, i, "^"); - NColors::WUnsetColor(body_info_win_, NColors::BRIGHT_RED); - } -} - -void ScoutMonitor::UpdateScoutBodyInfo() -{ - // for (int i = 0; i < bi_win_sx_; i++) - // mvwprintw(body_info_win_, bi_win_sy_ - 1, i, "-"); - - ShowVehicleState(bi_win_sy_ / 2 - vehicle_fp_offset_y_, bi_win_sx_ / 2 - vehicle_fp_offset_x_); - - wrefresh(body_info_win_); -} - -void ScoutMonitor::UpdateScoutSystemInfo() -{ - for (int i = 0; i < si_win_sy_; i++) - mvwprintw(system_info_win_, i, 0, "|"); - - const int state_title_col = (si_win_sx_ - 24) / 2; - const int state_value_col = state_title_col + 20; - const int state_div_col = state_value_col - 2; - - // system state - const int sec1 = static_cast(std::round((si_win_sy_ - 20) / 2.0)); - ShowStatusItemName(sec1, state_title_col, "System state"); - - if (scout_state_.base_state == BASE_STATE_NORMAL) - { - NColors::WSetColor(system_info_win_, NColors::GREEN); - mvwprintw(system_info_win_, sec1, state_value_col, "NORMAL"); - NColors::WUnsetColor(system_info_win_, NColors::GREEN); - } - else if (scout_state_.base_state == BASE_STATE_ESTOP) - { - NColors::WSetColor(system_info_win_, NColors::YELLOW); - mvwprintw(system_info_win_, sec1, state_value_col, "ESTOP"); - NColors::WUnsetColor(system_info_win_, NColors::YELLOW); - } - else if (scout_state_.base_state == BASE_STATE_EXCEPTION) - { - NColors::WSetColor(system_info_win_, NColors::RED); - mvwprintw(system_info_win_, sec1, state_value_col, "EXCEPT"); - NColors::WUnsetColor(system_info_win_, NColors::RED); - } - - // control mode - ShowStatusItemName(sec1 + 1, state_title_col, "Control mode"); - if (scout_state_.control_mode == CTRL_MODE_REMOTE) - mvwprintw(system_info_win_, sec1 + 1, state_value_col, "REMOTE"); - else if (scout_state_.control_mode == CTRL_MODE_CMD_CAN) - mvwprintw(system_info_win_, sec1 + 1, state_value_col, "CMD"); - else if (scout_state_.control_mode == CTRL_MODE_CMD_UART) - mvwprintw(system_info_win_, sec1 + 1, state_value_col, "CMD"); - // mvwprintw(system_info_win_, sec1 + 1, state_value_col, std::to_string(scout_state_.control_mode).c_str()); - - // battery voltage - ShowStatusItemName(sec1 + 2, state_title_col, "Battery voltage"); - std::string bat_vol_str = ConvertFloatToString(scout_state_.battery_voltage, 1) + " v"; - mvwprintw(system_info_win_, sec1 + 2, state_value_col, bat_vol_str.c_str()); - - const int fault_col_1 = state_value_col; - const int fault_col_2 = fault_col_1 + 2; - const int fault_col_3 = fault_col_2 + 2; - - const int sec2 = sec1 + 4; - mvwprintw(system_info_win_, sec2, state_title_col, "System faults"); - - // motor driver over heat; - ShowStatusItemName(sec2 + 1, state_title_col, "-Drv over-heat"); - if ((scout_state_.fault_code & FAULT_MOTOR_DRV_OVERHEAT_W) == 0 && - (scout_state_.fault_code & FAULT_MOTOR_DRV_OVERHEAT_F) == 0) - { - NColors::WSetColor(system_info_win_, NColors::GREEN); - mvwprintw(system_info_win_, sec2 + 1, fault_col_1, "N"); - NColors::WUnsetColor(system_info_win_, NColors::GREEN); - } - else - { - if (scout_state_.fault_code & FAULT_MOTOR_DRV_OVERHEAT_W) - { - NColors::WSetColor(system_info_win_, NColors::YELLOW); - mvwprintw(system_info_win_, sec2 + 1, fault_col_2, "W"); - NColors::WUnsetColor(system_info_win_, NColors::YELLOW); - } - if (scout_state_.fault_code & FAULT_MOTOR_DRV_OVERHEAT_F) - { - NColors::WSetColor(system_info_win_, NColors::RED); - mvwprintw(system_info_win_, sec2 + 1, fault_col_3, "P"); - NColors::WUnsetColor(system_info_win_, NColors::RED); - } - } - - // motor driver over current - ShowStatusItemName(sec2 + 2, state_title_col, "-Mt over-current"); - if ((scout_state_.fault_code & FAULT_MOTOR_OVERCURRENT_W) == 0 && - (scout_state_.fault_code & FAULT_MOTOR_OVERCURRENT_F) == 0) - { - NColors::WSetColor(system_info_win_, NColors::GREEN); - mvwprintw(system_info_win_, sec2 + 2, fault_col_1, "N"); - NColors::WUnsetColor(system_info_win_, NColors::GREEN); - } - else - { - if (scout_state_.fault_code & FAULT_MOTOR_OVERCURRENT_W) - { - NColors::WSetColor(system_info_win_, NColors::YELLOW); - mvwprintw(system_info_win_, sec2 + 2, fault_col_2, "W"); - NColors::WUnsetColor(system_info_win_, NColors::YELLOW); - } - if (scout_state_.fault_code & FAULT_MOTOR_OVERCURRENT_F) - { - NColors::WSetColor(system_info_win_, NColors::RED); - mvwprintw(system_info_win_, sec2 + 2, fault_col_3, "P"); - NColors::WUnsetColor(system_info_win_, NColors::RED); - } - } - - // battery under voltage - ShowStatusItemName(sec2 + 3, state_title_col, "-Bat under volt"); - if ((scout_state_.fault_code & FAULT_BAT_UNDER_VOL_W) == 0 && - (scout_state_.fault_code & FAULT_BAT_UNDER_VOL_F) == 0) - { - NColors::WSetColor(system_info_win_, NColors::GREEN); - mvwprintw(system_info_win_, sec2 + 3, fault_col_1, "N"); - NColors::WUnsetColor(system_info_win_, NColors::GREEN); - } - else - { - if (scout_state_.fault_code & FAULT_BAT_UNDER_VOL_W) - { - NColors::WSetColor(system_info_win_, NColors::YELLOW); - mvwprintw(system_info_win_, sec2 + 3, fault_col_2, "W"); - NColors::WUnsetColor(system_info_win_, NColors::YELLOW); - } - if (scout_state_.fault_code & FAULT_BAT_UNDER_VOL_F) - { - NColors::WSetColor(system_info_win_, NColors::RED); - mvwprintw(system_info_win_, sec2 + 3, fault_col_3, "F"); - NColors::WUnsetColor(system_info_win_, NColors::RED); - } - } - - // battery over voltage - ShowStatusItemName(sec2 + 4, state_title_col, "-Bat over volt"); - ShowFault(sec2 + 4, fault_col_1, (scout_state_.fault_code & FAULT_BAT_OVER_VOL_F) == 0); - - ShowStatusItemName(sec2 + 5, state_title_col, "-RC signal loss"); - ShowFault(sec2 + 5, fault_col_1, (scout_state_.fault_code & FAULT_RC_SIGNAL_LOSS) == 0); - - const int sec3 = sec2 + 7; - mvwprintw(system_info_win_, sec3, state_title_col, "Comm faults"); - - // CAN cmd checksum - ShowStatusItemName(sec3 + 1, state_title_col, "-CAN cmd checksum"); - ShowFault(sec3 + 1, fault_col_1, (scout_state_.fault_code & FAULT_CAN_CHECKSUM_ERROR) == 0); - - // motor comm - ShowStatusItemName(sec3 + 2, state_title_col, "-Motor 1 comm"); - ShowFault(sec3 + 2, fault_col_1, (scout_state_.fault_code & FAULT_MOTOR1_COMM_F) == 0); - - ShowStatusItemName(sec3 + 3, state_title_col, "-Motor 2 comm"); - ShowFault(sec3 + 3, fault_col_1, (scout_state_.fault_code & FAULT_MOTOR2_COMM_F) == 0); - - ShowStatusItemName(sec3 + 4, state_title_col, "-Motor 3 comm"); - ShowFault(sec3 + 4, fault_col_1, (scout_state_.fault_code & FAULT_MOTOR3_COMM_F) == 0); - - ShowStatusItemName(sec3 + 5, state_title_col, "-Motor 4 comm"); - ShowFault(sec3 + 5, fault_col_1, (scout_state_.fault_code & FAULT_MOTOR4_COMM_F) == 0); - - const int sec4 = sec3 + 8; - NColors::WSetColor(system_info_win_, NColors::GREEN); - mvwprintw(system_info_win_, sec4, state_title_col + 1, "N: Normal"); - NColors::WUnsetColor(system_info_win_, NColors::GREEN); - - NColors::WSetColor(system_info_win_, NColors::YELLOW); - mvwprintw(system_info_win_, sec4, state_title_col + 12, "W: Warning"); - NColors::WUnsetColor(system_info_win_, NColors::YELLOW); - - NColors::WSetColor(system_info_win_, NColors::RED); - mvwprintw(system_info_win_, sec4 + 1, state_title_col + 1, "F: Fault P: Protection"); - NColors::WUnsetColor(system_info_win_, NColors::RED); - - wrefresh(system_info_win_); -} - -void ScoutMonitor::ShowStatusItemName(int y, int x, std::string name) -{ - const int state_value_col = x + 20; - const int state_div_col = state_value_col - 2; - - mvwprintw(system_info_win_, y, x, name.c_str()); - mvwprintw(system_info_win_, y, state_div_col, ":"); -} - -void ScoutMonitor::ShowFault(int y, int x, bool no_fault) -{ - const int fault_col_1 = x; - const int fault_col_2 = x + 2; - const int fault_col_3 = fault_col_2 + 2; - - if (no_fault) - { - NColors::WSetColor(system_info_win_, NColors::GREEN); - mvwprintw(system_info_win_, y, fault_col_1, "N"); - NColors::WUnsetColor(system_info_win_, NColors::GREEN); - } - else - { - NColors::WSetColor(system_info_win_, NColors::RED); - mvwprintw(system_info_win_, y, fault_col_3, "F"); - NColors::WUnsetColor(system_info_win_, NColors::RED); - } -} - -// (y,x): position of the top left point of corresponding wheel -void ScoutMonitor::ShowMotorInfo(int y, int x, double cur, int rpm, int temp, bool is_right) -{ - int col_title = x; - if (is_right) - col_title += 6; - else - col_title -= 9; - - std::string cur_str = "CUR:" + ConvertFloatToString(cur, 1); - mvwprintw(body_info_win_, y + 1, col_title, cur_str.c_str()); - - std::string rpm_str = "RPM:" + ConvertFloatToString(rpm, 0); - mvwprintw(body_info_win_, y + 2, col_title, rpm_str.c_str()); - - std::string temp_str = "TMP:" + ConvertFloatToString(temp, 0); - mvwprintw(body_info_win_, y + 3, col_title, temp_str.c_str()); -} -} // namespace westonrobot diff --git a/apps/scout_monitor/tests/CMakeLists.txt b/apps/scout_monitor/tests/CMakeLists.txt deleted file mode 100644 index e178b58..0000000 --- a/apps/scout_monitor/tests/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -# Add executables -add_executable(test_scout_monitor test_scout_monitor.cpp) -target_link_libraries(test_scout_monitor monitor) - -add_executable(test_scout_monitor_virtual test_scout_monitor_virtual.cpp) -target_link_libraries(test_scout_monitor_virtual monitor) - -# add_executable(test_ncurses test_ncurses.cpp) -# target_link_libraries(test_ncurses monitor) - -# add_executable(test_ncolor test_ncolor.c) -# target_link_libraries(test_ncolor monitor) - -# add_executable(test_ncolor2 test_ncolor2.cpp) -# target_link_libraries(test_ncolor2 monitor) \ No newline at end of file diff --git a/apps/scout_monitor/tests/test_ncolor.c b/apps/scout_monitor/tests/test_ncolor.c deleted file mode 100644 index 3c036dd..0000000 --- a/apps/scout_monitor/tests/test_ncolor.c +++ /dev/null @@ -1,158 +0,0 @@ -/* color-demo.c */ -// source: https://www.linuxjournal.com/content/about-ncurses-colors-0 - -#include -#include -#include - -int is_bold(int fg); -void init_colorpairs(void); -short curs_color(int fg); -int colornum(int fg, int bg); -void setcolor(int fg, int bg); -void unsetcolor(int fg, int bg); - -void init_colorpairs(void) -{ - int fg, bg; - int colorpair; - - for (bg = 0; bg <= 7; bg++) - { - for (fg = 0; fg <= 7; fg++) - { - colorpair = colornum(fg, bg); - init_pair(colorpair, curs_color(fg), curs_color(bg)); - } - } -} - -short curs_color(int fg) -{ - switch (7 & fg) - { /* RGB */ - case 0: /* 000 */ - return (COLOR_BLACK); - case 1: /* 001 */ - return (COLOR_BLUE); - case 2: /* 010 */ - return (COLOR_GREEN); - case 3: /* 011 */ - return (COLOR_CYAN); - case 4: /* 100 */ - return (COLOR_RED); - case 5: /* 101 */ - return (COLOR_MAGENTA); - case 6: /* 110 */ - return (COLOR_YELLOW); - case 7: /* 111 */ - return (COLOR_WHITE); - } -} - -int colornum(int fg, int bg) -{ - int B, bbb, ffff; - - B = 1 << 7; - bbb = (7 & bg) << 4; - ffff = 7 & fg; - - return (B | bbb | ffff); -} - -void setcolor(int fg, int bg) -{ - /* set the color pair (colornum) and bold/bright (A_BOLD) */ - - attron(COLOR_PAIR(colornum(fg, bg))); - if (is_bold(fg)) - { - attron(A_BOLD); - } -} - -void unsetcolor(int fg, int bg) -{ - /* unset the color pair (colornum) and - bold/bright (A_BOLD) */ - - attroff(COLOR_PAIR(colornum(fg, bg))); - if (is_bold(fg)) - { - attroff(A_BOLD); - } -} - -int is_bold(int fg) -{ - /* return the intensity bit */ - - int i; - - i = 1 << 3; - return (i & fg); -} - -int main(void) -{ - int fg, bg; - - /* initialize curses */ - - initscr(); - keypad(stdscr, TRUE); - cbreak(); - noecho(); - - /* initialize colors */ - - if (has_colors() == FALSE) - { - endwin(); - puts("Your terminal does not support color"); - exit(1); - } - - start_color(); - init_colorpairs(); - - /* draw test pattern */ - - if ((LINES < 24) || (COLS < 80)) - { - endwin(); - puts("Your terminal needs to be at least 80x24"); - exit(2); - } - - mvaddstr(0, 35, "COLOR DEMO"); - mvaddstr(2, 0, "low intensity text colors (0-7)"); - mvaddstr(12, 0, "high intensity text colors (8-15)"); - - for (bg = 0; bg <= 7; bg++) - { - for (fg = 0; fg <= 7; fg++) - { - setcolor(fg, bg); - mvaddstr(fg + 3, bg * 10, "...test..."); - unsetcolor(fg, bg); - } - - for (fg = 8; fg <= 15; fg++) - { - setcolor(fg, bg); - mvaddstr(fg + 5, bg * 10, "...test..."); - unsetcolor(fg, bg); - } - } - - mvaddstr(LINES - 1, 0, "press any key to quit"); - - refresh(); - - getch(); - endwin(); - - exit(0); -} diff --git a/apps/scout_monitor/tests/test_ncolor2.cpp b/apps/scout_monitor/tests/test_ncolor2.cpp deleted file mode 100644 index 643d045..0000000 --- a/apps/scout_monitor/tests/test_ncolor2.cpp +++ /dev/null @@ -1,65 +0,0 @@ -#include "monitor/ncolors.hpp" - -using namespace westonrobot; - -int main(void) -{ - int fg, bg; - - /* initialize curses */ - - initscr(); - keypad(stdscr, TRUE); - cbreak(); - noecho(); - - /* initialize colors */ - - if (has_colors() == FALSE) - { - endwin(); - puts("Your terminal does not support color"); - return 1; - } - - NColors::InitColors(); - - /* draw test pattern */ - - if ((LINES < 24) || (COLS < 80)) - { - endwin(); - puts("Your terminal needs to be at least 80x24"); - return 2; - } - - mvaddstr(0, 35, "COLOR DEMO"); - mvaddstr(2, 0, "low intensity text colors (0-7)"); - mvaddstr(12, 0, "high intensity text colors (8-15)"); - - for (bg = 0; bg <= 7; bg++) - { - for (fg = 0; fg <= 7; fg++) - { - NColors::SetColor(fg, bg); - mvaddstr(fg + 3, bg * 10, "...test..."); - NColors::UnsetColor(fg, bg); - } - - for (fg = 8; fg <= 15; fg++) - { - NColors::SetColor(fg, bg); - mvaddstr(fg + 5, bg * 10, "...test..."); - NColors::UnsetColor(fg, bg); - } - } - - mvaddstr(LINES - 1, 0, "press any key to quit"); - - refresh(); - - getch(); - endwin(); - - return 0; -} \ No newline at end of file diff --git a/apps/scout_monitor/tests/test_ncurses.cpp b/apps/scout_monitor/tests/test_ncurses.cpp deleted file mode 100644 index 8841639..0000000 --- a/apps/scout_monitor/tests/test_ncurses.cpp +++ /dev/null @@ -1,118 +0,0 @@ -#include -#include - -#define NLINES 10 -#define NCOLS 40 - -void init_wins(WINDOW **wins, int n); -void win_show(WINDOW *win, char *label, int label_color); -void print_in_middle(WINDOW *win, int starty, int startx, int width, char *string, chtype color); - -int main() -{ WINDOW *my_wins[3]; - PANEL *my_panels[3]; - PANEL *top; - int ch; - - /* Initialize curses */ - initscr(); - start_color(); - cbreak(); - noecho(); - keypad(stdscr, TRUE); - - /* Initialize all the colors */ - init_pair(1, COLOR_RED, COLOR_BLACK); - init_pair(2, COLOR_GREEN, COLOR_BLACK); - init_pair(3, COLOR_BLUE, COLOR_BLACK); - init_pair(4, COLOR_CYAN, COLOR_BLACK); - - init_wins(my_wins, 3); - - /* Attach a panel to each window */ /* Order is bottom up */ - my_panels[0] = new_panel(my_wins[0]); /* Push 0, order: stdscr-0 */ - my_panels[1] = new_panel(my_wins[1]); /* Push 1, order: stdscr-0-1 */ - my_panels[2] = new_panel(my_wins[2]); /* Push 2, order: stdscr-0-1-2 */ - - /* Set up the user pointers to the next panel */ - set_panel_userptr(my_panels[0], my_panels[1]); - set_panel_userptr(my_panels[1], my_panels[2]); - set_panel_userptr(my_panels[2], my_panels[0]); - - /* Update the stacking order. 2nd panel will be on top */ - update_panels(); - - /* Show it on the screen */ - attron(COLOR_PAIR(4)); - mvprintw(LINES - 2, 0, "Use tab to browse through the windows (F1 to Exit)"); - attroff(COLOR_PAIR(4)); - doupdate(); - - top = my_panels[2]; - while((ch = getch()) != KEY_F(1)) - { switch(ch) - { case 9: - top = (PANEL *)panel_userptr(top); - top_panel(top); - break; - } - update_panels(); - doupdate(); - } - endwin(); - return 0; -} - -/* Put all the windows */ -void init_wins(WINDOW **wins, int n) -{ int x, y, i; - char label[80]; - - y = 2; - x = 10; - for(i = 0; i < n; ++i) - { wins[i] = newwin(NLINES, NCOLS, y, x); - sprintf(label, "Window Number %d", i + 1); - win_show(wins[i], label, i + 1); - y += 3; - x += 7; - } -} - -/* Show the window with a border and a label */ -void win_show(WINDOW *win, char *label, int label_color) -{ int startx, starty, height, width; - - getbegyx(win, starty, startx); - getmaxyx(win, height, width); - - box(win, 0, 0); - mvwaddch(win, 2, 0, ACS_LTEE); - mvwhline(win, 2, 1, ACS_HLINE, width - 2); - mvwaddch(win, 2, width - 1, ACS_RTEE); - - print_in_middle(win, 1, 0, width, label, COLOR_PAIR(label_color)); -} - -void print_in_middle(WINDOW *win, int starty, int startx, int width, char *string, chtype color) -{ int length, x, y; - float temp; - - if(win == NULL) - win = stdscr; - getyx(win, y, x); - if(startx != 0) - x = startx; - if(starty != 0) - y = starty; - if(width == 0) - width = 80; - - length = strlen(string); - temp = (width - length)/ 2; - x = startx + (int)temp; - wattron(win, color); - mvwprintw(win, y, x, "%s", string); - wattroff(win, color); - refresh(); -} \ No newline at end of file diff --git a/apps/scout_monitor/tests/test_scout_monitor.cpp b/apps/scout_monitor/tests/test_scout_monitor.cpp deleted file mode 100644 index 7747122..0000000 --- a/apps/scout_monitor/tests/test_scout_monitor.cpp +++ /dev/null @@ -1,33 +0,0 @@ -#include -#include -#include -#include - -#include - -#include "monitor/scout_monitor.hpp" - -using namespace westonrobot; - -ScoutMonitor monitor; - -void SignalHandler(int s) -{ - printf("Caught signal %d\n", s); - monitor.Terminate(); - exit(1); -} - -int main(int argc, char **argv) -{ - struct sigaction sigIntHandler; - sigIntHandler.sa_handler = SignalHandler; - sigemptyset(&sigIntHandler.sa_mask); - sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); - - std::cout << "scout monitor started" << std::endl; - monitor.Run("can1"); - - return 0; -} \ No newline at end of file diff --git a/apps/scout_monitor/tests/test_scout_monitor_virtual.cpp b/apps/scout_monitor/tests/test_scout_monitor_virtual.cpp deleted file mode 100644 index 5bc0e07..0000000 --- a/apps/scout_monitor/tests/test_scout_monitor_virtual.cpp +++ /dev/null @@ -1,33 +0,0 @@ -#include -#include -#include -#include - -#include - -#include "monitor/scout_monitor.hpp" - -using namespace westonrobot; - -ScoutMonitor monitor; - -void SignalHandler(int s) -{ - printf("Caught signal %d\n", s); - monitor.Terminate(); - exit(1); -} - -int main(int argc, char **argv) -{ - struct sigaction sigIntHandler; - sigIntHandler.sa_handler = SignalHandler; - sigemptyset(&sigIntHandler.sa_mask); - sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); - - std::cout << "scout monitor started" << std::endl; - monitor.Run(); - - return 0; -} \ No newline at end of file diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt new file mode 100755 index 0000000..6756879 --- /dev/null +++ b/demo/CMakeLists.txt @@ -0,0 +1,11 @@ +# demo +add_subdirectory(scout_demo) + +# add_executable(app_hunter_demo hunter_demo/hunter_demo.cpp) +# target_link_libraries(app_hunter_demo ugv_sdk) + +# add_executable(app_tracer_demo tracer_demo/tracer_demo.cpp) +# target_link_libraries(app_tracer_demo ugv_sdk) + +# add_executable(app_ranger_demo ranger_demo/ranger_demo.cpp) +# target_link_libraries(app_ranger_demo ugv_sdk) diff --git a/apps/hunter_demo/hunter_demo.cpp b/demo/hunter_demo/hunter_demo.cpp similarity index 100% rename from apps/hunter_demo/hunter_demo.cpp rename to demo/hunter_demo/hunter_demo.cpp diff --git a/apps/ranger_demo/ranger_demo.cpp b/demo/ranger_demo/ranger_demo.cpp similarity index 97% rename from apps/ranger_demo/ranger_demo.cpp rename to demo/ranger_demo/ranger_demo.cpp index cf36f51..5a1be9c 100644 --- a/apps/ranger_demo/ranger_demo.cpp +++ b/demo/ranger_demo/ranger_demo.cpp @@ -7,7 +7,7 @@ * Copyright : AgileX Robotics **/ -#include "ugv_sdk/ranger_base.hpp" +#include "ugv_sdk/protocol_v2/ranger_base.hpp" using namespace westonrobot; diff --git a/demo/scout_demo/CMakeLists.txt b/demo/scout_demo/CMakeLists.txt new file mode 100755 index 0000000..a2830d6 --- /dev/null +++ b/demo/scout_demo/CMakeLists.txt @@ -0,0 +1,6 @@ +# tests +add_executable(demo_scout_v2 scout_v2_demo.cpp) +target_link_libraries(demo_scout_v2 ugv_sdk) + +# add_executable(demo_scout_robot scout_robot_demo.cpp) +# target_link_libraries(demo_scout_robot ugv_sdk) \ No newline at end of file diff --git a/demo/scout_demo/scout_robot_demo.cpp b/demo/scout_demo/scout_robot_demo.cpp new file mode 100644 index 0000000..9205cfb --- /dev/null +++ b/demo/scout_demo/scout_robot_demo.cpp @@ -0,0 +1,78 @@ +/* + * scout_robot_demo.cpp + * + * Created on: Jul 08, 2021 11:12 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#include + +#include "ugv_sdk/mobile_base/scout_robot.hpp" + +using namespace westonrobot; + +int main(int argc, char **argv) { + std::string device_name; + + if (argc == 2) { + device_name = {argv[1]}; + std::cout << "Specified CAN: " << device_name << std::endl; + } else { + std::cout << "Usage: app_scout_demo " << std::endl + << "Example 1: ./app_scout_demo can0" << std::endl; + return -1; + } + + ScoutRobot scout(AgilexProtocol::V1); +// scout.Connect(device_name); + +// scout.EnableCommandedMode(); + +// // // light control +// std::cout << "Light: const off" << std::endl; +// scout.SetLightCommand({CONST_OFF, 0, CONST_OFF, 0}); +// sleep(3); +// std::cout << "Light: const on" << std::endl; +// scout.SetLightCommand({CONST_ON, 0, CONST_ON, 0}); +// sleep(3); +// std::cout << "Light: breath" << std::endl; +// scout.SetLightCommand({BREATH, 0, BREATH, 0}); +// sleep(3); +// std::cout << "Light: custom 90-80" << std::endl; +// scout.SetLightCommand({CUSTOM, 90, CUSTOM, 80}); +// sleep(3); +// std::cout << "Light: diabled cmd control" << std::endl; +// scout.DisableLightControl(); + +// int count = 0; +// while (true) { +// // motion control +// if (count < 100) { +// std::cout << "Motor: 0.2, 0" << std::endl; +// scout.SetMotionCommand(0.2, 0.0); +// } + +// // get robot state +// auto state = scout.GetScoutState(); +// std::cout << "-------------------------------" << std::endl; +// std::cout << "count: " << count << std::endl; +// std::cout << "control mode: " +// << static_cast(state.system_state.control_mode) +// << " , vehicle state: " +// << static_cast(state.system_state.vehicle_state) +// << std::endl; +// std::cout << "battery voltage: " << state.system_state.battery_voltage +// << std::endl; +// std::cout << "velocity (linear, angular): " +// << state.motion_state.linear_velocity << ", " +// << state.motion_state.angular_velocity << std::endl; +// std::cout << "-------------------------------" << std::endl; + +// usleep(20000); +// ++count; +// } + + return 0; +} \ No newline at end of file diff --git a/demo/scout_demo/scout_v2_demo.cpp b/demo/scout_demo/scout_v2_demo.cpp new file mode 100644 index 0000000..5e732eb --- /dev/null +++ b/demo/scout_demo/scout_v2_demo.cpp @@ -0,0 +1,75 @@ +/* + * demo_scout_can.cpp + * + * Created on: Jun 12, 2019 05:03 + * Description: + * + * Copyright (c) 2019 Ruixiang Du (rdu) + */ + +#include "ugv_sdk/protocol_v2/scout_base_v2.hpp" + +using namespace westonrobot; + +int main(int argc, char **argv) { + std::string device_name; + + if (argc == 2) { + device_name = {argv[1]}; + std::cout << "Specified CAN: " << device_name << std::endl; + } else { + std::cout << "Usage: app_scout_demo " << std::endl + << "Example 1: ./app_scout_demo can0" << std::endl; + return -1; + } + + ScoutBase scout; + scout.Connect(device_name); + + scout.EnableCommandedMode(); + + // // light control + std::cout << "Light: const off" << std::endl; + scout.SetLightCommand(CONST_OFF, 0, CONST_OFF, 0); + sleep(3); + std::cout << "Light: const on" << std::endl; + scout.SetLightCommand(CONST_ON, 0, CONST_ON, 0); + sleep(3); + std::cout << "Light: breath" << std::endl; + scout.SetLightCommand(BREATH, 0, BREATH, 0); + sleep(3); + std::cout << "Light: custom 90-80" << std::endl; + scout.SetLightCommand(CUSTOM, 90, CUSTOM, 80); + sleep(3); + std::cout << "Light: diabled cmd control" << std::endl; + scout.DisableLightControl(); + + int count = 0; + while (true) { + // motion control + if (count < 500) { + std::cout << "Motor: 0.2, 0" << std::endl; + scout.SetMotionCommand(0.2, 0.0); + } + + auto state = scout.GetRobotState(); + std::cout << "-------------------------------" << std::endl; + std::cout << "count: " << count << std::endl; + std::cout << "control mode: " + << static_cast(state.system_state.control_mode) + << " , vehicle state: " + << static_cast(state.system_state.vehicle_state) + << std::endl; + std::cout << "battery voltage: " << state.system_state.battery_voltage + << std::endl; + std::cout << "velocity (linear, angular): " + << state.motion_state.linear_velocity << ", " + << state.motion_state.angular_velocity << std::endl; + std::cout << "-------------------------------" << std::endl; + + usleep(20000); + ++count; + } + + return 0; +} \ No newline at end of file diff --git a/apps/tracer_demo/tracer_demo.cpp b/demo/tracer_demo/tracer_demo.cpp similarity index 98% rename from apps/tracer_demo/tracer_demo.cpp rename to demo/tracer_demo/tracer_demo.cpp index 8bb5c9a..e7b22cc 100644 --- a/apps/tracer_demo/tracer_demo.cpp +++ b/demo/tracer_demo/tracer_demo.cpp @@ -7,7 +7,7 @@ * Copyright (c) 2019 Ruixiang Du (rdu) */ -#include "ugv_sdk/tracer_base.hpp" +#include "ugv_sdk/protocol_v2/tracer_base.hpp" using namespace westonrobot; diff --git a/include/ugv_sdk/agilex_base.hpp b/include/ugv_sdk/agilex_base.hpp deleted file mode 100644 index 5fec03e..0000000 --- a/include/ugv_sdk/agilex_base.hpp +++ /dev/null @@ -1,104 +0,0 @@ -/* - * agilex_base.hpp - * - * Created on: Dec 22, 2020 17:14 - * Description: - * - * Each robot class derived from this base class should provide implementation - * for the following two functions: - * - * - virtual void Connect(std::string dev_name) = 0; - * - virtual void ParseCANFrame(can_frame *rx_frame) = 0; - * - * Copyright (c) 2020 Ruixiang Du (rdu) - */ - -#ifndef AGILEX_BASE_HPP -#define AGILEX_BASE_HPP - -#include -#include -#include -#include -#include - -#include "ugv_sdk/details/async_port/async_can.hpp" -#include "ugv_sdk/agilex_message.h" - -namespace westonrobot { -class AgilexBase { - public: - AgilexBase() = default; - virtual ~AgilexBase(); - - // do not allow copy or assignment - AgilexBase(const AgilexBase &hunter) = delete; - AgilexBase &operator=(const AgilexBase &hunter) = delete; - - // any derived robot must implement this method with proper call back defined - virtual void Connect(std::string dev_name) = 0; - - // cmd thread runs at 50Hz (20ms) by default - void SetCmdThreadPeriodMs(int32_t period_ms); - - // timeout: robot stops if user does not call SetMotionCommand() periodically - void EnableCmdTimeout(uint32_t timeout_ms = 100); - void DisableTimeout() { enable_timeout_ = false; } - - // switch to commanded mode - void EnableCommandedMode(); - - // enforce 50Hz command loop for all AgileX robots internally - void SetMotionCommand(double linear_vel, double angular_vel, - double lateral_velocity, double steering_angle); - - // one-shot light command - void SendLightCommand(LightMode front_mode, uint8_t front_custom_value, - LightMode rear_mode, uint8_t rear_custom_value); - void DisableLightControl(); - - // motion mode change - void SetMotionMode(uint8_t mode); - - // reset fault states - void ResetRobotState(); - - protected: - std::mutex state_mutex_; - std::mutex motion_cmd_mutex_; - MotionCommandMessage current_motion_cmd_; - - // communication interface - bool can_connected_ = false; - std::shared_ptr can_; - - // timeout to be implemented in each vehicle - bool enable_timeout_ = true; - uint32_t timeout_ms_ = 500; - uint32_t watchdog_counter_ = 0; - void FeedCmdTimeoutWatchdog() { watchdog_counter_ = 0; }; - - // command thread - std::thread cmd_thread_; - int32_t cmd_thread_period_ms_ = 20; - bool cmd_thread_started_ = false; - std::atomic keep_running_; - - // internal functions - void StartCmdThread(); - void ControlLoop(int32_t period_ms); - - // connect to roboot from CAN or serial - using CANFrameRxCallback = AsyncCAN::ReceiveCallback; - void Connect(std::string dev_name, CANFrameRxCallback cb); - void Disconnect(); - - // ask background thread to shutdown properly - void Terminate(); - - void SendRobotCmd(); - virtual void ParseCANFrame(can_frame *rx_frame) = 0; -}; -} // namespace westonrobot - -#endif /* AGILEX_BASE_HPP */ diff --git a/src/stopwatch.hpp b/include/ugv_sdk/details/stopwatch.hpp similarity index 100% rename from src/stopwatch.hpp rename to include/ugv_sdk/details/stopwatch.hpp diff --git a/include/ugv_sdk/agilex_message.h b/include/ugv_sdk/interface/agilex_message.h similarity index 100% rename from include/ugv_sdk/agilex_message.h rename to include/ugv_sdk/interface/agilex_message.h diff --git a/include/ugv_sdk/interface/parser_interface.hpp b/include/ugv_sdk/interface/parser_interface.hpp new file mode 100644 index 0000000..bb4f693 --- /dev/null +++ b/include/ugv_sdk/interface/parser_interface.hpp @@ -0,0 +1,50 @@ +/* + * parser_interface.hpp + * + * Created on: Jul 08, 2021 14:43 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#ifndef PASER_INTERFACE_HPP +#define PASER_INTERFACE_HPP + +#include +#include +#include + +#ifdef __linux__ +#include +#else +struct can_frame { + uint32_t can_id; + uint8_t can_dlc; + uint8_t data[8] __attribute__((aligned(8))); +}; +#endif + +#include "ugv_sdk/interface/agilex_message.h" + +struct ParserInterface { + // CAN support + virtual bool DecodeMessage(const struct can_frame *rx_frame, + AgxMessage *msg) = 0; + virtual void EncodeMessage(const AgxMessage *msg, + struct can_frame *tx_frame) = 0; + virtual uint8_t CalculateChecksum(uint16_t id, uint8_t *data, + uint8_t dlc) = 0; + + // UART support + virtual bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg){ + // throw exception + }; + virtual void EncodeMessage(const AgxMessage *msg, uint8_t *buf, uint8_t *len){ + // throw exception + }; + virtual uint8_t CalculateChecksum(uint8_t *buf, uint8_t len){ + // throw exception + }; +}; + +#endif /* PASER_INTERFACE_HPP */ diff --git a/include/ugv_sdk/interface/robot_interface.hpp b/include/ugv_sdk/interface/robot_interface.hpp new file mode 100644 index 0000000..8b0b1e3 --- /dev/null +++ b/include/ugv_sdk/interface/robot_interface.hpp @@ -0,0 +1,40 @@ +/* + * robot_interface.hpp + * + * Created on: Jul 08, 2021 11:48 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#ifndef ROBOT_INTERFACE_HPP +#define ROBOT_INTERFACE_HPP + +#include + +#include "ugv_sdk/interface/agilex_message.h" + +namespace westonrobot { +struct RobotInterface { + // functions to be implemented by class AgilexBase + virtual void EnableCommandedMode() = 0; + + virtual void SetMotionMode(uint8_t mode){}; + + virtual void SendMotionCommand(double linear_vel, double angular_vel, + double lateral_velocity, + double steering_angle) = 0; + virtual void SendLightCommand(LightMode front_mode, + uint8_t front_custom_value, LightMode rear_mode, + uint8_t rear_custom_value) = 0; + virtual void DisableLightControl() = 0; + + // functions to be implemented by each robot class + virtual void Connect(std::string can_name) = 0; + virtual void Connect(std::string uart_name, uint32_t baudrate) = 0; + + virtual void ResetRobotState() = 0; +}; +} // namespace westonrobot + +#endif /* ROBOT_INTERFACE_HPP */ diff --git a/include/ugv_sdk/interface/scout_interface.hpp b/include/ugv_sdk/interface/scout_interface.hpp index 3ce0d26..b642dda 100644 --- a/include/ugv_sdk/interface/scout_interface.hpp +++ b/include/ugv_sdk/interface/scout_interface.hpp @@ -1,8 +1,8 @@ /* * scout_interface.hpp * - * Created on: Jul 07, 2021 09:10 - * Description: + * Created on: Jul 08, 2021 12:02 + * Description: Scout-specific interface * * Copyright (c) 2021 Weston Robot Pte. Ltd. */ @@ -10,19 +10,33 @@ #ifndef SCOUT_INTERFACE_HPP #define SCOUT_INTERFACE_HPP -namespace westonrobot { -class ScoutInterface { - public: - // set up connection - void Connect(std::string dev_name) override; +#include - // robot control - void SetMotionCommand(double linear_vel, double angular_vel); - void SetLightCommand(const ScoutLightCmd &cmd); - void DisableLightCmdControl(); +#include "ugv_sdk/interface/agilex_message.h" + +namespace westonrobot { +struct ScoutState { + // system state + SystemStateMessage system_state; + MotionStateMessage motion_state; + LightStateMessage light_state; + + RcStateMessage rc_state; + + ActuatorHSStateMessage actuator_hs_state[4]; + ActuatorLSStateMessage actuator_ls_state[4]; + + // sensor data + OdometryMessage odometry; +}; + +struct ScoutInterface { + virtual void SetMotionCommand(double linear_vel, double angular_vel) = 0; + virtual void SetLightCommand(LightMode f_mode, uint8_t f_value, + LightMode r_mode, uint8_t r_value) = 0; // get robot state - ScoutState GetScoutState(); + virtual ScoutState GetRobotState() = 0; }; } // namespace westonrobot diff --git a/include/ugv_sdk/legacy/agilex_base_v1.hpp b/include/ugv_sdk/legacy/agilex_base_v1.hpp deleted file mode 100644 index 5fec03e..0000000 --- a/include/ugv_sdk/legacy/agilex_base_v1.hpp +++ /dev/null @@ -1,104 +0,0 @@ -/* - * agilex_base.hpp - * - * Created on: Dec 22, 2020 17:14 - * Description: - * - * Each robot class derived from this base class should provide implementation - * for the following two functions: - * - * - virtual void Connect(std::string dev_name) = 0; - * - virtual void ParseCANFrame(can_frame *rx_frame) = 0; - * - * Copyright (c) 2020 Ruixiang Du (rdu) - */ - -#ifndef AGILEX_BASE_HPP -#define AGILEX_BASE_HPP - -#include -#include -#include -#include -#include - -#include "ugv_sdk/details/async_port/async_can.hpp" -#include "ugv_sdk/agilex_message.h" - -namespace westonrobot { -class AgilexBase { - public: - AgilexBase() = default; - virtual ~AgilexBase(); - - // do not allow copy or assignment - AgilexBase(const AgilexBase &hunter) = delete; - AgilexBase &operator=(const AgilexBase &hunter) = delete; - - // any derived robot must implement this method with proper call back defined - virtual void Connect(std::string dev_name) = 0; - - // cmd thread runs at 50Hz (20ms) by default - void SetCmdThreadPeriodMs(int32_t period_ms); - - // timeout: robot stops if user does not call SetMotionCommand() periodically - void EnableCmdTimeout(uint32_t timeout_ms = 100); - void DisableTimeout() { enable_timeout_ = false; } - - // switch to commanded mode - void EnableCommandedMode(); - - // enforce 50Hz command loop for all AgileX robots internally - void SetMotionCommand(double linear_vel, double angular_vel, - double lateral_velocity, double steering_angle); - - // one-shot light command - void SendLightCommand(LightMode front_mode, uint8_t front_custom_value, - LightMode rear_mode, uint8_t rear_custom_value); - void DisableLightControl(); - - // motion mode change - void SetMotionMode(uint8_t mode); - - // reset fault states - void ResetRobotState(); - - protected: - std::mutex state_mutex_; - std::mutex motion_cmd_mutex_; - MotionCommandMessage current_motion_cmd_; - - // communication interface - bool can_connected_ = false; - std::shared_ptr can_; - - // timeout to be implemented in each vehicle - bool enable_timeout_ = true; - uint32_t timeout_ms_ = 500; - uint32_t watchdog_counter_ = 0; - void FeedCmdTimeoutWatchdog() { watchdog_counter_ = 0; }; - - // command thread - std::thread cmd_thread_; - int32_t cmd_thread_period_ms_ = 20; - bool cmd_thread_started_ = false; - std::atomic keep_running_; - - // internal functions - void StartCmdThread(); - void ControlLoop(int32_t period_ms); - - // connect to roboot from CAN or serial - using CANFrameRxCallback = AsyncCAN::ReceiveCallback; - void Connect(std::string dev_name, CANFrameRxCallback cb); - void Disconnect(); - - // ask background thread to shutdown properly - void Terminate(); - - void SendRobotCmd(); - virtual void ParseCANFrame(can_frame *rx_frame) = 0; -}; -} // namespace westonrobot - -#endif /* AGILEX_BASE_HPP */ diff --git a/include/ugv_sdk/mobile_base/agilex_base.hpp b/include/ugv_sdk/mobile_base/agilex_base.hpp new file mode 100644 index 0000000..63a8695 --- /dev/null +++ b/include/ugv_sdk/mobile_base/agilex_base.hpp @@ -0,0 +1,151 @@ +/* + * agilex_base.hpp + * + * Created on: Dec 22, 2020 17:14 + * Description: + * + * Each robot class derived from this base class should provide implementation + * for the following two functions: + * + * - virtual void Connect(std::string dev_name) = 0; + * - virtual void ParseCANFrame(can_frame *rx_frame) = 0; + * + * Copyright (c) 2020 Ruixiang Du (rdu) + */ + +#ifndef AGILEX_BASE_HPP +#define AGILEX_BASE_HPP + +#include +#include +#include +#include +#include + +#include "ugv_sdk/details/stopwatch.hpp" +#include "ugv_sdk/details/async_port/async_can.hpp" + +#include "ugv_sdk/interface/robot_interface.hpp" +#include "ugv_sdk/protocol_v2/agilex_msg_parser.h" + +namespace westonrobot { +enum class AgilexProtocol { V1, V2 }; + +template +class AgilexBase : public RobotInterface { + public: + AgilexBase() = default; + virtual ~AgilexBase() { DisconnectPort(); } + + // do not allow copy or assignment + AgilexBase(const AgilexBase &hunter) = delete; + AgilexBase &operator=(const AgilexBase &hunter) = delete; + + // switch to commanded mode + void EnableCommandedMode() { + // construct message + AgxMessage msg; + msg.type = AgxMsgControlModeConfig; + msg.body.control_mode_config_msg.mode = CONTROL_MODE_CAN; + + // encode msg to can frame and send to bus + can_frame frame; + parser_.EncodeMessage(&msg, &frame); + can_->SendFrame(frame); + } + + // must be called at a frequency >= 50Hz + void SendMotionCommand(double linear_vel, double angular_vel, + double lateral_velocity, double steering_angle) { + if (can_connected_) { + // motion control message + AgxMessage msg; + msg.type = AgxMsgMotionCommand; + msg.body.motion_command_msg.linear_velocity = linear_vel; + msg.body.motion_command_msg.angular_velocity = angular_vel; + msg.body.motion_command_msg.lateral_velocity = lateral_velocity; + msg.body.motion_command_msg.steering_angle = steering_angle; + + // send to can bus + can_frame frame; + parser_.EncodeMessage(&msg, &frame); + can_->SendFrame(frame); + } + } + + // one-shot light command + void SendLightCommand(LightMode front_mode, uint8_t front_custom_value, + LightMode rear_mode, uint8_t rear_custom_value) { + if (can_connected_) { + AgxMessage msg; + msg.type = AgxMsgLightCommand; + msg.body.light_command_msg.cmd_ctrl_allowed = true; + msg.body.light_command_msg.front_light.mode = front_mode; + msg.body.light_command_msg.front_light.custom_value = front_custom_value; + msg.body.light_command_msg.rear_light.mode = rear_mode; + msg.body.light_command_msg.rear_light.custom_value = rear_custom_value; + + // send to can bus + can_frame frame; + parser_.EncodeMessage(&msg, &frame); + can_->SendFrame(frame); + } + } + + void DisableLightControl() { + if (can_connected_) { + AgxMessage msg; + msg.type = AgxMsgLightCommand; + + msg.body.light_command_msg.cmd_ctrl_allowed = false; + + // send to can bus + can_frame frame; + parser_.EncodeMessage(&msg, &frame); + can_->SendFrame(frame); + } + } + + // motion mode change + void SetMotionMode(uint8_t mode) { + if (can_connected_) { + AgxMessage msg; + msg.type = AgxMsgSetMotionMode; + msg.body.motion_mode_msg.motion_mode = mode; + + // send to can bus + can_frame frame; + parser_.EncodeMessage(&msg, &frame); + can_->SendFrame(frame); + } + } + + protected: + std::mutex state_mutex_; + // std::mutex motion_cmd_mutex_; + MotionCommandMessage current_motion_cmd_; + + ParserType parser_; + + // communication interface + bool can_connected_ = false; + std::shared_ptr can_; + + // connect to roboot from CAN or serial + using CANFrameRxCallback = AsyncCAN::ReceiveCallback; + void ConnectPort(std::string dev_name, CANFrameRxCallback cb) { + can_ = std::make_shared(dev_name); + can_->SetReceiveCallback(cb); + can_->StartListening(); + can_connected_ = true; + } + + void DisconnectPort() { + if (can_connected_) can_->StopService(); + } + + virtual void ParseCANFrame(can_frame *rx_frame) = 0; +}; +} // namespace westonrobot + +#endif /* AGILEX_BASE_HPP */ diff --git a/include/ugv_sdk/ranger_base.hpp b/include/ugv_sdk/mobile_base/ranger_interface.hpp similarity index 53% rename from include/ugv_sdk/ranger_base.hpp rename to include/ugv_sdk/mobile_base/ranger_interface.hpp index 2df733e..46963c6 100644 --- a/include/ugv_sdk/ranger_base.hpp +++ b/include/ugv_sdk/mobile_base/ranger_interface.hpp @@ -1,21 +1,18 @@ -/** - * @Kit : Qt-Creator: Desktop - * @Author : Wang Zhe - * @Date : 2021-04-19 19:12:52 - * @FileName : ranger_base.hpp - * @Mail : wangzheqie@qq.com - * Copyright : AgileX Robotics - **/ +/* + * ranger_interface.hpp + * + * Created on: Jul 08, 2021 09:40 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ -#ifndef RANGER_BASE_HPP -#define RANGER_BASE_HPP +#ifndef RANGER_INTERFACE_HPP +#define RANGER_INTERFACE_HPP -#include -#include #include -#include -#include "ugv_sdk/agilex_base.hpp" +#include "ugv_sdk/interface/agilex_message.h" namespace westonrobot { struct RangerState { @@ -24,8 +21,6 @@ struct RangerState { MotionStateMessage motion_state; LightStateMessage light_state; - - RcStateMessage rc_state; ActuatorHSStateMessage actuator_hs_state[8]; @@ -60,28 +55,18 @@ struct RangerLightCmd { ///////////////////////////////////////////////////////////////////////// -class RangerBase : public AgilexBase { - public: - RangerBase() : AgilexBase(){}; - ~RangerBase() = default; - - // set up connection - void Connect(std::string dev_name) override; +struct RangerInterface { + virtual void Connect(std::string dev_name) = 0; // robot control - void SetMotionCommand(double linear_vel, double steer_angle, - double lateral_vel = 0.0, double angular_vel = 0.0); - void SetLightCommand(const RangerLightCmd &cmd); - void SetMotionMode(uint8_t mode); + virtual void SetMotionCommand(double linear_vel, double steer_angle, + double lateral_vel, double angular_vel) = 0; + virtual void SetLightCommand(const RangerLightCmd &cmd) = 0; + virtual void SetMotionMode(uint8_t mode) = 0; // get robot state - RangerState GetRangerState(); - - private: - RangerState ranger_state_; - - void ParseCANFrame(can_frame *rx_frame) override; - void UpdateRangerState(const AgxMessage &status_msg, RangerState &state); + virtual RangerState GetRangerState() = 0; }; } // namespace westonrobot -#endif // RANGER_BASE_HPP + +#endif /* RANGER_INTERFACE_HPP */ diff --git a/include/ugv_sdk/mobile_base/scout_robot.hpp b/include/ugv_sdk/mobile_base/scout_robot.hpp new file mode 100644 index 0000000..c0aa1e4 --- /dev/null +++ b/include/ugv_sdk/mobile_base/scout_robot.hpp @@ -0,0 +1,27 @@ +/* + * scout_robot.hpp + * + * Created on: Jul 08, 2021 10:59 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#ifndef SCOUT_ROBOT_HPP +#define SCOUT_ROBOT_HPP + +#include + +#include "ugv_sdk/mobile_base/common.hpp" + +namespace westonrobot { +class ScoutRobot { + public: + ScoutRobot(AgilexProtocol protocol); + + private: + std::unique_ptr robot_; +}; +} // namespace westonrobot + +#endif /* SCOUT_ROBOT_HPP */ diff --git a/include/ugv_sdk/tracer_base.hpp b/include/ugv_sdk/mobile_base/tracer_interface.hpp similarity index 56% rename from include/ugv_sdk/tracer_base.hpp rename to include/ugv_sdk/mobile_base/tracer_interface.hpp index ac60d64..a219e96 100644 --- a/include/ugv_sdk/tracer_base.hpp +++ b/include/ugv_sdk/mobile_base/tracer_interface.hpp @@ -1,21 +1,18 @@ /* - * tracer_base.hpp + * tracer_interface.hpp * - * Created on: Apr 14, 2020 10:21 + * Created on: Jul 08, 2021 09:36 * Description: * - * Copyright (c) 2020 Weston Robot Pte. Ltd. + * Copyright (c) 2021 Weston Robot Pte. Ltd. */ -#ifndef TRACER_BASE_HPP -#define TRACER_BASE_HPP +#ifndef TRACER_INTERFACE_HPP +#define TRACER_INTERFACE_HPP #include -#include -#include -#include -#include "ugv_sdk/agilex_base.hpp" +#include "ugv_sdk/interface/agilex_message.h" namespace westonrobot { struct TracerState { @@ -52,27 +49,18 @@ struct TracerLightCmd { ///////////////////////////////////////////////////////////////////////// -class TracerBase : public AgilexBase { - public: - TracerBase() : AgilexBase(){}; - ~TracerBase() = default; - +struct TracerInterface { // set up connection - void Connect(std::string dev_name) override; + virtual void Connect(std::string can_name) = 0; + virtual void Connect(std::string uart_name, uint32_t baudrate) = 0; // robot control - void SetMotionCommand(double linear_vel, double angular_vel); - void SetLightCommand(const TracerLightCmd &cmd); + virtual void SetMotionCommand(double linear_vel, double angular_vel) = 0; + virtual void SetLightCommand(const TracerLightCmd &cmd) = 0; // get robot state - TracerState GetTracerState(); - - private: - TracerState tracer_state_; - - void ParseCANFrame(can_frame *rx_frame) override; - void UpdateTracerState(const AgxMessage &status_msg, TracerState &state); + virtual TracerState GetTracerState() = 0; }; } // namespace westonrobot -#endif /* TRACER_BASE_HPP */ +#endif /* TRACER_INTERFACE_HPP */ diff --git a/include/ugv_sdk/legacy/scout_can_parser_v1.h b/include/ugv_sdk/protocol_v1/scout_can_parser_v1.h similarity index 93% rename from include/ugv_sdk/legacy/scout_can_parser_v1.h rename to include/ugv_sdk/protocol_v1/scout_can_parser_v1.h index c465652..7d7166d 100644 --- a/include/ugv_sdk/legacy/scout_can_parser_v1.h +++ b/include/ugv_sdk/protocol_v1/scout_can_parser_v1.h @@ -17,7 +17,7 @@ extern "C" { #include #include -#include "ugv_sdk/legacy/scout_protocol_v1.h" +#include "ugv_sdk/protocol_v1/scout_protocol_v1.h" #ifdef __linux__ #include diff --git a/include/ugv_sdk/legacy/scout_protocol_v1.h b/include/ugv_sdk/protocol_v1/scout_protocol_v1.h similarity index 100% rename from include/ugv_sdk/legacy/scout_protocol_v1.h rename to include/ugv_sdk/protocol_v1/scout_protocol_v1.h diff --git a/include/ugv_sdk/details/agilex_msg_parser.h b/include/ugv_sdk/protocol_v2/agilex_msg_parser.h similarity index 94% rename from include/ugv_sdk/details/agilex_msg_parser.h rename to include/ugv_sdk/protocol_v2/agilex_msg_parser.h index 0c12a27..3d91594 100644 --- a/include/ugv_sdk/details/agilex_msg_parser.h +++ b/include/ugv_sdk/protocol_v2/agilex_msg_parser.h @@ -28,7 +28,7 @@ struct can_frame { }; #endif -#include "ugv_sdk/agilex_message.h" +#include "ugv_sdk/interface/agilex_message.h" bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg); void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame); diff --git a/include/ugv_sdk/protocol_v2/protocol_v2_parser.hpp b/include/ugv_sdk/protocol_v2/protocol_v2_parser.hpp new file mode 100644 index 0000000..5f86ed8 --- /dev/null +++ b/include/ugv_sdk/protocol_v2/protocol_v2_parser.hpp @@ -0,0 +1,26 @@ +/* + * protocol_v2_parser.hpp + * + * Created on: Jul 08, 2021 14:51 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#ifndef PROTOCOL_V2_PARSER_HPP +#define PROTOCOL_V2_PARSER_HPP + +#include "ugv_sdk/interface/parser_interface.hpp" + +namespace westonrobot { +class ProtocolV2Parser : public ParserInterface { + public: + bool DecodeMessage(const struct can_frame *rx_frame, + AgxMessage *msg) override; + void EncodeMessage(const AgxMessage *msg, + struct can_frame *tx_frame) override; + uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) override; +}; +} // namespace westonrobot + +#endif /* PROTOCOL_V2_PARSER_HPP */ diff --git a/include/ugv_sdk/protocol_v2/ranger_base.hpp b/include/ugv_sdk/protocol_v2/ranger_base.hpp new file mode 100644 index 0000000..7ae4c74 --- /dev/null +++ b/include/ugv_sdk/protocol_v2/ranger_base.hpp @@ -0,0 +1,46 @@ +/** + * @Kit : Qt-Creator: Desktop + * @Author : Wang Zhe + * @Date : 2021-04-19 19:12:52 + * @FileName : ranger_base.hpp + * @Mail : wangzheqie@qq.com + * Copyright : AgileX Robotics + **/ + +#ifndef RANGER_BASE_HPP +#define RANGER_BASE_HPP + +#include +#include +#include +#include + +#include "ugv_sdk/interface/ranger_interface.hpp" +#include "ugv_sdk/protocol_v2/agilex_base.hpp" + +namespace westonrobot { +class RangerBase : public AgilexBase, public RangerInterface { + public: + RangerBase() : AgilexBase(){}; + ~RangerBase() = default; + + // set up connection + void Connect(std::string dev_name) override; + + // robot control + void SetMotionCommand(double linear_vel, double steer_angle, + double lateral_vel = 0.0, double angular_vel = 0.0); + void SetLightCommand(const RangerLightCmd &cmd); + void SetMotionMode(uint8_t mode); + + // get robot state + RangerState GetRangerState(); + + private: + RangerState ranger_state_; + + void ParseCANFrame(can_frame *rx_frame) override; + void UpdateRangerState(const AgxMessage &status_msg, RangerState &state); +}; +} // namespace westonrobot +#endif // RANGER_BASE_HPP diff --git a/include/ugv_sdk/protocol_v2/scout_base_v2.hpp b/include/ugv_sdk/protocol_v2/scout_base_v2.hpp new file mode 100644 index 0000000..d24650d --- /dev/null +++ b/include/ugv_sdk/protocol_v2/scout_base_v2.hpp @@ -0,0 +1,51 @@ +/* + * scout_base.hpp + * + * Created on: Dec 23, 2020 14:39 + * Description: + * + * Copyright (c) 2020 Ruixiang Du (rdu) + */ + +#ifndef SCOUT_BASE_HPP +#define SCOUT_BASE_HPP + +#include +#include +#include +#include + +#include "ugv_sdk/interface/scout_interface.hpp" + +#include "ugv_sdk/mobile_base/agilex_base.hpp" +#include "ugv_sdk/protocol_v2/protocol_v2_parser.hpp" + +namespace westonrobot { +class ScoutBase : public AgilexBase, public ScoutInterface { + public: + ScoutBase() : AgilexBase(){}; + ~ScoutBase() = default; + + // set up connection + void Connect(std::string can_name) override; + void Connect(std::string uart_name, uint32_t baudrate) override; + + // robot control + void SetMotionCommand(double linear_vel, double angular_vel) override; + void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode, + uint8_t r_value) override; + + // get robot state + ScoutState GetRobotState() override; + + void ResetRobotState() override {} + + private: + ScoutState scout_state_; + + void ParseCANFrame(can_frame *rx_frame) override; + void UpdateScoutState(const AgxMessage &status_msg, ScoutState &state); +}; +} // namespace westonrobot + +#endif /* SCOUT_BASE_HPP */ diff --git a/include/ugv_sdk/protocol_v2/tracer_base.hpp b/include/ugv_sdk/protocol_v2/tracer_base.hpp new file mode 100644 index 0000000..684ccac --- /dev/null +++ b/include/ugv_sdk/protocol_v2/tracer_base.hpp @@ -0,0 +1,46 @@ +/* + * tracer_base.hpp + * + * Created on: Apr 14, 2020 10:21 + * Description: + * + * Copyright (c) 2020 Weston Robot Pte. Ltd. + */ + +#ifndef TRACER_BASE_HPP +#define TRACER_BASE_HPP + +#include +#include +#include +#include + +#include "ugv_sdk/interface/tracer_interface.hpp" +#include "ugv_sdk/protocol_v2/agilex_base.hpp" + +namespace westonrobot { +class TracerBase : public AgilexBase, public TracerInterface { + public: + TracerBase() : AgilexBase(){}; + ~TracerBase() = default; + + // set up connection + void Connect(std::string can_name) override; + void Connect(std::string uart_name, uint32_t baudrate) override; + + // robot control + void SetMotionCommand(double linear_vel, double angular_vel); + void SetLightCommand(const TracerLightCmd &cmd); + + // get robot state + TracerState GetTracerState(); + + private: + TracerState tracer_state_; + + void ParseCANFrame(can_frame *rx_frame) override; + void UpdateTracerState(const AgxMessage &status_msg, TracerState &state); +}; +} // namespace westonrobot + +#endif /* TRACER_BASE_HPP */ diff --git a/include/ugv_sdk/scout_base.hpp b/include/ugv_sdk/scout_base.hpp deleted file mode 100644 index 2ca10ae..0000000 --- a/include/ugv_sdk/scout_base.hpp +++ /dev/null @@ -1,83 +0,0 @@ -/* - * scout_base.hpp - * - * Created on: Dec 23, 2020 14:39 - * Description: - * - * Copyright (c) 2020 Ruixiang Du (rdu) - */ - -#ifndef SCOUT_BASE_HPP -#define SCOUT_BASE_HPP - -#include -#include -#include -#include - -#include "ugv_sdk/agilex_base.hpp" - -namespace westonrobot { -struct ScoutState { - // system state - SystemStateMessage system_state; - MotionStateMessage motion_state; - LightStateMessage light_state; - - RcStateMessage rc_state; - - ActuatorHSStateMessage actuator_hs_state[4]; - ActuatorLSStateMessage actuator_ls_state[4]; - - // sensor data - OdometryMessage odometry; -}; - -struct ScoutMotionCmd { - double linear_velocity; - double angular_velocity; -}; - -struct ScoutLightCmd { - ScoutLightCmd() = default; - ScoutLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, - uint8_t r_value) - : cmd_ctrl_allowed(true), - front_mode(f_mode), - front_custom_value(f_value), - rear_mode(r_mode), - rear_custom_value(r_value) {} - - bool cmd_ctrl_allowed = false; - LightMode front_mode; - uint8_t front_custom_value; - LightMode rear_mode; - uint8_t rear_custom_value; -}; - -///////////////////////////////////////////////////////////////////////// - -class ScoutBase : public AgilexBase { - public: - ScoutBase() : AgilexBase(){}; - ~ScoutBase() = default; - - // set up connection - void Connect(std::string dev_name) override; - - // robot control - void SetMotionCommand(double linear_vel, double angular_vel); - void SetLightCommand(const ScoutLightCmd &cmd); - - // get robot state - ScoutState GetScoutState(); - - private: - ScoutState scout_state_; - - void ParseCANFrame(can_frame *rx_frame) override; - void UpdateScoutState(const AgxMessage &status_msg, ScoutState &state); -}; -} // namespace westonrobot - -#endif /* SCOUT_BASE_HPP */ diff --git a/src/agilex_base.cpp b/src/agilex_base.cpp deleted file mode 100644 index 3898925..0000000 --- a/src/agilex_base.cpp +++ /dev/null @@ -1,170 +0,0 @@ -/* - * agilex_base.cpp - * - * Created on: Dec 22, 2020 17:20 - * Description: - * - * Copyright (c) 2020 Ruixiang Du (rdu) - */ - -#include "ugv_sdk/agilex_base.hpp" - -#include "ugv_sdk/details/agilex_msg_parser.h" -#include "stopwatch.hpp" - -namespace westonrobot { -AgilexBase::~AgilexBase() { - // release resource if occupied - Disconnect(); - - // joint cmd thread - if (cmd_thread_.joinable()) cmd_thread_.join(); -} - -void AgilexBase::Connect(std::string dev_name, CANFrameRxCallback cb) { - can_ = std::make_shared(dev_name); - can_->SetReceiveCallback(cb); - can_->StartListening(); - can_connected_ = true; -} - -void AgilexBase::Disconnect() { - if (can_connected_) can_->StopService(); -} - -void AgilexBase::Terminate() { - keep_running_ = false; - std::terminate(); -} - -void AgilexBase::SetCmdThreadPeriodMs(int32_t period_ms) { - cmd_thread_period_ms_ = period_ms; -}; - -void AgilexBase::EnableCmdTimeout(uint32_t timeout_ms) { - enable_timeout_ = true; - timeout_ms_ = timeout_ms; -} - -void AgilexBase::StartCmdThread() { - keep_running_ = true; - cmd_thread_ = std::thread( - std::bind(&AgilexBase::ControlLoop, this, cmd_thread_period_ms_)); - cmd_thread_started_ = true; -} - -void AgilexBase::ControlLoop(int32_t period_ms) { - uint32_t timeout_iter_num; - - if (enable_timeout_) { - if (timeout_ms_ < period_ms) timeout_ms_ = period_ms; - timeout_iter_num = static_cast(timeout_ms_ / period_ms); - // std::cout << "Timeout iteration number: " << timeout_iter_num << - // std::endl; - } - - Timer tm; - while (keep_running_) { - tm.reset(); - if (enable_timeout_) { - if (watchdog_counter_ < timeout_iter_num) { - SendRobotCmd(); - ++watchdog_counter_; - } - // else { - // std::cout << "Warning: cmd timeout, no cmd sent to robot" << - // std::endl; - // } - } else { - SendRobotCmd(); - } - tm.sleep_until_ms(period_ms); - } -} - -void AgilexBase::EnableCommandedMode() { - // construct message - AgxMessage msg; - msg.type = AgxMsgControlModeConfig; - msg.body.control_mode_config_msg.mode = CONTROL_MODE_CAN; - - // encode msg to can frame and send to bus - can_frame frame; - EncodeCanFrame(&msg, &frame); - can_->SendFrame(frame); -} - -void AgilexBase::SetMotionCommand(double linear_vel, double angular_vel, - double lateral_velocity, - double steering_angle) { - // make sure cmd thread is started before attempting to send commands - if (!cmd_thread_started_) StartCmdThread(); - - std::lock_guard guard(motion_cmd_mutex_); - current_motion_cmd_.linear_velocity = linear_vel; - current_motion_cmd_.angular_velocity = angular_vel; - current_motion_cmd_.lateral_velocity = lateral_velocity; - current_motion_cmd_.steering_angle = steering_angle; - - FeedCmdTimeoutWatchdog(); -} - -void AgilexBase::SendRobotCmd() { - if (can_connected_) { - // motion control message - AgxMessage msg; - msg.type = AgxMsgMotionCommand; - - motion_cmd_mutex_.lock(); - msg.body.motion_command_msg = current_motion_cmd_; - motion_cmd_mutex_.unlock(); - - // send to can bus - can_frame frame; - EncodeCanFrame(&msg, &frame); - can_->SendFrame(frame); - } -} - -void AgilexBase::SendLightCommand(LightMode front_mode, - uint8_t front_custom_value, - LightMode rear_mode, - uint8_t rear_custom_value) { - AgxMessage msg; - msg.type = AgxMsgLightCommand; - - msg.body.light_command_msg.cmd_ctrl_allowed = true; - msg.body.light_command_msg.front_light.mode = front_mode; - msg.body.light_command_msg.front_light.custom_value = front_custom_value; - msg.body.light_command_msg.rear_light.mode = rear_mode; - msg.body.light_command_msg.rear_light.custom_value = rear_custom_value; - - // send to can bus - can_frame frame; - EncodeCanFrame(&msg, &frame); - can_->SendFrame(frame); -} - -void AgilexBase::DisableLightControl() { - AgxMessage msg; - msg.type = AgxMsgLightCommand; - - msg.body.light_command_msg.cmd_ctrl_allowed = false; - - // send to can bus - can_frame frame; - EncodeCanFrame(&msg, &frame); - can_->SendFrame(frame); -} - -void AgilexBase::SetMotionMode(uint8_t mode) { - AgxMessage msg; - msg.type = AgxMsgSetMotionMode; - msg.body.motion_mode_msg.motion_mode = mode; - - // send to can bus - can_frame frame; - EncodeCanFrame(&msg, &frame); - can_->SendFrame(frame); -} -} // namespace westonrobot diff --git a/src/mobile_base/scout_robot.cpp b/src/mobile_base/scout_robot.cpp new file mode 100644 index 0000000..1986467 --- /dev/null +++ b/src/mobile_base/scout_robot.cpp @@ -0,0 +1,14 @@ +/* + * scout_robot.cpp + * + * Created on: Jul 08, 2021 11:13 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#include "ugv_sdk/mobile_base/scout_robot.hpp" + +namespace westonrobot { +ScoutRobot::ScoutRobot(AgilexProtocol protocol) {} +} // namespace westonrobot \ No newline at end of file diff --git a/src/legacy/scout_can_parser_v1.c b/src/protocol_v1/scout_can_parser_v1.c similarity index 99% rename from src/legacy/scout_can_parser_v1.c rename to src/protocol_v1/scout_can_parser_v1.c index 6db95ca..7df8640 100644 --- a/src/legacy/scout_can_parser_v1.c +++ b/src/protocol_v1/scout_can_parser_v1.c @@ -7,7 +7,7 @@ * Copyright (c) 2019 Ruixiang Du (rdu) */ -#include "ugv_sdk/legacy/scout_can_parser_v1.h" +#include "ugv_sdk/protocol_v1/scout_can_parser_v1.h" #include "string.h" diff --git a/src/agx_msg_parser.c b/src/protocol_v2/agx_msg_parser.c similarity index 99% rename from src/agx_msg_parser.c rename to src/protocol_v2/agx_msg_parser.c index a7b5cbe..813c3cb 100644 --- a/src/agx_msg_parser.c +++ b/src/protocol_v2/agx_msg_parser.c @@ -8,7 +8,7 @@ */ #include "agx_protocol_v2.h" -#include "ugv_sdk/details/agilex_msg_parser.h" +#include "ugv_sdk/protocol_v2/agilex_msg_parser.h" #include "stdio.h" #include "string.h" diff --git a/src/agx_protocol_v2.h b/src/protocol_v2/agx_protocol_v2.h similarity index 100% rename from src/agx_protocol_v2.h rename to src/protocol_v2/agx_protocol_v2.h diff --git a/src/protocol_v2/protocol_v2_parser.cpp b/src/protocol_v2/protocol_v2_parser.cpp new file mode 100644 index 0000000..41c7e66 --- /dev/null +++ b/src/protocol_v2/protocol_v2_parser.cpp @@ -0,0 +1,29 @@ +/* + * protocol_v2_parser.cpp + * + * Created on: Jul 08, 2021 14:53 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#include "ugv_sdk/protocol_v2/agilex_msg_parser.h" + +#include "ugv_sdk/protocol_v2/protocol_v2_parser.hpp" + +namespace westonrobot { +bool ProtocolV2Parser::DecodeMessage(const struct can_frame *rx_frame, + AgxMessage *msg) { + return DecodeCanFrame(rx_frame, msg); +} + +void ProtocolV2Parser::EncodeMessage(const AgxMessage *msg, + struct can_frame *tx_frame) { + EncodeCanFrame(msg, tx_frame); +} + +uint8_t ProtocolV2Parser::CalculateChecksum(uint16_t id, uint8_t *data, + uint8_t dlc) { + return CalcCanFrameChecksum(id, data, dlc); +} +} // namespace westonrobot \ No newline at end of file diff --git a/src/ranger_base.cpp b/src/protocol_v2/ranger_base.cpp similarity index 94% rename from src/ranger_base.cpp rename to src/protocol_v2/ranger_base.cpp index 8d19674..173b409 100644 --- a/src/ranger_base.cpp +++ b/src/protocol_v2/ranger_base.cpp @@ -7,7 +7,7 @@ * Copyright : AgileX Robotics **/ -#include "ugv_sdk/ranger_base.hpp" +#include "ugv_sdk/protocol_v2/ranger_base.hpp" #include #include @@ -21,11 +21,11 @@ #include "stopwatch.hpp" -#include "ugv_sdk/details/agilex_msg_parser.h" +#include "ugv_sdk/protocol_v2/agilex_msg_parser.h" namespace westonrobot { void RangerBase::Connect(std::string dev_name) { - AgilexBase::Connect(dev_name, std::bind(&RangerBase::ParseCANFrame, this, + AgilexBase::ConnectPort(dev_name, std::bind(&RangerBase::ParseCANFrame, this, std::placeholders::_1)); } diff --git a/src/scout_base.cpp b/src/protocol_v2/scout_base.cpp similarity index 73% rename from src/scout_base.cpp rename to src/protocol_v2/scout_base.cpp index 4799f26..b7e74ea 100644 --- a/src/scout_base.cpp +++ b/src/protocol_v2/scout_base.cpp @@ -1,4 +1,13 @@ -#include "ugv_sdk/scout_base.hpp" +/* + * scout_base.cpp + * + * Created on: Jul 08, 2021 12:07 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#include "ugv_sdk/protocol_v2/scout_base_v2.hpp" #include #include @@ -10,28 +19,28 @@ #include #include -#include "stopwatch.hpp" - -#include "ugv_sdk/details/agilex_msg_parser.h" +#include "ugv_sdk/protocol_v2/agilex_msg_parser.h" namespace westonrobot { void ScoutBase::Connect(std::string dev_name) { - AgilexBase::Connect(dev_name, std::bind(&ScoutBase::ParseCANFrame, this, - std::placeholders::_1)); + AgilexBase::ConnectPort(dev_name, std::bind(&ScoutBase::ParseCANFrame, this, + std::placeholders::_1)); +} + +void ScoutBase::Connect(std::string uart_name, uint32_t baudrate) { + // TODO } void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel) { - AgilexBase::SetMotionCommand(linear_vel, angular_vel, 0.0, 0.0); + AgilexBase::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0); } -void ScoutBase::SetLightCommand(const ScoutLightCmd &cmd) { - if (cmd.cmd_ctrl_allowed) { - AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value, - LightMode::CONST_OFF, 0); - } +void ScoutBase::SetLightCommand(LightMode f_mode, uint8_t f_value, + LightMode r_mode, uint8_t r_value) { + AgilexBase::SendLightCommand(f_mode, f_value, r_mode, r_value); } -ScoutState ScoutBase::GetScoutState() { +ScoutState ScoutBase::GetRobotState() { std::lock_guard guard(state_mutex_); return scout_state_; } diff --git a/src/tracer_base.cpp b/src/protocol_v2/tracer_base.cpp similarity index 88% rename from src/tracer_base.cpp rename to src/protocol_v2/tracer_base.cpp index 3041d72..d234c2d 100644 --- a/src/tracer_base.cpp +++ b/src/protocol_v2/tracer_base.cpp @@ -1,4 +1,4 @@ -#include "ugv_sdk/tracer_base.hpp" +#include "ugv_sdk/protocol_v2/tracer_base.hpp" #include #include @@ -12,12 +12,16 @@ #include "stopwatch.hpp" -#include "ugv_sdk/details/agilex_msg_parser.h" +#include "ugv_sdk/protocol_v2/agilex_msg_parser.h" namespace westonrobot { void TracerBase::Connect(std::string dev_name) { - AgilexBase::Connect(dev_name, std::bind(&TracerBase::ParseCANFrame, this, - std::placeholders::_1)); + AgilexBase::ConnectPort(dev_name, std::bind(&TracerBase::ParseCANFrame, this, + std::placeholders::_1)); +} + +void TracerBase::Connect(std::string uart_name, uint32_t baudrate) { + // TODO } void TracerBase::SetMotionCommand(double linear_vel, double angular_vel) {