diff --git a/include/ugv_sdk/details/interface/bunker_interface.hpp b/include/ugv_sdk/details/interface/bunker_interface.hpp index 57c12d5..f064c06 100644 --- a/include/ugv_sdk/details/interface/bunker_interface.hpp +++ b/include/ugv_sdk/details/interface/bunker_interface.hpp @@ -24,6 +24,16 @@ struct BunkerCoreState { RcStateMessage rc_state; }; +struct BunkerActuatorState { + AgxMsgTimeStamp time_stamp; + + // actuator state + ActuatorHSStateMessage actuator_hs_state[2]; + ActuatorLSStateMessage actuator_ls_state[2]; + // - for v1 robots only + ActuatorStateMessageV1 actuator_state[2]; +}; + struct BunkerInterface { virtual ~BunkerInterface() = default; @@ -31,6 +41,7 @@ struct BunkerInterface { // get robot state virtual BunkerCoreState GetRobotState() = 0; + virtual BunkerActuatorState GetActuatorState() = 0; }; } // namespace westonrobot diff --git a/include/ugv_sdk/details/robot_base/bunker_base.hpp b/include/ugv_sdk/details/robot_base/bunker_base.hpp index 16427c3..4d92f20 100644 --- a/include/ugv_sdk/details/robot_base/bunker_base.hpp +++ b/include/ugv_sdk/details/robot_base/bunker_base.hpp @@ -47,6 +47,19 @@ class BunkerBase : public AgilexBase, public BunkerInterface { bunker_state.rc_state = state.rc_state; return bunker_state; } + + BunkerActuatorState GetActuatorState() override { + auto actuator = AgilexBase::GetActuatorStateMsgGroup(); + + BunkerActuatorState bunker_actuator; + bunker_actuator.time_stamp = actuator.time_stamp; + for (int i = 0; i < 3; ++i) { + bunker_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i]; + bunker_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i]; + bunker_actuator.actuator_state[i] = actuator.actuator_state[i]; + } + return bunker_actuator; + } }; } // namespace westonrobot diff --git a/include/ugv_sdk/mobile_robot/bunker_robot.hpp b/include/ugv_sdk/mobile_robot/bunker_robot.hpp index 8c437fb..2f189c7 100644 --- a/include/ugv_sdk/mobile_robot/bunker_robot.hpp +++ b/include/ugv_sdk/mobile_robot/bunker_robot.hpp @@ -33,6 +33,7 @@ class BunkerRobot : public RobotCommonInterface, public BunkerInterface { // get robot state BunkerCoreState GetRobotState() override; + BunkerActuatorState GetActuatorState() override; private: RobotCommonInterface* robot_; diff --git a/src/mobile_robot/bunker_robot.cpp b/src/mobile_robot/bunker_robot.cpp index 2c03c11..481cb9a 100644 --- a/src/mobile_robot/bunker_robot.cpp +++ b/src/mobile_robot/bunker_robot.cpp @@ -42,4 +42,9 @@ BunkerCoreState BunkerRobot::GetRobotState() { auto bunker = dynamic_cast(robot_); return bunker->GetRobotState(); } + +BunkerActuatorState BunkerRobot::GetActuatorState() { + auto bunker = dynamic_cast(robot_); + return bunker->GetActuatorState(); +} } // namespace westonrobot \ No newline at end of file