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

1738_29102025

Browse Source
This commit is contained in:
duongtd
2025-10-29 17:38:43 +07:00
parent 7c1dcfd352
commit cdb9ded893
51 changed files with 2055 additions and 1216 deletions
Download Patch File Download Diff File Expand all files Collapse all files

251
plugins/static_layer.cpp
View File

@@ -1,48 +1,10 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* Copyright (c) 2015, Fetch Robotics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/static_layer.h>
#include <costmap_2d/costmap_math.h>
#include <pluginlib/class_list_macros.hpp>
#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
PLUGINLIB_EXPORT_CLASS(costmap_2d::StaticLayer, costmap_2d::Layer)
#include <tf2/utils.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
@@ -51,92 +13,78 @@ using costmap_2d::FREE_SPACE;
namespace costmap_2d
{
StaticLayer::StaticLayer() : dsrv_(NULL)
StaticLayer::StaticLayer()
{
threshold_ = &lethal_threshold_;
}
StaticLayer::~StaticLayer()
{
if (dsrv_)
delete dsrv_;
}
{}
void StaticLayer::onInitialize()
{
ros::NodeHandle nh("~/" + name_), g_nh;
// ros::NodeHandle nh("~/" + name_), g_nh;
current_ = true;
global_frame_ = layered_costmap_->getGlobalFrameID();
nh.param("map_topic", map_topic_, std::string("map"));
nh.param("first_map_only", first_map_only_, false);
nh.param("subscribe_to_updates", subscribe_to_updates_, false);
// nh.param("map_topic", map_topic_, std::string("map"));
// nh.param("first_map_only", first_map_only_, false);
// nh.param("subscribe_to_updates", subscribe_to_updates_, false);
nh.param("track_unknown_space", track_unknown_space_, true);
nh.param("use_maximum", use_maximum_, false);
// nh.param("track_unknown_space", track_unknown_space_, true);
// nh.param("use_maximum", use_maximum_, false);
int temp_lethal_threshold, temp_unknown_cost_value;
nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
nh.param("trinary_costmap", trinary_costmap_, true);
nh.param("base_frame_id", base_frame_id_, std::string("base_footprint"));
// int temp_lethal_threshold, temp_unknown_cost_value;
// nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
// nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
// nh.param("trinary_costmap", trinary_costmap_, true);
// nh.param("base_frame_id", base_frame_id_, std::string("base_footprint"));
// ROS_WARN("global_frame_[%s] map_frame_[%s] map_topic[%s] first_map_only[%d] subscribe_to_updates[%d] track_unknown_space[%d] use_maximum[%d] \n lethal_cost_threshold[%d] unknown_cost_value[%d] trinary_costmap[%d]",
// global_frame_.c_str(), map_frame_.c_str(),map_topic_.c_str(), first_map_only_, subscribe_to_updates_, track_unknown_space_, use_maximum_, temp_lethal_threshold, temp_unknown_cost_value, trinary_costmap_);
lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
unknown_cost_value_ = temp_unknown_cost_value;
// lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
// unknown_cost_value_ = temp_unknown_cost_value;
// Only resubscribe if topic has changed
if (map_sub_.getTopic() != ros::names::resolve(map_topic_))
{
// we'll subscribe to the latched topic that the map server uses
ROS_INFO("Requesting the map...");
map_sub_ = g_nh.subscribe(map_topic_, 1, &StaticLayer::incomingMap, this);
map_received_ = false;
has_updated_data_ = false;
// if (map_sub_.getTopic() != ros::names::resolve(map_topic_))
// {
// // we'll subscribe to the latched topic that the map server uses
// printf("Requesting the map...");
// map_sub_ = g_nh.subscribe(map_topic_, 1, &StaticLayer::incomingMap, this);
// map_received_ = false;
// has_updated_data_ = false;
ros::Rate r(10);
while (!map_received_ && g_nh.ok())
{
ros::spinOnce();
r.sleep();
}
// ros::Rate r(10);
// while (!map_received_ && g_nh.ok())
// {
// ros::spinOnce();
// r.sleep();
// }
ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
// printf("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
if (subscribe_to_updates_)
{
ROS_INFO("Subscribing to updates");
map_update_sub_ = g_nh.subscribe(map_topic_ + "_updates", 10, &StaticLayer::incomingUpdate, this);
}
}
else
{
has_updated_data_ = true;
}
// if (subscribe_to_updates_)
// {
// printf("Subscribing to updates");
// map_update_sub_ = g_nh.subscribe(map_topic_ + "_updates", 10, &StaticLayer::incomingUpdate, this);
// }
// }
// else
// {
// has_updated_data_ = true;
// }
if (dsrv_)
{
delete dsrv_;
}
dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb =
[this](auto& config, auto level){ reconfigureCB(config, level); };
dsrv_->setCallback(cb);
}
void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
if (config.enabled != enabled_)
{
enabled_ = config.enabled;
has_updated_data_ = true;
x_ = y_ = 0;
width_ = size_x_;
height_ = size_y_;
}
}
// void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
// {
// if (config.enabled != enabled_)
// {
// enabled_ = config.enabled;
// has_updated_data_ = true;
// x_ = y_ = 0;
// width_ = size_x_;
// height_ = size_y_;
// }
// }
void StaticLayer::matchSize()
{
@@ -166,36 +114,36 @@ unsigned char StaticLayer::interpretValue(unsigned char value)
return scale * LETHAL_OBSTACLE;
}
void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
{
unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
unsigned int size_x = new_map.info.width, size_y = new_map.info.height;
ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);
printf("Received a %d X %d map at %f m/pix", size_x, size_y, new_map.info.resolution);
// resize costmap if size, resolution or origin do not match
Costmap2D* master = layered_costmap_->getCostmap();
if (!layered_costmap_->isRolling() &&
(master->getSizeInCellsX() != size_x ||
master->getSizeInCellsY() != size_y ||
master->getResolution() != new_map->info.resolution ||
master->getOriginX() != new_map->info.origin.position.x ||
master->getOriginY() != new_map->info.origin.position.y))
master->getResolution() != new_map.info.resolution ||
master->getOriginX() != new_map.info.origin.position.x ||
master->getOriginY() != new_map.info.origin.position.y))
{
// Update the size of the layered costmap (and all layers, including this one)
ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
new_map->info.origin.position.y,
printf("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map.info.resolution);
layered_costmap_->resizeMap(size_x, size_y, new_map.info.resolution, new_map.info.origin.position.x,
new_map.info.origin.position.y,
true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
}
else if (size_x_ != size_x || size_y_ != size_y ||
resolution_ != new_map->info.resolution ||
origin_x_ != new_map->info.origin.position.x ||
origin_y_ != new_map->info.origin.position.y)
resolution_ != new_map.info.resolution ||
origin_x_ != new_map.info.origin.position.x ||
origin_y_ != new_map.info.origin.position.y)
{
// only update the size of the costmap stored locally in this layer
ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
resizeMap(size_x, size_y, new_map->info.resolution,
new_map->info.origin.position.x, new_map->info.origin.position.y);
printf("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map.info.resolution);
resizeMap(size_x, size_y, new_map.info.resolution,
new_map.info.origin.position.x, new_map.info.origin.position.y);
}
unsigned int index = 0;
@@ -205,12 +153,12 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
{
for (unsigned int j = 0; j < size_x; ++j)
{
unsigned char value = new_map->data[index];
unsigned char value = new_map.data[index];
costmap_[index] = interpretValue(value);
++index;
}
}
map_frame_ = new_map->header.frame_id;
map_frame_ = new_map.header.frame_id;
// we have a new map, update full size of map
x_ = y_ = 0;
@@ -222,27 +170,27 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
// shutdown the map subscrber if firt_map_only_ flag is on
if (first_map_only_)
{
ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
map_sub_.shutdown();
printf("Shutting down the map subscriber. first_map_only flag is on");
// map_sub_.shutdown();
}
}
void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update)
void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdate& update)
{
unsigned int di = 0;
for (unsigned int y = 0; y < update->height ; y++)
for (unsigned int y = 0; y < update.height ; y++)
{
unsigned int index_base = (update->y + y) * size_x_;
for (unsigned int x = 0; x < update->width ; x++)
unsigned int index_base = (update.y + y) * size_x_;
for (unsigned int x = 0; x < update.width ; x++)
{
unsigned int index = index_base + x + update->x;
costmap_[index] = interpretValue(update->data[di++]);
unsigned int index = index_base + x + update.x;
costmap_[index] = interpretValue(update.data[di++]);
}
}
x_ = update->x;
y_ = update->y;
width_ = update->width;
height_ = update->height;
x_ = update.x;
y_ = update.y;
width_ = update.width;
height_ = update.height;
has_updated_data_ = true;
}
@@ -251,12 +199,12 @@ void StaticLayer::activate()
onInitialize();
}
void StaticLayer::deactivate()
{
map_sub_.shutdown();
if (subscribe_to_updates_)
map_update_sub_.shutdown();
}
// void StaticLayer::deactivate()
// {
// map_sub_.shutdown();
// if (subscribe_to_updates_)
// map_update_sub_.shutdown();
// }
void StaticLayer::reset()
{
@@ -314,18 +262,23 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
double wx, wy;
// Might even be in a different frame
geometry_msgs::TransformStamped transform;
tf2::TransformStampedMsg transformMsg;
try
{
transform = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
// transformMsg = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
tf_->canTransform(map_frame_, global_frame_, tf2::Time());
tf_->lookupTransform(map_frame_,
global_frame_,
tf2::Time());
}
catch (tf2::TransformException ex)
{
ROS_ERROR("%s", ex.what());
printf("%s", ex.what());
return;
}
// Copy map data given proper transformations
tf2::Transform tf2_transform;
tf2::convert(transform.transform, tf2_transform);
// tf2::convert(transform.transform, tf2_transform);
for (unsigned int i = min_i; i < max_i; ++i)
{
for (unsigned int j = min_j; j < max_j; ++j)
@@ -334,7 +287,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_
tf2::Vector3 p(wx, wy, 0);
p = tf2_transform*p;
// p = tf2_transform*p;
// Set master_grid with cell from map
if (worldToMap(p.x(), p.y(), mx, my))
{
@@ -348,4 +301,12 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
}
}
// Export factory function
static PluginPtr create_static_plugin() {
return std::make_shared<StaticLayer>();
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_static_plugin, create_plugin)
} // namespace costmap_2d