From 0d445fdfd1300aaf358b38b7d58e945223ebc40e Mon Sep 17 00:00:00 2001 From: wangzheqie Date: Mon, 19 Apr 2021 20:25:30 +0800 Subject: [PATCH] add ranger vehicle --- CMakeLists.txt | 1 + apps/CMakeLists.txt | 5 +- apps/ranger_demo/ranger_demo.cpp | 57 +++++++++++++++++++ include/ugv_sdk/ranger_base.hpp | 82 ++++++++++++++++++++++++++ src/ranger_base.cpp | 98 ++++++++++++++++++++++++++++++++ 5 files changed, 242 insertions(+), 1 deletion(-) create mode 100644 apps/ranger_demo/ranger_demo.cpp create mode 100644 include/ugv_sdk/ranger_base.hpp create mode 100644 src/ranger_base.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index bf39434..5d96258 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -89,6 +89,7 @@ add_library(${PROJECT_NAME} # src/hunter_base.cpp src/tracer_base.cpp # src/bunker_base.cpp + src/ranger_base.cpp ) if(BUILD_WITHOUT_ROS) target_link_libraries(${PROJECT_NAME} westonrobot::async_port Threads::Threads) diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index 4747016..eec58c8 100755 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -14,6 +14,9 @@ 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) + # if(BUILD_MONITOR) # add_subdirectory(scout_monitor) -# endif() \ No newline at end of file +# endif() diff --git a/apps/ranger_demo/ranger_demo.cpp b/apps/ranger_demo/ranger_demo.cpp new file mode 100644 index 0000000..cf36f51 --- /dev/null +++ b/apps/ranger_demo/ranger_demo.cpp @@ -0,0 +1,57 @@ +/** + * @Kit : Qt-Creator: Desktop + * @Author : Wang Zhe + * @Date : 2021-04-19 19:41:52 + * @FileName : ranger_demo.cpp + * @Mail : wangzheqie@qq.com + * Copyright : AgileX Robotics + **/ + +#include "ugv_sdk/ranger_base.hpp" + +using namespace westonrobot; + +int main(int argc, char *argv[]) { + std::string device_name; + + if (argc == 2) { + device_name = {argv[1]}; + std::cout << "Specified CAN: " << device_name << std::endl; + } else { + std::cout << "Usage: app_ranger_demo " << std::endl + << "Example 1: ./app_ranger_demo can0" << std::endl; + return -1; + } + + RangerBase ranger; + ranger.Connect(device_name); + + ranger.EnableCommandedMode(); + + int count = 0; + while (true) { + if (count < 100) { + std::cout << "Motor: 0.2, 0" << std::endl; + ranger.SetMotionCommand(0.2, 0.0); + } + + auto state = ranger.GetRangerState(); + 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) + << std::endl; + std::cout << "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; +} diff --git a/include/ugv_sdk/ranger_base.hpp b/include/ugv_sdk/ranger_base.hpp new file mode 100644 index 0000000..fe250fb --- /dev/null +++ b/include/ugv_sdk/ranger_base.hpp @@ -0,0 +1,82 @@ +/** + * @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/agilex_base.hpp" + +namespace westonrobot { +struct RangerState { + // system state + SystemStateMessage system_state; + MotionStateMessage motion_state; + LightStateMessage light_state; + + RcStateMessage rc_state; + + ActuatorHSStateMessage actuator_hs_state[4]; + ActuatorLSStateMessage actuator_ls_state[4]; + + // sensor data + 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) + : cmd_ctrl_allowed(true), + front_mode(f_mode), + front_custom_value(f_value), + rear_mode(r_mode), + rear_custom_value(r_value) {} + + bool cmd_ctrl_allowed = false; + LightMode front_mode; + uint8_t front_custom_value; + LightMode rear_mode; + uint8_t rear_custom_value; +}; + +///////////////////////////////////////////////////////////////////////// + +class RangerBase : public AgilexBase { + public: + RangerBase() : AgilexBase(){}; + ~RangerBase() = default; + + // set up connection + void Connect(std::string dev_name) override; + + // robot control + void SetMotionCommand(double linear_vel, double angular_vel); + void SetLightCommand(const RangerLightCmd &cmd); + + // get robot state + RangerState GetRangerState(); + + private: + RangerState ranger_state_; + + void ParseCANFrame(can_frame *rx_frame) override; + void UpdateRangerState(const AgxMessage &status_msg, RangerState &state); +}; +} // namespace westonrobot +#endif // RANGER_BASE_HPP diff --git a/src/ranger_base.cpp b/src/ranger_base.cpp new file mode 100644 index 0000000..55f6855 --- /dev/null +++ b/src/ranger_base.cpp @@ -0,0 +1,98 @@ +/** + * @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/ranger_base.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "stopwatch.hpp" + +#include "ugv_sdk/details/agilex_msg_parser.h" + +namespace westonrobot { +void RangerBase::Connect(std::string dev_name) { + AgilexBase::Connect(dev_name, std::bind(&RangerBase::ParseCANFrame, this, + std::placeholders::_1)); +} + +void RangerBase::SetMotionCommand(double linear_vel, double angular_vel) { + AgilexBase::SetMotionCommand(linear_vel, angular_vel, 0.0, 0.0); +} + +void RangerBase::SetLightCommand(const RangerLightCmd &cmd) { + if (cmd.cmd_ctrl_allowed) { + AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value, + LightMode::CONST_OFF, 0); + } +} + +RangerState RangerBase::GetRangerState() { + std::lock_guard guard(state_mutex_); + return ranger_state_; +} + +void RangerBase::ParseCANFrame(can_frame *rx_frame) { + AgxMessage status_msg; + DecodeCanFrame(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; + 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; + } + /* sensor feedback */ + case AgxMsgOdometry: { + // std::cout << "Odometer msg feedback received" << std::endl; + state.odometry = status_msg.body.odometry_msg; + } + default: + break; + } +} +} // namespace westonrobot