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

added scout mini support

Browse Source
This commit is contained in:
Ruixiang Du
2020-12-21 11:29:48 +08:00
parent a7460f197b
commit 411fbb9a1e
5 changed files with 132 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
README.md
View File

@@ -15,7 +15,10 @@ This software package provides a C++ interface to communicate with the mobile pl
Supported robot platforms Supported robot platforms
* **Scout**: skid-steer mobile base * **Scout**: skid-steer mobile base
* **Scout-Mini**: skid-steer mobile base
* **Hunter**: ackermann mobile base * **Hunter**: ackermann mobile base
* **Tracer**: differential-drive mobile base
* **Bunker**: tracked mobile base
Supported environments Supported environments

4
ugv_sdk/apps/scout_monitor/src/scout_monitor.cpp
View File

@@ -279,7 +279,7 @@ void ScoutMonitor::ShowVehicleState(int y, int x)
for (int i = linear_axis_origin_y + 1; i < linear_axis_negative_y; ++i) for (int i = linear_axis_origin_y + 1; i < linear_axis_negative_y; ++i)
mvwprintw(body_info_win_, i, linear_axis_x, "-"); mvwprintw(body_info_win_, i, linear_axis_x, "-");
mvwprintw(body_info_win_, linear_axis_negative_y, linear_axis_x, "v"); mvwprintw(body_info_win_, linear_axis_negative_y, linear_axis_x, "v");
double linear_percentage = scout_state_.linear_velocity / ScoutMotionCmd::max_linear_velocity; double linear_percentage = scout_state_.linear_velocity / ScoutCmdLimits::max_linear_velocity;
int linear_bars = std::abs(static_cast<int>(linear_percentage * 5)) + 1; int linear_bars = std::abs(static_cast<int>(linear_percentage * 5)) + 1;
if (std::abs(scout_state_.linear_velocity) < 0.001) if (std::abs(scout_state_.linear_velocity) < 0.001)
linear_bars = 0; linear_bars = 0;
@@ -317,7 +317,7 @@ void ScoutMonitor::ShowVehicleState(int y, int x)
mvwprintw(body_info_win_, angular_axis_y, i, "-"); mvwprintw(body_info_win_, angular_axis_y, i, "-");
mvwprintw(body_info_win_, angular_axis_y, angular_axis_positive_x, ">"); mvwprintw(body_info_win_, angular_axis_y, angular_axis_positive_x, ">");
double angular_percentage = scout_state_.angular_velocity / ScoutMotionCmd::max_angular_velocity; double angular_percentage = scout_state_.angular_velocity / ScoutCmdLimits::max_angular_velocity;
int angular_bars = std::abs(static_cast<int>(angular_percentage * 5)) + 1; int angular_bars = std::abs(static_cast<int>(angular_percentage * 5)) + 1;
if (std::abs(scout_state_.angular_velocity) < 0.001) if (std::abs(scout_state_.angular_velocity) < 0.001)
angular_bars = 0; angular_bars = 0;

5
ugv_sdk/include/ugv_sdk/scout/scout_base.hpp
View File

