CoaxSimpleControl::CoaxSimpleControl(ros::NodeHandle &node)
:reach_nav_state(node.serviceClient<coax_msgs::CoaxReachNavState>("reach_nav_state"))
,configure_comm(node.serviceClient<coax_msgs::CoaxConfigureComm>("configure_comm"))
,configure_control(node.serviceClient<coax_msgs::CoaxConfigureControl>("configure_control"))
,set_timeout(node.serviceClient<coax_msgs::CoaxSetTimeout>("set_timeout"))
,coax_state_sub(node.subscribe("state",1, &CoaxSimpleControl::coaxStateCallback, this))
,coax_tf_sub(node.subscribe("tf",1, &CoaxSimpleControl::coaxTfCallback, this))
,raw_control_pub(node.advertise<coax_msgs::CoaxRawControl>("rawcontrol",1))
,simple_control_pub(node.advertise<coax_msgs::CoaxControl>("simplecontrol",1))
,LOW_POWER_DETECTED(false)
,CONTROL_MODE(CONTROL_LANDED)
,FIRST_START(false)
,FIRST_LANDING(false)
,FIRST_HOVER(false)
,INIT_DESIRE(false)
,coax_nav_mode(0)
,coax_control_mode(0)
,coax_state_age(0)
,raw_control_age(0)
,init_count(0)
,mil_count(0)
,rotor_ready_count(-1)
,battery_voltage(12.22)
,imu_y(0.0),imu_r(0.0),imu_p(0.0)
,range_al(0.0)
,rc_th(0.0),rc_y(0.0),rc_r(0.0),rc_p(0.0)
,rc_trim_th(0.0),rc_trim_y(0.104),rc_trim_r(0.054),rc_trim_p(0.036)
,img_th(0.0),img_y(0.0),img_r(0.0),img_p(0.0)
,gyro_ch1(0.0),gyro_ch2(0.0),gyro_ch3(0.0)
,accel_x(0.0),accel_y(0.0),accel_z(0.0)
,mag_1(0.0), mag_2(0.0), mag_3(0.0)
,motor_up(0),motor_lo(0)
,servo_roll(0),servo_pitch(0)
,roll_trim(0),pitch_trim(0)
,motor1_des(0.0),motor2_des(0.0),servo1_des(0.0),servo2_des(0.0)
,yaw_des(0.0),yaw_rate_des(0.0)
,roll_des(0.0),roll_rate_des(0.0)
,pitch_des(0.0),pitch_rate_des(0.0)
,altitude_des(1.5), coax_global_x(0.0), coax_global_y(0.0), coax_global_z(0.0)
,qnd0(0.0), qnd1(0.0), qnd2(0.0), qnd3(0.0)
,Gx_des(0.0), Gy_des(0.0)
,PI(3.14159265), r2d(57.2958), d2r(0.0175)
,ang_err_lim(0.14)
,x_vel_hist_1(0.0), x_vel_hist_2(0.0), x_vel_hist_3(0.0) , x_vel_hist_4(0.0)
,y_vel_hist_1(0.0), y_vel_hist_2(0.0), y_vel_hist_3(0.0) , y_vel_hist_4(0.0)
,x_gyro_1(0.0), x_gyro_2(0.0)
,y_gyro_1(0.0), y_gyro_2(0.0)
,Gz_old1(0.0), Gz_old2(0.0), Gz_old3(0.0)
,way_changed(0)
,way_old{0.0, 0.0, 0.0}
{
set_nav_mode.push_back(node.advertiseService("set_nav_mode", &CoaxSimpleControl::setNavMode, this));
set_control_mode.push_back(node.advertiseService("set_control_mode", &CoaxSimpleControl::setControlMode, this));
set_waypoint.push_back(node.advertiseService("set_waypoint", &CoaxSimpleControl::setWaypoint, this));
loadParams(node);
}
void Stage0::loadValuesFromStage() {
loadParams();
for(int i = 0; i < ui.listWidget_ECTypes->count(); i++) {
ui.listWidget_ECTypes->item(i)->setCheckState(Qt::Unchecked);
}
int ecTypesCount = controller->stage0.initialECTypes.size();
allECTypes = controller->stage0.initialECTypes;
std::vector<AxiomLib::ElemCondPlus> &ecs = allECTypes;
ui.listWidget_ECTypes->clear();
for(int i = 0; i < ecTypesCount; i++)
{
QString name = QString(ecs[i].elemCondition->name().c_str());
if(ecs[i].sign == false) {
name.prepend("not ");
}
QListWidgetItem *item = new QListWidgetItem(name, ui.listWidget_ECTypes);
item->setFlags(
Qt::ItemIsUserCheckable | Qt::ItemIsEnabled
);
item->setTextAlignment(Qt::AlignRight);
item->setCheckState(
controller->stage0.checkedECTypes[i] ?
Qt::Checked :
Qt::Unchecked
);
}
}
LaneDetector::LaneDetector(ros::NodeHandle& node_handle) {
loadParams(node_handle);
// Grass Removal
kernel_size = 8;
svm = new SVM();
svm->init(kernel_size * kernel_size * 3);
std::string model_path = ros::package::getPath("lane_detector")+"/data/"+training_data_file + ".model";
svm->loadModel(model_path.c_str());
setupComms();
if (debug_mode > 0) {
original_image_window = ros::this_node::getName() + std::string("/original_image");
cv::namedWindow(original_image_window, CV_WINDOW_AUTOSIZE);
result_window = ros::this_node::getName() + std::string("/result");
cv::namedWindow(result_window, CV_WINDOW_AUTOSIZE);
grass_removal_output_window = ros::this_node::getName() + std::string("/grass_removal_output");
cv::namedWindow(grass_removal_output_window, CV_WINDOW_AUTOSIZE);
ipt_output_window = ros::this_node::getName() + std::string("/ipt_output");
cv::namedWindow(ipt_output_window, 0);
obstacle_removal_output_window = ros::this_node::getName() + std::string("/obstacle_removal_output");
cv::namedWindow(obstacle_removal_output_window, CV_WINDOW_AUTOSIZE);
lane_binary_output = ros::this_node::getName() + std::string("/lane_binary_output");
cv::namedWindow(lane_binary_output, CV_WINDOW_AUTOSIZE);
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "image_publisher");
ros::NodeHandle nh, pnh("~");
FakeCameraParams params = loadParams(pnh);
image_transport::ImageTransport it(nh);
image_transport::Publisher pub =
it.advertise(params.topic+"/image", 1);
ros::Publisher info_pub =
nh.advertise<sensor_msgs::CameraInfo>(params.topic+"/camera_info", 1);
sensor_msgs::CameraInfo info = loadCameraInfo(params);
cv::Mat image =
cv::Mat(params.width, params.height, CV_8UC3, cv::Scalar(50,50,50));
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
msg->header.frame_id = params.frame;
msg->header.stamp = ros::Time::now();
ros::Rate loop_rate(params.rate);
while (nh.ok()) {
msg->header.stamp = ros::Time::now();
info.header.stamp = ros::Time::now();
pub.publish(msg);
info_pub.publish(info);
ros::spinOnce();
loop_rate.sleep();
}
}
LauncherApp::LauncherApp()
: wxApp()
{
completed = false;
SetAppName(_("IzPack launcher"));
loadParams();
}
//copypaste de LR
void SVMachine::run(){
// std::cout << "I'm running the mode ";
if(C_executionMode == 0){
// std::cout << "Test" << std::endl;
loadTrainingSet(C_trainingFile);
loadTestingSet(C_testingFile);
// //Comprobamos que se han cargado bien los dos archivos
//
// for(unsigned int i = 0; i < C_trainingSet.size(); i++){
// for(unsigned int j = 0; j < C_trainingSet[i].getInput().size(); j++){
// std::cout << C_trainingSet[i].getInput()[j] << ' ';
// }
// std::cout << std::endl << C_trainingSet[i].getResult()[0] << std::endl;
// }
//
// std::cout << std::endl;
//
// for(unsigned int i = 0; i < C_testingSet.size(); i++){
// for(unsigned int j = 0; j < C_testingSet[i].getInput().size(); j++){
// std::cout << C_testingSet[i].getInput()[j] << ' ';
// }
// std::cout << std::endl << C_testingSet[i].getResult()[0] << std::endl;
// }
//
// //Fin de la comprobacion
train();
test();
} else if(C_executionMode == 1){
// std::cout << "Predict" << std::endl;
loadInput(C_inputFile);
loadParams();
// //Comprobamos que se leen bien los archivos
//
// for(unsigned int i = 0; i < C_theta.size(); i++){
// std::cout << C_theta[i] << ' ';
// }
// std::cout << std::endl << std::endl;
//
// for(unsigned int i = 0; i < C_input.getInput().size(); i++){
// std::cout << C_input.getInput()[i] << ' ';
// }
// std::cout << std::endl << C_input.getResult() << std::endl;
//
// //Fin de la comprobacion
predict(C_input);
showParams();
//Escribir lo que devuelve input en algun lado?
}
}
/**
* main is the entry point of the programm. Since this programm accepts params,
* this function has the two default params.
*
* @param aArgc : number of params given to the programm
* @param aArgv : params given to the params
*
* @return int : this function will return 0 and exit the program is no error happened
*/
int main(int aArgc, char** aArgv){
srand ( time(NULL) );
Params* wParams = loadParams(aArgc, aArgv);
makePassword(*wParams);
unloadParams(wParams);
return 0;
}
bool DiffController::init(hardware_interface::VelocityJointInterface* hw, ros::NodeHandle &n)
{
sub_twist_ = n.subscribe("/roundbot/cmd_vel", 1, &DiffController::recvTwist, this);
loadParams(n);
left_target_speed_ = 0.0;
right_target_speed_ = 0.0;
left_joint_ = hw->getHandle(left_joint_name_);
right_joint_ = hw->getHandle(right_joint_name_);
return true;
}
LogitechCamera::LogitechCamera(int argc, char** argv) : Sensor(argc, argv) {
camera_id = std::atoi(argv[1]);
node_name = argv[2];
std::cout<<"camera_id:"<<camera_id<<std::endl;
std::cout<<"node_name:"<<node_name<<std::endl;
ros::init(argc, argv, node_name.c_str());
ros::NodeHandle node_handle;
loadParams(node_handle);
setupComms(node_handle);
}
void exec_loadParams()
{
cc_loadParams();
ser_loadParams();
cam_loadParams();
rcs_loadParams();
ptLoadParams();
loadParams(); // local
}
bool WheelSpeedController::init(hardware_interface::VelocityJointInterface* hw, ros::NodeHandle &n)
{
sub_left_command_ = n.subscribe("left_command", 1, &WheelSpeedController::recvLeftCommand, this);
sub_right_command_ = n.subscribe("right_command", 1, &WheelSpeedController::recvRightCommand, this);
loadParams(n);
left_target_speed_ = 0.0;
right_target_speed_ = 0.0;
left_joint_ = hw->getHandle(left_joint_name_);
right_joint_ = hw->getHandle(right_joint_name_);
return true;
}
void exec_loadParams()
{
cc_loadParams();
ser_loadParams();
cam_loadParams();
#ifndef LEGO
rcs_loadParams();
ptLoadParams();
//chaseLoadParams();
#endif
loadParams(); // local
}
ObstacleDetector::ObstacleDetector(std::string node_name, ros::NodeHandle& node_handle) {
loadParams(node_handle);
this->node_name = node_name;
if (debug_mode > 0) {
cv::namedWindow(std::string("/") + node_name + std::string("/raw_scan"), CV_WINDOW_AUTOSIZE);
cv::namedWindow(std::string("/") + node_name + std::string("/dilate_filter"), CV_WINDOW_AUTOSIZE);
}
obstacle_map = cv::Mat(map_size, map_size, CV_8UC1, cvScalarAll(0));
image_transport = new image_transport::ImageTransport(node_handle);
obstacle_publisher = image_transport->advertise(obstacles_topic_name, 10);
scan_subscriber = node_handle.subscribe(scan_topic_name, 2, &ObstacleDetector::scanCallback, this);
}
int exec_init(Chirp *chirp)
{
g_bMachine = new ButtonMachine;
chirp->registerModule(g_module);
g_runM0 = g_chirpM0->getProc("run", NULL);
g_runningM0 = g_chirpM0->getProc("running", NULL);
g_stopM0 = g_chirpM0->getProc("stop", NULL);
loadParams();
return 0;
}
bool Parameters::loadParams(int argc, char ** argv) {
// load params from commandline args
//if( argc < 3 ) {
// fprintf(stderr, "ERROR: No parameters. Use \"-config\" or \"-f\" to specify configuration file.\n");
// return false;
//}
bool load_from_file = false;
std::set<std::string> setParams;
int jumpBy = 0;
for( int i = 1; i < argc; i += jumpBy ) {
std::string param = argv[i];
if(param[0] != '-') {
std::cerr << "Unknown parameter: " << param << std::endl;
return false;
}
Utils::ltrim(param, "- ");
// normalise parameter to long name
param = normaliseParamName(param);
// check if valid param name
if(!isValidParamName(param)) {
std::cerr << "Unknown param option \"" << param << "\"\n";
exit(EXIT_FAILURE);
}
setParams.insert(param); // needed to not overwrite param value if file is specified
//if the parameter is of type booL no corresponding value
if( getValueType(param) == kBoolValue ) {
jumpBy = 1;
assert(setParamValue(param, kTrueValue));
} else { //not of type bool so must have corresponding value
assert(i+1 < argc);
jumpBy = 2;
std::string val = argv[i+1];
Utils::trim(val);
if( param == "config" )
load_from_file = true;
if(!setParamValue(param, val)) {
std::cerr << "Invalid Param name->value " << param << "->" << val << std::endl;
return false;
}
}
}
bool success = true;
// load from file if specified
if (load_from_file)
success = loadParams(getParamValue("config"), setParams);
return success;
}
double SVMachine::predict(Sample input){
// std::cout << "I'm predicting with the SVMachine" << std::endl;
if(C_executionMode == 1)
loadParams();
ET aux(0.0);
// Hago esto provisional para escalar
std::vector<double> entradaScalada;
std::vector<Sample> sInput;
sInput.push_back(input);
// Utils::scalation(sInput);
arma::Col<double> Input(input.getNFeatures());
for(int i=0; i<input.getNFeatures(); i++)
Input(i)=sInput[0].getInput()[i];
for(int i=0; i<C_trainingSet.size(); i++){
if(C_SupportVectors.at(i) > 0.0){
aux += ET(C_SupportVectors.at(i))*ET(C_y.at(i))*C_kernel->K(C_X.row(i).t(), Input);
// std::cout << "La suma auxiliar vale: " << aux+b << std::endl;
}
}
double p = CGAL::to_double(aux + C_b);
std::cout << "Predigo p=" << p << std::endl;
double treshold = 0.0;
if(p > treshold){
return 1.0;
} else if(p <= treshold){
return -1.0;
}
}
bool AlchemyCreateProfile::processApplication()
{
// process parameters passed in by user
if (!loadParams())
{
getContext() << Context::PRIORITY_error
<< "Application failed while loading parameters"
<< Context::endl;
return false;
}
printParams();
if (!readData(m_trainFile, m_trainData, "training data"))
{
getContext() << Context::PRIORITY_error
<< "Application failed while reading training data"
<< Context::endl;
return false;
}
else if (!m_trainData.size())
{
getContext() << Context::PRIORITY_error
<< "No training data was read; aborting execution"
<< Context::endl;
return false;
}
if (!createProfile())
{
getContext() << Context::PRIORITY_error
<< "Application failed while creating prediction profile"
<< Context::endl;
return false;
}
return true;
}
void Advanced::prepareUi()
{
Selection *s = new Selection(this);
connect(ui->actionOpen, SIGNAL(triggered()), this, SLOT(openRaster()));
connect(ui->actionSave_Reprojection, SIGNAL(triggered()), this, SLOT(saveReprojection()));
connect(ui->actionSelection_Screen, SIGNAL(triggered()), s, SLOT(showSelection()));
connect(ui->actionSelection_Screen, SIGNAL(triggered()), this, SLOT(close()));
connect(ui->actionExit, SIGNAL(triggered()), this, SLOT(close()));
connect(ui->actionLoad_Projection_Info, SIGNAL(triggered()), this, SLOT(loadParams()));
connect(ui->actionSave_Projection_Info, SIGNAL(triggered()), this, SLOT(saveParams()));
connect(ui->actionToggle_Preview, SIGNAL(triggered()), this, SLOT(togglePreview()));
connect(ui->actionAbout_dRasterBlaster, SIGNAL(triggered()), s, SLOT(about()));
connect(ui->actionAbout_Qt, SIGNAL(triggered()), s, SLOT(aboutQt()));
connect(ui->actionEdit_Author, SIGNAL(triggered()), s, SLOT(showEditAuthor()));
connect(ui->actionUser_Guide, SIGNAL(triggered()), s, SLOT(showUserGuide()));
connect(ui->fillEnable, SIGNAL(stateChanged(int)), this, SLOT(fillEnable(int)));
connect(ui->noDataValueEnable, SIGNAL(stateChanged(int)), this, SLOT(noDataEnable(int)));
//Validators
QIntValidator *intValid = new QIntValidator(this);
intValid->setBottom(0);
QDoubleValidator *doubleValid = new QDoubleValidator(this);
doubleValid->setNotation(QDoubleValidator::StandardNotation);
doubleValid->setBottom(0.0);
ui->Rows->setValidator(intValid);
ui->Cols->setValidator(intValid);
ui->FillValue->setValidator(intValid);
ui->noDataValue->setValidator(intValid);
ui->pixelSize->setValidator(doubleValid);
//projections p;
//p.callGenerate(_UTM);
//ui->tabProjectionInfo->setLayout(p.projVLayout);
}
CHOMPInterfaceROS::CHOMPInterfaceROS(const planning_models::RobotModelConstPtr& kmodel) :
ChompPlanner(kmodel), nh_("~")
{
loadParams();
}
void Interpreter::handleData(const void *args[])
{
int i;
uint8_t type;
QColor color = CW_DEFAULT_COLOR;
if (args[0])
{
type = Chirp::getType(args[0]);
if (type==CRP_TYPE_HINT)
{
if (g_debug)
m_print += printType(*(uint32_t *)args[0]) + " data\n";
if (*(uint32_t *)args[0]==FOURCC('E','V','T','1'))
{
uint32_t event = *(uint32_t *)args[1];
if (event==EVT_PARAM_CHANGE)
loadParams();
}
else
{
// check modules, see if they handle the fourcc
for (i=0; i<m_modules.size(); i++)
{
if (m_modules[i]->render(*(uint32_t *)args[0], args+1))
break;
}
}
}
else if (type==CRP_HSTRING)
{
m_print += (char *)args[0];
color = Qt::blue;
}
else
DBG("unknown type: %d", type);
}
if (m_print.size()>0 && !m_print.endsWith('\n'))
m_print += '\n';
// wait queue business keeps worker thread from getting too far ahead of gui thread
// (when this happens, things can get sluggish.)
// update: this causes the worker thread to block. For example, bringing up a file
// dialog will cause the gui to block and a block in the gui causes ths console to block
// the console blocks and the xdata console print blocks, blocking the worker thread.
// Removing this fixes the issue. But it's now possible for the console prints to queue
// up if they are coming in too fast, causing gui sluggishness. Limit size of queue?
// Need some kind of throttling mechanism -- putting sleeps in the worker thread? Or
// a call somewhere to processevents?
#if 0
if (m_localProgramRunning || m_running)
m_console->m_mutexPrint.lock();
#endif
if (m_print.size()>0)
{
emit textOut(m_print, color);
m_print = "";
}
#if 0
if (m_localProgramRunning || m_running)
{
m_console->m_waitPrint.wait(&m_console->m_mutexPrint);
m_console->m_mutexPrint.unlock();
}
#endif
}
void GodrayTool::onReset(){
wt::Scene::GodRayParams params;
WTE_CTX.getScene()->setGodRayParams(params);
loadParams();
}
void GodrayTool::onSceneLoaded(){
loadParams();
}
void FrameGrabber<Camera>::
initialise()
{
loadParams();
frame_data.disp.create(frame_data.cam_vec[0].image_size(),
CV_32F);
#ifdef SCAVISLAM_CUDA_SUPPORT
frame_data.gpu_disp_32f.create(frame_data.cam_vec[0].image_size(),
CV_32F);
for (int l=0; l<NUM_PYR_LEVELS; ++l)
{
frame_data.cur_left().gpu_pyr_float32[l]
.create(frame_data.cam_vec[l].image_size(), CV_32FC1);
frame_data.gpu_pyr_float32_dx[l]
.create(frame_data.cam_vec[l].image_size(), CV_32FC1);
frame_data.gpu_pyr_float32_dy[l]
.create(frame_data.cam_vec[l].image_size(), CV_32FC1);
frame_data.prev_left().gpu_pyr_float32[l]
.create(frame_data.cam_vec[l].image_size(), CV_32FC1);
frame_data.gpu_pyr_float32_dx[l]
.create(frame_data.cam_vec[l].image_size(), CV_32FC1);
frame_data.gpu_pyr_float32_dy[l]
.create(frame_data.cam_vec[l].image_size(), CV_32FC1);
}
frame_data.right.gpu_pyr_float32[0]
.create(frame_data.cam_vec[0].image_size(), CV_32FC1);
#else
frame_data.pyr_float32.resize(NUM_PYR_LEVELS);
frame_data.pyr_float32_dx.resize(NUM_PYR_LEVELS);
frame_data.pyr_float32_dy.resize(NUM_PYR_LEVELS);
for (int l=0; l<NUM_PYR_LEVELS; ++l)
{
frame_data.pyr_float32[l]
.create(frame_data.cam_vec[l].image_size(), CV_32FC1);
frame_data.pyr_float32_dx[l]
.create(frame_data.cam_vec[l].image_size(), CV_32FC1);
frame_data.pyr_float32_dy[l]
.create(frame_data.cam_vec[l].image_size(), CV_32FC1);
}
#endif
if (params_.livestream)
{
#ifdef SCAVISLAM_PCL_SUPPORT
grabber.initialize();
boost::thread(boost::ref(grabber));
#else
assert(false);
#endif
}
else
{
//preprocessFiles(params_.path_str,true);
file_grabber_.initialize(params_.path_str,
params_.base_str,
params_.format_str,
frame_data.frame_id,
params_.color_img,
!params_.color_img,
params_.right_img,
params_.disp_img,
params_.right_img,
&file_grabber_mon_);
if (params_.rectify_frame)
{
intializeRectifier();
}
}
#ifdef SCAVISLAM_CUDA_SUPPORT
dx_filter_ =
cv::gpu::createDerivFilter_GPU(frame_data.cur_left()
.gpu_pyr_float32[0].type(),
frame_data.gpu_pyr_float32_dx[0]
.type(),
1, 0, 1, cv::BORDER_REPLICATE);
dy_filter_ =
cv::gpu::createDerivFilter_GPU(frame_data.cur_left()
.gpu_pyr_float32[0].type(),
frame_data.gpu_pyr_float32_dy[0]
.type(),
0, 1, 1, cv::BORDER_REPLICATE);
#endif
processNextFrame();
}
std::shared_ptr<storage::AbstractTable> Loader::shortcuts::load(const std::string& filepath, Loader::params& params) {
return Loader::load(loadParams(filepath, params));
};
bool loadParams(int argc, char ** argv, std::string& worldFilename, std::string& robotFilename,
std::string& objectFilename, std::string& outputDirectory, bool& saveSeparate, Eigen::Vector3d& objPos,
int& maxIterations)
{
saveSeparate = false;
worldFilename.clear();
robotFilename.clear();
objectFilename.clear();
outputDirectory.clear();
boost::program_options::variables_map vm;
try
{
vm = loadParams(argc, argv);
}
catch (std::exception const& e)
{
PRINTERROR("Exception caught: " << e.what());
return false;
}
catch (...)
{
PRINTERROR("Exception caught");
return false;
}
boost::program_options::options_description desc = getOptions();
// desc=getOptions();
if (vm.count("help"))
{
PRINTMSG(desc);
return false;
}
if (vm.count("dir") < 1)
{
PRINTERROR("Must specify an output directory");
PRINTMSG(desc);
return false;
}
if (vm.count("wld") && (vm.count("rob") || vm.count("obj")))
{
PRINTERROR("Cannot specify a world and a robot and/or object at the same time.");
PRINTMSG(desc);
return false;
}
if (!vm.count("wld") && !vm.count("rob"))
{
PRINTERROR("Have to specify either a robot or a world.");
PRINTMSG(desc);
return false;
}
if (vm.count("rob") != vm.count("obj"))
{
PRINTERROR("If you specify a robot, you also have to specify an object, and vice versa.");
PRINTMSG(desc);
return false;
}
if (vm.count("rob") > 1)
{
PRINTERROR("You can only specify one robot at this stage.");
PRINTMSG(desc);
return false;
}
if (vm.count("obj") > 1)
{
PRINTERROR("You can only specify one object at this stage.");
PRINTMSG(desc);
return false;
}
if (vm.count("obj") != vm.count("rob"))
{
PRINTERROR("If you specify a robot, you should also specify an object.");
PRINTMSG(desc);
return false;
}
if (vm.count("wld"))
{
worldFilename = vm["wld"].as<std::string>();
PRINTMSG("World file is " << worldFilename);
}
if (vm.count("rob"))
{
robotFilename = vm["rob"].as<std::string>();
PRINTMSG("Robot file is " << robotFilename);
}
if (vm.count("obj"))
{
objectFilename = vm["obj"].as<std::string>();
PRINTMSG("Object file is " << objectFilename);
}
if (vm.count("dir"))
{
outputDirectory = vm["dir"].as<std::string>();
PRINTMSG("Output dir is " << outputDirectory);
}
if (vm.count("iter"))
{
maxIterations = vm["iter"].as<int>();
PRINTMSG("Number of iterations: " << maxIterations);
if (maxIterations < 35000)
{
PRINTWARN("Planning is not working well with max iterations < 35000");
}
}
if (vm.count("obj-pos"))
{
std::vector<float> vals=vm["obj-pos"].as<std::vector<float> >();
if (vals.size()!=3)
{
PRINTERROR("Must specify 3 values for --obj-pos: x, y and z (specified "<<vals.size()<<")");
PRINTMSG(desc);
}
PRINTMSG("Using initial object pose "<<vals[0]<<", "<<vals[1]<<", "<<vals[2]);
objPos=Eigen::Vector3d(vals[0],vals[1],vals[2]);
}
if (vm.count("save-separate"))
{
saveSeparate=true;
}
return true;
}
Parameters::Parameters(int argc, char ** argv, const ParamDefs * paramdefs,
const count_t paramNum) {
initialize(paramdefs, paramNum);
loadParams(argc, argv);
}
int wrap_loadParams(cgiRequestObj *request, char* (*getenv2)(const char*, void* thread_context),
char *raw_post_data, ms_uint32 raw_post_data_length, void* thread_context) {
return loadParams(request, getenv2, raw_post_data, raw_post_data_length, thread_context);
}
bool loadParams(int argc, char ** argv, std::string& worldFilename, std::string& robotFilename,
std::string& objectFilename, std::string& outputDirectory)
{
worldFilename.clear();
robotFilename.clear();
objectFilename.clear();
outputDirectory.clear();
boost::program_options::variables_map vm;
try
{
vm = loadParams(argc, argv);
}
catch (std::exception const& e)
{
PRINTERROR("Exception caught: " << e.what());
return false;
}
catch (...)
{
PRINTERROR("Exception caught");
return false;
}
boost::program_options::options_description desc = getOptions();
// desc=getOptions();
if (vm.count("help"))
{
PRINTMSG(desc);
return false;
}
if (vm.count("dir") < 1)
{
PRINTERROR("Must specify an output directory");
PRINTMSG(desc);
return false;
}
if (vm.count("wld") && (vm.count("rob") || vm.count("obj")))
{
PRINTERROR("Cannot specify a world and a robot and/or object at the same time.");
PRINTMSG(desc);
return false;
}
if (!vm.count("wld") && !vm.count("rob"))
{
PRINTERROR("Have to specify either a robot or a world.");
PRINTMSG(desc);
return false;
}
if (vm.count("rob") != vm.count("obj"))
{
PRINTERROR("If you specify a robot, you also have to specify an object, and vice versa.");
PRINTMSG(desc);
return false;
}
if (vm.count("rob") > 1)
{
PRINTERROR("You can only specify one robot at this stage.");
PRINTMSG(desc);
return false;
}
if (vm.count("obj") > 1)
{
PRINTERROR("You can only specify one object at this stage.");
PRINTMSG(desc);
return false;
}
if (vm.count("obj") != vm.count("rob"))
{
PRINTERROR("If you specify a robot, you should also specify an object.");
PRINTMSG(desc);
return false;
}
if (vm.count("wld"))
{
worldFilename = vm["wld"].as<std::string>();
PRINTMSG("World file is " << worldFilename);
}
if (vm.count("rob"))
{
robotFilename = vm["rob"].as<std::string>();
PRINTMSG("Robot file is " << robotFilename);
}
if (vm.count("obj"))
{
objectFilename = vm["obj"].as<std::string>();
PRINTMSG("Object file is " << objectFilename);
}
if (vm.count("dir"))
{
outputDirectory = vm["dir"].as<std::string>();
PRINTMSG("Output dir is " << outputDirectory);
}
return true;
}
int main(int argc, char **argv)
{
signal(SIGSEGV, handler);
signal(SIGABRT, handler);
PRINT_INIT_STD();
std::string worldFilename;
std::string robotFilename;
std::string objectFilename;
std::string outputDirectory;
bool saveSeparate;
Eigen::Vector3d objPos;
int maxPlanningSteps = 50000;
if (!loadParams(argc, argv, worldFilename, robotFilename, objectFilename, outputDirectory, saveSeparate, objPos, maxPlanningSteps))
{
PRINTERROR("Could not read arguments");
return 1;
}
PRINTMSG("Creating planner");
std::string name = "EigenGraspPlanner1"; // TODO make parameter
SHARED_PTR<GraspIt::GraspItSceneManager> graspitMgr(new GraspIt::GraspItSceneManagerHeadless());
#ifdef USE_EIGENGRASP_NOQT
SHARED_PTR<GraspIt::EigenGraspPlannerNoQt> p(new GraspIt::EigenGraspPlannerNoQt(name, graspitMgr));
#else
SHARED_PTR<GraspIt::EigenGraspPlanner> p(new GraspIt::EigenGraspPlanner(name, graspitMgr));
#endif
// TODO parameterize:
// Names for robot and object if not loaded from a world file.
// If loaded from a world file, will be overwritten.
std::string useRobotName="Robot1";
std::string useObjectName="Object1";
if (!worldFilename.empty())
{
PRINTMSG("Loading world");
graspitMgr->loadWorld(worldFilename);
std::vector<std::string> robs = graspitMgr->getRobotNames();
std::vector<std::string> objs = graspitMgr->getObjectNames(true);
if (robs.empty())
{
PRINTERROR("No robots loaded");
return 1;
}
if (objs.empty())
{
PRINTERROR("No graspable objects loaded");
return 1;
}
if (robs.size()!=1)
{
PRINTERROR("Exactly 1 robot should have been loaded");
return 1;
}
if (objs.size()!=1)
{
PRINTERROR("Exactly 1 graspable object should have been loaded");
return 1;
}
useRobotName=robs.front();
useObjectName=objs.front();
PRINTMSG("Using robot "<<useRobotName<<" and object "<<useObjectName);
}
else
{
// TODO add an option to set the transforms.
// For now, they're put in the origin. For the planning, this should not really matter...
GraspIt::EigenTransform robotTransform;
GraspIt::EigenTransform objectTransform;
robotTransform.setIdentity();
objectTransform.setIdentity();
objectTransform.translate(objPos);
// objectTransform.translate(Eigen::Vector3d(100,0,0));
std::string robotName(useRobotName);
std::string objectName(useObjectName);
if ((graspitMgr->loadRobot(robotFilename, robotName, robotTransform) != 0) ||
(graspitMgr->loadObject(objectFilename, objectName, true, objectTransform)))
{
PRINTERROR("Could not load robot or object");
return 1;
}
}
bool createDir = true;
bool saveIV = true;
bool forceWrite = createDir; // only enforce if creating dir is also allowed
// in case one wants to view the initial world before planning, save it:
graspitMgr->saveGraspItWorld(outputDirectory + "/startWorld.xml", createDir);
graspitMgr->saveInventorWorld(outputDirectory + "/startWorld.iv", createDir);
if (saveSeparate)
{
graspitMgr->saveRobotAsInventor(outputDirectory + "/robotStartpose.iv", useRobotName, createDir, forceWrite);
graspitMgr->saveObjectAsInventor(outputDirectory + "/object.iv", useObjectName, createDir, forceWrite);
}
int repeatPlanning = 1;
int keepMaxPlanningResults = 3;
bool finishWithAutograsp = false;
p->plan(maxPlanningSteps, repeatPlanning, keepMaxPlanningResults, finishWithAutograsp);
PRINTMSG("Saving results as world files");
bool saveWorld = true;
std::string resultsWorldDirectory = outputDirectory;
std::string filenamePrefix = "world";
p->saveResultsAsWorldFiles(resultsWorldDirectory, filenamePrefix, saveWorld, saveIV, createDir, saveSeparate);
std::vector<GraspIt::EigenGraspResult> allGrasps;
p->getResults(allGrasps);
PRINTMSG("Grasp results:");
std::vector<GraspIt::EigenGraspResult>::iterator it;
for (it = allGrasps.begin(); it != allGrasps.end(); ++it)
{
PRINTMSG(*it);
}
PRINTMSG("Quitting program.");
return 1;
}
ompl_interface_ros::OMPLInterfaceROS::OMPLInterfaceROS(const robot_model::RobotModelConstPtr &kmodel) :
ompl_interface::OMPLInterface(kmodel),
nh_("~")
{
loadParams();
}
