Skip to content

Commit

Permalink
Updated to work with ugv_sdk ver0.10
Browse files Browse the repository at this point in the history
  • Loading branch information
@Nabeelkii
Nabeelkii committed Jan 10, 2025
1 parent b6c8c9e commit e2e64a9
Show file tree
Hide file tree
Showing 4 changed files with 73 additions and 39 deletions.
5 changes: 5 additions & 0 deletions .vscode/settings.json
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
{
"files.associations": {
"ostream": "cpp"
}
}
8 changes: 2 additions & 6 deletions bunker_base/include/bunker_base/bunker_base_ros.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,15 +32,11 @@ class BunkerBaseRos : public rclcpp::Node {
std::string base_frame_;
std::string odom_topic_name_;

bool is_bunker_mini_ = false;
// bool is_omni_wheel_ = false;

bool simulated_robot_ = false;
int sim_control_rate_ = 50;

int bunker_variant_;
int sim_control_rate_ = 50;

std::shared_ptr<BunkerRobot> robot_;
// std::shared_ptr<BunkerMiniOmniRobot> omni_robot_;

std::atomic<bool> keep_running_;

Expand Down
8 changes: 4 additions & 4 deletions bunker_base/launch/bunker_base.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,8 @@ def generate_launch_description():
description='Base link frame id')
odom_topic_arg = DeclareLaunchArgument('odom_topic_name', default_value='odom',
description='Odometry topic name')
is_bunker_mini_arg = DeclareLaunchArgument('is_bunker_mini', default_value='false',
description='Scout mini model')
bunker_variant_arg = DeclareLaunchArgument('bunker_variant', default_value='0',
description='Bunker robot')
simulated_robot_arg = DeclareLaunchArgument('simulated_robot', default_value='false',
description='Whether running with simulator')
sim_control_rate_arg = DeclareLaunchArgument('control_rate', default_value='50',
Expand All @@ -39,7 +39,7 @@ def generate_launch_description():
'odom_frame': launch.substitutions.LaunchConfiguration('odom_frame'),
'base_frame': launch.substitutions.LaunchConfiguration('base_frame'),
'odom_topic_name': launch.substitutions.LaunchConfiguration('odom_topic_name'),
'is_bunker_mini': launch.substitutions.LaunchConfiguration('is_bunker_mini'),
'bunker_variant': launch.substitutions.LaunchConfiguration('bunker_variant'),
'simulated_robot': launch.substitutions.LaunchConfiguration('simulated_robot'),
'control_rate': launch.substitutions.LaunchConfiguration('control_rate'),
}])
Expand All @@ -50,7 +50,7 @@ def generate_launch_description():
odom_frame_arg,
base_link_frame_arg,
odom_topic_arg,
is_bunker_mini_arg,
bunker_variant_arg,
simulated_robot_arg,
sim_control_rate_arg,
bunker_base_node
Expand Down
91 changes: 62 additions & 29 deletions bunker_base/src/bunker_base_ros.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,30 +13,33 @@
#include "ugv_sdk/utilities/protocol_detector.hpp"

