96 lines
2.6 KiB
C++
96 lines
2.6 KiB
C++
#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);
|
|
}
|