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
2021-07-08 18:20:34 +08:00
parent d2f210e6c1
commit dd810eccba
13 changed files with 148 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
src/mobile_base/scout_robot.cpp
View File

@@ -8,7 +8,45 @@
*/
#include "ugv_sdk/mobile_base/scout_robot.hpp"
#include "ugv_sdk/protocol_v2/scout_base_v2.hpp"
namespace westonrobot {
ScoutRobot::ScoutRobot(AgilexProtocol protocol) {}
ScoutRobot::ScoutRobot(ProtocolType protocol) {
if (protocol == ProtocolType::AGX_V1) {
} else if (protocol == ProtocolType::AGX_V2) {
robot_ = new ScoutBaseV2();
}
}
ScoutRobot::~ScoutRobot() {
if (robot_) delete robot_;
}
void ScoutRobot::EnableCommandedMode() { robot_->EnableCommandedMode(); }
void ScoutRobot::Connect(std::string can_name) { robot_->Connect(can_name); }
void ScoutRobot::Connect(std::string uart_name, uint32_t baudrate) {
robot_->Connect(uart_name, baudrate);
}
void ScoutRobot::ResetRobotState() { robot_->ResetRobotState(); }
void ScoutRobot::SetMotionCommand(double linear_vel, double angular_vel) {
auto scout = dynamic_cast<ScoutInterface*>(robot_);
scout->SetMotionCommand(linear_vel, angular_vel);
}
void ScoutRobot::DisableLightControl() { robot_->DisableLightControl(); }
void ScoutRobot::SetLightCommand(LightMode f_mode, uint8_t f_value,
LightMode r_mode, uint8_t r_value) {
auto scout = dynamic_cast<ScoutInterface*>(robot_);
scout->SetLightCommand(f_mode, f_value, r_mode, r_value);
}
ScoutState ScoutRobot::GetRobotState() {
auto scout = dynamic_cast<ScoutInterface*>(robot_);
return scout->GetRobotState();
}
} // namespace westonrobot

16
src/protocol_v2/scout_base.cpp
View File

@@ -22,37 +22,37 @@
#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
namespace westonrobot {
void ScoutBase::Connect(std::string dev_name) {
AgilexBase::ConnectPort(dev_name, std::bind(&ScoutBase::ParseCANFrame, this,
void ScoutBaseV2::Connect(std::string dev_name) {
AgilexBase::ConnectPort(dev_name, std::bind(&ScoutBaseV2::ParseCANFrame, this,
std::placeholders::_1));
}
void ScoutBase::Connect(std::string uart_name, uint32_t baudrate) {
void ScoutBaseV2::Connect(std::string uart_name, uint32_t baudrate) {
// TODO
}
void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel) {
void ScoutBaseV2::SetMotionCommand(double linear_vel, double angular_vel) {
AgilexBase::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
}
void ScoutBase::SetLightCommand(LightMode f_mode, uint8_t f_value,
void ScoutBaseV2::SetLightCommand(LightMode f_mode, uint8_t f_value,
LightMode r_mode, uint8_t r_value) {
AgilexBase::SendLightCommand(f_mode, f_value, r_mode, r_value);
}
ScoutState ScoutBase::GetRobotState() {
ScoutState ScoutBaseV2::GetRobotState() {
std::lock_guard<std::mutex> guard(state_mutex_);
return scout_state_;
}
void ScoutBase::ParseCANFrame(can_frame *rx_frame) {
void ScoutBaseV2::ParseCANFrame(can_frame *rx_frame) {
AgxMessage status_msg;
DecodeCanFrame(rx_frame, &status_msg);
std::lock_guard<std::mutex> guard(state_mutex_);
UpdateScoutState(status_msg, scout_state_);
}
void ScoutBase::UpdateScoutState(const AgxMessage &status_msg,
void ScoutBaseV2::UpdateScoutState(const AgxMessage &status_msg,
ScoutState &state) {
switch (status_msg.type) {
case AgxMsgSystemState: {