git commit -m "first commit for v2"
This commit is contained in:
699
Localizations/Packages/loc_base/src/loc_base.cpp
Normal file
699
Localizations/Packages/loc_base/src/loc_base.cpp
Normal file
@@ -0,0 +1,699 @@
|
||||
#include <XmlRpcException.h>
|
||||
#include "loc_base/loc_base.h"
|
||||
#include "loc_base/loc_base_util.h"
|
||||
#include "map_server/map_generator.h"
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <signal.h>
|
||||
#include <boost/version.hpp>
|
||||
|
||||
std::shared_ptr<amcl::Amcl> loc_base::LocBase::localization_althm_ = nullptr;
|
||||
std::shared_ptr<slam_toolbox::AsynchronousSlamToolbox> loc_base::LocBase::mapping_althm_ = nullptr;
|
||||
TFListenerPtr loc_base::LocBase::tf_;
|
||||
std::string loc_base::LocBase::base_frame_id_;
|
||||
std::string loc_base::LocBase::global_frame_id_;
|
||||
std::string* loc_base::LocBase::working_dir_ptr_ = NULL;
|
||||
|
||||
loc_base::LocBase::LocBase()
|
||||
: initialized_(false)
|
||||
{
|
||||
ROS_INFO_STREAM("Boost version: " << BOOST_LIB_VERSION);
|
||||
}
|
||||
|
||||
loc_base::LocBase::~LocBase() {
|
||||
cleanup();
|
||||
}
|
||||
|
||||
void loc_base::LocBase::cleanup() {
|
||||
// Clean up map servers
|
||||
for (auto& map_pair : static_map_vt_) {
|
||||
map_pair.second.reset();
|
||||
}
|
||||
static_map_vt_.clear();
|
||||
|
||||
// Clean up localization
|
||||
if (loc_base::LocBase::localization_althm_)
|
||||
loc_base::LocBase::localization_althm_.reset();
|
||||
// Clean up mapping
|
||||
if (loc_base::LocBase::mapping_althm_)
|
||||
loc_base::LocBase::mapping_althm_.reset();
|
||||
}
|
||||
|
||||
void loc_base::LocBase::initialize(ros::NodeHandle nh, TFListenerPtr tf)
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
nh_ = nh;
|
||||
tf_ = tf;
|
||||
private_nh_ = ros::NodeHandle("~");
|
||||
this->loadParameters();
|
||||
std::string activated_map_filename;
|
||||
if (this->loadActivateMapFileName(activated_map_filename))
|
||||
{
|
||||
loc_base::LocBase::activated_map_filename_ = activated_map_filename;
|
||||
ROS_INFO("Active map name %s", loc_base::LocBase::activated_map_filename_.c_str());
|
||||
}
|
||||
else ROS_WARN("Can not get active map name");
|
||||
|
||||
ros::NodeHandle node;
|
||||
init_pub_ = node.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1);
|
||||
|
||||
switch (slam_state_)
|
||||
{
|
||||
case loc_core::Ready:
|
||||
this->startLocalization();
|
||||
break;
|
||||
case loc_core::Localization:
|
||||
this->startLocalization();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
initialized_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::loadActivateMapFileName(std::string& activated_map_filename)
|
||||
{
|
||||
if (this->working_dir_.empty()) return false;
|
||||
std::string filename = std::string(this->working_dir_ + "/activated_map.txt");
|
||||
bool result = loc_base::LocBaseUtil::loadActivateMapFileName(filename, activated_map_filename);
|
||||
return result;
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::saveActivateMapFileName(const std::string& activated_map_filename)
|
||||
{
|
||||
if (activated_map_filename.empty()) return false;
|
||||
if (this->working_dir_.empty()) return false;
|
||||
std::string filename = std::string(this->working_dir_ + "/activated_map.txt");
|
||||
|
||||
bool result = this->createStaticMap(activated_map_filename);
|
||||
if (!result) return false;
|
||||
|
||||
if (slam_state_ != loc_core::Mapping)
|
||||
{
|
||||
std::string static_map_folder = std::string(this->working_dir_ + "/" + activated_map_filename);
|
||||
if (!boost::filesystem::exists(static_map_folder))
|
||||
{
|
||||
ROS_ERROR("%s file is not existed", static_map_folder.c_str());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
result = loc_base::LocBaseUtil::saveActivateMapFileName(filename, activated_map_filename);
|
||||
if (!result) return false;;
|
||||
activated_map_filename_ = activated_map_filename;
|
||||
return result;
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::createStaticMap(const std::string map_name)
|
||||
{
|
||||
if (slam_state_ == loc_core::Mapping)
|
||||
{
|
||||
if (this->working_dir_.empty())
|
||||
{
|
||||
ROS_WARN("Working directory is not seting");
|
||||
return false;
|
||||
}
|
||||
std::string static_map_folder = std::string(this->working_dir_ + "/" + map_name);
|
||||
if (!boost::filesystem::exists(static_map_folder))
|
||||
boost::filesystem::create_directories(static_map_folder);
|
||||
else
|
||||
{
|
||||
ROS_WARN("Map file is existed");
|
||||
return false;
|
||||
}
|
||||
std::string static_map_name = std::string(static_map_folder + "/" + map_name);
|
||||
std::shared_ptr<map_server::MapGenerator> map_generator_ptr =
|
||||
std::make_shared<map_server::MapGenerator>(static_map_name, this->map_topic_);
|
||||
return map_generator_ptr->waitingGenerator(5.0);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::changeStaticMap(const std::string filename)
|
||||
{
|
||||
if (filename.empty()) return false;
|
||||
ROS_INFO("filename %s", filename.c_str());
|
||||
if (!loc_base::LocBase::activated_map_filename_.empty() && activated_map_filename_ == filename) return true;
|
||||
try
|
||||
{
|
||||
for (int32_t i = 0; i < plugins_.size(); ++i)
|
||||
{
|
||||
std::string pname = static_cast<std::string>(plugins_[i]["name"]);
|
||||
std::string type = static_cast<std::string>(plugins_[i]["type"]);
|
||||
if (type == std::string("costmap_2d::StaticLayer"))
|
||||
{
|
||||
if (!nh_.hasParam(pname))
|
||||
continue;
|
||||
else
|
||||
{
|
||||
std::string map_topic, ns;
|
||||
nh_.param(std::string(pname + "/namespace"), ns, std::string(""));
|
||||
nh_.param(std::string(pname + "/map_topic"), map_topic, std::string(""));
|
||||
|
||||
if (ns == std::string("") && (map_topic == std::string("map") || map_topic == std::string("/map")))
|
||||
{
|
||||
if (std::map<std::string, std::shared_ptr<map_server::MapServer>>::iterator ms = static_map_vt_.find(pname); ms != static_map_vt_.end())
|
||||
{
|
||||
std::string path_to_yaml = std::string(this->working_dir_ + "/" + filename + "/" + filename + ".yaml");
|
||||
ROS_INFO("path_to_yaml %s", path_to_yaml.c_str());
|
||||
if (!boost::filesystem::exists(path_to_yaml))
|
||||
{
|
||||
ROS_WARN("%s is not exist", path_to_yaml.c_str());
|
||||
return false;
|
||||
}
|
||||
ROS_INFO("loadMapFromYaml %s", path_to_yaml.c_str());
|
||||
bool result = ms->second->loadMapFromYaml(path_to_yaml);
|
||||
if (result) this->saveActivateMapFileName(filename);
|
||||
return result;
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Not found");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (XmlRpc::XmlRpcException& e)
|
||||
{
|
||||
ROS_ERROR("Change map is error: %s", e.getMessage().c_str());
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::changeVirtualWallsMap(const std::string filename)
|
||||
{
|
||||
if (filename.empty()) return false;
|
||||
try
|
||||
{
|
||||
for (int32_t i = 0; i < plugins_.size(); ++i)
|
||||
{
|
||||
std::string pname = static_cast<std::string>(plugins_[i]["name"]);
|
||||
std::string type = static_cast<std::string>(plugins_[i]["type"]);
|
||||
if (type == std::string("costmap_2d::StaticLayer"))
|
||||
{
|
||||
if (!nh_.hasParam(pname))
|
||||
continue;
|
||||
else
|
||||
{
|
||||
std::string map_topic, ns;
|
||||
nh_.param(std::string(pname + "/namespace"), ns, std::string(""));
|
||||
nh_.param(std::string(pname + "/map_topic"), map_topic, std::string(""));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (XmlRpc::XmlRpcException& e)
|
||||
{
|
||||
ROS_ERROR("Change virtual map is error: %s", e.getMessage().c_str());
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void loc_base::LocBase::listMapFiles(std::stringstream& directories)
|
||||
{
|
||||
directories = loc_base::LocBaseUtil::listMapFiles(*loc_base::LocBase::working_dir_ptr_);
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::startMapping()
|
||||
{
|
||||
if (slam_state_ != loc_core::Mapping)
|
||||
{
|
||||
this->stopLocalization();
|
||||
this->init_mapping();
|
||||
slam_state_ = loc_core::Mapping;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::stopMapping()
|
||||
{
|
||||
if (slam_state_ == loc_core::Mapping)
|
||||
{
|
||||
this->close_mapping();
|
||||
slam_state_ = loc_core::Ready;
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::startLocalization()
|
||||
{
|
||||
if (slam_state_ != loc_core::Localization)
|
||||
{
|
||||
this->stopMapping();
|
||||
bool resule_map_server = this->init_map_server();
|
||||
bool resule_localization = this->init_localization();
|
||||
|
||||
if (!resule_map_server || !resule_localization)
|
||||
{
|
||||
this->stopLocalization();
|
||||
slam_state_ = loc_core::Error;
|
||||
return false;
|
||||
}
|
||||
slam_state_ = loc_core::Localization;
|
||||
return resule_map_server && resule_localization;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::stopLocalization()
|
||||
{
|
||||
if (slam_state_ == loc_core::Localization)
|
||||
{
|
||||
this->close_localization();
|
||||
this->close_map_server();
|
||||
slam_state_ = loc_core::Ready;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
loc_core::slam_state_en loc_base::LocBase::getState()
|
||||
{
|
||||
return slam_state_;
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::loadParameters()
|
||||
{
|
||||
std::string package;
|
||||
std::string map_topic;
|
||||
std::string ns;
|
||||
plugins_.clear();
|
||||
if (!nh_.getParam("plugins", plugins_))
|
||||
{
|
||||
ROS_WARN("Can not get plugins");
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
try
|
||||
{
|
||||
for (int32_t i = 0; i < plugins_.size(); ++i)
|
||||
{
|
||||
std::string pname = static_cast<std::string>(plugins_[i]["name"]);
|
||||
std::string type = static_cast<std::string>(plugins_[i]["type"]);
|
||||
if (pname != std::string("navigation_map")) continue;
|
||||
if (type == std::string("costmap_2d::StaticLayer"))
|
||||
{
|
||||
if (!nh_.hasParam(pname))
|
||||
continue;
|
||||
else
|
||||
{
|
||||
nh_.param(std::string(pname + "/map_pkg"), package, std::string(""));
|
||||
nh_.param(std::string(pname + "/namespace"), ns, std::string(""));
|
||||
nh_.param(std::string(pname + "/map_topic"), map_topic, std::string(""));
|
||||
if (package.empty())
|
||||
{
|
||||
ROS_FATAL("'/map_pkg' is empty");
|
||||
exit(1);
|
||||
}
|
||||
if (map_topic.empty())
|
||||
{
|
||||
ROS_WARN("'/map_topic' is empty");
|
||||
exit(1);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (XmlRpc::XmlRpcException& e)
|
||||
{
|
||||
ROS_ERROR("Load map is error: %s", e.getMessage().c_str());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
std::string working_dir = loc_base::LocBaseUtil::filepathFromPackage(package, "");
|
||||
std::string static_map_folder = std::string(working_dir + "/maps");
|
||||
if (!boost::filesystem::exists(static_map_folder))
|
||||
boost::filesystem::create_directories(static_map_folder);
|
||||
ROS_INFO("static_map_folder : %s", static_map_folder.c_str());
|
||||
loc_base::LocBase::working_dir_ptr_ = &this->working_dir_;
|
||||
this->working_dir_ = static_map_folder;
|
||||
this->map_topic_ = map_topic;
|
||||
}
|
||||
catch (std::exception& e)
|
||||
{
|
||||
ROS_ERROR("Error to check working directory: %s", e.what());
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::init_map_server()
|
||||
{
|
||||
ROS_INFO("Init Map Server");
|
||||
ROS_INFO("Getting %s/plugins .....", nh_.getNamespace().c_str());
|
||||
try
|
||||
{
|
||||
for (int32_t i = 0; i < plugins_.size(); ++i)
|
||||
{
|
||||
std::string pname = static_cast<std::string>(plugins_[i]["name"]);
|
||||
std::string type = static_cast<std::string>(plugins_[i]["type"]);
|
||||
if (type == std::string("costmap_2d::StaticLayer"))
|
||||
{
|
||||
if (!nh_.hasParam(pname))
|
||||
ROS_WARN("Can not get plugins/%s", pname.c_str());
|
||||
else
|
||||
{
|
||||
try
|
||||
{
|
||||
std::string map_pkg, map_file, ns;
|
||||
nh_.param(std::string(pname + "/map_pkg"), map_pkg, std::string(""));
|
||||
nh_.param(std::string(pname + "/map_file"), map_file, std::string(""));
|
||||
nh_.param(std::string(pname + "/namespace"), ns, std::string(""));
|
||||
std::string path_map_file;
|
||||
if (loc_base::LocBase::activated_map_filename_.empty())
|
||||
{
|
||||
path_map_file = loc_base::LocBaseUtil::filepathFromPackage(map_pkg, std::string("/maps/" + map_file + "/" + map_file + ".yaml"));
|
||||
if (pname == std::string("navigation_map"))
|
||||
{
|
||||
if (!boost::filesystem::exists(std::string(this->working_dir_ + "/activated_map.txt")))
|
||||
{
|
||||
loc_base::LocBase::activated_map_filename_ = map_file;
|
||||
this->saveActivateMapFileName(loc_base::LocBase::activated_map_filename_);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
path_map_file = std::string(this->working_dir_ + "/" + loc_base::LocBase::activated_map_filename_ + "/" + loc_base::LocBase::activated_map_filename_ + ".yaml");
|
||||
|
||||
ROS_INFO(" Using plugin \"%s\" with type %s, map_file %s", pname.c_str(), type.c_str(), path_map_file.c_str());
|
||||
static_map_vt_[pname] = std::make_shared<map_server::MapServer>(nh_, ns, path_map_file, 0.0);
|
||||
}
|
||||
catch (std::runtime_error& e)
|
||||
{
|
||||
ROS_ERROR("map_server exception: %s", e.what());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (XmlRpc::XmlRpcException& e)
|
||||
{
|
||||
ROS_ERROR("Init map is error: %s", e.getMessage().c_str());
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::close_map_server()
|
||||
{
|
||||
// plugins_.clear();
|
||||
// map_file_vt_.clear();
|
||||
ROS_INFO("Waiting MapServer be closed");
|
||||
static_map_vt_.clear();
|
||||
ROS_INFO("Closed MapServer");
|
||||
return true;
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::init_localization()
|
||||
{
|
||||
ROS_INFO("Init localization");
|
||||
signal(SIGINT, &loc_base::LocBase::signalHandler);
|
||||
ros::NodeHandle nh = ros::NodeHandle(private_nh_, "Amcl");
|
||||
nh.param("base_frame_id", loc_base::LocBase::base_frame_id_, std::string("base_link"));
|
||||
nh.param("global_frame_id", loc_base::LocBase::global_frame_id_, std::string("map"));
|
||||
|
||||
geometry_msgs::PoseWithCovariance init_pose;
|
||||
init_pose.pose.position.x = init_pose.pose.position.y = 0.0;
|
||||
init_pose.covariance[6 * 0 + 0] = 0.5 * 0.5;
|
||||
init_pose.covariance[6 * 1 + 1] = 0.5 * 0.5;
|
||||
init_pose.covariance[6 * 5 + 5] = M_PI / 12.0 * M_PI / 12.0;
|
||||
|
||||
std::string path_file = std::string(this->working_dir_ + "/initial_pose.txt");
|
||||
if (boost::filesystem::exists(path_file))
|
||||
{
|
||||
try
|
||||
{
|
||||
std::ifstream file(path_file, std::ios::in);
|
||||
if (!file.is_open())
|
||||
{
|
||||
ROS_WARN_THROTTLE(2.0, "There was a problem opening the input file!\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
double x, y, theta;
|
||||
float init_cov[3];
|
||||
|
||||
file >> x >> y >> theta >> init_cov[0] >> init_cov[1] >> init_cov[2];
|
||||
// Check if the read operation was successful
|
||||
if (file.fail())
|
||||
{
|
||||
ROS_ERROR("Failed to read data from the file: %s", path_file.c_str());
|
||||
}
|
||||
ROS_INFO_STREAM(x << " " << y << " " << theta << " " << init_cov[0] << " " << init_cov[1] << " " << init_cov[2]);
|
||||
|
||||
init_pose.pose.position.x = std::isnan(x) || fabs(x) < 1e-5 ? 0.0 : x;
|
||||
init_pose.pose.position.y = std::isnan(y) || fabs(y) < 1e-5 ? 0.0 : y;
|
||||
theta = std::isnan(theta) || fabs(theta) < 1e-5 ? 0.0 : theta;
|
||||
tf::Quaternion quat;
|
||||
quat.setRPY(0.0, 0.0, theta);
|
||||
tf::quaternionTFToMsg(quat, init_pose.pose.orientation);
|
||||
|
||||
init_pose.covariance[6 * 0 + 0] = std::isnan(init_cov[0]) || fabs(init_cov[0]) < 1e-4 ? 0.5 * 0.5 : init_cov[0];
|
||||
init_pose.covariance[6 * 1 + 1] = std::isnan(init_cov[1]) || fabs(init_cov[1]) < 1e-4 ? 0.5 * 0.5 : init_cov[1];
|
||||
init_pose.covariance[6 * 5 + 5] = std::isnan(init_cov[2]) || fabs(init_cov[2]) < 1e-4 ? M_PI / 12.0 * M_PI / 12.0 : init_cov[2];
|
||||
file.close(); // Optionally close the file explicitly, though it's closed when it goes out of scope
|
||||
}
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
ROS_ERROR("Caught an exception: %s", e.what());
|
||||
// Handle the exception appropriately, maybe return or do additional logging
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_INFO("Creating file: %s", path_file.c_str());
|
||||
double zero = 0.0;
|
||||
double initial_cov[3];
|
||||
initial_cov[0] = 0.5 * 0.5;
|
||||
initial_cov[1] = 0.5 * 0.5;
|
||||
initial_cov[2] = M_PI / 12.0 * M_PI / 12.0;
|
||||
std::ofstream file(path_file);
|
||||
file << zero << " " << zero << " " << zero << " " << initial_cov[0] << " " << initial_cov[1] << " " << initial_cov[3];
|
||||
file.close();
|
||||
}
|
||||
|
||||
loc_base::LocBase::localization_althm_ = std::make_shared<amcl::Amcl>(nh);
|
||||
while (loc_base::LocBase::localization_althm_ &&
|
||||
!loc_base::LocBase::localization_althm_->getInitialized())
|
||||
{
|
||||
ros::spinOnce();
|
||||
}
|
||||
|
||||
this->initalPose(init_pose);
|
||||
localization_timer_ptr_ = std::make_shared<ros::WallTimer>(
|
||||
private_nh_.createWallTimer(ros::WallDuration(1.0), &loc_base::LocBase::savePoseCallback, this));
|
||||
return true;
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::close_localization()
|
||||
{
|
||||
localization_timer_ptr_->stop();
|
||||
if (localization_timer_ptr_) localization_timer_ptr_.reset();
|
||||
|
||||
if (loc_base::LocBase::localization_althm_)
|
||||
loc_base::LocBase::localization_althm_.reset();
|
||||
ROS_INFO("Waiting Localization be closed");
|
||||
while (ros::ok() && loc_base::LocBase::localization_althm_)
|
||||
{
|
||||
ros::spinOnce();
|
||||
}
|
||||
ROS_INFO("Closed Localization");
|
||||
return true;
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::init_mapping()
|
||||
{
|
||||
int stack_size;
|
||||
ros::NodeHandle nh = ros::NodeHandle(private_nh_, "SlamToolBox");
|
||||
if (nh.getParam("stack_size_to_use", stack_size))
|
||||
{
|
||||
ROS_INFO("Node using stack size %i", (int)stack_size);
|
||||
const rlim_t max_stack_size = stack_size;
|
||||
struct rlimit stack_limit;
|
||||
getrlimit(RLIMIT_STACK, &stack_limit);
|
||||
if (stack_limit.rlim_cur < stack_size)
|
||||
{
|
||||
stack_limit.rlim_cur = stack_size;
|
||||
}
|
||||
setrlimit(RLIMIT_STACK, &stack_limit);
|
||||
}
|
||||
ROS_INFO("Waiting for init mapping");
|
||||
loc_base::LocBase::mapping_althm_ = std::make_shared<slam_toolbox::AsynchronousSlamToolbox>(nh_);
|
||||
while (ros::ok() && !loc_base::LocBase::mapping_althm_->getInitialized())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
ros::spinOnce();
|
||||
}
|
||||
|
||||
ROS_INFO("Initialized Mapping");
|
||||
return true;
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::close_mapping()
|
||||
{
|
||||
if (loc_base::LocBase::mapping_althm_)
|
||||
loc_base::LocBase::mapping_althm_.reset();
|
||||
|
||||
ROS_INFO("Waiting for close mapping");
|
||||
while (ros::ok() && loc_base::LocBase::mapping_althm_)
|
||||
{
|
||||
ros::spinOnce();
|
||||
}
|
||||
ROS_INFO("Closed Mapping");
|
||||
return true;
|
||||
}
|
||||
|
||||
void loc_base::LocBase::savePose()
|
||||
{
|
||||
geometry_msgs::PoseStamped global_pose;
|
||||
geometry_msgs::Pose2D pose2d;
|
||||
float initial_cov[3];
|
||||
bool updating;
|
||||
if (loc_base::LocBase::localization_althm_)
|
||||
{
|
||||
if (loc_base::LocBase::localization_althm_->getInitialized())
|
||||
{
|
||||
geometry_msgs::PoseWithCovarianceStamped amcl_pose;
|
||||
localization_althm_->savePoseToServer(amcl_pose);
|
||||
pose2d.x = amcl_pose.pose.pose.position.x;
|
||||
pose2d.y = amcl_pose.pose.pose.position.y;
|
||||
pose2d.theta = tf2::getYaw(amcl_pose.pose.pose.orientation);
|
||||
initial_cov[0] = amcl_pose.pose.covariance[6 * 0 + 0];
|
||||
initial_cov[1] = amcl_pose.pose.covariance[6 * 1 + 1];
|
||||
initial_cov[2] = amcl_pose.pose.covariance[6 * 5 + 5];
|
||||
updating = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (loc_base::LocBase::getPose(loc_base::LocBase::base_frame_id_, loc_base::LocBase::global_frame_id_, global_pose))
|
||||
{
|
||||
pose2d.x = global_pose.pose.position.x;
|
||||
pose2d.y = global_pose.pose.position.y;
|
||||
pose2d.theta = tf2::getYaw(global_pose.pose.orientation);
|
||||
initial_cov[0] = 0.5 * 0.5;
|
||||
initial_cov[1] = 0.5 * 0.5;
|
||||
initial_cov[2] = M_PI / 12.0 * M_PI / 12.0;
|
||||
updating = true;
|
||||
}
|
||||
}
|
||||
std::string path_file;
|
||||
if (loc_base::LocBase::working_dir_ptr_)
|
||||
path_file = std::string((*loc_base::LocBase::working_dir_ptr_) + "/initial_pose.txt");
|
||||
else return;
|
||||
|
||||
if (updating)
|
||||
{
|
||||
try
|
||||
{
|
||||
std::ofstream file(path_file, std::ios::out);
|
||||
//check to see that the file was opened correctly:
|
||||
if (!file.is_open()) {
|
||||
ROS_WARN_THROTTLE(2.0, "There was a problem opening the input file!\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
// ROS_INFO_STREAM(pose2d.x << " " << pose2d.y << " " << pose2d.theta << " " << initial_cov[0] << " " << initial_cov[1] << " " << initial_cov[2]);
|
||||
pose2d.x = std::isnan(pose2d.x) || fabs(pose2d.x) < 1e-4 ? 0.0 : pose2d.x;
|
||||
pose2d.y = std::isnan(pose2d.y) || fabs(pose2d.y) < 1e-4 ? 0.0 : pose2d.y;
|
||||
pose2d.theta = std::isnan(pose2d.theta) || fabs(pose2d.theta) < 1e-3 ? 0.0 : pose2d.theta;
|
||||
|
||||
initial_cov[0] = std::isnan(initial_cov[0]) || fabs(initial_cov[0]) < 1e-4 ? 0.5 * 0.5 : initial_cov[0];
|
||||
initial_cov[1] = std::isnan(initial_cov[1]) || fabs(initial_cov[1]) < 1e-4 ? 0.5 * 0.5 : initial_cov[1];
|
||||
initial_cov[2] = std::isnan(initial_cov[2]) || fabs(initial_cov[2]) < 1e-4 ? M_PI / 12.0 * M_PI / 12.0 : initial_cov[2];
|
||||
// ROS_INFO_STREAM(pose2d.x << " " << pose2d.y << " " << pose2d.theta << " " << initial_cov[0] << " " << initial_cov[1] << " " << initial_cov[2]);
|
||||
file << pose2d.x << " " << pose2d.y << " " << pose2d.theta << " "
|
||||
<< initial_cov[0] << " " << initial_cov[1] << " " << initial_cov[2];
|
||||
// file << pose2d.x << " " << pose2d.y << " " << pose2d.theta;
|
||||
file.close();
|
||||
}
|
||||
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void loc_base::LocBase::signalHandler(int sign)
|
||||
{
|
||||
loc_base::LocBase::savePose();
|
||||
ros::shutdown();
|
||||
}
|
||||
|
||||
void loc_base::LocBase::savePoseCallback(const ros::WallTimerEvent& event)
|
||||
{
|
||||
loc_base::LocBase::savePose();
|
||||
}
|
||||
|
||||
bool loc_base::LocBase::getPose(std::string base_frame_id, std::string map_frame,
|
||||
geometry_msgs::PoseStamped& global_pose, double transform_tolerance)
|
||||
{
|
||||
geometry_msgs::PoseStamped global_pose_stamped;
|
||||
tf2::toMsg(tf2::Transform::getIdentity(), global_pose_stamped.pose);
|
||||
geometry_msgs::PoseStamped robot_pose;
|
||||
tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
|
||||
robot_pose.header.frame_id = base_frame_id;
|
||||
robot_pose.header.stamp = ros::Time();
|
||||
ros::Time current_time = ros::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 (loc_base::LocBase::tf_->canTransform(map_frame, base_frame_id, current_time))
|
||||
{
|
||||
geometry_msgs::TransformStamped transform = loc_base::LocBase::tf_->lookupTransform(map_frame, base_frame_id, current_time);
|
||||
tf2::doTransform(robot_pose, global_pose_stamped, transform);
|
||||
}
|
||||
// use the latest otherwise
|
||||
else
|
||||
{
|
||||
loc_base::LocBase::tf_->transform(robot_pose, global_pose_stamped, map_frame);
|
||||
}
|
||||
global_pose = global_pose_stamped;
|
||||
}
|
||||
catch (tf2::LookupException& ex)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
catch (tf2::ConnectivityException& ex)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
catch (tf2::ExtrapolationException& ex)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (!global_pose_stamped.header.stamp.isZero() && current_time.toSec() - global_pose_stamped.header.stamp.toSec() > transform_tolerance)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void loc_base::LocBase::initalPose(geometry_msgs::PoseWithCovariance init_pose)
|
||||
{
|
||||
geometry_msgs::PoseWithCovarianceStamped pose;
|
||||
pose.header.frame_id = loc_base::LocBase::global_frame_id_;
|
||||
pose.header.stamp = ros::Time::now();
|
||||
pose.pose = init_pose;
|
||||
this->init_pub_.publish(pose);
|
||||
}
|
||||
|
||||
// register this planner as a LocalPlanner plugin
|
||||
PLUGINLIB_EXPORT_CLASS(loc_base::LocBase, loc_core::BaseLocalization)
|
||||
95
Localizations/Packages/loc_base/src/loc_base_node.cpp
Normal file
95
Localizations/Packages/loc_base/src/loc_base_node.cpp
Normal file
@@ -0,0 +1,95 @@
|
||||
#include <loc_base/loc_base.h>
|
||||
#include <loc_core/localization.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <std_msgs/UInt16.h>
|
||||
|
||||
// std::shared_ptr<loc_base::LocBase> loc_base_ptr = nullptr;
|
||||
std::shared_ptr<loc_core::BaseLocalization> loc_base_ptr = nullptr;
|
||||
static loc_core::BaseLocalization* loc_base_;
|
||||
|
||||
void callBack(const std_msgs::UInt16& msg)
|
||||
{
|
||||
std::stringstream directories;
|
||||
if (loc_base_)
|
||||
{
|
||||
switch (msg.data)
|
||||
{
|
||||
case 1:
|
||||
loc_base_->startMapping();
|
||||
/* code */
|
||||
break;
|
||||
case 2:
|
||||
loc_base_->stopMapping();
|
||||
/* code */
|
||||
break;
|
||||
case 3:
|
||||
loc_base_->listMapFiles(directories);
|
||||
ROS_INFO_STREAM(directories.str());
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void createMapCallBack(const std_msgs::String& msg)
|
||||
{
|
||||
if (loc_base_)
|
||||
loc_base_->saveActivateMapFileName(msg.data);
|
||||
}
|
||||
|
||||
void changeMapCallBack(const std_msgs::String& msg)
|
||||
{
|
||||
if (loc_base_)
|
||||
loc_base_->changeStaticMap(msg.data);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
ros::init(argc, argv, "loc_base_node");
|
||||
ros::start();
|
||||
ros::NodeHandle nh(ros::this_node::getName());
|
||||
|
||||
std::shared_ptr<tf2_ros::Buffer> tf = std::make_shared<tf2_ros::Buffer>();
|
||||
tf2_ros::TransformListener tf2(*tf);
|
||||
// loc_base_ptr = std::make_shared<loc_base::LocBase>();
|
||||
pluginlib::ClassLoader<loc_core::BaseLocalization> loc_base_loader_("loc_core", "loc_core::BaseLocalization");
|
||||
|
||||
std::string obj_name("loc_base::LocBase");
|
||||
if (tf == nullptr)
|
||||
throw std::runtime_error("tf2_ros::Buffer object is null");
|
||||
try
|
||||
{
|
||||
loc_base_ptr = loc_base_loader_.createUniqueInstance(obj_name);
|
||||
loc_base_ptr->initialize(nh, tf);
|
||||
loc_base_ = loc_base_ptr.get();
|
||||
}
|
||||
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", obj_name.c_str(), ex.what());
|
||||
exit(1);
|
||||
}
|
||||
catch (std::exception& e)
|
||||
{
|
||||
ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what());
|
||||
exit(1);
|
||||
}
|
||||
|
||||
ros::Subscriber sub = nh.subscribe("mode", 1000, callBack);
|
||||
ros::Subscriber save_map_sub = nh.subscribe("save_map", 1000, createMapCallBack);
|
||||
ros::Subscriber change_map_sub = nh.subscribe("change_map", 1000, changeMapCallBack);
|
||||
ros::Publisher state = nh.advertise<std_msgs::UInt16>("state", 1);
|
||||
ros::Rate rate(3);
|
||||
|
||||
while (ros::ok())
|
||||
{
|
||||
std_msgs::UInt16 state_msg;
|
||||
state_msg.data = loc_base_->getState();
|
||||
state.publish(state_msg);
|
||||
rate.sleep();
|
||||
ros::spinOnce();
|
||||
}
|
||||
|
||||
ros::spin();
|
||||
loc_base_ptr.reset();
|
||||
return(0);
|
||||
}
|
||||
169
Localizations/Packages/loc_base/src/loc_base_util.cpp
Normal file
169
Localizations/Packages/loc_base/src/loc_base_util.cpp
Normal file
@@ -0,0 +1,169 @@
|
||||
#include <boost/filesystem.hpp>
|
||||
#include <boost/exception/exception.hpp>
|
||||
#include <boost/system/error_code.hpp>
|
||||
#include <ros/package.h>
|
||||
#include <fstream>
|
||||
#include "loc_base/loc_base_util.h"
|
||||
|
||||
std::string loc_base::LocBaseUtil::filepathFromPackage(const std::string& package, const std::string& filename)
|
||||
{
|
||||
std::string filepath = filename;
|
||||
|
||||
if (!package.empty())
|
||||
{
|
||||
// Get the package path using ROS package utility
|
||||
std::string pkg_path = ros::package::getPath(package);
|
||||
|
||||
// Log package path (info level, could be changed to ROS_DEBUG for less verbosity)
|
||||
// ROS_INFO_STREAM("Package path: " << pkg_path);
|
||||
|
||||
if (!pkg_path.empty())
|
||||
{
|
||||
// Construct the full file path using Boost Filesystem (or std::filesystem)
|
||||
filepath = (boost::filesystem::path(pkg_path) / filename).make_preferred().native();
|
||||
// For C++17 and later, you can use the following line instead:
|
||||
// filepath = (std::filesystem::path(pkg_path) / filename).string();
|
||||
|
||||
// Check if the file exists
|
||||
if (!boost::filesystem::exists(filepath)) // Or std::filesystem::exists(filepath) for C++17+
|
||||
{
|
||||
// If file does not exist, throw an exception
|
||||
std::string error_message = "File does not exist at: " + filepath;
|
||||
// ROS_ERROR_STREAM(error_message);
|
||||
throw std::runtime_error(error_message);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// If package path is empty, throw an exception
|
||||
std::string error_message = "Failed to get path for package '" + package + "'.";
|
||||
// ROS_ERROR_STREAM(error_message);
|
||||
throw std::runtime_error(error_message);
|
||||
}
|
||||
}
|
||||
|
||||
return filepath;
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool loc_base::LocBaseUtil::loadActivateMapFileName(const std::string& filename, std::string& activated_map_filename)
|
||||
{
|
||||
// Check if saved path file is existed
|
||||
std::ifstream inputFile;
|
||||
inputFile.open(filename);
|
||||
std::stringstream buffer;
|
||||
if (inputFile)
|
||||
{
|
||||
buffer << inputFile.rdbuf();
|
||||
inputFile.close();
|
||||
if (buffer.str().empty())
|
||||
return false;
|
||||
}
|
||||
else return false;
|
||||
activated_map_filename = buffer.str();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool loc_base::LocBaseUtil::saveActivateMapFileName(const std::string& filename, const std::string& activated_map_filename)
|
||||
{
|
||||
bool result = false;
|
||||
// Check if saved path file is existed
|
||||
std::ofstream outputFile(filename, std::ios::out);
|
||||
if (outputFile.is_open()) {
|
||||
outputFile << activated_map_filename;
|
||||
outputFile.close();
|
||||
result = true;
|
||||
}
|
||||
else
|
||||
result = false;
|
||||
return result;
|
||||
}
|
||||
|
||||
bool loc_base::LocBaseUtil::convertPgmToPng(const std::string& pgmFileName, const std::string& pngFileName)
|
||||
{
|
||||
std::string cmd;
|
||||
int retVal;
|
||||
cmd = "convert " + pgmFileName + " " + pngFileName;
|
||||
retVal = system(cmd.c_str());
|
||||
if (retVal != 0)
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool loc_base::LocBaseUtil::writeGridMap(const std::string& filename)
|
||||
{
|
||||
std::string pgmFileName = filename + ".pgm";
|
||||
std::string pngFileName = filename + ".png";
|
||||
std::string yamlFileName = filename + ".yaml";
|
||||
// bool is_SaveStateAsFiles = SaveStateAsFiles(filename); // cái này tham khảo https://github.com/ros-planning/navigation/blob/ac41d2480c4cf1602daf39a6e9629142731d92b0/map_server/src/map_saver.cpp#L114
|
||||
// if(is_SaveStateAsFiles)
|
||||
// {
|
||||
// if(convertPgmToPng(pgmFileName, pngFileName))
|
||||
// return true;
|
||||
// }
|
||||
return false;
|
||||
}
|
||||
|
||||
bool loc_base::LocBaseUtil::checkIfMapFilesExist(const std::string& filename)
|
||||
{
|
||||
|
||||
std::string SavedGridMapFileName = filename + ".png";
|
||||
std::string SavedMapInfoFileName = filename + ".yaml";
|
||||
|
||||
std::ifstream SavedGridMapFile(SavedGridMapFileName);
|
||||
std::ifstream SavedMapInfoFile(SavedMapInfoFileName);
|
||||
if (SavedGridMapFile.good() && SavedMapInfoFile.good())
|
||||
{
|
||||
ROS_INFO("map file: %s is found", filename.c_str());
|
||||
return true;
|
||||
}
|
||||
else ROS_WARN("map file: %s is not found", filename.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
bool loc_base::LocBaseUtil::replaceMapDirectory(const std::string& mapDirectory)
|
||||
{
|
||||
int retVal_1 = -1;
|
||||
int retVal_2 = -1;
|
||||
std::string removeCmd = "rm -rf " + mapDirectory;
|
||||
retVal_1 = system(removeCmd.c_str());
|
||||
if (retVal_1 == 0)
|
||||
{
|
||||
std::string createCmd = "mkdir " + mapDirectory;
|
||||
retVal_2 = system(createCmd.c_str());
|
||||
}
|
||||
if (retVal_2 == 0)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
std::stringstream loc_base::LocBaseUtil::listMapFiles(const std::string& path)
|
||||
{
|
||||
std::stringstream directories;
|
||||
try
|
||||
{
|
||||
for (const auto& entry : boost::filesystem::directory_iterator(path)) {
|
||||
if(entry.path().extension() == ".txt")
|
||||
continue;
|
||||
directories << entry.path().filename() << std::endl;
|
||||
}
|
||||
}
|
||||
catch (const boost::system::system_error& e) {
|
||||
// Catching boost::system::system_error (which inherits from boost::exception)
|
||||
ROS_ERROR_STREAM("Error reading directory: " << e.what());
|
||||
}
|
||||
catch (const boost::exception& e) {
|
||||
// Catching general boost exceptions
|
||||
ROS_ERROR_STREAM("Boost exception: List Files failed");
|
||||
}
|
||||
return directories;
|
||||
}
|
||||
Reference in New Issue
Block a user