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

add function computeCost file layer.h

Browse Source
This commit is contained in:
duongtd
2026-07-10 11:27:20 +07:00
parent 2fcd211ccf
commit a2a021c114
10 changed files with 1355 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
plugins/static_layer.cpp
View File

@@ -44,6 +44,7 @@
#include <boost/dll/alias.hpp>
#include <fstream>
#include <cxxabi.h>
using robot_costmap_2d::NO_INFORMATION;
@@ -71,6 +72,7 @@ void StaticLayer::onInitialize()
global_frame_ = layered_costmap_->getGlobalFrameID();
std::string config_file_name = "static_layer_params.yaml";
getParams(config_file_name, priv_nh);
robot::log_warning("Initializing static layer with map topic \"%s\" in frame \"%s\"", map_topic_.c_str(), global_frame_.c_str());
}
bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
@@ -191,12 +193,23 @@ void StaticLayer::handleImpl(const void* data,
const std::type_info& type,
const std::string& topic)
{
if (type == typeid(robot_nav_msgs::OccupancyGrid) && topic == map_topic_) {
if (type == typeid(robot_nav_msgs::OccupancyGrid) &&
(topic == map_topic_ || topic == "/" + map_topic_)) {
incomingMap(*static_cast<const robot_nav_msgs::OccupancyGrid*>(data));
} else if (type == typeid(robot_map_msgs::OccupancyGridUpdate) && topic == map_topic_ + "_updates") {
} else if (type == typeid(robot_map_msgs::OccupancyGridUpdate) &&
(topic == map_topic_ + "_updates" || topic == "/" + map_topic_ + "_updates")) {
incomingUpdate(*static_cast<const robot_map_msgs::OccupancyGridUpdate*>(data));
} else {
std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
std::string readable = boost::core::demangle(type.name());
size_t pos = readable.find("<");
if (pos != std::string::npos)
{
readable = readable.substr(0, pos);
}
robot::log_error("[] con1: %x, con2: %x ", type == typeid(robot_nav_msgs::OccupancyGrid), (topic == map_topic_ || topic == "/" + map_topic_));
robot::log_error("[StaticLayer] Received data of unknown type: %s on topic: %s, map_topic_: %s\n", readable.c_str(), topic.c_str(), map_topic_.c_str());
// std::cout << "[StaticLayer] Unknown type: " << type.name() << " on topic: " << topic << std::endl;
}
}
@@ -204,7 +217,7 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
{
if(!map_shutdown_)
{
std::cout << "Received new map!" << std::endl;
std::cout << "[StaticLayer] Received new map!" << std::endl;
unsigned int size_x = new_map.info.width, size_y = new_map.info.height;
robot::log_info("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution);
@@ -381,7 +394,7 @@ void StaticLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_
tf3::TransformStampedMsg transformMsg;
try
{
transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time::now());
transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time());
}
catch (tf3::TransformException ex)
{