static const char *remove_table_set(struct subdbinfo *info,
const char *suffix)
{
const char *r;
if ((r = remove_table(info,suffix,"_mlog")) != 0
|| (r = remove_table(info,suffix,"_cookie")) != 0
|| (r = remove_table(info,suffix,"_slog")) != 0
|| (r = remove_table(info,suffix,"")) != 0)
return r;
return 0;
}
static void drop_tbl ()
{
char tbl_name[30];
fscanf (in_s, " table %s;", tbl_name);
trim_pending_semicolon (tbl_name);
remove_table ( get_table(tbl_name) );
}
void test_db() {
// Create a database with the default options.
auto db = grnxx::open_db("");
assert(db->num_tables() == 0);
// Create a table named "Table_1".
auto table = db->create_table("Table_1");
assert(table->name() == "Table_1");
assert(db->num_tables() == 1);
assert(db->get_table(0) == table);
assert(db->find_table("Table_1") == table);
assert(!db->find_table("Table_X"));
// The following create_table() must fail because "Table_1" already exists.
try {
db->create_table("Table_1");
assert(false);
} catch (...) {
}
// Create tables named "Table_2" and "Table_3".
assert(db->create_table("Table_2"));
assert(db->create_table("Table_3"));
assert(db->num_tables() == 3);
assert(db->get_table(0)->name() == "Table_1");
assert(db->get_table(1)->name() == "Table_2");
assert(db->get_table(2)->name() == "Table_3");
// Remove "Table_2".
db->remove_table("Table_2");
assert(db->num_tables() == 2);
assert(db->get_table(0)->name() == "Table_1");
assert(db->get_table(1)->name() == "Table_3");
// Recreate "Table_2".
assert(db->create_table("Table_2"));
// Move "Table_3" to the next to "Table_2".
db->reorder_table("Table_3", "Table_2");
assert(db->get_table(0)->name() == "Table_1");
assert(db->get_table(1)->name() == "Table_2");
assert(db->get_table(2)->name() == "Table_3");
// Move "Table_3" to the head.
db->reorder_table("Table_3", "");
assert(db->get_table(0)->name() == "Table_3");
assert(db->get_table(1)->name() == "Table_1");
assert(db->get_table(2)->name() == "Table_2");
// Move "Table_2" to the next to "Table_3".
db->reorder_table("Table_2", "Table_3");
assert(db->get_table(0)->name() == "Table_3");
assert(db->get_table(1)->name() == "Table_2");
assert(db->get_table(2)->name() == "Table_1");
}
// Process contents of all Init tables
void MYOInitTableList::process()
{
OFFLOAD_DEBUG_TRACE(2, "Process MYO Init tables:\n");
m_lock.lock();
for (Node *n = m_head; n != 0; n = n->next) {
__offload_myo_shared_init_table_process(
(InitTableEntry*)n->table.entries);
}
for (Node *n = m_head; n != 0; n = n->next) {
remove_table(n);
}
m_lock.unlock();
}
int main(int argc, char** argv)
{
AppArgs::Init();
if (!( AppArgs::AddOption("about", '?', AAT_NO_VALUE, false)
&& AppArgs::AddOption("help", 'h', AAT_NO_VALUE, false)
&& AppArgs::AddOption("list", 'l', AAT_NO_VALUE, false)
&& AppArgs::AddOption("remove", 'r', AAT_NO_VALUE, false)
&& AppArgs::AddOption("edit", 'e', AAT_NO_VALUE, false)
&& AppArgs::AddOption("types", 't', AAT_NO_VALUE, false)
&& AppArgs::AddOption("reload", 'd', AAT_NO_VALUE, false)
&& AppArgs::AddOption("user", 'u', AAT_MANDATORY_VALUE, false)
&& AppArgs::AddOption("config", 'f', AAT_MANDATORY_VALUE, false)
&& AppArgs::AddOption("version", 'V', AAT_NO_VALUE, false)))
{
fprintf(stderr, "error while initializing application");
return 1;
}
AppArgs::Parse(argc, argv);
if (AppArgs::ExistsOption("help")) {
fprintf(stderr, "%s\n", INCRONTAB_HELP);
return 0;
}
if (AppArgs::ExistsOption("about")) {
fprintf(stderr, "%s\n", INCRONTAB_DESCRIPTION);
return 0;
}
if (AppArgs::ExistsOption("version")) {
fprintf(stderr, "%s\n", INCRONTAB_VERSION);
return 0;
}
bool oper = AppArgs::ExistsOption("list")
|| AppArgs::ExistsOption("remove")
|| AppArgs::ExistsOption("edit")
|| AppArgs::ExistsOption("types")
|| AppArgs::ExistsOption("reload");
size_t vals = AppArgs::GetValueCount();
if (!oper && vals == 0) {
fprintf(stderr, "invalid arguments - specify operation or source file\n");
return 1;
}
if (oper && vals > 0) {
fprintf(stderr, "invalid arguments - operation and source file cannot be combined\n");
return 1;
}
uid_t uid = getuid();
std::string user;
bool chuser = AppArgs::GetOption("user", user);
if (uid != 0 && chuser) {
fprintf(stderr, "cannot override user to '%s': insufficient privileges\n", user.c_str());
return 1;
}
struct passwd* ppwd = NULL;
if (chuser) {
if ((ppwd = getpwnam(user.c_str())) != NULL) {
if ( setenv("LOGNAME", ppwd->pw_name, 1) != 0
|| setenv("USER", ppwd->pw_name, 1) != 0
|| setenv("USERNAME", ppwd->pw_name, 1) != 0
|| setenv("HOME", ppwd->pw_dir, 1) != 0
|| setenv("SHELL", ppwd->pw_shell, 1) != 0)
{
perror("cannot set environment variables");
return 1;
}
} else {
fprintf(stderr, "user '%s' not found\n", user.c_str());
return 1;
}
} else {
ppwd = getpwuid(uid);
if (ppwd == NULL) {
fprintf(stderr, "cannot determine current user\n");
return 1;
}
user = ppwd->pw_name;
}
try {
IncronCfg::Init();
std::string cfg(INCRON_CONFIG);
if (AppArgs::GetOption("config", cfg)) {
if (uid != 0) {
fprintf(stderr, "insufficient privileges to use custom configuration (will use default)\n");
}
else if (euidaccess(cfg.c_str(), R_OK) != 0) {
perror("cannot read configuration file (will use default)");
}
}
IncronCfg::Load(cfg);
if (!IncronTab::CheckUser(user)) {
fprintf(stderr, "user '%s' is not allowed to use incron\n", user.c_str());
return 1;
}
if (!oper) {
std::string file;
if (!AppArgs::GetValue(0, file)
|| !copy_from_file(file, user))
{
return 1;
}
}
else {
if (AppArgs::ExistsOption("list")) {
if (!list_table(user))
return 1;
}
else if (AppArgs::ExistsOption("remove")) {
if (!remove_table(user))
return 1;
}
else if (AppArgs::ExistsOption("edit")) {
if (!edit_table(user))
return 1;
}
else if (AppArgs::ExistsOption("types")) {
list_types();
}
else if (AppArgs::ExistsOption("reload")) {
if (!reload_table(user))
return 1;
}
else {
fprintf(stderr, "invalid usage\n");
return 1;
}
}
return 0;
} catch (InotifyException e) {
fprintf(stderr, "*** unhandled exception occurred ***\n");
fprintf(stderr, "%s\n", e.GetMessage().c_str());
fprintf(stderr, "error: (%i) %s\n", e.GetErrorNumber(), strerror(e.GetErrorNumber()));
return 1;
}
}
void remove_schema ( schema_p s )
{
if (s == NULL) return;
remove_table (s->tbl);
}
int main(int argc, char** argv)
{
struct arguments arguments;
arguments.user = NULL;
arguments.oper = OPER_NONE;
arguments.file = NULL;
argp_parse (&argp, argc, argv, 0, 0, &arguments);
if (arguments.file != NULL && arguments.oper != OPER_NONE) {
fprintf(stderr, "invalid arguments - specify source file or operation\n");
return 1;
}
if (arguments.file == NULL && arguments.oper == OPER_NONE) {
fprintf(stderr, "invalid arguments - specify source file or operation\n");
return 1;
}
uid_t uid = getuid();
if (uid != 0 && arguments.user != NULL) {
fprintf(stderr, "cannot access table for user %s: permission denied\n", arguments.user);
return 1;
}
struct passwd pwd;
if (arguments.user == NULL) {
struct passwd* ppwd = getpwuid(uid);
if (ppwd == NULL) {
fprintf(stderr, "cannot determine current user\n");
return 1;
}
memcpy(&pwd, ppwd, sizeof(pwd));
arguments.user = pwd.pw_name;
}
else if (getpwnam(arguments.user) == NULL) {
fprintf(stderr, "user %s not found\n", arguments.user);
return 1;
}
if (!InCronTab::CheckUser(arguments.user)) {
fprintf(stderr, "user %s is not allowed to use incron\n", arguments.user);
return 1;
}
switch (arguments.oper) {
case OPER_NONE:
fprintf(stderr, "copying table from file: %s\n", arguments.file);
if (!copy_from_file(arguments.file, arguments.user))
return 1;
break;
case OPER_LIST:
if (!list_table(arguments.user))
return 1;
break;
case OPER_REMOVE:
fprintf(stderr, "removing table for user %s\n", arguments.user);
if (!remove_table(arguments.user))
return 1;
break;
case OPER_EDIT:
if (!edit_table(arguments.user))
return 1;
break;
default:
fprintf(stderr, "invalid usage\n");
return 1;
}
return 0;
}
bool change_detection (tms_msg_ss::ods_furniture::Request &req, tms_msg_ss::ods_furniture::Response &res)
{
//***************************
// local variable declaration
//***************************
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr model (new pcl::PointCloud<pcl::PointXYZRGB>);
std::cout << mkdir("/tmp/tms_ss_ods", S_IRUSR | S_IWUSR | S_IXUSR) << std::endl;
std::cout << mkdir("/tmp/tms_ss_ods/ods_change_detection", S_IRUSR | S_IWUSR | S_IXUSR) << std::endl;
std::cout << mkdir("/tmp/tms_ss_ods/ods_change_detection/table", S_IRUSR | S_IWUSR | S_IXUSR) << std::endl;
//***************************
// capture kinect data
//***************************
tms_msg_ss::ods_pcd srv;
srv.request.id = 3;
if(commander_to_kinect_capture.call(srv)){
pcl::fromROSMsg (srv.response.cloud, *cloud1);
}
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/input.pcd", *cloud1);
//pcl::fromROSMsg (req.model, *model);
pcl::io::loadPCDFile("/tmp/tms_ss_ods/ods_change_detection/table/input.pcd", *cloud1);
TABLE = req.id;
if(TABLE == 1)
pcl::io::loadPCDFile ("/tmp/tms_ss_ods/ods_change_detection/table/model_table1.pcd", *model);
else if(TABLE == 2)
pcl::io::loadPCDFile ("/tmp/tms_ss_ods/ods_change_detection/table/model_table2.pcd", *model);
//***************************
// Fill in the cloud data
//***************************
// table.x = req.furniture.position.x/1000.0;
// table.y = req.furniture.position.y/1000.0;
// table.z = req.furniture.position.z/1000.0;
// table.theta = req.furniture.orientation.z;
// robot.x = req.robot.x/1000.0;
// robot.y = req.robot.y/1000.0;
// robot.theta = req.robot.theta;
// sensor.x = req.sensor.position.x;
// sensor.y = req.sensor.position.y;
// sensor.z = req.sensor.position.z;
// sensor.theta = req.sensor.orientation.y;
table.x = 1.0;
table.y = 1.5;
table.z = 0.7;
table.theta = 0.0;
robot.x = 2.6;
robot.y = 1.9;
robot.theta = 180.0;
sensor.x = 0.0;
sensor.y = 0.0;
sensor.z = 1.0;
sensor.theta = 20.0;
std::cout << robot.x << " " << robot.y << " " << robot.theta << std::endl;
//***************************
// transform input cloud
//***************************
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tfm_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
transformation(*cloud1, *model);
for(int i=0;i<model->points.size();i++){
model->points[i].r = 255;
model->points[i].g = 0;
model->points[i].b = 0;
}
*tfm_cloud = *model + *cloud1;
if(!(tfm_cloud->points.size())){
std::cout << "tfm_cloud has no point" << std::endl;
return false;
}
else
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/tfm_cloud1.pcd", *tfm_cloud);
//***************************
// filtering by using model
//***************************
std::cout << "filtering" << std::endl;
filtering(*cloud1, *model);
if(!(cloud1->points.size())){
std::cout << "filtered cloud has no point" << std::endl;
return false;
}
else
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/filter.pcd", *cloud1);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr dsp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud1, *dsp_cloud);
downsampling(*dsp_cloud, 0.01);
//***************************
// registration between two input pcd data
//***************************
std::cout << "registration" << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgs_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr view_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Matrix4f m;
int n = 0;
while (1){
registration(*dsp_cloud, *model, *rgs_cloud, *cloud1, m);
if((double)(m(0,0)+m(1,1)+m(2,2)+m(3,3)) >= 4){
if(n > 2) break;
}
n++;
}
pcl::copyPointCloud(*cloud1, *view_cloud);
if(!(rgs_cloud->points.size())){
std::cout << "registered cloud has no point" << std::endl;
return false;
}
else
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/rgs_cloud.pcd", *rgs_cloud);
//***************************
// init t_voxel
//***************************
std::cout << "init table_voxel" << std::endl;
make_tablevoxel(*model);
//***************************
// remove table
//***************************
std::cout << "remove table" << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rmc_cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rmc_cloud2 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile("/tmp/tms_ss_ods/ods_change_detection/table/memory.pcd", *rmc_cloud2);
remove_table(*cloud1, *rmc_cloud1);
if(!(rmc_cloud1->size() != 0)){
std::cout << "removed table cloud has no point" << std::endl;
return false;
}
else
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/remove_table1.pcd", *rmc_cloud1, false);
//***************************
// segmentation
//***************************
segmentation(*rmc_cloud1, 0.015, 1);
if(rmc_cloud2->size() != 0)
segmentation(*rmc_cloud2, 0.015, 2);
if(rmc_cloud1->size() != 0)
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/memory.pcd", *rmc_cloud1, true);
if(rmc_cloud2->size() != 0)
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/obj_cloud2.pcd", *rmc_cloud2, true);
//***************************
// compare segments with memory
//***************************
segment_matching();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_rgb1 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_rgb2 (new pcl::PointCloud<pcl::PointXYZRGB>);
//***************************************
// rewrite Object data on TMS database
//***************************************
tms_msg_db::tmsdb_ods_object_data srv3;
ros::Time time = ros::Time::now() + ros::Duration(9*60*60);
int c=0;
float colors[6][3] ={{255, 0, 0}, {0,255,0}, {0,0,255}, {255,255,0}, {0,255,255}, {255,0,255}};
for(int i=0;i<Object_Map.size();i++){
std::cout << i << " → " << Object_Map[i].tf << std::endl;
if((Object_Map[i].tf == 1)||(Object_Map[i].tf == 2)){
std::stringstream ss;
ss << "/tmp/tms_ss_ods/ods_change_detection/table/result_rgb" << i << ".pcd";
if(Object_Map[i].cloud_rgb.size() != 0){
pcl::io::savePCDFile(ss.str (), Object_Map[i].cloud_rgb);
for(int j=0;j<Object_Map[i].cloud_rgb.points.size();j++){
Object_Map[i].cloud_rgb.points[j].r = colors[c%6][0];
Object_Map[i].cloud_rgb.points[j].g = colors[c%6][1];
Object_Map[i].cloud_rgb.points[j].b = colors[c%6][2];
}
c++;
}
else
std::cout << "no cloud_rgb data" << std::endl;
if(Object_Map[i].tf == 1){
*tmp_rgb1 += Object_Map[i].cloud_rgb;
tms_msg_db::tmsdb_data data;
data.tMeasuredTime = time;
data.iType = 5;
data.iID = 53;
data.fX = 1000.0 * (Object_Map[i].g.x - 1.0);
data.fY = 1000.0 * (Object_Map[i].g.y - 1.5);
data.fZ = 700.0;
data.fTheta = 0.0;
data.iPlace = 14;
data.iState = 1;
srv3.request.srvTMSInfo.push_back(data);
geometry_msgs::Pose pose;
pose.position.x = Object_Map[i].g.x; pose.position.y = Object_Map[i].g.y; pose.position.z = Object_Map[i].g.z;
pose.orientation.x = 0.0; pose.orientation.y = 0.0; pose.orientation.z = 0.0; pose.orientation.w = 0.0;
res.objects.poses.push_back(pose);
std::cout << Object_Map[i].g.x << " " << Object_Map[i].g.y << " " << Object_Map[i].g.z << std::endl;
}
else if(Object_Map[i].tf == 2)
*tmp_rgb2 += Object_Map[i].cloud_rgb;
}
}
if(srv3.request.srvTMSInfo.size() != 0){
if(commander_to_ods_object_data.call(srv3))
std::cout << "Rewrite TMS database!!" << std::endl;
}
if((tmp_rgb1->size() == 0) && (tmp_rgb2->size() == 0)){
std::cout << "No change on table!!" << std::endl;
return true;
}
if(tmp_rgb1->size() != 0)
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/result1.pcd", *tmp_rgb1, true);
if(tmp_rgb2->size() != 0)
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/result2.pcd", *tmp_rgb2, true);
*view_cloud += *tmp_rgb1;
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/view_result.pcd", *view_cloud, true);
//pcl::toROSMsg (*cloud1, res.cloud);
Object_Map.clear();
return true;
}
static void select_rows ()
{
char in_str[200] = "", from_str[100] = "";
char *where_str, *p;
char attr[11] = "", op[3] = "";
int val;
tbl_p where_tbl = NULL, res_tbl = NULL, from_tbl = NULL;
char *attrs[10];
int num_res_attrs = 0, i;
for ( i = 0; i < 10; i++ ) attrs[i] = NULL;
fscanf (in_s, "%[^;];", in_str);
p = strstr (in_str, " from ");
if ( p == NULL )
{
put_msg (ERROR, "select %s: from which table to select?\n", in_str);
return;
}
*p = '\0';
p += 6;
if ( sscanf (p, "%s", from_str) != 1 )
{
put_msg (ERROR, "select from what?\n");
return;
}
num_res_attrs = str_split ( in_str, ',', attrs, 10);
if (num_res_attrs == 0)
{
put_msg (ERROR, "select from %s: select what?\n", p);
return;
}
p += 2;
where_str = strstr (p, " where ");
if ( where_str != NULL ) where_str += 7;
put_msg (DEBUG, "from: \"%s\", where: \"%s\"\n", from_str, where_str);
from_tbl = get_table (from_str);
if (from_tbl == NULL)
{
put_msg (ERROR, "select: table \"%s\" does not exist.\n", from_str);
return;
}
if ( where_str != NULL )
{
if (sscanf ( where_str, "%s %s %d", attr, op, &val) != 3)
{
put_msg (ERROR, "query \"%s\" is not supported.\n", where_str);
return;
}
where_tbl = table_search_binary ( from_tbl, attr, op, val);
if (where_tbl == NULL)
return;
}
if ( attrs[0][0] == '*' )
table_display (where_tbl ? where_tbl : from_tbl);
else
{
res_tbl = table_project ( where_tbl ? where_tbl : from_tbl,
"tmp_res_01", num_res_attrs, attrs);
table_display (res_tbl);
}
remove_table (where_tbl);
remove_table (res_tbl);
}
