diff --git a/CMakeLists.txt b/CMakeLists.txt index 52293c2..ae0f1d9 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,6 +51,7 @@ set(WRP_SDK_SRC src/async_can.cpp src/asyncio_utils.cpp # agilex mobile platforms + src/platforms/mobile_base.cpp src/platforms/hunter_base.cpp src/platforms/hunter_can_parser.c src/platforms/scout_base.cpp diff --git a/include/wrp_sdk/platforms/common/mobile_base.hpp b/include/wrp_sdk/platforms/common/mobile_base.hpp index 0f06999..2dfb976 100644 --- a/include/wrp_sdk/platforms/common/mobile_base.hpp +++ b/include/wrp_sdk/platforms/common/mobile_base.hpp @@ -43,53 +43,31 @@ class MobileBase { cmd_thread_period_ms_ = period_ms; }; - // motion control - void SetMotionCommand(double linear_vel, double steering_angle, - HunterMotionCmd::FaultClearFlag fault_clr_flag = - HunterMotionCmd::FaultClearFlag::NO_FAULT); - - // get robot state - HunterState GetHunterState(); - protected: - // hardware communication interface - std::shared_ptr can_if_; - std::shared_ptr serial_if_; - - // CAN priority higher than serial if both connected + // communication interface bool can_connected_ = false; bool serial_connected_ = false; - // serial port related variables - uint8_t tx_cmd_len_; - uint8_t tx_buffer_[HUNTER_CMD_BUF_LEN]; + std::shared_ptr can_if_; + std::shared_ptr serial_if_; - // cmd/status update related variables + // command thread std::thread cmd_thread_; - std::mutex hunter_state_mutex_; - std::mutex motion_cmd_mutex_; - - HunterState hunter_state_; - HunterMotionCmd current_motion_cmd_; - int32_t cmd_thread_period_ms_ = 10; bool cmd_thread_started_ = false; // internal functions - void ConfigureCANBus(const std::string &can_if_name = "can1"); + void ConfigureCAN(const std::string &can_if_name = "can0"); void ConfigureSerial(const std::string uart_name = "/dev/ttyUSB0", int32_t baud_rate = 115200); void StartCmdThread(); void ControlLoop(int32_t period_ms); - void SendMotionCmd(uint8_t count); - - void ParseCANFrame(can_frame *rx_frame); - void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, - size_t bytes_received); - - void NewStatusMsgReceivedCallback(const HunterMessage &msg); + virtual void SendRobotCmd() = 0; + virtual void ParseCANFrame(can_frame *rx_frame) = 0; + virtual void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, + size_t bytes_received) = 0; }; } // namespace westonrobot diff --git a/src/platforms/mobile_base.cpp b/src/platforms/mobile_base.cpp index 9ee03f0..553875c 100644 --- a/src/platforms/mobile_base.cpp +++ b/src/platforms/mobile_base.cpp @@ -7,7 +7,7 @@ * Copyright (c) 2020 Ruixiang Du (rdu) */ -#include "wrp_sdk/platforms/mobile_base.hpp" +#include "wrp_sdk/platforms/common/mobile_base.hpp" #include #include @@ -17,33 +17,32 @@ namespace westonrobot { MobileBase::~MobileBase() { - if (serial_connected_) serial_if_->close(); + // release resource if occupied + Disconnect(); + // joint cmd thread if (cmd_thread_.joinable()) cmd_thread_.join(); } void MobileBase::Connect(std::string dev_name, int32_t baud_rate) { if (baud_rate == 0) { - ConfigureCANBus(dev_name); + ConfigureCAN(dev_name); } else { ConfigureSerial(dev_name, baud_rate); - if (!serial_connected_) std::cerr << "ERROR: Failed to connect to serial port" << std::endl; } } void MobileBase::Disconnect() { - if (serial_connected_) { - if (serial_if_->is_open()) serial_if_->close(); + if (serial_connected_ && serial_if_->is_open()) { + serial_if_->close(); } } -void MobileBase::ConfigureCANBus(const std::string &can_if_name) { +void MobileBase::ConfigureCAN(const std::string &can_if_name) { can_if_ = std::make_shared(can_if_name); - can_if_->set_receive_callback( std::bind(&MobileBase::ParseCANFrame, this, std::placeholders::_1)); - can_connected_ = true; } @@ -51,300 +50,28 @@ void MobileBase::ConfigureSerial(const std::string uart_name, int32_t baud_rate) { serial_if_ = std::make_shared(uart_name, baud_rate); serial_if_->open(); - if (serial_if_->is_open()) serial_connected_ = true; - serial_if_->set_receive_callback( std::bind(&MobileBase::ParseUARTBuffer, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } void MobileBase::StartCmdThread() { - current_motion_cmd_.linear_velocity = 0; - current_motion_cmd_.angular_velocity = 0; - current_motion_cmd_.fault_clear_flag = - ScoutMotionCmd::FaultClearFlag::NO_FAULT; - cmd_thread_ = std::thread( std::bind(&MobileBase::ControlLoop, this, cmd_thread_period_ms_)); cmd_thread_started_ = true; } -void MobileBase::SendMotionCmd(uint8_t count) { - // motion control message - ScoutMessage m_msg; - m_msg.type = ScoutMotionControlMsg; - - if (can_connected_) - m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN; - else if (serial_connected_) - m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART; - - motion_cmd_mutex_.lock(); - m_msg.body.motion_control_msg.data.cmd.fault_clear_flag = - static_cast(current_motion_cmd_.fault_clear_flag); - m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd = - current_motion_cmd_.linear_velocity; - m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd = - current_motion_cmd_.angular_velocity; - motion_cmd_mutex_.unlock(); - - m_msg.body.motion_control_msg.data.cmd.reserved0 = 0; - m_msg.body.motion_control_msg.data.cmd.reserved1 = 0; - m_msg.body.motion_control_msg.data.cmd.count = count; - - if (can_connected_) - m_msg.body.motion_control_msg.data.cmd.checksum = - CalcScoutCANChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID, - m_msg.body.motion_control_msg.data.raw, 8); - // serial_connected_: checksum will be calculated later when packed into a - // complete serial frame - - if (can_connected_) { - // send to can bus - can_frame m_frame; - EncodeScoutMsgToCAN(&m_msg, &m_frame); - can_if_->send_frame(m_frame); - } else { - // send to serial port - EncodeScoutMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_); - serial_if_->send_bytes(tx_buffer_, tx_cmd_len_); - } -} - -void MobileBase::SendLightCmd(uint8_t count) { - ScoutMessage l_msg; - l_msg.type = ScoutLightControlMsg; - - light_cmd_mutex_.lock(); - if (light_ctrl_enabled_) { - l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_ENABLE_CTRL; - - l_msg.body.light_control_msg.data.cmd.front_light_mode = - static_cast(current_light_cmd_.front_mode); - l_msg.body.light_control_msg.data.cmd.front_light_custom = - current_light_cmd_.front_custom_value; - l_msg.body.light_control_msg.data.cmd.rear_light_mode = - static_cast(current_light_cmd_.rear_mode); - l_msg.body.light_control_msg.data.cmd.rear_light_custom = - current_light_cmd_.rear_custom_value; - - // std::cout << "cmd: " << l_msg.data.cmd.front_light_mode << " , " << - // l_msg.data.cmd.front_light_custom << " , " - // << l_msg.data.cmd.rear_light_mode << " , " << - // l_msg.data.cmd.rear_light_custom << std::endl; - // std::cout << "light cmd generated" << std::endl; - } else { - l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = - LIGHT_DISABLE_CTRL; - - l_msg.body.light_control_msg.data.cmd.front_light_mode = - LIGHT_MODE_CONST_OFF; - l_msg.body.light_control_msg.data.cmd.front_light_custom = 0; - l_msg.body.light_control_msg.data.cmd.rear_light_mode = - LIGHT_MODE_CONST_OFF; - l_msg.body.light_control_msg.data.cmd.rear_light_custom = 0; - } - light_ctrl_requested_ = false; - light_cmd_mutex_.unlock(); - - l_msg.body.light_control_msg.data.cmd.reserved0 = 0; - l_msg.body.light_control_msg.data.cmd.count = count; - - if (can_connected_) - l_msg.body.light_control_msg.data.cmd.checksum = CalcScoutCANChecksum( - CAN_MSG_LIGHT_CONTROL_CMD_ID, l_msg.body.light_control_msg.data.raw, 8); - // serial_connected_: checksum will be calculated later when packed into a - // complete serial frame - - if (can_connected_) { - // send to can bus - can_frame l_frame; - EncodeScoutMsgToCAN(&l_msg, &l_frame); - - can_if_->send_frame(l_frame); - } else { - // send to serial port - EncodeScoutMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_); - serial_if_->send_bytes(tx_buffer_, tx_cmd_len_); - } - - // std::cout << "cmd: " << static_cast(l_msg.data.cmd.front_light_mode) - // << " , " << static_cast(l_msg.data.cmd.front_light_custom) << " , " - // << static_cast(l_msg.data.cmd.rear_light_mode) << " , " << - // static_cast(l_msg.data.cmd.rear_light_custom) << std::endl; - // std::cout << "can: "; - // for (int i = 0; i < 8; ++i) - // std::cout << static_cast(l_frame.data[i]) << " "; - // std::cout << "uart: "; - // for (int i = 0; i < tx_cmd_len_; ++i) - // std::cout << static_cast(tx_buffer_[i]) << " "; - // std::cout << std::endl; -} - void MobileBase::ControlLoop(int32_t period_ms) { StopWatch ctrl_sw; - uint8_t cmd_count = 0; - uint8_t light_cmd_count = 0; + bool print_loop_freq = false; while (true) { ctrl_sw.tic(); - - // motion control message - SendMotionCmd(cmd_count++); - - // check if there is request for light control - if (light_ctrl_requested_) SendLightCmd(light_cmd_count++); - + SendRobotCmd(); ctrl_sw.sleep_until_ms(period_ms); - // std::cout << "control loop update frequency: " << 1.0 / ctrl_sw.toc() << - // std::endl; - } -} - -ScoutState MobileBase::GetScoutState() { - std::lock_guard guard(scout_state_mutex_); - return scout_state_; -} - -void MobileBase::SetMotionCommand( - double linear_vel, double angular_vel, - ScoutMotionCmd::FaultClearFlag fault_clr_flag) { - // make sure cmd thread is started before attempting to send commands - if (!cmd_thread_started_) StartCmdThread(); - - if (linear_vel < ScoutMotionCmd::min_linear_velocity) - linear_vel = ScoutMotionCmd::min_linear_velocity; - if (linear_vel > ScoutMotionCmd::max_linear_velocity) - linear_vel = ScoutMotionCmd::max_linear_velocity; - if (angular_vel < ScoutMotionCmd::min_angular_velocity) - angular_vel = ScoutMotionCmd::min_angular_velocity; - if (angular_vel > ScoutMotionCmd::max_angular_velocity) - angular_vel = ScoutMotionCmd::max_angular_velocity; - - std::lock_guard guard(motion_cmd_mutex_); - current_motion_cmd_.linear_velocity = static_cast( - linear_vel / ScoutMotionCmd::max_linear_velocity * 100.0); - current_motion_cmd_.angular_velocity = static_cast( - angular_vel / ScoutMotionCmd::max_angular_velocity * 100.0); - current_motion_cmd_.fault_clear_flag = fault_clr_flag; -} - -void MobileBase::SetLightCommand(ScoutLightCmd cmd) { - if (!cmd_thread_started_) StartCmdThread(); - - std::lock_guard guard(light_cmd_mutex_); - current_light_cmd_ = cmd; - light_ctrl_enabled_ = true; - light_ctrl_requested_ = true; -} - -void MobileBase::DisableLightCmdControl() { - std::lock_guard guard(light_cmd_mutex_); - light_ctrl_enabled_ = false; - light_ctrl_requested_ = true; -} - -void MobileBase::ParseCANFrame(can_frame *rx_frame) { - // validate checksum, discard frame if fails - if (!rx_frame->data[7] == CalcScoutCANChecksum(rx_frame->can_id, - rx_frame->data, - rx_frame->can_dlc)) { - std::cerr << "ERROR: checksum mismatch, discard frame with id " - << rx_frame->can_id << std::endl; - return; - } - - // otherwise, update robot state with new frame - ScoutMessage status_msg; - DecodeScoutMsgFromCAN(rx_frame, &status_msg); - NewStatusMsgReceivedCallback(status_msg); -} - -void MobileBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize, - size_t bytes_received) { - // std::cout << "bytes received from serial: " << bytes_received << std::endl; - // serial_parser_.PrintStatistics(); - // serial_parser_.ParseBuffer(buf, bytes_received); - ScoutMessage status_msg; - for (int i = 0; i < bytes_received; ++i) { - if (DecodeScoutMsgFromUART(buf[i], &status_msg)) - NewStatusMsgReceivedCallback(status_msg); - } -} - -void MobileBase::NewStatusMsgReceivedCallback(const ScoutMessage &msg) { - // std::cout << "new status msg received" << std::endl; - std::lock_guard guard(scout_state_mutex_); - UpdateScoutState(msg, scout_state_); -} - -void MobileBase::UpdateScoutState(const ScoutMessage &status_msg, - ScoutState &state) { - switch (status_msg.type) { - case ScoutMotionStatusMsg: { - // std::cout << "motion control feedback received" << std::endl; - const MotionStatusMessage &msg = status_msg.body.motion_status_msg; - state.linear_velocity = - static_cast( - static_cast(msg.data.status.linear_velocity.low_byte) | - static_cast(msg.data.status.linear_velocity.high_byte) - << 8) / - 1000.0; - state.angular_velocity = - static_cast( - static_cast(msg.data.status.angular_velocity.low_byte) | - static_cast(msg.data.status.angular_velocity.high_byte) - << 8) / - 1000.0; - break; - } - case ScoutLightStatusMsg: { - // std::cout << "light control feedback received" << std::endl; - const LightStatusMessage &msg = status_msg.body.light_status_msg; - if (msg.data.status.light_ctrl_enable == LIGHT_DISABLE_CTRL) - state.light_control_enabled = false; - else - state.light_control_enabled = true; - state.front_light_state.mode = msg.data.status.front_light_mode; - state.front_light_state.custom_value = msg.data.status.front_light_custom; - state.rear_light_state.mode = msg.data.status.rear_light_mode; - state.rear_light_state.custom_value = msg.data.status.rear_light_custom; - break; - } - case ScoutSystemStatusMsg: { - // std::cout << "system status feedback received" << std::endl; - const SystemStatusMessage &msg = status_msg.body.system_status_msg; - state.control_mode = msg.data.status.control_mode; - state.base_state = msg.data.status.base_state; - state.battery_voltage = - (static_cast(msg.data.status.battery_voltage.low_byte) | - static_cast(msg.data.status.battery_voltage.high_byte) - << 8) / - 10.0; - state.fault_code = - (static_cast(msg.data.status.fault_code.low_byte) | - static_cast(msg.data.status.fault_code.high_byte) << 8); - break; - } - case ScoutMotorDriverStatusMsg: { - // std::cout << "motor 1 driver feedback received" << std::endl; - const MotorDriverStatusMessage &msg = - status_msg.body.motor_driver_status_msg; - for (int i = 0; i < 4; ++i) { - state.motor_states[status_msg.body.motor_driver_status_msg.motor_id] - .current = - (static_cast(msg.data.status.current.low_byte) | - static_cast(msg.data.status.current.high_byte) << 8) / - 10.0; - state.motor_states[status_msg.body.motor_driver_status_msg.motor_id] - .rpm = static_cast( - static_cast(msg.data.status.rpm.low_byte) | - static_cast(msg.data.status.rpm.high_byte) << 8); - state.motor_states[status_msg.body.motor_driver_status_msg.motor_id] - .temperature = msg.data.status.temperature; - } - break; - } + if (print_loop_freq) + std::cout << "control loop frequency: " << 1.0 / ctrl_sw.toc() + << std::endl; } } } // namespace westonrobot