[c++][python] Move RosRobotModel from c++ to python

CMakeLists.txt

@@ -132,7 +132,7 @@ compile_plugin(ros_joint_state)
 
 target_link_libraries(ros_joint_state "${DYNAMIC_GRAPH_PLUGINDIR}/dynamic.so")
 
-compile_plugin(robot_model)
+#compile_plugin(robot_model)
 
 # ros_interperter library.
 add_library(ros_interpreter src/ros_interpreter.cpp)

src/dynamic_graph/ros/__init__.py

+from dynamic_graph.sot.dynamics import Dynamic
 from ros_publish import RosPublish
 from ros_subscribe import RosSubscribe
 from ros_joint_state import RosJointState
-from robot_model import RosRobotModel
 
 from ros import Ros
 
 # aliases, for retro compatibility
-ros_import = ros_publish
-ros_export = ros_subscribe
 from ros import RosPublish as RosImport
 from ros import RosSubscribe as RosExport
+
+import rospy
+
+class RosRobotModel(Dynamic):
+    def __init__(self):
+        Dynamic.__init__(self)
+        self.namespace = "sot_controller"
+        self.jointsParameterName_ = "jrl_map"
+
+    def setJointsNamesParameter(self):
+        if self.model is not None:
+            parameter_name = self.namespace + "/" + jointsParameterName_
+            jointsName = []
+            for i in xrange(self.model.njoints):
+                jointsName.append(self.model.names[i])
+            rospy.set_param(parameter_name,jointsName)
+        return
+
+    def setNamespace(self, ns):
+        self.namespace = ns
+        return
+
+    def curConf(self):
+        return self.position.value

src/robot_model.cpp

@@ -58,13 +58,6 @@ RosRobotModel::~RosRobotModel()
 
 void RosRobotModel::loadUrdf (const std::string& filename)
 {
-  // jrl::dynamics::urdf::Parser parser;
-
-  //TODO: Specific rep name. link them to the operational frames in pinocchio
-  //  std::map<std::string, std::string>::const_iterator it = specialJoints_.begin();
-  //  for (;it!=specialJoints_.end();++it) {
-  //    parser.specifyREPName(it->first, it->second);
-  //  }
   rosInit (false);
   m_model = se3::urdf::buildModel(filename);
   this->m_urdfPath = filename;
@@ -89,13 +82,6 @@ void RosRobotModel::setNamespace (const std::string& ns)
 
 void RosRobotModel::loadFromParameterServer()
 {
-  //jrl::dynamics::urdf::Parser parser;
-
-  //TODO: Specific rep name. link them to the operational frames in pinocchio
-  //    std::map<std::string, std::string>::const_iterator it = specialJoints_.begin();
-  //    for (;it!=specialJoints_.end();++it) {
-  //        parser.specifyREPName(it->first, it->second);
-  //    }
     rosInit (false);
     std::string robotDescription;
     ros::param::param<std::string> ("/robot_description", robotDescription, "");
@@ -119,7 +105,8 @@ void RosRobotModel::loadFromParameterServer()
     
     XmlRpc::XmlRpcValue JointsNamesByRank_;
     JointsNamesByRank_.setSize(m_model.names.size());
-    std::vector<std::string>::const_iterator it = m_model.names.begin()+2; //first joint is universe, second is freeflyer
+    //first joint is universe, second is freeflyer
+    std::vector<std::string>::const_iterator it = m_model.names.begin()+2;
     for (int i=0;it!=m_model.names.end();++it, ++i) JointsNamesByRank_[i]= (*it);
     nh.setParam(jointsParameterName_, JointsNamesByRank_);
 }
@@ -151,7 +138,7 @@ Vector RosRobotModel::curConf() const
     throw std::runtime_error ("no robot loaded");
   else {
     //TODO: confirm accesscopy is for asynchronous commands
-    Vector currConf = jointPositionSIN.accessCopy();  
+    Vector currConf = jointPositionSIN.accessCopy();
     for (int32_t i = 0; i < ffpose.size(); ++i)
       currConf(i) = static_cast<double>(ffpose[i]);
 

src/ros_joint_state.cpp

@@ -36,21 +36,21 @@ namespace dynamicgraph
 }
 
     namespace {
-      void  buildJointNames (sensor_msgs::JointState& jointState, se3::Model& robot_model) {
-	for (int i=0;i<robot_model.nbody-1;i++) {	  
+      void  buildJointNames (sensor_msgs::JointState& jointState, se3::Model* robot_model) {
+	for (int i=0;i<robot_model->nbody-1;i++) {
 	  // Ignore anchors.
-	  if (se3::nv(robot_model.joints[i]) != 0) {
+	  if (se3::nv(robot_model->joints[i]) != 0) {
 	    // If we only have one dof, the dof name is the joint name.
-	    if (se3::nv(robot_model.joints[i]) == 1) {
-	      jointState.name[i] =  robot_model.names[i];
+	    if (se3::nv(robot_model->joints[i]) == 1) {
+	      jointState.name[i] =  robot_model->names[i];
 	    }
 	    else {
 	      // ...otherwise, the dof name is the joint name on which
 	      // the dof id is appended.
-	      int joint_dof = se3::nv(robot_model.joints[i]);
+	      int joint_dof = se3::nv(robot_model->joints[i]);
 	      for(int j = 0; j<joint_dof; j++) {
 		boost::format fmt("%1%_%2%");
-		fmt % robot_model.names[i];
+		fmt % robot_model->names[i];
 		fmt % j;
 		jointState.name[i + j] =  fmt.str();
 	      }
@@ -82,14 +82,14 @@ namespace dynamicgraph
 	  return Value ();
 	}
 
-      se3::Model& robot_model = dynamic->m_model;
-      if (robot_model.nbody == 1)
+      se3::Model* robot_model = dynamic->m_model;
+      if (robot_model->nbody == 1)
 	{
 	  std::cerr << "no robot in the dynamic entity" << std::endl;
 	  return Value ();
 	}
 
-      entity.jointState ().name.resize (robot_model.nv);
+      entity.jointState ().name.resize (robot_model->nv);
       buildJointNames (entity.jointState (), robot_model);
       return Value ();
     }