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

moved state update function to agilex base class

Browse Source
This commit is contained in:
Ruixiang Du
2021-07-14 10:53:24 +08:00
parent 257787eb1b
commit 0d99c56f95
12 changed files with 176 additions and 166 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
demo/scout_demo/scout_robot_demo.cpp
View File

@@ -94,13 +94,15 @@ int main(int argc, char **argv) {
<< state.motion_state.linear_velocity << ", "
<< state.motion_state.angular_velocity << std::endl;
auto actuator = scout->GetActuatorState();
if (scout->GetProtocolVersion() == ProtocolVersion::AGX_V1) {
for (int i = 0; i < 4; ++i) {
printf("motor %d: current %f, rpm %d, driver temp %f, motor temp %f\n",
state.actuator_state[i].motor_id,
state.actuator_state[i].current, state.actuator_state[i].rpm,
state.actuator_state[i].driver_temp,
state.actuator_state[i].motor_temp);
actuator.actuator_state[i].motor_id,
actuator.actuator_state[i].current,
actuator.actuator_state[i].rpm,
actuator.actuator_state[i].driver_temp,
actuator.actuator_state[i].motor_temp);
}
} else {
}

10
include/ugv_sdk/details/interface/agilex_message.h
View File

@@ -127,7 +127,7 @@ typedef struct {
typedef struct {
uint8_t motion_mode;
uint8_t mode_changing;
} MotionModeFeedbackMessage;
} MotionModeStateMessage;
// V1-only messages
typedef struct {
@@ -273,10 +273,11 @@ typedef enum {
AgxMsgSystemState,
AgxMsgMotionState,
AgxMsgLightState,
AgxMsgMotionModeState,
AgxMsgRcState,
// actuator feedback
AgxMsgActuatorHSState,
AgxMsgActuatorLSState,
AgxMsgMotionModeState,
// sensor
AgxMsgOdometry,
AgxMsgImuAccel,
@@ -309,14 +310,15 @@ typedef struct {
LightCommandMessage light_command_msg;
BrakingCommandMessage braking_command_msg;
MotionModeCommandMessage motion_mode_msg;
// state feedback
// core state feedback
SystemStateMessage system_state_msg;
MotionStateMessage motion_state_msg;
LightStateMessage light_state_msg;
MotionModeStateMessage motion_mode_state_msg;
RcStateMessage rc_state_msg;
// actuator feedback
ActuatorHSStateMessage actuator_hs_state_msg;
ActuatorLSStateMessage actuator_ls_state_msg;
MotionModeFeedbackMessage motion_mode_feedback_msg;
// sensor
OdometryMessage odometry_msg;
ImuAccelMessage imu_accel_msg;

2
include/ugv_sdk/details/interface/ranger_interface.hpp
View File

@@ -25,7 +25,7 @@ struct RangerState {
ActuatorHSStateMessage actuator_hs_state[8];
ActuatorLSStateMessage actuator_ls_state[8];
MotionModeFeedbackMessage current_motion_mode;
MotionModeStateMessage current_motion_mode;
// sensor data
OdometryMessage odometry;

20
include/ugv_sdk/details/interface/robot_interface.hpp
View File

@@ -15,7 +15,25 @@
#include "ugv_sdk/details/interface/agilex_message.h"
#include "ugv_sdk/details/interface/parser_interface.hpp"
#define AGX_MAX_ACTUATOR_NUM 8
namespace westonrobot {
struct CoreStateMsgGroup {
SystemStateMessage system_state;
MotionStateMessage motion_state;
LightStateMessage light_state;
MotionModeStateMessage motion_mode_state;
RcStateMessage rc_state;
};
struct ActuatorStateMsgGroup {
ActuatorHSStateMessage actuator_hs_state[AGX_MAX_ACTUATOR_NUM]; // v2 only
ActuatorLSStateMessage actuator_ls_state[AGX_MAX_ACTUATOR_NUM]; // v2 only
ActuatorStateMessageV1 actuator_state[AGX_MAX_ACTUATOR_NUM]; // v1 only
};
struct CommonSensorStateMsgGroup {};
class RobotInterface {
public:
~RobotInterface() = default;
@@ -38,6 +56,8 @@ class RobotInterface {
/****** functions not available/valid to all robots ******/
// functions to be implemented by class AgilexBase
virtual void SetMotionMode(uint8_t mode){};
virtual CoreStateMsgGroup GetRobotCoreStateMsgGroup(){};
virtual ActuatorStateMsgGroup GetActuatorStateMsgGroup(){};
// any specific robot will use a specialized version of the two functions
virtual void SendMotionCommand(double linear_vel, double angular_vel,

11
include/ugv_sdk/details/interface/scout_interface.hpp
View File

@@ -15,11 +15,14 @@
#include "ugv_sdk/details/interface/agilex_message.h"
namespace westonrobot {
struct ScoutState {
struct ScoutCoreState {
SystemStateMessage system_state;
MotionStateMessage motion_state;
LightStateMessage light_state;
RcStateMessage rc_state;
};
struct ScoutActuatorState {
// actuator state
// - for v2 robots only
ActuatorHSStateMessage actuator_hs_state[4];
@@ -36,10 +39,8 @@ struct ScoutInterface {
LightMode r_mode, uint8_t r_value) = 0;
// get robot state
virtual ScoutState GetRobotState() = 0;
virtual RcStateMessage GetRcState(){
// TODO
};
virtual ScoutCoreState GetRobotState() = 0;
virtual ScoutActuatorState GetActuatorState() = 0;
};
} // namespace westonrobot

93
include/ugv_sdk/details/robot_base/agilex_base.hpp
View File

@@ -37,6 +37,11 @@ class AgilexBase : public RobotInterface {
AgilexBase(const AgilexBase &hunter) = delete;
AgilexBase &operator=(const AgilexBase &hunter) = delete;
void Connect(std::string can_name) override {
ConnectPort(can_name, std::bind(&AgilexBase<ParserType>::ParseCANFrame,
this, std::placeholders::_1));
}
// switch to commanded mode
void EnableCommandedMode() {
// construct message
@@ -123,13 +128,38 @@ class AgilexBase : public RobotInterface {
}
}
void ResetRobotState() override {}
ProtocolVersion GetProtocolVersion() override {
return parser_.GetProtocolVersion();
}
CoreStateMsgGroup GetRobotCoreStateMsgGroup() override {
std::lock_guard<std::mutex> guard(core_state_mtx_);
return core_state_msgs_;
}
ActuatorStateMsgGroup GetActuatorStateMsgGroup() override {
std::lock_guard<std::mutex> guard(actuator_state_mtx_);
return actuator_state_msgs_;
}
protected:
ParserType parser_;
std::mutex state_mutex_;
// divide feedback messages into smaller groups to avoid the
// state mutex being locked for too often such that accessing
// the data become difficult
/* feedback group 1: core state */
std::mutex core_state_mtx_;
CoreStateMsgGroup core_state_msgs_;
/* feedback group 2: actuator state */
std::mutex actuator_state_mtx_;
ActuatorStateMsgGroup actuator_state_msgs_;
/* feedback group 3: common sensor */
// communication interface
bool can_connected_ = false;
@@ -148,7 +178,66 @@ class AgilexBase : public RobotInterface {
if (can_connected_) can_->StopService();
}
virtual void ParseCANFrame(can_frame *rx_frame) = 0;
virtual void ParseCANFrame(can_frame *rx_frame) {
AgxMessage status_msg;
if (parser_.DecodeMessage(rx_frame, &status_msg)) {
UpdateRobotCoreState(status_msg);
UpdateActuatorState(status_msg);
}
}
void UpdateRobotCoreState(const AgxMessage &status_msg) {
std::lock_guard<std::mutex> guard(core_state_mtx_);
switch (status_msg.type) {
case AgxMsgSystemState: {
// std::cout << "system status feedback received" << std::endl;
core_state_msgs_.system_state = status_msg.body.system_state_msg;
break;
}
case AgxMsgMotionState: {
// std::cout << "motion control feedback received" << std::endl;
core_state_msgs_.motion_state = status_msg.body.motion_state_msg;
break;
}
case AgxMsgLightState: {
// std::cout << "light control feedback received" << std::endl;
core_state_msgs_.light_state = status_msg.body.light_state_msg;
break;
}
default:
break;
}
}
void UpdateActuatorState(const AgxMessage &status_msg) {
std::lock_guard<std::mutex> guard(actuator_state_mtx_);
switch (status_msg.type) {
case AgxMsgActuatorHSState: {
// std::cout << "actuator hs feedback received" << std::endl;
actuator_state_msgs_
.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;
actuator_state_msgs_
.actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] =
status_msg.body.actuator_ls_state_msg;
break;
}
case AgxMsgActuatorStateV1: {
// std::cout << "actuator v1 feedback received" << std::endl;
actuator_state_msgs_
.actuator_state[status_msg.body.v1_actuator_state_msg.motor_id] =
status_msg.body.v1_actuator_state_msg;
break;
}
default:
break;
}
}
};
} // namespace westonrobot

2
include/ugv_sdk/details/robot_base/ranger_base.cpp
View File

@@ -96,7 +96,7 @@ void RangerBase::UpdateRangerState(const AgxMessage &status_msg,
break;
}
case AgxMsgMotionModeState: {
state.current_motion_mode = status_msg.body.motion_mode_feedback_msg;
state.current_motion_mode = status_msg.body.motion_mode_state_msg;
break;
}
/* sensor feedback */

105
include/ugv_sdk/details/robot_base/scout_base.hpp
View File

@@ -19,17 +19,15 @@
#include "ugv_sdk/details/robot_base/agilex_base.hpp"
namespace westonrobot {
template <typename Parser>
class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
template <typename ParserType>
class ScoutBase : public AgilexBase<ParserType>, public ScoutInterface {
public:
ScoutBase() : AgilexBase<Parser>(){};
ScoutBase() : AgilexBase<ParserType>(){};
~ScoutBase() = default;
// set up connection
void Connect(std::string can_name) override {
AgilexBase<Parser>::ConnectPort(
can_name, std::bind(&ScoutBase<Parser>::ParseCANFrame, this,
std::placeholders::_1));
AgilexBase<ParserType>::Connect(can_name);
}
void Connect(std::string uart_name, uint32_t baudrate) override {
@@ -38,92 +36,37 @@ class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
// robot control
void SetMotionCommand(double linear_vel, double angular_vel) override {
AgilexBase<Parser>::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
AgilexBase<ParserType>::SendMotionCommand(linear_vel, angular_vel, 0.0,
0.0);
}
void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode,
uint8_t r_value) override {
AgilexBase<Parser>::SendLightCommand(f_mode, f_value, r_mode, r_value);
AgilexBase<ParserType>::SendLightCommand(f_mode, f_value, r_mode, r_value);
}
// get robot state
ScoutState GetRobotState() override {
std::lock_guard<std::mutex> guard(AgilexBase<Parser>::state_mutex_);
return scout_state_;
ScoutCoreState GetRobotState() override {
auto state = AgilexBase<ParserType>::GetRobotCoreStateMsgGroup();
ScoutCoreState scout_state;
scout_state.system_state = state.system_state;
scout_state.motion_state = state.motion_state;
scout_state.light_state = state.light_state;
scout_state.rc_state = state.rc_state;
return scout_state;
}
RcStateMessage GetRcState() override {}
ScoutActuatorState GetActuatorState() override {
auto actuator = AgilexBase<ParserType>::GetActuatorStateMsgGroup();
void ResetRobotState() override {
std::lock_guard<std::mutex> guard(rc_state_mutex_);
}
private:
ScoutState scout_state_;
RcStateMessage rc_state_;
std::mutex rc_state_mutex_;
void ParseCANFrame(can_frame *rx_frame) override {
AgxMessage status_msg;
if (AgilexBase<Parser>::parser_.DecodeMessage(rx_frame, &status_msg)) {
if (status_msg.type == AgxMsgRcState)
UpdateRcState(status_msg);
else
UpdateScoutState(status_msg, scout_state_);
}
}
void UpdateRcState(const AgxMessage &status_msg) {
std::lock_guard<std::mutex> guard(rc_state_mutex_);
rc_state_ = status_msg.body.rc_state_msg;
}
void UpdateScoutState(const AgxMessage &status_msg, ScoutState &state) {
std::lock_guard<std::mutex> guard(AgilexBase<Parser>::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 AgxMsgActuatorStateV1: {
// std::cout << "actuator v1 feedback received" << std::endl;
state.actuator_state[status_msg.body.v1_actuator_state_msg.motor_id] =
status_msg.body.v1_actuator_state_msg;
break;
}
/* sensor feedback */
// case AgxMsgOdometry: {
// // std::cout << "Odometer msg feedback received" << std::endl;
// state.odometry = status_msg.body.odometry_msg;
// }
default:
break;
ScoutActuatorState scout_actuator;
for (int i = 0; i < 4; ++i) {
scout_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i];
scout_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i];
scout_actuator.actuator_state[i] = actuator.actuator_state[i];
}
return scout_actuator;
}
};
} // namespace westonrobot

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

@@ -21,7 +21,8 @@
#include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp"
namespace westonrobot {
class TracerBaseV2 : public AgilexBase<ProtocolV2Parser>, public TracerInterface {
class TracerBaseV2 : public AgilexBase<ProtocolV2Parser>,
public TracerInterface {
public:
TracerBaseV2() : AgilexBase<ProtocolV2Parser>(){};
~TracerBaseV2() = default;
@@ -29,8 +30,8 @@ class TracerBaseV2 : public AgilexBase<ProtocolV2Parser>, public TracerInterface
// set up connection
void Connect(std::string can_name) override {
AgilexBase<ProtocolV2Parser>::ConnectPort(
can_name, std::bind(&TracerBaseV2::ParseCANFrame, this,
std::placeholders::_1));
can_name,
std::bind(&TracerBaseV2::ParseCANFrame, this, std::placeholders::_1));
}
void Connect(std::string uart_name, uint32_t baudrate) override {
// TODO
@@ -38,78 +39,24 @@ class TracerBaseV2 : public AgilexBase<ProtocolV2Parser>, public TracerInterface
// robot control
void SetMotionCommand(double linear_vel, double angular_vel) override {
AgilexBase<ProtocolV2Parser>::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
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);
AgilexBase<ProtocolV2Parser>::SendLightCommand(f_mode, f_value, CONST_OFF,
0);
}
// get robot state
TracerState GetRobotState() override {
std::lock_guard<std::mutex> guard(AgilexBase<ProtocolV2Parser>::state_mutex_);
return tracer_state_;
// 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 {
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

3
include/ugv_sdk/mobile_robot/scout_robot.hpp
View File

@@ -37,7 +37,8 @@ class ScoutRobot : public RobotInterface, public ScoutInterface {
ProtocolVersion GetProtocolVersion() override;
// get robot state
ScoutState GetRobotState() override;
ScoutCoreState GetRobotState() override;
ScoutActuatorState GetActuatorState() override;
private:
RobotInterface* robot_;

7
src/mobile_robot/scout_robot.cpp
View File

@@ -58,8 +58,13 @@ void ScoutRobot::SetLightCommand(LightMode f_mode, uint8_t f_value,
scout->SetLightCommand(f_mode, f_value, r_mode, r_value);
}
ScoutState ScoutRobot::GetRobotState() {
ScoutCoreState ScoutRobot::GetRobotState() {
auto scout = dynamic_cast<ScoutInterface*>(robot_);
return scout->GetRobotState();
}
ScoutActuatorState ScoutRobot::GetActuatorState() {
auto scout = dynamic_cast<ScoutInterface*>(robot_);
return scout->GetActuatorState();
}
} // namespace westonrobot

4
src/protocol_v2/agilex_msg_parser_v2.c
View File

@@ -194,8 +194,8 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) {
msg->type = AgxMsgMotionModeState;
MotionModeStateFrame *frame = (MotionModeStateFrame *)(rx_frame->data);
msg->body.motion_mode_feedback_msg.motion_mode = frame->motion_mode;
msg->body.motion_mode_feedback_msg.mode_changing = frame->mode_changing;
msg->body.motion_mode_state_msg.motion_mode = frame->motion_mode;
msg->body.motion_mode_state_msg.mode_changing = frame->mode_changing;
break;
}
/****************** sensor frame *****************/