/*! @brief configure the detector state
*
* The configure method is called once when the ECTO cell is launched,
* and is designed to initialise the detector state and load models,
* parameters, etc.
*
* @param params the parameters made available through the config file and
* python bindings
* @param inputs for initializing inputs, if necessary
* @param outputs for initializing outputs, if necessary
*/
void
configure(const tendrils& params, const tendrils& inputs, const tendrils& outputs) {
// create the model object and deserialize it
FileStorageModel model;
model.deserialize(*model_file_);
// create the visualizer
visualizer_.reset(new Visualize(model.name()));
// create the PartsBasedDetector and distribute the model parameters
detector_.reset(new PartsBasedDetector<float>);
detector_->distributeModel(model);
}
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;
}
/*! @brief configure the detector state
*
* The configure method is called once when the ECTO cell is launched,
* and is designed to initialise the detector state and load models,
* parameters, etc.
*
* @param params the parameters made available through the config file and
* python bindings
* @param inputs for initializing inputs, if necessary
* @param outputs for initializing outputs, if necessary
*/
void configure(const tendrils& params, const tendrils& inputs,
const tendrils& outputs)
{
std::cout << "MODEL: " << *model_file_ << std::endl;
// create the model object and deserialize it
FileStorageModel model;
model.deserialize(*model_file_);
// create the visualizer
visualizer_.reset(new Visualize(model.name()));
// create the PartsBasedDetector and distribute the model parameters
detector_.reset(new PartsBasedDetector<double>);
detector_->distributeModel(model);
// set the model_name
model_name_ = model.name();
}
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;
}
