diff --git a/CMakeLists.txt b/CMakeLists.txt index c01b2c3..660da26 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -88,7 +88,7 @@ add_library(${PROJECT_NAME} ######################## ## public interface to access robot src/mobile_robot/scout_robot.cpp - # src/mobile_robot/tracer_robot.cpp + src/mobile_robot/bunker_robot.cpp ######################## ## protocol v2 support src/protocol_v2/agilex_msg_parser_v2.c diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index da97880..7bef930 100755 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -2,6 +2,7 @@ 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) diff --git a/demo/bunker_demo/CMakeLists.txt b/demo/bunker_demo/CMakeLists.txt new file mode 100755 index 0000000..de599a2 --- /dev/null +++ b/demo/bunker_demo/CMakeLists.txt @@ -0,0 +1,3 @@ +# tests +add_executable(demo_bunker_robot bunker_robot_demo.cpp) +target_link_libraries(demo_bunker_robot ugv_sdk) \ No newline at end of file diff --git a/demo/bunker_demo/bunker_robot_demo.cpp b/demo/bunker_demo/bunker_robot_demo.cpp new file mode 100644 index 0000000..369b666 --- /dev/null +++ b/demo/bunker_demo/bunker_robot_demo.cpp @@ -0,0 +1,86 @@ +/* + * bunker_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/bunker_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_bunker_demo " + << std::endl + << "Example 1: ./app_bunker_demo v1 can0" << std::endl; + return -1; + } + + std::unique_ptr bunker; + if (protocol_version == "v1") { + bunker = + std::unique_ptr(new BunkerRobot(ProtocolVersion::AGX_V1)); + } else if (protocol_version == "v2") { + bunker = + std::unique_ptr(new BunkerRobot(ProtocolVersion::AGX_V2)); + } else { + std::cout << "Error: invalid protocol version string" << std::endl; + return -1; + } + + if (bunker == nullptr) + std::cout << "Failed to create robot object" << std::endl; + + bunker->Connect(device_name); + + if (bunker->GetProtocolVersion() == ProtocolVersion::AGX_V2) { + bunker->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; + bunker->SetMotionCommand(1.0, 0.0); + + // get robot state + auto state = bunker->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; + std::cout << "-------------------------------" << std::endl; + + usleep(20000); + ++count; + } + + return 0; +} \ No newline at end of file diff --git a/include/ugv_sdk/details/interface/bunker_interface.hpp b/include/ugv_sdk/details/interface/bunker_interface.hpp new file mode 100644 index 0000000..d6b0d81 --- /dev/null +++ b/include/ugv_sdk/details/interface/bunker_interface.hpp @@ -0,0 +1,34 @@ +/* + * bunker_interface.hpp + * + * Created on: Jul 14, 2021 23:04 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#ifndef BUNKER_INTERFACE_HPP +#define BUNKER_INTERFACE_HPP + +#include + +#include "ugv_sdk/details/interface/agilex_message.h" + +namespace westonrobot { +struct BunkerCoreState { + SystemStateMessage system_state; + MotionStateMessage motion_state; + RcStateMessage rc_state; +}; + +struct BunkerInterface { + virtual ~BunkerInterface() = default; + + virtual void SetMotionCommand(double linear_vel, double angular_vel) = 0; + + // get robot state + virtual BunkerCoreState GetRobotState() = 0; +}; +} // namespace westonrobot + +#endif /* BUNKER_INTERFACE_HPP */ diff --git a/include/ugv_sdk/details/interface/robot_interface.hpp b/include/ugv_sdk/details/interface/robot_interface.hpp index 69014c8..c407731 100644 --- a/include/ugv_sdk/details/interface/robot_interface.hpp +++ b/include/ugv_sdk/details/interface/robot_interface.hpp @@ -40,7 +40,6 @@ class RobotInterface { // functions to be implemented by class AgilexBase virtual void EnableCommandedMode() = 0; - virtual void DisableLightControl() = 0; // functions to be implemented by each robot class virtual void Connect(std::string can_name) = 0; @@ -50,6 +49,10 @@ class RobotInterface { }; virtual void ResetRobotState() = 0; + virtual void DisableLightControl() { + // use derived version + } + virtual ProtocolVersion GetProtocolVersion() = 0; protected: @@ -72,9 +75,7 @@ class RobotInterface { virtual void SendLightCommand(LightMode front_mode, uint8_t front_custom_value, LightMode rear_mode, - uint8_t rear_custom_value){ - // use derived version - }; + uint8_t rear_custom_value){}; }; } // 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 1759954..761a9b2 100644 --- a/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp +++ b/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp @@ -65,6 +65,8 @@ class ProtocolV1Parser : public ParserInterface { using ScoutProtocolV1Parser = ProtocolV1Parser; using ScoutMiniProtocolV1Parser = ProtocolV1Parser; +using BunkerProtocolV1Parser = ProtocolV1Parser; + using HunterProtocolV1Parser = ProtocolV1Parser; } // namespace westonrobot diff --git a/include/ugv_sdk/details/protocol_v1/robot_limits.hpp b/include/ugv_sdk/details/protocol_v1/robot_limits.hpp index f3f5a6d..6473c2e 100644 --- a/include/ugv_sdk/details/protocol_v1/robot_limits.hpp +++ b/include/ugv_sdk/details/protocol_v1/robot_limits.hpp @@ -25,6 +25,13 @@ struct ScoutMiniLimits { static constexpr double min_angular = -max_angular; }; +struct BunkerLimits { + static constexpr double max_linear = 3.0; // m/s + static constexpr double min_linear = -max_linear; + static constexpr double max_angular = 2.5235; // rad/s + static constexpr double min_angular = -max_angular; +}; + struct HunterV1Limits { static constexpr double max_linear = 1.5; // m/s static constexpr double min_linear = -max_linear; diff --git a/include/ugv_sdk/details/robot_base/bunker_base.hpp b/include/ugv_sdk/details/robot_base/bunker_base.hpp new file mode 100644 index 0000000..d424151 --- /dev/null +++ b/include/ugv_sdk/details/robot_base/bunker_base.hpp @@ -0,0 +1,60 @@ +/* + * bunker_base.hpp + * + * Created on: Jul 14, 2021 23:05 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#ifndef BUNKER_BASE_HPP +#define BUNKER_BASE_HPP + +#include +#include +#include +#include + +#include "ugv_sdk/details/interface/bunker_interface.hpp" +#include "ugv_sdk/details/robot_base/agilex_base.hpp" + +namespace westonrobot { +template +class BunkerBase : public AgilexBase, public BunkerInterface { + public: + BunkerBase() : AgilexBase(){}; + ~BunkerBase() = 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, angular_vel, 0.0, + 0.0); + } + + // get robot state + BunkerCoreState GetRobotState() override { + auto state = AgilexBase::GetRobotCoreStateMsgGroup(); + + BunkerCoreState bunker_state; + bunker_state.system_state = state.system_state; + bunker_state.motion_state = state.motion_state; + bunker_state.rc_state = state.rc_state; + return bunker_state; + } +}; +} // 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 BunkerBaseV1 = BunkerBase; +using BunkerBaseV2 = BunkerBase; +} // namespace westonrobot + +#endif /* BUNKER_BASE_HPP */ diff --git a/include/ugv_sdk/mobile_robot/bunker_robot.hpp b/include/ugv_sdk/mobile_robot/bunker_robot.hpp new file mode 100644 index 0000000..01bdce7 --- /dev/null +++ b/include/ugv_sdk/mobile_robot/bunker_robot.hpp @@ -0,0 +1,42 @@ +/* + * bunker_robot.hpp + * + * Created on: Jul 14, 2021 23:08 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#ifndef BUNKER_ROBOT_HPP +#define BUNKER_ROBOT_HPP + +#include + +#include "ugv_sdk/details/interface/robot_interface.hpp" +#include "ugv_sdk/details/interface/bunker_interface.hpp" + +namespace westonrobot { +class BunkerRobot : public RobotInterface, public BunkerInterface { + public: + BunkerRobot(ProtocolVersion protocol = ProtocolVersion::AGX_V2); + ~BunkerRobot(); + + 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 + BunkerCoreState GetRobotState() override; + + private: + RobotInterface* robot_; +}; +} // namespace westonrobot + +#endif /* BUNKER_ROBOT_HPP */ diff --git a/src/mobile_robot/bunker_robot.cpp b/src/mobile_robot/bunker_robot.cpp new file mode 100644 index 0000000..3f838a1 --- /dev/null +++ b/src/mobile_robot/bunker_robot.cpp @@ -0,0 +1,45 @@ +/* + * bunker_robot.cpp + * + * Created on: Jul 14, 2021 23:14 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#include "ugv_sdk/mobile_robot/bunker_robot.hpp" +#include "ugv_sdk/details/robot_base/bunker_base.hpp" + +namespace westonrobot { +BunkerRobot::BunkerRobot(ProtocolVersion protocol) { + if (protocol == ProtocolVersion::AGX_V1) { + robot_ = new BunkerBaseV1(); + } else if (protocol == ProtocolVersion::AGX_V2) { + robot_ = new BunkerBaseV2(); + } +} + +BunkerRobot::~BunkerRobot() { + if (robot_) delete robot_; +} + +void BunkerRobot::EnableCommandedMode() { robot_->EnableCommandedMode(); } + +void BunkerRobot::Connect(std::string can_name) { robot_->Connect(can_name); } + +void BunkerRobot::ResetRobotState() { robot_->ResetRobotState(); } + +ProtocolVersion BunkerRobot::GetProtocolVersion() { + return robot_->GetProtocolVersion(); +} + +void BunkerRobot::SetMotionCommand(double linear_vel, double angular_vel) { + auto bunker = dynamic_cast(robot_); + bunker->SetMotionCommand(linear_vel, angular_vel); +} + +BunkerCoreState BunkerRobot::GetRobotState() { + auto bunker = dynamic_cast(robot_); + return bunker->GetRobotState(); +} +} // namespace westonrobot \ No newline at end of file