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

add file costmap_2d_robot

Browse Source
This commit is contained in:
duongtd
2025-11-18 11:44:01 +07:00
parent 0c61984d3e
commit 59ec58a018
18 changed files with 203 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
plugins/directional_layer.cpp
View File

@@ -412,12 +412,12 @@ namespace costmap_2d
return false;
}
// Copy map data given proper transformations
tf3::Transform tf2_transform;
tf2_transform = convertToTf2Transform(transformMsg.transform);
x = tf2_transform.getOrigin().x();
y = tf2_transform.getOrigin().y();
tf3::Transform tf3_transform;
tf3_transform = convertTotf3Transform(transformMsg.transform);
x = tf3_transform.getOrigin().x();
y = tf3_transform.getOrigin().y();
// Extract the rotation as a quaternion from the transform
tf3::Quaternion rotation = tf2_transform.getRotation();
tf3::Quaternion rotation = tf3_transform.getRotation();
// Convert the quaternion to a yaw angle (in radians)
yaw = tf3::getYaw(rotation);
return true;

29
plugins/static_layer.cpp
View File

@@ -6,7 +6,9 @@
#include <tf3/convert.h>
#include <tf3/utils.h>
#include <boost/dll/alias.hpp>
#include <filesystem>
#include <string>
#include <iostream>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
@@ -35,7 +37,22 @@ void StaticLayer::onInitialize()
bool StaticLayer::getParams()
{
try {
YAML::Node config = YAML::LoadFile("../costmap_2d/config/config.yaml");
std::string config_file_name = "config.yaml";
std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
if(path_source != " ")
{
std::cout << "Path source: " << path_source << std::endl;
}
else
{
std::cout << "/cfg folder not found!" << std::endl;
}
YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["static_layer"];
@@ -299,9 +316,9 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
return;
}
// Copy map data given proper transformations
tf3::Transform tf2_transform;
tf2_transform = convertToTf2Transform(transformMsg.transform);
// tf3::convert(transformMsg.transform, tf2_transform);
tf3::Transform tf3_transform;
tf3_transform = convertTotf3Transform(transformMsg.transform);
// tf3::convert(transformMsg.transform, tf3_transform);
for (unsigned int i = min_i; i < max_i; ++i)
{
for (unsigned int j = min_j; j < max_j; ++j)
@@ -310,7 +327,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
// Transform from global_frame_ to map_frame_
tf3::Vector3 p(wx, wy, 0);
p = tf2_transform*p;
p = tf3_transform*p;
// Set master_grid with cell from map
if (worldToMap(p.x(), p.y(), mx, my))
{

14
plugins/voxel_layer.cpp
View File

@@ -65,15 +65,15 @@ bool VoxelLayer::getParams()
YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml");
YAML::Node layer = config["voxel_layer"];
publish_voxel_ = loadParam(layer, "publish_voxel_map", false);
// publish_voxel_ = loadParam(layer, "publish_voxel_map", false);
enabled_ = loadParam(layer, "enabled", true);
footprint_clearing_enabled_ = loadParam(layer, "footprint_clearing_enabled", true);
max_obstacle_height_ = loadParam(layer, "max_obstacle_height", true);
size_z_ = loadParam(layer, "z_voxels", true);
origin_z_ = loadParam(layer, "origin_z", true);
z_resolution_ = loadParam(layer, "z_resolution", true);
unknown_threshold_ = loadParam(layer, "max_obstacle_height", true);
mark_threshold_ = loadParam(layer, "mark_threshold", true);
max_obstacle_height_ = loadParam(layer, "max_obstacle_height", 2.0);
size_z_ = loadParam(layer, "z_voxels", 10);
origin_z_ = loadParam(layer, "origin_z", 0);
z_resolution_ = loadParam(layer, "z_resolution", 0.2);
unknown_threshold_ = loadParam(layer, "max_obstacle_height", 15);
mark_threshold_ = loadParam(layer, "mark_threshold", 0);
combination_method_ = loadParam(layer, "combination_method", true);
this->matchSize();
}