Merge pull request #47 from agilexrobotics/devel

support titan robot

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

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 <string>
+
+#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 */

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 <cstdint>
+#include <mutex>
+#include <string>
+#include <thread>
+
+#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<ProtocolV2Parser>,
+                     public TitanInterface {
+ public:
+  TitanBase() : AgilexBase<ProtocolV2Parser>(){};
+  virtual ~TitanBase() = default;
+
+  // set up connection
+  bool Connect(std::string can_name) override {
+    return AgilexBase<ProtocolV2Parser>::Connect(can_name);
+  }
+
+  // robot control
+  void SetMotionCommand(double linear_vel, double steer_angle) override {
+    AgilexBase<ProtocolV2Parser>::SendMotionCommand(linear_vel, 0.0,
+                                                    0.0, steer_angle);
+  }
+
+  // get robot state
+  TitanCoreState GetRobotState() override {
+    auto state = AgilexBase<ProtocolV2Parser>::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<ProtocolV2Parser>::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<ProtocolV2Parser>::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<ProtocolV2Parser>::SetBrakeMode(AgxBrakeMode::BRAKE_MODE_UNLOCK);
+  }
+
+  void ReleaseBrake() override {
+    AgilexBase<ProtocolV2Parser>::SetBrakeMode(AgxBrakeMode::BRAKE_MODE_LOCK);
+  }
+
+};
+}  // namespace westonrobot
+#endif /* TITAN_BASE_HPP */

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 <cmath>
+
+#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 */

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<TitanInterface*>(robot_);
+  return titan->SetMotionCommand(linear_vel, steer_angle);
+}
+
+void TitanRobot::ActivateBrake() {
+  auto hunter = dynamic_cast<TitanInterface*>(robot_);
+  hunter->ActivateBrake();
+}
+
+void TitanRobot::ReleaseBrake() {
+  auto hunter = dynamic_cast<TitanInterface*>(robot_);
+  hunter->ReleaseBrake();
+}
+
+// get robot state
+TitanCoreState TitanRobot::GetRobotState() {
+  auto titan = dynamic_cast<TitanInterface*>(robot_);
+  return titan->GetRobotState();
+}
+
+TitanActuatorState TitanRobot::GetActuatorState() {
+  auto titan = dynamic_cast<TitanInterface*>(robot_);
+  return titan->GetActuatorState();
+}
+TitanCommonSensorState TitanRobot::GetCommonSensorState() {
+  auto titan = dynamic_cast<TitanInterface*>(robot_);
+  return titan->GetCommonSensorState();
+}
+}  // namespace westonrobot