diff --git a/CMakeLists.txt b/CMakeLists.txt index 1882e98a..69da3c6a 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -100,6 +100,7 @@ add_library(${PROJECT_NAME} src/mobile_robot/hunter_robot.cpp src/mobile_robot/bunker_robot.cpp src/mobile_robot/ranger_robot.cpp + src/mobile_robot/titan_robot.cpp ######################## ## protocol v2 support src/protocol_v2/agilex_msg_parser_v2.c diff --git a/include/ugv_sdk/details/interface/titan_interface.hpp b/include/ugv_sdk/details/interface/titan_interface.hpp new file mode 100644 index 00000000..25f9e882 --- /dev/null +++ b/include/ugv_sdk/details/interface/titan_interface.hpp @@ -0,0 +1,62 @@ +/* + * ranger_interface.hpp + * + * Created on: Jul 08, 2021 09:40 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#ifndef TITAN_INTERFACE_HPP +#define TITAN_INTERFACE_HPP + +#include + +#include "ugv_sdk/details/interface/agilex_message.h" +#include "ugv_sdk/details/interface/robot_common_interface.hpp" + +namespace westonrobot { +struct TitanCoreState { + // system state + SdkTimePoint time_stamp; + + SystemStateMessage system_state; + MotionStateMessage motion_state; + MotionModeStateMessage motion_mode_state; + + RcStateMessage rc_state; + OdometryMessage odometry; +}; + +struct TitanActuatorState { + SdkTimePoint time_stamp; + + ActuatorHSStateMessage actuator_hs_state[4]; + ActuatorLSStateMessage actuator_ls_state[4]; +}; + +struct TitanCommonSensorState { + SdkTimePoint time_stamp; + + BmsBasicMessage bms_basic_state; +}; + +///////////////////////////////////////////////////////////////////////// + +struct TitanInterface { + + virtual ~TitanInterface() = default; + + // robot control + virtual void SetMotionCommand(double linear_vel, double steer_angle) = 0; + virtual void ActivateBrake() = 0; + virtual void ReleaseBrake() = 0; + + // get robot state + virtual TitanCoreState GetRobotState() = 0; + virtual TitanActuatorState GetActuatorState() = 0; + virtual TitanCommonSensorState GetCommonSensorState() = 0; +}; +} // namespace westonrobot + +#endif /* TITAN_INTERFACE_HPP */ diff --git a/include/ugv_sdk/details/robot_base/titan_base.hpp b/include/ugv_sdk/details/robot_base/titan_base.hpp new file mode 100644 index 00000000..782182ab --- /dev/null +++ b/include/ugv_sdk/details/robot_base/titan_base.hpp @@ -0,0 +1,99 @@ +/** + * @Author : Junyu Luo + * @Date : 2024-03-4 + * @FileName : titan_base.hpp + * @Mail : wangzheqie@qq.com + * Copyright : AgileX Robotics + **/ + +#ifndef TITAN_BASE_HPP +#define TITAN_BASE_HPP + +#include +#include +#include +#include + +#include "ugv_sdk/details/interface/titan_interface.hpp" +#include "ugv_sdk/details/robot_base/agilex_base.hpp" + +#include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp" + +namespace westonrobot { +class TitanBase : public AgilexBase, + public TitanInterface { + public: + TitanBase() : AgilexBase(){}; + virtual ~TitanBase() = default; + + // set up connection + bool Connect(std::string can_name) override { + return AgilexBase::Connect(can_name); + } + + // robot control + void SetMotionCommand(double linear_vel, double steer_angle) override { + AgilexBase::SendMotionCommand(linear_vel, 0.0, + 0.0, steer_angle); + } + + // get robot state + TitanCoreState GetRobotState() override { + auto state = AgilexBase::GetRobotCoreStateMsgGroup(); + + TitanCoreState titan_state; + titan_state.time_stamp = state.time_stamp; + titan_state.system_state = state.system_state; + titan_state.motion_state = state.motion_state; + titan_state.rc_state = state.rc_state; + titan_state.motion_mode_state = state.motion_mode_state; + + return titan_state; + } + + TitanActuatorState GetActuatorState() override { + auto actuator = AgilexBase::GetActuatorStateMsgGroup(); + + TitanActuatorState titan_actuator; + titan_actuator.time_stamp = actuator.time_stamp; + for (int i = 0; i < 4; ++i) { + titan_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i]; + titan_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i]; + } + return titan_actuator; + } + + TitanCommonSensorState GetCommonSensorState() override { + auto common_sensor = + AgilexBase::GetCommonSensorStateMsgGroup(); + + TitanCommonSensorState titan_bms; + + titan_bms.time_stamp = common_sensor.time_stamp; + + titan_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. + titan_bms.bms_basic_state.voltage = + common_sensor.bms_basic_state.voltage * 0.1f; + titan_bms.bms_basic_state.battery_soc = + common_sensor.bms_basic_state.battery_soc; + titan_bms.bms_basic_state.battery_soh = + common_sensor.bms_basic_state.battery_soh; + titan_bms.bms_basic_state.temperature = + common_sensor.bms_basic_state.temperature; + + return titan_bms; + } + void ActivateBrake() override { + AgilexBase::SetBrakeMode(AgxBrakeMode::BRAKE_MODE_UNLOCK); + } + + void ReleaseBrake() override { + AgilexBase::SetBrakeMode(AgxBrakeMode::BRAKE_MODE_LOCK); + } + +}; +} // namespace westonrobot +#endif /* TITAN_BASE_HPP */ diff --git a/include/ugv_sdk/mobile_robot/titan_robot.hpp b/include/ugv_sdk/mobile_robot/titan_robot.hpp new file mode 100644 index 00000000..606a4615 --- /dev/null +++ b/include/ugv_sdk/mobile_robot/titan_robot.hpp @@ -0,0 +1,49 @@ +/* + * ranger_robot.hpp + * + * Created on: Jul 14, 2021 21:45 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#ifndef TITAN_ROBOT_HPP +#define TITAN_ROBOT_HPP + +#include + +#include "ugv_sdk/details/robot_base/titan_base.hpp" + +namespace westonrobot { +class TitanRobot : public RobotCommonInterface, public TitanInterface { + public: + TitanRobot(); + ~TitanRobot(); + + bool Connect(std::string can_name) override; + + void EnableCommandedMode() override; + std::string RequestVersion(int timeout_sec) override; + + void ActivateBrake() override; + void ReleaseBrake() override; + + // functions to be implemented by each robot class + void ResetRobotState() override; + + ProtocolVersion GetParserProtocolVersion() override; + + // robot control + void SetMotionCommand(double linear_vel, double steer_angle) override; + + // get robot state + TitanCoreState GetRobotState() override; + TitanActuatorState GetActuatorState() override; + TitanCommonSensorState GetCommonSensorState() override; + + private: + RobotCommonInterface* robot_; +}; +} // namespace westonrobot + +#endif /* TITAN_ROBOT_HPP */ diff --git a/src/mobile_robot/titan_robot.cpp b/src/mobile_robot/titan_robot.cpp new file mode 100644 index 00000000..97d92d2c --- /dev/null +++ b/src/mobile_robot/titan_robot.cpp @@ -0,0 +1,68 @@ +/* + * titan_robot.cpp + * + * Created on: Feb 23, 2023 17:10 + * Description: + * + * Copyright (c) 2023 Weston Robot Pte. Ltd. + */ +#include "ugv_sdk/mobile_robot/titan_robot.hpp" +#include "ugv_sdk/details/robot_base/titan_base.hpp" + +namespace westonrobot { +TitanRobot::TitanRobot() { + robot_ = new TitanBase(); +} + +TitanRobot::~TitanRobot() { + if (robot_) delete robot_; +} + +bool TitanRobot::Connect(std::string can_name) { + return robot_->Connect(can_name); +} + +void TitanRobot::EnableCommandedMode() { robot_->EnableCommandedMode(); } + +std::string TitanRobot::RequestVersion(int timeout_sec) { + return robot_->RequestVersion(timeout_sec); +} + +// functions to be implemented by each robot class +void TitanRobot::ResetRobotState() { robot_->ResetRobotState(); } + +ProtocolVersion TitanRobot::GetParserProtocolVersion() { + return robot_->GetParserProtocolVersion(); +} + +// robot control +void TitanRobot::SetMotionCommand(double linear_vel, double steer_angle) { + auto titan = dynamic_cast(robot_); + return titan->SetMotionCommand(linear_vel, steer_angle); +} + +void TitanRobot::ActivateBrake() { + auto hunter = dynamic_cast(robot_); + hunter->ActivateBrake(); +} + +void TitanRobot::ReleaseBrake() { + auto hunter = dynamic_cast(robot_); + hunter->ReleaseBrake(); +} + +// get robot state +TitanCoreState TitanRobot::GetRobotState() { + auto titan = dynamic_cast(robot_); + return titan->GetRobotState(); +} + +TitanActuatorState TitanRobot::GetActuatorState() { + auto titan = dynamic_cast(robot_); + return titan->GetActuatorState(); +} +TitanCommonSensorState TitanRobot::GetCommonSensorState() { + auto titan = dynamic_cast(robot_); + return titan->GetCommonSensorState(); +} +} // namespace westonrobot \ No newline at end of file