int main (int argc, char **argv)
{
std::string map_file_name;
ros::init(argc, argv, "sync_set_stcm_node");
ros::NodeHandle nh("~");
ros::ServiceClient client = nh.serviceClient<slamware_ros_sdk::SyncGetStcm>("/slamware_ros_sdk_server_node/sync_get_stcm");
slamware_ros_sdk::SyncGetStcm srv;
//get file name from paramter server
if (!nh.getParam("map_file_name",map_file_name))
{
ROS_ERROR("invalied file path......");
return 1;
}
//call service
if (client.call(srv))
{
ROS_INFO("get raw_stcm data success,writing to file......");
//check whether the target folder exists. create one if not
std::string dir;
dir = map_file_name.substr(0,map_file_name.rfind("/"));
if(access(dir.c_str(),0))
{
std::string cmd = "mkdir -p " + dir;
system(cmd.c_str());
}
std::ofstream fout(map_file_name, std::ios::out | std::ios::binary);
fout.write(reinterpret_cast<char *>(srv.response.raw_stcm.data()),(srv.response.raw_stcm.size()) * sizeof(srv.response.raw_stcm.front()));
fout.close();
ROS_INFO("raw_stcm data size: %zu",srv.response.raw_stcm.size());
ROS_INFO("save map at %s success......",map_file_name.c_str());
}
else
{
ROS_ERROR("Failed to get raw_stcm data......");
return 1;
}
return 0;
} |