@@ -26,7 +26,8 @@
namespace westonrobot { namespace westonrobot {
class ScoutBase : public MobileBase { class ScoutBase : public MobileBase {
public: public:
ScoutBase() : MobileBase(){}; ScoutBase(bool is_scout_mini = false)
: MobileBase(), is_scout_mini_(is_scout_mini){};
~ScoutBase() = default; ~ScoutBase() = default;
public: public:
@@ -43,6 +44,8 @@ class ScoutBase : public MobileBase {
ScoutState GetScoutState(); ScoutState GetScoutState();
private: private:
bool is_scout_mini_ = false;
// serial port buffer // serial port buffer
uint8_t tx_cmd_len_; uint8_t tx_cmd_len_;
uint8_t tx_buffer_[SCOUT_CMD_BUF_LEN]; uint8_t tx_buffer_[SCOUT_CMD_BUF_LEN];

47
ugv_sdk/include/ugv_sdk/scout/scout_types.hpp
View File

@@ -13,27 +13,22 @@
#include <cstdint> #include <cstdint>
#include <iostream> #include <iostream>
namespace westonrobot namespace westonrobot {
{ struct ScoutState {
struct ScoutState enum MotorID {
{
enum MotorID
{
FRONT_RIGHT = 0, FRONT_RIGHT = 0,
FRONT_LEFT = 1, FRONT_LEFT = 1,
REAR_LEFT = 2, REAR_LEFT = 2,
REAR_RIGHT = 3 REAR_RIGHT = 3
}; };
struct MotorState struct MotorState {
{
double current = 0; // in A double current = 0; // in A
double rpm = 0; double rpm = 0;
double temperature = 0; double temperature = 0;
}; };
struct LightState struct LightState {
{
uint8_t mode = 0; uint8_t mode = 0;
uint8_t custom_value = 0; uint8_t custom_value = 0;
}; };
@@ -58,10 +53,8 @@ struct ScoutState
double angular_velocity = 0; double angular_velocity = 0;
}; };
struct ScoutMotionCmd struct ScoutMotionCmd {
{ enum class FaultClearFlag {
enum class FaultClearFlag
{
NO_FAULT = 0x00, NO_FAULT = 0x00,
BAT_UNDER_VOL = 0x01, BAT_UNDER_VOL = 0x01,
BAT_OVER_VOL = 0x02, BAT_OVER_VOL = 0x02,
@@ -75,23 +68,31 @@ struct ScoutMotionCmd
ScoutMotionCmd(int8_t linear = 0, int8_t angular = 0, ScoutMotionCmd(int8_t linear = 0, int8_t angular = 0,
FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT) FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT)
: linear_velocity(linear), angular_velocity(angular), : linear_velocity(linear),
angular_velocity(angular),
fault_clear_flag(fault_clr_flag) {} fault_clear_flag(fault_clr_flag) {}
int8_t linear_velocity; int8_t linear_velocity;
int8_t angular_velocity; int8_t angular_velocity;
FaultClearFlag fault_clear_flag; FaultClearFlag fault_clear_flag;
};
struct ScoutCmdLimits {
static constexpr double max_linear_velocity = 1.5; // 1.5 m/s static constexpr double max_linear_velocity = 1.5; // 1.5 m/s
static constexpr double min_linear_velocity = -1.5; // -1.5 m/s static constexpr double min_linear_velocity = -1.5; // -1.5 m/s
static constexpr double max_angular_velocity = 0.5235; // 0.5235 rad/s static constexpr double max_angular_velocity = 0.5235; // 0.5235 rad/s
static constexpr double min_angular_velocity = -0.5235; // -0.5235 rad/s static constexpr double min_angular_velocity = -0.5235; // -0.5235 rad/s
}; };
struct ScoutLightCmd struct ScoutMiniCmdLimits {
{ static constexpr double max_linear_velocity = 3.0; // 3.0 m/s
enum class LightMode static constexpr double min_linear_velocity = -3.0; // -3.0 m/s
{ static constexpr double max_angular_velocity = 2.5235; // 2.5235 rad/s
static constexpr double min_angular_velocity = -2.5235; // -2.5235 rad/s
};
struct ScoutLightCmd {
enum class LightMode {
CONST_OFF = 0x00, CONST_OFF = 0x00,
CONST_ON = 0x01, CONST_ON = 0x01,
BREATH = 0x02, BREATH = 0x02,
@@ -99,8 +100,12 @@ struct ScoutLightCmd
}; };
ScoutLightCmd() = default; ScoutLightCmd() = default;
ScoutLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, uint8_t r_value) : front_mode(f_mode), front_custom_value(f_value), ScoutLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode,
rear_mode(r_mode), rear_custom_value(r_value) {} uint8_t r_value)
: front_mode(f_mode),
front_custom_value(f_value),
rear_mode(r_mode),
rear_custom_value(r_value) {}
LightMode front_mode; LightMode front_mode;
uint8_t front_custom_value; uint8_t front_custom_value;

38
ugv_sdk/src/scout_base.cpp
View File

@@ -145,21 +145,39 @@ void ScoutBase::SetMotionCommand(
// make sure cmd thread is started before attempting to send commands // make sure cmd thread is started before attempting to send commands
if (!cmd_thread_started_) StartCmdThread(); if (!cmd_thread_started_) StartCmdThread();
if (linear_vel < ScoutMotionCmd::min_linear_velocity) if (!is_scout_mini_) {
linear_vel = ScoutMotionCmd::min_linear_velocity; if (linear_vel < ScoutCmdLimits::min_linear_velocity)
if (linear_vel > ScoutMotionCmd::max_linear_velocity) linear_vel = ScoutCmdLimits::min_linear_velocity;
linear_vel = ScoutMotionCmd::max_linear_velocity; if (linear_vel > ScoutCmdLimits::max_linear_velocity)
if (angular_vel < ScoutMotionCmd::min_angular_velocity) linear_vel = ScoutCmdLimits::max_linear_velocity;
angular_vel = ScoutMotionCmd::min_angular_velocity; if (angular_vel < ScoutCmdLimits::min_angular_velocity)
if (angular_vel > ScoutMotionCmd::max_angular_velocity) angular_vel = ScoutCmdLimits::min_angular_velocity;
angular_vel = ScoutMotionCmd::max_angular_velocity; if (angular_vel > ScoutCmdLimits::max_angular_velocity)
angular_vel = ScoutCmdLimits::max_angular_velocity;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_); std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = static_cast<int8_t>( current_motion_cmd_.linear_velocity = static_cast<int8_t>(
linear_vel / ScoutMotionCmd::max_linear_velocity * 100.0); linear_vel / ScoutCmdLimits::max_linear_velocity * 100.0);
current_motion_cmd_.angular_velocity = static_cast<int8_t>( current_motion_cmd_.angular_velocity = static_cast<int8_t>(
angular_vel / ScoutMotionCmd::max_angular_velocity * 100.0); angular_vel / ScoutCmdLimits::max_angular_velocity * 100.0);
current_motion_cmd_.fault_clear_flag = fault_clr_flag; current_motion_cmd_.fault_clear_flag = fault_clr_flag;
} else {
if (linear_vel < ScoutMiniCmdLimits::min_linear_velocity)
linear_vel = ScoutMiniCmdLimits::min_linear_velocity;
if (linear_vel > ScoutMiniCmdLimits::max_linear_velocity)
linear_vel = ScoutMiniCmdLimits::max_linear_velocity;
if (angular_vel < ScoutMiniCmdLimits::min_angular_velocity)
angular_vel = ScoutMiniCmdLimits::min_angular_velocity;
if (angular_vel > ScoutMiniCmdLimits::max_angular_velocity)
angular_vel = ScoutMiniCmdLimits::max_angular_velocity;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = static_cast<int8_t>(
linear_vel / ScoutMiniCmdLimits::max_linear_velocity * 100.0);
current_motion_cmd_.angular_velocity = static_cast<int8_t>(
angular_vel / ScoutMiniCmdLimits::max_angular_velocity * 100.0);
current_motion_cmd_.fault_clear_flag = fault_clr_flag;
}
FeedCmdTimeoutWatchdog(); FeedCmdTimeoutWatchdog();
} }