From 2a94b95f4abea32ab63d28bcf4cfb78e12b678c6 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Fri, 16 Jul 2021 14:15:12 +0800 Subject: [PATCH] added scout mini omni support, updated scout related demo --- demo/scout_demo/CMakeLists.txt | 5 +- demo/scout_demo/scout_mini_omni_demo.cpp | 140 ++++++++++++++++++ demo/scout_demo/scout_robot_demo.cpp | 27 +++- .../details/interface/scout_interface.hpp | 7 + .../protocol_v1/protocol_v1_parser.hpp | 8 + .../details/protocol_v1/robot_limits.hpp | 12 ++ .../details/robot_base/agilex_base.hpp | 11 +- .../ugv_sdk/details/robot_base/scout_base.hpp | 21 ++- include/ugv_sdk/mobile_robot/scout_robot.hpp | 19 ++- src/mobile_robot/scout_robot.cpp | 22 ++- 10 files changed, 255 insertions(+), 17 deletions(-) create mode 100644 demo/scout_demo/scout_mini_omni_demo.cpp diff --git a/demo/scout_demo/CMakeLists.txt b/demo/scout_demo/CMakeLists.txt index b553a56..539e4c2 100755 --- a/demo/scout_demo/CMakeLists.txt +++ b/demo/scout_demo/CMakeLists.txt @@ -1,2 +1,5 @@ add_executable(demo_scout_robot scout_robot_demo.cpp) -target_link_libraries(demo_scout_robot ugv_sdk) \ No newline at end of file +target_link_libraries(demo_scout_robot ugv_sdk) + +add_executable(demo_scout_mini_omni_robot scout_mini_omni_demo.cpp) +target_link_libraries(demo_scout_mini_omni_robot ugv_sdk) \ No newline at end of file diff --git a/demo/scout_demo/scout_mini_omni_demo.cpp b/demo/scout_demo/scout_mini_omni_demo.cpp new file mode 100644 index 0000000..0e91c26 --- /dev/null +++ b/demo/scout_demo/scout_mini_omni_demo.cpp @@ -0,0 +1,140 @@ +/* + * scout_robot_demo.cpp + * + * Created on: Jul 08, 2021 11:12 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#include + +#include +#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 device_name; + + if (argc == 2) { + device_name = {argv[1]}; + std::cout << "Selected interface " << device_name << std::endl; + } else { + std::cout << "Usage: demo_scout_mini_omni_robot " << std::endl + << "Example 1: ./demo_scout_mini_omni_robot can0" << std::endl; + return -1; + } + + std::unique_ptr scout; + + 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 ScoutMiniOmniRobot(ProtocolVersion::AGX_V1)); + + } else if (proto == ProtocolVersion::AGX_V2) { + std::cout << "Detected protocol: AGX_V2" << std::endl; + scout = std::unique_ptr( + new ScoutMiniOmniRobot(ProtocolVersion::AGX_V2)); + } else { + std::cout << "Detected protocol: UNKONWN" << std::endl; + return -1; + } + + if (scout == nullptr) + std::cout << "Failed to create robot object" << std::endl; + + scout->Connect(device_name); + + if (scout->GetParserProtocolVersion() == ProtocolVersion::AGX_V2) { + 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 30-40" << std::endl; + scout->SetLightCommand(CUSTOM, 30, CUSTOM, 40); + sleep(3); + scout->SetLightCommand(CONST_OFF, 0, CONST_OFF, 0); + + int count = 0; + while (true) { + // motion control + std::cout << "Setting motion command: 0.0, 0,0, 0.8" << std::endl; + scout->SetMotionCommand(0.0, 0.0, 0.8); + + // get robot state + 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) + << " , 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, lateral): " + << state.motion_state.linear_velocity << ", " + << state.motion_state.angular_velocity << ", " + << state.motion_state.lateral_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) { + for (int i = 0; i < 4; ++i) { + printf("motor %d: current %f, rpm %d, driver temp %f, motor temp %f\n", + actuator.actuator_state[i].motor_id, + actuator.actuator_state[i].current, + actuator.actuator_state[i].rpm, + 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; + + usleep(20000); + ++count; + } + + return 0; +} \ 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 aedd5fa..8d8441a 100644 --- a/demo/scout_demo/scout_robot_demo.cpp +++ b/demo/scout_demo/scout_robot_demo.cpp @@ -19,17 +19,34 @@ using namespace westonrobot; int main(int argc, char **argv) { std::string device_name; + std::string robot_subtype; if (argc == 2) { device_name = {argv[1]}; - std::cout << "Selected interface " << device_name << std::endl; + std::cout << "Selected interface " << device_name << ", robot type: scout" + << std::endl; + } else if (argc == 3) { + robot_subtype = {argv[1]}; + device_name = {argv[2]}; + std::cout << "Selected interface " << device_name + << ", robot type: " << robot_subtype << std::endl; } else { - std::cout << "Usage: app_scout_demo " + std::cout << "Usage: demo_scout_robot [] " << std::endl - << "Example 1: ./app_scout_demo can0" << std::endl; + << "Example 1: ./demo_scout_robot can0" << std::endl + << "\t : mini" << std::endl; return -1; } + bool is_scout_mini = false; + if (robot_subtype == "mini") { + is_scout_mini = true; + } else if (!robot_subtype.empty() && robot_subtype != "scout") { + std::cout + << "Unkonwn robot subtype. Supported subtypes: \"scout\" or \"mini\"" + << std::endl; + } + std::unique_ptr scout; ProtocolDectctor detector; @@ -38,11 +55,11 @@ int main(int argc, char **argv) { if (proto == ProtocolVersion::AGX_V1) { std::cout << "Detected protocol: AGX_V1" << std::endl; scout = std::unique_ptr( - new ScoutRobot(ProtocolVersion::AGX_V1, true)); + new ScoutRobot(ProtocolVersion::AGX_V1, is_scout_mini)); } else if (proto == ProtocolVersion::AGX_V2) { std::cout << "Detected protocol: AGX_V2" << std::endl; scout = std::unique_ptr( - new ScoutRobot(ProtocolVersion::AGX_V2, true)); + new ScoutRobot(ProtocolVersion::AGX_V2, is_scout_mini)); } else { std::cout << "Detected protocol: UNKONWN" << std::endl; return -1; diff --git a/include/ugv_sdk/details/interface/scout_interface.hpp b/include/ugv_sdk/details/interface/scout_interface.hpp index af20fbd..eb4974e 100644 --- a/include/ugv_sdk/details/interface/scout_interface.hpp +++ b/include/ugv_sdk/details/interface/scout_interface.hpp @@ -51,6 +51,13 @@ struct ScoutInterface { virtual ScoutCoreState GetRobotState() = 0; virtual ScoutActuatorState GetActuatorState() = 0; }; + +struct ScoutOmniInterface { + virtual ~ScoutOmniInterface() = default; + + virtual void SetMotionCommand(double linear_vel, double angular_vel, + double lateral_velocity) = 0; +}; } // namespace westonrobot #endif /* SCOUT_INTERFACE_HPP */ diff --git a/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp b/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp index 761a9b2..a665824 100644 --- a/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp +++ b/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp @@ -30,6 +30,8 @@ class ProtocolV1Parser : public ParserInterface { if (msg->type == AgxMsgMotionCommandV1) { float linear = msg->body.v1_motion_command_msg.linear; float angular = msg->body.v1_motion_command_msg.angular; + float lateral = msg->body.v1_motion_command_msg.lateral; + if (linear > RobotLimitsType::max_linear) linear = RobotLimitsType::max_linear; else if (linear < RobotLimitsType::min_linear) @@ -38,11 +40,17 @@ class ProtocolV1Parser : public ParserInterface { angular = RobotLimitsType::max_angular; else if (angular < RobotLimitsType::min_angular) angular = RobotLimitsType::min_angular; + if (lateral > RobotLimitsType::max_lateral) + lateral = RobotLimitsType::max_lateral; + else if (lateral < RobotLimitsType::min_lateral) + lateral = RobotLimitsType::min_lateral; msg_v1.body.v1_motion_command_msg.linear = linear / RobotLimitsType::max_linear * 100.0; msg_v1.body.v1_motion_command_msg.angular = angular / RobotLimitsType::max_angular * 100.0; + msg_v1.body.v1_motion_command_msg.lateral = + lateral / RobotLimitsType::max_lateral * 100.0; } return EncodeCanFrameV1(&msg_v1, tx_frame); } diff --git a/include/ugv_sdk/details/protocol_v1/robot_limits.hpp b/include/ugv_sdk/details/protocol_v1/robot_limits.hpp index 6473c2e..5bbbd58 100644 --- a/include/ugv_sdk/details/protocol_v1/robot_limits.hpp +++ b/include/ugv_sdk/details/protocol_v1/robot_limits.hpp @@ -16,6 +16,9 @@ struct ScoutV2Limits { static constexpr double min_linear = -max_linear; static constexpr double max_angular = 0.5235; // rad/s static constexpr double min_angular = -max_angular; + + static constexpr double max_lateral = 0.0; + static constexpr double min_lateral = 0.0; }; struct ScoutMiniLimits { @@ -23,6 +26,9 @@ struct ScoutMiniLimits { static constexpr double min_linear = -max_linear; static constexpr double max_angular = 2.5235; // rad/s static constexpr double min_angular = -max_angular; + + static constexpr double max_lateral = 2.0; + static constexpr double min_lateral = -max_lateral; }; struct BunkerLimits { @@ -30,6 +36,9 @@ struct BunkerLimits { static constexpr double min_linear = -max_linear; static constexpr double max_angular = 2.5235; // rad/s static constexpr double min_angular = -max_angular; + + static constexpr double max_lateral = 0.0; + static constexpr double min_lateral = 0.0; }; struct HunterV1Limits { @@ -37,6 +46,9 @@ struct HunterV1Limits { static constexpr double min_linear = -max_linear; static constexpr double max_angular = 25.5; // degree static constexpr double min_angular = -max_angular; + + static constexpr double max_lateral = 0.0; + static constexpr double min_lateral = 0.0; }; } // namespace westonrobot diff --git a/include/ugv_sdk/details/robot_base/agilex_base.hpp b/include/ugv_sdk/details/robot_base/agilex_base.hpp index d0b512b..42f3a70 100644 --- a/include/ugv_sdk/details/robot_base/agilex_base.hpp +++ b/include/ugv_sdk/details/robot_base/agilex_base.hpp @@ -55,7 +55,7 @@ class AgilexBase : public RobotCommonInterface { // must be called at a frequency >= 50Hz void SendMotionCommand(double linear_vel, double angular_vel, - double lateral_velocity, double steering_angle) { + double lateral_vel, double steering_angle) { if (can_connected_) { // motion control message AgxMessage msg; @@ -68,19 +68,18 @@ class AgilexBase : public RobotCommonInterface { msg.body.v1_motion_command_msg.angular = std::abs(angular_vel) > std::abs(steering_angle) ? angular_vel : steering_angle; - msg.body.v1_motion_command_msg.lateral = lateral_velocity; + msg.body.v1_motion_command_msg.lateral = lateral_vel; } else if (parser_.GetParserProtocolVersion() == ProtocolVersion::AGX_V2) { 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.lateral_velocity = lateral_vel; 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 + << "," << lateral_vel << std::endl; // send to can bus can_frame frame; diff --git a/include/ugv_sdk/details/robot_base/scout_base.hpp b/include/ugv_sdk/details/robot_base/scout_base.hpp index f42d356..604c2ce 100644 --- a/include/ugv_sdk/details/robot_base/scout_base.hpp +++ b/include/ugv_sdk/details/robot_base/scout_base.hpp @@ -40,8 +40,9 @@ class ScoutBase : public AgilexBase, public ScoutInterface { 0.0); } - void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode, - uint8_t r_value) override { + void SetLightCommand(LightMode f_mode, uint8_t f_value, + LightMode r_mode = LightMode::CONST_ON, + uint8_t r_value = 0) override { AgilexBase::SendLightCommand(f_mode, f_value, r_mode, r_value); } @@ -71,6 +72,20 @@ class ScoutBase : public AgilexBase, public ScoutInterface { return scout_actuator; } }; + +template +class ScoutMiniOmniBase : public ScoutBase, + public ScoutOmniInterface { + public: + void SetMotionCommand(double linear_vel, double angular_vel, + double lateral_velocity) override { + AgilexBase::SendMotionCommand(linear_vel, angular_vel, + lateral_velocity, 0.0); + } + + private: + using ScoutBase::SetMotionCommand; +}; } // namespace westonrobot #include "ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp" @@ -79,9 +94,11 @@ class ScoutBase : public AgilexBase, public ScoutInterface { namespace westonrobot { using ScoutBaseV1 = ScoutBase; using ScoutMiniBaseV1 = ScoutBase; +using ScoutMiniOmniBaseV1 = ScoutMiniOmniBase; using ScoutBaseV2 = ScoutBase; using ScoutMiniBaseV2 = ScoutBase; +using ScoutMiniOmniBaseV2 = ScoutMiniOmniBase; } // namespace westonrobot #endif /* SCOUT_BASE_HPP */ diff --git a/include/ugv_sdk/mobile_robot/scout_robot.hpp b/include/ugv_sdk/mobile_robot/scout_robot.hpp index 4301ddb..56a5569 100644 --- a/include/ugv_sdk/mobile_robot/scout_robot.hpp +++ b/include/ugv_sdk/mobile_robot/scout_robot.hpp @@ -20,7 +20,7 @@ class ScoutRobot : public RobotCommonInterface, public ScoutInterface { public: ScoutRobot(ProtocolVersion protocol = ProtocolVersion::AGX_V2, bool is_mini_model = false); - ~ScoutRobot(); + virtual ~ScoutRobot(); void Connect(std::string can_name) override; void Connect(std::string uart_name, uint32_t baudrate) override; @@ -40,9 +40,24 @@ class ScoutRobot : public RobotCommonInterface, public ScoutInterface { ScoutCoreState GetRobotState() override; ScoutActuatorState GetActuatorState() override; - private: + protected: RobotCommonInterface* robot_; }; + +/////////////////////////////////////////////////////////////////////////// + +class ScoutMiniOmniRobot : public ScoutRobot, public ScoutOmniInterface { + public: + ScoutMiniOmniRobot(ProtocolVersion protocol = ProtocolVersion::AGX_V2); + ~ScoutMiniOmniRobot(); + + void SetMotionCommand(double linear_vel, double angular_vel, + double lateral_velocity) override; + + private: + using ScoutRobot::SetMotionCommand; + // void SetMotionCommand(double linear_vel, double angular_vel) = delete; +}; } // namespace westonrobot #endif /* SCOUT_ROBOT_HPP */ diff --git a/src/mobile_robot/scout_robot.cpp b/src/mobile_robot/scout_robot.cpp index 238e0f9..2b85a44 100644 --- a/src/mobile_robot/scout_robot.cpp +++ b/src/mobile_robot/scout_robot.cpp @@ -36,7 +36,7 @@ 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); + // robot_->Connect(uart_name, baudrate); } void ScoutRobot::ResetRobotState() { robot_->ResetRobotState(); } @@ -67,4 +67,24 @@ ScoutActuatorState ScoutRobot::GetActuatorState() { auto scout = dynamic_cast(robot_); return scout->GetActuatorState(); } + +/////////////////////////////////////////////////////////////////////////// + +ScoutMiniOmniRobot::ScoutMiniOmniRobot(ProtocolVersion protocol) { + if (protocol == ProtocolVersion::AGX_V1) { + robot_ = new ScoutMiniOmniBaseV1(); + } else if (protocol == ProtocolVersion::AGX_V2) { + robot_ = new ScoutMiniOmniBaseV2(); + } +} + +ScoutMiniOmniRobot::~ScoutMiniOmniRobot() { + if (robot_) delete robot_; +} + +void ScoutMiniOmniRobot::SetMotionCommand(double linear_vel, double angular_vel, + double lateral_velocity) { + auto scout = dynamic_cast(robot_); + scout->SetMotionCommand(linear_vel, angular_vel, lateral_velocity); +} } // namespace westonrobot \ No newline at end of file