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

fix core dumped err when loadplugin

Browse Source
This commit is contained in:
duongtd
2025-12-01 17:34:23 +07:00
parent 2439f2a71d
commit 11ff1baa79
34 changed files with 1177 additions and 760 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
plugins/critical_layer.cpp
View File

@@ -36,12 +36,12 @@ void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, i
}
// Export factory function
static PluginLayerPtr create_critical_plugin() {
static std::shared_ptr<Layer> create_critical_plugin() {
return std::make_shared<CriticalLayer>();
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_critical_plugin, create_plugin)
BOOST_DLL_ALIAS(create_critical_plugin, create_critical_layer)
}

4
plugins/directional_layer.cpp
View File

@@ -430,10 +430,10 @@ namespace costmap_2d
}
// Export factory function
static PluginLayerPtr create_directional_plugin() {
static std::shared_ptr<Layer> create_directional_plugin() {
return std::make_shared<DirectionalLayer>();
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_directional_plugin, create_plugin)
BOOST_DLL_ALIAS(create_directional_plugin, create_directional_layer)
}

70
plugins/inflation_layer.cpp Normal file → Executable file
View File

@@ -1,11 +1,48 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, 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 <algorithm>
#include <costmap_2d/inflation_layer.h>
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/footprint.h>
#include <costmap_2d/utils.h>
#include <boost/thread.hpp>
#include <boost/dll/alias.hpp>
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
using costmap_2d::NO_INFORMATION;
@@ -21,7 +58,6 @@ InflationLayer::InflationLayer()
, inflate_unknown_(false)
, cell_inflation_radius_(0)
, cached_cell_inflation_radius_(0)
// , dsrv_(NULL)
, seen_(NULL)
, cached_costs_(NULL)
, cached_distances_(NULL)
@@ -44,16 +80,27 @@ void InflationLayer::onInitialize()
seen_size_ = 0;
need_reinflation_ = false;
getParams();
}
InflationLayer::matchSize();
matchSize();
}
bool InflationLayer::getParams()
{
try {
YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/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["inflation_layer"];
double cost_scaling_factor = loadParam(layer, "cost_scaling_factor", 15.0);
double inflation_radius = loadParam(layer, "inflation_radius", 0.55);
@@ -146,21 +193,19 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
return;
// make sure the inflation list is empty at the beginning of the cycle (should always be true)
if (!inflation_cells_.empty()) {
throw std::runtime_error("The inflation list must be empty at the beginning of inflation");
}
printf("The inflation list must be empty at the beginning of inflation\n");
unsigned char* master_array = master_grid.getCharMap();
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
if (seen_ == NULL) {
std::cerr <<"InflationLayer::updateCosts(): seen_ array is NULL" <<std::endl;
printf("InflationLayer::updateCosts(): seen_ array is NULL\n");
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
else if (seen_size_ != size_x * size_y)
{
std::cerr <<"InflationLayer::updateCosts(): seen_ array size is wrong" <<std::endl;
printf("InflationLayer::updateCosts(): seen_ array size is wrong\n");
delete[] seen_;
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
@@ -357,12 +402,13 @@ void InflationLayer::handleImpl(const void* data,
printf("This function is not available!\n");
}
// Export factory function
static PluginLayerPtr create_inflation_plugin() {
static std::shared_ptr<Layer> create_inflation_plugin() {
return std::make_shared<InflationLayer>();
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_inflation_plugin, create_plugin)
BOOST_DLL_ALIAS(create_inflation_plugin, create_inflation_layer)
} // namespace costmap_2d

215
plugins/obstacle_layer.cpp Normal file → Executable file
View File

@@ -1,11 +1,49 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, 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/obstacle_layer.h>
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/utils.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <tf3/convert.h>
#include <tf3/utils.h>
#include <tf3/exceptions.h>
#include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;
@@ -27,91 +65,103 @@ void ObstacleLayer::onInitialize()
getParams();
}
ObstacleLayer::~ObstacleLayer()
{}
bool ObstacleLayer::getParams()
{
try {
YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml");
YAML::Node layer = config["obstacle_layer"];
bool track_unknown_space = loadParam(layer, "track_unknown_space", layered_costmap_->isTrackingUnknown());
if (track_unknown_space)
default_value_ = NO_INFORMATION;
else
default_value_ = FREE_SPACE;
double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2);
// get the topics that we'll subscribe to from the parameter server
std::string topics_string = loadParam(layer,"observation_sources", std::string(""));
printf(" Subscribed to Topics: %s\n", topics_string.c_str());
// get the parameters for the specific topic
double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2;
std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud";
bool inf_is_valid = false, clearing=false, marking=true;
topic = loadParam(layer,"topic", topic);
sensor_frame = loadParam(layer,"sensor_frame", std::string(""));
observation_keep_time = loadParam(layer,"observation_persistence", 0.0);
expected_update_rate = loadParam(layer,"expected_update_rate", 0.0);
data_type = loadParam(layer,"data_type", std::string("PointCloud"));
min_obstacle_height = loadParam(layer,"min_obstacle_height", 0.0);
max_obstacle_height = loadParam(layer,"max_obstacle_height", 2.0);
inf_is_valid = loadParam(layer,"inf_is_valid", false);
clearing = loadParam(layer,"clearing", false);
marking = loadParam(layer,"marking", true);
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
try {
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 != " ")
{
printf("Only topics that use point clouds or laser scans are currently supported\n");
throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
std::cout << "Path source: " << path_source << std::endl;
}
else
{
std::cout << "/cfg folder not found!" << std::endl;
}
std::string raytrace_range_param_name, obstacle_range_param_name;
YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["obstacle_layer"];
double obstacle_range = 2.5;
obstacle_range = loadParam(layer,"obstacle_range", obstacle_range);
double raytrace_range = 3.0;
raytrace_range = loadParam(layer,"raytrace_range", raytrace_range);
bool track_unknown_space = loadParam(layer, "track_unknown_space", layered_costmap_->isTrackingUnknown());
if (track_unknown_space)
default_value_ = NO_INFORMATION;
else
default_value_ = FREE_SPACE;
double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2);
// get the topics that we'll subscribe to from the parameter server
std::string topics_string = loadParam(layer,"observation_sources", std::string(""));
printf(" Subscribed to Topics: %s\n", topics_string.c_str());
bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true);
int combination_method = loadParam(layer, "combination_method", 1);
// enabled_ = enabled;
footprint_clearing_enabled_ = footprint_clearing_enabled;
max_obstacle_height_ = max_obstacle_height;
combination_method_ = combination_method;
printf("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
sensor_frame.c_str());
// create an observation buffer
observation_buffers_.push_back(
boost::shared_ptr < ObservationBuffer
> (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
sensor_frame, transform_tolerance)));
if (marking)
marking_buffers_.push_back(observation_buffers_.back());
// check if we'll also add this buffer to our clearing observation buffers
if (clearing)
clearing_buffers_.push_back(observation_buffers_.back());
printf(
"Created an observation buffer for topic %s, global frame: %s, "
"expected update rate: %.2f, observation persistence: %.2f\n",
topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
}
catch (const YAML::BadFile& e) {
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
return false;
// get the parameters for the specific topic
double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2;
std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud";
bool inf_is_valid = false, clearing=false, marking=true;
topic = loadParam(layer,"topic", topic);
sensor_frame = loadParam(layer,"sensor_frame", std::string(""));
observation_keep_time = loadParam(layer,"observation_persistence", 0.0);
expected_update_rate = loadParam(layer,"expected_update_rate", 0.0);
data_type = loadParam(layer,"data_type", std::string("PointCloud"));
min_obstacle_height = loadParam(layer,"min_obstacle_height", 0.0);
max_obstacle_height = loadParam(layer,"max_obstacle_height", 2.0);
inf_is_valid = loadParam(layer,"inf_is_valid", false);
clearing = loadParam(layer,"clearing", false);
marking = loadParam(layer,"marking", true);
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
{
printf("Only topics that use point clouds or laser scans are currently supported\n");
throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
}
return true;
std::string raytrace_range_param_name, obstacle_range_param_name;
double obstacle_range = 2.5;
obstacle_range = loadParam(layer,"obstacle_range", obstacle_range);
double raytrace_range = 3.0;
raytrace_range = loadParam(layer,"raytrace_range", raytrace_range);
bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true);
int combination_method = loadParam(layer, "combination_method", 1);
// enabled_ = enabled;
footprint_clearing_enabled_ = footprint_clearing_enabled;
max_obstacle_height_ = max_obstacle_height;
combination_method_ = combination_method;
printf("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
sensor_frame.c_str());
// create an observation buffer
observation_buffers_.push_back(
boost::shared_ptr < ObservationBuffer
> (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
sensor_frame, transform_tolerance)));
if (marking)
marking_buffers_.push_back(observation_buffers_.back());
// check if we'll also add this buffer to our clearing observation buffers
if (clearing)
clearing_buffers_.push_back(observation_buffers_.back());
printf(
"Created an observation buffer for topic %s, global frame: %s, "
"expected update rate: %.2f, observation persistence: %.2f\n",
topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
}
catch (const YAML::BadFile& e) {
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
return false;
}
return true;
}
void ObstacleLayer::handleImpl(const void* data,
@@ -121,7 +171,7 @@ void ObstacleLayer::handleImpl(const void* data,
if(!stop_receiving_data_)
{
if(observation_buffers_.empty()) return;
boost::shared_ptr<costmap_2d::ObservationBuffer> buffer = observation_buffers_.back();
boost::shared_ptr<ObservationBuffer> buffer = observation_buffers_.back();
if (type == typeid(sensor_msgs::LaserScan) && topic == "laser") {
laserScanCallback(*static_cast<const sensor_msgs::LaserScan*>(data), buffer);
} else if (type == typeid(sensor_msgs::LaserScan) && topic == "laser_valid") {
@@ -141,13 +191,12 @@ void ObstacleLayer::handleImpl(const void* data,
}
}
void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message,
void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message,
const boost::shared_ptr<ObservationBuffer>& buffer)
{
// project the laser into a point cloud
sensor_msgs::PointCloud2 cloud;
cloud.header = message.header;
// printf("TEST PLUGIN OBSTACLE!!!\n");
// project the scan into a point cloud
try
@@ -172,10 +221,9 @@ void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message,
buffer->unlock();
}
void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_message,
void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_message,
const boost::shared_ptr<ObservationBuffer>& buffer)
{
// printf("TEST PLUGIN OBSTACLE 2!!!\n");
// Filter positive infinities ("Inf"s) to max_range.
float epsilon = 0.0001; // a tenth of a millimeter
sensor_msgs::LaserScan message = raw_message;
@@ -216,9 +264,8 @@ void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_
}
void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloud& message,
const boost::shared_ptr<ObservationBuffer>& buffer)
const boost::shared_ptr<ObservationBuffer>& buffer)
{
// printf("TEST PLUGIN OBSTACLE 3!!!\n");
sensor_msgs::PointCloud2 cloud2;
if (!sensor_msgs::convertPointCloudToPointCloud2(message, cloud2))
@@ -516,11 +563,11 @@ void ObstacleLayer::reset()
}
// Export factory function
static PluginLayerPtr create_obstacle_plugin() {
static std::shared_ptr<Layer> create_obstacle_plugin() {
return std::make_shared<ObstacleLayer>();
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_obstacle_plugin, create_plugin)
BOOST_DLL_ALIAS(create_obstacle_plugin, create_obstacle_layer)
} // namespace costmap_2d

