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

update file costmap_2d_robot

Browse Source
This commit is contained in:
duongtd
2025-11-26 15:00:26 +07:00
parent 19a96c27d5
commit 2e0a4348dd
9 changed files with 551 additions and 1200 deletions
Download Patch File Download Diff File Expand all files Collapse all files

647
src/costmap_2d_robot.cpp
View File

@@ -40,7 +40,7 @@
#include <algorithm>
#include <vector>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <costmap_2d/utils.h>
@@ -48,24 +48,10 @@
#include <tf3/utils.h>
#include <exception>
// #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
using namespace std;
namespace costmap_2d
{
// void move_parameter(ros::NodeHandle& old_h, ros::NodeHandle& new_h, std::string name, bool should_delete = true)
// {
// if (!old_h.hasParam(name))
// return;
// XmlRpc::XmlRpcValue value;
// old_h.getParam(name, value);
// new_h.setParam(name, value);
// if (should_delete) old_h.deleteParam(name);
// }
Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
layered_costmap_(NULL),
name_(name),
@@ -78,159 +64,144 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
map_update_thread_(NULL),
footprint_padding_(0.0)
{
getParams("config.yaml");
// ros::NodeHandle private_nh("~/" + name);
// ros::NodeHandle g_nh;
// // get global and robot base frame names
// private_nh.param("global_frame", global_frame_, std::string("map"));
// private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
// ros::Time last_error = ros::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 (ros::ok()
// && !tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), &tf_error))
// {
// ros::spinOnce();
// if (last_error + ros::Duration(5.0) < ros::Time::now())
// {
// ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",
// robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
// last_error = ros::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, track_unknown_space, always_send_full_costmap;
// private_nh.param("rolling_window", rolling_window, false);
// private_nh.param("track_unknown_space", track_unknown_space, false);
// private_nh.param("always_send_full_costmap", always_send_full_costmap, false);
// layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
// if (!private_nh.hasParam("plugins"))
// {
// loadOldParameters(private_nh);
// } else {
// warnForOldParameters(private_nh);
// }
// if (private_nh.hasParam("plugins"))
// {
// XmlRpc::XmlRpcValue my_list;
// private_nh.getParam("plugins", my_list);
// for (int32_t i = 0; i < my_list.size(); ++i)
// {
// std::string pname = static_cast<std::string>(my_list[i]["name"]);
// std::string type = static_cast<std::string>(my_list[i]["type"]);
// ROS_INFO("%s: Using plugin \"%s\" with type %s", name_.c_str(), pname.c_str(), type.c_str());
// try
// {
// copyParentParameters(pname, type, private_nh);
// boost::shared_ptr<costmap_2d::Layer> plugin = plugin_loader_.createInstance(type);
// layered_costmap_->addPlugin(plugin);
// plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
// }
// catch (pluginlib::PluginlibException &ex)
// {
// ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", type.c_str(), ex.what());
// }
// }
// }
// // subscribe to the footprint topic
// std::string topic_param, topic;
// if (!private_nh.searchParam("footprint_topic", topic_param))
// {
// topic_param = "footprint_topic";
// }
// private_nh.param(topic_param, topic, std::string("footprint"));
// footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROBOT::setUnpaddedRobotFootprintPolygon, this);
// if (!private_nh.searchParam("published_footprint_topic", topic_param))
// {
// topic_param = "published_footprint";
// }
// private_nh.param(topic_param, topic, std::string("footprint")); // TODO: revert to oriented_footprint in N-turtle
// footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>(topic, 1);
// setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));
// publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",
// always_send_full_costmap);
// // create a thread to handle updating the map
// stop_updates_ = false;
// initialized_ = true;
// stopped_ = false;
// dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));
// dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = [this](auto& config, auto level){ reconfigureCB(config, level); };
// dsrv_->setCallback(cb);
// create a thread to handle updating the map
stop_updates_ = false;
initialized_ = true;
stopped_ = false;
}
void Costmap2DROBOT::getParams()
void Costmap2DROBOT::getParams(const std::string& config_file_name)
{
YAML::Node config = YAML::LoadFile("../costmap_2d/config/config.yaml");
YAML::Node layer = config["static_layer"];
global_frame_ = loadParam(layer, "global_frame", std::string("map"));
robot_base_frame_ = loadParam(layer, "robot_base_frame", std::string("base_link"));
robot::Time last_error = robot::Time::now();
std::string tf_error;
// check if we want a rolling window version of the costmap
bool rolling_window, track_unknown_space, always_send_full_costmap;
rolling_window = loadParam(layer, "rolling_window", false);
track_unknown_space = loadParam(layer, "track_unknown_space", false);
always_send_full_costmap = loadParam(layer, "always_send_full_costmap", false);
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
struct PluginInfo { std::string path; std::string name; };
std::vector<PluginInfo> plugins_to_load = {
{"./costmap_2d/libstatic_layer.so", "static_layer"},
{"./costmap_2d/libinflation_layer.so", "inflation_layer"},
{"./costmap_2d/libobstacle_layer.so", "obstacle_layer"},
{"./costmap_2d/libpreferred_layer.so", "preferred_layer"}
};
// if (private_nh.hasParam("plugins"))
// {
// my_list = loadParam(layer, "plugins", my_list);
// for (int32_t i = 0; i < my_list.size(); ++i)
// {
// std::string pname = static_cast<std::string>(my_list[i]["name"]);
// std::string type = static_cast<std::string>(my_list[i]["type"]);
// printf("%s: Using plugin \"%s\" with type %s\n", name_.c_str(), pname.c_str(), type.c_str());
for (auto& info : plugins_to_load)
try
{
try
std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
if(path_source != " ")
{
// copyParentParameters(pname, type, private_nh);
auto creator = boost::dll::import_alias<PluginLayerPtr()>(
info.path, "create_plugin", boost::dll::load_mode::append_decorations
);
PluginLayerPtr plugin = creator();
std::cout << "Plugin created: " << info.name << std::endl;
plugin->initialize(layered_costmap_, info.name, &tf_);
layered_costmap_->addPlugin(plugin);
std::cout << "Path source: " << path_source << std::endl;
}
catch (std::exception &ex)
else
{
printf("Failed to create the s, are you sure it is properly registered and that the containing library is built? Exception: %s\n", ex.what());
std::cout << "/cfg folder not found!" << std::endl;
}
YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["static_layer"];
global_frame_ = loadParam(layer, "global_frame", std::string("map"));
robot_base_frame_ = loadParam(layer, "robot_base_frame", std::string("base_link"));
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::now(), &tf_error))
{
if (last_error + robot::Duration(5.0) < robot::Time::now())
{
printf("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
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, track_unknown_space, always_send_full_costmap;
rolling_window = loadParam(layer, "rolling_window", false);
track_unknown_space = loadParam(layer, "track_unknown_space", false);
always_send_full_costmap = loadParam(layer, "always_send_full_costmap", false);
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
struct PluginInfo { std::string path; std::string name; };
std::vector<PluginInfo> plugins_to_load = {
{"./costmap_2d/libstatic_layer.so", "static_layer"},
{"./costmap_2d/libinflation_layer.so", "inflation_layer"},
{"./costmap_2d/libobstacle_layer.so", "obstacle_layer"},
{"./costmap_2d/libpreferred_layer.so", "preferred_layer"}
};
// if (private_nh.hasParam("plugins"))
// {
// my_list = loadParam(layer, "plugins", my_list);
// for (int32_t i = 0; i < my_list.size(); ++i)
// {
// std::string pname = static_cast<std::string>(my_list[i]["name"]);
// std::string type = static_cast<std::string>(my_list[i]["type"]);
// printf("%s: Using plugin \"%s\" with type %s\n", name_.c_str(), pname.c_str(), type.c_str());
for (auto& info : plugins_to_load)
{
try
{
// copyParentParameters(pname, type, private_nh);
auto creator = boost::dll::import_alias<PluginLayerPtr()>(
info.path, "create_plugin", boost::dll::load_mode::append_decorations
);
PluginLayerPtr plugin = creator();
std::cout << "Plugin created: " << info.name << std::endl;
plugin->initialize(layered_costmap_, info.name, &tf_);
layered_costmap_->addPlugin(plugin);
}
catch (std::exception &ex)
{
printf("Failed to create the s, are you sure it is properly registered and that the containing library is built? Exception: %s\n", ex.what());
}
}
// setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));
transform_tolerance_ = loadParam(layer, "transform_tolerance", false);
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", false);
// find size parameters
double map_width_meters = loadParam(layer, "width", false);
double map_height_meters = loadParam(layer, "height", false);
double resolution = loadParam(layer, "resolution", false);
double origin_x = loadParam(layer, "origin_x", false);
double origin_y = loadParam(layer, "origin_y", false);
if (!layered_costmap_->isSizeLocked())
{
layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
(unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
}
// If the padding has changed, call setUnpaddedRobotFootprint() to
// re-apply the padding.
float footprint_padding = loadParam(layer, "footprint_padding", 0.0);
if (footprint_padding_ != footprint_padding)
{
footprint_padding_ = footprint_padding;
setUnpaddedRobotFootprint(unpadded_footprint_);
}
double robot_radius = loadParam(layer, "robot_radius", 0.0);
std::vector<geometry_msgs::Point> new_footprint;// = loadParam(layer, "footprint", 0.0);
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::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint)
@@ -246,215 +217,40 @@ Costmap2DROBOT::~Costmap2DROBOT()
map_update_thread_->join();
delete map_update_thread_;
}
// if (publisher_ != NULL)
// delete publisher_;
delete layered_costmap_;
// delete dsrv_;
}
// void Costmap2DROBOT::loadOldParameters(ros::NodeHandle& nh)
// {
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::loadOldParameters");
// ROS_WARN("%s: Parameter \"plugins\" not provided, loading pre-Hydro parameters", name_.c_str());
// bool flag;
// std::string s;
// std::vector < XmlRpc::XmlRpcValue > plugins;
// XmlRpc::XmlRpcValue::ValueStruct map;
// SuperValue super_map;
// SuperValue super_array;
// if (nh.getParam("static_map", flag) && flag)
// {
// map["name"] = XmlRpc::XmlRpcValue("static_layer");
// map["type"] = XmlRpc::XmlRpcValue("costmap_2d::StaticLayer");
// super_map.setStruct(&map);
// plugins.push_back(super_map);
// ros::NodeHandle map_layer(nh, "static_layer");
// move_parameter(nh, map_layer, "map_topic");
// move_parameter(nh, map_layer, "unknown_cost_value");
// move_parameter(nh, map_layer, "lethal_cost_threshold");
// move_parameter(nh, map_layer, "track_unknown_space", false);
// }
// ros::NodeHandle obstacles(nh, "obstacle_layer");
// if (nh.getParam("map_type", s) && s == "voxel")
// {
// map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
// map["type"] = XmlRpc::XmlRpcValue("costmap_2d::VoxelLayer");
// super_map.setStruct(&map);
// plugins.push_back(super_map);
// move_parameter(nh, obstacles, "origin_z");
// move_parameter(nh, obstacles, "z_resolution");
// move_parameter(nh, obstacles, "z_voxels");
// move_parameter(nh, obstacles, "mark_threshold");
// move_parameter(nh, obstacles, "unknown_threshold");
// move_parameter(nh, obstacles, "publish_voxel_map");
// }
// else
// {
// map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
// map["type"] = XmlRpc::XmlRpcValue("costmap_2d::ObstacleLayer");
// super_map.setStruct(&map);
// plugins.push_back(super_map);
// }
// move_parameter(nh, obstacles, "max_obstacle_height");
// move_parameter(nh, obstacles, "raytrace_range");
// move_parameter(nh, obstacles, "obstacle_range");
// move_parameter(nh, obstacles, "track_unknown_space", true);
// nh.param("observation_sources", s, std::string(""));
// std::stringstream ss(s);
// std::string source;
// while (ss >> source)
// {
// move_parameter(nh, obstacles, source);
// }
// move_parameter(nh, obstacles, "observation_sources");
// ros::NodeHandle inflation(nh, "inflation_layer");
// move_parameter(nh, inflation, "cost_scaling_factor");
// move_parameter(nh, inflation, "inflation_radius");
// map["name"] = XmlRpc::XmlRpcValue("inflation_layer");
// map["type"] = XmlRpc::XmlRpcValue("costmap_2d::InflationLayer");
// super_map.setStruct(&map);
// plugins.push_back(super_map);
// super_array.setArray(&plugins);
// nh.setParam("plugins", super_array);
// }
// void Costmap2DROBOT::copyParentParameters(const std::string& plugin_name, const std::string& plugin_type, ros::NodeHandle& nh)
// {
// ros::NodeHandle target_layer(nh, plugin_name);
// if(plugin_type == "costmap_2d::StaticLayer")
// {
// move_parameter(nh, target_layer, "map_topic", false);
// move_parameter(nh, target_layer, "unknown_cost_value", false);
// move_parameter(nh, target_layer, "lethal_cost_threshold", false);
// move_parameter(nh, target_layer, "track_unknown_space", false);
// }
// else if(plugin_type == "costmap_2d::VoxelLayer")
// {
// move_parameter(nh, target_layer, "origin_z", false);
// move_parameter(nh, target_layer, "z_resolution", false);
// move_parameter(nh, target_layer, "z_voxels", false);
// move_parameter(nh, target_layer, "mark_threshold", false);
// move_parameter(nh, target_layer, "unknown_threshold", false);
// move_parameter(nh, target_layer, "publish_voxel_map", false);
// }
// else if(plugin_type == "costmap_2d::ObstacleLayer")
// {
// move_parameter(nh, target_layer, "max_obstacle_height", false);
// move_parameter(nh, target_layer, "raytrace_range", false);
// move_parameter(nh, target_layer, "obstacle_range", false);
// move_parameter(nh, target_layer, "track_unknown_space", false);
// }
// else if(plugin_type == "costmap_2d::InflationLayer")
// {
// move_parameter(nh, target_layer, "cost_scaling_factor", false);
// move_parameter(nh, target_layer, "inflation_radius", false);
// }
// }
// void Costmap2DROBOT::warnForOldParameters(ros::NodeHandle& nh)
// {
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::warnForOldParameters");
// checkOldParam(nh, "static_map");
// checkOldParam(nh, "map_type");
// }
// void Costmap2DROBOT::checkOldParam(ros::NodeHandle& nh, const std::string &param_name){
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::checkOldParam");
// if(nh.hasParam(param_name)){
// ROS_WARN("%s: Pre-Hydro parameter \"%s\" unused since \"plugins\" is provided", name_.c_str(), param_name.c_str());
// }
// }
// void Costmap2DROBOT::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level)
// {
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::reconfigureCB");
// transform_tolerance_ = config.transform_tolerance;
// 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 = config.update_frequency;
// double map_publish_frequency = config.publish_frequency;
// if (map_publish_frequency > 0)
// publish_cycle = ros::Duration(1 / map_publish_frequency);
// else
// publish_cycle = ros::Duration(-1);
// // find size parameters
// double map_width_meters = config.width, map_height_meters = config.height, resolution = config.resolution, origin_x =
// config.origin_x,
// origin_y = config.origin_y;
// if (!layered_costmap_->isSizeLocked())
// {
// layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
// (unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
// }
// // If the padding has changed, call setUnpaddedRobotFootprint() to
// // re-apply the padding.
// if (footprint_padding_ != config.footprint_padding)
// {
// footprint_padding_ = config.footprint_padding;
// setUnpaddedRobotFootprint(unpadded_footprint_);
// }
// readFootprintFromConfig(config, old_config_);
// old_config_ = config;
// // 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));
// }
// void Costmap2DROBOT::readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config,
// const costmap_2d::Costmap2DConfig &old_config)
// {
// // Only change the footprint if footprint or robot_radius has
// // changed. Otherwise we might overwrite a footprint sent on a
// // topic by a dynamic_reconfigure call which was setting some other
// // variable.
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::readFootprintFromConfig");
// if (new_config.footprint == old_config.footprint &&
// new_config.robot_radius == old_config.robot_radius)
// {
// return;
// }
// if (new_config.footprint != "" && new_config.footprint != "[]")
// {
// std::vector<geometry_msgs::Point> new_footprint;
// if (makeFootprintFromString(new_config.footprint, new_footprint))
// {
// setUnpaddedRobotFootprint(new_footprint);
// }
// else
// {
// ROS_ERROR("Invalid footprint string from dynamic reconfigure");
// }
// }
// else
// {
// // robot_radius may be 0, but that must be intended at this point.
// setUnpaddedRobotFootprint(makeFootprintFromRadius(new_config.robot_radius));
// }
// }
void Costmap2DROBOT::readFootprintFromConfig(const std::vector<geometry_msgs::Point> &new_footprint,
const std::vector<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<geometry_msgs::Point>& points)
{
@@ -465,62 +261,27 @@ void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::
layered_costmap_->setFootprint(padded_footprint_);
}
// void Costmap2DROBOT::movementCB(const ros::TimerEvent &event)
// {
// // don't allow configuration to happen while this check occurs
// // boost::recursive_mutex::scoped_lock mcl(configuration_mutex_);
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::movementCB");
// geometry_msgs::PoseStamped new_pose;
void Costmap2DROBOT::checkMovement()
{
geometry_msgs::PoseStamped new_pose;
if (!getRobotPose(new_pose))
std::cout << "Cannot get robot pose\n";
}
// if (!getRobotPose(new_pose))
// {
// ROS_WARN_THROTTLE(1.0, "Could not get robot pose, cancelling reconfiguration");
// }
// }
void Costmap2DROBOT::mapUpdateLoop(double frequency)
{
robot::Rate r(frequency);
while (!map_update_thread_shutdown_)
{
updateMap();
// void Costmap2DROBOT::mapUpdateLoop(double frequency)
// {
// ros::NodeHandle nh;
// ros::Rate r(frequency);
// while (nh.ok() && !map_update_thread_shutdown_)
// {
// #ifdef HAVE_SYS_TIME_H
// struct timeval start, end;
// double start_t, end_t, t_diff;
// gettimeofday(&start, NULL);
// #endif
// updateMap();
// #ifdef HAVE_SYS_TIME_H
// gettimeofday(&end, NULL);
// start_t = start.tv_sec + double(start.tv_usec) / 1e6;
// end_t = end.tv_sec + double(end.tv_usec) / 1e6;
// t_diff = end_t - start_t;
// ROS_DEBUG("Map update time: %.9f", t_diff);
// #endif
// if (publish_cycle.toSec() > 0 && layered_costmap_->isInitialized())
// {
// unsigned int x0, y0, xn, yn;
// layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
// publisher_->updateBounds(x0, xn, y0, yn);
// ros::Time now = ros::Time::now();
// if (last_publish_ + publish_cycle < now)
// {
// publisher_->publishCostmap();
// last_publish_ = now;
// }
// }
// ros::spinOnce();
// r.sleep();
// // make sure to sleep for the remainder of our cycle time
// if (r.cycleTime() > ros::Duration(1 / frequency))
// ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency,
// r.cycleTime().toSec());
// }
// }
r.sleep();
// make sure to sleep for the remainder of our cycle time
if (r.cycleTime() > robot::Duration(1 / frequency))
printf("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds\n", frequency,
r.cycleTime().toSec());
}
}
void Costmap2DROBOT::updateMap()
{
@@ -534,12 +295,10 @@ void Costmap2DROBOT::updateMap()
y = pose.pose.position.y,
yaw = getYaw(pose.pose.orientation);
layered_costmap_->updateMap(x, y, yaw);
// geometry_msgs::PolygonStamped footprint;
// footprint.header.frame_id = global_frame_;
// footprint.header.stamp = ros::Time::now();
// transformFootprint(x, y, yaw, padded_footprint_, footprint);
// footprint_pub_.publish(footprint);
geometry_msgs::PolygonStamped footprint;
footprint.header.frame_id = global_frame_;
footprint.header.stamp = robot::Time::now();
transformFootprint(x, y, yaw, padded_footprint_, footprint);
initialized_ = true;
}
@@ -565,9 +324,17 @@ void Costmap2DROBOT::start()
// 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::Rate r(100.0);
// while (ros::ok() && !initialized_ && map_update_thread_)
// r.sleep();
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))
{
printf("Costmap2DROBOT::start() timed out waiting for costmap to initialize or map_update_thread\n");
break;
}
r.sleep();
}
}
void Costmap2DROBOT::stop()
@@ -598,9 +365,18 @@ void Costmap2DROBOT::resume()
stop_updates_ = false;
// block until the costmap is re-initialized.. meaning one update cycle has run
// robot::Rate r(100.0);
// while (!initialized_)
// r.sleep();
robot::Time start_time = robot::Time::now();
robot::Rate r(100.0);
while (!initialized_)
{
if (robot::Time::now() - start_time > robot::Duration(5.0))
{
printf("Costmap2DROBOT::resume() timed out waiting for costmap to initialize\n");
break;
}
r.sleep();
}
}
@@ -619,10 +395,11 @@ void Costmap2DROBOT::resetLayers()
bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
{
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::getRobotPose");
// tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
geometry_msgs::PoseStamped robot_pose;
// tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose);
geometry_msgs::Pose pose_default;
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
@@ -634,12 +411,18 @@ bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
if (tf_.canTransform(global_frame_, robot_base_frame_, convertTime(current_time)))
{
tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, convertTime(current_time));
// tf3::doTransform(robot_pose, global_pose, transform);
tf3::doTransform(robot_pose, global_pose, transform);
}
// 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::convertTime(robot_pose.header.stamp)
);
tf3::doTransform(robot_pose, global_pose, transform);
}
}
catch (tf3::LookupException& ex)

