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 omni support, updated scout related demo

Browse Source
This commit is contained in:
Ruixiang Du
2021-07-16 14:15:12 +08:00
parent 58314696a9
commit 2a94b95f4a
10 changed files with 255 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -51,6 +51,13 @@ struct ScoutInterface {
virtual ScoutCoreState GetRobotState() = 0;
virtual ScoutActuatorState GetActuatorState() = 0;
};
struct ScoutOmniInterface {
virtual ~ScoutOmniInterface() = default;
virtual void SetMotionCommand(double linear_vel, double angular_vel,
double lateral_velocity) = 0;
};
} // namespace westonrobot
#endif /* SCOUT_INTERFACE_HPP */

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

@@ -30,6 +30,8 @@ class ProtocolV1Parser : public ParserInterface<ProtocolVersion::AGX_V1> {
if (msg->type == AgxMsgMotionCommandV1) {
float linear = msg->body.v1_motion_command_msg.linear;
float angular = msg->body.v1_motion_command_msg.angular;
float lateral = msg->body.v1_motion_command_msg.lateral;
if (linear > RobotLimitsType::max_linear)
linear = RobotLimitsType::max_linear;
else if (linear < RobotLimitsType::min_linear)
@@ -38,11 +40,17 @@ class ProtocolV1Parser : public ParserInterface<ProtocolVersion::AGX_V1> {
angular = RobotLimitsType::max_angular;
else if (angular < RobotLimitsType::min_angular)
angular = RobotLimitsType::min_angular;
if (lateral > RobotLimitsType::max_lateral)
lateral = RobotLimitsType::max_lateral;
else if (lateral < RobotLimitsType::min_lateral)
lateral = RobotLimitsType::min_lateral;
msg_v1.body.v1_motion_command_msg.linear =
linear / RobotLimitsType::max_linear * 100.0;
msg_v1.body.v1_motion_command_msg.angular =
angular / RobotLimitsType::max_angular * 100.0;
msg_v1.body.v1_motion_command_msg.lateral =
lateral / RobotLimitsType::max_lateral * 100.0;
}
return EncodeCanFrameV1(&msg_v1, tx_frame);
}

12
include/ugv_sdk/details/protocol_v1/robot_limits.hpp
View File

@@ -16,6 +16,9 @@ struct ScoutV2Limits {
static constexpr double min_linear = -max_linear;
static constexpr double max_angular = 0.5235; // rad/s
static constexpr double min_angular = -max_angular;
static constexpr double max_lateral = 0.0;
static constexpr double min_lateral = 0.0;
};
struct ScoutMiniLimits {
@@ -23,6 +26,9 @@ struct ScoutMiniLimits {
static constexpr double min_linear = -max_linear;
static constexpr double max_angular = 2.5235; // rad/s
static constexpr double min_angular = -max_angular;
static constexpr double max_lateral = 2.0;
static constexpr double min_lateral = -max_lateral;
};
struct BunkerLimits {
@@ -30,6 +36,9 @@ struct BunkerLimits {
static constexpr double min_linear = -max_linear;
static constexpr double max_angular = 2.5235; // rad/s
static constexpr double min_angular = -max_angular;
static constexpr double max_lateral = 0.0;
static constexpr double min_lateral = 0.0;
};
struct HunterV1Limits {
@@ -37,6 +46,9 @@ struct HunterV1Limits {
static constexpr double min_linear = -max_linear;
static constexpr double max_angular = 25.5; // degree
static constexpr double min_angular = -max_angular;
static constexpr double max_lateral = 0.0;
static constexpr double min_lateral = 0.0;
};
} // namespace westonrobot

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

@@ -55,7 +55,7 @@ class AgilexBase : public RobotCommonInterface {
// must be called at a frequency >= 50Hz
void SendMotionCommand(double linear_vel, double angular_vel,
double lateral_velocity, double steering_angle) {
double lateral_vel, double steering_angle) {
if (can_connected_) {
// motion control message
AgxMessage msg;
@@ -68,19 +68,18 @@ class AgilexBase : public RobotCommonInterface {
msg.body.v1_motion_command_msg.angular =
std::abs(angular_vel) > std::abs(steering_angle) ? angular_vel
: steering_angle;
msg.body.v1_motion_command_msg.lateral = lateral_velocity;
msg.body.v1_motion_command_msg.lateral = lateral_vel;
} else if (parser_.GetParserProtocolVersion() ==
ProtocolVersion::AGX_V2) {
msg.type = AgxMsgMotionCommand;
msg.body.motion_command_msg.linear_velocity = linear_vel;
msg.body.motion_command_msg.angular_velocity = angular_vel;
msg.body.motion_command_msg.lateral_velocity = lateral_velocity;
msg.body.motion_command_msg.lateral_velocity = lateral_vel;
msg.body.motion_command_msg.steering_angle = steering_angle;
}
// std::cout << "sending motion cmd: " << linear_vel << "," <<
// angular_vel
// << std::endl;
std::cout << "sending motion cmd: " << linear_vel << "," << angular_vel
<< "," << lateral_vel << std::endl;
// send to can bus
can_frame frame;

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

@@ -40,8 +40,9 @@ class ScoutBase : public AgilexBase<ParserType>, public ScoutInterface {
0.0);
}
void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode,
uint8_t r_value) override {
void SetLightCommand(LightMode f_mode, uint8_t f_value,
LightMode r_mode = LightMode::CONST_ON,
uint8_t r_value = 0) override {
AgilexBase<ParserType>::SendLightCommand(f_mode, f_value, r_mode, r_value);
}
@@ -71,6 +72,20 @@ class ScoutBase : public AgilexBase<ParserType>, public ScoutInterface {
return scout_actuator;
}
};
template <typename ParserType>
class ScoutMiniOmniBase : public ScoutBase<ParserType>,
public ScoutOmniInterface {
public:
void SetMotionCommand(double linear_vel, double angular_vel,
double lateral_velocity) override {
AgilexBase<ParserType>::SendMotionCommand(linear_vel, angular_vel,
lateral_velocity, 0.0);
}
private:
using ScoutBase<ParserType>::SetMotionCommand;
};
} // namespace westonrobot
#include "ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp"
@@ -79,9 +94,11 @@ class ScoutBase : public AgilexBase<ParserType>, public ScoutInterface {
namespace westonrobot {
using ScoutBaseV1 = ScoutBase<ScoutProtocolV1Parser>;
using ScoutMiniBaseV1 = ScoutBase<ScoutMiniProtocolV1Parser>;
using ScoutMiniOmniBaseV1 = ScoutMiniOmniBase<ScoutMiniProtocolV1Parser>;
using ScoutBaseV2 = ScoutBase<ProtocolV2Parser>;
using ScoutMiniBaseV2 = ScoutBase<ProtocolV2Parser>;
using ScoutMiniOmniBaseV2 = ScoutMiniOmniBase<ProtocolV2Parser>;
} // namespace westonrobot
#endif /* SCOUT_BASE_HPP */

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

@@ -20,7 +20,7 @@ class ScoutRobot : public RobotCommonInterface, public ScoutInterface {
public:
ScoutRobot(ProtocolVersion protocol = ProtocolVersion::AGX_V2,
bool is_mini_model = false);
~ScoutRobot();
virtual ~ScoutRobot();
void Connect(std::string can_name) override;
void Connect(std::string uart_name, uint32_t baudrate) override;
@@ -40,9 +40,24 @@ class ScoutRobot : public RobotCommonInterface, public ScoutInterface {
ScoutCoreState GetRobotState() override;
ScoutActuatorState GetActuatorState() override;
private:
protected:
RobotCommonInterface* robot_;
};
///////////////////////////////////////////////////////////////////////////
class ScoutMiniOmniRobot : public ScoutRobot, public ScoutOmniInterface {
public:
ScoutMiniOmniRobot(ProtocolVersion protocol = ProtocolVersion::AGX_V2);
~ScoutMiniOmniRobot();
void SetMotionCommand(double linear_vel, double angular_vel,
double lateral_velocity) override;
private:
using ScoutRobot::SetMotionCommand;
// void SetMotionCommand(double linear_vel, double angular_vel) = delete;
};
} // namespace westonrobot
#endif /* SCOUT_ROBOT_HPP */