4
plugins/preferred_layer.cpp
View File

@@ -25,11 +25,11 @@ unsigned char PreferredLayer::interpretValue(unsigned char value)
}
// Export factory function
static PluginLayerPtr create_preferred_plugin() {
static std::shared_ptr<Layer> create_preferred_plugin() {
return std::make_shared<PreferredLayer>();
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_preferred_plugin, create_plugin)
BOOST_DLL_ALIAS(create_preferred_plugin, create_preferred_layer)
}

70
plugins/static_layer.cpp Normal file → Executable file
View File

@@ -1,16 +1,50 @@
/*********************************************************************
*
* 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 <data_convert/data_convert.h>
#include <costmap_2d/utils.h>
#include <tf3/convert.h>
#include <tf3/utils.h>
#include <boost/dll/alias.hpp>
#include <filesystem>
#include <string>
#include <iostream>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;
@@ -137,7 +171,6 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
if(!map_shutdown_)
{
std::cout << "Received new map!" << std::endl;
unsigned int size_x = new_map.info.width, size_y = new_map.info.height;
printf("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution);
@@ -188,18 +221,17 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
height_ = size_y_;
map_received_ = true;
has_updated_data_ = true;
}
else
{
printf("Stop receive new map!");
}
// shutdown the map subscrber if firt_map_only_ flag is on
if (first_map_only_)
{
printf("Shutting down the map subscriber. first_map_only flag is on");
printf("Shutting down the map subscriber. first_map_only flag is on\n");
map_shutdown_ = true;
// map_sub_.shutdown();
}
}
@@ -302,12 +334,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
tf3::TransformStampedMsg transformMsg;
try
{
// transformMsg = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
bool status =tf_->canTransform(map_frame_, global_frame_, tf3::Time());
if(!status) throw tf3::TransformException("[static_layer] Cannot transform");
transformMsg = tf_->lookupTransform(map_frame_,
global_frame_,
tf3::Time());
transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time::now());
}
catch (tf3::TransformException ex)
{
@@ -317,7 +344,8 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
// Copy map data given proper transformations
tf3::Transform tf3_transform;
tf3_transform = data_convert::convertTotf3Transform(transformMsg.transform);
// tf3::convert(transformMsg.transform, tf3_transform);
// tf2::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)
@@ -341,11 +369,11 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
}
// Export factory function
static PluginLayerPtr create_static_plugin() {
static std::shared_ptr<Layer> 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)
BOOST_DLL_ALIAS(create_static_plugin, create_static_layer)
} // namespace costmap_2d

4
plugins/unpreferred_layer.cpp
View File

@@ -23,11 +23,11 @@ unsigned char UnPreferredLayer::interpretValue(unsigned char value)
}
// Export factory function
static PluginLayerPtr create_unpreferred_plugin() {
static std::shared_ptr<Layer> create_unpreferred_plugin() {
return std::make_shared<UnPreferredLayer>();
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_unpreferred_plugin, create_plugin)
BOOST_DLL_ALIAS(create_unpreferred_plugin, create_unpreferred_layer)
}

51
plugins/voxel_layer.cpp Normal file → Executable file
View File

@@ -36,10 +36,8 @@
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/voxel_layer.h>
#include <costmap_2d/utils.h>
#include <boost/dll/alias.hpp>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <boost/dll/alias.hpp>
#define VOXEL_BITS 16
@@ -59,10 +57,25 @@ void VoxelLayer::onInitialize()
getParams();
}
VoxelLayer::~VoxelLayer()
{}
bool VoxelLayer::getParams()
{
try {
YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/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["voxel_layer"];
// publish_voxel_ = loadParam(layer, "publish_voxel_map", false);
@@ -83,12 +96,8 @@ bool VoxelLayer::getParams()
}
return true;
}
VoxelLayer::~VoxelLayer()
{}
void VoxelLayer::matchSize()
{
ObstacleLayer::matchSize();
@@ -211,14 +220,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
// grid_msg.resolutions.z = z_resolution_;
// grid_msg.header.frame_id = global_frame_;
// grid_msg.header.stamp = robot::Time::now();
// ///////////////////////////////////////////
// ////////////THAY THẾ PUBLISH NÀY///////////
// ///////////////////////////////////////////
// // voxel_pub_.publish(grid_msg);
// ///////////////////////////////////////////
// ///////////////////////////////////////////
// ///////////////////////////////////////////
// voxel_pub_.publish(grid_msg);
// }
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
@@ -289,23 +291,18 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
{
printf("The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
printf(
"The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
ox, oy, oz);
return;
}
///////////////////////////////////////////
////////////THAY THẾ PUBLISH NÀY///////////
///////////////////////////////////////////
// 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);
// }
///////////////////////////////////////////
///////////////////////////////////////////
///////////////////////////////////////////
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
double map_end_x = origin_x_ + getSizeInMetersX();
@@ -375,7 +372,7 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
{
unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
// voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
cell_raytrace_range);
@@ -399,7 +396,7 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
// clearing_endpoints_pub_.publish(clearing_endpoints_);
// clearing_endpoints_pub_.publish(clearing_endpoints_);
// }
}
@@ -461,11 +458,11 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
}
// Export factory function
static PluginLayerPtr create_voxel_plugin() {
static std::shared_ptr<Layer> create_voxel_plugin() {
return std::make_shared<VoxelLayer>();
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_voxel_plugin, create_plugin)
BOOST_DLL_ALIAS(create_voxel_plugin, create_voxel_layer)
} // namespace costmap_2d