Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <sys/types.h>
00038 #include <signal.h>
00039 #include <unistd.h>
00040 #include <ros/ros.h>
00041 #include <moveit/warehouse/warehouse_connector.h>
00042
00043 namespace moveit_warehouse
00044 {
00045 WarehouseConnector::WarehouseConnector(const std::string& dbexec) : dbexec_(dbexec), child_pid_(0)
00046 {
00047 }
00048
00049 WarehouseConnector::~WarehouseConnector()
00050 {
00051 if (child_pid_ != 0)
00052 kill(child_pid_, SIGTERM);
00053 }
00054
00055 bool WarehouseConnector::connectToDatabase(const std::string& dirname)
00056 {
00057 if (child_pid_ != 0)
00058 kill(child_pid_, SIGTERM);
00059
00060 child_pid_ = fork();
00061 if (child_pid_ == -1)
00062 {
00063 ROS_ERROR("Error forking process.");
00064 child_pid_ = 0;
00065 return false;
00066 }
00067
00068 if (child_pid_ == 0)
00069 {
00070 std::size_t exec_file_pos = dbexec_.find_last_of("/\\");
00071 if (exec_file_pos != std::string::npos)
00072 {
00073 char** argv = new char*[4];
00074 std::size_t exec_length = 1 + dbexec_.length() - exec_file_pos;
00075 argv[0] = new char[1 + exec_length];
00076 snprintf(argv[0], exec_length, "%s", dbexec_.substr(exec_file_pos + 1).c_str());
00077
00078 argv[1] = new char[16];
00079 snprintf(argv[1], 15, "--dbpath");
00080
00081 argv[2] = new char[1024];
00082 snprintf(argv[2], 1023, "%s", dirname.c_str());
00083
00084 argv[3] = NULL;
00085
00086 int code = execv(dbexec_.c_str(), argv);
00087 delete[] argv[0];
00088 delete[] argv[1];
00089 delete[] argv[2];
00090 delete[] argv;
00091 ROS_ERROR_STREAM("execv() returned " << code << ", errno=" << errno << " string errno = " << strerror(errno));
00092 }
00093 return false;
00094 }
00095 else
00096 {
00097
00098 ros::WallDuration(1.0).sleep();
00099 }
00100 return true;
00101 }
00102 }