update for ROS

This commit is contained in:
2026-01-07 17:03:07 +07:00
parent ec6d5746e3
commit 76b563ad4d
5 changed files with 14 additions and 8 deletions

View File

@@ -3,6 +3,7 @@
#include <robot/plugin_loader_helper.h>
#include <robot/console.h>
#include <robot/node_handle.h>
#include <tf3/buffer_core.h>
namespace amr_control
{
@@ -16,6 +17,7 @@ namespace amr_control
loc_base_ptr_(nullptr),
move_base_ptr_(nullptr),
amr_safety_ptr_(nullptr),
tf_core_ptr_(nullptr),
loc_base_loader_("loc_core", "loc_core::BaseLocalization")
{
this->init(nh, buffer);
@@ -31,6 +33,7 @@ namespace amr_control
loc_base_ptr_(nullptr),
move_base_ptr_(nullptr),
amr_safety_ptr_(nullptr),
tf_core_ptr_(nullptr),
loc_base_loader_("loc_core", "loc_core::BaseLocalization")
{
}
@@ -79,6 +82,8 @@ namespace amr_control
{
nh_ = nh;
tf_ = buffer;
// Create tf3::BufferCore instance for move_base
tf_core_ptr_ = std::make_shared<tf3::BufferCore>();
monitor_thr_ = std::make_shared<std::thread>(
[this]()
@@ -231,7 +236,7 @@ namespace amr_control
robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__);
move_base_ptr_ = move_base_loader_();
// move_base_ptr_->initialize(tf_core_ptr_);
move_base_ptr_->initialize(tf_core_ptr_);
ros::Rate r(3);
do

View File

@@ -21,7 +21,7 @@ int main(int argc, char** argv)
}
catch (const std::exception& exc)
{
std::cout << exc.what() << std::endl;
ROS_ERROR("Exception: %s", exc.what());
}
return 0;
}