From f74ee40fc38e6c7f1ded71e60b2864baa70022c8 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Thu, 15 Jul 2021 15:25:49 +0800 Subject: [PATCH] added time stamp to robot states --- CMakeLists.txt | 3 + demo/CMakeLists.txt | 2 + demo/bunker_demo/CMakeLists.txt | 1 - demo/hunter_demo/CMakeLists.txt | 1 - demo/hunter_demo/hunter_demo.cpp | 92 ----------------- demo/ranger_demo/CMakeLists.txt | 2 +- ...{ranger_demo.cpp => ranger_robot_demo.cpp} | 0 demo/scout_demo/CMakeLists.txt | 4 - demo/scout_demo/scout_robot_demo.cpp | 57 +++++++---- demo/scout_demo/scout_v2_demo.cpp | 75 -------------- demo/tracer_demo/tracer_demo.cpp | 99 ------------------- demo/utils_demo/CMakeLists.txt | 2 + demo/utils_demo/demo_protocol_detector.cpp | 32 ++++++ .../details/interface/agilex_message.h | 4 +- .../details/interface/bunker_interface.hpp | 3 + .../details/interface/hunter_interface.hpp | 6 +- .../details/interface/ranger_interface.hpp | 5 + .../interface/robot_common_interface.hpp | 8 ++ .../details/interface/scout_interface.hpp | 5 + .../details/interface/tracer_interface.hpp | 5 + .../details/robot_base/agilex_base.hpp | 15 ++- .../details/robot_base/bunker_base.hpp | 1 + .../details/robot_base/hunter_base.hpp | 38 ++++--- .../details/robot_base/ranger_base.hpp | 2 + .../ugv_sdk/details/robot_base/scout_base.hpp | 2 + .../details/robot_base/tracer_base.hpp | 4 +- .../ugv_sdk/utilities/protocol_detector.hpp | 35 +++++++ .../{details => utilities}/stopwatch.hpp | 2 +- src/protocol_v1/agilex_msg_parser_v1.c | 2 +- src/protocol_v2/agilex_msg_parser_v2.c | 10 +- src/protocol_v2/agilex_protocol_v2.h | 4 +- src/utilities/protocol_detector.cpp | 55 +++++++++++ 32 files changed, 248 insertions(+), 328 deletions(-) delete mode 100644 demo/hunter_demo/hunter_demo.cpp rename demo/ranger_demo/{ranger_demo.cpp => ranger_robot_demo.cpp} (100%) delete mode 100644 demo/scout_demo/scout_v2_demo.cpp delete mode 100644 demo/tracer_demo/tracer_demo.cpp create mode 100644 demo/utils_demo/CMakeLists.txt create mode 100644 demo/utils_demo/demo_protocol_detector.cpp create mode 100644 include/ugv_sdk/utilities/protocol_detector.hpp rename include/ugv_sdk/{details => utilities}/stopwatch.hpp (99%) create mode 100644 src/utilities/protocol_detector.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index d60273a..1154df6 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -86,6 +86,9 @@ add_library(${PROJECT_NAME} src/async_port/async_serial.cpp src/async_port/async_can.cpp ######################## + ## utilities + src/utilities/protocol_detector.cpp + ######################## ## public interface to access robot src/mobile_robot/scout_robot.cpp src/mobile_robot/hunter_robot.cpp diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index baa1fd1..764a724 100755 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -4,3 +4,5 @@ add_subdirectory(tracer_demo) add_subdirectory(ranger_demo) add_subdirectory(bunker_demo) add_subdirectory(hunter_demo) + +add_subdirectory(utils_demo) diff --git a/demo/bunker_demo/CMakeLists.txt b/demo/bunker_demo/CMakeLists.txt index de599a2..7134cf0 100755 --- a/demo/bunker_demo/CMakeLists.txt +++ b/demo/bunker_demo/CMakeLists.txt @@ -1,3 +1,2 @@ -# tests add_executable(demo_bunker_robot bunker_robot_demo.cpp) target_link_libraries(demo_bunker_robot ugv_sdk) \ No newline at end of file diff --git a/demo/hunter_demo/CMakeLists.txt b/demo/hunter_demo/CMakeLists.txt index f176db6..f120129 100755 --- a/demo/hunter_demo/CMakeLists.txt +++ b/demo/hunter_demo/CMakeLists.txt @@ -1,3 +1,2 @@ -# tests add_executable(demo_hunter_robot hunter_robot_demo.cpp) target_link_libraries(demo_hunter_robot ugv_sdk) \ No newline at end of file diff --git a/demo/hunter_demo/hunter_demo.cpp b/demo/hunter_demo/hunter_demo.cpp deleted file mode 100644 index bd8d1c7..0000000 --- a/demo/hunter_demo/hunter_demo.cpp +++ /dev/null @@ -1,92 +0,0 @@ -/* - * demo_hunter_can.cpp - * - * Created on: Jun 12, 2019 05:03 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#include "ugv_sdk/hunter_base.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 - { - std::cout << "Usage: app_hunter_demo " << std::endl - << "Example 1: ./app_hunter_demo can0" << std::endl; - return -1; - } - - HunterBase hunter; - hunter.Connect(device_name); - - int count = 0; - while (true) - { - // motion control - if (count < 5) - { - std::cout << "Motor: 0.2, 0.0" << std::endl; - hunter.SetMotionCommand(0.2, 0.0); - } - else if (count < 10) - { - std::cout << "Motor: 0.8, 0.3" << std::endl; - hunter.SetMotionCommand(0.8, 0.3); - } - else if (count < 15) - { - std::cout << "Motor: 1.5, 0.5" << std::endl; - hunter.SetMotionCommand(1.5, 0.5); - } - else if (count < 20) - { - std::cout << "Motor: 1.0, 0.3" << std::endl; - hunter.SetMotionCommand(1.0, 0.3); - } - else if (count < 25) - { - std::cout << "Motor: 0.0, 0.0" << std::endl; - hunter.SetMotionCommand(0.0, 0.0); - } - else if (count < 30) - { - std::cout << "Motor: -0.5, -0.3" << std::endl; - hunter.SetMotionCommand(-0.5, -0.3); - } - else if (count < 35) - { - std::cout << "Motor: -1.0, -0.5" << std::endl; - hunter.SetMotionCommand(-1.0, -0.5); - } - else if (count < 40) - { - std::cout << "Motor: 0.0, 0.0," << std::endl; - hunter.SetMotionCommand(0.0, 0.0); - } - - auto state = hunter.GetHunterState(); - std::cout << "-------------------------------" << std::endl; - std::cout << "count: " << count << std::endl; - std::cout << "control mode: " << static_cast(state.control_mode) << " , base state: " << static_cast(state.base_state) << std::endl; - std::cout << "battery voltage: " << state.battery_voltage << std::endl; - std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " << state.steering_angle << std::endl; - std::cout << "-------------------------------" << std::endl; - - sleep(1); - ++count; - } - - return 0; -} \ No newline at end of file diff --git a/demo/ranger_demo/CMakeLists.txt b/demo/ranger_demo/CMakeLists.txt index c2ba6c7..9a40c0c 100644 --- a/demo/ranger_demo/CMakeLists.txt +++ b/demo/ranger_demo/CMakeLists.txt @@ -1,2 +1,2 @@ -add_executable(demo_ranger_robot ranger_demo.cpp) +add_executable(demo_ranger_robot ranger_robot_demo.cpp) target_link_libraries(demo_ranger_robot ugv_sdk) \ No newline at end of file diff --git a/demo/ranger_demo/ranger_demo.cpp b/demo/ranger_demo/ranger_robot_demo.cpp similarity index 100% rename from demo/ranger_demo/ranger_demo.cpp rename to demo/ranger_demo/ranger_robot_demo.cpp diff --git a/demo/scout_demo/CMakeLists.txt b/demo/scout_demo/CMakeLists.txt index e21fdf4..b553a56 100755 --- a/demo/scout_demo/CMakeLists.txt +++ b/demo/scout_demo/CMakeLists.txt @@ -1,6 +1,2 @@ -# 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 index dac42fe..aedd5fa 100644 --- a/demo/scout_demo/scout_robot_demo.cpp +++ b/demo/scout_demo/scout_robot_demo.cpp @@ -13,34 +13,38 @@ #include #include "ugv_sdk/mobile_robot/scout_robot.hpp" +#include "ugv_sdk/utilities/protocol_detector.hpp" using namespace westonrobot; int main(int argc, char **argv) { - std::string protocol_version; std::string device_name; - if (argc == 3) { - protocol_version = {argv[1]}; - device_name = {argv[2]}; - std::cout << "Use protocol " << protocol_version << " on interface " - << device_name << std::endl; + if (argc == 2) { + device_name = {argv[1]}; + std::cout << "Selected interface " << device_name << std::endl; } else { std::cout << "Usage: app_scout_demo " << std::endl - << "Example 1: ./app_scout_demo v1 can0" << std::endl; + << "Example 1: ./app_scout_demo can0" << std::endl; return -1; } std::unique_ptr scout; - if (protocol_version == "v1") { + + ProtocolDectctor detector; + detector.Connect("can0"); + auto proto = detector.DetectProtocolVersion(5); + if (proto == ProtocolVersion::AGX_V1) { + std::cout << "Detected protocol: AGX_V1" << std::endl; scout = std::unique_ptr( new ScoutRobot(ProtocolVersion::AGX_V1, true)); - } else if (protocol_version == "v2") { + } else if (proto == ProtocolVersion::AGX_V2) { + std::cout << "Detected protocol: AGX_V2" << std::endl; scout = std::unique_ptr( new ScoutRobot(ProtocolVersion::AGX_V2, true)); } else { - std::cout << "Error: invalid protocol version string" << std::endl; + std::cout << "Detected protocol: UNKONWN" << std::endl; return -1; } @@ -51,9 +55,6 @@ int main(int argc, char **argv) { if (scout->GetParserProtocolVersion() == ProtocolVersion::AGX_V2) { scout->EnableCommandedMode(); - std::cout << "Protocol version 2" << std::endl; - } else { - std::cout << "Protocol version 1" << std::endl; } // light control @@ -66,11 +67,9 @@ int main(int argc, char **argv) { std::cout << "Light: breath" << std::endl; scout->SetLightCommand(BREATH, 0, BREATH, 0); sleep(3); - std::cout << "Light: custom 30-80" << std::endl; - scout->SetLightCommand(CUSTOM, 30, CUSTOM, 80); + std::cout << "Light: custom 30-40" << std::endl; + scout->SetLightCommand(CUSTOM, 30, CUSTOM, 40); sleep(3); - // std::cout << "Light: diabled cmd control" << std::endl; - // scout->DisableLightControl(); scout->SetLightCommand(CONST_OFF, 0, CONST_OFF, 0); int count = 0; @@ -88,11 +87,17 @@ int main(int argc, char **argv) { << " , vehicle state: " << static_cast(state.system_state.vehicle_state) << " , error code: " << std::hex << state.system_state.error_code + << std::dec << ", 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 << "core state age (ms): " + << std::chrono::duration_cast( + AgxMsgRefClock::now() - state.time_stamp) + .count() + << std::endl; auto actuator = scout->GetActuatorState(); if (scout->GetParserProtocolVersion() == ProtocolVersion::AGX_V1) { @@ -104,7 +109,25 @@ int main(int argc, char **argv) { actuator.actuator_state[i].driver_temp, actuator.actuator_state[i].motor_temp); } + std::cout << "actuator state age (ms): " + << std::chrono::duration_cast( + AgxMsgRefClock::now() - actuator.time_stamp) + .count() + << std::endl; } else { + for (int i = 0; i < 4; ++i) { + printf("motor %d: current %f, rpm %d, driver temp %f, motor temp %f\n", + actuator.actuator_hs_state[i].motor_id, + actuator.actuator_hs_state[i].current, + actuator.actuator_hs_state[i].rpm, + actuator.actuator_ls_state[i].driver_temp, + actuator.actuator_ls_state[i].motor_temp); + } + std::cout << "actuator state age (ms): " + << std::chrono::duration_cast( + AgxMsgRefClock::now() - actuator.time_stamp) + .count() + << std::endl; } std::cout << "-------------------------------" << std::endl; diff --git a/demo/scout_demo/scout_v2_demo.cpp b/demo/scout_demo/scout_v2_demo.cpp deleted file mode 100644 index 92bfe62..0000000 --- a/demo/scout_demo/scout_v2_demo.cpp +++ /dev/null @@ -1,75 +0,0 @@ -/* - * demo_scout_can.cpp - * - * Created on: Jun 12, 2019 05:03 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#include "ugv_sdk/details/robot_base/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; - } - - ScoutBaseV2 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/demo/tracer_demo/tracer_demo.cpp b/demo/tracer_demo/tracer_demo.cpp deleted file mode 100644 index a738871..0000000 --- a/demo/tracer_demo/tracer_demo.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/* - * demo_tracer_can.cpp - * - * Created on: Jun 12, 2019 05:03 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#include "ugv_sdk/protocol_v2/tracer_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_tracer_demo " << std::endl - << "Example 1: ./app_tracer_demo can0" << std::endl; - return -1; - } - - TracerBaseV2 tracer; - tracer.Connect(device_name); - - tracer.EnableCommandedMode(); - // tracer.DisableTimeout(); - - // light control - std::cout << "Light: const off" << std::endl; - tracer.SetLightCommand({CONST_OFF, 0}); - // usleep(50000); - sleep(3); - std::cout << "Light: const on" << std::endl; - tracer.SetLightCommand({CONST_ON, 0}); - // usleep(50000); - sleep(3); - std::cout << "Light: breath" << std::endl; - tracer.SetLightCommand({BREATH, 0}); - // usleep(50000); - sleep(8); - std::cout << "Light: custom 90-80" << std::endl; - tracer.SetLightCommand({CUSTOM, 90}); - // usleep(50000); - sleep(3); - std::cout << "Light: diabled cmd control" << std::endl; - tracer.SetLightCommand(TracerLightCmd()); - - int count = 0; - while (true) { - // motion control - if (count < 5) { - std::cout << "Motor: 0.2, 0.0" << std::endl; - tracer.SetMotionCommand(0.2, 0.0); - } else if (count < 10) { - std::cout << "Motor: 0.8, 0.3" << std::endl; - tracer.SetMotionCommand(0.8, 0.3); - } else if (count < 15) { - std::cout << "Motor: 1.5, 0.5" << std::endl; - tracer.SetMotionCommand(1.5, 0.5); - } else if (count < 20) { - std::cout << "Motor: 1.0, 0.3" << std::endl; - tracer.SetMotionCommand(1.0, 0.3); - } else if (count < 25) { - std::cout << "Motor: 0.0, 0.0" << std::endl; - tracer.SetMotionCommand(0.0, 0.0); - } else if (count < 30) { - std::cout << "Motor: -0.5, -0.3" << std::endl; - tracer.SetMotionCommand(-0.5, -0.3); - } else if (count < 35) { - std::cout << "Motor: -1.0, -0.5" << std::endl; - tracer.SetMotionCommand(-1.0, -0.5); - } else if (count < 40) { - std::cout << "Motor: 0.0, 0.0," << std::endl; - tracer.SetMotionCommand(0.0, 0.0); - } - // tracer.SetMotionCommand(0.8, 0.8); - - auto state = tracer.GetTracerState(); - 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); - sleep(1); - ++count; - } - - return 0; -} \ No newline at end of file diff --git a/demo/utils_demo/CMakeLists.txt b/demo/utils_demo/CMakeLists.txt new file mode 100644 index 0000000..5a49410 --- /dev/null +++ b/demo/utils_demo/CMakeLists.txt @@ -0,0 +1,2 @@ +add_executable(demo_protocol_detector demo_protocol_detector.cpp) +target_link_libraries(demo_protocol_detector ugv_sdk) \ No newline at end of file diff --git a/demo/utils_demo/demo_protocol_detector.cpp b/demo/utils_demo/demo_protocol_detector.cpp new file mode 100644 index 0000000..7765ddc --- /dev/null +++ b/demo/utils_demo/demo_protocol_detector.cpp @@ -0,0 +1,32 @@ +/* + * demo_protocol_detect.cpp + * + * Created on: Jul 15, 2021 14:10 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#include +#include + +#include "ugv_sdk/utilities/protocol_detector.hpp" + +using namespace westonrobot; + +int main(int argc, char **argv) { + ProtocolDectctor detector; + detector.Connect("can0"); + + auto proto = detector.DetectProtocolVersion(5); + + if (proto == ProtocolVersion::AGX_V1) { + std::cout << "Detected protocol: AGX_V1" << std::endl; + } else if (proto == ProtocolVersion::AGX_V2) { + std::cout << "Detected protocol: AGX_V2" << std::endl; + } else { + std::cout << "Detected protocol: UNKONWN" << std::endl; + } + + return 0; +} \ No newline at end of file diff --git a/include/ugv_sdk/details/interface/agilex_message.h b/include/ugv_sdk/details/interface/agilex_message.h index b9def6c..f621c45 100644 --- a/include/ugv_sdk/details/interface/agilex_message.h +++ b/include/ugv_sdk/details/interface/agilex_message.h @@ -119,8 +119,8 @@ typedef struct { typedef struct { uint8_t motor_id; float driver_voltage; - float driver_temperature; - int8_t motor_temperature; + float driver_temp; + float motor_temp; uint8_t driver_state; } ActuatorLSStateMessage; diff --git a/include/ugv_sdk/details/interface/bunker_interface.hpp b/include/ugv_sdk/details/interface/bunker_interface.hpp index d6b0d81..57c12d5 100644 --- a/include/ugv_sdk/details/interface/bunker_interface.hpp +++ b/include/ugv_sdk/details/interface/bunker_interface.hpp @@ -13,9 +13,12 @@ #include #include "ugv_sdk/details/interface/agilex_message.h" +#include "ugv_sdk/details/interface/robot_common_interface.hpp" namespace westonrobot { struct BunkerCoreState { + AgxMsgTimeStamp time_stamp; + SystemStateMessage system_state; MotionStateMessage motion_state; RcStateMessage rc_state; diff --git a/include/ugv_sdk/details/interface/hunter_interface.hpp b/include/ugv_sdk/details/interface/hunter_interface.hpp index feaaf54..a07ce2c 100644 --- a/include/ugv_sdk/details/interface/hunter_interface.hpp +++ b/include/ugv_sdk/details/interface/hunter_interface.hpp @@ -13,16 +13,20 @@ #include #include "ugv_sdk/details/interface/agilex_message.h" +#include "ugv_sdk/details/interface/robot_common_interface.hpp" namespace westonrobot { struct HunterCoreState { + AgxMsgTimeStamp time_stamp; + SystemStateMessage system_state; MotionStateMessage motion_state; - LightStateMessage light_state; RcStateMessage rc_state; }; struct HunterActuatorState { + AgxMsgTimeStamp time_stamp; + // actuator state ActuatorHSStateMessage actuator_hs_state[3]; ActuatorLSStateMessage actuator_ls_state[3]; diff --git a/include/ugv_sdk/details/interface/ranger_interface.hpp b/include/ugv_sdk/details/interface/ranger_interface.hpp index 60ad026..770e2c8 100644 --- a/include/ugv_sdk/details/interface/ranger_interface.hpp +++ b/include/ugv_sdk/details/interface/ranger_interface.hpp @@ -13,10 +13,13 @@ #include #include "ugv_sdk/details/interface/agilex_message.h" +#include "ugv_sdk/details/interface/robot_common_interface.hpp" namespace westonrobot { struct RangerCoreState { // system state + AgxMsgTimeStamp time_stamp; + SystemStateMessage system_state; MotionStateMessage motion_state; LightStateMessage light_state; @@ -27,6 +30,8 @@ struct RangerCoreState { }; struct RangerActuatorState { + AgxMsgTimeStamp time_stamp; + ActuatorHSStateMessage actuator_hs_state[8]; ActuatorLSStateMessage actuator_ls_state[8]; }; diff --git a/include/ugv_sdk/details/interface/robot_common_interface.hpp b/include/ugv_sdk/details/interface/robot_common_interface.hpp index b36b874..c16d1bf 100644 --- a/include/ugv_sdk/details/interface/robot_common_interface.hpp +++ b/include/ugv_sdk/details/interface/robot_common_interface.hpp @@ -11,6 +11,7 @@ #define ROBOT_INTERFACE_HPP #include +#include #include "ugv_sdk/details/interface/agilex_message.h" #include "ugv_sdk/details/interface/parser_interface.hpp" @@ -18,7 +19,12 @@ #define AGX_MAX_ACTUATOR_NUM 8 namespace westonrobot { +using AgxMsgRefClock = std::chrono::steady_clock; +using AgxMsgTimeStamp = std::chrono::time_point; + struct CoreStateMsgGroup { + AgxMsgTimeStamp time_stamp; + SystemStateMessage system_state; MotionStateMessage motion_state; LightStateMessage light_state; @@ -27,6 +33,8 @@ struct CoreStateMsgGroup { }; struct ActuatorStateMsgGroup { + AgxMsgTimeStamp time_stamp; + ActuatorHSStateMessage actuator_hs_state[AGX_MAX_ACTUATOR_NUM]; // v2 only ActuatorLSStateMessage actuator_ls_state[AGX_MAX_ACTUATOR_NUM]; // v2 only ActuatorStateMessageV1 actuator_state[AGX_MAX_ACTUATOR_NUM]; // v1 only diff --git a/include/ugv_sdk/details/interface/scout_interface.hpp b/include/ugv_sdk/details/interface/scout_interface.hpp index 9e2bac2..af20fbd 100644 --- a/include/ugv_sdk/details/interface/scout_interface.hpp +++ b/include/ugv_sdk/details/interface/scout_interface.hpp @@ -13,9 +13,12 @@ #include #include "ugv_sdk/details/interface/agilex_message.h" +#include "ugv_sdk/details/interface/robot_common_interface.hpp" namespace westonrobot { struct ScoutCoreState { + AgxMsgTimeStamp time_stamp; + SystemStateMessage system_state; MotionStateMessage motion_state; LightStateMessage light_state; @@ -23,6 +26,8 @@ struct ScoutCoreState { }; struct ScoutActuatorState { + AgxMsgTimeStamp time_stamp; + // actuator state // - for v2 robots only ActuatorHSStateMessage actuator_hs_state[4]; diff --git a/include/ugv_sdk/details/interface/tracer_interface.hpp b/include/ugv_sdk/details/interface/tracer_interface.hpp index 4fc4a68..2293ebb 100644 --- a/include/ugv_sdk/details/interface/tracer_interface.hpp +++ b/include/ugv_sdk/details/interface/tracer_interface.hpp @@ -13,9 +13,12 @@ #include #include "ugv_sdk/details/interface/agilex_message.h" +#include "ugv_sdk/details/interface/robot_common_interface.hpp" namespace westonrobot { struct TracerCoreState { + AgxMsgTimeStamp time_stamp; + SystemStateMessage system_state; MotionStateMessage motion_state; LightStateMessage light_state; @@ -23,6 +26,8 @@ struct TracerCoreState { }; struct TracerActuatorState { + AgxMsgTimeStamp time_stamp; + // actuator state ActuatorHSStateMessage actuator_hs_state[2]; ActuatorLSStateMessage actuator_ls_state[2]; diff --git a/include/ugv_sdk/details/robot_base/agilex_base.hpp b/include/ugv_sdk/details/robot_base/agilex_base.hpp index 8941d5c..d0b512b 100644 --- a/include/ugv_sdk/details/robot_base/agilex_base.hpp +++ b/include/ugv_sdk/details/robot_base/agilex_base.hpp @@ -22,7 +22,6 @@ #include #include -#include "ugv_sdk/details/stopwatch.hpp" #include "ugv_sdk/details/async_port/async_can.hpp" #include "ugv_sdk/details/interface/robot_common_interface.hpp" @@ -79,8 +78,9 @@ class AgilexBase : public RobotCommonInterface { msg.body.motion_command_msg.steering_angle = steering_angle; } - std::cout << "sending motion cmd: " << linear_vel << "," << angular_vel - << std::endl; + // std::cout << "sending motion cmd: " << linear_vel << "," << + // angular_vel + // << std::endl; // send to can bus can_frame frame; @@ -195,27 +195,32 @@ class AgilexBase : public RobotCommonInterface { switch (status_msg.type) { case AgxMsgSystemState: { // std::cout << "system status feedback received" << std::endl; + core_state_msgs_.time_stamp = AgxMsgRefClock::now(); core_state_msgs_.system_state = status_msg.body.system_state_msg; break; } case AgxMsgMotionState: { // std::cout << "motion control feedback received" << std::endl; + core_state_msgs_.time_stamp = AgxMsgRefClock::now(); core_state_msgs_.motion_state = status_msg.body.motion_state_msg; break; } case AgxMsgLightState: { // std::cout << "light control feedback received" << std::endl; + core_state_msgs_.time_stamp = AgxMsgRefClock::now(); core_state_msgs_.light_state = status_msg.body.light_state_msg; break; } case AgxMsgMotionModeState: { // std::cout << "motion mode feedback received" << std::endl; + core_state_msgs_.time_stamp = AgxMsgRefClock::now(); core_state_msgs_.motion_mode_state = status_msg.body.motion_mode_state_msg; break; } case AgxMsgRcState: { // std::cout << "rc feedback received" << std::endl; + core_state_msgs_.time_stamp = AgxMsgRefClock::now(); core_state_msgs_.rc_state = status_msg.body.rc_state_msg; break; } @@ -229,6 +234,7 @@ class AgilexBase : public RobotCommonInterface { switch (status_msg.type) { case AgxMsgActuatorHSState: { // std::cout << "actuator hs feedback received" << std::endl; + actuator_state_msgs_.time_stamp = AgxMsgRefClock::now(); actuator_state_msgs_ .actuator_hs_state[status_msg.body.actuator_hs_state_msg.motor_id] = status_msg.body.actuator_hs_state_msg; @@ -236,7 +242,7 @@ class AgilexBase : public RobotCommonInterface { } case AgxMsgActuatorLSState: { // std::cout << "actuator ls feedback received" << std::endl; - + actuator_state_msgs_.time_stamp = AgxMsgRefClock::now(); actuator_state_msgs_ .actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] = status_msg.body.actuator_ls_state_msg; @@ -244,6 +250,7 @@ class AgilexBase : public RobotCommonInterface { } case AgxMsgActuatorStateV1: { // std::cout << "actuator v1 feedback received" << std::endl; + actuator_state_msgs_.time_stamp = AgxMsgRefClock::now(); actuator_state_msgs_ .actuator_state[status_msg.body.v1_actuator_state_msg.motor_id] = status_msg.body.v1_actuator_state_msg; diff --git a/include/ugv_sdk/details/robot_base/bunker_base.hpp b/include/ugv_sdk/details/robot_base/bunker_base.hpp index d424151..16427c3 100644 --- a/include/ugv_sdk/details/robot_base/bunker_base.hpp +++ b/include/ugv_sdk/details/robot_base/bunker_base.hpp @@ -41,6 +41,7 @@ class BunkerBase : public AgilexBase, public BunkerInterface { auto state = AgilexBase::GetRobotCoreStateMsgGroup(); BunkerCoreState bunker_state; + bunker_state.time_stamp = state.time_stamp; bunker_state.system_state = state.system_state; bunker_state.motion_state = state.motion_state; bunker_state.rc_state = state.rc_state; diff --git a/include/ugv_sdk/details/robot_base/hunter_base.hpp b/include/ugv_sdk/details/robot_base/hunter_base.hpp index f9cd255..2f3fce7 100644 --- a/include/ugv_sdk/details/robot_base/hunter_base.hpp +++ b/include/ugv_sdk/details/robot_base/hunter_base.hpp @@ -15,12 +15,12 @@ #include #include -#include "ugv_sdk/details/interface/scout_interface.hpp" +#include "ugv_sdk/details/interface/hunter_interface.hpp" #include "ugv_sdk/details/robot_base/agilex_base.hpp" namespace westonrobot { template -class HunterBase : public AgilexBase, public ScoutInterface { +class HunterBase : public AgilexBase, public HunterInterface { public: HunterBase() : AgilexBase(){}; ~HunterBase() = default; @@ -36,33 +36,29 @@ class HunterBase : public AgilexBase, public ScoutInterface { angular_vel); } - void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode, - uint8_t r_value) override { - AgilexBase::SendLightCommand(f_mode, f_value, r_mode, r_value); - } - // get robot state - ScoutCoreState GetRobotState() override { + HunterCoreState GetRobotState() override { auto state = AgilexBase::GetRobotCoreStateMsgGroup(); - ScoutCoreState scout_state; - scout_state.system_state = state.system_state; - scout_state.motion_state = state.motion_state; - scout_state.light_state = state.light_state; - scout_state.rc_state = state.rc_state; - return scout_state; + HunterCoreState hunter_state; + hunter_state.time_stamp = state.time_stamp; + hunter_state.system_state = state.system_state; + hunter_state.motion_state = state.motion_state; + hunter_state.rc_state = state.rc_state; + return hunter_state; } - ScoutActuatorState GetActuatorState() override { + HunterActuatorState GetActuatorState() override { auto actuator = AgilexBase::GetActuatorStateMsgGroup(); - ScoutActuatorState scout_actuator; - for (int i = 0; i < 4; ++i) { - scout_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i]; - scout_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i]; - scout_actuator.actuator_state[i] = actuator.actuator_state[i]; + HunterActuatorState hunter_actuator; + hunter_actuator.time_stamp = actuator.time_stamp; + for (int i = 0; i < 3; ++i) { + hunter_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i]; + hunter_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i]; + hunter_actuator.actuator_state[i] = actuator.actuator_state[i]; } - return scout_actuator; + return hunter_actuator; } }; } // namespace westonrobot diff --git a/include/ugv_sdk/details/robot_base/ranger_base.hpp b/include/ugv_sdk/details/robot_base/ranger_base.hpp index 8c70e9c..0ae99e1 100644 --- a/include/ugv_sdk/details/robot_base/ranger_base.hpp +++ b/include/ugv_sdk/details/robot_base/ranger_base.hpp @@ -55,6 +55,7 @@ class RangerBase : public AgilexBase, public RangerInterface { auto state = AgilexBase::GetRobotCoreStateMsgGroup(); RangerCoreState ranger_state; + ranger_state.time_stamp = state.time_stamp; ranger_state.system_state = state.system_state; ranger_state.motion_state = state.motion_state; ranger_state.light_state = state.light_state; @@ -66,6 +67,7 @@ class RangerBase : public AgilexBase, public RangerInterface { auto actuator = AgilexBase::GetActuatorStateMsgGroup(); RangerActuatorState ranger_actuator; + ranger_actuator.time_stamp = actuator.time_stamp; for (int i = 0; i < 8; ++i) { ranger_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i]; ranger_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i]; diff --git a/include/ugv_sdk/details/robot_base/scout_base.hpp b/include/ugv_sdk/details/robot_base/scout_base.hpp index 6ceec91..f42d356 100644 --- a/include/ugv_sdk/details/robot_base/scout_base.hpp +++ b/include/ugv_sdk/details/robot_base/scout_base.hpp @@ -50,6 +50,7 @@ class ScoutBase : public AgilexBase, public ScoutInterface { auto state = AgilexBase::GetRobotCoreStateMsgGroup(); ScoutCoreState scout_state; + scout_state.time_stamp = state.time_stamp; scout_state.system_state = state.system_state; scout_state.motion_state = state.motion_state; scout_state.light_state = state.light_state; @@ -61,6 +62,7 @@ class ScoutBase : public AgilexBase, public ScoutInterface { auto actuator = AgilexBase::GetActuatorStateMsgGroup(); ScoutActuatorState scout_actuator; + scout_actuator.time_stamp = actuator.time_stamp; for (int i = 0; i < 4; ++i) { scout_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i]; scout_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i]; diff --git a/include/ugv_sdk/details/robot_base/tracer_base.hpp b/include/ugv_sdk/details/robot_base/tracer_base.hpp index 7dd2221..86f40f5 100644 --- a/include/ugv_sdk/details/robot_base/tracer_base.hpp +++ b/include/ugv_sdk/details/robot_base/tracer_base.hpp @@ -31,7 +31,7 @@ class TracerBaseV2 : public AgilexBase, void Connect(std::string can_name) override { AgilexBase::Connect(can_name); } - + void Connect(std::string uart_name, uint32_t baudrate) override { // TODO } @@ -52,6 +52,7 @@ class TracerBaseV2 : public AgilexBase, auto state = AgilexBase::GetRobotCoreStateMsgGroup(); TracerCoreState tracer_state; + tracer_state.time_stamp = state.time_stamp; tracer_state.system_state = state.system_state; tracer_state.motion_state = state.motion_state; tracer_state.light_state = state.light_state; @@ -63,6 +64,7 @@ class TracerBaseV2 : public AgilexBase, auto actuator = AgilexBase::GetActuatorStateMsgGroup(); TracerActuatorState tracer_actuator; + tracer_actuator.time_stamp = actuator.time_stamp; for (int i = 0; i < 2; ++i) { tracer_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i]; tracer_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i]; diff --git a/include/ugv_sdk/utilities/protocol_detector.hpp b/include/ugv_sdk/utilities/protocol_detector.hpp new file mode 100644 index 0000000..c418b77 --- /dev/null +++ b/include/ugv_sdk/utilities/protocol_detector.hpp @@ -0,0 +1,35 @@ +/* + * protocol_detector.hpp + * + * Created on: Jul 15, 2021 14:03 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#ifndef PROTOCOL_DETECTOR_HPP +#define PROTOCOL_DETECTOR_HPP + +#include + +#include "ugv_sdk/details/async_port/async_can.hpp" +#include "ugv_sdk/details/interface/parser_interface.hpp" + +namespace westonrobot { +class ProtocolDectctor { + public: + void Connect(std::string can_name); + void Connect(std::string uart_name, uint32_t baudrate); + + ProtocolVersion DetectProtocolVersion(uint32_t timeout_sec); + + private: + std::shared_ptr can_; + void ParseCANFrame(can_frame *rx_frame); + + std::atomic msg_v1_detected_; + std::atomic msg_v2_detected_; +}; +} // namespace westonrobot + +#endif /* PROTOCOL_DETECTOR_HPP */ diff --git a/include/ugv_sdk/details/stopwatch.hpp b/include/ugv_sdk/utilities/stopwatch.hpp similarity index 99% rename from include/ugv_sdk/details/stopwatch.hpp rename to include/ugv_sdk/utilities/stopwatch.hpp index acc9924..24ee06f 100644 --- a/include/ugv_sdk/details/stopwatch.hpp +++ b/include/ugv_sdk/utilities/stopwatch.hpp @@ -9,7 +9,7 @@ * [2] https://github.com/rxdu/stopwatch * * Copyright (c) 2019 sailormoon - * Copyright (c) 2020 Ruixiang Du (rdu) + * Copyright (c) 2020 Weston Robot Pte. Ltd. * * License: */ diff --git a/src/protocol_v1/agilex_msg_parser_v1.c b/src/protocol_v1/agilex_msg_parser_v1.c index 79cd375..24abdcd 100644 --- a/src/protocol_v1/agilex_msg_parser_v1.c +++ b/src/protocol_v1/agilex_msg_parser_v1.c @@ -18,7 +18,7 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) { // if checksum not correct if (!CalcCanFrameChecksumV1(rx_frame->can_id, (uint8_t *)rx_frame->data, rx_frame->can_dlc)) { - printf("wrong checksum for id: %x-------------->\n", rx_frame->can_id); + // printf("wrong checksum for id: %x-------------->\n", rx_frame->can_id); return false; } diff --git a/src/protocol_v2/agilex_msg_parser_v2.c b/src/protocol_v2/agilex_msg_parser_v2.c index da07e29..f8fff49 100644 --- a/src/protocol_v2/agilex_msg_parser_v2.c +++ b/src/protocol_v2/agilex_msg_parser_v2.c @@ -182,11 +182,11 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) { ((uint16_t)(frame->driver_voltage.low_byte) | (uint16_t)(frame->driver_voltage.high_byte) << 8) * 0.1; - msg->body.actuator_ls_state_msg.driver_temperature = - (int16_t)((uint16_t)(frame->driver_temperature.low_byte) | - (uint16_t)(frame->driver_temperature.high_byte) << 8); - msg->body.actuator_ls_state_msg.motor_temperature = - frame->motor_temperature; + msg->body.actuator_ls_state_msg.driver_temp = + (int16_t)((uint16_t)(frame->driver_temp.low_byte) | + (uint16_t)(frame->driver_temp.high_byte) << 8); + msg->body.actuator_ls_state_msg.motor_temp = + frame->motor_temp; msg->body.actuator_ls_state_msg.driver_state = frame->driver_state; break; } diff --git a/src/protocol_v2/agilex_protocol_v2.h b/src/protocol_v2/agilex_protocol_v2.h index 7cc2f28..8cfe8b9 100644 --- a/src/protocol_v2/agilex_protocol_v2.h +++ b/src/protocol_v2/agilex_protocol_v2.h @@ -242,8 +242,8 @@ typedef struct { typedef struct { struct16_t driver_voltage; - struct16_t driver_temperature; - int8_t motor_temperature; + struct16_t driver_temp; + int8_t motor_temp; uint8_t driver_state; uint8_t reserved0; uint8_t reserved1; diff --git a/src/utilities/protocol_detector.cpp b/src/utilities/protocol_detector.cpp new file mode 100644 index 0000000..c6a7b4e --- /dev/null +++ b/src/utilities/protocol_detector.cpp @@ -0,0 +1,55 @@ +/* + * protocol_detector.cpp + * + * Created on: Jul 15, 2021 14:05 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#include "ugv_sdk/utilities/protocol_detector.hpp" +#include "ugv_sdk/utilities/stopwatch.hpp" + +namespace westonrobot { +void ProtocolDectctor::Connect(std::string can_name) { + can_ = std::make_shared(can_name); + can_->SetReceiveCallback( + std::bind(&ProtocolDectctor::ParseCANFrame, this, std::placeholders::_1)); + can_->StartListening(); +} +void ProtocolDectctor::Connect(std::string uart_name, uint32_t baudrate) {} + +ProtocolVersion ProtocolDectctor::DetectProtocolVersion(uint32_t timeout_sec) { + msg_v1_detected_ = false; + msg_v2_detected_ = false; + + StopWatch sw; + Timer timer; + while (sw.stoc() < timeout_sec) { + timer.reset(); + if (msg_v1_detected_ || msg_v2_detected_) break; + timer.sleep_until_ms(50); + } + + // make sure only one version is detected + if (msg_v1_detected_ && msg_v2_detected_) return ProtocolVersion::UNKONWN; + + if (msg_v1_detected_) + return ProtocolVersion::AGX_V1; + else if (msg_v2_detected_) + return ProtocolVersion::AGX_V2; + + return ProtocolVersion::UNKONWN; +}; + +void ProtocolDectctor::ParseCANFrame(can_frame *rx_frame) { + // state feedback frame with id 0x151 is unique to V1 protocol + if (rx_frame->can_id == 0x151) { + msg_v1_detected_ = true; + } + // rc state frame with id 0x241 is unique to V2 protocol + else if (rx_frame->can_id == 0x241) { + msg_v2_detected_ = true; + } +} +} // namespace westonrobot \ No newline at end of file