From 61d2219c11a6d0bbc00822e5a8ec4e3ced597a7f Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Wed, 14 Jul 2021 23:33:52 +0800 Subject: [PATCH] added hunter support --- CMakeLists.txt | 1 + demo/CMakeLists.txt | 10 +- demo/hunter_demo/CMakeLists.txt | 3 + demo/hunter_demo/hunter_robot_demo.cpp | 99 +++++++++++++++++++ .../details/interface/hunter_interface.hpp | 44 +++++++++ .../details/robot_base/agilex_base.hpp | 8 +- .../details/robot_base/hunter_base.hpp | 78 +++++++++++++++ include/ugv_sdk/mobile_robot/hunter_robot.hpp | 43 ++++++++ src/mobile_robot/hunter_robot.cpp | 50 ++++++++++ 9 files changed, 325 insertions(+), 11 deletions(-) create mode 100755 demo/hunter_demo/CMakeLists.txt create mode 100644 demo/hunter_demo/hunter_robot_demo.cpp create mode 100644 include/ugv_sdk/details/interface/hunter_interface.hpp create mode 100644 include/ugv_sdk/details/robot_base/hunter_base.hpp create mode 100644 include/ugv_sdk/mobile_robot/hunter_robot.hpp create mode 100644 src/mobile_robot/hunter_robot.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 660da26..d60273a 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -88,6 +88,7 @@ add_library(${PROJECT_NAME} ######################## ## public interface to access robot src/mobile_robot/scout_robot.cpp + src/mobile_robot/hunter_robot.cpp src/mobile_robot/bunker_robot.cpp ######################## ## protocol v2 support diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index 7bef930..baa1fd1 100755 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -3,12 +3,4 @@ add_subdirectory(scout_demo) add_subdirectory(tracer_demo) add_subdirectory(ranger_demo) add_subdirectory(bunker_demo) - -# add_executable(app_hunter_demo hunter_demo/hunter_demo.cpp) -# target_link_libraries(app_hunter_demo ugv_sdk) - -# add_executable(app_tracer_demo tracer_demo/tracer_demo.cpp) -# target_link_libraries(app_tracer_demo ugv_sdk) - -# add_executable(app_ranger_demo ranger_demo/ranger_demo.cpp) -# target_link_libraries(app_ranger_demo ugv_sdk) +add_subdirectory(hunter_demo) diff --git a/demo/hunter_demo/CMakeLists.txt b/demo/hunter_demo/CMakeLists.txt new file mode 100755 index 0000000..f176db6 --- /dev/null +++ b/demo/hunter_demo/CMakeLists.txt @@ -0,0 +1,3 @@ +# tests +add_executable(demo_hunter_robot hunter_robot_demo.cpp) +target_link_libraries(demo_hunter_robot ugv_sdk) \ No newline at end of file diff --git a/demo/hunter_demo/hunter_robot_demo.cpp b/demo/hunter_demo/hunter_robot_demo.cpp new file mode 100644 index 0000000..d51b3a9 --- /dev/null +++ b/demo/hunter_demo/hunter_robot_demo.cpp @@ -0,0 +1,99 @@ +/* + * hunter_robot_demo.cpp + * + * Created on: Jul 08, 2021 11:12 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#include + +#include +#include + +#include "ugv_sdk/mobile_robot/hunter_robot.hpp" + +using namespace westonrobot; + +int main(int argc, char **argv) { + std::string protocol_version; + std::string device_name; + + if (argc == 3) { + protocol_version = {argv[1]}; + device_name = {argv[2]}; + std::cout << "Use protocol " << protocol_version << " on interface " + << device_name << std::endl; + } else { + std::cout << "Usage: app_hunter_demo " + << std::endl + << "Example 1: ./app_hunter_demo v1 can0" << std::endl; + return -1; + } + + std::unique_ptr hunter; + if (protocol_version == "v1") { + hunter = + std::unique_ptr(new HunterRobot(ProtocolVersion::AGX_V1)); + } else if (protocol_version == "v2") { + hunter = + std::unique_ptr(new HunterRobot(ProtocolVersion::AGX_V2)); + } else { + std::cout << "Error: invalid protocol version string" << std::endl; + return -1; + } + + if (hunter == nullptr) + std::cout << "Failed to create robot object" << std::endl; + + hunter->Connect(device_name); + + if (hunter->GetProtocolVersion() == ProtocolVersion::AGX_V2) { + hunter->EnableCommandedMode(); + std::cout << "Protocol version 2" << std::endl; + } else { + std::cout << "Protocol version 1" << std::endl; + } + + int count = 0; + while (true) { + // motion control + std::cout << "Motor: 1.0, 0" << std::endl; + hunter->SetMotionCommand(1.0, 0.0); + + // get robot state + auto state = hunter->GetRobotState(); + std::cout << "-------------------------------" << std::endl; + std::cout << "count: " << count << std::endl; + std::cout << "control mode: " + << static_cast(state.system_state.control_mode) + << " , vehicle state: " + << static_cast(state.system_state.vehicle_state) + << " , error code: " << std::hex << state.system_state.error_code + << ", battery voltage: " << state.system_state.battery_voltage + << std::endl; + std::cout << "velocity (linear, angular): " + << state.motion_state.linear_velocity << ", " + << state.motion_state.angular_velocity << std::endl; + + auto actuator = hunter->GetActuatorState(); + if (hunter->GetProtocolVersion() == ProtocolVersion::AGX_V1) { + for (int i = 0; i < 3; ++i) { + printf("motor %d: current %f, rpm %d, driver temp %f, motor temp %f\n", + actuator.actuator_state[i].motor_id, + actuator.actuator_state[i].current, + actuator.actuator_state[i].rpm, + actuator.actuator_state[i].driver_temp, + actuator.actuator_state[i].motor_temp); + } + } else { + } + std::cout << "-------------------------------" << std::endl; + + usleep(20000); + ++count; + } + + return 0; +} \ No newline at end of file diff --git a/include/ugv_sdk/details/interface/hunter_interface.hpp b/include/ugv_sdk/details/interface/hunter_interface.hpp new file mode 100644 index 0000000..feaaf54 --- /dev/null +++ b/include/ugv_sdk/details/interface/hunter_interface.hpp @@ -0,0 +1,44 @@ +/* + * hunter_interface.hpp + * + * Created on: Jul 14, 2021 23:21 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#ifndef HUNTER_INTERFACE_HPP +#define HUNTER_INTERFACE_HPP + +#include + +#include "ugv_sdk/details/interface/agilex_message.h" + +namespace westonrobot { +struct HunterCoreState { + SystemStateMessage system_state; + MotionStateMessage motion_state; + LightStateMessage light_state; + RcStateMessage rc_state; +}; + +struct HunterActuatorState { + // actuator state + ActuatorHSStateMessage actuator_hs_state[3]; + ActuatorLSStateMessage actuator_ls_state[3]; + // - for v1 robots only + ActuatorStateMessageV1 actuator_state[3]; +}; + +struct HunterInterface { + virtual ~HunterInterface() = default; + + virtual void SetMotionCommand(double linear_vel, double steering_angle) = 0; + + // get robot state + virtual HunterCoreState GetRobotState() = 0; + virtual HunterActuatorState GetActuatorState() = 0; +}; +} // namespace westonrobot + +#endif /* HUNTER_INTERFACE_HPP */ diff --git a/include/ugv_sdk/details/robot_base/agilex_base.hpp b/include/ugv_sdk/details/robot_base/agilex_base.hpp index 0be064f..4d42e3a 100644 --- a/include/ugv_sdk/details/robot_base/agilex_base.hpp +++ b/include/ugv_sdk/details/robot_base/agilex_base.hpp @@ -65,7 +65,10 @@ class AgilexBase : public RobotInterface { msg.body.v1_motion_command_msg.control_mode = CONTROL_MODE_CAN; msg.body.v1_motion_command_msg.clear_all_error = false; msg.body.v1_motion_command_msg.linear = linear_vel; - msg.body.v1_motion_command_msg.angular = angular_vel; + // normally only one of angular_vel and steering_angle can be non-zero + msg.body.v1_motion_command_msg.angular = + std::abs(angular_vel) > std::abs(steering_angle) ? angular_vel + : steering_angle; msg.body.v1_motion_command_msg.lateral = lateral_velocity; } else if (parser_.GetProtocolVersion() == ProtocolVersion::AGX_V2) { msg.type = AgxMsgMotionCommand; @@ -206,7 +209,8 @@ class AgilexBase : public RobotInterface { } case AgxMsgMotionModeState: { // std::cout << "motion mode feedback received" << std::endl; - core_state_msgs_.motion_mode_state = status_msg.body.motion_mode_state_msg; + core_state_msgs_.motion_mode_state = + status_msg.body.motion_mode_state_msg; break; } case AgxMsgRcState: { diff --git a/include/ugv_sdk/details/robot_base/hunter_base.hpp b/include/ugv_sdk/details/robot_base/hunter_base.hpp new file mode 100644 index 0000000..f9cd255 --- /dev/null +++ b/include/ugv_sdk/details/robot_base/hunter_base.hpp @@ -0,0 +1,78 @@ +/* + * hunter_base.hpp + * + * Created on: Jul 14, 2021 23:23 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#ifndef HUNTER_BASE_HPP +#define HUNTER_BASE_HPP + +#include +#include +#include +#include + +#include "ugv_sdk/details/interface/scout_interface.hpp" +#include "ugv_sdk/details/robot_base/agilex_base.hpp" + +namespace westonrobot { +template +class HunterBase : public AgilexBase, public ScoutInterface { + public: + HunterBase() : AgilexBase(){}; + ~HunterBase() = default; + + // set up connection + void Connect(std::string can_name) override { + AgilexBase::Connect(can_name); + } + + // robot control + void SetMotionCommand(double linear_vel, double angular_vel) override { + AgilexBase::SendMotionCommand(linear_vel, 0.0, 0.0, + angular_vel); + } + + 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); + } + + // get robot state + ScoutCoreState GetRobotState() override { + auto state = AgilexBase::GetRobotCoreStateMsgGroup(); + + ScoutCoreState scout_state; + scout_state.system_state = state.system_state; + scout_state.motion_state = state.motion_state; + scout_state.light_state = state.light_state; + scout_state.rc_state = state.rc_state; + return scout_state; + } + + ScoutActuatorState GetActuatorState() override { + auto actuator = AgilexBase::GetActuatorStateMsgGroup(); + + ScoutActuatorState scout_actuator; + for (int i = 0; i < 4; ++i) { + scout_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i]; + scout_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i]; + scout_actuator.actuator_state[i] = actuator.actuator_state[i]; + } + return scout_actuator; + } +}; +} // 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 HunterBaseV1 = HunterBase; +using HunterBaseV2 = HunterBase; +} // namespace westonrobot + +#endif /* HUNTER_BASE_HPP */ diff --git a/include/ugv_sdk/mobile_robot/hunter_robot.hpp b/include/ugv_sdk/mobile_robot/hunter_robot.hpp new file mode 100644 index 0000000..30df4e4 --- /dev/null +++ b/include/ugv_sdk/mobile_robot/hunter_robot.hpp @@ -0,0 +1,43 @@ +/* + * hunter_robot.hpp + * + * Created on: Jul 08, 2021 10:59 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#ifndef SCOUT_ROBOT_HPP +#define SCOUT_ROBOT_HPP + +#include + +#include "ugv_sdk/details/interface/robot_interface.hpp" +#include "ugv_sdk/details/interface/hunter_interface.hpp" + +namespace westonrobot { +class HunterRobot : public RobotInterface, public HunterInterface { + public: + HunterRobot(ProtocolVersion protocol = ProtocolVersion::AGX_V2); + ~HunterRobot(); + + void Connect(std::string can_name) override; + + void EnableCommandedMode() override; + + void SetMotionCommand(double linear_vel, double angular_vel) override; + + void ResetRobotState() override; + + ProtocolVersion GetProtocolVersion() override; + + // get robot state + HunterCoreState GetRobotState() override; + HunterActuatorState GetActuatorState() override; + + private: + RobotInterface* robot_; +}; +} // namespace westonrobot + +#endif /* SCOUT_ROBOT_HPP */ diff --git a/src/mobile_robot/hunter_robot.cpp b/src/mobile_robot/hunter_robot.cpp new file mode 100644 index 0000000..c29d09d --- /dev/null +++ b/src/mobile_robot/hunter_robot.cpp @@ -0,0 +1,50 @@ +/* + * hunter_robot.cpp + * + * Created on: Jul 14, 2021 23:30 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#include "ugv_sdk/mobile_robot/hunter_robot.hpp" +#include "ugv_sdk/details/robot_base/hunter_base.hpp" + +namespace westonrobot { +HunterRobot::HunterRobot(ProtocolVersion protocol) { + if (protocol == ProtocolVersion::AGX_V1) { + robot_ = new HunterBaseV1(); + } else if (protocol == ProtocolVersion::AGX_V2) { + robot_ = new HunterBaseV2(); + } +} + +HunterRobot::~HunterRobot() { + if (robot_) delete robot_; +} + +void HunterRobot::EnableCommandedMode() { robot_->EnableCommandedMode(); } + +void HunterRobot::Connect(std::string can_name) { robot_->Connect(can_name); } + +void HunterRobot::ResetRobotState() { robot_->ResetRobotState(); } + +ProtocolVersion HunterRobot::GetProtocolVersion() { + return robot_->GetProtocolVersion(); +} + +void HunterRobot::SetMotionCommand(double linear_vel, double angular_vel) { + auto hunter = dynamic_cast(robot_); + hunter->SetMotionCommand(linear_vel, angular_vel); +} + +HunterCoreState HunterRobot::GetRobotState() { + auto hunter = dynamic_cast(robot_); + return hunter->GetRobotState(); +} + +HunterActuatorState HunterRobot::GetActuatorState() { + auto hunter = dynamic_cast(robot_); + return hunter->GetActuatorState(); +} +} // namespace westonrobot \ No newline at end of file