From d234d8caa17fecaa64b02acbce9a7fe6df25d113 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Wed, 14 Jul 2021 23:00:51 +0800 Subject: [PATCH] updated ranger base --- .editorconfig | 0 demo/CMakeLists.txt | 1 + demo/ranger_demo/CMakeLists.txt | 2 + demo/ranger_demo/ranger_demo.cpp | 6 +- .../details/interface/ranger_interface.hpp | 42 ++----- .../details/interface/robot_interface.hpp | 8 +- .../protocol_v1/protocol_v1_parser.hpp | 6 +- .../details/robot_base/ranger_base.cpp | 111 ------------------ .../details/robot_base/ranger_base.hpp | 59 ++++++++-- include/ugv_sdk/mobile_robot/ranger_robot.hpp | 19 +++ 10 files changed, 92 insertions(+), 162 deletions(-) create mode 100644 .editorconfig create mode 100644 demo/ranger_demo/CMakeLists.txt delete mode 100644 include/ugv_sdk/details/robot_base/ranger_base.cpp create mode 100644 include/ugv_sdk/mobile_robot/ranger_robot.hpp diff --git a/.editorconfig b/.editorconfig new file mode 100644 index 0000000..e69de29 diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index ba568fd..da97880 100755 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -1,6 +1,7 @@ # demo add_subdirectory(scout_demo) add_subdirectory(tracer_demo) +add_subdirectory(ranger_demo) # add_executable(app_hunter_demo hunter_demo/hunter_demo.cpp) # target_link_libraries(app_hunter_demo ugv_sdk) diff --git a/demo/ranger_demo/CMakeLists.txt b/demo/ranger_demo/CMakeLists.txt new file mode 100644 index 0000000..c2ba6c7 --- /dev/null +++ b/demo/ranger_demo/CMakeLists.txt @@ -0,0 +1,2 @@ +add_executable(demo_ranger_robot ranger_demo.cpp) +target_link_libraries(demo_ranger_robot ugv_sdk) \ No newline at end of file diff --git a/demo/ranger_demo/ranger_demo.cpp b/demo/ranger_demo/ranger_demo.cpp index 5a1be9c..901345c 100644 --- a/demo/ranger_demo/ranger_demo.cpp +++ b/demo/ranger_demo/ranger_demo.cpp @@ -7,7 +7,7 @@ * Copyright : AgileX Robotics **/ -#include "ugv_sdk/protocol_v2/ranger_base.hpp" +#include "ugv_sdk/mobile_robot/ranger_robot.hpp" using namespace westonrobot; @@ -23,7 +23,7 @@ int main(int argc, char *argv[]) { return -1; } - RangerBase ranger; + RangerRobot ranger; ranger.Connect(device_name); ranger.EnableCommandedMode(); @@ -35,7 +35,7 @@ int main(int argc, char *argv[]) { ranger.SetMotionCommand(0.2, 0.0); } - auto state = ranger.GetRangerState(); + auto state = ranger.GetRobotState(); std::cout << "-------------------------------" << std::endl; std::cout << "count: " << count << std::endl; std::cout << "control mode: " diff --git a/include/ugv_sdk/details/interface/ranger_interface.hpp b/include/ugv_sdk/details/interface/ranger_interface.hpp index 34a8694..60ad026 100644 --- a/include/ugv_sdk/details/interface/ranger_interface.hpp +++ b/include/ugv_sdk/details/interface/ranger_interface.hpp @@ -12,45 +12,23 @@ #include -#include "ugv_sdk/interface/agilex_message.h" +#include "ugv_sdk/details/interface/agilex_message.h" namespace westonrobot { -struct RangerState { +struct RangerCoreState { // system state SystemStateMessage system_state; MotionStateMessage motion_state; LightStateMessage light_state; - - RcStateMessage rc_state; - - ActuatorHSStateMessage actuator_hs_state[8]; - ActuatorLSStateMessage actuator_ls_state[8]; MotionModeStateMessage current_motion_mode; - // sensor data + RcStateMessage rc_state; OdometryMessage odometry; }; -struct RangerMotionCmd { - double linear_velocity; - double angular_velocity; -}; - -struct RangerLightCmd { - RangerLightCmd() = default; - RangerLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, - uint8_t r_value) - : enable_cmd_ctrl(true), - front_mode(f_mode), - front_custom_value(f_value), - rear_mode(r_mode), - rear_custom_value(r_value) {} - - bool enable_cmd_ctrl = false; - LightMode front_mode; - uint8_t front_custom_value; - LightMode rear_mode; - uint8_t rear_custom_value; +struct RangerActuatorState { + ActuatorHSStateMessage actuator_hs_state[8]; + ActuatorLSStateMessage actuator_ls_state[8]; }; ///////////////////////////////////////////////////////////////////////// @@ -59,13 +37,15 @@ struct RangerInterface { virtual void Connect(std::string dev_name) = 0; // robot control + virtual void SetMotionMode(uint8_t mode) = 0; virtual void SetMotionCommand(double linear_vel, double steer_angle, double lateral_vel, double angular_vel) = 0; - virtual void SetLightCommand(const RangerLightCmd &cmd) = 0; - virtual void SetMotionMode(uint8_t mode) = 0; + virtual void SetLightCommand(LightMode f_mode, uint8_t f_value, + LightMode r_mode, uint8_t r_value) = 0; // get robot state - virtual RangerState GetRangerState() = 0; + virtual RangerCoreState GetRobotState() = 0; + virtual RangerActuatorState GetActuatorState() = 0; }; } // namespace westonrobot diff --git a/include/ugv_sdk/details/interface/robot_interface.hpp b/include/ugv_sdk/details/interface/robot_interface.hpp index 79f3d9e..69014c8 100644 --- a/include/ugv_sdk/details/interface/robot_interface.hpp +++ b/include/ugv_sdk/details/interface/robot_interface.hpp @@ -56,8 +56,12 @@ class RobotInterface { /****** functions not available/valid to all robots ******/ // functions to be implemented by class AgilexBase virtual void SetMotionMode(uint8_t mode){}; - virtual CoreStateMsgGroup GetRobotCoreStateMsgGroup(){}; - virtual ActuatorStateMsgGroup GetActuatorStateMsgGroup(){}; + virtual CoreStateMsgGroup GetRobotCoreStateMsgGroup() { + return CoreStateMsgGroup{}; + }; + virtual ActuatorStateMsgGroup GetActuatorStateMsgGroup() { + return ActuatorStateMsgGroup{}; + }; // any specific robot will use a specialized version of the two functions virtual void SendMotionCommand(double linear_vel, double angular_vel, 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 4cc395c..1759954 100644 --- a/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp +++ b/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp @@ -52,12 +52,14 @@ class ProtocolV1Parser : public ParserInterface { } // UART support - bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg) override {} + bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg) override { + return false; + } void EncodeMessage(const AgxMessage *msg, uint8_t *buf, uint8_t *len) override {} - uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) override {} + uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) override { return 0; } }; using ScoutProtocolV1Parser = ProtocolV1Parser; diff --git a/include/ugv_sdk/details/robot_base/ranger_base.cpp b/include/ugv_sdk/details/robot_base/ranger_base.cpp deleted file mode 100644 index eca8a20..0000000 --- a/include/ugv_sdk/details/robot_base/ranger_base.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/** - * @Kit : Qt-Creator: Desktop - * @Author : Wang Zhe - * @Date : 2021-04-19 19:36:24 - * @FileName : ranger_base.cpp - * @Mail : wangzheqie@qq.com - * Copyright : AgileX Robotics - **/ - -#include "ugv_sdk/protocol_v2/ranger_base.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "stopwatch.hpp" - -#include "ugv_sdk/protocol_v2/agilex_msg_parser.h" - -namespace westonrobot { -void RangerBase::Connect(std::string dev_name) { - AgilexBase::ConnectPort(dev_name, std::bind(&RangerBase::ParseCANFrame, this, - std::placeholders::_1)); -} - -void RangerBase::SetMotionCommand(double linear_vel, double steer_angle, - double lateral_vel, double angular_vel) { - AgilexBase::SetMotionCommand(linear_vel, angular_vel, lateral_vel, - steer_angle / 10.0); -} - -void RangerBase::SetLightCommand(const RangerLightCmd &cmd) { - if (cmd.enable_cmd_ctrl) { - AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value, - LightMode::CONST_OFF, 0); - } -} - -void RangerBase::SetMotionMode(uint8_t mode) { - AgilexBase::SetMotionMode(mode); -} - -RangerState RangerBase::GetRangerState() { - std::lock_guard guard(state_mutex_); - return ranger_state_; -} - -void RangerBase::ParseCANFrame(can_frame *rx_frame) { - AgxMessage status_msg; - DecodeCanFrameV2(rx_frame, &status_msg); - std::lock_guard guard(state_mutex_); - UpdateRangerState(status_msg, ranger_state_); -} - -void RangerBase::UpdateRangerState(const AgxMessage &status_msg, - RangerState &state) { - switch (status_msg.type) { - case AgxMsgSystemState: { - // std::cout << "system status feedback received" << std::endl; - state.system_state = status_msg.body.system_state_msg; - break; - } - case AgxMsgMotionState: { - // std::cout << "motion control feedback received" << std::endl; - state.motion_state = status_msg.body.motion_state_msg; - state.motion_state.steering_angle *= 10; - // std::cout << "steering angle: " << state.motion_state.steering_angle << - // std::endl; - break; - } - case AgxMsgLightState: { - // std::cout << "light control feedback received" << std::endl; - state.light_state = status_msg.body.light_state_msg; - break; - } - case AgxMsgRcState: { - state.rc_state = status_msg.body.rc_state_msg; - break; - } - case AgxMsgActuatorHSState: { - // std::cout << "actuator hs feedback received" << std::endl; - state.actuator_hs_state[status_msg.body.actuator_hs_state_msg.motor_id] = - status_msg.body.actuator_hs_state_msg; - break; - } - case AgxMsgActuatorLSState: { - // std::cout << "actuator ls feedback received" << std::endl; - state.actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] = - status_msg.body.actuator_ls_state_msg; - break; - } - case AgxMsgMotionModeState: { - state.current_motion_mode = status_msg.body.motion_mode_state_msg; - break; - } - /* sensor feedback */ - case AgxMsgOdometry: { - // std::cout << "Odometer msg feedback received" << std::endl; - state.odometry = status_msg.body.odometry_msg; - } - default: - break; - } -} -} // 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 7ae4c74..8c70e9c 100644 --- a/include/ugv_sdk/details/robot_base/ranger_base.hpp +++ b/include/ugv_sdk/details/robot_base/ranger_base.hpp @@ -15,32 +15,65 @@ #include #include -#include "ugv_sdk/interface/ranger_interface.hpp" -#include "ugv_sdk/protocol_v2/agilex_base.hpp" +#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 { +class RangerBase : public AgilexBase, public RangerInterface { public: - RangerBase() : AgilexBase(){}; + RangerBase() : AgilexBase(){}; ~RangerBase() = default; // set up connection - void Connect(std::string dev_name) override; + void Connect(std::string dev_name) override { + AgilexBase::ConnectPort(dev_name, std::bind(&RangerBase::ParseCANFrame, + this, std::placeholders::_1)); + } // robot control + void SetMotionCommand() {} + void SetMotionCommand(double linear_vel, double steer_angle, - double lateral_vel = 0.0, double angular_vel = 0.0); - void SetLightCommand(const RangerLightCmd &cmd); - void SetMotionMode(uint8_t mode); + double lateral_vel = 0.0, + double angular_vel = 0.0) override { + AgilexBase::SendMotionCommand( + linear_vel, angular_vel, lateral_vel, steer_angle / 10.0); + } + + void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode, + uint8_t r_value) override { + AgilexBase::SendLightCommand(f_mode, f_value, r_mode, + r_value); + } + + void SetMotionMode(uint8_t mode) { AgilexBase::SetMotionMode(mode); } // get robot state - RangerState GetRangerState(); + RangerCoreState GetRobotState() override { + auto state = AgilexBase::GetRobotCoreStateMsgGroup(); - private: - RangerState ranger_state_; + RangerCoreState ranger_state; + 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; + return ranger_state; + } - void ParseCANFrame(can_frame *rx_frame) override; - void UpdateRangerState(const AgxMessage &status_msg, RangerState &state); + RangerActuatorState GetActuatorState() override { + auto actuator = AgilexBase::GetActuatorStateMsgGroup(); + + RangerActuatorState ranger_actuator; + 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; + } }; + +using RangerBaseV2 = RangerBase; } // namespace westonrobot #endif // RANGER_BASE_HPP diff --git a/include/ugv_sdk/mobile_robot/ranger_robot.hpp b/include/ugv_sdk/mobile_robot/ranger_robot.hpp new file mode 100644 index 0000000..3b222b5 --- /dev/null +++ b/include/ugv_sdk/mobile_robot/ranger_robot.hpp @@ -0,0 +1,19 @@ +/* + * ranger_robot.hpp + * + * Created on: Jul 14, 2021 21:45 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#ifndef RANGER_ROBOT_HPP +#define RANGER_ROBOT_HPP + +#include "ugv_sdk/details/robot_base/ranger_base.hpp" + +namespace westonrobot { +using RangerRobot = RangerBaseV2; +} // namespace westonrobot + +#endif /* RANGER_ROBOT_HPP */