int main (int argc, char **argv)
{
std::string map_file_name;
long data_size;
ros::init(argc, argv, "sync_set_stcm_node");
ros::NodeHandle nh("~");
ros::ServiceClient client = nh.serviceClient<slamware_ros_sdk::SyncSetStcm>("/slamware_ros_sdk_server_node/sync_set_stcm");
slamware_ros_sdk::SyncSetStcm srv;
//get file name from paramter server
if (!nh.getParam("map_file_name",map_file_name))
{
ROS_ERROR("invalied file path......");
return 1;
}
ROS_INFO("map file path: %s",map_file_name.c_str());
//open map file and get data
std::ifstream fin(map_file_name ,std::ios::in | std::ios::binary | std::ios::ate);
if(!fin.is_open())
{
ROS_ERROR("open map file failed......");
return 1;
}
ROS_INFO("open map file success,reading data......");
data_size = fin.tellg();
fin.seekg(0,std::ios::beg);
srv.request.raw_stcm.resize(data_size);
fin.read(reinterpret_cast<char *>(srv.request.raw_stcm.data()),data_size*sizeof(srv.request.raw_stcm.front()));
fin.close();
ROS_INFO("raw_stcm data size: %zu",srv.request.raw_stcm.size());
//set robot pose
srv.request.robot_pose.position.x = 0;
srv.request.robot_pose.position.y = 0;
srv.request.robot_pose.position.z = 0;
srv.request.robot_pose.orientation.x = 0;
srv.request.robot_pose.orientation.y = 0;
srv.request.robot_pose.orientation.z = 0;
srv.request.robot_pose.orientation.w = 1;
//call service
if (client.call(srv))
{
ROS_INFO("set stcm map success......");
}
else
{
ROS_ERROR("Failed to set stcm map......");
return 1;
}
return 0;
} |