void MaskImageToROI::convert(
const sensor_msgs::Image::ConstPtr& mask_msg)
{
vital_checker_->poke();
boost::mutex::scoped_lock lock(mutex_);
if (latest_camera_info_) {
sensor_msgs::CameraInfo camera_info(*latest_camera_info_);
std::vector<cv::Point> indices;
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(
mask_msg, sensor_msgs::image_encodings::MONO8);
cv::Mat mask = cv_ptr->image;
for (size_t j = 0; j < mask.rows; j++) {
for (size_t i = 0; i < mask.cols; i++) {
if (mask.at<uchar>(j, i) == 255) {
indices.push_back(cv::Point(i, j));
}
}
}
cv::Rect mask_rect = cv::boundingRect(indices);
camera_info.roi.x_offset = mask_rect.x;
camera_info.roi.y_offset = mask_rect.y;
camera_info.roi.width = mask_rect.width;
camera_info.roi.height = mask_rect.height;
camera_info.header = mask_msg->header;
pub_.publish(camera_info);
}
else {
NODELET_ERROR("camera info is not yet available");
}
}
int main(int argc, char** argv) {
int ret, i;
int nerrors[NUMBER_OF_DIFFERENT_SYNTAXES + 1];
/* argtable setup */
/* SYNTAX 1: [--info] */
info1 = arg_lit1(NULL, "info", "Show information about the FLI camera");
end1 = arg_end(20);
void * argtable1[] = {info1, end1};
argtable[1] = argtable1;
/* SYNTAX 2: [--get-temperature] */
gettemp2 = arg_lit1(NULL, "get-temperature", "Print the temperature of the CCD, base and cooling power");
end2 = arg_end(20);
void * argtable2[] = {gettemp2, end2};
argtable[2] = argtable2;
/* SYNTAX 3: [--set-temperature] */
settemp3 = arg_int1(NULL, "set-temperature", "N", "Set the temperature of the CCD (in Celsius)");
end3 = arg_end(20);
void * argtable3[] = {settemp3, end3};
argtable[3] = argtable3;
/* SYNTAX 4: [--fan] */
fan4 = arg_str1(NULL, "fan", "(on|off)", "Turn on or off the fan");
end4 = arg_end(20);
void * argtable4[] = {fan4, end4};
argtable[4] = argtable4;
/* SYNTAX 5: [--shutter] */
shutter5 = arg_str1(NULL, "shutter", "(open|close)", "Open or close the shutter");
end5 = arg_end(20);
void * argtable5[] = {shutter5, end5};
argtable[5] = argtable5;
/* SYNTAX 6: --acquire <time/sec> --shutter {open|close} --output xyz.fits
[--bin <bx>,<by>] [--offset <x0>,<y0>] [--size <sx>,<sy>]
[--mode <mode>...] [--verbose]
*/
acquire6 = arg_str1(NULL, "acquire", "<time>", "Exposure time in sec");
shutter6 = arg_str1(NULL, "shutter", "(open|close)", "Whether to open or close the shutter");
output6 = arg_file1(NULL, "output", "xyz.fits", "Output filename for the FITS file");
bin6 = arg_str0(NULL, "bin", "<bx,by>", "Binning options in X,Y format");
offset6 = arg_str0(NULL, "offset", "<x0,y0>", "Offset options in X,Y format");
size6 = arg_str0(NULL, "size", "<sx,sy>", "Sizes in X,Y format");
mode6 = arg_str0(NULL, "mode", "(1mhz|8mhz)", "Download speed");
verbose6 = arg_lit0(NULL, "verbose", "Show verbose output");
end6 = arg_end(20);
void * argtable6[] = {acquire6, shutter6, output6, bin6, offset6, size6, mode6, verbose6, end6};
argtable[6] = argtable6;
/* SYNTAX 7: [--help]*/
help7 = arg_lit1(NULL, "help", "Print this help");
end7 = arg_end(20);
void * argtable7[] = {help7, end7};
argtable[7] = argtable7;
/* verify all argtable[] entries were allocated successfully */
for (i = 1; i <= NUMBER_OF_DIFFERENT_SYNTAXES; i++) {
if (arg_nullcheck(argtable[i]) != 0) {
printf("%s: insufficient memory\n", progname);
return EXIT_FAILURE;
}
}
/* set defaults for the optional arguments */
bin6->sval[0] = "1,1";
offset6->sval[0] = "0,0";
size6->sval[0] = "4096,4096";
mode6->sval[0] = "8mhz";
/* parse all argument possibilities */
for (i = 1; i <= NUMBER_OF_DIFFERENT_SYNTAXES; i++) {
nerrors[i] = arg_parse(argc, argv, argtable[i]);
}
/* select the right command */
/* --info */
if (nerrors[1] == 0) {
if (info1->count > 0) {
ret = camera_info();
if (ret) return ret;
return 0;
}
/* --get-temperature */
} else if (nerrors[2] == 0) {
if (gettemp2->count > 0) {
ret = camera_get_temp();
if (ret) return ret;
return 0;
}
/* --set-temperature */
} else if (nerrors[3] == 0) {
if (settemp3->count > 0) {
ret = camera_set_temp(settemp3->ival[0]);
if (ret) return ret;
return 0;
}
/* --fan */
} else if (nerrors[4] == 0) {
int fan;
if (strcmp("on", fan4->sval[0]) == 0) {
fan = 1;
} else if (strcmp("off", fan4->sval[0]) == 0) {
fan = 0;
} else {
fprintf(stderr, "Cannot parse the option for --fan, see --help.\n");
return -1;
}
ret = camera_set_fan(fan);
if (ret) return ret;
return 0;
/* --shutter */
} else if (nerrors[5] == 0) {
int open_shutter;
if (strcmp("open", shutter5->sval[0]) == 0) {
open_shutter = 1;
} else if (strcmp("close", shutter5->sval[0]) == 0) {
open_shutter = 0;
} else {
fprintf(stderr, "Cannot parse the option for --shutter, see --help.\n");
return -1;
}
ret = camera_control_shutter(open_shutter);
if (ret) return ret;
return 0;
/* --acquire */
} else if (nerrors[6] == 0) {
if (acquire6->count > 0) {
/* local variables to store the arguments */
float exposure_time;
int is_dark;
char * output_filename;
char * bin_options;
char * offset_options;
char * size_options;
int bx, by;
int x0, y0;
int sx, sy;
int one_mhz_speed;
int is_verbose = 0;
/* copy const char arrays to char arrays to suppress warnings */
output_filename = (char*) malloc((strlen(output6->filename[0])) * sizeof (char));
bin_options = (char*) malloc((strlen(bin6->sval[0])) * sizeof (char));
offset_options = (char*) malloc((strlen(offset6->sval[0])) * sizeof (char));
size_options = (char*) malloc((strlen(size6->sval[0])) * sizeof (char));
strcpy(output_filename, output6->filename[0]);
strcpy(bin_options, bin6->sval[0]);
strcpy(offset_options, offset6->sval[0]);
strcpy(size_options, size6->sval[0]);
/* process arguments */
/* exposure time */
exposure_time = atof(acquire6->sval[0]);
if (exposure_time < 0) {
fprintf(stderr, "Exposure time cannot be below zero (given %f).\n", exposure_time);
return -1;
}
/* shutter */
if (strcmp("open", shutter6->sval[0]) == 0) {
is_dark = 0;
} else if (strcmp("close", shutter6->sval[0]) == 0) {
is_dark = 1;
} else {
fprintf(stderr, "Cannot parse the option for --shutter, see --help.\n");
return -1;
}
/* bin, size and offset */
ret = parse_comma_separated_values(bin_options, &bx, &by, "bin");
if (ret) return ret;
ret = parse_comma_separated_values(offset_options, &x0, &y0, "offset");
if (ret) return ret;
ret = parse_comma_separated_values(size_options, &sx, &sy, "size");
if (ret) return ret;
/* mode */
if (strcmp("1mhz", mode6->sval[0]) == 0) {
one_mhz_speed = 1;
} else if (strcmp("8mhz", mode6->sval[0]) == 0) {
one_mhz_speed = 0;
} else {
fprintf(stderr, "Cannot parse the option for --mode, see --help.\n");
return -1;
}
/* verbose */
if (verbose6->count > 0) is_verbose = 1;
ret = camera_acquire(exposure_time, is_dark, output_filename,
bx, by, x0, y0, sx, sy, one_mhz_speed, is_verbose);
if (ret) return ret;
return 0;
}
/* --help */
} else if (nerrors[7] == 0) {
if (help7) {
ret = camera_help();
if (ret) return ret;
return 0;
}
/* incorrect or partially incorrect argument syntaxes */
} else {
if (settemp3->count > 0) {
arg_print_errors(stdout, end3, progname);
printf("usage: %s ", progname);
arg_print_syntax(stdout, argtable3, "\n");
} else if (fan4->count > 0) {
arg_print_errors(stdout, end4, progname);
printf("usage: %s ", progname);
arg_print_syntax(stdout, argtable4, "\n");
} else if (acquire6->count > 0) {
arg_print_errors(stdout, end5, progname);
printf("usage: %s ", progname);
arg_print_syntax(stdout, argtable5, "\n");
} else {
printf("%s: unable to parse arguments, see syntax below:\n", progname);
ret = 0;
ret = camera_help();
return ret;
}
return EXIT_FAILURE;
}
/* no command line options at all */
printf("Try '%s --help' for more information.\n", progname);
return (EXIT_SUCCESS);
}
bool Sketch::loadSvg(const char *path, Terrain *terrain)
{
QFile file(path);
if (!file.open(QFile::ReadOnly | QFile::Text)) {
qWarning("Cannot open file");
return false;
}
QString localName;
QXmlStreamAttributes attributes;
QXmlStreamReader *pXml = new QXmlStreamReader(&file);
pXml->setNamespaceProcessing(false);
while (!pXml->atEnd()) {
switch (pXml->readNext()) {
case QXmlStreamReader::StartElement:
localName = pXml->name().toString();
if (localName.compare("svg") == 0) {
attributes = pXml->attributes();
std::stringstream ss(attributes.value("viewBox").toString().toStdString());
Eigen::Vector2i origin;
ss >> origin[0] >> origin[1] >> camera_info_.screen_width >> camera_info_.screen_height;
}
if (localName.compare("path") == 0) {
attributes = pXml->attributes();
QStringRef data = attributes.value("d");
std::vector<Eigen::Vector2i> control_points;
std::vector<Eigen::Vector2i> cur_points;
int count = data.count();
std::string cur_str;
Eigen::Vector2i cur_point;
for (int i = 0; i < count; ++i)
{
char c = data.at(i).toLatin1();
switch (c) {
case 'M':
case 'C':
control_points.insert(control_points.end(),
cur_points.begin(),
cur_points.end());
cur_str.clear();
cur_points.clear();
break;
case ',':
cur_point[0] = round(atof(cur_str.c_str()));
cur_str.clear();
break;
case ' ':
case '\n':
case '\t':
if (cur_str.size() > 0)
{
cur_point[1] = camera_info_.screen_height-1-round(atof(cur_str.c_str()));
cur_points.push_back(cur_point);
cur_str.clear();
} else
{
control_points.insert(control_points.end(),
cur_points.begin(),
cur_points.end());
cur_str.clear();
cur_points.clear();
}
break;
default:
cur_str += c;
break;
}
}
if (cur_str.size() > 0)
{
cur_point[1] = camera_info_.screen_height-1-atof(cur_str.c_str());
cur_points.push_back(cur_point);
control_points.insert(control_points.end(),
cur_points.begin(),
cur_points.end());
cur_str.clear();
cur_points.clear();
}
if (control_points.size() <= 2)
continue;
Stroke *stroke = new Stroke ();
stroke->set_identifier(attributes.value("id").toString().toStdString());
if (terrain == NULL)
{
for (unsigned int i = 0; i < control_points.size(); ++i)
stroke->addControlPoint(Stroke::Point(control_points[i][0], 0.0f, control_points[i][1]));
} else
{
OfflineTerrainRenderer terrain_renderer(terrain);
terrain_renderer.setCameraInfo(camera_info());
std::vector<Eigen::Vector3f> unprojected_points;
terrain_renderer.unProject(control_points, unprojected_points);
for (unsigned int i = 0; i < unprojected_points.size(); ++i)
stroke->addControlPoint(unprojected_points[i]);
}
addStroke(stroke);
}
break;
default:
break;
}
}
