diff --git a/CMakeLists.txt b/CMakeLists.txt index 1bd44b6f..95bc7e86 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -101,6 +101,7 @@ add_library(${PROJECT_NAME} src/mobile_robot/bunker_robot.cpp src/mobile_robot/ranger_robot.cpp src/mobile_robot/titan_robot.cpp + src/mobile_robot/tracer_robot.cpp ######################## ## protocol v2 support src/protocol_v2/agilex_msg_parser_v2.c diff --git a/include/ugv_sdk/details/interface/bunker_interface.hpp b/include/ugv_sdk/details/interface/bunker_interface.hpp index 6b5f68e1..a4ac4c74 100644 --- a/include/ugv_sdk/details/interface/bunker_interface.hpp +++ b/include/ugv_sdk/details/interface/bunker_interface.hpp @@ -34,6 +34,12 @@ struct BunkerActuatorState { ActuatorStateMessageV1 actuator_state[2]; }; +struct BunkerCommonSensorState { + SdkTimePoint time_stamp; + + BmsBasicMessage bms_basic_state; + BmsExtendedMessage bms_extended_state; +}; struct BunkerInterface { virtual ~BunkerInterface() = default; @@ -42,6 +48,8 @@ struct BunkerInterface { // get robot state virtual BunkerCoreState GetRobotState() = 0; virtual BunkerActuatorState GetActuatorState() = 0; + virtual BunkerCommonSensorState GetCommonSensorState() = 0; + }; } // namespace westonrobot diff --git a/include/ugv_sdk/details/interface/ranger_interface.hpp b/include/ugv_sdk/details/interface/ranger_interface.hpp index 00a8c720..51b49fcc 100644 --- a/include/ugv_sdk/details/interface/ranger_interface.hpp +++ b/include/ugv_sdk/details/interface/ranger_interface.hpp @@ -43,6 +43,8 @@ struct RangerCommonSensorState { SdkTimePoint time_stamp; BmsBasicMessage bms_basic_state; + BmsExtendedMessage bms_extended_state; + }; ///////////////////////////////////////////////////////////////////////// diff --git a/include/ugv_sdk/details/interface/scout_interface.hpp b/include/ugv_sdk/details/interface/scout_interface.hpp index 38733df6..8687ff9a 100644 --- a/include/ugv_sdk/details/interface/scout_interface.hpp +++ b/include/ugv_sdk/details/interface/scout_interface.hpp @@ -40,6 +40,7 @@ struct ScoutCommonSensorState { SdkTimePoint time_stamp; BmsBasicMessage bms_basic_state; + BmsExtendedMessage bms_extended_state; }; struct ScoutInterface { diff --git a/include/ugv_sdk/details/interface/tracer_interface.hpp b/include/ugv_sdk/details/interface/tracer_interface.hpp index 58d794a0..160494c3 100644 --- a/include/ugv_sdk/details/interface/tracer_interface.hpp +++ b/include/ugv_sdk/details/interface/tracer_interface.hpp @@ -32,6 +32,12 @@ struct TracerActuatorState { ActuatorHSStateMessage actuator_hs_state[2]; ActuatorLSStateMessage actuator_ls_state[2]; }; +struct TracerCommonSensorState { + SdkTimePoint time_stamp; + + BmsBasicMessage bms_basic_state; + BmsExtendedMessage bms_extended_state; +}; struct TracerInterface { virtual ~TracerInterface() = default; @@ -43,6 +49,8 @@ struct TracerInterface { // get robot state virtual TracerCoreState GetRobotState() = 0; virtual TracerActuatorState GetActuatorState() = 0; + virtual TracerCommonSensorState GetCommonSensorState() = 0; + }; } // namespace westonrobot 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 a7e01add..da3d1dd6 100644 --- a/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp +++ b/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp @@ -76,6 +76,7 @@ using ScoutMiniProtocolV1Parser = ProtocolV1Parser; using BunkerProtocolV1Parser = ProtocolV1Parser; using HunterProtocolV1Parser = ProtocolV1Parser; +using TracerProtocolV1Parser = ProtocolV1Parser; } // 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 e0120ef3..94b7e9a1 100644 --- a/include/ugv_sdk/details/robot_base/bunker_base.hpp +++ b/include/ugv_sdk/details/robot_base/bunker_base.hpp @@ -60,6 +60,20 @@ class BunkerBase : public AgilexBase, public BunkerInterface { } return bunker_actuator; } + + BunkerCommonSensorState GetCommonSensorState() override { + auto common_sensor = + AgilexBase::GetCommonSensorStateMsgGroup(); + + BunkerCommonSensorState bunker_bms; + + bunker_bms.time_stamp = common_sensor.time_stamp; + bunker_bms.bms_basic_state = common_sensor.bms_basic_state; + + return bunker_bms; + } + + }; } // namespace westonrobot diff --git a/include/ugv_sdk/details/robot_base/ranger_base.hpp b/include/ugv_sdk/details/robot_base/ranger_base.hpp index 0d2a16d8..3df09f46 100644 --- a/include/ugv_sdk/details/robot_base/ranger_base.hpp +++ b/include/ugv_sdk/details/robot_base/ranger_base.hpp @@ -1,215 +1,229 @@ -/** - * @Kit : Qt-Creator: Desktop - * @Author : Wang Zhe - * @Date : 2021-04-19 19:12:52 - * @FileName : ranger_base.hpp - * @Mail : wangzheqie@qq.com - * Copyright : AgileX Robotics - **/ - #ifndef RANGER_BASE_HPP -#define RANGER_BASE_HPP - -#include -#include -#include -#include - -#include "ugv_sdk/details/interface/ranger_interface.hpp" -#include "ugv_sdk/details/robot_base/agilex_base.hpp" - -#include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp" - -namespace westonrobot { -class RangerBaseV2 : public AgilexBase, - public RangerInterface { - public: - RangerBaseV2() : AgilexBase(){}; - virtual ~RangerBaseV2() = default; - - // set up connection - bool Connect(std::string can_name) override { - return AgilexBase::Connect(can_name); - } - - // robot control - void SetMotionMode(uint8_t mode) { AgilexBase::SetMotionMode(mode); } - - void SetMotionCommand(double linear_vel, double steer_angle, - double angular_vel = 0.0) override { - AgilexBase::SendMotionCommand(linear_vel, angular_vel, - 0.0, steer_angle); - } - - void SetLightCommand(AgxLightMode f_mode, uint8_t f_value, - AgxLightMode r_mode, uint8_t r_value) override { - AgilexBase::SendLightCommand(f_mode, f_value, r_mode, - r_value); - } - - void DisableLightControl() override { - AgilexBase::DisableLightControl(); - } - - // get robot state - RangerCoreState GetRobotState() override { - auto state = AgilexBase::GetRobotCoreStateMsgGroup(); - - RangerCoreState ranger_state; - ranger_state.time_stamp = state.time_stamp; - ranger_state.system_state = state.system_state; - ranger_state.motion_state = state.motion_state; - ranger_state.light_state = state.light_state; - ranger_state.rc_state = state.rc_state; - ranger_state.motion_mode_state = state.motion_mode_state; - - return ranger_state; - } - - RangerActuatorState GetActuatorState() override { - auto actuator = AgilexBase::GetActuatorStateMsgGroup(); - - RangerActuatorState ranger_actuator; - ranger_actuator.time_stamp = actuator.time_stamp; - - ranger_actuator.motor_speeds.speed_1 = actuator.motor_speeds.speed_1; - ranger_actuator.motor_speeds.speed_2 = actuator.motor_speeds.speed_2; - ranger_actuator.motor_speeds.speed_3 = actuator.motor_speeds.speed_3; - ranger_actuator.motor_speeds.speed_4 = actuator.motor_speeds.speed_4; - ranger_actuator.motor_angles.angle_5 = actuator.motor_angles.angle_5; - ranger_actuator.motor_angles.angle_6 = actuator.motor_angles.angle_6; - ranger_actuator.motor_angles.angle_7 = actuator.motor_angles.angle_7; - ranger_actuator.motor_angles.angle_8 = actuator.motor_angles.angle_8; - - for (int i = 0; i < 8; ++i) { - ranger_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i]; - ranger_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i]; - } - return ranger_actuator; - } - - RangerCommonSensorState GetCommonSensorState() override { - auto common_sensor = - AgilexBase::GetCommonSensorStateMsgGroup(); - - RangerCommonSensorState 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 -// Here we provide a work-around fix as no new firmware will be provided from -// AgileX to properly fix the issue. -class RangerMiniV1Base : public RangerBaseV2 { - public: - RangerMiniV1Base() : RangerBaseV2(){}; - ~RangerMiniV1Base() = default; - - // robot control - void SetMotionMode(uint8_t mode) override { - if (mode == RangerInterface::MotionMode::kPark) { - return; - } else if (mode == RangerInterface::MotionMode::kSideSlip) { - mode = 3; - } - AgilexBase::SetMotionMode(mode); - } - - void SetMotionCommand(double linear_vel, double steer_angle, - double angular_vel) override { - auto state = GetRobotState(); - if (state.motion_mode_state.motion_mode == - RangerInterface::MotionMode::kSpinning) { - angular_vel *= 0.254558; - } - AgilexBase::SendMotionCommand( - linear_vel, 0.0, -angular_vel, -steer_angle / 10.0 / 3.14 * 180); - } - - RangerCoreState GetRobotState() override { - auto state = AgilexBase::GetRobotCoreStateMsgGroup(); - - RangerCoreState ranger_state; - ranger_state.time_stamp = state.time_stamp; - ranger_state.system_state = state.system_state; - - ranger_state.light_state = state.light_state; - ranger_state.rc_state = state.rc_state; - ranger_state.motion_mode_state = state.motion_mode_state; - - if (ranger_state.motion_mode_state.motion_mode == - RangerInterface::MotionMode::kSpinning) { - ranger_state.motion_state.linear_velocity = 0; - ranger_state.motion_state.angular_velocity = - -state.motion_state.linear_velocity / 0.254558; - ranger_state.motion_state.lateral_velocity = - state.motion_state.lateral_velocity; - ranger_state.motion_state.steering_angle = - -state.motion_state.steering_angle * 10 / 180.0 * 3.14; - } else if (ranger_state.motion_mode_state.motion_mode == - RangerInterface::MotionMode::kSideSlip) { - ranger_state.motion_state.linear_velocity = - -state.motion_state.linear_velocity; - state.motion_state.angular_velocity; - ranger_state.motion_state.lateral_velocity = - state.motion_state.lateral_velocity; - ranger_state.motion_state.steering_angle = - -state.motion_state.steering_angle * 10 / 180.0 * 3.14; - } else { - ranger_state.motion_state.linear_velocity = - state.motion_state.linear_velocity; - ranger_state.motion_state.angular_velocity = - -state.motion_state.angular_velocity; - ranger_state.motion_state.lateral_velocity = - state.motion_state.lateral_velocity; - ranger_state.motion_state.steering_angle = - -state.motion_state.steering_angle * 10 / 180.0 * 3.14; - } - - return ranger_state; - } - - RangerActuatorState GetActuatorState() override { - auto actuator = AgilexBase::GetActuatorStateMsgGroup(); - - RangerActuatorState ranger_actuator; - ranger_actuator.time_stamp = actuator.time_stamp; - - ranger_actuator.motor_speeds.speed_1 = actuator.motor_speeds.speed_1; - ranger_actuator.motor_speeds.speed_2 = actuator.motor_speeds.speed_2; - ranger_actuator.motor_speeds.speed_3 = actuator.motor_speeds.speed_3; - ranger_actuator.motor_speeds.speed_4 = actuator.motor_speeds.speed_4; - ranger_actuator.motor_angles.angle_5 = - -actuator.motor_angles.angle_5 / 18.0 * M_PI; - ranger_actuator.motor_angles.angle_6 = - -actuator.motor_angles.angle_6 / 18.0 * M_PI; - ranger_actuator.motor_angles.angle_7 = - -actuator.motor_angles.angle_7 / 18.0 * M_PI; - ranger_actuator.motor_angles.angle_8 = - -actuator.motor_angles.angle_8 / 18.0 * M_PI; - - for (int i = 0; i < 8; ++i) { - ranger_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i]; - ranger_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i]; - } - return ranger_actuator; - } -}; -} // namespace westonrobot -#endif /* RANGER_BASE_HPP */ + #define RANGER_BASE_HPP + + #include + #include + #include + #include + + #include "ugv_sdk/details/interface/ranger_interface.hpp" + #include "ugv_sdk/details/robot_base/agilex_base.hpp" + + #include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp" + + namespace westonrobot { + class RangerBase : public AgilexBase, public RangerInterface { + public: + RangerBase() : AgilexBase() {}; + virtual ~RangerBase() = default; + + // set up connection + bool Connect(std::string can_name) override { + return AgilexBase::Connect(can_name); + } + + // robot control + void SetMotionMode(uint8_t mode) { AgilexBase::SetMotionMode(mode); } + + void SetMotionCommand(double linear_vel, double steer_angle, + double angular_vel = 0.0) override { + AgilexBase::SendMotionCommand(linear_vel, angular_vel, + 0.0, steer_angle); + } + + void SetLightCommand(AgxLightMode f_mode, uint8_t f_value, + AgxLightMode r_mode, uint8_t r_value) override { + AgilexBase::SendLightCommand(f_mode, f_value, r_mode, + r_value); + } + + void DisableLightControl() override { + AgilexBase::DisableLightControl(); + } + + // get robot state + RangerCoreState GetRobotState() override { + auto state = AgilexBase::GetRobotCoreStateMsgGroup(); + + RangerCoreState ranger_state; + ranger_state.time_stamp = state.time_stamp; + ranger_state.system_state = state.system_state; + ranger_state.motion_state = state.motion_state; + ranger_state.light_state = state.light_state; + ranger_state.rc_state = state.rc_state; + ranger_state.motion_mode_state = state.motion_mode_state; + + return ranger_state; + } + + RangerActuatorState GetActuatorState() override { + auto actuator = AgilexBase::GetActuatorStateMsgGroup(); + + RangerActuatorState ranger_actuator; + ranger_actuator.time_stamp = actuator.time_stamp; + + ranger_actuator.motor_speeds.speed_1 = actuator.motor_speeds.speed_1; + ranger_actuator.motor_speeds.speed_2 = actuator.motor_speeds.speed_2; + ranger_actuator.motor_speeds.speed_3 = actuator.motor_speeds.speed_3; + ranger_actuator.motor_speeds.speed_4 = actuator.motor_speeds.speed_4; + ranger_actuator.motor_angles.angle_5 = actuator.motor_angles.angle_5; + ranger_actuator.motor_angles.angle_6 = actuator.motor_angles.angle_6; + ranger_actuator.motor_angles.angle_7 = actuator.motor_angles.angle_7; + ranger_actuator.motor_angles.angle_8 = actuator.motor_angles.angle_8; + + for (int i = 0; i < 8; ++i) { + ranger_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i]; + ranger_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i]; + } + return ranger_actuator; + } + + RangerCommonSensorState GetCommonSensorState() override { + auto common_sensor = + AgilexBase::GetCommonSensorStateMsgGroup(); + + RangerCommonSensorState ranger_bms; + + ranger_bms.time_stamp = common_sensor.time_stamp; + + ranger_bms.bms_basic_state.current = common_sensor.bms_basic_state.current; + ranger_bms.bms_basic_state.voltage = + common_sensor.bms_basic_state.voltage; + 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; + } + }; + + using RangerMiniV3Base = RangerBase; + class RangerMiniV2Base : public RangerBase { + RangerCommonSensorState GetCommonSensorState() override { + auto common_sensor = + AgilexBase::GetCommonSensorStateMsgGroup(); + + RangerCommonSensorState 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. + // RM2 BMS voltage data follows unit: 0.01V + 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 + // Here we provide a work-around fix as no new firmware will be provided from + // AgileX to properly fix the issue. + class RangerMiniV1Base : public RangerBase { + public: + RangerMiniV1Base() : RangerBase() {}; + ~RangerMiniV1Base() = default; + + // robot control + void SetMotionMode(uint8_t mode) override { + if (mode == RangerInterface::MotionMode::kPark) { + return; + } else if (mode == RangerInterface::MotionMode::kSideSlip) { + mode = 3; + } + AgilexBase::SetMotionMode(mode); + } + + void SetMotionCommand(double linear_vel, double steer_angle, + double angular_vel) override { + auto state = GetRobotState(); + if (state.motion_mode_state.motion_mode == + RangerInterface::MotionMode::kSpinning) { + angular_vel *= 0.254558; + } + AgilexBase::SendMotionCommand( + linear_vel, 0.0, -angular_vel, -steer_angle / 10.0 / 3.14 * 180); + } + + RangerCoreState GetRobotState() override { + auto state = AgilexBase::GetRobotCoreStateMsgGroup(); + + RangerCoreState ranger_state; + ranger_state.time_stamp = state.time_stamp; + ranger_state.system_state = state.system_state; + + ranger_state.light_state = state.light_state; + ranger_state.rc_state = state.rc_state; + ranger_state.motion_mode_state = state.motion_mode_state; + + if (ranger_state.motion_mode_state.motion_mode == + RangerInterface::MotionMode::kSpinning) { + ranger_state.motion_state.linear_velocity = 0; + ranger_state.motion_state.angular_velocity = + -state.motion_state.linear_velocity / 0.254558; + ranger_state.motion_state.lateral_velocity = + state.motion_state.lateral_velocity; + ranger_state.motion_state.steering_angle = + -state.motion_state.steering_angle * 10 / 180.0 * 3.14; + } else if (ranger_state.motion_mode_state.motion_mode == + RangerInterface::MotionMode::kSideSlip) { + ranger_state.motion_state.linear_velocity = + -state.motion_state.linear_velocity; + state.motion_state.angular_velocity; + ranger_state.motion_state.lateral_velocity = + state.motion_state.lateral_velocity; + ranger_state.motion_state.steering_angle = + -state.motion_state.steering_angle * 10 / 180.0 * 3.14; + } else { + ranger_state.motion_state.linear_velocity = + state.motion_state.linear_velocity; + ranger_state.motion_state.angular_velocity = + -state.motion_state.angular_velocity; + ranger_state.motion_state.lateral_velocity = + state.motion_state.lateral_velocity; + ranger_state.motion_state.steering_angle = + -state.motion_state.steering_angle * 10 / 180.0 * 3.14; + } + + return ranger_state; + } + + RangerActuatorState GetActuatorState() override { + auto actuator = AgilexBase::GetActuatorStateMsgGroup(); + + RangerActuatorState ranger_actuator; + ranger_actuator.time_stamp = actuator.time_stamp; + + ranger_actuator.motor_speeds.speed_1 = actuator.motor_speeds.speed_1; + ranger_actuator.motor_speeds.speed_2 = actuator.motor_speeds.speed_2; + ranger_actuator.motor_speeds.speed_3 = actuator.motor_speeds.speed_3; + ranger_actuator.motor_speeds.speed_4 = actuator.motor_speeds.speed_4; + ranger_actuator.motor_angles.angle_5 = + -actuator.motor_angles.angle_5 / 18.0 * M_PI; + ranger_actuator.motor_angles.angle_6 = + -actuator.motor_angles.angle_6 / 18.0 * M_PI; + ranger_actuator.motor_angles.angle_7 = + -actuator.motor_angles.angle_7 / 18.0 * M_PI; + ranger_actuator.motor_angles.angle_8 = + -actuator.motor_angles.angle_8 / 18.0 * M_PI; + + for (int i = 0; i < 8; ++i) { + ranger_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i]; + ranger_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i]; + } + return ranger_actuator; + } + }; + } // namespace westonrobot + #endif /* RANGER_BASE_HPP */ + \ No newline at end of file diff --git a/include/ugv_sdk/details/robot_base/tracer_base.hpp b/include/ugv_sdk/details/robot_base/tracer_base.hpp index 6061e54c..0492406d 100644 --- a/include/ugv_sdk/details/robot_base/tracer_base.hpp +++ b/include/ugv_sdk/details/robot_base/tracer_base.hpp @@ -21,35 +21,36 @@ #include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp" namespace westonrobot { -class TracerBaseV2 : public AgilexBase, +template +class TracerBase : public AgilexBase, public TracerInterface { public: - TracerBaseV2() : AgilexBase(){}; - ~TracerBaseV2() = default; + TracerBase() : AgilexBase(){}; + ~TracerBase() = default; // set up connection bool Connect(std::string can_name) override { - return AgilexBase::Connect(can_name); + return AgilexBase::Connect(can_name); } // robot control void SetMotionCommand(double linear_vel, double angular_vel) override { - AgilexBase::SendMotionCommand(linear_vel, angular_vel, + AgilexBase::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0); } void SetLightCommand(AgxLightMode f_mode, uint8_t f_value) override { - AgilexBase::SendLightCommand(f_mode, f_value, CONST_OFF, + AgilexBase::SendLightCommand(f_mode, f_value, CONST_OFF, 0); } void DisableLightControl() override { - AgilexBase::DisableLightControl(); + AgilexBase::DisableLightControl(); } // get robot state TracerCoreState GetRobotState() override { - auto state = AgilexBase::GetRobotCoreStateMsgGroup(); + auto state = AgilexBase::GetRobotCoreStateMsgGroup(); TracerCoreState tracer_state; tracer_state.time_stamp = state.time_stamp; @@ -61,7 +62,7 @@ class TracerBaseV2 : public AgilexBase, } TracerActuatorState GetActuatorState() override { - auto actuator = AgilexBase::GetActuatorStateMsgGroup(); + auto actuator = AgilexBase::GetActuatorStateMsgGroup(); TracerActuatorState tracer_actuator; tracer_actuator.time_stamp = actuator.time_stamp; @@ -72,10 +73,31 @@ class TracerBaseV2 : public AgilexBase, return tracer_actuator; } + TracerCommonSensorState GetCommonSensorState() override { + auto common_sensor = + AgilexBase::GetCommonSensorStateMsgGroup(); + + TracerCommonSensorState tracer_bms; + + tracer_bms.time_stamp = common_sensor.time_stamp; + tracer_bms.bms_basic_state = common_sensor.bms_basic_state; + + return tracer_bms; + } + + void ResetRobotState() override { // TODO } }; } // namespace westonrobot +#include "ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp" +#include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp" + +namespace westonrobot { +using TracerBaseV1 = TracerBase; +using TracerBaseV2 = TracerBase; +} // namespace westonrobot + #endif /* TRACER_BASE_HPP */ diff --git a/include/ugv_sdk/mobile_robot/bunker_robot.hpp b/include/ugv_sdk/mobile_robot/bunker_robot.hpp index 2f56bca2..f4285139 100644 --- a/include/ugv_sdk/mobile_robot/bunker_robot.hpp +++ b/include/ugv_sdk/mobile_robot/bunker_robot.hpp @@ -36,6 +36,7 @@ class BunkerRobot : public RobotCommonInterface, public BunkerInterface { // get robot state BunkerCoreState GetRobotState() override; BunkerActuatorState GetActuatorState() override; + BunkerCommonSensorState GetCommonSensorState() 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 62ad4294..98fcbe6a 100644 --- a/include/ugv_sdk/mobile_robot/ranger_robot.hpp +++ b/include/ugv_sdk/mobile_robot/ranger_robot.hpp @@ -17,13 +17,21 @@ namespace westonrobot { class RangerRobot : public RobotCommonInterface, public RangerInterface { public: - RangerRobot(bool is_mini_v1); + enum class Variant { + kRangerMiniV1 = 0, + kRangerMiniV2, + kRangerMiniV3, + kRanger, +}; + + RangerRobot(Variant variant); + ~RangerRobot(); bool Connect(std::string can_name) override; void EnableCommandedMode() override; - std::string RequestVersion(int timeout_sec) override; + std::string RequestVersion(int timeout_sec = 3) override; // functions to be implemented by each robot class void ResetRobotState() override; diff --git a/include/ugv_sdk/mobile_robot/titan_robot.hpp b/include/ugv_sdk/mobile_robot/titan_robot.hpp index 606a4615..a890ed5b 100644 --- a/include/ugv_sdk/mobile_robot/titan_robot.hpp +++ b/include/ugv_sdk/mobile_robot/titan_robot.hpp @@ -46,4 +46,5 @@ class TitanRobot : public RobotCommonInterface, public TitanInterface { }; } // namespace westonrobot + #endif /* TITAN_ROBOT_HPP */ diff --git a/include/ugv_sdk/mobile_robot/tracer_robot.hpp b/include/ugv_sdk/mobile_robot/tracer_robot.hpp index 72a419b4..9f1d2efa 100644 --- a/include/ugv_sdk/mobile_robot/tracer_robot.hpp +++ b/include/ugv_sdk/mobile_robot/tracer_robot.hpp @@ -1,19 +1,48 @@ /* - * tracer_robot.hpp + * scout_robot.hpp * - * Created on: Jul 13, 2021 21:59 + * Created on: Jul 08, 2021 10:59 * Description: * * Copyright (c) 2021 Weston Robot Pte. Ltd. */ -#ifndef TRACER_ROBOT_HPP -#define TRACER_ROBOT_HPP +#ifndef SCOUT_ROBOT_HPP +#define SCOUT_ROBOT_HPP -#include "ugv_sdk/details/robot_base/tracer_base.hpp" +#include + +#include "ugv_sdk/details/interface/robot_common_interface.hpp" +#include "ugv_sdk/details/interface/tracer_interface.hpp" namespace westonrobot { -using TracerRobot = TracerBaseV2; +class TracerRobot : public RobotCommonInterface, public TracerInterface { + public: + TracerRobot(ProtocolVersion protocol = ProtocolVersion::AGX_V2); + ~TracerRobot(); + + bool Connect(std::string can_name) override; + + void EnableCommandedMode() override; + std::string RequestVersion(int timeout_sec = 3) override; + + void SetMotionCommand(double linear_vel, double angular_vel) override; + void SetLightCommand(AgxLightMode f_mode, uint8_t f_value) override; + void DisableLightControl() override; + + void ResetRobotState() override; + + ProtocolVersion GetParserProtocolVersion() override; + + // get robot state + TracerCoreState GetRobotState() override; + TracerActuatorState GetActuatorState() override; + TracerCommonSensorState GetCommonSensorState() override; + + protected: + RobotCommonInterface* robot_; +}; + } // namespace westonrobot -#endif /* TRACER_ROBOT_HPP */ +#endif /* SCOUT_ROBOT_HPP */ diff --git a/sample/ranger_demo/ranger_robot_demo.cpp b/sample/ranger_demo/ranger_robot_demo.cpp index 55facea6..68849dbb 100644 --- a/sample/ranger_demo/ranger_robot_demo.cpp +++ b/sample/ranger_demo/ranger_robot_demo.cpp @@ -6,6 +6,7 @@ * @Mail : wangzheqie@qq.com * Copyright : AgileX Robotics **/ +#include #include @@ -15,7 +16,7 @@ using namespace westonrobot; int main(int argc, char *argv[]) { std::string device_name; - bool is_mini_v1 = false; + RangerRobot::Variant variant = RangerRobot::Variant::kRanger; if (argc == 2) { device_name = {argv[1]}; @@ -24,19 +25,28 @@ int main(int argc, char *argv[]) { device_name = {argv[1]}; std::string check = argv[2]; if (check == "mini_v1") { - is_mini_v1 = true; + variant = RangerRobot::Variant::kRangerMiniV1; std::cout << "Specified mini v1" << std::endl; + } else if (check == "mini_v2") { + variant = RangerRobot::Variant::kRangerMiniV2; + std::cout << "Specified mini v2" << std::endl; + } else if (check == "mini_v3") { + variant = RangerRobot::Variant::kRangerMiniV3; + std::cout << "Specified mini v3" << std::endl; + } else { + std::cout << "Default to ranger base" << std::endl; } std::cout << "Specified CAN: " << device_name << std::endl; } else { std::cout << "Usage: app_ranger_demo " << std::endl - << "Example 1: ./app_ranger_demo can0 mini_v1" << std::endl; + << "Example 1: ./app_ranger_demo can0 " + << std::endl; return -1; } // RangerMiniV1Robot ranger; - auto ranger = std::make_shared(is_mini_v1); + auto ranger = std::make_shared(variant); ranger->Connect(device_name); ranger->EnableCommandedMode(); @@ -92,4 +102,4 @@ int main(int argc, char *argv[]) { ++count; } return 0; -} +} \ No newline at end of file diff --git a/sample/tracer_demo/tracer_robot_demo.cpp b/sample/tracer_demo/tracer_robot_demo.cpp index e0447256..ae3db49e 100644 --- a/sample/tracer_demo/tracer_robot_demo.cpp +++ b/sample/tracer_demo/tracer_robot_demo.cpp @@ -31,7 +31,6 @@ int main(int argc, char **argv) { } std::unique_ptr tracer; - tracer = std::unique_ptr(new TracerRobot()); if (tracer == nullptr) std::cout << "Failed to create robot object" << std::endl; diff --git a/src/mobile_robot/bunker_robot.cpp b/src/mobile_robot/bunker_robot.cpp index b90d0ad3..986c3e86 100644 --- a/src/mobile_robot/bunker_robot.cpp +++ b/src/mobile_robot/bunker_robot.cpp @@ -53,4 +53,10 @@ BunkerActuatorState BunkerRobot::GetActuatorState() { auto bunker = dynamic_cast(robot_); return bunker->GetActuatorState(); } + +BunkerCommonSensorState BunkerRobot::GetCommonSensorState() { + auto bunker = dynamic_cast(robot_); + return bunker->GetCommonSensorState(); +} + } // 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 8e585e7d..c0987fd6 100644 --- a/src/mobile_robot/ranger_robot.cpp +++ b/src/mobile_robot/ranger_robot.cpp @@ -10,13 +10,17 @@ #include "ugv_sdk/details/robot_base/ranger_base.hpp" namespace westonrobot { -RangerRobot::RangerRobot(bool is_mini_v1) { - if (is_mini_v1) { - robot_ = new RangerMiniV1Base(); - } else { - robot_ = new RangerBaseV2(); + RangerRobot::RangerRobot(Variant variant) { + if (variant == Variant::kRangerMiniV1) { + robot_ = new RangerMiniV1Base(); + } else if (variant == Variant::kRangerMiniV2) { + robot_ = new RangerMiniV2Base(); + } else if (variant == Variant::kRangerMiniV3) { + robot_ = new RangerMiniV3Base(); + } else { + robot_ = new RangerBase(); + } } -} RangerRobot::~RangerRobot() { if (robot_) delete robot_; diff --git a/src/mobile_robot/tracer_robot.cpp b/src/mobile_robot/tracer_robot.cpp new file mode 100644 index 00000000..ec337778 --- /dev/null +++ b/src/mobile_robot/tracer_robot.cpp @@ -0,0 +1,72 @@ +/* + * Tracer_robot.cpp + * + * Created on: Jul 14, 2021 23:14 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#include "ugv_sdk/mobile_robot/tracer_robot.hpp" +#include "ugv_sdk/details/robot_base/tracer_base.hpp" + +namespace westonrobot { +TracerRobot::TracerRobot(ProtocolVersion protocol) { + if (protocol == ProtocolVersion::AGX_V1) { + robot_ = new TracerBaseV1(); + } else if (protocol == ProtocolVersion::AGX_V2) { + robot_ = new TracerBaseV2(); + } +} + +TracerRobot::~TracerRobot() { + if (robot_) delete robot_; +} + +std::string TracerRobot::RequestVersion(int timeout_sec) { + return robot_->RequestVersion(timeout_sec); +} + +void TracerRobot::EnableCommandedMode() { robot_->EnableCommandedMode(); } + +bool TracerRobot::Connect(std::string can_name) { + return robot_->Connect(can_name); +} + +void TracerRobot::ResetRobotState() { robot_->ResetRobotState(); } + +ProtocolVersion TracerRobot::GetParserProtocolVersion() { + return robot_->GetParserProtocolVersion(); +} + +void TracerRobot::SetMotionCommand(double linear_vel, double angular_vel) { + auto Tracer = dynamic_cast(robot_); + Tracer->SetMotionCommand(linear_vel, angular_vel); +} +void TracerRobot::SetLightCommand(AgxLightMode f_mode, uint8_t f_value) { + auto Tracer = dynamic_cast(robot_); + Tracer->SetLightCommand(f_mode, f_value); +} + +void TracerRobot::DisableLightControl() { + auto Tracer = dynamic_cast(robot_); + Tracer->DisableLightControl(); +} + + +TracerCoreState TracerRobot::GetRobotState() { + auto Tracer = dynamic_cast(robot_); + return Tracer->GetRobotState(); +} + +TracerActuatorState TracerRobot::GetActuatorState() { + auto Tracer = dynamic_cast(robot_); + return Tracer->GetActuatorState(); +} + +TracerCommonSensorState TracerRobot::GetCommonSensorState() { + auto scout = dynamic_cast(robot_); + return scout->GetCommonSensorState(); +} + +} // namespace westonrobot \ No newline at end of file diff --git a/src/protocol_v2/agilex_msg_parser_v2.c b/src/protocol_v2/agilex_msg_parser_v2.c index 13ec1654..25f150d5 100644 --- a/src/protocol_v2/agilex_msg_parser_v2.c +++ b/src/protocol_v2/agilex_msg_parser_v2.c @@ -153,7 +153,7 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) { msg->type = AgxMsgActuatorHSState; ActuatorHSStateFrame *frame = (ActuatorHSStateFrame *)(rx_frame->data); msg->body.actuator_hs_state_msg.motor_id = - rx_frame->can_id - CAN_MSG_ACTUATOR1_HS_STATE_ID; + rx_frame->can_id - CAN_MSG_ACTUATOR2_HS_STATE_ID; msg->body.actuator_hs_state_msg.rpm = (int16_t)((uint16_t)(frame->rpm.low_byte) | (uint16_t)(frame->rpm.high_byte) << 8); @@ -178,7 +178,7 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) { msg->type = AgxMsgActuatorLSState; ActuatorLSStateFrame *frame = (ActuatorLSStateFrame *)(rx_frame->data); msg->body.actuator_hs_state_msg.motor_id = - rx_frame->can_id - CAN_MSG_ACTUATOR1_LS_STATE_ID; + rx_frame->can_id - CAN_MSG_ACTUATOR2_LS_STATE_ID; msg->body.actuator_ls_state_msg.driver_voltage = ((uint16_t)(frame->driver_voltage.low_byte) | (uint16_t)(frame->driver_voltage.high_byte) << 8) *