PathPlannerSVC_impl.cpp
Go to the documentation of this file.
00001 // -*-C++-*-
00008 #include "PathPlannerSVC_impl.h"
00009 
00010 #include <hrpPlanner/Roadmap.h>
00011 #include <hrpPlanner/RoadmapNode.h>
00012 #include <hrpPlanner/TimeUtil.h>
00013 
00014 /*
00015  * Implementational code for IDL interface OpenHRP::PathPlanner
00016  */
00017 OpenHRP_PathPlannerSVC_impl::OpenHRP_PathPlannerSVC_impl()
00018 {
00019     path_ = new PathEngine::PathPlanner(3);
00020     PathEngine::ConfigurationSpace *cspace 
00021         = path_->getConfigurationSpace();
00022     cspace->unboundedRotation(2, true);
00023     cspace->bounds(0,-2,2);
00024     cspace->bounds(1,-2,2);
00025 }
00026 
00027 
00028 OpenHRP_PathPlannerSVC_impl::~OpenHRP_PathPlannerSVC_impl()
00029 {
00030     delete path_;
00031 }
00032 
00033 void OpenHRP_PathPlannerSVC_impl::stopPlanning()
00034 {
00035     path_->stopPlanning();
00036 }
00037 
00038 /*
00039  * Methods corresponding to IDL attributes and operations
00040  */
00041 void OpenHRP_PathPlannerSVC_impl::getRoadmap(OpenHRP::PathPlanner::Roadmap_out graph)
00042 {
00043     std::cout << "getRoadmap()" << std::endl;
00044 
00045     PathEngine::RoadmapPtr roadmap = path_->getRoadmap();
00046 
00047     graph = new OpenHRP::PathPlanner::Roadmap;
00048     std::cout << "the number of nodes = " << roadmap->nNodes() << std::endl;
00049     graph->length(roadmap->nNodes());
00050   
00051     for (unsigned int i=0; i<roadmap->nNodes(); i++) {
00052         PathEngine::RoadmapNodePtr node = roadmap->node(i);
00053         const PathEngine::Configuration& pos = node->position(); 
00054         graph[i].cfg[0] = pos.value(0);
00055         graph[i].cfg[1] = pos.value(1);
00056         graph[i].cfg[2] = pos.value(2);
00057 
00058         graph[i].neighbors.length(node->nChildren());
00059         for (unsigned int j=0; j<node->nChildren(); j++) {
00060             graph[i].neighbors[j] = roadmap->indexOfNode(node->child(j));
00061         }
00062     }
00063 }
00064 
00065 void OpenHRP_PathPlannerSVC_impl::clearRoadmap()
00066 {
00067     PathEngine::RoadmapPtr rm = path_->getRoadmap();
00068     rm->clear();
00069 }
00070 
00071 void OpenHRP_PathPlannerSVC_impl::getMobilityNames(OpenHRP::StringSequence_out mobilities)
00072 {
00073     mobilities = new OpenHRP::StringSequence;
00074     std::vector<std::string> mobilityNames;
00075     path_->getMobilityNames(mobilityNames);
00076     mobilities->length(mobilityNames.size());
00077 
00078     for (unsigned int i=0; i<mobilityNames.size(); i++) {
00079         mobilities[i] = CORBA::string_dup(mobilityNames[i].c_str());
00080     }
00081 }
00082 
00083 void OpenHRP_PathPlannerSVC_impl::getOptimizerNames(OpenHRP::StringSequence_out optimizers)
00084 {
00085     optimizers = new OpenHRP::StringSequence;
00086     std::vector<std::string> optimizerNames;
00087     path_->getOptimizerNames(optimizerNames);
00088     optimizers->length(optimizerNames.size());
00089 
00090     for (unsigned int i=0; i<optimizerNames.size(); i++) {
00091         optimizers[i] = CORBA::string_dup(optimizerNames[i].c_str());
00092     }
00093 }
00094 
00095 void OpenHRP_PathPlannerSVC_impl::setRobotName(const char* model)
00096 {
00097     path_->setRobotName(model);
00098 }
00099 
00100 void OpenHRP_PathPlannerSVC_impl::setAlgorithmName(const char* algorithm)
00101 {
00102     path_->setAlgorithmName(algorithm);
00103 }
00104 
00105 bool OpenHRP_PathPlannerSVC_impl::setMobilityName(const char* mobility)
00106 {
00107     return path_->setMobilityName(mobility);
00108 }
00109 
00110 void OpenHRP_PathPlannerSVC_impl::getAlgorithmNames(OpenHRP::StringSequence_out algos)
00111 {
00112     algos = new OpenHRP::StringSequence;
00113     std::vector<std::string> algoNames;
00114     path_->getAlgorithmNames(algoNames);
00115     algos->length(algoNames.size());
00116 
00117     for (unsigned int i=0; i<algoNames.size(); i++) {
00118         algos[i] = CORBA::string_dup(algoNames[i].c_str());
00119     }
00120 }
00121 
00122 bool OpenHRP_PathPlannerSVC_impl::getProperties(const char* alg, OpenHRP::StringSequence_out props, OpenHRP::StringSequence_out defaults)
00123 {
00124     props = new OpenHRP::StringSequence;
00125     defaults = new OpenHRP::StringSequence;
00126 
00127 
00128     std::vector<std::string> propNames;
00129     std::vector<std::string> defaultValues;
00130 
00131     propNames.push_back("weight-x");
00132     defaultValues.push_back("1.0");
00133     propNames.push_back("weight-y");
00134     defaultValues.push_back("1.0");
00135     propNames.push_back("weight-theta");
00136     defaultValues.push_back("1.0");
00137     
00138     propNames.push_back("min-x");
00139     defaultValues.push_back("-2");
00140     propNames.push_back("min-y");
00141     defaultValues.push_back("-2");
00142     propNames.push_back("max-x");
00143     defaultValues.push_back("2");
00144     propNames.push_back("max-y");
00145     defaultValues.push_back("2");
00146     
00147     if (!path_->getProperties(alg, propNames, defaultValues)) return false;
00148 
00149     props->length(propNames.size());
00150     defaults->length(propNames.size());
00151 
00152     for (unsigned int i=0; i<propNames.size(); i++) {
00153         props[i] = CORBA::string_dup(propNames[i].c_str());
00154         defaults[i] = CORBA::string_dup(defaultValues[i].c_str());
00155     }
00156     return true;
00157 }
00158 
00159 void OpenHRP_PathPlannerSVC_impl::initPlanner()
00160 {
00161     std::cout << "initPlanner()" << std::endl;
00162     path_->initPlanner(nameServer_);
00163     std::cout << "fin. " << std::endl;
00164 }
00165 
00166 void OpenHRP_PathPlannerSVC_impl::setStartPosition(CORBA::Double x, CORBA::Double y, CORBA::Double theta)
00167 {
00168     std::cout << "setStartPosition(" << x << ", " << y << ", " << theta << ")" << std::endl;
00169     PathEngine::Configuration pos(path_->getConfigurationSpace()->size());
00170     pos.value(0) = x;
00171     pos.value(1) = y;
00172     pos.value(2) = theta;
00173     path_->setStartConfiguration(pos);
00174     std::cout << "fin. " << std::endl;
00175 }
00176 
00177 void OpenHRP_PathPlannerSVC_impl::setGoalPosition(CORBA::Double x, CORBA::Double y, CORBA::Double theta)
00178 {
00179     std::cout << "setGoalPosition(" << x << ", " << y << ", " << theta << ")" << std::endl;
00180     PathEngine::Configuration pos(path_->getConfigurationSpace()->size());
00181     pos.value(0) = x;
00182     pos.value(1) = y;
00183     pos.value(2) = theta;
00184     path_->setGoalConfiguration(pos);
00185     std::cout << "fin. " << std::endl;
00186 }
00187 
00188 void OpenHRP_PathPlannerSVC_impl::setProperties(const OpenHRP::PathPlanner::Property& properites)
00189 {
00190     std::cout << "setProperties()" << std::endl;
00191     std::map<std::string, std::string> prop;
00192     for (unsigned int i=0; i<properites.length(); i++) {
00193         std::string name(properites[i][0]);
00194         std::string value(properites[i][1]);
00195         PathEngine::ConfigurationSpace *cspace = 
00196             path_->getConfigurationSpace();
00197         //std::cout << name << ": " << value << std::endl;
00198         if (name == "min-x"){
00199             cspace->lbound(0) = atof(value.c_str());
00200         }else if (name == "max-x"){
00201             cspace->ubound(0) = atof(value.c_str());
00202         }else if (name == "min-y"){
00203             cspace->lbound(1) = atof(value.c_str());
00204         }else if (name == "max-y"){
00205             cspace->ubound(1) = atof(value.c_str());
00206         }else if (name == "weight-x"){
00207             cspace->weight(0) = atof(value.c_str());
00208         }else if (name == "weight-y"){
00209             cspace->weight(1) = atof(value.c_str());
00210         }else if (name == "weight-theta"){
00211             cspace->weight(2) = atof(value.c_str());
00212         }else{
00213             prop.insert(std::map<std::string, std::string>::value_type(name, value));
00214         }
00215     }
00216     path_->setProperties(prop);
00217 
00218     std::cout << "fin. " << std::endl;
00219 }
00220 
00221 CORBA::Boolean OpenHRP_PathPlannerSVC_impl::calcPath()
00222 {
00223     std::cout << "OpenHRP_PathPlannerSVC_impl::calcPath()" << std::endl;
00224     tick_t t1 = get_tick(); 
00225     bool status = path_->calcPath();
00226     std::cout << "OpenHRP_PathPlannerSVC_impl::fin." << std::endl;
00227     std::cout << "total computation time = " << tick2sec(get_tick()-t1) << "[s]" 
00228               << std::endl;
00229     std::cout << "computation time for collision check = " 
00230               << path_->timeCollisionCheck() << "[s]" << std::endl;
00231     std::cout << "computation time for forward kinematics = " 
00232               << path_->timeForwardKinematics() << "[s]" << std::endl;
00233     std::cout << "collision check function was called " 
00234               << path_->countCollisionCheck() << " times" << std::endl;
00235     return status;
00236 }
00237 
00238 
00239 void OpenHRP_PathPlannerSVC_impl::getPath(OpenHRP::PathPlanner::PointArray_out path)
00240 {
00241     std::cerr << "OpenHRP_PathPlannerSVC_impl::getPath()" << std::endl;
00242     const std::vector<PathEngine::Configuration>& p = path_->getPath();
00243 
00244     path = new OpenHRP::PathPlanner::PointArray;
00245     path->length(p.size());
00246     std::cout << "length of path = " << p.size() << std::endl;
00247     for (unsigned int i=0; i<p.size(); i++) {
00248         //std::cerr << i << " : " << p[i] << std::endl;
00249         path[i].length(3);
00250         path[i][0] = p[i].value(0);
00251         path[i][1] = p[i].value(1);
00252         path[i][2] = p[i].value(2);
00253     }
00254     std::cerr << "OpenHRP_PathPlannerSVC_impl::fin. length of path = " 
00255               << p.size() << std::endl;
00256 }
00257 
00258 void OpenHRP_PathPlannerSVC_impl::registerIntersectionCheckPair(const char* char1, const char* name1, const char* char2, const char* name2, CORBA::Double tolerance)
00259 {
00260     path_->registerIntersectionCheckPair(char1,
00261                                          name1,
00262                                          char2,
00263                                          name2,
00264                                          tolerance);
00265 }
00266 
00267 void OpenHRP_PathPlannerSVC_impl::registerCharacter(const char* name, OpenHRP::BodyInfo_ptr cInfo)
00268 {
00269     path_->registerCharacter(name, cInfo);
00270 }
00271 
00272 void OpenHRP_PathPlannerSVC_impl::registerCharacterByURL(const char* name, const char* url)
00273 {
00274     path_->registerCharacterByURL(name, url);
00275 }
00276 
00277 void OpenHRP_PathPlannerSVC_impl::setCharacterPosition(const char* character,
00278                                                        const OpenHRP::DblSequence& pos)
00279 {
00280     path_->setCharacterPosition(character, pos);
00281 } 
00282 
00283 
00284 void OpenHRP_PathPlannerSVC_impl::initSimulation()
00285 {
00286     std::cout << "initSimulation()" << std::endl;
00287     path_->initSimulation();
00288     std::cout << "fin. " << std::endl;
00289 }
00290 
00291 // End of example implementational code
00292 
00293 
00294 CORBA::Boolean OpenHRP_PathPlannerSVC_impl::optimize(const char *optimizer)
00295 {
00296     return path_->optimize(optimizer);
00297 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:18