namespace westonrobot {

BunkerBaseRos::BunkerBaseRos(std::string node_name)
: rclcpp::Node(node_name), keep_running_(false) {
this->declare_parameter("port_name");
this->declare_parameter("port_name", "can0");

this->declare_parameter("odom_frame");
this->declare_parameter("base_frame");
this->declare_parameter("odom_topic_name");
this->declare_parameter("odom_frame", "odom");
this->declare_parameter("base_frame", "base_link");
this->declare_parameter("odom_topic_name", "odom");

this->declare_parameter("is_bunker_mini");
this->declare_parameter("simulated_robot");
this->declare_parameter("control_rate");
this->declare_parameter("simulated_robot", false);
this->declare_parameter("bunker_variant",
static_cast<int>(BunkerRobot::Variant::kBunkerV2));
this->declare_parameter("control_rate", 50);

LoadParameters();
}

void BunkerBaseRos::LoadParameters() {
this->get_parameter_or<std::string>("port_name", port_name_, "can0");

this->get_parameter_or<std::string>("odom_frame", odom_frame_, "odom");
this->get_parameter_or<std::string>("base_frame", base_frame_, "base_link");
this->get_parameter_or<std::string>("odom_topic_name", odom_topic_name_,
"odom");
this->get_parameter_or<bool>("is_bunker_mini", is_bunker_mini_, false);
this->get_parameter_or<bool>("simulated_robot", simulated_robot_, false);
this->get_parameter_or<int>(
"bunker_variant", bunker_variant_,
static_cast<int>(BunkerRobot::Variant::kBunkerV2));
this->get_parameter_or<int>("control_rate", sim_control_rate_, 50);

std::cout << "Loading parameters: " << std::endl;
Expand All @@ -45,9 +48,23 @@ void BunkerBaseRos::LoadParameters() {
std::cout << "- base frame name: " << base_frame_ << std::endl;
std::cout << "- odom topic name: " << odom_topic_name_ << std::endl;

std::cout << "- is bunker mini: " << std::boolalpha << is_bunker_mini_
<< std::endl;

std::cout << "- bunker variant: ";

if (bunker_variant_ == static_cast<int>(BunkerRobot::Variant::kBunkerV1)) {
std::cout << "Bunker V1" << std::endl;
} else if (bunker_variant_ ==
static_cast<int>(BunkerRobot::Variant::kBunkerV2)) {
std::cout << "Bunker V2" << std::endl;
} else if (bunker_variant_ ==
static_cast<int>(BunkerRobot::Variant::kBunkerPro)) {
std::cout << "Bunker Pro" << std::endl;
} else if (bunker_variant_ ==
static_cast<int>(BunkerRobot::Variant::kBunkerMini)) {
std::cout << "Bunker Mini" << std::endl;
}
else{
std::cout << "Unknown" << std::endl;
}

std::cout << "- simulated robot: " << std::boolalpha << simulated_robot_
<< std::endl;
Expand All @@ -56,26 +73,43 @@ void BunkerBaseRos::LoadParameters() {
}

bool BunkerBaseRos::Initialize() {
if (is_bunker_mini_) {
if (bunker_variant_ == static_cast<int>(BunkerRobot::Variant::kBunkerV1)) {
std::cout << "Robot base: Bunker V1" << std::endl;
} else if (bunker_variant_ ==
static_cast<int>(BunkerRobot::Variant::kBunkerV2)) {
std::cout << "Robot base: Bunker V2" << std::endl;
} else if (bunker_variant_ ==
static_cast<int>(BunkerRobot::Variant::kBunkerPro)) {
std::cout << "Robot base: Bunker Pro" << std::endl;
} else if (bunker_variant_ ==
static_cast<int>(BunkerRobot::Variant::kBunkerMini)) {
std::cout << "Robot base: Bunker Mini" << std::endl;
} else {
std::cout << "Robot base: Bunker" << std::endl;
std::cout << "Unknown" << std::endl;
}

ProtocolDetector detector;
if (detector.Connect(port_name_)) {
auto proto = detector.DetectProtocolVersion(5);
if (proto == ProtocolVersion::AGX_V1) {
std::cout << "Detected protocol: AGX_V1" << std::endl;
robot_ = std::unique_ptr<BunkerRobot>(
new BunkerRobot(ProtocolVersion::AGX_V1));
}
else if (proto == ProtocolVersion::AGX_V2)
{
std::cout << "Detected protocol: AGX_V2" << std::endl;
robot_ = std::unique_ptr<BunkerRobot>(
new BunkerRobot(ProtocolVersion::AGX_V2));
}else {
if (bunker_variant_ == static_cast<int>(BunkerRobot::Variant::kBunkerV1)) {
std::cout << "Detected protocol: AGX_V1" << std::endl;
robot_ = std::unique_ptr<BunkerRobot>(
new BunkerRobot(BunkerRobot::Variant::kBunkerV1));
} else if (bunker_variant_ ==
static_cast<int>(BunkerRobot::Variant::kBunkerV2)) {
std::cout << "Detected protocol: AGX_V2" << std::endl;
robot_ = std::unique_ptr<BunkerRobot>(
new BunkerRobot(BunkerRobot::Variant::kBunkerV2));
} else if (bunker_variant_ ==
static_cast<int>(BunkerRobot::Variant::kBunkerPro)) {
std::cout << "Detected protocol: AGX_V2" << std::endl;
robot_ = std::unique_ptr<BunkerRobot>(
new BunkerRobot(BunkerRobot::Variant::kBunkerPro));
} else if (bunker_variant_ ==
static_cast<int>(BunkerRobot::Variant::kBunkerMini)) {
std::cout << "Detected protocol: AGX_V2" << std::endl;
robot_ = std::unique_ptr<BunkerRobot>(
new BunkerRobot(BunkerRobot::Variant::kBunkerMini));
} else {
std::cout << "Detected protocol: UNKONWN" << std::endl;
return false;
}
Expand All @@ -89,9 +123,9 @@ bool BunkerBaseRos::Initialize() {
void BunkerBaseRos::Stop() { keep_running_ = false; }

void BunkerBaseRos::Run() {

std::unique_ptr<BunkerMessenger<BunkerRobot>> messenger =
std::unique_ptr<BunkerMessenger<BunkerRobot>>(new BunkerMessenger<BunkerRobot>(robot_,this));
std::unique_ptr<BunkerMessenger<BunkerRobot>>(
new BunkerMessenger<BunkerRobot>(robot_, this));

messenger->SetOdometryFrame(odom_frame_);
messenger->SetBaseFrame(base_frame_);
Expand Down Expand Up @@ -122,6 +156,5 @@ void BunkerBaseRos::Run() {
rclcpp::spin_some(shared_from_this());
rate.sleep();
}

}
} // namespace westonrobot

0 comments on commit e2e64a9

Please sign in to comment.