Adding parameter server access with buildReducedModel.

As this stage the main test is not working.

include/sot/pattern-generator/pg.h

@@ -189,9 +189,13 @@ class PatternGenerator_EXPORT PatternGenerator : public Entity {
     @{
   */
 
-  /*! \brief Build the pattern generator interface from a Urdf
-    and SRDF file. */
-  bool buildModel(void);
+  /*! \brief Build the pattern generator interface from the parameter
+    "/robot_description" and the informations in
+    "/robot/specifificities/feet/[right|left]/[size|anklePosition]*/
+  bool buildPGI(void);
+
+  /*! \brief Build the reduced model. */
+  bool buildReducedModel(void);
 
   /*! \brief Initialize the state of the robot. */
   bool InitState(void);

src/pg.cpp

@@ -640,10 +640,11 @@ bool PatternGenerator::InitState(void) {
   return true;
 }
 
-bool PatternGenerator::buildModel(void) {
-  bool ok = true;
+bool PatternGenerator::buildReducedModel(void) {
+
   // Name of the parameter
-  std::string lparameter_name("robot_description");
+  std::string lparameter_name("/robot_description");
+
   // Model of the robot inside a string.
   std::string lrobot_description;
 
@@ -662,12 +663,18 @@ bool PatternGenerator::buildModel(void) {
         << "The robot with name " << model_name
         << " was not found !";
     throw std::invalid_argument(oss.str());
-    ok=false;
     return false;
   }
 
-  // Then build a complete robot model.
-  lrobot_description = aRobotUtil->get_parameter<string>(lparameter_name);
+  try {
+    // Then build a complete robot model.
+    lrobot_description = aRobotUtil->get_parameter<string>(lparameter_name);
+  }  catch (...) {
+    SOT_THROW ExceptionPatternGenerator(
+        ExceptionPatternGenerator::PATTERN_GENERATOR_JRL,
+        "Error while getting parameter " + lparameter_name + " for the WPG.");
+    return false;
+  }
 
   pinocchio::Model lrobotModel;
   pinocchio::urdf::buildModelFromXML(lrobot_description,
@@ -685,7 +692,8 @@ bool PatternGenerator::buildModel(void) {
   for(auto it : list_of_joints_to_lock_by_name)
   {
     const std::string & joint_name = it;
-    if(m_robotModel.existJointName(joint_name)) // do not consider joint that are not in the model
+    if(m_robotModel.existJointName(joint_name))
+      // do not consider joint that are not in the model
       list_of_joints_to_lock_by_id.
           push_back(m_robotModel.getJointId(joint_name));
   }
@@ -693,49 +701,65 @@ bool PatternGenerator::buildModel(void) {
                                               list_of_joints_to_lock_by_id,
                                               q_neutral);
 
+  return true;
+}
+
+
+bool PatternGenerator::buildPGI(void) {
+
+  bool ok = true;
+
+  // Build the reduced model of the robot
+  buildReducedModel();
 
   // Creating the humanoid robot.
   m_PR = new pg::PinocchioRobot();
   m_PR->initializeRobotModelAndData(&m_robotModel, m_robotData);
+
   // Read xml/srdf stream
-  std::ifstream srdf_stream(m_srdfFile.c_str());
   using boost::property_tree::ptree;
   ptree pt;
   try {
-    read_xml(srdf_stream, pt);
+
     // Initialize the Right Foot
     pg::PRFoot aFoot;
-    string path = "robot.specificities.feet.right.size";
+    string path = "/robot/specificities/feet/right/size";
     BOOST_FOREACH (const ptree::value_type &v, pt.get_child(path.c_str())) {
       aFoot.soleHeight = v.second.get<double>("height");
       aFoot.soleWidth = v.second.get<double>("width");
       aFoot.soleDepth = v.second.get<double>("depth");
-    }  // BOOST_FOREACH
-    path = "robot.specificities.feet.right.anklePosition";
+    }
+
+    path = "/robot/specificities/feet/right/anklePosition";
     BOOST_FOREACH (const ptree::value_type &v, pt.get_child(path.c_str())) {
       aFoot.anklePosition(0) = v.second.get<double>("x");
       aFoot.anklePosition(1) = v.second.get<double>("y");
       aFoot.anklePosition(2) = v.second.get<double>("z");
-    }  // BOOST_FOREACH
+    }
+
     pinocchio::FrameIndex ra = m_robotModel.getFrameId("r_ankle");
     aFoot.associatedAnkle = m_robotModel.frames.at(ra).parent;
     m_PR->initializeRightFoot(aFoot);
+
     // Initialize the Left Foot
-    path = "robot.specificities.feet.left.size";
+    path = "/robot/specificities/feet/left/size";
     BOOST_FOREACH (const ptree::value_type &v, pt.get_child(path.c_str())) {
       aFoot.soleHeight = v.second.get<double>("height");
       aFoot.soleWidth = v.second.get<double>("width");
       aFoot.soleDepth = v.second.get<double>("depth");
     }  // BOOST_FOREACH
-    path = "robot.specificities.feet.left.anklePosition";
+
+    path = "/robot/specificities/feet/left/anklePosition";
     BOOST_FOREACH (const ptree::value_type &v, pt.get_child(path.c_str())) {
       aFoot.anklePosition(0) = v.second.get<double>("x");
       aFoot.anklePosition(1) = v.second.get<double>("y");
       aFoot.anklePosition(2) = v.second.get<double>("z");
     }  // BOOST_FOREACH
+
     pinocchio::FrameIndex la = m_robotModel.getFrameId("l_ankle");
     aFoot.associatedAnkle = m_robotModel.frames.at(la).parent;
     m_PR->initializeLeftFoot(aFoot);
+
   } catch (...) {
     cerr << "problem while reading the srdf file. File corrupted?" << endl;
     ok = false;
@@ -1682,8 +1706,9 @@ void PatternGenerator::initCommands(void) {
       "buildModel",
       makeCommandVoid0(
           *this,
-          (void (PatternGenerator::*)(void)) & PatternGenerator::buildModel,
-          docCommandVoid0("From the files, parse and build.")));
+          (void (PatternGenerator::*)(void)) & PatternGenerator::buildPGI,
+          docCommandVoid0("From the files, parse and build the robot model and"
+                          " the Walking Pattern Generator.")));
   addCommand(
       "initState",
       makeCommandVoid0(

tests/CMakeLists.txt

@@ -5,17 +5,7 @@
 ADD_DEFINITIONS(-DDEBUG=2)
 
 ADD_UNIT_TEST(main_test main.cc)
+
 TARGET_LINK_LIBRARIES(main_test
-  jrl-walkgen::jrl-walkgen
-  sot-core::sot-core
-  next-step
-  next-step-pg-sot
-  step-checker
-  step-computer-joystick
-  step-observer
-  step-queue
-  step-time-line
-  which-foot-upper
   pg
-  pg-manager
   )

tests/main.cc

 /*
  *  Copyright
  */
+#include <iostream>
+#include <string>
+#include <sstream>
+#include <algorithm>
+#include <iterator>
 
+#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
+#pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
+#undef BOOST_MPL_LIMIT_VECTOR_SIZE
+#define BOOST_MPL_LIMIT_VECTOR_SIZE_PUSH
+#endif
+
+#ifdef BOOST_MPL_LIMIT_LIST_SIZE
+#pragma push_macro("BOOST_MPL_LIMIT_LIST_SIZE")
+#undef BOOST_MPL_LIMIT_LIST_SIZE
+#define BOOST_MPL_LIMIT_LIST_SIZE_PUSH
+#endif
+
+#include <boost/property_tree/ptree.hpp>
+
+#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE_PUSH
+#pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
+#endif
+
+#ifdef BOOST_MPL_LIMIT_LIST_SIZE_PUSH
+#pragma pop_macro("BOOST_MPL_LIMIT_LIST_SIZE")
+#endif
+
+#include <boost/algorithm/string.hpp>
+#include <boost/filesystem.hpp>
 #include <sot/pattern-generator/pg.h>
+#include <sot/core/debug.hh>
+#include <sot/core/robot-utils.hh>
 
 int main(int, char**) {
+  using namespace std;
   dynamicgraph::sot::PatternGenerator aPG;
 
-  std::string aRobotURDF(
-      "/opt/openrobots/share/talos_data/urdf/talos_reduced_wpg.urdf");
-  std::string aRobotSRDF(
-      "/opt/openrobots/share/talos_data/srdf/talos_wpg.srdf");
+  // Get environment variable CMAKE_PREFIX_PATH
+  const string s_cmake_prefix_path = getenv( "CMAKE_PREFIX_PATH" );
+
+  // Read the various paths
+  vector<string> paths;
+  boost::split(paths, s_cmake_prefix_path, boost::is_any_of(":;"));
+
+  // Search talos_reduced_wpg.urdf
+  string filename="";
+  for (auto test_path : paths)
+  {
+    filename = test_path +
+        string("/share/talos_data/urdf/talos_reduced_wpg.urdf");
+    if ( boost::filesystem::exists(filename))
+      break;
+
+  }
+
+  // If not found fails
+  if (filename.size()==0)
+  {
+    cerr << "Unable to find talos_reduced_wpg.urdf" << endl;
+    exit(-1);
+  }
+
+  // Otherwise read the file
+  ifstream talos_reduced_wpg_file(filename);
+  ostringstream oss;
+  oss << talos_reduced_wpg_file.rdbuf();
+
+  // Name of the parameter
+  string lparameter_name("/robot_description");
+
+  // Model of the robot inside a string.
+  string lrobot_description = oss.str();
+
+  std::shared_ptr<std::vector<std::string>>
+      alist_of_robots = dynamicgraph::sot::getListOfRobots();
+
+  unsigned int idx = 0;
+  for (auto an_it_of_robot : *alist_of_robots)
+  {
+    std::cout << idx++ << " " << an_it_of_robot << std::endl;
+  }
+
+  // Reading the parameter.
+  string model_name("robot");
+
+  // Search for the robot util related to robot_name.
+  dynamicgraph::sot::RobotUtilShrPtr aRobotUtil =
+      dynamicgraph::sot::getRobotUtil(model_name);
+
+  std::cout << dynamicgraph::sot::RefVoidRobotUtil() << std::endl;
+  std::cout << aRobotUtil.get() << std::endl;
+  if (aRobotUtil == dynamicgraph::sot::RefVoidRobotUtil())
+    aRobotUtil = dynamicgraph::sot::createRobotUtil(model_name);
+  std::cout << aRobotUtil.get() << std::endl;
+
+  // Then build the complete robot model.
+  aRobotUtil->set_parameter<string>(lparameter_name,lrobot_description);
 
-  aPG.setURDFFile(aRobotURDF);
-  aPG.setSRDFFile(aRobotSRDF);
+  // Build the pattern generator
+  aPG.buildPGI();
 
-  aPG.buildModel();
   //  aPG.InitState();
   aPG.pgCommandLine(":samplingperiod 0.005");
   aPG.pgCommandLine(":previewcontroltime 1.6");