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

update from DuongTD

Browse Source
This commit is contained in:
HiepLM
2026-01-08 10:35:27 +07:00
parent f052dac142
commit 384897b750
15 changed files with 110 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/costmap_2d.cpp
View File

@@ -255,6 +255,8 @@ void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my)
{
my = (int)((wy - origin_y_) / resolution_);
}
// printf("CHECK FUNCTION: %f | %f\n",wx,wy);
// printf("CHECK FUNCTION: resolution_: %f\n",resolution_);
}
void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)

27
src/costmap_2d_robot.cpp
View File

@@ -128,6 +128,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
robot::PluginLoaderHelper loader;
if (nh.hasParam("rolling_window"))
nh.getParam("rolling_window", rolling_window);
@@ -395,6 +396,7 @@ void Costmap2DROBOT::updateMap()
double x = pose.pose.position.x,
y = pose.pose.position.y,
yaw = data_convert::getYaw(pose.pose.orientation);
// robot::log_error("ROBOT POSE: %f | %f | %f",x,y,yaw);
layered_costmap_->updateMap(x, y, yaw);
robot_geometry_msgs::PolygonStamped footprint;
footprint.header.frame_id = global_frame_;
@@ -498,6 +500,13 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
{
robot_geometry_msgs::PoseStamped robot_pose;
robot_geometry_msgs::Pose pose_default;
pose_default.orientation.x = 0;
pose_default.orientation.y = 0;
pose_default.orientation.z = 0;
pose_default.orientation.w = 1;
pose_default.position.x = 0;
pose_default.position.y = 0;
pose_default.position.z = 0;
global_pose.pose = pose_default;
robot_pose.pose = pose_default;
@@ -509,10 +518,22 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
try
{
// use current time if possible (makes sure it's not in the future)
if (tf_.canTransform(global_frame_, robot_base_frame_, data_convert::convertTime(current_time)))
if (tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time()))
{
tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, data_convert::convertTime(current_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
@@ -521,7 +542,7 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
tf3::TransformStampedMsg transform = tf_.lookupTransform(
global_frame_, // frame đích
robot_base_frame_, // frame nguồn
data_convert::convertTime(robot_pose.header.stamp)
tf3::Time()
);
tf3::doTransform(robot_pose, global_pose, transform);
}

7
src/layered_costmap.cpp
View File

@@ -111,7 +111,7 @@ namespace robot_costmap_2d
minx_ = miny_ = 1e30;
maxx_ = maxy_ = -1e30;
printf("START\n");
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
@@ -121,6 +121,7 @@ namespace robot_costmap_2d
double prev_miny = miny_;
double prev_maxx = maxx_;
double prev_maxy = maxy_;
std::cout << "robot x: " << robot_x << "\nrobot y: " << robot_y << "\nrobot yaw: " << robot_yaw << std::endl;
(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
{
@@ -131,6 +132,7 @@ namespace robot_costmap_2d
(*plugin)->getName().c_str());
}
}
printf("END\n");
int x0, xn, y0, yn;
costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
@@ -141,7 +143,8 @@ namespace robot_costmap_2d
y0 = std::max(0, y0);
yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
printf("Updating area x: [%d, %d] y: [%d, %d]\n", x0, xn, y0, yn);
// printf("Updating area x: [%d, %d] y: [%d, %d]\n", x0, xn, y0, yn);
// printf("Updating area x: [%f, %f] y: [%f, %f]\n", minx_, miny_, maxx_, maxy_);
if (xn < x0 || yn < y0)
return;

6
src/observation_buffer.cpp
View File

@@ -140,7 +140,8 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
global_frame_, // frame đích
local_origin.header.frame_id, // frame nguồn
data_convert::convertTime(local_origin.header.stamp)
tf3::Time()
// data_convert::convertTime(local_origin.header.stamp)
);
tf3::doTransform(local_origin, global_origin, tfm_1);
@@ -161,7 +162,8 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
global_frame_, // frame đích
cloud.header.frame_id, // frame nguồn
data_convert::convertTime(cloud.header.stamp)
tf3::Time()
// data_convert::convertTime(cloud.header.stamp)
);
tf3::doTransform(cloud, global_frame_cloud, tfm_2);
global_frame_cloud.header.stamp = cloud.header.stamp;