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

updated ranger base

Browse Source
This commit is contained in:
Ruixiang Du
2021-07-14 23:00:51 +08:00
parent 1b20ecd4d9
commit d234d8caa1
10 changed files with 92 additions and 162 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -12,45 +12,23 @@
#include <string>
#include "ugv_sdk/interface/agilex_message.h"
#include "ugv_sdk/details/interface/agilex_message.h"
namespace westonrobot {
struct RangerState {
struct RangerCoreState {
// system state
SystemStateMessage system_state;
MotionStateMessage motion_state;
LightStateMessage light_state;
RcStateMessage rc_state;
ActuatorHSStateMessage actuator_hs_state[8];
ActuatorLSStateMessage actuator_ls_state[8];
MotionModeStateMessage current_motion_mode;
// sensor data
RcStateMessage rc_state;
OdometryMessage odometry;
};
struct RangerMotionCmd {
double linear_velocity;
double angular_velocity;
};
struct RangerLightCmd {
RangerLightCmd() = default;
RangerLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode,
uint8_t r_value)
: enable_cmd_ctrl(true),
front_mode(f_mode),
front_custom_value(f_value),
rear_mode(r_mode),
rear_custom_value(r_value) {}
bool enable_cmd_ctrl = false;
LightMode front_mode;
uint8_t front_custom_value;
LightMode rear_mode;
uint8_t rear_custom_value;
struct RangerActuatorState {
ActuatorHSStateMessage actuator_hs_state[8];
ActuatorLSStateMessage actuator_ls_state[8];
};
/////////////////////////////////////////////////////////////////////////
@@ -59,13 +37,15 @@ struct RangerInterface {
virtual void Connect(std::string dev_name) = 0;
// robot control
virtual void SetMotionMode(uint8_t mode) = 0;
virtual void SetMotionCommand(double linear_vel, double steer_angle,
double lateral_vel, double angular_vel) = 0;
virtual void SetLightCommand(const RangerLightCmd &cmd) = 0;
virtual void SetMotionMode(uint8_t mode) = 0;
virtual void SetLightCommand(LightMode f_mode, uint8_t f_value,
LightMode r_mode, uint8_t r_value) = 0;
// get robot state
virtual RangerState GetRangerState() = 0;
virtual RangerCoreState GetRobotState() = 0;
virtual RangerActuatorState GetActuatorState() = 0;
};
} // namespace westonrobot

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

@@ -56,8 +56,12 @@ 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(){};
virtual CoreStateMsgGroup GetRobotCoreStateMsgGroup() {
return CoreStateMsgGroup{};
};
virtual ActuatorStateMsgGroup GetActuatorStateMsgGroup() {
return ActuatorStateMsgGroup{};
};
// any specific robot will use a specialized version of the two functions
virtual void SendMotionCommand(double linear_vel, double angular_vel,

6
include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp
View File

@@ -52,12 +52,14 @@ class ProtocolV1Parser : public ParserInterface<ProtocolVersion::AGX_V1> {
}
// UART support
bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg) override {}
bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg) override {
return false;
}
void EncodeMessage(const AgxMessage *msg, uint8_t *buf,
uint8_t *len) override {}
uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) override {}
uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) override { return 0; }
};
using ScoutProtocolV1Parser = ProtocolV1Parser<ScoutV2Limits>;

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

