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

8
plugins/directional_layer.cpp
View File

@@ -162,14 +162,6 @@ namespace robot_costmap_2d
robot_nav_msgs::OccupancyGrid lanes;
convertToMap(costmap_, lanes, 0.65, 0.196);
//////////////////////////////////
//////////////////////////////////
/////////THAY THẾ PUBLISH////////
// lane_mask_pub_.publish(lanes);
//////////////////////////////////
//////////////////////////////////
//////////////////////////////////
return false;
}

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)
{

20
plugins/voxel_layer.cpp
View File

@@ -331,8 +331,8 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
// bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
// if (publish_clearing_points)
// {
clearing_endpoints_.points.clear();
clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
// clearing_endpoints_.points.clear();
// clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
// }
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
@@ -412,20 +412,20 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
// if (publish_clearing_points)
// {
robot_geometry_msgs::Point32 point;
point.x = wpx;
point.y = wpy;
point.z = wpz;
clearing_endpoints_.points.push_back(point);
// robot_geometry_msgs::Point32 point;
// point.x = wpx;
// point.y = wpy;
// point.z = wpz;
// clearing_endpoints_.points.push_back(point);
// }
}
}
// if (publish_clearing_points)
// {
clearing_endpoints_.header.frame_id = global_frame_;
clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
// clearing_endpoints_.header.frame_id = global_frame_;
// clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
// clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
// clearing_endpoints_pub_.publish(clearing_endpoints_);
// }