From 8c97c60f752a43327c23994aebedda64f84e3d02 Mon Sep 17 00:00:00 2001 From: hanskw4267 Date: Wed, 10 Jan 2024 10:58:44 +0800 Subject: [PATCH] src: cleanup sensor state msgs --- .../details/interface/hunter_interface.hpp | 7 ++-- .../details/interface/ranger_interface.hpp | 8 +++- .../details/interface/scout_interface.hpp | 7 +++- .../details/robot_base/agilex_base.hpp | 17 ++++---- .../details/robot_base/hunter_base.hpp | 6 +-- .../details/robot_base/ranger_base.hpp | 40 +++++++++++++++---- .../ugv_sdk/details/robot_base/scout_base.hpp | 2 +- include/ugv_sdk/mobile_robot/hunter_robot.hpp | 2 +- include/ugv_sdk/mobile_robot/ranger_robot.hpp | 1 + src/mobile_robot/hunter_robot.cpp | 4 +- src/mobile_robot/ranger_robot.cpp | 4 ++ 11 files changed, 69 insertions(+), 29 deletions(-) diff --git a/include/ugv_sdk/details/interface/hunter_interface.hpp b/include/ugv_sdk/details/interface/hunter_interface.hpp index 1578a3bb..c8f58f07 100644 --- a/include/ugv_sdk/details/interface/hunter_interface.hpp +++ b/include/ugv_sdk/details/interface/hunter_interface.hpp @@ -34,12 +34,13 @@ struct HunterActuatorState { ActuatorStateMessageV1 actuator_state[3]; }; -struct HunterCommonSensorState { - AgxMsgTimeStamp time_stamp; +struct HunterSensorState { + SdkTimePoint time_stamp; BmsBasicMessage bms_basic_state; BmsExtendedMessage bms_extend_state; }; + struct HunterInterface { virtual ~HunterInterface() = default; @@ -51,7 +52,7 @@ struct HunterInterface { // get robot state virtual HunterCoreState GetRobotState() = 0; virtual HunterActuatorState GetActuatorState() = 0; - virtual HunterCommonSensorState GetCommonSensorState() = 0; + virtual HunterSensorState GetSensorState() = 0; }; } // namespace westonrobot diff --git a/include/ugv_sdk/details/interface/ranger_interface.hpp b/include/ugv_sdk/details/interface/ranger_interface.hpp index 4c96e62c..cdf2ee8f 100644 --- a/include/ugv_sdk/details/interface/ranger_interface.hpp +++ b/include/ugv_sdk/details/interface/ranger_interface.hpp @@ -27,7 +27,6 @@ struct RangerCoreState { RcStateMessage rc_state; OdometryMessage odometry; - BmsBasicMessage bms_basic_state; }; struct RangerActuatorState { @@ -40,6 +39,12 @@ struct RangerActuatorState { ActuatorLSStateMessage actuator_ls_state[8]; }; +struct RangerSensorState { + SdkTimePoint time_stamp; + + BmsBasicMessage bms_basic_state; +}; + ///////////////////////////////////////////////////////////////////////// struct RangerInterface { @@ -64,6 +69,7 @@ struct RangerInterface { // get robot state virtual RangerCoreState GetRobotState() = 0; virtual RangerActuatorState GetActuatorState() = 0; + virtual RangerSensorState GetSensorState() = 0; }; } // namespace westonrobot diff --git a/include/ugv_sdk/details/interface/scout_interface.hpp b/include/ugv_sdk/details/interface/scout_interface.hpp index 5e8e320a..14006e8b 100644 --- a/include/ugv_sdk/details/interface/scout_interface.hpp +++ b/include/ugv_sdk/details/interface/scout_interface.hpp @@ -23,7 +23,6 @@ struct ScoutCoreState { MotionStateMessage motion_state; LightStateMessage light_state; RcStateMessage rc_state; - BmsBasicMessage bms_basic_state; }; struct ScoutActuatorState { @@ -37,6 +36,12 @@ struct ScoutActuatorState { ActuatorStateMessageV1 actuator_state[4]; }; +struct ScoutSensorState { + SdkTimePoint time_stamp; + + BmsBasicMessage bms_basic_state; +}; + struct ScoutInterface { virtual ~ScoutInterface() = default; diff --git a/include/ugv_sdk/details/robot_base/agilex_base.hpp b/include/ugv_sdk/details/robot_base/agilex_base.hpp index 1a7b22d2..761bfe18 100644 --- a/include/ugv_sdk/details/robot_base/agilex_base.hpp +++ b/include/ugv_sdk/details/robot_base/agilex_base.hpp @@ -31,8 +31,6 @@ struct CoreStateMsgGroup { LightStateMessage light_state; MotionModeStateMessage motion_mode_state; RcStateMessage rc_state; - - BmsBasicMessage bms_basic_state; }; struct ActuatorStateMsgGroup { @@ -48,6 +46,7 @@ struct ActuatorStateMsgGroup { struct SensorStateMsgGroup { SdkTimePoint time_stamp; + BmsBasicMessage bms_basic_state; BmsExtendedMessage bms_extend_state; }; @@ -319,12 +318,12 @@ class AgilexBase : public RobotCommonInterface { core_state_msgs_.rc_state = status_msg.body.rc_state_msg; break; } - case AgxMsgBmsBasic: { - // std::cout << "system status feedback received" << std::endl; - core_state_msgs_.time_stamp = SdkClock::now(); - core_state_msgs_.bms_basic_state = status_msg.body.bms_basic_msg; - break; - } + // case AgxMsgBmsBasic: { + // // std::cout << "system status feedback received" << std::endl; + // core_state_msgs_.time_stamp = SdkClock::now(); + // core_state_msgs_.bms_basic_state = status_msg.body.bms_basic_msg; + // break; + // } default: break; } @@ -389,7 +388,7 @@ class AgilexBase : public RobotCommonInterface { switch (status_msg.type) { case AgxMsgBmsBasic: { // std::cout << "system status feedback received" << std::endl; - common_sensor_state_msgs_.time_stamp = AgxMsgRefClock::now(); + common_sensor_state_msgs_.time_stamp = SdkClock::now(); common_sensor_state_msgs_.bms_basic_state = status_msg.body.bms_basic_msg; break; diff --git a/include/ugv_sdk/details/robot_base/hunter_base.hpp b/include/ugv_sdk/details/robot_base/hunter_base.hpp index d3073512..81460609 100644 --- a/include/ugv_sdk/details/robot_base/hunter_base.hpp +++ b/include/ugv_sdk/details/robot_base/hunter_base.hpp @@ -61,11 +61,11 @@ class HunterBase : public AgilexBase, public HunterInterface { return hunter_actuator; } - HunterCommonSensorState GetCommonSensorState() override { + HunterSensorState GetSensorState() override { auto common_sensor = - AgilexBase::GetCommonSensorStateMsgGroup(); + AgilexBase::GetSensorStateMsgGroup(); - HunterCommonSensorState hunter_bms; + HunterSensorState hunter_bms; hunter_bms.time_stamp = common_sensor.time_stamp; diff --git a/include/ugv_sdk/details/robot_base/ranger_base.hpp b/include/ugv_sdk/details/robot_base/ranger_base.hpp index ee3c52f7..9662f40b 100644 --- a/include/ugv_sdk/details/robot_base/ranger_base.hpp +++ b/include/ugv_sdk/details/robot_base/ranger_base.hpp @@ -63,17 +63,17 @@ class RangerBaseV2 : public AgilexBase, ranger_state.rc_state = state.rc_state; ranger_state.motion_mode_state = state.motion_mode_state; - ranger_state.bms_basic_state.current = state.bms_basic_state.current; + // ranger_state.bms_basic_state.current = state.bms_basic_state.current; // Note: BMS CAN message definition is not consistent across AgileX robots. // Robots with steering mechanism should additionally divide the voltage by // 10. - ranger_state.bms_basic_state.voltage = state.bms_basic_state.voltage * 0.1f; - ranger_state.bms_basic_state.battery_soc = - state.bms_basic_state.battery_soc; - ranger_state.bms_basic_state.battery_soh = - state.bms_basic_state.battery_soh; - ranger_state.bms_basic_state.temperature = - state.bms_basic_state.temperature; + // ranger_state.bms_basic_state.voltage = state.bms_basic_state.voltage * 0.1f; + // ranger_state.bms_basic_state.battery_soc = + // state.bms_basic_state.battery_soc; + // ranger_state.bms_basic_state.battery_soh = + // state.bms_basic_state.battery_soh; + // ranger_state.bms_basic_state.temperature = + // state.bms_basic_state.temperature; return ranger_state; } @@ -99,6 +99,30 @@ class RangerBaseV2 : public AgilexBase, } return ranger_actuator; } + + RangerSensorState GetSensorState() override { + auto common_sensor = + AgilexBase::GetSensorStateMsgGroup(); + + RangerSensorState ranger_bms; + + ranger_bms.time_stamp = common_sensor.time_stamp; + + ranger_bms.bms_basic_state.current = common_sensor.bms_basic_state.current; + // Note: BMS CAN message definition is not consistent across AgileX robots. + // Robots with steering mechanism should additionally divide the voltage by + // 10. + ranger_bms.bms_basic_state.voltage = + common_sensor.bms_basic_state.voltage * 0.1f; + ranger_bms.bms_basic_state.battery_soc = + common_sensor.bms_basic_state.battery_soc; + ranger_bms.bms_basic_state.battery_soh = + common_sensor.bms_basic_state.battery_soh; + ranger_bms.bms_basic_state.temperature = + common_sensor.bms_basic_state.temperature; + + return ranger_bms; + } }; // Note: Ranger Mini V1 uses a modified AgileX V2 protocol diff --git a/include/ugv_sdk/details/robot_base/scout_base.hpp b/include/ugv_sdk/details/robot_base/scout_base.hpp index 15b7d899..b73ab520 100644 --- a/include/ugv_sdk/details/robot_base/scout_base.hpp +++ b/include/ugv_sdk/details/robot_base/scout_base.hpp @@ -56,7 +56,7 @@ class ScoutBase : public AgilexBase, public ScoutInterface { scout_state.motion_state = state.motion_state; scout_state.light_state = state.light_state; scout_state.rc_state = state.rc_state; - scout_state.bms_basic_state = state.bms_basic_state; + // scout_state.bms_basic_state = state.bms_basic_state; return scout_state; } diff --git a/include/ugv_sdk/mobile_robot/hunter_robot.hpp b/include/ugv_sdk/mobile_robot/hunter_robot.hpp index 49915efa..7b4e9651 100644 --- a/include/ugv_sdk/mobile_robot/hunter_robot.hpp +++ b/include/ugv_sdk/mobile_robot/hunter_robot.hpp @@ -39,7 +39,7 @@ class HunterRobot : public RobotCommonInterface, public HunterInterface { // get robot state HunterCoreState GetRobotState() override; HunterActuatorState GetActuatorState() override; - HunterCommonSensorState GetCommonSensorState() override; + HunterSensorState GetSensorState() override; private: RobotCommonInterface* robot_; diff --git a/include/ugv_sdk/mobile_robot/ranger_robot.hpp b/include/ugv_sdk/mobile_robot/ranger_robot.hpp index 237a7d6a..1d6331dd 100644 --- a/include/ugv_sdk/mobile_robot/ranger_robot.hpp +++ b/include/ugv_sdk/mobile_robot/ranger_robot.hpp @@ -44,6 +44,7 @@ class RangerRobot : public RobotCommonInterface, public RangerInterface { // get robot state RangerCoreState GetRobotState() override; RangerActuatorState GetActuatorState() override; + RangerSensorState GetSensorState() override; private: RobotCommonInterface* robot_; diff --git a/src/mobile_robot/hunter_robot.cpp b/src/mobile_robot/hunter_robot.cpp index 4bff2fe3..daea6043 100644 --- a/src/mobile_robot/hunter_robot.cpp +++ b/src/mobile_robot/hunter_robot.cpp @@ -63,8 +63,8 @@ HunterActuatorState HunterRobot::GetActuatorState() { auto hunter = dynamic_cast(robot_); return hunter->GetActuatorState(); } -HunterCommonSensorState HunterRobot::GetCommonSensorState() { +HunterSensorState HunterRobot::GetSensorState() { auto hunter = dynamic_cast(robot_); - return hunter->GetCommonSensorState(); + return hunter->GetSensorState(); } } // namespace westonrobot \ No newline at end of file diff --git a/src/mobile_robot/ranger_robot.cpp b/src/mobile_robot/ranger_robot.cpp index c9deca8c..53201673 100644 --- a/src/mobile_robot/ranger_robot.cpp +++ b/src/mobile_robot/ranger_robot.cpp @@ -67,4 +67,8 @@ RangerActuatorState RangerRobot::GetActuatorState() { auto ranger = dynamic_cast(robot_); return ranger->GetActuatorState(); } +RangerSensorState RangerRobot::GetSensorState() { + auto ranger = dynamic_cast(robot_); + return ranger->GetSensorState(); +} } // namespace westonrobot \ No newline at end of file