From dd810eccbadc3d9177deed3e38ec8556c0bb1ccc Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Thu, 8 Jul 2021 18:20:34 +0800 Subject: [PATCH] saved work --- CMakeLists.txt | 2 +- demo/scout_demo/CMakeLists.txt | 4 +- demo/scout_demo/scout_robot_demo.cpp | 86 +++++++++---------- demo/scout_demo/scout_v2_demo.cpp | 2 +- .../ranger_interface.hpp | 0 include/ugv_sdk/interface/robot_interface.hpp | 40 ++++++--- include/ugv_sdk/interface/scout_interface.hpp | 2 + .../tracer_interface.hpp | 0 include/ugv_sdk/mobile_base/agilex_base.hpp | 2 - include/ugv_sdk/mobile_base/scout_robot.hpp | 25 +++++- include/ugv_sdk/protocol_v2/scout_base_v2.hpp | 6 +- src/mobile_base/scout_robot.cpp | 40 ++++++++- src/protocol_v2/scout_base.cpp | 16 ++-- 13 files changed, 148 insertions(+), 77 deletions(-) rename include/ugv_sdk/{mobile_base => interface}/ranger_interface.hpp (100%) rename include/ugv_sdk/{mobile_base => interface}/tracer_interface.hpp (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index eab878e..da39283 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -77,7 +77,7 @@ add_library(${PROJECT_NAME} src/async_port/async_can.cpp ######################## ## public interface to access robot - # src/mobile_base/scout_robot.cpp + src/mobile_base/scout_robot.cpp ######################## ## protocol v2 support # parser diff --git a/demo/scout_demo/CMakeLists.txt b/demo/scout_demo/CMakeLists.txt index a2830d6..e21fdf4 100755 --- a/demo/scout_demo/CMakeLists.txt +++ b/demo/scout_demo/CMakeLists.txt @@ -2,5 +2,5 @@ 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 +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 9205cfb..0218ecf 100644 --- a/demo/scout_demo/scout_robot_demo.cpp +++ b/demo/scout_demo/scout_robot_demo.cpp @@ -25,54 +25,54 @@ int main(int argc, char **argv) { return -1; } - ScoutRobot scout(AgilexProtocol::V1); -// scout.Connect(device_name); + ScoutRobot scout(ProtocolType::AGX_V2); + // scout.Connect(device_name); -// scout.EnableCommandedMode(); + // 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(); + // // // 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); -// } + // 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; + // // 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; -// } + // 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 index 5e732eb..0f1c358 100644 --- a/demo/scout_demo/scout_v2_demo.cpp +++ b/demo/scout_demo/scout_v2_demo.cpp @@ -23,7 +23,7 @@ int main(int argc, char **argv) { return -1; } - ScoutBase scout; + ScoutBaseV2 scout; scout.Connect(device_name); scout.EnableCommandedMode(); diff --git a/include/ugv_sdk/mobile_base/ranger_interface.hpp b/include/ugv_sdk/interface/ranger_interface.hpp similarity index 100% rename from include/ugv_sdk/mobile_base/ranger_interface.hpp rename to include/ugv_sdk/interface/ranger_interface.hpp diff --git a/include/ugv_sdk/interface/robot_interface.hpp b/include/ugv_sdk/interface/robot_interface.hpp index 8b0b1e3..92fc823 100644 --- a/include/ugv_sdk/interface/robot_interface.hpp +++ b/include/ugv_sdk/interface/robot_interface.hpp @@ -15,25 +15,41 @@ #include "ugv_sdk/interface/agilex_message.h" namespace westonrobot { -struct RobotInterface { +enum class ProtocolType { AGX_V1, AGX_V2 }; + +class RobotInterface { + public: + ~RobotInterface() = default; + // 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; - + // functions to be implemented by each robot class + virtual void Connect(std::string uart_name, uint32_t baudrate){ + // use derived version + }; virtual void ResetRobotState() = 0; + + protected: + /****** functions not available/valid to all robots ******/ + // functions to be implemented by class AgilexBase + virtual void SetMotionMode(uint8_t mode){}; + + // any specific robot will use a specialized version of the two functions + virtual void SendMotionCommand(double linear_vel, double angular_vel, + double lateral_velocity, + double steering_angle){ + // use derived version + }; + + virtual void SendLightCommand(LightMode front_mode, + uint8_t front_custom_value, LightMode rear_mode, + uint8_t rear_custom_value){ + // use derived version + }; }; } // namespace westonrobot diff --git a/include/ugv_sdk/interface/scout_interface.hpp b/include/ugv_sdk/interface/scout_interface.hpp index b642dda..0f655fd 100644 --- a/include/ugv_sdk/interface/scout_interface.hpp +++ b/include/ugv_sdk/interface/scout_interface.hpp @@ -31,6 +31,8 @@ struct ScoutState { }; struct ScoutInterface { + ~ScoutInterface() = default; + 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; diff --git a/include/ugv_sdk/mobile_base/tracer_interface.hpp b/include/ugv_sdk/interface/tracer_interface.hpp similarity index 100% rename from include/ugv_sdk/mobile_base/tracer_interface.hpp rename to include/ugv_sdk/interface/tracer_interface.hpp diff --git a/include/ugv_sdk/mobile_base/agilex_base.hpp b/include/ugv_sdk/mobile_base/agilex_base.hpp index 63a8695..bca5a79 100644 --- a/include/ugv_sdk/mobile_base/agilex_base.hpp +++ b/include/ugv_sdk/mobile_base/agilex_base.hpp @@ -29,8 +29,6 @@ #include "ugv_sdk/protocol_v2/agilex_msg_parser.h" namespace westonrobot { -enum class AgilexProtocol { V1, V2 }; - template class AgilexBase : public RobotInterface { public: diff --git a/include/ugv_sdk/mobile_base/scout_robot.hpp b/include/ugv_sdk/mobile_base/scout_robot.hpp index c0aa1e4..84901ca 100644 --- a/include/ugv_sdk/mobile_base/scout_robot.hpp +++ b/include/ugv_sdk/mobile_base/scout_robot.hpp @@ -12,15 +12,32 @@ #include -#include "ugv_sdk/mobile_base/common.hpp" +#include "ugv_sdk/interface/robot_interface.hpp" +#include "ugv_sdk/interface/scout_interface.hpp" namespace westonrobot { -class ScoutRobot { +class ScoutRobot : public RobotInterface, public ScoutInterface { public: - ScoutRobot(AgilexProtocol protocol); + ScoutRobot(ProtocolType protocol = ProtocolType::AGX_V2); + ~ScoutRobot(); + + void Connect(std::string can_name) override; + void Connect(std::string uart_name, uint32_t baudrate) override; + + void EnableCommandedMode(); + + 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; + void DisableLightControl() override; + + void ResetRobotState() override; + + // get robot state + ScoutState GetRobotState() override; private: - std::unique_ptr robot_; + RobotInterface* robot_; }; } // namespace westonrobot diff --git a/include/ugv_sdk/protocol_v2/scout_base_v2.hpp b/include/ugv_sdk/protocol_v2/scout_base_v2.hpp index d24650d..4c0e908 100644 --- a/include/ugv_sdk/protocol_v2/scout_base_v2.hpp +++ b/include/ugv_sdk/protocol_v2/scout_base_v2.hpp @@ -21,10 +21,10 @@ #include "ugv_sdk/protocol_v2/protocol_v2_parser.hpp" namespace westonrobot { -class ScoutBase : public AgilexBase, public ScoutInterface { +class ScoutBaseV2 : public AgilexBase, public ScoutInterface { public: - ScoutBase() : AgilexBase(){}; - ~ScoutBase() = default; + ScoutBaseV2() : AgilexBase(){}; + ~ScoutBaseV2() = default; // set up connection void Connect(std::string can_name) override; diff --git a/src/mobile_base/scout_robot.cpp b/src/mobile_base/scout_robot.cpp index 1986467..bde260b 100644 --- a/src/mobile_base/scout_robot.cpp +++ b/src/mobile_base/scout_robot.cpp @@ -8,7 +8,45 @@ */ #include "ugv_sdk/mobile_base/scout_robot.hpp" +#include "ugv_sdk/protocol_v2/scout_base_v2.hpp" namespace westonrobot { -ScoutRobot::ScoutRobot(AgilexProtocol protocol) {} +ScoutRobot::ScoutRobot(ProtocolType protocol) { + if (protocol == ProtocolType::AGX_V1) { + } else if (protocol == ProtocolType::AGX_V2) { + robot_ = new ScoutBaseV2(); + } +} + +ScoutRobot::~ScoutRobot() { + if (robot_) delete robot_; +} + +void ScoutRobot::EnableCommandedMode() { robot_->EnableCommandedMode(); } + +void ScoutRobot::Connect(std::string can_name) { robot_->Connect(can_name); } + +void ScoutRobot::Connect(std::string uart_name, uint32_t baudrate) { + robot_->Connect(uart_name, baudrate); +} + +void ScoutRobot::ResetRobotState() { robot_->ResetRobotState(); } + +void ScoutRobot::SetMotionCommand(double linear_vel, double angular_vel) { + auto scout = dynamic_cast(robot_); + scout->SetMotionCommand(linear_vel, angular_vel); +} + +void ScoutRobot::DisableLightControl() { robot_->DisableLightControl(); } + +void ScoutRobot::SetLightCommand(LightMode f_mode, uint8_t f_value, + LightMode r_mode, uint8_t r_value) { + auto scout = dynamic_cast(robot_); + scout->SetLightCommand(f_mode, f_value, r_mode, r_value); +} + +ScoutState ScoutRobot::GetRobotState() { + auto scout = dynamic_cast(robot_); + return scout->GetRobotState(); +} } // namespace westonrobot \ No newline at end of file diff --git a/src/protocol_v2/scout_base.cpp b/src/protocol_v2/scout_base.cpp index b7e74ea..01e20a7 100644 --- a/src/protocol_v2/scout_base.cpp +++ b/src/protocol_v2/scout_base.cpp @@ -22,37 +22,37 @@ #include "ugv_sdk/protocol_v2/agilex_msg_parser.h" namespace westonrobot { -void ScoutBase::Connect(std::string dev_name) { - AgilexBase::ConnectPort(dev_name, std::bind(&ScoutBase::ParseCANFrame, this, +void ScoutBaseV2::Connect(std::string dev_name) { + AgilexBase::ConnectPort(dev_name, std::bind(&ScoutBaseV2::ParseCANFrame, this, std::placeholders::_1)); } -void ScoutBase::Connect(std::string uart_name, uint32_t baudrate) { +void ScoutBaseV2::Connect(std::string uart_name, uint32_t baudrate) { // TODO } -void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel) { +void ScoutBaseV2::SetMotionCommand(double linear_vel, double angular_vel) { AgilexBase::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0); } -void ScoutBase::SetLightCommand(LightMode f_mode, uint8_t f_value, +void ScoutBaseV2::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::GetRobotState() { +ScoutState ScoutBaseV2::GetRobotState() { std::lock_guard guard(state_mutex_); return scout_state_; } -void ScoutBase::ParseCANFrame(can_frame *rx_frame) { +void ScoutBaseV2::ParseCANFrame(can_frame *rx_frame) { AgxMessage status_msg; DecodeCanFrame(rx_frame, &status_msg); std::lock_guard guard(state_mutex_); UpdateScoutState(status_msg, scout_state_); } -void ScoutBase::UpdateScoutState(const AgxMessage &status_msg, +void ScoutBaseV2::UpdateScoutState(const AgxMessage &status_msg, ScoutState &state) { switch (status_msg.type) { case AgxMsgSystemState: {