void LocalizationPlugin::configure(tue::Configuration config)
{
std::string laser_topic;
config.value("laser_topic", laser_topic);
if (config.hasError())
return;
ros::NodeHandle nh;
// Subscribe to laser topic
ros::SubscribeOptions sub_options =
ros::SubscribeOptions::create<sensor_msgs::LaserScan>(
laser_topic, 1, boost::bind(&LocalizationPlugin::laserCallback, this, _1), ros::VoidPtr(), &cb_queue_);
sub_laser_ = nh.subscribe(sub_options);
a_current_ = 0;
laser_pose_ = geo::Pose3D::identity();
laser_pose_.t.z = 0.3;
}
void FaceDetector::configureClassification(tue::Configuration config)
{
// default values in case configure(...) is not called!
debug_mode_ = false;
classifier_front_scale_factor_ = 1.2;
classifier_front_min_neighbours_ = 3;
classif_front_min_size_ = cv::Size(20,20);
classifier_profile_scale_factor_= 1.2;
classifier_profile_min_neighbours_ = 3;
classif_profile_min_size_ = cv::Size(20,20);
debug_folder_ = "/tmp/face_detector/";
// load training files for frontal classifier
std::string cascade_front_files_path_;
if (config.value("cascade_front_files_path", cascade_front_files_path_))
{
if (!classifier_front_.load(cascade_front_files_path_))
config.addError("Unable to load front haar cascade files (" + cascade_front_files_path_ + ")");
}
// load training files for profile classifier
std::string cascade_profile_files_path_;
if (config.value("cascade_profile_front_path", cascade_profile_files_path_))
{
if (!classifier_profile_.load(cascade_profile_files_path_))
config.addError("Unable to load profile haar cascade files (" + cascade_profile_files_path_ + ")");
}
if (config.hasError())
return;
if (!config.value("debug_mode", debug_mode_, tue::OPTIONAL))
ed::log::info() << "Parameter 'debug_mode' not found. Using default: " << debug_mode_ << std::endl;
if (!config.value("debug_folder", debug_folder_, tue::OPTIONAL))
ed::log::info() << "Parameter 'debug_folder' not found. Using default: " << debug_folder_ << std::endl;
if (!config.value("classifier_front_scale_factor", classifier_front_scale_factor_, tue::OPTIONAL))
ed::log::info() << "Parameter 'classifier_front_scale_factor' not found. Using default: " << classifier_front_scale_factor_ << std::endl;
if (!config.value("classifier_front_min_neighbours", classifier_front_min_neighbours_, tue::OPTIONAL))
ed::log::info() << "Parameter 'classifier_front_min_neighbours' not found. Using default: " << classifier_front_min_neighbours_ << std::endl;
if (!config.value("classifier_profile_scale_factor", classifier_profile_scale_factor_, tue::OPTIONAL))
ed::log::info() << "Parameter 'classifier_profile_scale_factor' not found. Using default: " << classifier_profile_scale_factor_ << std::endl;
if (!config.value("classifier_profile_min_neighbours", classifier_profile_min_neighbours_, tue::OPTIONAL))
ed::log::info() << "Parameter 'classifier_profile_min_neighbours' not found. Using default: " << classifier_profile_min_neighbours_ << std::endl;
if (debug_mode_)
{
// clean the debug folder if debugging is active
try {
boost::filesystem::path dir(debug_folder_);
boost::filesystem::remove_all(dir);
boost::filesystem::create_directories(dir);
}
catch(const boost::filesystem::filesystem_error& e)
{
if(e.code() == boost::system::errc::permission_denied)
ed::log::error() << "boost::filesystem permission denied" << std::endl;
else
ed::log::error() << "boost::filesystem failed with error: " << e.code().message() << std::endl;
}
// create debug window
cv::namedWindow("Face Detector Output", CV_WINDOW_AUTOSIZE);
}
}
void GUIPlugin::configure(tue::Configuration config)
{
std::string srv_get_measurements, srv_set_label, srv_raise_event, srv_get_command, map_image_topic;
config.value("srv_get_measurements", srv_get_measurements);
config.value("srv_set_label", srv_set_label);
config.value("srv_raise_event", srv_raise_event);
config.value("srv_get_command", srv_get_command);
config.value("map_image_topic", map_image_topic);
// TODO: make configurable
trigger_map_publish_ = ed::EventClock(10);
int image_width, image_height;
config.value("map_image_width", image_width);
config.value("map_image_height", image_height);
double world_width, world_height;
config.value("world_width", world_width);
config.value("world_height", world_height);
double cam_x, cam_y, cam_z;
config.value("cam_x", cam_x);
config.value("cam_y", cam_y);
config.value("cam_z", cam_z);
if (config.hasError())
return;
ros::NodeHandle nh;
if (srv_get_measurements_.getService() != srv_get_measurements)
{
ros::AdvertiseServiceOptions opt_get_measurements =
ros::AdvertiseServiceOptions::create<ed_msgs::GetMeasurements>(
srv_get_measurements, boost::bind(&GUIPlugin::srvGetMeasurements, this, _1, _2), ros::VoidPtr(), &cb_queue_);
srv_get_measurements_ = nh.advertiseService(opt_get_measurements);
}
if (srv_set_label_.getService() != srv_set_label)
{
ros::AdvertiseServiceOptions opt_set_label =
ros::AdvertiseServiceOptions::create<ed_msgs::SetLabel>(
srv_set_label, boost::bind(&GUIPlugin::srvSetLabel, this, _1, _2), ros::VoidPtr(), &cb_queue_);
srv_set_label_ = nh.advertiseService(opt_set_label);
}
if (srv_raise_event_.getService() != srv_raise_event)
{
ros::AdvertiseServiceOptions opt_raise_event =
ros::AdvertiseServiceOptions::create<ed_msgs::RaiseEvent>(
srv_raise_event, boost::bind(&GUIPlugin::srvRaiseEvent, this, _1, _2), ros::VoidPtr(), &cb_queue_);
srv_raise_event_ = nh.advertiseService(opt_raise_event);
}
if (srv_get_command_.getService() != srv_get_command)
{
ros::AdvertiseServiceOptions opt_get_command =
ros::AdvertiseServiceOptions::create<ed_msgs::GetGUICommand>(
srv_get_command, boost::bind(&GUIPlugin::srvGetCommand, this, _1, _2), ros::VoidPtr(), &cb_queue_);
srv_get_command_ = nh.advertiseService(opt_get_command);
}
if (pub_image_map_.getTopic() != map_image_topic)
pub_image_map_ = nh.advertise<tue_serialization::Binary>("/ed/gui/map_image", 1);
map_image_ = cv::Mat(image_width, image_height, CV_8UC3, cv::Scalar(50, 50, 50));
projector_pose_.setOrigin(geo::Vector3(cam_x, cam_y, cam_z));
projector_pose_.setBasis(geo::Matrix3::identity());
projector_.setFocalLengths(image_width / (world_width / cam_z),
image_height / (world_height / cam_z));
projector_.setOpticalCenter(image_width / 2, image_height / 2);
projector_.setOpticalTranslation(0, 0);
}