void checkAndSetWhitebalance(CamData& cd, string param_name, dc1394feature_t feature)
{
string p = cd.name + string("/") + param_name;
string p_b = cd.name + string("/") + param_name + string("_b");
string p_r = cd.name + string("/") + param_name + string("_r");
if (has_param(p_b) && has_param(p_r))
{
XmlRpc::XmlRpcValue val;
XmlRpc::XmlRpcValue val_b;
XmlRpc::XmlRpcValue val_r;
get_param(p, val);
get_param(p_b, val_b);
get_param(p_r, val_r);
if (val.getType() == XmlRpc::XmlRpcValue::TypeString && val == string("auto"))
cd.cam->setFeatureMode(feature, DC1394_FEATURE_MODE_AUTO);
else if (val_b.getType() == XmlRpc::XmlRpcValue::TypeInt && val_r.getType() == XmlRpc::XmlRpcValue::TypeInt)
{
cd.cam->setFeatureMode(feature, DC1394_FEATURE_MODE_MANUAL);
cd.cam->setFeature(feature, (int)(val_b), (int)(val_r));
}
}
}
level_max_core(bool imax, level const & l1, level const & l2):
level_composite(imax ? level_kind::IMax : level_kind::Max,
hash(hash(l1), hash(l2)),
std::max(get_depth(l1), get_depth(l2)) + 1,
has_param(l1) || has_param(l2),
has_global(l1) || has_global(l2),
has_meta(l1) || has_meta(l2)),
m_lhs(l1), m_rhs(l2) {
lean_assert(!is_explicit(l1) || !is_explicit(l2));
}
void checkAndSetFeature(CamData& cd, string param_name, dc1394feature_t feature)
{
string p = cd.name + string("/") + param_name;
if (has_param(p))
{
XmlRpc::XmlRpcValue val;
get_param(p, val);
if (val.getType() == XmlRpc::XmlRpcValue::TypeString)
if (val == string("auto"))
cd.cam->setFeatureMode(feature, DC1394_FEATURE_MODE_AUTO);
else
cd.cam->setFeatureMode(feature, DC1394_FEATURE_MODE_MANUAL);
if (val.getType() == XmlRpc::XmlRpcValue::TypeInt)
{
cd.cam->setFeatureMode(feature, DC1394_FEATURE_MODE_MANUAL);
cd.cam->setFeature(feature, (int)(val));
}
if (val.getType() == XmlRpc::XmlRpcValue::TypeDouble)
{
cd.cam->setFeatureMode(feature, DC1394_FEATURE_MODE_MANUAL);
cd.cam->setFeatureAbsolute(feature, (double)(val));
}
}
}
void Camera::extract_focal_distance(
bool& autofocus_enabled,
Vector2d& autofocus_target,
double& focal_distance) const
{
const Vector2d DefaultAFTarget(0.5); // in NDC
const double DefaultFocalDistance = 1.0; // in meters
if (has_param("focal_distance"))
{
if (has_param("autofocus_target"))
{
RENDERER_LOG_WARNING(
"while defining camera \"%s\": autofocus is enabled; \"focal_distance\" parameter "
"will be ignored.",
get_path().c_str());
autofocus_enabled = true;
autofocus_target = m_params.get_required<Vector2d>("autofocus_target", DefaultAFTarget);
focal_distance = 0.0;
}
else
{
autofocus_enabled = false;
autofocus_target = DefaultAFTarget;
focal_distance = m_params.get_required<double>("focal_distance", DefaultFocalDistance);
}
}
else if (has_param("autofocus_target"))
{
autofocus_enabled = true;
autofocus_target = m_params.get_required<Vector2d>("autofocus_target", DefaultAFTarget);
focal_distance = 0.0;
}
else
{
RENDERER_LOG_ERROR(
"while defining camera \"%s\": no \"focal_distance\" or \"autofocus_target\" parameter found; "
"using default focal distance value \"%f\".",
get_path().c_str(),
DefaultFocalDistance);
autofocus_enabled = false;
autofocus_target = DefaultAFTarget;
focal_distance = DefaultFocalDistance;
}
}
void collect_univ_params_core(level const & l, name_set & r) {
for_each(l, [&](level const & l) {
if (!has_param(l))
return false;
if (is_param(l))
r.insert(param_id(l));
return true;
});
}
optional<name> get_undef_param(level const & l, level_param_names const & ps) {
optional<name> r;
for_each(l, [&](level const & l) {
if (!has_param(l) || r)
return false;
if (is_param(l) && std::find(ps.begin(), ps.end(), param_id(l)) == ps.end())
r = param_id(l);
return true;
});
return r;
}
double Camera::extract_focal_length(const double film_width) const
{
const double DefaultFocalLength = 0.035; // in meters
const double DefaultHFov = 54.0; // in degrees
if (has_param("focal_length"))
{
if (has_param("horizontal_fov"))
{
RENDERER_LOG_WARNING(
"while defining camera \"%s\": the parameter \"horizontal_fov\" "
"has precedence over \"focal_length\".",
get_path().c_str());
const double hfov = get_greater_than_zero("horizontal_fov", DefaultHFov);
return hfov_to_focal_length(film_width, hfov);
}
else
{
return get_greater_than_zero("focal_length", DefaultFocalLength);
}
}
else if (has_param("horizontal_fov"))
{
const double hfov = get_greater_than_zero("horizontal_fov", DefaultHFov);
return hfov_to_focal_length(film_width, hfov);
}
else
{
RENDERER_LOG_ERROR(
"while defining camera \"%s\": no \"horizontal_fov\" or \"focal_length\" parameter found; "
"using default focal length value \"%f\".",
get_path().c_str(),
DefaultFocalLength);
return DefaultFocalLength;
}
}
level instantiate(level const & l, level_param_names const & ps, levels const & ls) {
lean_assert(length(ps) == length(ls));
return replace(l, [=](level const & l) {
if (!has_param(l)) {
return some_level(l);
} else if (is_param(l)) {
name const & id = param_id(l);
list<name> const *it1 = &ps;
list<level> const * it2 = &ls;
while (!is_nil(*it1)) {
if (head(*it1) == id)
return some_level(head(*it2));
it1 = &tail(*it1);
it2 = &tail(*it2);
}
return some_level(l);
} else {
return none_level();
}
});
}
bool Camera::has_params(const char* name1, const char* name2) const
{
return has_param(name1) && has_param(name2);
}
expr_sort::expr_sort(level const & l, tag g):
expr_cell(expr_kind::Sort, ::lean::hash(l), false, has_meta(l), false, has_param(l), g),
m_level(l) {
}
expr_const::expr_const(name const & n, levels const & ls, tag g):
expr_cell(expr_kind::Constant, ::lean::hash(n.hash(), hash_levels(ls)), false,
has_meta(ls), false, has_param(ls), g),
m_name(n),
m_levels(ls) {
}
int
main (int argc,
char *const *argv)
{
int error_count = 0;
int res;
use_tls_tool = TLS_CLI_NO_TOOL;
test_tls = has_in_name(argv[0], "_tls");
if (has_param(argc, argv, "-v") || has_param(argc, argv, "--verbose"))
verbose = 1;
if (test_tls)
{
#ifdef HTTPS_SUPPORT
if (has_param(argc, argv, "--use-gnutls-cli"))
use_tls_tool = TLS_CLI_GNUTLS;
else if (has_param(argc, argv, "--use-openssl"))
use_tls_tool = TLS_CLI_OPENSSL;
else if (has_param(argc, argv, "--use-gnutls-lib"))
use_tls_tool = TLS_LIB_GNUTLS;
#if defined(HAVE_FORK) && defined(HAVE_WAITPID)
else if (0 == system ("gnutls-cli --version 1> /dev/null"))
use_tls_tool = TLS_CLI_GNUTLS;
else if (0 == system ("openssl version 1> /dev/null"))
use_tls_tool = TLS_CLI_OPENSSL;
#endif /* HAVE_FORK && HAVE_WAITPID */
else
use_tls_tool = TLS_LIB_GNUTLS; /* Should be available as MHD use it. */
if (verbose)
{
switch (use_tls_tool)
{
case TLS_CLI_GNUTLS:
printf ("GnuTLS-CLI will be used for testing.\n");
break;
case TLS_CLI_OPENSSL:
printf ("Command line version of OpenSSL will be used for testing.\n");
break;
case TLS_LIB_GNUTLS:
printf ("GnuTLS library will be used for testing.\n");
break;
default:
abort ();
}
}
if (TLS_LIB_GNUTLS == use_tls_tool && GNUTLS_E_SUCCESS != gnutls_global_init())
abort ();
#else /* ! HTTPS_SUPPORT */
fprintf (stderr, "HTTPS support was disabled by configure.\n");
return 99;
#endif /* ! HTTPS_SUPPORT */
}
/* run tests */
if (verbose)
printf ("Starting HTTP \"Upgrade\" tests with %s connections.\n", test_tls ? "TLS" : "plain");
/* try external select */
res = test_upgrade (0,
0);
error_count += res;
if (res)
fprintf (stderr, "FAILED: Upgrade with external select, return code %d.\n", res);
else if (verbose)
printf ("PASSED: Upgrade with external select.\n");
/* Try external auto */
res = test_upgrade (MHD_USE_AUTO,
0);
error_count += res;
if (res)
fprintf (stderr, "FAILED: Upgrade with external 'auto', return code %d.\n", res);
else if (verbose)
printf ("PASSED: Upgrade with external 'auto'.\n");
#ifdef EPOLL_SUPPORT
res = test_upgrade (MHD_USE_EPOLL,
0);
error_count += res;
if (res)
fprintf (stderr, "FAILED: Upgrade with external select with EPOLL, return code %d.\n", res);
else if (verbose)
printf ("PASSED: Upgrade with external select with EPOLL.\n");
#endif
/* Test thread-per-connection */
res = test_upgrade (MHD_USE_INTERNAL_POLLING_THREAD | MHD_USE_THREAD_PER_CONNECTION,
0);
error_count += res;
if (res)
fprintf (stderr, "FAILED: Upgrade with thread per connection, return code %d.\n", res);
else if (verbose)
printf ("PASSED: Upgrade with thread per connection.\n");
res = test_upgrade (MHD_USE_AUTO | MHD_USE_INTERNAL_POLLING_THREAD | MHD_USE_THREAD_PER_CONNECTION,
0);
error_count += res;
if (res)
fprintf (stderr, "FAILED: Upgrade with thread per connection and 'auto', return code %d.\n", res);
else if (verbose)
printf ("PASSED: Upgrade with thread per connection and 'auto'.\n");
#ifdef HAVE_POLL
res = test_upgrade (MHD_USE_INTERNAL_POLLING_THREAD | MHD_USE_THREAD_PER_CONNECTION | MHD_USE_POLL,
0);
error_count += res;
if (res)
fprintf (stderr, "FAILED: Upgrade with thread per connection and poll, return code %d.\n", res);
else if (verbose)
printf ("PASSED: Upgrade with thread per connection and poll.\n");
#endif /* HAVE_POLL */
/* Test different event loops, with and without thread pool */
res = test_upgrade (MHD_USE_INTERNAL_POLLING_THREAD,
0);
error_count += res;
if (res)
fprintf (stderr, "FAILED: Upgrade with internal select, return code %d.\n", res);
else if (verbose)
printf ("PASSED: Upgrade with internal select.\n");
res = test_upgrade (MHD_USE_INTERNAL_POLLING_THREAD,
2);
error_count += res;
if (res)
fprintf (stderr, "FAILED: Upgrade with internal select with thread pool, return code %d.\n", res);
else if (verbose)
printf ("PASSED: Upgrade with internal select with thread pool.\n");
res = test_upgrade (MHD_USE_AUTO | MHD_USE_INTERNAL_POLLING_THREAD,
0);
error_count += res;
if (res)
fprintf (stderr, "FAILED: Upgrade with internal 'auto' return code %d.\n", res);
else if (verbose)
printf ("PASSED: Upgrade with internal 'auto'.\n");
res = test_upgrade (MHD_USE_AUTO | MHD_USE_INTERNAL_POLLING_THREAD,
2);
error_count += res;
if (res)
fprintf (stderr, "FAILED: Upgrade with internal 'auto' with thread pool, return code %d.\n", res);
else if (verbose)
printf ("PASSED: Upgrade with internal 'auto' with thread pool.\n");
#ifdef HAVE_POLL
res = test_upgrade (MHD_USE_POLL_INTERNAL_THREAD,
0);
error_count += res;
if (res)
fprintf (stderr, "FAILED: Upgrade with internal poll, return code %d.\n", res);
else if (verbose)
printf ("PASSED: Upgrade with internal poll.\n");
res = test_upgrade (MHD_USE_POLL_INTERNAL_THREAD,
2);
if (res)
fprintf (stderr, "FAILED: Upgrade with internal poll with thread pool, return code %d.\n", res);
else if (verbose)
printf ("PASSED: Upgrade with internal poll with thread pool.\n");
#endif
#ifdef EPOLL_SUPPORT
res = test_upgrade (MHD_USE_EPOLL_INTERNAL_THREAD,
0);
if (res)
fprintf (stderr, "FAILED: Upgrade with internal epoll, return code %d.\n", res);
else if (verbose)
printf ("PASSED: Upgrade with internal epoll.\n");
res = test_upgrade (MHD_USE_EPOLL_INTERNAL_THREAD,
2);
if (res)
fprintf (stderr, "FAILED: Upgrade with internal epoll, return code %d.\n", res);
else if (verbose)
printf ("PASSED: Upgrade with internal epoll.\n");
#endif
/* report result */
if (0 != error_count)
fprintf (stderr,
"Error (code: %u)\n",
error_count);
#ifdef HTTPS_SUPPORT
if (test_tls && TLS_LIB_GNUTLS == use_tls_tool)
gnutls_global_deinit();
#endif /* HTTPS_SUPPORT */
return error_count != 0; /* 0 == pass */
}
void emit(Kind kind, jint code_offset, jint param) {
GUARANTEE(has_param(kind), "Sanity");
emit(kind, code_offset);
emit_ushort((jushort) param);
}
level_succ(level const & l):
level_composite(level_kind::Succ, hash(hash(l), 17u), get_depth(l) + 1, has_param(l), has_global(l), has_meta(l)),
m_l(l),
m_explicit(is_explicit(l)) {}
bool has_param(levels const & ls) { return std::any_of(ls.begin(), ls.end(), [](level const & l) { return has_param(l); }); }
Dc1394CamServer() : ros::node("dc1394_cam_server")
{
advertise_service("~check_params", &Dc1394CamServer::checkFeatureService);
dc1394_cam::init();
int num_cams;
param("num_cams", num_cams, 1);
for (int i = 0; i < num_cams; i++)
{
CamData cd;
ostringstream oss;
oss << "cam" << i;
param(oss.str(), cd.name, oss.str());
oss.str("");
oss << "FRAMEID_CAM" << i;
param(cd.name + string("/frameid"), cd.frameid, oss.str());
uint64_t guid;
if (has_param(cd.name + string("/guid")))
{
string guid_str;
get_param(cd.name + string("/guid"), guid_str);
guid = strtoll(guid_str.c_str(), NULL, 16);
} else {
guid = dc1394_cam::getGuid(i);
}
string str_speed;
dc1394speed_t speed;
param(cd.name + string("/speed"), str_speed, string("S400"));
if (str_speed == string("S100"))
speed = DC1394_ISO_SPEED_100;
else if (str_speed == string("S200"))
speed = DC1394_ISO_SPEED_200;
else
speed = DC1394_ISO_SPEED_400;
double dbl_fps;
dc1394framerate_t fps;
param(cd.name + string("/fps"), dbl_fps, 30.0);
if (dbl_fps >= 240.0)
fps = DC1394_FRAMERATE_240;
else if (dbl_fps >= 120.0)
fps = DC1394_FRAMERATE_120;
else if (dbl_fps >= 60.0)
fps = DC1394_FRAMERATE_60;
else if (dbl_fps >= 30.0)
fps = DC1394_FRAMERATE_30;
else if (dbl_fps >= 15.0)
fps = DC1394_FRAMERATE_15;
else if (dbl_fps >= 7.5)
fps = DC1394_FRAMERATE_7_5;
else if (dbl_fps >= 3.75)
fps = DC1394_FRAMERATE_3_75;
else
fps = DC1394_FRAMERATE_1_875;
cd.cam_type = NORMAL;
string str_mode;
dc1394video_mode_t mode;
param(cd.name + string("/video_mode"), str_mode, string("640x480_mono8"));
if (str_mode == string("640x480_rgb24"))
mode = DC1394_VIDEO_MODE_640x480_RGB8;
else if (str_mode == string("1024x768_rgb24"))
mode = DC1394_VIDEO_MODE_1024x768_RGB8;
else if (str_mode == string("1280x960_rgb24"))
mode = DC1394_VIDEO_MODE_1280x960_RGB8;
else if (str_mode == string("1600x1200_rgb24"))
mode = DC1394_VIDEO_MODE_1600x1200_RGB8;
else if (str_mode == string("640x480_mono8"))
mode = DC1394_VIDEO_MODE_640x480_MONO8;
else if (str_mode == string("1024x768_mono8"))
mode = DC1394_VIDEO_MODE_1024x768_MONO8;
else if (str_mode == string("1280x960_mono8"))
mode = DC1394_VIDEO_MODE_1280x960_MONO8;
else if (str_mode == string("1600x1200_mono8"))
mode = DC1394_VIDEO_MODE_1600x1200_MONO8;
else if (str_mode == string("640x480_videre"))
{
mode = DC1394_VIDEO_MODE_640x480_MONO8;
cd.cam_type = VIDERE;
}
else
mode = DC1394_VIDEO_MODE_640x480_MONO8;
int buffer_size;
param(cd.name + string("/buffer_size"), buffer_size, 8);
dc1394color_filter_t bayer;
bool colorize = false;
if ( mode == DC1394_VIDEO_MODE_640x480_MONO8 ||
mode == DC1394_VIDEO_MODE_1024x768_MONO8 ||
mode == DC1394_VIDEO_MODE_1280x960_MONO8 ||
mode == DC1394_VIDEO_MODE_1600x1200_MONO8)
{
colorize = true;
string str_bayer;
param(cd.name + string("/bayer"), str_bayer, string("none"));
if (str_bayer == string("rggb"))
bayer = DC1394_COLOR_FILTER_RGGB;
else if (str_bayer == string("gbrg"))
bayer = DC1394_COLOR_FILTER_GBRG;
else if (str_bayer == string("grbg"))
bayer = DC1394_COLOR_FILTER_GRBG;
else if (str_bayer == string("bggr"))
bayer = DC1394_COLOR_FILTER_BGGR;
else
colorize = false;
}
if (has_param(cd.name + string("/colorize")))
param(cd.name + string("/colorize"), colorize, false);
bool rectify = false;
param(cd.name + string("/rectify"), rectify, false);
videre_cam::VidereMode videre_mode;
if (cd.cam_type == VIDERE)
{
string str_videre_mode;
param(cd.name + string("/videre_param/mode"), str_videre_mode, string("none"));
if (str_videre_mode == string("none"))
videre_mode = videre_cam::PROC_MODE_NONE;
else if (str_videre_mode == string("rectified"))
videre_mode = videre_cam::PROC_MODE_RECTIFIED;
else if (str_videre_mode == string("disparity"))
videre_mode = videre_cam::PROC_MODE_DISPARITY;
else if (str_videre_mode == string("disparity_raw"))
videre_mode = videre_cam::PROC_MODE_DISPARITY_RAW;
else
videre_mode = videre_cam::PROC_MODE_NONE;
param(cd.name + string("/frameid_cloud"), cd.frameid_cloud, std::string("FRAMEID_STEREO_BLOCK"));
}
printf("Opening camera with guid: %llx\n", guid);
try
{
if (cd.cam_type == VIDERE)
{
cd.cam = new videre_cam::VidereCam(guid,
videre_mode,
speed,
fps,
buffer_size);
if (colorize)
((videre_cam::VidereCam*)(cd.cam))->enableColorization();
if (rectify)
((videre_cam::VidereCam*)(cd.cam))->enableRectification();
}
else
{
cd.cam = new dc1394_cam::Cam(guid,
speed,
mode,
fps,
buffer_size);
if (colorize)
cd.cam->enableColorization(bayer);
}
} catch(dc1394_cam::CamException e)
{
printf("Failed opening camera: %s\n", e.what());
cd.cleanup();
continue;
}
checkAndPushRanges(cd, "brightness", DC1394_FEATURE_BRIGHTNESS);
checkAndPushRanges(cd, "exposure", DC1394_FEATURE_EXPOSURE);
checkAndPushRanges(cd, "shutter", DC1394_FEATURE_SHUTTER);
checkAndPushRanges(cd, "gamma", DC1394_FEATURE_GAMMA);
checkAndPushRanges(cd, "gain", DC1394_FEATURE_GAIN);
checkAndPushRanges(cd, "whitebalance", DC1394_FEATURE_WHITE_BALANCE);
cd.cam->start();
checkAllFeatures(cd);
if (cd.cam_type == VIDERE)
{
advertise<std_msgs::String>(cd.name + string("/cal_params"), 1);
advertise<std_msgs::ImageArray>(cd.name + string("/images"), 1);
advertise<std_msgs::PointCloud>(cd.name + string("/cloud"), 1);
} else {
advertise<std_msgs::Image>(cd.name + string("/image"), 1);
advertise<std_msgs::ImageArray>(cd.name + string("/images"), 1);
}
cams_.push_back(cd);
}
next_time_ = ros::Time::now();
count_ = 0;
}