DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

otimal deep coppy obj

Browse Source
This commit is contained in:
duongtd
2026-07-14 11:08:23 +07:00
parent 6a9834d3a8
commit bdbb03aa51
17 changed files with 702 additions and 301 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
src/costmap_2d_robot.cpp
View File

@@ -155,9 +155,20 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name,const std::st
if (priv_nh.hasParam("track_unknown_space"))
priv_nh.getParam("track_unknown_space", track_unknown_space);
bool performance_metrics_enabled =
loadParam(layer, "performance_metrics_enabled", false);
double performance_metrics_period =
loadParam(layer, "performance_metrics_period", 5.0);
if (priv_nh.hasParam("performance_metrics_enabled"))
priv_nh.getParam("performance_metrics_enabled", performance_metrics_enabled);
if (priv_nh.hasParam("performance_metrics_period"))
priv_nh.getParam("performance_metrics_period", performance_metrics_period);
if (priv_nh.hasParam("library_path"))
path_plugins = loader.findLibraryPath(name_);
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
layered_costmap_->setPerformanceMetrics(
performance_metrics_enabled, performance_metrics_period);
// find size parameters
double map_width_meters = loadParam(layer, "width", 0.0);
@@ -692,35 +703,9 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
// get the global pose of the robot
try
{
// use current time if possible (makes sure it's not in the future)
if (tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time()))
{
tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_,tf3::Time());
tf3::doTransform(robot_pose, global_pose, transform);
// robot::log_error("%s ||| %f | %f | %f ||| %f | %f | %f | %f", transform.child_frame_id.c_str(),
// global_pose.pose.position.x,
// global_pose.pose.position.y,
// global_pose.pose.position.z,
// global_pose.pose.orientation.x,
// global_pose.pose.orientation.y,
// global_pose.pose.orientation.z,
// global_pose.pose.orientation.w);
// transform.transform.rotation.x,
// transform.transform.rotation.y,
// transform.transform.rotation.z,
// transform.transform.rotation.w);
}
// use the latest otherwise
else
{
// tf_.transform(robot_pose, global_pose, global_frame_);
tf3::TransformStampedMsg transform = tf_.lookupTransform(
global_frame_, // frame đích
robot_base_frame_, // frame nguồn
tf3::Time()
);
tf3::doTransform(robot_pose, global_pose, transform);
}
const tf3::TransformStampedMsg transform =
tf_.lookupTransform(global_frame_, robot_base_frame_, tf3::Time());
tf3::doTransform(robot_pose, global_pose, transform);
}
catch (tf3::LookupException& ex)
{