206
src/footprint.cpp
View File

@@ -208,117 +208,117 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
// std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
// {
// std::string full_param_name;
// std::string full_radius_param_name;
// std::vector<geometry_msgs::Point> points;
std::vector<geometry_msgs::Point> makeFootprintFromParams(const std::string& file_name)
{
std::string full_param_name;
std::string full_radius_param_name;
std::vector<geometry_msgs::Point> points;
// if (nh.searchParam("footprint", full_param_name))
// {
// XmlRpc::XmlRpcValue footprint_xmlrpc;
// nh.getParam(full_param_name, footprint_xmlrpc);
// if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
// footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
// {
// if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
// {
// writeFootprintToParam(nh, points);
// return points;
// }
// }
// else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
// {
// points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
// writeFootprintToParam(nh, points);
// return points;
// }
// }
// if (nh.searchParam("footprint", full_param_name))
// {
// XmlRpc::XmlRpcValue footprint_xmlrpc;
// nh.getParam(full_param_name, footprint_xmlrpc);
// if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
// footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
// {
// if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
// {
// writeFootprintToParam(nh, points);
// return points;
// }
// }
// else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
// {
// points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
// writeFootprintToParam(nh, points);
// return points;
// }
// }
// if (nh.searchParam("robot_radius", full_radius_param_name))
// {
// double robot_radius;
// nh.param(full_radius_param_name, robot_radius, 1.234);
// points = makeFootprintFromRadius(robot_radius);
// nh.setParam("robot_radius", robot_radius);
// }
// // Else neither param was found anywhere this knows about, so
// // defaults will come from dynamic_reconfigure stuff, set in
// // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
// return points;
// }
// if (nh.searchParam("robot_radius", full_radius_param_name))
// {
// double robot_radius;
// nh.param(full_radius_param_name, robot_radius, 1.234);
// points = makeFootprintFromRadius(robot_radius);
// nh.setParam("robot_radius", robot_radius);
// }
// // Else neither param was found anywhere this knows about, so
// // defaults will come from dynamic_reconfigure stuff, set in
// // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
// return points;
}
// void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint)
// {
// std::ostringstream oss;
// bool first = true;
// for (unsigned int i = 0; i < footprint.size(); i++)
// {
// geometry_msgs::Point p = footprint[ i ];
// if (first)
// {
// oss << "[[" << p.x << "," << p.y << "]";
// first = false;
// }
// else
// {
// oss << ",[" << p.x << "," << p.y << "]";
// }
// }
// oss << "]";
// nh.setParam("footprint", oss.str().c_str());
// }
void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint)
{
// std::ostringstream oss;
// bool first = true;
// for (unsigned int i = 0; i < footprint.size(); i++)
// {
// geometry_msgs::Point p = footprint[ i ];
// if (first)
// {
// oss << "[[" << p.x << "," << p.y << "]";
// first = false;
// }
// else
// {
// oss << ",[" << p.x << "," << p.y << "]";
// }
// }
// oss << "]";
// nh.setParam("footprint", oss.str().c_str());
}
// double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name)
// {
// // Make sure that the value we're looking at is either a double or an int.
// if (value.getType() != XmlRpc::XmlRpcValue::TypeInt &&
// value.getType() != XmlRpc::XmlRpcValue::TypeDouble)
// {
// std::string& value_string = value;
// std::printf("Values in the footprint specification (param %s) must be numbers. Found value %s.",
// full_param_name.c_str(), value_string.c_str());
// throw std::runtime_error("Values in the footprint specification must be numbers");
// }
// return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
// }
double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name)
{
// Make sure that the value we're looking at is either a double or an int.
if (value.getType() != XmlRpc::XmlRpcValue::TypeInt &&
value.getType() != XmlRpc::XmlRpcValue::TypeDouble)
{
std::string& value_string = value;
std::printf("Values in the footprint specification (param %s) must be numbers. Found value %s.\n",
full_param_name.c_str(), value_string.c_str());
throw std::runtime_error("Values in the footprint specification must be numbers\n");
}
return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
}
// std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpcValue& footprint_xmlrpc,
// const std::string& full_param_name)
// {
// // Make sure we have an array of at least 3 elements.
// if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
// footprint_xmlrpc.size() < 3)
// {
// std::printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s",
// full_param_name.c_str(), std::string(footprint_xmlrpc).c_str());
// throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least "
// "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
// }
std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc,
const std::string& full_param_name)
{
// Make sure we have an array of at least 3 elements.
if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
footprint_xmlrpc.size() < 3)
{
std::printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n",
full_param_name.c_str(), std::string(footprint_xmlrpc).c_str());
throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least "
"3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]\n");
}
// std::vector<geometry_msgs::Point> footprint;
// geometry_msgs::Point pt;
std::vector<geometry_msgs::Point> footprint;
geometry_msgs::Point pt;
// for (int i = 0; i < footprint_xmlrpc.size(); ++i)
// {
// // Make sure each element of the list is an array of size 2. (x and y coordinates)
// XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ];
// if (point.getType() != XmlRpc::XmlRpcValue::TypeArray ||
// point.size() != 2)
// {
// std::printf("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
// "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.",
// full_param_name.c_str());
// throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: "
// "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
// }
for (int i = 0; i < footprint_xmlrpc.size(); ++i)
{
// Make sure each element of the list is an array of size 2. (x and y coordinates)
XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ];
if (point.getType() != XmlRpc::XmlRpcValue::TypeArray ||
point.size() != 2)
{
std::printf("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
"[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.\n",
full_param_name.c_str());
throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: "
"[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form\n");
}
// pt.x = getNumberFromXMLRPC(point[ 0 ], full_param_name);
// pt.y = getNumberFromXMLRPC(point[ 1 ], full_param_name);
pt.x = getNumberFromXMLRPC(point[ 0 ], full_param_name);
pt.y = getNumberFromXMLRPC(point[ 1 ], full_param_name);
// footprint.push_back(pt);
// }
// return footprint;
// }
footprint.push_back(pt);
}
return footprint;
}
} // end namespace costmap_2d