@@ -1,111 +0,0 @@
/**
* @Kit : Qt-Creator: Desktop
* @Author : Wang Zhe
* @Date : 2021-04-19 19:36:24
* @FileName : ranger_base.cpp
* @Mail : wangzheqie@qq.com
* Copyright : AgileX Robotics
**/
#include "ugv_sdk/protocol_v2/ranger_base.hpp"
#include <algorithm>
#include <array>
#include <chrono>
#include <cstdint>
#include <cstring>
#include <iostream>
#include <ratio>
#include <string>
#include <thread>
#include "stopwatch.hpp"
#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
namespace westonrobot {
void RangerBase::Connect(std::string dev_name) {
AgilexBase::ConnectPort(dev_name, std::bind(&RangerBase::ParseCANFrame, this,
std::placeholders::_1));
}
void RangerBase::SetMotionCommand(double linear_vel, double steer_angle,
double lateral_vel, double angular_vel) {
AgilexBase::SetMotionCommand(linear_vel, angular_vel, lateral_vel,
steer_angle / 10.0);
}
void RangerBase::SetLightCommand(const RangerLightCmd &cmd) {
if (cmd.enable_cmd_ctrl) {
AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value,
LightMode::CONST_OFF, 0);
}
}
void RangerBase::SetMotionMode(uint8_t mode) {
AgilexBase::SetMotionMode(mode);
}
RangerState RangerBase::GetRangerState() {
std::lock_guard<std::mutex> guard(state_mutex_);
return ranger_state_;
}
void RangerBase::ParseCANFrame(can_frame *rx_frame) {
AgxMessage status_msg;
DecodeCanFrameV2(rx_frame, &status_msg);
std::lock_guard<std::mutex> guard(state_mutex_);
UpdateRangerState(status_msg, ranger_state_);
}
void RangerBase::UpdateRangerState(const AgxMessage &status_msg,
RangerState &state) {
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;
state.motion_state.steering_angle *= 10;
// std::cout << "steering angle: " << state.motion_state.steering_angle <<
// std::endl;
break;
}
case AgxMsgLightState: {
// std::cout << "light control feedback received" << std::endl;
state.light_state = status_msg.body.light_state_msg;
break;
}
case AgxMsgRcState: {
state.rc_state = status_msg.body.rc_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 AgxMsgMotionModeState: {
state.current_motion_mode = status_msg.body.motion_mode_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

59
include/ugv_sdk/details/robot_base/ranger_base.hpp
View File

@@ -15,32 +15,65 @@
#include <string>
#include <thread>
#include "ugv_sdk/interface/ranger_interface.hpp"
#include "ugv_sdk/protocol_v2/agilex_base.hpp"
#include "ugv_sdk/details/interface/ranger_interface.hpp"
#include "ugv_sdk/details/robot_base/agilex_base.hpp"
#include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp"
namespace westonrobot {
class RangerBase : public AgilexBase, public RangerInterface {
class RangerBase : public AgilexBase<ProtocolV2Parser>, public RangerInterface {
public:
RangerBase() : AgilexBase(){};
RangerBase() : AgilexBase<ProtocolV2Parser>(){};
~RangerBase() = default;
// set up connection
void Connect(std::string dev_name) override;
void Connect(std::string dev_name) override {
AgilexBase::ConnectPort(dev_name, std::bind(&RangerBase::ParseCANFrame,
this, std::placeholders::_1));
}
// robot control
void SetMotionCommand() {}
void SetMotionCommand(double linear_vel, double steer_angle,
double lateral_vel = 0.0, double angular_vel = 0.0);
void SetLightCommand(const RangerLightCmd &cmd);
void SetMotionMode(uint8_t mode);
double lateral_vel = 0.0,
double angular_vel = 0.0) override {
AgilexBase<ProtocolV2Parser>::SendMotionCommand(
linear_vel, angular_vel, lateral_vel, steer_angle / 10.0);
}
void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode,
uint8_t r_value) override {
AgilexBase<ProtocolV2Parser>::SendLightCommand(f_mode, f_value, r_mode,
r_value);
}
void SetMotionMode(uint8_t mode) { AgilexBase::SetMotionMode(mode); }
// get robot state
RangerState GetRangerState();
RangerCoreState GetRobotState() override {
auto state = AgilexBase<ProtocolV2Parser>::GetRobotCoreStateMsgGroup();
private:
RangerState ranger_state_;
RangerCoreState ranger_state;
ranger_state.system_state = state.system_state;
ranger_state.motion_state = state.motion_state;
ranger_state.light_state = state.light_state;
ranger_state.rc_state = state.rc_state;
return ranger_state;
}
void ParseCANFrame(can_frame *rx_frame) override;
void UpdateRangerState(const AgxMessage &status_msg, RangerState &state);
RangerActuatorState GetActuatorState() override {
auto actuator = AgilexBase<ProtocolV2Parser>::GetActuatorStateMsgGroup();
RangerActuatorState ranger_actuator;
for (int i = 0; i < 8; ++i) {
ranger_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i];
ranger_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i];
}
return ranger_actuator;
}
};
using RangerBaseV2 = RangerBase;
} // namespace westonrobot
#endif // RANGER_BASE_HPP

19
include/ugv_sdk/mobile_robot/ranger_robot.hpp Normal file
View File

@@ -0,0 +1,19 @@
/*
* ranger_robot.hpp
*
* Created on: Jul 14, 2021 21:45
* Description:
*
* Copyright (c) 2021 Ruixiang Du (rdu)
*/
#ifndef RANGER_ROBOT_HPP
#define RANGER_ROBOT_HPP
#include "ugv_sdk/details/robot_base/ranger_base.hpp"
namespace westonrobot {
using RangerRobot = RangerBaseV2;
} // namespace westonrobot
#endif /* RANGER_ROBOT_HPP */