mirrors/ugv_sdk
1
0
Fork 0
mirror of https://github.com/westonrobot/ugv_sdk synced 2023-04-08 06:32:14 +08:00
Code Issues Packages Projects Releases Wiki Activity

saved work

Browse Source
This commit is contained in:
Ruixiang Du
2020-06-17 14:47:37 +08:00
parent 741c521863
commit 19197facc0
5 changed files with 52 additions and 184 deletions
Download Patch File Download Diff File Expand all files Collapse all files

84
src/platforms/hunter_base.cpp
View File

@@ -13,60 +13,8 @@
#include "stopwatch.h"
namespace westonrobot {
HunterBase::~HunterBase() {
if (serial_connected_) serial_if_->close();
if (cmd_thread_.joinable()) cmd_thread_.join();
}
void HunterBase::Connect(std::string dev_name) {
// if (baud_rate == 0) {
ConfigureCANBus(dev_name);
// } else {
// ConfigureSerial(dev_name, baud_rate);
// if (!serial_connected_)
// std::cerr << "ERROR: Failed to connect to serial port" << std::endl;
// }
}
void HunterBase::Disconnect() {
if (serial_connected_) {
if (serial_if_->is_open()) serial_if_->close();
}
}
void HunterBase::ConfigureCANBus(const std::string &can_if_name) {
can_if_ = std::make_shared<ASyncCAN>(can_if_name);
can_if_->set_receive_callback(
std::bind(&HunterBase::ParseCANFrame, this, std::placeholders::_1));
can_connected_ = true;
}
void HunterBase::ConfigureSerial(const std::string uart_name,
int32_t baud_rate) {
serial_if_ = std::make_shared<ASyncSerial>(uart_name, baud_rate);
serial_if_->open();
if (serial_if_->is_open()) serial_connected_ = true;
serial_if_->set_receive_callback(
std::bind(&HunterBase::ParseUARTBuffer, this, std::placeholders::_1,
std::placeholders::_2, std::placeholders::_3));
}
void HunterBase::StartCmdThread() {
current_motion_cmd_.linear_velocity = 0;
current_motion_cmd_.angular_velocity = 0;
current_motion_cmd_.fault_clear_flag =
HunterMotionCmd::FaultClearFlag::NO_FAULT;
cmd_thread_ = std::thread(
std::bind(&HunterBase::ControlLoop, this, cmd_thread_period_ms_));
cmd_thread_started_ = true;
}
void HunterBase::SendRobotCmd() {}
void HunterBase::SendMotionCmd(uint8_t count) {
// motion control message
@@ -110,22 +58,6 @@ void HunterBase::SendMotionCmd(uint8_t count) {
}
}
void HunterBase::ControlLoop(int32_t period_ms) {
StopWatch ctrl_sw;
uint8_t cmd_count = 0;
uint8_t light_cmd_count = 0;
while (true) {
ctrl_sw.tic();
// motion control message
SendMotionCmd(cmd_count++);
ctrl_sw.sleep_until_ms(period_ms);
// std::cout << "control loop update frequency: " << 1.0 / ctrl_sw.toc() <<
// std::endl;
}
}
HunterState HunterBase::GetHunterState() {
std::lock_guard<std::mutex> guard(hunter_state_mutex_);
return hunter_state_;
@@ -170,20 +102,6 @@ void HunterBase::ParseCANFrame(can_frame *rx_frame) {
NewStatusMsgReceivedCallback(status_msg);
}
void HunterBase::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);
// TODO
// HunterMessage status_msg;
// for (int i = 0; i < bytes_received; ++i) {
// if (DecodeHunterMsgFromUART(buf[i], &status_msg))
// NewStatusMsgReceivedCallback(status_msg);
// }
}
void HunterBase::NewStatusMsgReceivedCallback(const HunterMessage &msg) {
// std::cout << "new status msg received" << std::endl;
std::lock_guard<std::mutex> guard(hunter_state_mutex_);