mirror of
https://github.com/westonrobot/ugv_sdk
synced 2023-04-08 06:32:14 +08:00
80 lines
2.1 KiB
C++
80 lines
2.1 KiB
C++
/*
|
|
* mobile_base.cpp
|
|
*
|
|
* Created on: Jun 17, 2020 11:26
|
|
* Description:
|
|
*
|
|
* Copyright (c) 2020 Ruixiang Du (rdu)
|
|
*/
|
|
|
|
#include "wrp_sdk/platforms/common/mobile_base.hpp"
|
|
|
|
#include <cstring>
|
|
#include <iostream>
|
|
#include <algorithm>
|
|
|
|
#include "stopwatch.h"
|
|
|
|
namespace westonrobot {
|
|
MobileBase::~MobileBase() {
|
|
// 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) {
|
|
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 (can_connected_) can_if_->StopService();
|
|
if (serial_connected_ && serial_if_->IsOpened()) {
|
|
serial_if_->StopService();
|
|
}
|
|
}
|
|
|
|
void MobileBase::ConfigureCAN(const std::string &can_if_name) {
|
|
can_if_ = std::make_shared<AsyncCAN>(can_if_name);
|
|
can_if_->SetReceiveCallback(
|
|
std::bind(&MobileBase::ParseCANFrame, this, std::placeholders::_1));
|
|
can_if_->StartListening();
|
|
can_connected_ = true;
|
|
}
|
|
|
|
void MobileBase::ConfigureSerial(const std::string uart_name,
|
|
int32_t baud_rate) {
|
|
serial_if_ = std::make_shared<AsyncSerial>(uart_name, baud_rate);
|
|
serial_if_->SetReceiveCallback(
|
|
std::bind(&MobileBase::ParseUARTBuffer, this, std::placeholders::_1,
|
|
std::placeholders::_2, std::placeholders::_3));
|
|
serial_if_->StartListening();
|
|
if (serial_if_->IsOpened()) serial_connected_ = true;
|
|
}
|
|
|
|
void MobileBase::StartCmdThread() {
|
|
cmd_thread_ = std::thread(
|
|
std::bind(&MobileBase::ControlLoop, this, cmd_thread_period_ms_));
|
|
cmd_thread_started_ = true;
|
|
}
|
|
|
|
void MobileBase::ControlLoop(int32_t period_ms) {
|
|
StopWatch ctrl_sw;
|
|
bool print_loop_freq = false;
|
|
while (true) {
|
|
ctrl_sw.tic();
|
|
SendRobotCmd();
|
|
ctrl_sw.sleep_until_ms(period_ms);
|
|
if (print_loop_freq)
|
|
std::cout << "control loop frequency: " << 1.0 / ctrl_sw.toc()
|
|
<< std::endl;
|
|
}
|
|
}
|
|
} // namespace westonrobot
|