HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
HiepLM
2026-02-27 06:45:35 +00:00
parent 83f0e85e4a
commit ff8a90cbaa
6 changed files with 61 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp
View File

@@ -49,7 +49,11 @@
namespace robot_nav_core_adapter
{
LocalPlannerAdapter::LocalPlannerAdapter() : has_active_goal_(false)
LocalPlannerAdapter::LocalPlannerAdapter()
: initialized_(false),
tf_(nullptr),
costmap_robot_(nullptr),
has_active_goal_(false)
{
}
@@ -139,6 +143,15 @@ namespace robot_nav_core_adapter
{
return false;
}
if(tf_ == nullptr)
{
robot::log_error("[%s:%d]\n ERROR: tf is nullptr", __FILE__, __LINE__);
throw std::runtime_error("Failed to create the tf");
}
else
{
robot::log_info("[%s:%d]\n INFO: tf is not nullptr", __FILE__, __LINE__);
}
if (planner_name != last_config_.planner_name)
{
robot_nav_core2::LocalPlanner::Ptr old_planner = planner_;
@@ -157,15 +170,6 @@ namespace robot_nav_core_adapter
robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s", planner_name.c_str());
throw std::runtime_error("Failed to load planner " + planner_name);
}
if(tf_ == nullptr)
{
robot::log_error("[%s:%d]\n ERROR: tf is nullptr", __FILE__, __LINE__);
throw std::runtime_error("Failed to create the tf");
}
else
{
robot::log_info("[%s:%d]\n INFO: tf is not nullptr", __FILE__, __LINE__);
}
new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_);
planner_ = new_planner;
planner_loader_ = new_loader;

41
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@@ -341,6 +341,8 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
}
initialized_ = true;
setup_ = true;
if(tf_)
robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str());
robot::log_info("========== End: initialize() - SUCCESS ==========");
}
else
@@ -715,13 +717,38 @@ void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_na
// robot::log_info("addOdometry: %s", odometry_name.c_str());
// robot::log_info("odometry header: %s", odometry.header.frame_id.c_str());
// robot::log_info("odometry child_frame_id: %s", odometry.child_frame_id.c_str());
// robot::log_info("odometry pose position x: %f", odometry.pose.pose.position.x);
// robot::log_info("odometry pose position y: %f", odometry.pose.pose.position.y);
// robot::log_info("odometry pose position z: %f", odometry.pose.pose.position.z);
// robot::log_info("odometry pose orientation x: %f", odometry.pose.pose.orientation.x);
// robot::log_info("odometry pose orientation y: %f", odometry.pose.pose.orientation.y);
// robot::log_info("odometry pose orientation z: %f", odometry.pose.pose.orientation.z);
// robot::log_info("odometry pose orientation w: %f", odometry.pose.pose.orientation.w);
if(tf_)
{
// try
// {
// robot_geometry_msgs::PoseStamped global_pose_stamped;
// tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose);
// robot_geometry_msgs::PoseStamped robot_pose;
// tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose);
// robot_pose.header.frame_id = robot_base_frame_;
// robot_pose.header.stamp = robot::Time();
// robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
// // Convert robot::Time to tf3::Time
// tf3::Time tf3_current_time = data_convert::convertTime(current_time);
// tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time());
// std::string error_msg;
// if (tf_->canTransform("odom", "base_link", tf3_current_time, &error_msg))
// {
// // Transform is available at current time
// tf3::TransformStampedMsg transform = tf_->lookupTransform("odom", "base_link", tf3_current_time);
// tf3::doTransform(robot_pose, global_pose_stamped, transform);
// robot::log_info("TF x: %f y: %f theta: %f", global_pose_stamped.pose.position.x, global_pose_stamped.pose.position.y, data_convert::getYaw(global_pose_stamped.pose.orientation));
// }
// }
// catch (const tf3::TransformException &ex)
// {
// robot::log_error("[addOdometry] Failed to lookup base_link in odom: %s", ex.what());
// }
robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str());
}
// robot::log_info("odometry x: %f y: %f theta: %f", odometry.pose.pose.position.x, odometry.pose.pose.position.y, data_convert::getYaw(odometry.pose.pose.orientation));
// std::stringstream pose_covariance_str;
// for(int i = 0; i < 36; i++) {
// pose_covariance_str << odometry.pose.covariance[i] << " ";