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 9, 2025
1 parent b6c8c9e commit 6c7408f
Show file tree
Hide file tree
Showing 3 changed files with 69 additions and 35 deletions.
5 changes: 3 additions & 2 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,11 +32,12 @@ class BunkerBaseRos : public rclcpp::Node {
std::string base_frame_;
std::string odom_topic_name_;

bool is_bunker_mini_ = false;
//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_;
Expand Down
6 changes: 3 additions & 3 deletions bunker_base/launch/bunker_base.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ 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',
bunker_version_arg = DeclareLaunchArgument('bunker_version', default_value='1',
description='Scout mini model')
simulated_robot_arg = DeclareLaunchArgument('simulated_robot', default_value='false',
description='Whether running with simulator')
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_version': launch.substitutions.LaunchConfiguration('bunker_version'),
'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_version_arg,
simulated_robot_arg,
sim_control_rate_arg,
bunker_base_node
Expand Down
93 changes: 63 additions & 30 deletions bunker_base/src/bunker_base_ros.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,42 +12,60 @@
#include "bunker_base/bunker_messenger.hpp"
#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::kBunkerV1));
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>("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::kBunkerV1));
this->get_parameter_or<int>("control_rate", sim_control_rate_, 50);

std::cout << "Loading parameters: " << std::endl;
std::cout << "- port name: " << port_name_ << std::endl;
std::cout << "- odom frame name: " << odom_frame_ << std::endl;
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 {
std::cout << "Bunker Mini" << std::endl;
}

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

bool BunkerBaseRos::Initialize() {
if (is_bunker_mini_) {
std::cout << "Robot base: Bunker Mini" << std::endl;
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 {
std::cout << "Robot base: Bunker" << std::endl;
std::cout << "Robot base: Bunker Mini" << 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 +121,11 @@ 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));
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 6c7408f

Please sign in to comment.