update file obstacle_layer and file test plugin
This commit is contained in:
parent
19683269c3
commit
c94de60a7b
|
|
@ -147,6 +147,9 @@ endif()
|
|||
if (NOT TARGET map_msgs)
|
||||
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../map_msgs ${CMAKE_BINARY_DIR}/map_msgs_build)
|
||||
endif()
|
||||
if (NOT TARGET laser_geometry)
|
||||
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../laser_geometry ${CMAKE_BINARY_DIR}/laser_geometry_build)
|
||||
endif()
|
||||
|
||||
include_directories(
|
||||
include
|
||||
|
|
@ -179,20 +182,40 @@ target_link_libraries(costmap_2d_new
|
|||
geometry_msgs
|
||||
nav_msgs
|
||||
map_msgs
|
||||
laser_geometry
|
||||
${TF2_LIBRARY}
|
||||
)
|
||||
target_include_directories(costmap_2d_new PRIVATE ${Boost_INCLUDE_DIRS})
|
||||
|
||||
# --- Plugin libraries ---
|
||||
add_library(layers SHARED
|
||||
add_library(static_layer SHARED
|
||||
plugins/static_layer.cpp
|
||||
)
|
||||
target_link_libraries(layers
|
||||
target_link_libraries(static_layer
|
||||
costmap_2d_new
|
||||
${Boost_LIBRARIES}
|
||||
yaml-cpp
|
||||
)
|
||||
|
||||
add_library(obstacle_layer SHARED
|
||||
plugins/obstacle_layer.cpp
|
||||
)
|
||||
target_link_libraries(obstacle_layer
|
||||
costmap_2d_new
|
||||
${Boost_LIBRARIES}
|
||||
yaml-cpp
|
||||
)
|
||||
|
||||
add_library(inflation_layer SHARED
|
||||
plugins/inflation_layer.cpp
|
||||
)
|
||||
target_link_libraries(inflation_layer
|
||||
costmap_2d_new
|
||||
${Boost_LIBRARIES}
|
||||
yaml-cpp
|
||||
)
|
||||
|
||||
|
||||
# --- Test executables ---
|
||||
add_executable(test_array_parser test/array_parser_test.cpp)
|
||||
add_executable(test_costmap test/coordinates_test.cpp)
|
||||
|
|
@ -215,7 +238,8 @@ target_link_libraries(test_costmap PRIVATE
|
|||
target_link_libraries(test_plugin PRIVATE
|
||||
${TF2_LIBRARY}
|
||||
costmap_2d_new
|
||||
layers
|
||||
static_layer
|
||||
obstacle_layer
|
||||
${Boost_LIBRARIES}
|
||||
Boost::filesystem
|
||||
Boost::system
|
||||
|
|
|
|||
|
|
@ -72,19 +72,21 @@ public:
|
|||
void addExtraBounds(double mx0, double my0, double mx1, double my1);
|
||||
|
||||
template<typename T>
|
||||
void dataCallBack(const T& value) {
|
||||
handle(value);
|
||||
void dataCallBack(const T& value, const std::string& topic) {
|
||||
handle(value, topic);
|
||||
}
|
||||
|
||||
protected:
|
||||
// Hàm template public, dùng để gửi dữ liệu
|
||||
template<typename T>
|
||||
void handle(const T& data) {
|
||||
handleImpl(static_cast<const void*>(&data), typeid(T));
|
||||
void handle(const T& data, const std::string& topic) {
|
||||
handleImpl(static_cast<const void*>(&data), typeid(T), topic);
|
||||
}
|
||||
|
||||
// Hàm ảo duy nhất mà plugin sẽ override
|
||||
virtual void handleImpl(const void* data, const std::type_info& type) = 0;
|
||||
virtual void handleImpl(const void* data,
|
||||
const std::type_info& type,
|
||||
const std::string& topic) = 0;
|
||||
/*
|
||||
* Updates the master_grid within the specified
|
||||
* bounding box using this layer's values.
|
||||
|
|
|
|||
|
|
@ -1,19 +0,0 @@
|
|||
#ifndef COSTMAP_2D_CRITICAL_LAYER_H_
|
||||
#define COSTMAP_2D_CRITICAL_LAYER_H_
|
||||
|
||||
#include <costmap_2d/static_layer.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
class CriticalLayer : public StaticLayer
|
||||
{
|
||||
public:
|
||||
CriticalLayer();
|
||||
virtual ~CriticalLayer();
|
||||
private:
|
||||
unsigned char interpretValue(unsigned char value);
|
||||
void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
|
||||
};
|
||||
}
|
||||
|
||||
#endif // COSTMAP_2D_CRITICAL_LAYER_H_
|
||||
|
|
@ -146,6 +146,7 @@ private:
|
|||
std::vector<geometry_msgs::Point> footprint_spec_;
|
||||
};
|
||||
|
||||
using PluginLayerPtr = std::shared_ptr<Layer>;
|
||||
|
||||
} // namespace costmap_2d
|
||||
|
||||
|
|
|
|||
|
|
@ -51,7 +51,7 @@
|
|||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/point_cloud_conversion.h>
|
||||
|
||||
// #include <laser_geometry/laser_geometry.h>
|
||||
#include <laser_geometry/laser_geometry.hpp>
|
||||
// #include <tf2_ros/message_filter.h>
|
||||
// #include <message_filters/subscriber.h>
|
||||
|
||||
|
|
@ -114,6 +114,19 @@ public:
|
|||
|
||||
protected:
|
||||
|
||||
void handleImpl(const void* data,
|
||||
const std::type_info&,
|
||||
const std::string& topic) override;
|
||||
void laserScanCallback(const sensor_msgs::LaserScan& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer);
|
||||
void laserScanValidInfCallback(const sensor_msgs::LaserScan& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer);
|
||||
void pointCloudCallback(const sensor_msgs::PointCloud& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer);
|
||||
void pointCloud2Callback(const sensor_msgs::PointCloud2& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer);
|
||||
// void process(const map_msgs::OccupancyGridUpdate& update);
|
||||
|
||||
/**
|
||||
* @brief Get the observations used to mark space
|
||||
* @param marking_observations A reference to a vector that will be populated with the observations
|
||||
|
|
@ -150,7 +163,7 @@ protected:
|
|||
std::string global_frame_; ///< @brief The global frame for the costmap
|
||||
double max_obstacle_height_; ///< @brief Max Obstacle Height
|
||||
|
||||
// laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
|
||||
laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
|
||||
|
||||
// std::vector<boost::shared_ptr<message_filters::SubscriberBase> > observation_subscribers_; ///< @brief Used for the observation message filters
|
||||
// std::vector<boost::shared_ptr<tf2_ros::MessageFilterBase> > observation_notifiers_; ///< @brief Used to make sure that transforms are available for each sensor
|
||||
|
|
@ -162,6 +175,7 @@ protected:
|
|||
std::vector<costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
|
||||
|
||||
bool rolling_window_;
|
||||
bool stop_receiving_data_ = false;
|
||||
|
||||
int combination_method_;
|
||||
|
||||
|
|
|
|||
|
|
@ -5,7 +5,6 @@
|
|||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <map_msgs/OccupancyGridUpdate.h>
|
||||
// #include <message_filters/subscriber.h>
|
||||
#include <string>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
|
||||
|
|
@ -27,12 +26,12 @@ public:
|
|||
virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
|
||||
|
||||
virtual void matchSize();
|
||||
virtual void incomingMap(const nav_msgs::OccupancyGrid& new_map);
|
||||
virtual void incomingUpdate(const map_msgs::OccupancyGridUpdate& update);
|
||||
protected:
|
||||
void handleImpl(const void* data, const std::type_info& type) override;
|
||||
void process(const nav_msgs::OccupancyGrid& new_map);
|
||||
void process(const map_msgs::OccupancyGridUpdate& update);
|
||||
void handleImpl(const void* data,
|
||||
const std::type_info& type,
|
||||
const std::string& topic) override;
|
||||
void incomingMap(const nav_msgs::OccupancyGrid& new_map);
|
||||
void incomingUpdate(const map_msgs::OccupancyGridUpdate& update);
|
||||
|
||||
virtual unsigned char interpretValue(unsigned char value);
|
||||
unsigned char* threshold_;
|
||||
|
|
@ -65,6 +64,8 @@ private:
|
|||
bool getParams();
|
||||
};
|
||||
|
||||
using PluginStaticLayerPtr = std::shared_ptr<StaticLayer>;
|
||||
|
||||
} // namespace costmap_2d
|
||||
|
||||
#endif // COSTMAP_2D_STATIC_LAYER_H_
|
||||
|
|
|
|||
|
|
@ -34,10 +34,10 @@ void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, i
|
|||
}
|
||||
|
||||
// Export factory function
|
||||
static PluginPtr create_critical_plugin() {
|
||||
static PluginStaticLayerPtr 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_plugin_static_layer)
|
||||
}
|
||||
|
|
@ -362,11 +362,11 @@ void InflationLayer::setInflationParameters(double inflation_radius, double cost
|
|||
}
|
||||
|
||||
// Export factory function
|
||||
static PluginCostmapLayerPtr create_inflation_plugin() {
|
||||
static PluginLayerPtr 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_plugin_layer)
|
||||
|
||||
} // namespace costmap_2d
|
||||
|
|
|
|||
|
|
@ -1,8 +1,8 @@
|
|||
#include <costmap_2d/obstacle_layer.h>
|
||||
#include <costmap_2d/costmap_math.h>
|
||||
#include <costmap_2d/utils.h>
|
||||
// #include <tf2_ros/message_filter.h>
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
#include <tf2/convert.h>
|
||||
#include <tf2/utils.h>
|
||||
#include <boost/dll/alias.hpp>
|
||||
|
||||
|
|
@ -18,77 +18,74 @@ namespace costmap_2d
|
|||
|
||||
void ObstacleLayer::onInitialize()
|
||||
{
|
||||
getParams();
|
||||
// ros::NodeHandle nh("~/" + name_), g_nh;
|
||||
rolling_window_ = layered_costmap_->isRolling();
|
||||
|
||||
|
||||
bool track_unknown_space;
|
||||
// nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
|
||||
ObstacleLayer::matchSize();
|
||||
current_ = true;
|
||||
stop_receiving_data_ = false;
|
||||
global_frame_ = layered_costmap_->getGlobalFrameID();
|
||||
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;
|
||||
|
||||
ObstacleLayer::matchSize();
|
||||
current_ = true;
|
||||
|
||||
global_frame_ = layered_costmap_->getGlobalFrameID();
|
||||
double transform_tolerance;
|
||||
// nh.param("transform_tolerance", transform_tolerance, 0.2);
|
||||
|
||||
std::string topics_string;
|
||||
double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2);
|
||||
// get the topics that we'll subscribe to from the parameter server
|
||||
// nh.param("observation_sources", topics_string, std::string(""));
|
||||
// ROS_INFO(" Subscribed to Topics: %s", topics_string.c_str());
|
||||
|
||||
// now we need to split the topics based on whitespace which we can use a stringstream for
|
||||
std::stringstream ss(topics_string);
|
||||
/*
|
||||
std::string source;
|
||||
while (ss >> source)
|
||||
{
|
||||
ros::NodeHandle source_node(nh, source);
|
||||
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, expected_update_rate, min_obstacle_height, max_obstacle_height;
|
||||
std::string topic, sensor_frame, data_type;
|
||||
bool inf_is_valid, clearing, marking;
|
||||
|
||||
source_node.param("topic", topic, source);
|
||||
source_node.param("sensor_frame", sensor_frame, std::string(""));
|
||||
source_node.param("observation_persistence", observation_keep_time, 0.0);
|
||||
source_node.param("expected_update_rate", expected_update_rate, 0.0);
|
||||
source_node.param("data_type", data_type, std::string("PointCloud"));
|
||||
source_node.param("min_obstacle_height", min_obstacle_height, 0.0);
|
||||
source_node.param("max_obstacle_height", max_obstacle_height, 2.0);
|
||||
source_node.param("inf_is_valid", inf_is_valid, false);
|
||||
source_node.param("clearing", clearing, false);
|
||||
source_node.param("marking", marking, true);
|
||||
|
||||
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"))
|
||||
{
|
||||
ROS_FATAL("Only topics that use point clouds or laser scans are currently supported");
|
||||
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::string raytrace_range_param_name, obstacle_range_param_name;
|
||||
|
||||
// get the obstacle range for the sensor
|
||||
double obstacle_range = 2.5;
|
||||
if (source_node.searchParam("obstacle_range", obstacle_range_param_name))
|
||||
{
|
||||
source_node.getParam(obstacle_range_param_name, obstacle_range);
|
||||
}
|
||||
|
||||
// get the raytrace range for the sensor
|
||||
obstacle_range = loadParam(layer,"obstacle_range", obstacle_range);
|
||||
double raytrace_range = 3.0;
|
||||
if (source_node.searchParam("raytrace_range", raytrace_range_param_name))
|
||||
{
|
||||
source_node.getParam(raytrace_range_param_name, raytrace_range);
|
||||
}
|
||||
raytrace_range = loadParam(layer,"raytrace_range", raytrace_range);
|
||||
|
||||
printf("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.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
|
||||
|
|
@ -97,8 +94,6 @@ void ObstacleLayer::onInitialize()
|
|||
> (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)));
|
||||
|
||||
// check if we'll add this buffer to our marking observation buffers
|
||||
if (marking)
|
||||
marking_buffers_.push_back(observation_buffers_.back());
|
||||
|
||||
|
|
@ -107,107 +102,10 @@ void ObstacleLayer::onInitialize()
|
|||
clearing_buffers_.push_back(observation_buffers_.back());
|
||||
|
||||
printf(
|
||||
"Created an observation buffer for source %s, topic %s, global frame: %s, "
|
||||
"Created an observation buffer for topic %s, global frame: %s, "
|
||||
"expected update rate: %.2f, observation persistence: %.2f",
|
||||
source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
|
||||
topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
|
||||
|
||||
// create a callback for the topic
|
||||
if (data_type == "LaserScan")
|
||||
{
|
||||
boost::shared_ptr < message_filters::Subscriber<sensor_msgs::LaserScan>
|
||||
> sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));
|
||||
|
||||
boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > filter(
|
||||
new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_,50 , g_nh));
|
||||
|
||||
if (inf_is_valid)
|
||||
{
|
||||
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanValidInfCallback(msg, buffer); });
|
||||
}
|
||||
else
|
||||
{
|
||||
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanCallback(msg, buffer); });
|
||||
}
|
||||
|
||||
observation_subscribers_.push_back(sub);
|
||||
observation_notifiers_.push_back(filter);
|
||||
|
||||
observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
|
||||
}
|
||||
else if (data_type == "PointCloud")
|
||||
{
|
||||
boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud>
|
||||
> sub(new message_filters::Subscriber<sensor_msgs::PointCloud>(g_nh, topic, 50));
|
||||
|
||||
if (inf_is_valid)
|
||||
{
|
||||
ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
|
||||
}
|
||||
|
||||
boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud>
|
||||
> filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50, g_nh));
|
||||
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloudCallback(msg, buffer); });
|
||||
|
||||
observation_subscribers_.push_back(sub);
|
||||
observation_notifiers_.push_back(filter);
|
||||
}
|
||||
else
|
||||
{
|
||||
boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2>
|
||||
> sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));
|
||||
|
||||
if (inf_is_valid)
|
||||
{
|
||||
ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
|
||||
}
|
||||
|
||||
boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud2>
|
||||
> filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50, g_nh));
|
||||
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloud2Callback(msg, buffer); });
|
||||
|
||||
observation_subscribers_.push_back(sub);
|
||||
observation_notifiers_.push_back(filter);
|
||||
}
|
||||
|
||||
if (sensor_frame != "")
|
||||
{
|
||||
std::vector < std::string > target_frames;
|
||||
target_frames.push_back(global_frame_);
|
||||
target_frames.push_back(sensor_frame);
|
||||
observation_notifiers_.back()->setTargetFrames(target_frames);
|
||||
}
|
||||
}
|
||||
*/
|
||||
// dsrv_ = NULL;
|
||||
// setupDynamicReconfigure(nh);
|
||||
}
|
||||
|
||||
// void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
|
||||
// {
|
||||
// dsrv_ = new dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>(nh);
|
||||
// dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>::CallbackType cb =
|
||||
// [this](auto& config, auto level){ reconfigureCB(config, level); };
|
||||
// dsrv_->setCallback(cb);
|
||||
// }
|
||||
|
||||
ObstacleLayer::~ObstacleLayer()
|
||||
{}
|
||||
|
||||
bool StaticLayer::getParams()
|
||||
{
|
||||
try {
|
||||
YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml");
|
||||
YAML::Node layer = config["obstacle_layer"];
|
||||
|
||||
bool enabled = loadParam(layer, "enabled", true);
|
||||
bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true);
|
||||
double max_obstacle_height = loadParam(layer, "max_obstacle_height", 2);
|
||||
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;
|
||||
}
|
||||
catch (const YAML::BadFile& e) {
|
||||
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
|
||||
|
|
@ -217,103 +115,133 @@ bool StaticLayer::getParams()
|
|||
return true;
|
||||
}
|
||||
|
||||
// void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
|
||||
// const boost::shared_ptr<ObservationBuffer>& buffer)
|
||||
// {
|
||||
// // project the laser into a point cloud
|
||||
// sensor_msgs::PointCloud2 cloud;
|
||||
// cloud.header = message->header;
|
||||
void ObstacleLayer::handleImpl(const void* data,
|
||||
const std::type_info& type,
|
||||
const std::string& topic)
|
||||
{
|
||||
if(!stop_receiving_data_)
|
||||
{
|
||||
if(observation_buffers_.empty()) return;
|
||||
boost::shared_ptr<costmap_2d::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") {
|
||||
laserScanValidInfCallback(*static_cast<const sensor_msgs::LaserScan*>(data), buffer);
|
||||
} else if (type == typeid(sensor_msgs::PointCloud) && topic == "pcl_cb") {
|
||||
pointCloudCallback(*static_cast<const sensor_msgs::PointCloud*>(data), buffer);
|
||||
} else if (type == typeid(sensor_msgs::PointCloud2) && topic == "pcl2_cb") {
|
||||
pointCloud2Callback(*static_cast<const sensor_msgs::PointCloud2*>(data), buffer);
|
||||
} else {
|
||||
std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "Stop receiving data!" << std::endl;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// // project the scan into a point cloud
|
||||
// try
|
||||
// {
|
||||
// projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_);
|
||||
// }
|
||||
// catch (tf2::TransformException &ex)
|
||||
// {
|
||||
// ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(),
|
||||
// ex.what());
|
||||
// projector_.projectLaser(*message, cloud);
|
||||
// }
|
||||
// catch (std::runtime_error &ex)
|
||||
// {
|
||||
// ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
|
||||
// return; //ignore this 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");
|
||||
|
||||
// // buffer the point cloud
|
||||
// buffer->lock();
|
||||
// buffer->bufferCloud(cloud);
|
||||
// buffer->unlock();
|
||||
// }
|
||||
// project the scan into a point cloud
|
||||
try
|
||||
{
|
||||
projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
|
||||
}
|
||||
catch (tf2::TransformException &ex)
|
||||
{
|
||||
printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", global_frame_.c_str(),
|
||||
ex.what());
|
||||
projector_.projectLaser(message, cloud);
|
||||
}
|
||||
catch (std::runtime_error &ex)
|
||||
{
|
||||
printf("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
|
||||
return; //ignore this message
|
||||
}
|
||||
|
||||
// void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message,
|
||||
// const boost::shared_ptr<ObservationBuffer>& buffer)
|
||||
// {
|
||||
// // Filter positive infinities ("Inf"s) to max_range.
|
||||
// float epsilon = 0.0001; // a tenth of a millimeter
|
||||
// sensor_msgs::LaserScan message = *raw_message;
|
||||
// for (size_t i = 0; i < message.ranges.size(); i++)
|
||||
// {
|
||||
// float range = message.ranges[ i ];
|
||||
// if (!std::isfinite(range) && range > 0)
|
||||
// {
|
||||
// message.ranges[ i ] = message.range_max - epsilon;
|
||||
// }
|
||||
// }
|
||||
// buffer the point cloud
|
||||
buffer->lock();
|
||||
buffer->bufferCloud(cloud);
|
||||
buffer->unlock();
|
||||
}
|
||||
|
||||
// // project the laser into a point cloud
|
||||
// sensor_msgs::PointCloud2 cloud;
|
||||
// cloud.header = message.header;
|
||||
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;
|
||||
for (size_t i = 0; i < message.ranges.size(); i++)
|
||||
{
|
||||
float range = message.ranges[ i ];
|
||||
if (!std::isfinite(range) && range > 0)
|
||||
{
|
||||
message.ranges[ i ] = message.range_max - epsilon;
|
||||
}
|
||||
}
|
||||
|
||||
// // project the scan into a point cloud
|
||||
// try
|
||||
// {
|
||||
// projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
|
||||
// }
|
||||
// catch (tf2::TransformException &ex)
|
||||
// {
|
||||
// ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s",
|
||||
// global_frame_.c_str(), ex.what());
|
||||
// projector_.projectLaser(message, cloud);
|
||||
// }
|
||||
// catch (std::runtime_error &ex)
|
||||
// {
|
||||
// ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
|
||||
// return; //ignore this message
|
||||
// }
|
||||
// project the laser into a point cloud
|
||||
sensor_msgs::PointCloud2 cloud;
|
||||
cloud.header = message.header;
|
||||
|
||||
// // buffer the point cloud
|
||||
// buffer->lock();
|
||||
// buffer->bufferCloud(cloud);
|
||||
// buffer->unlock();
|
||||
// }
|
||||
// project the scan into a point cloud
|
||||
try
|
||||
{
|
||||
projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
|
||||
}
|
||||
catch (tf2::TransformException &ex)
|
||||
{
|
||||
printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n",
|
||||
global_frame_.c_str(), ex.what());
|
||||
projector_.projectLaser(message, cloud);
|
||||
}
|
||||
catch (std::runtime_error &ex)
|
||||
{
|
||||
printf("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
|
||||
return; //ignore this message
|
||||
}
|
||||
|
||||
// void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message,
|
||||
// const boost::shared_ptr<ObservationBuffer>& buffer)
|
||||
// {
|
||||
// sensor_msgs::PointCloud2 cloud2;
|
||||
// buffer the point cloud
|
||||
buffer->lock();
|
||||
buffer->bufferCloud(cloud);
|
||||
buffer->unlock();
|
||||
}
|
||||
|
||||
// if (!sensor_msgs::convertPointCloudToPointCloud2(*message, cloud2))
|
||||
// {
|
||||
// ROS_ERROR("Failed to convert a PointCloud to a PointCloud2, dropping message");
|
||||
// return;
|
||||
// }
|
||||
void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloud& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer)
|
||||
{
|
||||
// printf("TEST PLUGIN OBSTACLE 3!!!\n");
|
||||
sensor_msgs::PointCloud2 cloud2;
|
||||
|
||||
// // buffer the point cloud
|
||||
// buffer->lock();
|
||||
// buffer->bufferCloud(cloud2);
|
||||
// buffer->unlock();
|
||||
// }
|
||||
if (!sensor_msgs::convertPointCloudToPointCloud2(message, cloud2))
|
||||
{
|
||||
printf("Failed to convert a PointCloud to a PointCloud2, dropping message\n");
|
||||
return;
|
||||
}
|
||||
|
||||
// void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message,
|
||||
// const boost::shared_ptr<ObservationBuffer>& buffer)
|
||||
// {
|
||||
// // buffer the point cloud
|
||||
// buffer->lock();
|
||||
// buffer->bufferCloud(*message);
|
||||
// buffer->unlock();
|
||||
// }
|
||||
// buffer the point cloud
|
||||
buffer->lock();
|
||||
buffer->bufferCloud(cloud2);
|
||||
buffer->unlock();
|
||||
}
|
||||
|
||||
void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer)
|
||||
{
|
||||
// buffer the point cloud
|
||||
buffer->lock();
|
||||
buffer->bufferCloud(message);
|
||||
buffer->unlock();
|
||||
}
|
||||
|
||||
void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
|
||||
double* min_y, double* max_x, double* max_y)
|
||||
|
|
@ -360,7 +288,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
|
|||
// if the obstacle is too high or too far away from the robot we won't add it
|
||||
if (pz > max_obstacle_height_)
|
||||
{
|
||||
printf("The point is too high");
|
||||
printf("The point is too high\n");
|
||||
continue;
|
||||
}
|
||||
|
||||
|
|
@ -371,7 +299,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
|
|||
// if the point is far enough away... we won't consider it
|
||||
if (sq_dist >= sq_obstacle_range)
|
||||
{
|
||||
printf("The point is too far away");
|
||||
printf("The point is too far away\n");
|
||||
continue;
|
||||
}
|
||||
|
||||
|
|
@ -379,7 +307,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
|
|||
unsigned int mx, my;
|
||||
if (!worldToMap(px, py, mx, my))
|
||||
{
|
||||
printf("Computing map coords failed");
|
||||
printf("Computing map coords failed\n");
|
||||
continue;
|
||||
}
|
||||
|
||||
|
|
@ -485,7 +413,7 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
|
|||
if (!worldToMap(ox, oy, x0, y0))
|
||||
{
|
||||
printf(
|
||||
"The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
|
||||
"The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
|
||||
ox, oy);
|
||||
return;
|
||||
}
|
||||
|
|
@ -556,29 +484,19 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
|
|||
}
|
||||
}
|
||||
|
||||
// void ObstacleLayer::activate()
|
||||
// {
|
||||
// // if we're stopped we need to re-subscribe to topics
|
||||
// for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
|
||||
// {
|
||||
// if (observation_subscribers_[i] != NULL)
|
||||
// observation_subscribers_[i]->subscribe();
|
||||
// }
|
||||
|
||||
// for (unsigned int i = 0; i < observation_buffers_.size(); ++i)
|
||||
// {
|
||||
// if (observation_buffers_[i])
|
||||
// observation_buffers_[i]->resetLastUpdated();
|
||||
// }
|
||||
// }
|
||||
// void ObstacleLayer::deactivate()
|
||||
// {
|
||||
// for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
|
||||
// {
|
||||
// if (observation_subscribers_[i] != NULL)
|
||||
// observation_subscribers_[i]->unsubscribe();
|
||||
// }
|
||||
// }
|
||||
void ObstacleLayer::activate()
|
||||
{
|
||||
stop_receiving_data_ = false;
|
||||
for (unsigned int i = 0; i < observation_buffers_.size(); ++i)
|
||||
{
|
||||
if (observation_buffers_[i])
|
||||
observation_buffers_[i]->resetLastUpdated();
|
||||
}
|
||||
}
|
||||
void ObstacleLayer::deactivate()
|
||||
{
|
||||
stop_receiving_data_ = true;
|
||||
}
|
||||
|
||||
void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range,
|
||||
double* min_x, double* min_y, double* max_x, double* max_y)
|
||||
|
|
@ -590,16 +508,16 @@ void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double
|
|||
touch(ex, ey, min_x, min_y, max_x, max_y);
|
||||
}
|
||||
|
||||
// void ObstacleLayer::reset()
|
||||
// {
|
||||
void ObstacleLayer::reset()
|
||||
{
|
||||
// deactivate();
|
||||
// resetMaps();
|
||||
// current_ = true;
|
||||
// activate();
|
||||
// }
|
||||
}
|
||||
|
||||
// Export factory function
|
||||
static PluginPtr create_obstacle_plugin() {
|
||||
static PluginCostmapLayerPtr create_obstacle_plugin() {
|
||||
return std::make_shared<ObstacleLayer>();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -5,7 +5,6 @@
|
|||
#include <costmap_2d/utils.h>
|
||||
#include <tf2/convert.h>
|
||||
#include <tf2/utils.h>
|
||||
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <boost/dll/alias.hpp>
|
||||
|
||||
|
||||
|
|
@ -26,55 +25,10 @@ StaticLayer::~StaticLayer()
|
|||
|
||||
void StaticLayer::onInitialize()
|
||||
{
|
||||
// ros::NodeHandle nh("~/" + name_), g_nh;
|
||||
current_ = true;
|
||||
|
||||
global_frame_ = layered_costmap_->getGlobalFrameID();
|
||||
getParams();
|
||||
// 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);
|
||||
|
||||
// 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"));
|
||||
|
||||
// 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
|
||||
// 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();
|
||||
// }
|
||||
|
||||
// printf("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
|
||||
|
||||
// if (subscribe_to_updates_)
|
||||
// {
|
||||
std::cout<<"Subscribing to updates"<<std::endl;
|
||||
// map_update_sub_ = g_nh.subscribe(map_topic_ + "_updates", 10, &StaticLayer::incomingUpdate, this);
|
||||
// }
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// has_updated_data_ = true;
|
||||
// }
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -84,6 +38,7 @@ bool StaticLayer::getParams()
|
|||
YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml");
|
||||
YAML::Node layer = config["static_layer"];
|
||||
|
||||
|
||||
bool enabled = loadParam(layer, "enabled", true);
|
||||
bool first_map_only = loadParam(layer, "first_map_only", false);
|
||||
bool subscribe_to_updates = loadParam(layer, "subscribe_to_updates", false);
|
||||
|
|
@ -120,18 +75,6 @@ bool StaticLayer::getParams()
|
|||
|
||||
}
|
||||
|
||||
// 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()
|
||||
{
|
||||
// If we are using rolling costmap, the static map size is
|
||||
|
|
@ -160,18 +103,20 @@ unsigned char StaticLayer::interpretValue(unsigned char value)
|
|||
return scale * LETHAL_OBSTACLE;
|
||||
}
|
||||
|
||||
void StaticLayer::handleImpl(const void* data, const std::type_info& type)
|
||||
void StaticLayer::handleImpl(const void* data,
|
||||
const std::type_info& type,
|
||||
const std::string& topic)
|
||||
{
|
||||
if (type == typeid(nav_msgs::OccupancyGrid)) {
|
||||
process(*static_cast<const nav_msgs::OccupancyGrid*>(data));
|
||||
} else if (type == typeid(map_msgs::OccupancyGridUpdate)) {
|
||||
process(*static_cast<const map_msgs::OccupancyGridUpdate*>(data));
|
||||
if (type == typeid(nav_msgs::OccupancyGrid) && topic == "map") {
|
||||
incomingMap(*static_cast<const nav_msgs::OccupancyGrid*>(data));
|
||||
} else if (type == typeid(map_msgs::OccupancyGridUpdate) && topic == "map_update") {
|
||||
incomingUpdate(*static_cast<const map_msgs::OccupancyGridUpdate*>(data));
|
||||
} else {
|
||||
std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void StaticLayer::process(const nav_msgs::OccupancyGrid& new_map)
|
||||
void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
|
||||
{
|
||||
if(!map_shutdown_)
|
||||
{
|
||||
|
|
@ -221,25 +166,13 @@ void StaticLayer::process(const nav_msgs::OccupancyGrid& new_map)
|
|||
}
|
||||
map_frame_ = new_map.header.frame_id;
|
||||
|
||||
// // Print current costmap
|
||||
// std::cout << "[StaticLayer] Costmap (" << size_y_ << " x " << size_x_ << "):" << std::endl;
|
||||
// for (unsigned int i = 0; i < size_y_; ++i)
|
||||
// {
|
||||
// for (unsigned int j = 0; j < size_x_; ++j)
|
||||
// {
|
||||
// unsigned int idx = i * size_x_ + j;
|
||||
// std::cout << static_cast<int>(costmap_[idx]);
|
||||
// if (j + 1 < size_x_) std::cout << ' ';
|
||||
// }
|
||||
// std::cout << std::endl;
|
||||
// }
|
||||
|
||||
// we have a new map, update full size of map
|
||||
x_ = y_ = 0;
|
||||
width_ = size_x_;
|
||||
height_ = size_y_;
|
||||
map_received_ = true;
|
||||
has_updated_data_ = true;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
|
|
@ -253,7 +186,8 @@ void StaticLayer::process(const nav_msgs::OccupancyGrid& new_map)
|
|||
// map_sub_.shutdown();
|
||||
}
|
||||
}
|
||||
void StaticLayer::process(const map_msgs::OccupancyGridUpdate& update)
|
||||
|
||||
void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdate& update)
|
||||
{
|
||||
if(!map_update_shutdown_)
|
||||
{
|
||||
|
|
@ -276,94 +210,10 @@ void StaticLayer::process(const map_msgs::OccupancyGridUpdate& update)
|
|||
}
|
||||
}
|
||||
|
||||
void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
|
||||
{
|
||||
if(!map_shutdown_)
|
||||
{
|
||||
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", 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))
|
||||
{
|
||||
// Update the size of the layered costmap (and all layers, including this one)
|
||||
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)
|
||||
{
|
||||
// only update the size of the costmap stored locally in this layer
|
||||
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;
|
||||
|
||||
// initialize the costmap with static data
|
||||
for (unsigned int i = 0; i < size_y; ++i)
|
||||
{
|
||||
for (unsigned int j = 0; j < size_x; ++j)
|
||||
{
|
||||
unsigned char value = new_map.data[index];
|
||||
costmap_[index] = interpretValue(value);
|
||||
++index;
|
||||
}
|
||||
}
|
||||
map_frame_ = new_map.header.frame_id;
|
||||
|
||||
// we have a new map, update full size of map
|
||||
x_ = y_ = 0;
|
||||
width_ = size_x_;
|
||||
height_ = size_y_;
|
||||
map_received_ = true;
|
||||
has_updated_data_ = true;
|
||||
}
|
||||
// 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");
|
||||
map_shutdown_ = true;
|
||||
// map_sub_.shutdown();
|
||||
}
|
||||
}
|
||||
|
||||
void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdate& update)
|
||||
{
|
||||
if(!map_update_shutdown_)
|
||||
{
|
||||
unsigned int di = 0;
|
||||
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 = index_base + x + update.x;
|
||||
costmap_[index] = interpretValue(update.data[di++]);
|
||||
}
|
||||
}
|
||||
x_ = update.x;
|
||||
y_ = update.y;
|
||||
width_ = update.width;
|
||||
height_ = update.height;
|
||||
has_updated_data_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
void StaticLayer::activate()
|
||||
{
|
||||
map_shutdown_ = false;
|
||||
map_update_shutdown_ = false;
|
||||
onInitialize();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
#include <costmap_2d/observation_buffer.h>
|
||||
|
||||
#include <costmap_2d/data_convert.h>
|
||||
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
// #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
|
||||
#include<tf2/convert.h>
|
||||
|
|
@ -32,18 +32,13 @@ ObservationBuffer::~ObservationBuffer()
|
|||
|
||||
bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
|
||||
{
|
||||
geometry_msgs::Point A;
|
||||
// ros::Time transform_time = ros::Time::now();
|
||||
// double transform_time =
|
||||
// std::chrono::duration<double>(
|
||||
// std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
robot::Time transform_time = robot::Time::now();
|
||||
|
||||
std::string tf_error;
|
||||
|
||||
if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, tf2::Time::now(), &tf_error))
|
||||
{
|
||||
printf("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
|
||||
printf("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
|
||||
global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
|
||||
return false;
|
||||
}
|
||||
|
|
@ -63,19 +58,19 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
|
|||
// we need to transform the origin of the observation to the new global frame
|
||||
tf2::doTransform(origin, origin,
|
||||
tf2_buffer_.lookupTransform(new_global_frame,
|
||||
tf2::getFrameId(origin),
|
||||
tf2::getTimestamp(origin)));
|
||||
origin.header.frame_id,
|
||||
convertTime(origin.header.stamp)));
|
||||
obs.origin_ = origin.point;
|
||||
|
||||
// we also need to transform the cloud of the observation to the new global frame
|
||||
tf2::doTransform(*(obs.cloud_), *(obs.cloud_),
|
||||
tf2_buffer_.lookupTransform(new_global_frame,
|
||||
tf2::getFrameId(*(obs.cloud_)),
|
||||
tf2::getTimestamp(*(obs.cloud_))));
|
||||
obs.cloud_->header.frame_id,
|
||||
convertTime(obs.cloud_->header.stamp)));
|
||||
}
|
||||
catch (TransformException& ex)
|
||||
{
|
||||
printf("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(),
|
||||
printf("TF Error attempting to transform an observation from %s to %s: %s\n", global_frame_.c_str(),
|
||||
new_global_frame.c_str(), ex.what());
|
||||
return false;
|
||||
}
|
||||
|
|
@ -107,8 +102,8 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
|
|||
local_origin.point.z = 0;
|
||||
tf2::doTransform(local_origin, global_origin,
|
||||
tf2_buffer_.lookupTransform(global_frame_,
|
||||
tf2::getFrameId(local_origin),
|
||||
tf2::getTimestamp(local_origin)));
|
||||
local_origin.header.frame_id,
|
||||
convertTime(local_origin.header.stamp)));
|
||||
tf2::convert(global_origin.point, observation_list_.front().origin_);
|
||||
|
||||
// make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
|
||||
|
|
@ -120,8 +115,8 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
|
|||
// transform the point cloud
|
||||
tf2::doTransform(cloud, global_frame_cloud,
|
||||
tf2_buffer_.lookupTransform(global_frame_,
|
||||
tf2::getFrameId(cloud),
|
||||
tf2::getTimestamp(cloud)));
|
||||
(cloud.header.frame_id),
|
||||
convertTime(cloud.header.stamp)));
|
||||
global_frame_cloud.header.stamp = cloud.header.stamp;
|
||||
|
||||
// now we need to remove observations from the cloud that are below or above our height thresholds
|
||||
|
|
@ -163,7 +158,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
|
|||
{
|
||||
// if an exception occurs, we need to remove the empty observation from the list
|
||||
observation_list_.pop_front();
|
||||
printf("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
|
||||
printf("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s\n", sensor_frame_.c_str(),
|
||||
cloud.header.frame_id.c_str(), ex.what());
|
||||
return;
|
||||
}
|
||||
|
|
@ -224,7 +219,7 @@ bool ObservationBuffer::isCurrent() const
|
|||
if (!current)
|
||||
{
|
||||
printf(
|
||||
"The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
|
||||
"The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.\n",
|
||||
topic_name_.c_str(), (robot::Time::now() - last_updated_).toSec(),
|
||||
expected_update_rate_.toSec());
|
||||
}
|
||||
|
|
|
|||
|
|
@ -6,12 +6,13 @@
|
|||
#include <tf2/time.h>
|
||||
#include "nav_msgs/OccupancyGrid.h"
|
||||
#include <map_msgs/OccupancyGridUpdate.h>
|
||||
#include <costmap_2d/obstacle_layer.h>
|
||||
using namespace costmap_2d;
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
|
||||
auto creator = boost::dll::import_alias<costmap_2d::PluginCostmapLayerPtr()>(
|
||||
"./costmap_2d/liblayers.so", "create_plugin", boost::dll::load_mode::append_decorations
|
||||
"./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
|
||||
);
|
||||
|
||||
costmap_2d::PluginCostmapLayerPtr plugin = creator();
|
||||
|
|
@ -42,7 +43,7 @@ int main(int argc, char* argv[]) {
|
|||
grid.data[1] = 100;
|
||||
grid.data[2] = 10;
|
||||
grid.data[3] = 0;
|
||||
plugin->dataCallBack(grid);
|
||||
plugin->dataCallBack(grid, "map");
|
||||
|
||||
map_msgs::OccupancyGridUpdate update;
|
||||
|
||||
|
|
@ -56,7 +57,96 @@ int main(int argc, char* argv[]) {
|
|||
update.data[1] = 100;
|
||||
update.data[2] = 10;
|
||||
update.data[3] = 0;
|
||||
plugin->dataCallBack(update);
|
||||
plugin->dataCallBack(update, "map_update");
|
||||
|
||||
|
||||
creator = boost::dll::import_alias<costmap_2d::PluginCostmapLayerPtr()>(
|
||||
"./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
|
||||
);
|
||||
|
||||
plugin = creator();
|
||||
std::cout << "Plugin created successfully" << std::endl;
|
||||
|
||||
plugin->initialize(&layered_costmap, "obstacle_layer", &tf_buffer);
|
||||
|
||||
sensor_msgs::LaserScan scan;
|
||||
|
||||
// --- Header ---
|
||||
scan.header.seq = 1;
|
||||
scan.header.stamp = robot::Time::now(); // nếu bạn chưa có Time class, gán tạm số
|
||||
scan.header.frame_id = "laser_frame";
|
||||
|
||||
// --- Cấu hình góc ---
|
||||
scan.angle_min = -M_PI; // -180 độ
|
||||
scan.angle_max = M_PI; // +180 độ
|
||||
scan.angle_increment = M_PI / 180; // 1 độ mỗi tia
|
||||
|
||||
// --- Cấu hình thời gian ---
|
||||
scan.time_increment = 0.0001f; // mỗi tia cách nhau 0.1 ms
|
||||
scan.scan_time = 0.04f; // tổng thời gian quét 40 ms (~25Hz)
|
||||
|
||||
// --- Giới hạn đo ---
|
||||
scan.range_min = 0.1f;
|
||||
scan.range_max = 10.0f;
|
||||
|
||||
// --- Tạo dữ liệu ---
|
||||
int num_readings = static_cast<int>((scan.angle_max - scan.angle_min) / scan.angle_increment);
|
||||
scan.ranges.resize(num_readings);
|
||||
scan.intensities.resize(num_readings);
|
||||
|
||||
for (int i = 0; i < num_readings; ++i)
|
||||
{
|
||||
float angle = scan.angle_min + i * scan.angle_increment;
|
||||
|
||||
// Tạo dữ liệu giả lập: vật thể cách 2m, dao động nhẹ theo góc
|
||||
scan.ranges[i] = 2.0f + 0.2f * std::sin(3.0f * angle);
|
||||
|
||||
// Giả lập cường độ phản xạ
|
||||
scan.intensities[i] = 100.0f + 20.0f * std::cos(angle);
|
||||
}
|
||||
|
||||
// --- In thử 10 giá trị đầu ---
|
||||
// for (int i = 0; i < 10; ++i)
|
||||
// {
|
||||
// std::cout << "Angle " << (scan.angle_min + i * scan.angle_increment)
|
||||
// << " rad | Range " << scan.ranges[i]
|
||||
// << " m | Intensity " << scan.intensities[i]
|
||||
// << std::endl;
|
||||
// }
|
||||
plugin->deactivate();
|
||||
plugin->dataCallBack(scan, "laser_valid");
|
||||
|
||||
sensor_msgs::PointCloud cloud;
|
||||
|
||||
// 2. Thiết lập header
|
||||
cloud.header.stamp.sec = 1234567890; // hoặc ros::Time::now().sec
|
||||
cloud.header.stamp.nsec = 0;
|
||||
cloud.header.frame_id = "laser_frame";
|
||||
|
||||
// 3. Thêm một vài điểm
|
||||
geometry_msgs::Point32 pt1;
|
||||
pt1.x = 1.0f; pt1.y = 0.0f; pt1.z = 0.0f;
|
||||
|
||||
geometry_msgs::Point32 pt2;
|
||||
pt2.x = 0.0f; pt2.y = 1.0f; pt2.z = 0.0f;
|
||||
|
||||
geometry_msgs::Point32 pt3;
|
||||
pt3.x = 1.0f; pt3.y = 1.0f; pt3.z = 0.0f;
|
||||
|
||||
cloud.points.push_back(pt1);
|
||||
cloud.points.push_back(pt2);
|
||||
cloud.points.push_back(pt3);
|
||||
|
||||
// 4. Thêm dữ liệu channels (tùy chọn)
|
||||
cloud.channels.resize(1);
|
||||
cloud.channels[0].name = "intensity";
|
||||
cloud.channels[0].values.push_back(0.5f);
|
||||
cloud.channels[0].values.push_back(1.0f);
|
||||
cloud.channels[0].values.push_back(0.8f);
|
||||
|
||||
plugin->dataCallBack(cloud, "pcl_cb");
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user