From 1ada8ddebe26641d7d1c675f9b1d7a77757ad5ca Mon Sep 17 00:00:00 2001
From: Rohan Budhiraja <budhiraja@laas.fr>
Date: Wed, 17 Aug 2016 19:25:59 +0200
Subject: [PATCH] [c++][python] Move RosRobotModel from c++ to python
---
CMakeLists.txt | 2 +-
src/dynamic_graph/ros/__init__.py | 28 +++++++++++++++++++++++++---
src/robot_model.cpp | 19 +++----------------
src/ros_joint_state.cpp | 20 ++++++++++----------
4 files changed, 39 insertions(+), 30 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index cc8407e..41bf45e 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/src/dynamic_graph/ros/__init__.py b/src/dynamic_graph/ros/__init__.py
index 3a14f18..f18ed00 100644
--- a/src/dynamic_graph/ros/__init__.py
+++ b/src/dynamic_graph/ros/__init__.py
@@ -1,12 +1,34 @@
+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
diff --git a/src/robot_model.cpp b/src/robot_model.cpp
index b94f61c..6632b24 100644
--- a/src/robot_model.cpp
+++ b/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]);
diff --git a/src/ros_joint_state.cpp b/src/ros_joint_state.cpp
index c12655b..367fca1 100644
--- a/src/ros_joint_state.cpp
+++ b/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 ();
}
--
GitLab