int CVxParamArray::Initialize(vx_context context, vx_graph graph, const char * desc)
{
// get object parameters and create object
char objType[64];
const char * ioParams = ScanParameters(desc, "array|virtual-array:", "s:", objType);
if (!_stricmp(objType, "array") || !_stricmp(objType, "virtual-array") ||
!_stricmp(objType, "array-virtual"))
{
// syntax: [virtual-]array:<format>,<capacity>[:<io-params>]
char itemType[64];
ioParams = ScanParameters(ioParams, "<format>,<capacity>", "s,D", &itemType, &m_capacity);
bool found_userStruct = false;
for (auto it = m_userStructMap->begin(); it != m_userStructMap->end(); ++it){
if (strcmp(itemType, it->first.c_str()) == 0){
found_userStruct = true;
m_format = it->second;
}
}
if (found_userStruct == false){
m_format = ovxName2Enum(itemType);
if (m_format == 0) {
ReportError("ERROR: invalid array item type specified: %s\n", itemType);
}
}
// create array object
if (!_stricmp(objType, "virtual-array") || !_stricmp(objType, "array-virtual")) {
m_array = vxCreateVirtualArray(graph, m_format, m_capacity);
m_isVirtualObject = true;
}
else {
m_array = vxCreateArray(context, m_format, m_capacity);
}
}
else ReportError("ERROR: unsupported array type: %s\n", desc);
vx_status ovxStatus = vxGetStatus((vx_reference)m_array);
if (ovxStatus != VX_SUCCESS){
printf("ERROR: array creation failed => %d (%s)\n", ovxStatus, ovxEnum2Name(ovxStatus));
if (m_array) vxReleaseArray(&m_array);
throw - 1;
}
m_vxObjRef = (vx_reference)m_array;
// io initialize
return InitializeIO(context, graph, m_vxObjRef, ioParams);
}
int CVxParamDistribution::Initialize(vx_context context, vx_graph graph, const char * desc)
{
// get object parameters and create object
char objType[64];
const char * ioParams = ScanParameters(desc, "distribution:<numBins>,<offset>,<range>", "s:D,d,d", objType, &m_numBins, &m_offset, &m_range);
if (!_stricmp(objType, "distribution")) {
m_distribution = vxCreateDistribution(context, m_numBins, m_offset, m_range);
}
else ReportError("ERROR: unsupported distribution type: %s\n", desc);
vx_status ovxStatus = vxGetStatus((vx_reference)m_distribution);
if (ovxStatus != VX_SUCCESS){
printf("ERROR: distribution creation failed => %d (%s)\n", ovxStatus, ovxEnum2Name(ovxStatus));
if (m_distribution) vxReleaseDistribution(&m_distribution);
throw - 1;
}
m_vxObjRef = (vx_reference)m_distribution;
// io initialize
return InitializeIO(context, graph, m_vxObjRef, ioParams);
}
int CVxParamRemap::Initialize(vx_context context, vx_graph graph, const char * desc)
{
// get object parameters and create object
// syntax: remap:<srcWidth>,<srcHeight>,<dstWidth>,<dstHeight>[:<io-params>]
char objType[64];
const char * ioParams = ScanParameters(desc, "remap:<srcWidth>,<srcHeight>,<dstWidth>,<dstHeight>", "s:d,d,d,d", objType, &m_srcWidth, &m_srcHeight, &m_dstWidth, &m_dstHeight);
if (!_stricmp(objType, "remap")) {
m_remap = vxCreateRemap(context, m_srcWidth, m_srcHeight, m_dstWidth, m_dstHeight);
}
else ReportError("ERROR: invalid remap type: %s\n", objType);
vx_status ovxStatus = vxGetStatus((vx_reference)m_remap);
if (ovxStatus != VX_SUCCESS){
printf("ERROR: pyramid creation failed => %d (%s)\n", ovxStatus, ovxEnum2Name(ovxStatus));
if (m_remap) vxReleaseRemap(&m_remap);
throw - 1;
}
// io initialize
return InitializeIO(context, graph, (vx_reference)m_remap, ioParams);
}
int CVxParamMatrix::Initialize(vx_context context, vx_graph graph, const char * desc)
{
// get object parameters and create object
char objType[64], data_type[64];
const char * ioParams = ScanParameters(desc, "matrix:<data-type>,<columns>,<rows>", "s:s,D,D", objType, data_type, &m_columns, &m_rows);
if (!_stricmp(objType, "matrix")) {
m_data_type = ovxName2Enum(data_type);
m_matrix = vxCreateMatrix(context, m_data_type, m_columns, m_rows);
}
else ReportError("ERROR: unsupported matrix type: %s\n", desc);
vx_status ovxStatus = vxGetStatus((vx_reference)m_matrix);
if (ovxStatus != VX_SUCCESS){
printf("ERROR: matrix creation failed => %d (%s)\n", ovxStatus, ovxEnum2Name(ovxStatus));
if (m_matrix) vxReleaseMatrix(&m_matrix);
throw - 1;
}
m_vxObjRef = (vx_reference)m_matrix;
// io initialize
return InitializeIO(context, graph, m_vxObjRef, ioParams);
}
int CVxParamDistribution::InitializeIO(vx_context context, vx_graph graph, vx_reference ref, const char * io_params)
{
// save reference object and get object attributes
m_vxObjRef = ref;
m_distribution = (vx_distribution)m_vxObjRef;
ERROR_CHECK(vxQueryDistribution(m_distribution, VX_DISTRIBUTION_ATTRIBUTE_BINS, &m_numBins, sizeof(m_numBins)));
ERROR_CHECK(vxQueryDistribution(m_distribution, VX_DISTRIBUTION_ATTRIBUTE_OFFSET, &m_offset, sizeof(m_offset)));
ERROR_CHECK(vxQueryDistribution(m_distribution, VX_DISTRIBUTION_ATTRIBUTE_RANGE, &m_range, sizeof(m_range)));
// process I/O parameters
if (*io_params == ':') io_params++;
while (*io_params) {
char ioType[64], fileName[256];
io_params = ScanParameters(io_params, "<io-operation>,<parameter>", "s,S", ioType, fileName);
if (!_stricmp(ioType, "read"))
{ // read request syntax: read,<fileName>[,ascii|binary]
m_fileNameRead.assign(RootDirUpdated(fileName));
m_fileNameForReadHasIndex = (m_fileNameRead.find("%") != m_fileNameRead.npos) ? true : false;
m_readFileIsBinary = (m_fileNameRead.find(".txt") != m_fileNameRead.npos) ? false : true;
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",ascii|binary", ",s", option);
if (!_stricmp(option, "ascii")) {
m_readFileIsBinary = false;
}
else if (!_stricmp(option, "binary")) {
m_readFileIsBinary = true;
}
else ReportError("ERROR: invalid distribution read option: %s\n", option);
}
}
else if (!_stricmp(ioType, "write"))
{ // write request syntax: write,<fileName>[,ascii|binary]
m_fileNameWrite.assign(RootDirUpdated(fileName));
m_writeFileIsBinary = (m_fileNameWrite.find(".txt") != m_fileNameWrite.npos) ? false : true;
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",ascii|binary", ",s", option);
if (!_stricmp(option, "ascii")) {
m_writeFileIsBinary = false;
}
else if (!_stricmp(option, "binary")) {
m_writeFileIsBinary = true;
}
else ReportError("ERROR: invalid distribution write option: %s\n", option);
}
}
else if (!_stricmp(ioType, "compare"))
{ // compare syntax: compare,fileName[,ascii|binary]
m_fileNameCompare.assign(RootDirUpdated(fileName));
m_compareFileIsBinary = (m_fileNameCompare.find(".txt") != m_fileNameCompare.npos) ? false : true;
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",ascii|binary", ",s", option);
if (!_stricmp(option, "ascii")) {
m_compareFileIsBinary = false;
}
else if (!_stricmp(option, "binary")) {
m_compareFileIsBinary = true;
}
else ReportError("ERROR: invalid distribution compare option: %s\n", option);
}
}
else if (!_stricmp(ioType, "view")) {
m_displayName.assign(fileName);
m_paramList.push_back(this);
}
else ReportError("ERROR: invalid distribution operation: %s\n", ioType);
if (*io_params == ':') io_params++;
else if (*io_params) ReportError("ERROR: unexpected character sequence in parameter specification: %s\n", io_params);
}
return 0;
}
int CVxParamArray::InitializeIO(vx_context context, vx_graph graph, vx_reference ref, const char * io_params)
{
// save reference object and get object attributes
m_vxObjRef = ref;
m_array = (vx_array)m_vxObjRef;
ERROR_CHECK(vxQueryArray(m_array, VX_ARRAY_ATTRIBUTE_ITEMTYPE, &m_format, sizeof(m_format)));
ERROR_CHECK(vxQueryArray(m_array, VX_ARRAY_ATTRIBUTE_CAPACITY, &m_capacity, sizeof(m_capacity)));
ERROR_CHECK(vxQueryArray(m_array, VX_ARRAY_ATTRIBUTE_ITEMSIZE, &m_itemSize, sizeof(m_itemSize)));
// process I/O parameters
if (*io_params == ':') io_params++;
while (*io_params) {
char ioType[64], fileName[256];
io_params = ScanParameters(io_params, "<io-operation>,<parameter>", "s,S", ioType, fileName);
if (!_stricmp(ioType, "read"))
{ // read request syntax: read,<fileName>[,ascii|binary]
m_fileNameRead.assign(RootDirUpdated(fileName));
m_fileNameForReadHasIndex = (m_fileNameRead.find("%") != m_fileNameRead.npos) ? true : false;
m_readFileIsBinary = (m_fileNameRead.find(".txt") != m_fileNameRead.npos) ? false : true;
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",ascii|binary", ",s", option);
if (!_stricmp(option, "ascii")) {
m_readFileIsBinary = false;
}
else if (!_stricmp(option, "binary")) {
m_readFileIsBinary = true;
}
else ReportError("ERROR: invalid array read option: %s\n", option);
}
}
else if (!_stricmp(ioType, "write"))
{ // write request syntax: write,<fileName>[,ascii|binary]
m_fileNameWrite.assign(RootDirUpdated(fileName));
m_writeFileIsBinary = (m_fileNameWrite.find(".txt") != m_fileNameWrite.npos) ? false : true;
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",ascii|binary", ",s", option);
if (!_stricmp(option, "ascii")) {
m_writeFileIsBinary = false;
}
else if (!_stricmp(option, "binary")) {
m_writeFileIsBinary = true;
}
else ReportError("ERROR: invalid array write option: %s\n", option);
}
}
else if (!_stricmp(ioType, "compare"))
{ // compare syntax: compare,fileName[,ascii|binary][,err{<x>;<y>[;<strength>][;<%mismatch>]}][,log{<fileName>}]
m_fileNameCompareLog = "";
m_fileNameCompare.assign(RootDirUpdated(fileName));
m_compareFileIsBinary = (m_fileNameCompare.find(".txt") != m_fileNameCompare.npos) ? false : true;
while (*io_params == ',') {
char option[256];
io_params = ScanParameters(io_params, ",ascii|binary|err{<x>;<y>[;<strength>][;<%mismatch>]}|log{<fileName>}", ",S", option);
if (!_stricmp(option, "ascii")) {
m_compareFileIsBinary = false;
}
else if (!_stricmp(option, "binary")) {
m_compareFileIsBinary = true;
}
else if (!_strnicmp(option, "err{", 4)) {
if (m_format == VX_TYPE_KEYPOINT) {
const char * p = ScanParameters(&option[3], "{<x>;<y>;<strength>[;<%mismatch>]}", "{d;d;f", &m_errX, &m_errY, &m_errStrength);
if (*p == ';') {
ScanParameters(p, ";<%mismatch>}", ";f}", &m_errMismatchPercent);
}
}
else if (m_format == VX_TYPE_COORDINATES2D) {
const char * p = ScanParameters(&option[3], "{<x>;<y>[;<%mismatch>]}", "{d;d", &m_errX, &m_errY);
if (*p == ';') {
ScanParameters(p, ";<%mismatch>}", ";f}", &m_errMismatchPercent);
}
}
else ReportError("ERROR: array compare option not supported for this array: %s\n", option);
}
else if (!_strnicmp(option, "log{", 4)) {
option[strlen(option) - 1] = 0;
m_fileNameCompareLog.assign(RootDirUpdated(&option[4]));
}
else ReportError("ERROR: invalid array compare option: %s\n", option);
}
}
else if (!_stricmp(ioType, "view")) {
m_displayName.assign(fileName);
m_paramList.push_back(this);
}
else if (!_stricmp(ioType, "directive") && (!_stricmp(fileName, "VX_DIRECTIVE_AMD_COPY_TO_OPENCL") || !_stricmp(fileName, "sync-cl-write"))) {
m_useSyncOpenCLWriteDirective = true;
}
else if (!_stricmp(ioType, "init")) {
m_fileNameRead.assign(RootDirUpdated(fileName));
m_fileNameForReadHasIndex = (m_fileNameRead.find("%") != m_fileNameRead.npos) ? true : false;
m_readFileIsBinary = (m_fileNameRead.find(".txt") != m_fileNameRead.npos) ? false : true;
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",ascii|binary", ",s", option);
if (!_stricmp(option, "ascii")) {
m_readFileIsBinary = false;
}
else if (!_stricmp(option, "binary")) {
m_readFileIsBinary = true;
}
else ReportError("ERROR: invalid array init option: %s\n", option);
}
if (ReadFrame(0) < 0)
ReportError("ERROR: reading from input file for array init\n");
}
else ReportError("ERROR: invalid array operation: %s\n", ioType);
if (*io_params == ':') io_params++;
else if (*io_params) ReportError("ERROR: unexpected character sequence in parameter specification: %s\n", io_params);
}
return 0;
}
int CVxParamRemap::InitializeIO(vx_context context, vx_graph graph, vx_reference ref, const char * io_params)
{
// save reference object and get object attributes
m_vxObjRef = ref;
m_remap = (vx_remap)m_vxObjRef;
ERROR_CHECK(vxQueryRemap(m_remap, VX_REMAP_ATTRIBUTE_SOURCE_WIDTH, &m_srcWidth, sizeof(m_srcWidth)));
ERROR_CHECK(vxQueryRemap(m_remap, VX_REMAP_ATTRIBUTE_SOURCE_HEIGHT, &m_srcHeight, sizeof(m_srcHeight)));
ERROR_CHECK(vxQueryRemap(m_remap, VX_REMAP_ATTRIBUTE_DESTINATION_WIDTH, &m_dstWidth, sizeof(m_dstWidth)));
ERROR_CHECK(vxQueryRemap(m_remap, VX_REMAP_ATTRIBUTE_DESTINATION_HEIGHT, &m_dstHeight, sizeof(m_dstHeight)));
// process I/O parameters
if (*io_params == ':') io_params++;
while (*io_params) {
char ioType[64], fileName[256];
io_params = ScanParameters(io_params, "<io-operation>,<parameter>", "s,S", ioType, fileName);
if (!_stricmp(ioType, "read")) {
m_fileNameRead.assign(RootDirUpdated(fileName));
m_usingMultiFrameCapture = (m_fileNameRead.find("%") != std::string::npos) ? true : false;
m_readFileIsBinary = (m_fileNameRead.find(".txt") != m_fileNameRead.npos) ? false : true;
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",ascii|binary", ",s", option);
if (!_stricmp(option, "ascii")) {
m_readFileIsBinary = false;
}
else if (!_stricmp(option, "binary")) {
m_readFileIsBinary = true;
}
else ReportError("ERROR: invalid remap read option: %s\n", option);
}
}
else if (!_stricmp(ioType, "write")) {
m_fileNameWrite.assign(RootDirUpdated(fileName));
}
else if (!_stricmp(ioType, "compare"))
{ // compare request syntax: compare,<fileName>[,err{<x>;<y>}]
m_fileNameCompare.assign(RootDirUpdated(fileName));
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",err{<x>;<y>}", ",s", option);
if (!_strnicmp(option, "err{", 4)) {
ScanParameters(&option[3], "{<errX>;<errY>}", "{f;f}", &m_xyErr[0], &m_xyErr[1]);
}
else ReportError("ERROR: invalid remap compare option: %s\n", option);
}
}
else if (!_stricmp(ioType, "directive") && (!_stricmp(fileName, "VX_DIRECTIVE_AMD_COPY_TO_OPENCL") || !_stricmp(fileName, "sync-cl-write"))) {
m_useSyncOpenCLWriteDirective = true;
}
else if (!_stricmp(ioType, "init")) {
const char * patternName = fileName;
if (!_stricmp(patternName, "same")) {
for (vx_uint32 y = 0; y < m_dstHeight; y++){
for (vx_uint32 x = 0; x < m_dstWidth; x++){
vx_float32 sx = (vx_float32)x, sy = (vx_float32)y;
vx_status status = vxSetRemapPoint(m_remap, x, y, sx, sy);
if (status) {
printf("ERROR: vxSetRemapPoint(*,%d,%d,%g,%g) failed, status = %d\n", x, y, sx, sy, status);
return -1;
}
}
}
}
else if (!_stricmp(patternName, "rotate-90")) {
for (vx_uint32 y = 0; y < m_dstHeight; y++){
for (vx_uint32 x = 0; x < m_dstWidth; x++){
vx_float32 sx = (vx_float32)m_dstHeight - 1 - y, sy = (vx_float32)x;
vx_status status = vxSetRemapPoint(m_remap, x, y, sx, sy);
if (status) {
printf("ERROR: vxSetRemapPoint(*,%d,%d,%g,%g) failed, status = %d\n", x, y, sx, sy, status);
return -1;
}
}
}
}
else if (!_stricmp(patternName, "rotate-180")) {
for (vx_uint32 y = 0; y < m_dstHeight; y++){
for (vx_uint32 x = 0; x < m_dstWidth; x++){
vx_float32 sx = (vx_float32)m_dstWidth - 1 - x, sy = (vx_float32)m_dstHeight - 1 - y;
vx_status status = vxSetRemapPoint(m_remap, x, y, sx, sy);
if (status) {
printf("ERROR: vxSetRemapPoint(*,%d,%d,%g,%g) failed, status = %d\n", x, y, sx, sy, status);
return -1;
}
}
}
}
else if (!_stricmp(patternName, "rotate-270")) {
for (vx_uint32 y = 0; y < m_dstHeight; y++){
for (vx_uint32 x = 0; x < m_dstWidth; x++){
vx_float32 sx = (vx_float32)y, sy = (vx_float32)m_dstWidth - 1 - x;
vx_status status = vxSetRemapPoint(m_remap, x, y, sx, sy);
if (status) {
printf("ERROR: vxSetRemapPoint(*,%d,%d,%g,%g) failed, status = %d\n", x, y, sx, sy, status);
return -1;
}
}
}
}
else if (!_stricmp(patternName, "scale")) {
for (vx_uint32 y = 0; y < m_dstHeight; y++){
for (vx_uint32 x = 0; x < m_dstWidth; x++){
vx_float32 sx = (x + 0.5f) * (vx_float32)m_srcWidth / (vx_float32)m_dstWidth - 0.5f;
vx_float32 sy = (y + 0.5f) * (vx_float32)m_srcHeight / (vx_float32)m_dstHeight - 0.5f;
vx_status status = vxSetRemapPoint(m_remap, x, y, sx, sy);
if (status) {
printf("ERROR: vxSetRemapPoint(*,%d,%d,%g,%g) failed, status = %d\n", x, y, sx, sy, status);
return -1;
}
}
}
}
else if (!_stricmp(patternName, "hflip")) {
for (vx_uint32 y = 0; y < m_dstHeight; y++){
for (vx_uint32 x = 0; x < m_dstWidth; x++){
vx_float32 sx = (vx_float32)m_dstWidth - 1 - x, sy = (vx_float32)y;
vx_status status = vxSetRemapPoint(m_remap, x, y, sx, sy);
if (status) {
printf("ERROR: vxSetRemapPoint(*,%d,%d,%g,%g) failed, status = %d\n", x, y, sx, sy, status);
return -1;
}
}
}
}
else if (!_stricmp(patternName, "vflip")) {
for (vx_uint32 y = 0; y < m_dstHeight; y++){
for (vx_uint32 x = 0; x < m_dstWidth; x++){
vx_float32 sx = (vx_float32)x, sy = (vx_float32)m_dstHeight - 1 - y;
vx_status status = vxSetRemapPoint(m_remap, x, y, sx, sy);
if (status) {
printf("ERROR: vxSetRemapPoint(*,%d,%d,%g,%g) failed, status = %d\n", x, y, sx, sy, status);
return -1;
}
}
}
}
else {
// initialize from binary file
FILE * fp = fopen(fileName, "rb");
if (!fp) ReportError("ERROR: CVxParamRemap::InitializeIO: unable to open: %s\n", fileName);
for (vx_uint32 y = 0; y < m_dstHeight; y++){
for (vx_uint32 x = 0; x < m_dstWidth; x++){
vx_float32 src_xy[2];
if (fread(src_xy, sizeof(src_xy), 1, fp) != 1)
ReportError("ERROR: detected EOF at (%d,%d) on remap input file: %s\n", x, y, fileName);
ERROR_CHECK(vxSetRemapPoint(m_remap, x, y, src_xy[0], src_xy[1]));
}
}
fclose(fp);
}
}
else {
printf("ERROR: invalid remap I/O operation: %s\n", ioType);
return -1;
}
if (*io_params == ':') io_params++;
else if (*io_params) ReportError("ERROR: unexpected character sequence in parameter specification: %s\n", io_params);
}
return 0;
}
int CVxParamTensor::Initialize(vx_context context, vx_graph graph, const char * desc)
{
// get object parameters and create object
const char * ioParams = desc;
if (!_strnicmp(desc, "tensor:", 7) || !_strnicmp(desc, "virtual-tensor:", 15)) {
bool isVirtual = false;
if (!_strnicmp(desc, "virtual-tensor:", 15)) {
isVirtual = true;
desc += 8;
}
char objType[64], data_type[64];
ioParams = ScanParameters(desc, "tensor:<num-of-dims>,{dims},<data-type>,<fixed-point-pos>", "s:D,L,s,d", objType, &m_num_of_dims, &m_num_of_dims, m_dims, data_type, &m_fixed_point_pos);
m_data_type = ovxName2Enum(data_type);
if (isVirtual) {
m_tensor = vxCreateVirtualTensor(graph, m_num_of_dims, m_dims, m_data_type, m_fixed_point_pos);
}
else {
m_tensor = vxCreateTensor(context, m_num_of_dims, m_dims, m_data_type, m_fixed_point_pos);
}
}
else if (!_strnicmp(desc, "tensor-from-roi:", 16)) {
char objType[64], masterName[64];
ioParams = ScanParameters(desc, "tensor-from-view:<tensor>,<view>", "s:s,D,L,L", objType, masterName, &m_num_of_dims, &m_num_of_dims, m_start, &m_num_of_dims, m_end);
auto itMaster = m_paramMap->find(masterName);
if (itMaster == m_paramMap->end())
ReportError("ERROR: tensor [%s] doesn't exist for %s\n", masterName, desc);
vx_tensor masterTensor = (vx_tensor)itMaster->second->GetVxObject();
m_tensor = vxCreateTensorFromView(masterTensor, m_num_of_dims, m_start, m_end);
}
else if (!_strnicmp(desc, "tensor-from-handle:", 19)) {
char objType[64], data_type[64], memory_type_str[64];
ioParams = ScanParameters(desc, "tensor-from-handle:<num-of-dims>,{dims},<data-type>,<fixed-point-pos>,{strides},<num-handles>,<memory-type>",
"s:D,L,s,d,L,D,s", objType, &m_num_of_dims, &m_num_of_dims, m_dims, data_type, &m_fixed_point_pos, &m_num_of_dims, m_stride, &m_num_handles, memory_type_str);
if(m_num_handles > MAX_BUFFER_HANDLES)
ReportError("ERROR: num-handles is out of range: " VX_FMT_SIZE " (must be less than %d)\n", m_num_handles, MAX_BUFFER_HANDLES);
m_data_type = ovxName2Enum(data_type);
vx_uint64 memory_type = 0;
if (GetScalarValueFromString(VX_TYPE_ENUM, memory_type_str, &memory_type) < 0)
ReportError("ERROR: invalid memory type enum: %s\n", memory_type_str);
m_memory_type = (vx_enum)memory_type;
memset(m_memory_handle, 0, sizeof(m_memory_handle));
if (m_memory_type == VX_MEMORY_TYPE_HOST) {
// allocate all handles on host
for (vx_size active_handle = 0; active_handle < m_num_handles; active_handle++) {
vx_size size = m_dims[m_num_of_dims-1] * m_stride[m_num_of_dims-1];
m_memory_handle[active_handle] = malloc(size);
if (!m_memory_handle[active_handle])
ReportError("ERROR: malloc(%d) failed\n", (int)size);
}
}
#if ENABLE_OPENCL
else if (m_memory_type == VX_MEMORY_TYPE_OPENCL) {
// allocate all handles on opencl
cl_context opencl_context = nullptr;
vx_status status = vxQueryContext(context, VX_CONTEXT_ATTRIBUTE_AMD_OPENCL_CONTEXT, &opencl_context, sizeof(opencl_context));
if (status)
ReportError("ERROR: vxQueryContext(*,VX_CONTEXT_ATTRIBUTE_AMD_OPENCL_CONTEXT,...) failed (%d)\n", status);
for (vx_size active_handle = 0; active_handle < m_num_handles; active_handle++) {
vx_size size = m_dims[m_num_of_dims-1] * m_stride[m_num_of_dims-1];
cl_int err = CL_SUCCESS;
m_memory_handle[active_handle] = clCreateBuffer(opencl_context, CL_MEM_READ_WRITE, size, NULL, &err);
if (!m_memory_handle[active_handle] || err)
ReportError("ERROR: clCreateBuffer(*,CL_MEM_READ_WRITE,%d,NULL,*) failed (%d)\n", (int)size, err);
}
}
#endif
else ReportError("ERROR: invalid memory-type enum: %s\n", memory_type_str);
m_active_handle = 0;
m_tensor = vxCreateTensorFromHandle(context, m_num_of_dims, m_dims, m_data_type, m_fixed_point_pos, m_stride, m_memory_handle[m_active_handle], m_memory_type);
}
else ReportError("ERROR: unsupported tensor type: %s\n", desc);
vx_status ovxStatus = vxGetStatus((vx_reference)m_tensor);
if (ovxStatus != VX_SUCCESS){
printf("ERROR: tensor creation failed => %d (%s)\n", ovxStatus, ovxEnum2Name(ovxStatus));
if (m_tensor) vxReleaseTensor(&m_tensor);
throw - 1;
}
m_vxObjRef = (vx_reference)m_tensor;
// io initialize
return InitializeIO(context, graph, m_vxObjRef, ioParams);
}
int CVxParamTensor::InitializeIO(vx_context context, vx_graph graph, vx_reference ref, const char * io_params)
{
// save reference object and get object attributes
m_vxObjRef = ref;
m_tensor = (vx_tensor)m_vxObjRef;
ERROR_CHECK(vxQueryTensor(m_tensor, VX_TENSOR_NUMBER_OF_DIMS, &m_num_of_dims, sizeof(m_num_of_dims)));
ERROR_CHECK(vxQueryTensor(m_tensor, VX_TENSOR_DIMS, &m_dims, sizeof(m_dims[0])*m_num_of_dims));
ERROR_CHECK(vxQueryTensor(m_tensor, VX_TENSOR_DATA_TYPE, &m_data_type, sizeof(m_data_type)));
ERROR_CHECK(vxQueryTensor(m_tensor, VX_TENSOR_FIXED_POINT_POSITION, &m_fixed_point_pos, sizeof(vx_uint8)));
if(m_data_type == VX_TYPE_UINT8 || m_data_type == VX_TYPE_INT8)
m_size = 1;
else if(m_data_type == VX_TYPE_UINT16 || m_data_type == VX_TYPE_INT16 || m_data_type == VX_TYPE_FLOAT16)
m_size = 2;
else
m_size = 4;
for (vx_uint32 i = 0; i < m_num_of_dims; i++) {
m_stride[i] = m_size;
m_size *= m_dims[i];
}
m_data = new vx_uint8[m_size];
if (!m_data) ReportError("ERROR: memory allocation failed for tensor: %u\n", (vx_uint32)m_size);
// process I/O parameters
if (*io_params == ':') io_params++;
while (*io_params) {
char ioType[64], fileName[256];
io_params = ScanParameters(io_params, "<io-operation>,<parameter>", "s,S", ioType, fileName);
if (!_stricmp(ioType, "read"))
{ // read request syntax: read,<fileName>[,ascii|binary]
m_fileNameRead.assign(RootDirUpdated(fileName));
m_fileNameForReadHasIndex = (m_fileNameRead.find("%") != m_fileNameRead.npos) ? true : false;
m_readFileIsBinary = true;
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",binary", ",s", option);
if (!_stricmp(option, "binary")) {
m_readFileIsBinary = true;
}
else ReportError("ERROR: invalid tensor read option: %s\n", option);
}
}
else if (!_stricmp(ioType, "init"))
{ // init request syntax: init,<fileName>
if(!_strnicmp(fileName, "@fill~f32~", 10)) {
float value = (float)atof(&fileName[10]);
float * buf = (float *)m_data;
for(size_t i = 0; i < m_size/4; i++)
buf[i] = value;
}
else if(!_strnicmp(fileName, "@fill~i32~", 10)) {
vx_int32 value = atoi(&fileName[10]);
vx_int32 * buf = (vx_int32 *)m_data;
for(size_t i = 0; i < m_size/4; i++)
buf[i] = value;
}
else if(!_strnicmp(fileName, "@fill~i16~", 10)) {
vx_int16 value = (vx_int16)atoi(&fileName[10]);
vx_int16 * buf = (vx_int16 *)m_data;
for(size_t i = 0; i < m_size/2; i++)
buf[i] = value;
}
else if(!_strnicmp(fileName, "@fill~u8~", 9)) {
int value = atoi(&fileName[9]);
memset(m_data, value, m_size);
}
else {
int count = 1;
const char * tensorFileName = fileName;
if(!_strnicmp(tensorFileName, "@repeat~", 8)) {
tensorFileName += 8;
for(count = 0; *tensorFileName >= '0' && *tensorFileName <= '9'; tensorFileName++) {
count = count * 10 + *tensorFileName - '0';
}
if(*tensorFileName++ != '~' || count < 1)
ReportError("ERROR: invalid init @repeat~<n>~fileName syntax -- %s\n", fileName);
if((m_size % count) != 0)
ReportError("ERROR: file size is not multiple of tensor size -- %s\n", fileName);
}
if(!_stricmp(fileName + strlen(fileName) - 4, ".dat")) {
ReportError("ERROR: read from .dat files not supported: %s\n", fileName);
}
FILE * fp = fopen(RootDirUpdated(tensorFileName), "rb");
if (!fp) {
ReportError("ERROR: Unable to open: %s\n", tensorFileName);
}
vx_size size = m_size / count;
if (fread(m_data, 1, size, fp) != size)
ReportError("ERROR: not enough data (%d bytes) in %s\n", (vx_uint32)size, tensorFileName);
for(int i = 1; i < count; i++) {
memcpy(m_data + i * size, m_data, size);
}
vx_status status = vxCopyTensorPatch(m_tensor, m_num_of_dims, nullptr, nullptr, m_stride, m_data, VX_WRITE_ONLY, VX_MEMORY_TYPE_HOST);
fclose(fp);
if (status != VX_SUCCESS)
ReportError("ERROR: vxCopyTensorPatch: write failed (%d)\n", status);
}
}
else if (!_stricmp(ioType, "write"))
{ // write request syntax: write,<fileName>[,ascii|binary]
m_fileNameWrite.assign(RootDirUpdated(fileName));
m_writeFileIsBinary = true;
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",binary", ",s", option);
if (!_stricmp(option, "binary")) {
m_writeFileIsBinary = true;
}
else ReportError("ERROR: invalid tensor write option: %s\n", option);
}
}
else if (!_stricmp(ioType, "compare"))
{ // write request syntax: compare,<fileName>[,ascii|binary]
m_fileNameCompare.assign(RootDirUpdated(fileName));
m_compareFileIsBinary = true;
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",binary|maxerr=<value>|avgerr=<value>", ",s", option);
if (!_stricmp(option, "binary")) {
m_compareFileIsBinary = true;
}
else if (!_strnicmp(option, "maxerr=", 7)) {
m_maxErrorLimit = (float)atof(&option[7]);
m_avgErrorLimit = 1e20f;
}
else if (!_strnicmp(option, "avgerr=", 7)) {
m_avgErrorLimit = (float)atof(&option[7]);
m_maxErrorLimit = 1e20f;
}
else ReportError("ERROR: invalid tensor compare option: %s\n", option);
}
}
else if (!_stricmp(ioType, "directive") && (!_stricmp(fileName, "VX_DIRECTIVE_AMD_COPY_TO_OPENCL") || !_stricmp(fileName, "sync-cl-write"))) {
m_useSyncOpenCLWriteDirective = true;
}
else ReportError("ERROR: invalid tensor operation: %s\n", ioType);
if (*io_params == ':') io_params++;
else if (*io_params) ReportError("ERROR: unexpected character sequence in parameter specification: %s\n", io_params);
}
return 0;
}
int CVxParamMatrix::InitializeIO(vx_context context, vx_graph graph, vx_reference ref, const char * io_params)
{
// save reference object and get object attributes
m_vxObjRef = ref;
m_matrix = (vx_matrix)m_vxObjRef;
ERROR_CHECK(vxQueryMatrix(m_matrix, VX_MATRIX_ATTRIBUTE_TYPE, &m_data_type, sizeof(m_data_type)));
ERROR_CHECK(vxQueryMatrix(m_matrix, VX_MATRIX_ATTRIBUTE_COLUMNS, &m_columns, sizeof(m_columns)));
ERROR_CHECK(vxQueryMatrix(m_matrix, VX_MATRIX_ATTRIBUTE_ROWS, &m_rows, sizeof(m_rows)));
ERROR_CHECK(vxQueryMatrix(m_matrix, VX_MATRIX_ATTRIBUTE_SIZE, &m_size, sizeof(m_size)));
// process I/O parameters
if (*io_params == ':') io_params++;
while (*io_params) {
char ioType[64], fileName[256];
io_params = ScanParameters(io_params, "<io-operation>,<parameter>", "s,S", ioType, fileName);
if (!_stricmp(ioType, "read"))
{ // read request syntax: read,<fileName>[,ascii|binary]
m_fileNameRead.assign(RootDirUpdated(fileName));
m_fileNameForReadHasIndex = (m_fileNameRead.find("%") != m_fileNameRead.npos) ? true : false;
m_readFileIsBinary = (m_fileNameRead.find(".txt") != m_fileNameRead.npos) ? false : true;
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",ascii|binary", ",s", option);
if (!_stricmp(option, "ascii")) {
m_readFileIsBinary = false;
}
else if (!_stricmp(option, "binary")) {
m_readFileIsBinary = true;
}
else ReportError("ERROR: invalid matrix read option: %s\n", option);
}
}
else if (!_stricmp(ioType, "write"))
{ // write request syntax: write,<fileName>[,ascii|binary]
m_fileNameWrite.assign(RootDirUpdated(fileName));
m_writeFileIsBinary = (m_fileNameWrite.find(".txt") != m_fileNameWrite.npos) ? false : true;
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",ascii|binary", ",s", option);
if (!_stricmp(option, "ascii")) {
m_writeFileIsBinary = false;
}
else if (!_stricmp(option, "binary")) {
m_writeFileIsBinary = true;
}
else ReportError("ERROR: invalid matrix write option: %s\n", option);
}
}
else if (!_stricmp(ioType, "compare"))
{ // compare request syntax: compare,<fileName>[,ascii|binary][,err{<tolerance>}]
m_fileNameCompare.assign(RootDirUpdated(fileName));
m_compareFileIsBinary = (m_fileNameCompare.find(".txt") != m_fileNameCompare.npos) ? false : true;
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",ascii|binary|err{<tolerance>}", ",s", option);
if (!_stricmp(option, "ascii")) {
m_compareFileIsBinary = false;
}
else if (!_stricmp(option, "binary")) {
m_compareFileIsBinary = true;
}
else if (!_strnicmp(option, "err{", 4)) {
ScanParameters(&option[3], "{<tolerance>}", "{f}", &m_errTolerance);
}
else ReportError("ERROR: invalid matrix compare option: %s\n", option);
}
}
else if (!_stricmp(ioType, "init"))
{ // write request syntax: init,{<value1>;<value2>;...<valueN>}
NULLPTR_CHECK(m_bufForAccess = new vx_uint8[m_size]);
vx_size index = 0; char fmt[3] = { '{', (m_data_type == VX_TYPE_FLOAT32) ? 'f' : 'd', 0 };
for (const char * s = fileName; *s && index < (m_columns * m_rows); fmt[0] = ';', index++) {
if (m_data_type == VX_TYPE_INT32 || m_data_type == VX_TYPE_UINT8) {
vx_uint32 value;
s = ScanParameters(s, "<value>", fmt, &value);
if (m_data_type == VX_TYPE_UINT8) ((vx_uint8 *)m_bufForAccess)[index] = (vx_uint8)value;
else ((vx_int32 *)m_bufForAccess)[index] = value;
}
else if (m_data_type == VX_TYPE_FLOAT32) {
s = ScanParameters(s, "<value>", fmt, &((vx_float32 *)m_bufForAccess)[index]);
}
else ReportError("ERROR: matrix init option not support for data_type of %s\n", GetVxObjectName());
}
if (index < (m_columns * m_rows)) ReportError("ERROR: matrix init have too few values: %s\n", fileName);
ERROR_CHECK(vxWriteMatrix(m_matrix, m_bufForAccess));
}
else if (!_stricmp(ioType, "directive") && !_stricmp(fileName, "readonly")) {
ERROR_CHECK(vxDirective((vx_reference)m_matrix, VX_DIRECTIVE_AMD_READ_ONLY));
}
else if (!_stricmp(ioType, "ui") && !_strnicmp(fileName, "f", 1) && m_data_type == VX_TYPE_FLOAT32 && m_columns == 3 && m_rows == 3) {
int id = 0;
float valueR = 200.0f, valueInc = 0.5f;
if (sscanf(&fileName[1], "%d,%g,%g", &id, &valueR, &valueInc) != 3) {
printf("ERROR: invalid matrix UI configuration '%s'\n", fileName);
return -1;
}
id--;
GuiTrackBarInitializeMatrix((vx_reference)m_matrix, id, valueR, valueInc);
GuiTrackBarProcessKey(0); // just initialize the matrix
}
else ReportError("ERROR: invalid matrix operation: %s\n", ioType);
if (*io_params == ':') io_params++;
else if (*io_params) ReportError("ERROR: unexpected character sequence in parameter specification: %s\n", io_params);
}
return 0;
}
void Node::onInit()
{
// map_size parameter
XmlRpc::XmlRpcValue p_map_size;
if (getPrivateNodeHandle().getParam("map_size", p_map_size)) {
Size &map_size = parameters_("map_size", Size(0, 0, 0));
if (p_map_size.getType() == XmlRpc::XmlRpcValue::TypeInt) {
map_size.x() = map_size.y() = static_cast<int>(p_map_size);
} else if (p_map_size.getType() == XmlRpc::XmlRpcValue::TypeArray) {
if (p_map_size.size() >= 2) {
map_size.x() = static_cast<int>(p_map_size[0]);
map_size.y() = static_cast<int>(p_map_size[1]);
}
if (p_map_size.size() >= 3) {
map_size.z() = static_cast<int>(p_map_size[2]);
}
}
}
// map_resolution parameter
XmlRpc::XmlRpcValue p_map_resolution;
if (getPrivateNodeHandle().getParam("map_resolution", p_map_resolution)) {
Resolution &resolution = parameters_("map_resolution", Resolution(0.0, 0.0, 0.0));
if (p_map_resolution.getType() == XmlRpc::XmlRpcValue::TypeDouble) {
resolution.x() = resolution.y() = static_cast<double>(p_map_resolution);
} else if (p_map_resolution.getType() == XmlRpc::XmlRpcValue::TypeArray) {
if (p_map_resolution.size() >= 2) {
resolution.x() = static_cast<double>(p_map_resolution[0]);
resolution.y() = static_cast<double>(p_map_resolution[1]);
}
if (p_map_resolution.size() >= 3) {
resolution.z() = static_cast<double>(p_map_resolution[2]);
}
}
}
// map_offset parameter
XmlRpc::XmlRpcValue p_map_offset;
if (getPrivateNodeHandle().getParam("map_offset", p_map_offset)) {
Point &offset = parameters_("map_offset", Point(0.0, 0.0, 0.0));
if (p_map_offset.getType() == XmlRpc::XmlRpcValue::TypeArray) {
if (p_map_offset.size() >= 2) {
offset.x() = static_cast<double>(p_map_offset[0]);
offset.y() = static_cast<double>(p_map_offset[1]);
}
if (p_map_offset.size() >= 3) {
offset.z() = static_cast<double>(p_map_offset[2]);
}
}
}
// map_type parameter
std::string p_map_type = "OccupancyGridMap2D";
getPrivateNodeHandle().getParam("map_type", p_map_type);
map_ = MapFactory(parameters_).create<OccupancyGridMapBase>(p_map_type);
if (!map_) {
ROS_FATAL("Unknown map type: %s.\n\nAvailable map types:\n%s", p_map_type.c_str(), MapFactory::getMapTypes().c_str());
ros::shutdown();
return;
}
// scan matcher parameters
ScanMatcherParameters &matcher_parameters = parameters_("matcher", ScanMatcherParameters());
getPrivateNodeHandle().getParam("match_level_minimum", matcher_parameters.match_level_minimum());
getPrivateNodeHandle().getParam("match_level_maximum", matcher_parameters.match_level_maximum());
getPrivateNodeHandle().getParam("occupied_space_residual_weight", matcher_parameters.occupied_space_residual_weight());
getPrivateNodeHandle().getParam("free_space_residual_weight", matcher_parameters.free_space_residual_weight());
getPrivateNodeHandle().getParam("motion_residual_weight", matcher_parameters.motion_residual_weight());
getPrivateNodeHandle().getParam("function_tolerance", matcher_parameters.function_tolerance());
getPrivateNodeHandle().getParam("gradient_tolerance", matcher_parameters.gradient_tolerance());
getPrivateNodeHandle().getParam("parameter_tolerance", matcher_parameters.parameter_tolerance());
getPrivateNodeHandle().getParam("max_num_iterations", matcher_parameters.max_num_iterations());
getPrivateNodeHandle().getParam("max_solver_time_in_seconds", matcher_parameters.max_solver_time_in_seconds());
// get occupancy parameters
OccupancyParameters &occupancy_parameters = parameters_("occupancy", OccupancyParameters::Default());
double p_update_factor_free, p_update_factor_occupied;
// private_nh_.param("update_factor_free", p_update_factor_free_, 0.4);
// private_nh_.param("update_factor_occupied", p_update_factor_occupied_, 0.9);
if (getPrivateNodeHandle().getParam("update_factor_free", p_update_factor_free))
occupancy_parameters.step_free() = occupancy_parameters.getOccupancy(p_update_factor_free);
if (getPrivateNodeHandle().getParam("update_factor_occupied", p_update_factor_occupied))
occupancy_parameters.step_occupied() = occupancy_parameters.getOccupancy(p_update_factor_occupied);
// get scan parameters
ScanParameters &scan_parameters = parameters_("scan", ScanParameters());
getPrivateNodeHandle().getParam("laser_min_dist", scan_parameters.min_distance());
getPrivateNodeHandle().getParam("laser_max_dist", scan_parameters.max_distance());
getPrivateNodeHandle().getParam("laser_z_min_value", scan_parameters.min_z());
getPrivateNodeHandle().getParam("laser_z_max_value", scan_parameters.max_z());
// get other parameters
getPrivateNodeHandle().getParam("map_frame", p_map_frame_);
getPrivateNodeHandle().getParam("base_frame", p_base_frame_);
getPrivateNodeHandle().getParam("odom_frame", p_odom_frame_);
getPrivateNodeHandle().getParam("use_tf_scan_transformation", p_use_tf_scan_transformation_);
getPrivateNodeHandle().getParam("use_tf_pose_start_estimate", p_use_tf_pose_start_estimate_);
getPrivateNodeHandle().getParam("pub_map_odom_transform", p_pub_map_odom_transform_);
getPrivateNodeHandle().getParam("advertise_map_service", p_advertise_map_service_);
getPrivateNodeHandle().getParam("map_update_distance_thresh", p_map_update_translational_threshold_);
getPrivateNodeHandle().getParam("map_update_angle_thresh", p_map_update_angular_threshold_);
// private_nh_.param("pub_drawings", p_pub_drawings, false);
// private_nh_.param("pub_debug_output", p_pub_debug_output_, false);
// private_nh_.param("pub_map_odom_transform", p_pub_map_odom_transform_,true);
// private_nh_.param("pub_odometry", p_pub_odometry_,false);
// private_nh_.param("advertise_map_service", p_advertise_map_service_,true);
// private_nh_.param("scan_subscriber_queue_size", p_scan_subscriber_queue_size_, 5);
// private_nh_.param("map_resolution", p_map_resolution_, 0.025);
// private_nh_.param("map_size", p_map_size_, 1024);
// private_nh_.param("map_start_x", p_map_start_x_, 0.5);
// private_nh_.param("map_start_y", p_map_start_y_, 0.5);
// private_nh_.param("map_multi_res_levels", p_map_multi_res_levels_, 3);
// private_nh_.param("update_factor_free", p_update_factor_free_, 0.4);
// private_nh_.param("update_factor_occupied", p_update_factor_occupied_, 0.9);
// private_nh_.param("map_update_distance_thresh", p_map_update_distance_threshold_, 0.4);
// private_nh_.param("map_update_angle_thresh", p_map_update_angle_threshold_, 0.9);
// private_nh_.param("scan_topic", p_scan_topic_, std::string("scan"));
// private_nh_.param("sys_msg_topic", p_sys_msg_topic_, std::string("syscommand"));
// private_nh_.param("pose_update_topic", p_pose_update_topic_, std::string("poseupdate"));
// private_nh_.param("use_tf_scan_transformation", p_use_tf_scan_transformation_,true);
// private_nh_.param("use_tf_pose_start_estimate", p_use_tf_pose_start_estimate_,false);
// private_nh_.param("map_with_known_poses", p_map_with_known_poses_, false);
// private_nh_.param("base_frame", p_base_frame_, std::string("base_link"));
// private_nh_.param("map_frame", p_map_frame_, std::string("map"));
// private_nh_.param("odom_frame", p_odom_frame_, std::string("odom"));
// private_nh_.param("pub_map_scanmatch_transform", p_pub_map_scanmatch_transform_,true);
// private_nh_.param("tf_map_scanmatch_transform_frame_name", p_tf_map_scanmatch_transform_frame_name_, std::string("scanmatcher_frame"));
// private_nh_.param("output_timing", p_timing_output_,false);
// private_nh_.param("map_pub_period", p_map_pub_period_, 2.0);
// double tmp = 0.0;
// private_nh_.param("laser_min_dist", tmp, 0.4);
// p_sqr_laser_min_dist_ = static_cast<float>(tmp*tmp);
// private_nh_.param("laser_max_dist", tmp, 30.0);
// p_sqr_laser_max_dist_ = static_cast<float>(tmp*tmp);
// private_nh_.param("laser_z_min_value", tmp, -1.0);
// p_laser_z_min_value_ = static_cast<float>(tmp);
// private_nh_.param("laser_z_max_value", tmp, 1.0);
// p_laser_z_max_value_ = static_cast<float>(tmp);
// initialize scan and scan matcher
if (p_use_tf_scan_transformation_) scan_.setTransformer(getTransformListener(), p_base_frame_);
matcher_ = ScanMatcher::Factory(parameters_);
// subscribe scan
scan_subscriber_ = getNodeHandle().subscribe<sensor_msgs::LaserScan>("scan", 10, &Node::scanCallback, this);
cloud_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>("point_cloud", 10, &Node::cloudCallback, this);
// initial pose subscriber
initial_pose_subscriber_ = getNodeHandle().subscribe<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 10, &Node::initialPoseCallback, this);
// static map subscriber
static_map_subscriber_ = getNodeHandle().subscribe<nav_msgs::OccupancyGrid>("static_map", 10, &Node::staticMapCallback, this);
// subscribe syscommand (reset)
syscommand_subscriber_ = getNodeHandle().subscribe<std_msgs::String>("syscommand", 10, &Node::syscommandCallback, this);
// advertise map
map_publisher_ = getNodeHandle().advertise<nav_msgs::OccupancyGrid>("map", 1);
map_metadata_publisher_ = getNodeHandle().advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
ROS_INFO("Advertised map as %s", map_publisher_.getTopic().c_str());
// advertise map service
if (p_advertise_map_service_) {
map_service_ = getNodeHandle().advertiseService("map", &Node::mapServiceCallback, this);
}
// advertise pose
pose_with_covariance_publisher_ = getNodeHandle().advertise<geometry_msgs::PoseWithCovarianceStamped>("poseupdate", 1);
pose_publisher_ = getPrivateNodeHandle().advertise<geometry_msgs::PoseStamped>("pose", 1);
covariance_publisher_ = getPrivateNodeHandle().advertise<visualization_msgs::Marker>("covariance", 1);
// advertise tf
if (p_pub_map_odom_transform_) {
getTransformListener();
getTransformBroadcaster();
}
// advertise scan cloud
bool p_publish_scan_cloud = true;
getPrivateNodeHandle().getParam("publish_scan_cloud", p_publish_scan_cloud);
if (p_publish_scan_cloud) scan_.advertisePointCloud(getPrivateNodeHandle());
// setup map publish thread
double p_map_publish_period = 1.0;
getPrivateNodeHandle().getParam("map_publish_period", p_map_publish_period);
if (p_map_publish_period > 0.0) {
map_publish_thread_ = boost::thread(boost::bind(&Node::mapPublishThread, this, ros::Rate(ros::Duration(p_map_publish_period))));
}
// advertise timing information
#ifdef USE_HECTOR_TIMING
timing_publisher_ = getPrivateNodeHandle().advertise<hector_diagnostics::TimingInfo>("timing", 1);
#endif
// reset
reset();
}
