bool PartsBasedDetectorNode::init(void) { // attempt to load the model file and distribute the parameters string modelfile; nh_.getParam("model", modelfile); string ext = boost::filesystem::path(modelfile).extension().c_str(); // OpenCV FileStorageModel if (ext.compare(".xml") == 0 || ext.compare(".yaml") == 0) { FileStorageModel model; bool ok = model.deserialize(modelfile); if (!ok) { fprintf(stderr, "Error deserializing file\n"); return false; } pbd_.distributeModel(model); name_ = model.name(); } #ifdef WITH_MATLABIO // cvmatio MatlabIOModel else if (ext.compare(".mat") == 0) { MatlabIOModel model; bool ok = model.deserialize(modelfile); if (!ok) { fprintf(stderr, "Error deserializing file\n"); return false; } pbd_.distributeModel(model); name_ = model.name(); } #endif else { fprintf(stderr, "Unsupported model format: %s\n", ext.c_str()); return false; } // setup the detector publishers // register the callback for synchronised depth and camera images sync_.registerCallback( boost::bind(&PartsBasedDetectorNode::detectorCallback, this, _1, _2 ) ); info_sub_d_.registerCallback(&PartsBasedDetectorNode::depthCameraCallback, this); // initialise the publishers image_pub_d_ = it_.advertise(ns_ + name_ + "/depth_rect", 1); image_pub_rgb_ = it_.advertise(ns_ + name_ + "/candidates_rect_color", 1); mask_pub_ = it_.advertise(ns_ + name_ + "/mask", 1); bb_pub_ = nh_.advertise<MarkerArray>(ns_ + name_ + "/bounding_box", 1); // if we got here, everything is okay return true; }
bool PartsBasedDetectorNode::init(void) { // attempt to load the model file and distribute the parameters string modelfile; // private nodehandle ros::NodeHandle priv_nh("~"); priv_nh.getParam("model", modelfile); priv_nh.getParam("remove_planes", remove_planes_); string ext = boost::filesystem::path(modelfile).extension().c_str(); ROS_INFO("Loading model %s", modelfile.c_str()); // OpenCV FileStorageModel if (ext.compare(".xml") == 0 || ext.compare(".yaml") == 0) { FileStorageModel model; bool ok = model.deserialize(modelfile); if (!ok) { ROS_ERROR("Error deserializing file\n"); return false; } pbd_.distributeModel(model); name_ = model.name(); } #ifdef WITH_MATLABIO // cvmatio MatlabIOModel else if (ext.compare(".mat") == 0) { MatlabIOModel model; bool ok = model.deserialize(modelfile); if (!ok) { ROS_ERROR("Error deserializing file\n"); return false; } pbd_.distributeModel(model); name_ = model.name(); } #endif else { ROS_ERROR("Unsupported model format: %s\n", ext.c_str()); return false; } // setup the detector publishers // register the callback for synchronised depth and camera images sync_.registerCallback( boost::bind(&PartsBasedDetectorNode::detectorCallback, this, _1, _2, _3)); info_sub_d_.registerCallback(&PartsBasedDetectorNode::depthCameraCallback, this); // initialise the publishers // image_pub_d_ = it_.advertise(ns_ + name_ + "/depth_rect", 1); image_pub_rgb_ = it_.advertise(ns_ + name_ + "/candidates_rect_color", 1); mask_pub_ = it_.advertise(ns_ + name_ + "/mask", 1); // segmented_pub_ = it_.advertise(ns_ + name_ + "/segmented_image", 1); bb_pub_ = nh_.advertise<MarkerArray>(ns_ + name_ + "/bounding_box", 1); cloud_pub_ = nh_.advertise<PointCloud>(ns_ + name_ + "/cleaned_cloud", 1); part_center_pub_ = nh_.advertise<MarkerArray>(ns_ + name_ + "/part_centers", 1); object_pose_pub_ = nh_.advertise<PoseArray>(ns_ + name_ + "/object_poses", 1); ROS_INFO("Initialization successful"); // if we got here, everything is okay return true; }