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

update tracer_interface and tracer_base

Browse Source
This commit is contained in:
pinloon.lee
2021-07-13 22:04:36 +08:00
parent ce8d9a6e66
commit ddf6bf694f
2 changed files with 86 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
include/ugv_sdk/details/interface/tracer_interface.hpp
View File

@@ -12,7 +12,7 @@
#include <string>
#include "ugv_sdk/interface/agilex_message.h"
#include "ugv_sdk/details/interface/agilex_message.h"
namespace westonrobot {
struct TracerState {
@@ -21,6 +21,7 @@ struct TracerState {
MotionStateMessage motion_state;
LightStateMessage light_state;
// Is this still needed?
RcStateMessage rc_state;
ActuatorHSStateMessage actuator_hs_state[2];
@@ -30,36 +31,14 @@ struct TracerState {
OdometryMessage odometry;
};
struct TracerMotionCmd {
double linear_velocity;
double angular_velocity;
};
struct TracerLightCmd {
TracerLightCmd() = default;
TracerLightCmd(LightMode f_mode, uint8_t f_value)
: enable_cmd_ctrl(true),
front_mode(f_mode),
front_custom_value(f_value) {}
bool enable_cmd_ctrl = false;
LightMode front_mode;
uint8_t front_custom_value;
};
/////////////////////////////////////////////////////////////////////////
struct TracerInterface {
// set up connection
virtual void Connect(std::string can_name) = 0;
virtual void Connect(std::string uart_name, uint32_t baudrate) = 0;
virtual ~TracerInterface() = default;
// robot control
virtual void SetMotionCommand(double linear_vel, double angular_vel) = 0;
virtual void SetLightCommand(const TracerLightCmd &cmd) = 0;
virtual void SetLightCommand(LightMode f_mode, uint8_t f_value) = 0;
// get robot state
virtual TracerState GetTracerState() = 0;
virtual TracerState GetRobotState() = 0;
};
} // namespace westonrobot

92
include/ugv_sdk/details/robot_base/tracer_base.hpp
View File

@@ -15,31 +15,101 @@
#include <thread>
#include <mutex>
#include "ugv_sdk/interface/tracer_interface.hpp"
#include "ugv_sdk/protocol_v2/agilex_base.hpp"
#include "ugv_sdk/details/interface/tracer_interface.hpp"
#include "ugv_sdk/details/robot_base/agilex_base.hpp"
#include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp"
namespace westonrobot {
class TracerBaseV2 : public AgilexBase, public TracerInterface {
class TracerBaseV2 : public AgilexBase<ProtocolV2Parser>, public TracerInterface {
public:
TracerBaseV2() : AgilexBase(){};
TracerBaseV2() : AgilexBase<ProtocolV2Parser>(){};
~TracerBaseV2() = default;
// set up connection
void Connect(std::string can_name) override;
void Connect(std::string uart_name, uint32_t baudrate) override;
void Connect(std::string can_name) override {
AgilexBase<ProtocolV2Parser>::ConnectPort(
can_name, std::bind(&TracerBaseV2::ParseCANFrame, this,
std::placeholders::_1));
}
void Connect(std::string uart_name, uint32_t baudrate) override {
// TODO
}
// robot control
void SetMotionCommand(double linear_vel, double angular_vel);
void SetLightCommand(const TracerLightCmd &cmd);
void SetMotionCommand(double linear_vel, double angular_vel) override {
AgilexBase<ProtocolV2Parser>::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
}
void SetLightCommand(LightMode f_mode, uint8_t f_value) override {
AgilexBase<ProtocolV2Parser>::SendLightCommand(f_mode, f_value, CONST_OFF, 0);
}
// get robot state
TracerState GetTracerState();
TracerState GetRobotState() override {
std::lock_guard<std::mutex> guard(AgilexBase<ProtocolV2Parser>::state_mutex_);
return tracer_state_;
}
void ResetRobotState() override {
// TODO
}
private:
TracerState tracer_state_;
void ParseCANFrame(can_frame *rx_frame) override;
void UpdateTracerState(const AgxMessage &status_msg, TracerState &state);
void ParseCANFrame(can_frame *rx_frame) override {
AgxMessage status_msg;
if (AgilexBase<ProtocolV2Parser>::parser_.DecodeMessage(rx_frame, &status_msg)) {
UpdateTracerState(status_msg, tracer_state_);
}
}
void UpdateTracerState(const AgxMessage &status_msg, TracerState &state) {
std::lock_guard<std::mutex> guard(AgilexBase<ProtocolV2Parser>::state_mutex_);
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 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;
}
// case AgxMsgRcState: {
// state.rc_state = status_msg.body.rc_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