void EpicNavCorePlugin::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
{
if (name.length() == 0 || costmap_ros == nullptr) {
ROS_ERROR("Error[EpicNavCorePlugin::initialize]: Costmap2DROS object is not initialized.");
return;
}
uninitialize();
costmap = costmap_ros->getCostmap();
harmonic.n = 2;
harmonic.m = new unsigned int[2];
harmonic.m[0] = costmap->getSizeInCellsY();
harmonic.m[1] = costmap->getSizeInCellsX();
harmonic.u = new float[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()];
harmonic.locked = new unsigned int[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()];
setCellsFromCostmap();
setBoundariesAsObstacles();
ros::NodeHandle privateNodeHandle("~/" + name);
pub_plan = privateNodeHandle.advertise<nav_msgs::Path>("plan", 1);
//pub_potential = privateNodeHandle.advertise<nav_msgs::OccupancyGrid>("potential", 1);
initialized = true;
}
/**
* \brief Load the configuration from file.
* @param configurationPrefix The entry node inside the configuration YAML file.
* @return The complete image provider configuration.
*/
ImageProviderConfiguration::ConstPtr getImageProviderConfiguration () {
ImageProviderConfiguration::Ptr configuration = boost::make_shared<ImageProviderConfiguration>();
// private parameters
ros::NodeHandle privateNodeHandle ("~");
std::string configurationPrefix;
privateNodeHandle.param("cfgprefix", configurationPrefix, std::string("configuration"));
privateNodeHandle.param("startpos", configuration->startPos, 0);
privateNodeHandle.param("length", configuration->length, -1);
privateNodeHandle.param("rate", configuration->rate, 1);
privateNodeHandle.param("loop", configuration->loop, false);
privateNodeHandle.param("topicprefix", configuration->topicPrefix, std::string("cameras"));
ros::NodeHandle configNodeHandle (privateNodeHandle, configurationPrefix);
// configuration parameters (supposed to be loaded via YAML file)
if (!configNodeHandle.getParam("srcdir", configuration->sourceDir)) {
configuration->sourceDir = ".";
}
if (!configNodeHandle.getParam("filepattern", configuration->filePattern)) {
throw ImageProviderException("Parameter 'filepattern' not found");
}
try {
XmlRpc::XmlRpcValue cameraNames;
if (configNodeHandle.getParam("cameras", cameraNames)) {
std::map<std::string, XmlRpc::XmlRpcValue>::iterator nameIterator;
for (nameIterator = cameraNames.begin(); nameIterator != cameraNames.end(); ++nameIterator) {
std::string cameraName = nameIterator->first;
ROS_INFO_STREAM("Reading camera configuration for camera: " << cameraName);
CameraConfigEntry::ConstPtr cameraConfig = getCameraConfigurationEntry(configNodeHandle, cameraName);
configuration->cameraConfigs.push_back(cameraConfig);
}
}
}
catch (XmlRpc::XmlRpcException& e) {
throw ImageProviderException(std::string("Failed to retrieve camera names: ") + std::string(e.getMessage()));
}
return configuration;
}
