765 lines
30 KiB
C++
765 lines
30 KiB
C++
/*********************************************************************
|
|
*
|
|
* 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 <robot/console.h>
|
|
#include <cstdio>
|
|
#include <string>
|
|
#include <algorithm>
|
|
#include <vector>
|
|
|
|
#include <robot_costmap_2d/layered_costmap.h>
|
|
#include <robot_costmap_2d/costmap_2d_robot.h>
|
|
#include <robot_costmap_2d/utils.h>
|
|
|
|
#include <tf3/convert.h>
|
|
#include <tf3/utils.h>
|
|
#include <exception>
|
|
|
|
#include <boost/dll/import.hpp>
|
|
|
|
using namespace std;
|
|
|
|
namespace robot_costmap_2d
|
|
{
|
|
template <class T>
|
|
void move_parameter(robot::NodeHandle& old_h, robot::NodeHandle& new_h, std::string name,T& value)
|
|
{
|
|
if (!old_h.hasParam(name))
|
|
return;
|
|
|
|
old_h.getParam(name, value);
|
|
new_h.setParam(name, value);
|
|
}
|
|
|
|
Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
|
|
layered_costmap_(NULL),
|
|
name_(name),
|
|
tf_(tf),
|
|
transform_tolerance_(0.3),
|
|
map_update_thread_shutdown_(false),
|
|
stop_updates_(false),
|
|
initialized_(true),
|
|
stopped_(false),
|
|
map_update_thread_(NULL),
|
|
footprint_padding_(0.0)
|
|
{
|
|
robot::NodeHandle nh("~");
|
|
robot::NodeHandle priv_nh(nh, name);
|
|
name_ = name;
|
|
std::string config_file_name = "costmap_params.yaml";
|
|
getParams(config_file_name, name_, nh);
|
|
|
|
// create a thread to handle updating the map
|
|
stop_updates_ = false;
|
|
initialized_ = true;
|
|
stopped_ = false;
|
|
}
|
|
|
|
void Costmap2DROBOT::getParams(const std::string& config_file_name,const std::string& name, robot::NodeHandle& nh)
|
|
{
|
|
try
|
|
{
|
|
const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
|
|
std::string folder;
|
|
if (env_config && std::filesystem::exists(env_config))
|
|
{
|
|
folder = std::string(env_config);
|
|
// robot::log_error("config_directory: %s", folder.c_str());
|
|
}
|
|
std::string path_source = getSourceFile(folder,config_file_name);
|
|
|
|
YAML::Node config = YAML::LoadFile(path_source);
|
|
YAML::Node layer = config["robot_costmap_2d"];
|
|
|
|
robot::NodeHandle priv_nh(priv_nh, name);
|
|
|
|
std::string global_frame =
|
|
loadParam(layer, "global_frame", std::string("map"));
|
|
std::string robot_base_frame =
|
|
loadParam(layer, "robot_base_frame", std::string("base_link"));
|
|
|
|
if (priv_nh.hasParam("global_frame"))
|
|
priv_nh.getParam("global_frame", global_frame);
|
|
|
|
if (nh.hasParam("robot_base_frame"))
|
|
nh.getParam("robot_base_frame", robot_base_frame);
|
|
|
|
global_frame_ = global_frame;
|
|
robot_base_frame_ = robot_base_frame;
|
|
|
|
robot::Time last_error = robot::Time::now();
|
|
std::string tf_error;
|
|
|
|
// we need to make sure that the transform between the robot base frame and the global frame is available
|
|
while (!tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time(), &tf_error))
|
|
{
|
|
if (last_error + robot::Duration(5.0) < robot::Time::now())
|
|
{
|
|
std::string all_frames_string = tf_.allFramesAsString();
|
|
robot::log_info("[%s:%d]\n INFO: tf allFramesAsString: %s", __FILE__, __LINE__, all_frames_string.c_str());
|
|
// std::cout << std::fixed << std::setprecision(6) << robot::Time::now().toSec() << std::endl;
|
|
robot::log_warning("[%s:%d] %0.6f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
|
|
__FILE__, __LINE__, robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
|
|
last_error = robot::Time::now();
|
|
}
|
|
// The error string will accumulate and errors will typically be the same, so the last
|
|
// will do for the warning above. Reset the string here to avoid accumulation.
|
|
tf_error.clear();
|
|
}
|
|
|
|
// check if we want a rolling window version of the costmap
|
|
bool rolling_window = loadParam(layer, "rolling_window", false);
|
|
bool track_unknown_space = loadParam(layer, "track_unknown_space", false);
|
|
std::string path_plugins = loadParam(layer, "library_path", std::string(" "));
|
|
|
|
robot::PluginLoaderHelper loader;
|
|
|
|
|
|
if (priv_nh.hasParam("rolling_window"))
|
|
priv_nh.getParam("rolling_window", rolling_window);
|
|
|
|
if (priv_nh.hasParam("track_unknown_space"))
|
|
priv_nh.getParam("track_unknown_space", track_unknown_space);
|
|
|
|
if (priv_nh.hasParam("library_path"))
|
|
path_plugins = loader.findLibraryPath(name_);
|
|
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
|
|
|
|
// find size parameters
|
|
double map_width_meters = loadParam(layer, "width", 0.0);
|
|
double map_height_meters = loadParam(layer, "height", 0.0);
|
|
double resolution = loadParam(layer, "resolution", 0.0);
|
|
double origin_x = loadParam(layer, "origin_x", 0.0);
|
|
double origin_y = loadParam(layer, "origin_y", 0.0);
|
|
|
|
if (priv_nh.hasParam("width"))
|
|
priv_nh.getParam("width", map_width_meters);
|
|
if (priv_nh.hasParam("height"))
|
|
priv_nh.getParam("height", map_height_meters);
|
|
if (priv_nh.hasParam("resolution"))
|
|
priv_nh.getParam("resolution", resolution);
|
|
if (priv_nh.hasParam("origin_x"))
|
|
priv_nh.getParam("origin_x", origin_x);
|
|
if (priv_nh.hasParam("origin_y"))
|
|
priv_nh.getParam("origin_y", origin_y);
|
|
|
|
if (!layered_costmap_->isSizeLocked())
|
|
{
|
|
// robot::log_warning("ROBOT origin_x: %f | origin_y: %f", origin_x, origin_y);
|
|
layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
|
|
(unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
|
|
}
|
|
|
|
struct PluginConfig
|
|
{
|
|
std::string name;
|
|
std::string type;
|
|
};
|
|
std::vector<PluginConfig> my_list;
|
|
|
|
if(priv_nh.hasParam("plugins"))
|
|
{
|
|
my_list.clear();
|
|
YAML::Node my_plugins = priv_nh.getParamValue("plugins");
|
|
if (my_plugins && my_plugins.IsSequence())
|
|
{
|
|
for (size_t i = 0; i < my_plugins.size(); ++i)
|
|
{
|
|
YAML::Node plugin_i = my_plugins[i].as<YAML::Node>();
|
|
|
|
PluginConfig p;
|
|
p.type = plugin_i["type"].as<std::string>();
|
|
p.name = plugin_i["name"].as<std::string>();
|
|
my_list.push_back(p);
|
|
if (!plugin_i.IsMap())
|
|
{
|
|
std::cerr << "Plugins must be specified as maps. We'll use the default recovery behaviors instead." << std::endl;
|
|
return;
|
|
}
|
|
|
|
if (!plugin_i["name"] || !plugin_i["type"])
|
|
{
|
|
std::cerr << "Plugins must have a name and a type. Using the default recovery behaviors instead." << std::endl;
|
|
return;
|
|
}
|
|
|
|
// check for recovery behaviors with the same name
|
|
std::string name_i = plugin_i["name"].as<std::string>();
|
|
for (size_t j = i + 1; j < my_plugins.size(); ++j)
|
|
{
|
|
YAML::Node plugin_j = my_plugins[j].as<YAML::Node>();
|
|
if (plugin_j.IsMap() && plugin_j["name"])
|
|
{
|
|
std::string name_j = plugin_j["name"].as<std::string>();
|
|
if (name_i == name_j)
|
|
{
|
|
std::cerr << "A plugin with the name " << name_i << " already exists, this is not allowed. Using the default recovery behaviors instead." << std::endl;
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
my_list.clear();
|
|
if (layer["plugins"] && layer["plugins"].IsSequence())
|
|
{
|
|
for (const auto& plugin_node : layer["plugins"]) {
|
|
PluginConfig p;
|
|
p.name = loadParam(plugin_node, "name", std::string(" "));
|
|
p.type = loadParam(plugin_node, "type", std::string(" "));
|
|
my_list.push_back(p);
|
|
}
|
|
}
|
|
}
|
|
robot::NodeHandle private_nh("~");
|
|
for (auto& info : my_list)
|
|
{
|
|
try
|
|
{
|
|
copyParentParameters(name_, info.name, info.type, private_nh);
|
|
creators_.push_back(
|
|
boost::dll::import_alias<PluginLayerPtr()>(
|
|
path_plugins, info.type, boost::dll::load_mode::append_decorations)
|
|
);
|
|
PluginLayerPtr plugin = creators_.back()();
|
|
std::cout << "Plugin created: " << info.name << std::endl;
|
|
|
|
plugin->initialize(layered_costmap_,name_ + "/" + info.name, &tf_);
|
|
layered_costmap_->addPlugin(plugin);
|
|
}
|
|
catch (std::exception &ex)
|
|
{
|
|
robot::log_error("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s\n", info.name.c_str(), ex.what());
|
|
return;
|
|
}
|
|
}
|
|
|
|
std::vector<robot_geometry_msgs::Point> new_footprint;// = loadParam(layer, "footprint", 0.0);
|
|
new_footprint = loadFootprint(layer["footprint"], new_footprint);
|
|
transform_tolerance_ = loadParam(layer, "transform_tolerance", 0.0);
|
|
|
|
if (priv_nh.hasParam("footprint"))
|
|
{
|
|
std::cout <<"FOOTPRINT ROBOT:"<<std::endl;
|
|
new_footprint = makeFootprintFromParams(priv_nh);
|
|
}
|
|
|
|
if (nh.hasParam("transform_tolerance"))
|
|
nh.getParam("transform_tolerance", transform_tolerance_);
|
|
// robot::log_info("transform_tolerance: %d", transform_tolerance_);
|
|
|
|
setUnpaddedRobotFootprint(new_footprint);
|
|
|
|
|
|
|
|
if (map_update_thread_ != NULL)
|
|
{
|
|
map_update_thread_shutdown_ = true;
|
|
map_update_thread_->join();
|
|
delete map_update_thread_;
|
|
map_update_thread_ = NULL;
|
|
}
|
|
map_update_thread_shutdown_ = false;
|
|
double map_update_frequency = loadParam(layer, "update_frequency", 0.0);
|
|
|
|
if (priv_nh.hasParam("update_frequency"))
|
|
priv_nh.getParam("update_frequency", map_update_frequency);
|
|
|
|
// If the padding has changed, call setUnpaddedRobotFootprint() to
|
|
// re-apply the padding.
|
|
float footprint_padding = loadParam(layer, "footprint_padding", 0.0);
|
|
if (nh.hasParam("footprint_padding"))
|
|
nh.getParam("footprint_padding", footprint_padding);
|
|
if (footprint_padding_ != footprint_padding)
|
|
{
|
|
footprint_padding_ = footprint_padding;
|
|
setUnpaddedRobotFootprint(unpadded_footprint_);
|
|
}
|
|
|
|
double robot_radius = loadParam(layer, "robot_radius", 0.0);
|
|
if (priv_nh.hasParam("robot_radius"))
|
|
priv_nh.getParam("robot_radius", robot_radius);
|
|
readFootprintFromConfig(new_footprint, unpadded_footprint_, robot_radius);
|
|
|
|
// only construct the thread if the frequency is positive
|
|
if(map_update_frequency > 0.0)
|
|
map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROBOT::mapUpdateLoop, this, map_update_frequency));
|
|
}
|
|
catch (const YAML::BadFile& e)
|
|
{
|
|
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
|
|
return;
|
|
}
|
|
}
|
|
|
|
void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
|
|
const std::string& plugin_name,
|
|
const std::string& plugin_type,
|
|
robot::NodeHandle& nh)
|
|
{
|
|
robot::NodeHandle costmap_nh(nh, costmap_name);
|
|
robot::NodeHandle costmap_plugin_nh(costmap_nh, plugin_name);
|
|
robot::NodeHandle plugin_nh(nh, plugin_name);
|
|
if(plugin_type == "StaticLayer")
|
|
{
|
|
std::string map_topic;
|
|
int unknown_cost_value;
|
|
int lethal_cost_threshold;
|
|
bool track_unknown_space;
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "map_topic", map_topic);
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "unknown_cost_value", unknown_cost_value);
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "lethal_cost_threshold", lethal_cost_threshold);
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "track_unknown_space", track_unknown_space);
|
|
}
|
|
else if(plugin_type == "VoxelLayer")
|
|
{
|
|
double origin_z;
|
|
double z_resolution;
|
|
int z_voxels;
|
|
int mark_threshold;
|
|
int unknown_threshold;
|
|
bool publish_voxel_map;
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "origin_z", origin_z);
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "z_resolution", z_resolution);
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "z_voxels", z_voxels);
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "mark_threshold", mark_threshold);
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "unknown_threshold", unknown_threshold);
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "publish_voxel_map", publish_voxel_map);
|
|
if(plugin_nh.hasParam("observation_sources"))
|
|
{
|
|
std::string topics_string;
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "observation_sources", topics_string);
|
|
robot::log_error("topics_string: %s", topics_string.c_str());
|
|
std::stringstream ss(topics_string);
|
|
std::string source;
|
|
while (ss >> source)
|
|
{
|
|
robot::NodeHandle plugin_nh_element(plugin_nh, source);
|
|
robot::NodeHandle costmap_plugin_nh_element(costmap_plugin_nh, source);
|
|
std::string topic;
|
|
std::string data_type;
|
|
bool clearing;
|
|
bool marking;
|
|
bool inf_is_valid;
|
|
std::string sensor_frame;
|
|
double observation_persistence;
|
|
double expected_update_rate;
|
|
double min_obstacle_height;
|
|
double max_obstacle_height;
|
|
double obstacle_range;
|
|
double raytrace_range;
|
|
bool frustum_clearing_enabled = false;
|
|
int frustum_clearing_pixel_step = 8;
|
|
double frustum_min_range = 0.2;
|
|
double frustum_max_range = 3.0;
|
|
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "observation_persistence", observation_persistence);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "expected_update_rate", expected_update_rate);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "obstacle_range", obstacle_range);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "raytrace_range", raytrace_range);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_enabled", frustum_clearing_enabled);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_pixel_step", frustum_clearing_pixel_step);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_min_range", frustum_min_range);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_max_range", frustum_max_range);
|
|
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
|
|
robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range);
|
|
}
|
|
}
|
|
}
|
|
else if(plugin_type == "ObstacleLayer")
|
|
{
|
|
double max_obstacle_height;
|
|
double raytrace_range;
|
|
double obstacle_range;
|
|
bool track_unknown_space;
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "max_obstacle_height", max_obstacle_height);
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "raytrace_range", raytrace_range);
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "obstacle_range", obstacle_range);
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "track_unknown_space", track_unknown_space);
|
|
if(plugin_nh.hasParam("observation_sources"))
|
|
{
|
|
std::string topics_string;
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "observation_sources", topics_string);
|
|
robot::log_error("topics_string: %s", topics_string.c_str());
|
|
std::stringstream ss(topics_string);
|
|
std::string source;
|
|
while (ss >> source)
|
|
{
|
|
robot::NodeHandle plugin_nh_element(plugin_nh, source);
|
|
robot::NodeHandle costmap_plugin_nh_element(costmap_plugin_nh, source);
|
|
std::string topic;
|
|
std::string data_type;
|
|
bool clearing;
|
|
bool marking;
|
|
bool inf_is_valid;
|
|
std::string sensor_frame;
|
|
double observation_persistence;
|
|
double expected_update_rate;
|
|
double min_obstacle_height;
|
|
double max_obstacle_height;
|
|
double obstacle_range;
|
|
double raytrace_range;
|
|
bool frustum_clearing_enabled = false;
|
|
int frustum_clearing_pixel_step = 8;
|
|
double frustum_min_range = 0.2;
|
|
double frustum_max_range = 3.0;
|
|
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "observation_persistence", observation_persistence);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "expected_update_rate", expected_update_rate);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "obstacle_range", obstacle_range);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "raytrace_range", raytrace_range);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_enabled", frustum_clearing_enabled);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_pixel_step", frustum_clearing_pixel_step);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_min_range", frustum_min_range);
|
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_max_range", frustum_max_range);
|
|
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
|
|
robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range);
|
|
}
|
|
}
|
|
}
|
|
else if(plugin_type == "InflationLayer")
|
|
{
|
|
double cost_scaling_factor;
|
|
double inflation_radius;
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "cost_scaling_factor", cost_scaling_factor);
|
|
move_parameter(plugin_nh, costmap_plugin_nh, "inflation_radius", inflation_radius);
|
|
}
|
|
|
|
}
|
|
|
|
void Costmap2DROBOT::setUnpaddedRobotFootprintPolygon(const robot_geometry_msgs::Polygon& footprint)
|
|
{
|
|
setUnpaddedRobotFootprint(toPointVector(footprint));
|
|
}
|
|
|
|
Costmap2DROBOT::~Costmap2DROBOT()
|
|
{
|
|
map_update_thread_shutdown_ = true;
|
|
if (map_update_thread_ != NULL)
|
|
{
|
|
map_update_thread_->join();
|
|
delete map_update_thread_;
|
|
}
|
|
|
|
delete layered_costmap_;
|
|
}
|
|
|
|
|
|
void Costmap2DROBOT::readFootprintFromConfig(const std::vector<robot_geometry_msgs::Point> &new_footprint,
|
|
const std::vector<robot_geometry_msgs::Point> &old_footprint,
|
|
const double &robot_radius)
|
|
{
|
|
// Only change the footprint if footprint or robot_radius has
|
|
// changed.
|
|
if(new_footprint.size() == old_footprint.size())
|
|
{
|
|
for(size_t i = 0; i < new_footprint.size(); i++)
|
|
{
|
|
if(new_footprint[i].x != old_footprint[i].x ||
|
|
new_footprint[i].y != old_footprint[i].y ||
|
|
new_footprint[i].z != old_footprint[i].z)
|
|
{
|
|
break;
|
|
}
|
|
if(i == (new_footprint.size()-1)) return;
|
|
}
|
|
}
|
|
if (!new_footprint.empty())
|
|
{
|
|
setUnpaddedRobotFootprint(new_footprint);
|
|
}
|
|
else
|
|
{
|
|
// robot_radius may be 0, but that must be intended at this point.
|
|
setUnpaddedRobotFootprint(makeFootprintFromRadius(robot_radius));
|
|
}
|
|
}
|
|
|
|
void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector<robot_geometry_msgs::Point>& points)
|
|
{
|
|
unpadded_footprint_ = points;
|
|
padded_footprint_ = points;
|
|
padFootprint(padded_footprint_, footprint_padding_);
|
|
|
|
layered_costmap_->setFootprint(padded_footprint_);
|
|
}
|
|
|
|
void Costmap2DROBOT::checkMovement()
|
|
{
|
|
robot_geometry_msgs::PoseStamped new_pose;
|
|
if (!getRobotPose(new_pose))
|
|
std::cout << "Cannot get robot pose\n";
|
|
}
|
|
|
|
void Costmap2DROBOT::mapUpdateLoop(double frequency)
|
|
{
|
|
std::cout << robot::Time::now().toNSec()<< " :Costmap2DROBOT::mapUpdateLoop updateMap\n";
|
|
robot::Rate r(frequency);
|
|
while (!map_update_thread_shutdown_)
|
|
{
|
|
updateMap();
|
|
r.sleep();
|
|
// make sure to sleep for the remainder of our cycle time
|
|
if (r.cycleTime() > robot::Duration(1 / frequency))
|
|
robot::log_warning("Map update %s loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds\n", name_.c_str(), frequency,
|
|
r.cycleTime().toSec());
|
|
}
|
|
}
|
|
|
|
void Costmap2DROBOT::updateMap()
|
|
{
|
|
if (!stop_updates_)
|
|
{
|
|
// get global pose
|
|
robot_geometry_msgs::PoseStamped pose;
|
|
if (getRobotPose (pose))
|
|
{
|
|
double x = pose.pose.position.x,
|
|
y = pose.pose.position.y,
|
|
yaw = data_convert::getYaw(pose.pose.orientation);
|
|
// robot::log_error("ROBOT POSE: %f | %f | %f",x,y,yaw);
|
|
layered_costmap_->updateMap(x, y, yaw);
|
|
|
|
footprint_.header.frame_id = global_frame_;
|
|
footprint_.header.stamp = robot::Time::now();
|
|
transformFootprint(x, y, yaw, padded_footprint_, footprint_);
|
|
|
|
initialized_ = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void Costmap2DROBOT::start()
|
|
{
|
|
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::start");
|
|
std::vector < PluginLayerPtr> *plugins = layered_costmap_->getPlugins();
|
|
// check if we're stopped or just paused
|
|
if (stopped_)
|
|
{
|
|
// if we're stopped we need to re-subscribe to topics
|
|
for (vector<PluginLayerPtr>::iterator plugin = plugins->begin(); plugin != plugins->end();
|
|
++plugin)
|
|
{
|
|
(*plugin)->activate();
|
|
}
|
|
stopped_ = false;
|
|
}
|
|
stop_updates_ = false;
|
|
|
|
// block until the costmap is re-initialized.. meaning one update cycle has run
|
|
// note: this does not hold, if the user has disabled map-updates allgother
|
|
robot::Time start_time = robot::Time::now();
|
|
robot::Rate r(100.0);
|
|
while (!initialized_ && map_update_thread_)
|
|
{
|
|
if (robot::Time::now() - start_time > robot::Duration(5.0))
|
|
{
|
|
robot::log_warning_throttle(3.0, "Costmap2DROBOT::start() timed out waiting for costmap to initialize or map_update_thread\n");
|
|
break;
|
|
}
|
|
r.sleep();
|
|
}
|
|
}
|
|
|
|
void Costmap2DROBOT::stop()
|
|
{
|
|
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::stop");
|
|
stop_updates_ = true;
|
|
std::vector <PluginLayerPtr> *plugins = layered_costmap_->getPlugins();
|
|
// unsubscribe from topics
|
|
for (vector<PluginLayerPtr>::iterator plugin = plugins->begin(); plugin != plugins->end();
|
|
++plugin)
|
|
{
|
|
(*plugin)->deactivate();
|
|
}
|
|
initialized_ = false;
|
|
stopped_ = true;
|
|
}
|
|
|
|
void Costmap2DROBOT::pause()
|
|
{
|
|
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::pause");
|
|
stop_updates_ = true;
|
|
initialized_ = false;
|
|
}
|
|
|
|
void Costmap2DROBOT::resume()
|
|
{
|
|
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::resume");
|
|
stop_updates_ = false;
|
|
|
|
// block until the costmap is re-initialized.. meaning one update cycle has run
|
|
robot::Time start_time = robot::Time::now();
|
|
robot::Rate r(100.0);
|
|
while (!initialized_)
|
|
{
|
|
if (robot::Time::now() - start_time > robot::Duration(5.0))
|
|
{
|
|
robot::log_warning("Costmap2DROBOT::resume() timed out waiting for costmap to initialize\n");
|
|
break;
|
|
}
|
|
r.sleep();
|
|
}
|
|
|
|
}
|
|
|
|
|
|
void Costmap2DROBOT::resetLayers()
|
|
{
|
|
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::resetLayers");
|
|
Costmap2D* top = layered_costmap_->getCostmap();
|
|
top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());
|
|
std::vector <PluginLayerPtr> *plugins = layered_costmap_->getPlugins();
|
|
for (vector<PluginLayerPtr>::iterator plugin = plugins->begin(); plugin != plugins->end();
|
|
++plugin)
|
|
{
|
|
(*plugin)->reset();
|
|
}
|
|
}
|
|
|
|
bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose) const
|
|
{
|
|
robot_geometry_msgs::PoseStamped robot_pose;
|
|
robot_geometry_msgs::Pose pose_default;
|
|
pose_default.orientation.x = 0;
|
|
pose_default.orientation.y = 0;
|
|
pose_default.orientation.z = 0;
|
|
pose_default.orientation.w = 1;
|
|
pose_default.position.x = 0;
|
|
pose_default.position.y = 0;
|
|
pose_default.position.z = 0;
|
|
global_pose.pose = pose_default;
|
|
robot_pose.pose = pose_default;
|
|
|
|
robot_pose.header.frame_id = robot_base_frame_;
|
|
robot_pose.header.stamp = robot::Time();
|
|
robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
|
|
|
|
// get the global pose of the robot
|
|
try
|
|
{
|
|
// use current time if possible (makes sure it's not in the future)
|
|
if (tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time()))
|
|
{
|
|
tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_,tf3::Time());
|
|
tf3::doTransform(robot_pose, global_pose, transform);
|
|
// robot::log_error("%s ||| %f | %f | %f ||| %f | %f | %f | %f", transform.child_frame_id.c_str(),
|
|
// global_pose.pose.position.x,
|
|
// global_pose.pose.position.y,
|
|
// global_pose.pose.position.z,
|
|
// global_pose.pose.orientation.x,
|
|
// global_pose.pose.orientation.y,
|
|
// global_pose.pose.orientation.z,
|
|
// global_pose.pose.orientation.w);
|
|
// transform.transform.rotation.x,
|
|
// transform.transform.rotation.y,
|
|
// transform.transform.rotation.z,
|
|
// transform.transform.rotation.w);
|
|
}
|
|
// use the latest otherwise
|
|
else
|
|
{
|
|
// tf_.transform(robot_pose, global_pose, global_frame_);
|
|
tf3::TransformStampedMsg transform = tf_.lookupTransform(
|
|
global_frame_, // frame đích
|
|
robot_base_frame_, // frame nguồn
|
|
tf3::Time()
|
|
);
|
|
tf3::doTransform(robot_pose, global_pose, transform);
|
|
}
|
|
}
|
|
catch (tf3::LookupException& ex)
|
|
{
|
|
robot::log_error("Costmap2DROBOT %s No Transform available Error looking up robot pose: %s\n", name_.c_str(), ex.what());
|
|
return false;
|
|
}
|
|
catch (tf3::ConnectivityException& ex)
|
|
{
|
|
robot::log_error("Costmap2DROBOT %s Connectivity Error looking up robot pose: %s\n", name_.c_str(), ex.what());
|
|
return false;
|
|
}
|
|
catch (tf3::ExtrapolationException& ex)
|
|
{
|
|
// robot::log_error("Costmap2DROBOT %s Extrapolation Error looking up robot pose: %s\n", name_.c_str(), ex.what());
|
|
return false;
|
|
}
|
|
// ROS_INFO_THROTTLE(1.0, "Time Delay %f , p %f %f", current_time.toSec() - global_pose.header.stamp.toSec(), global_pose.pose.position.x, global_pose.pose.position.y);
|
|
// check global_pose timeout
|
|
if (!global_pose.header.stamp.isZero() && current_time.toSec() - global_pose.header.stamp.toSec() > transform_tolerance_)
|
|
{
|
|
robot::log_warning("Costmap2DROBOT %s transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f\n", name_.c_str(),
|
|
current_time.toSec(), global_pose.header.stamp.toSec(), transform_tolerance_);
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void Costmap2DROBOT::getOrientedFootprint(std::vector<robot_geometry_msgs::Point>& oriented_footprint) const
|
|
{
|
|
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::getOrientedFootprint");
|
|
robot_geometry_msgs::PoseStamped global_pose;
|
|
if (!getRobotPose(global_pose))
|
|
return;
|
|
|
|
double yaw = data_convert::getYaw(global_pose.pose.orientation);
|
|
transformFootprint(global_pose.pose.position.x, global_pose.pose.position.y, yaw,
|
|
padded_footprint_, oriented_footprint);
|
|
}
|
|
|
|
} // namespace robot_costmap_2d
|