Publish joint-states using the SoT

Setting up ROS:
rosparam load `rospack find hrp2_14_description`/sot/hrp2_14_reduced.yaml
rosrun robot_state_publisher state_publisher

Display the robot using the info given in
https://trac.laas.fr/gepetto/hrp2/hrp2_14_description/html/reference/

To repeat at each simulation:
rosrun dynamic_graph_bridge geometric_simu --input-file ~/devel/ros-unstable-2/install/lib/libsot-hrp2-14-controller.so
rosrun dynamic_graph_bridge run_command
>>> from dynamic_graph.sot.application.velocity.precomputed_tasks import *
>>> solver=initialize(robot)
rosservice call /start_dynamic_graph
>>> Put_whatever_you_want_in_python

The rationale is to use a state vector map provided by the robot stack.
For instance for hrp-2 we have a yaml file providing the ros parameter:
/sot/state_vector_map
which specifies the state vector:
[RLEG_JOINT0, RLEG_JOINT1, RLEG_JOINT2, RLEG_JOINT3, RLEG_JOINT4, RLEG_JOINT5, LLEG_JOINT0,
  LLEG_JOINT1, LLEG_JOINT2, LLEG_JOINT3, LLEG_JOINT4, LLEG_JOINT5, CHEST_JOINT0, CHEST_JOINT1,
  HEAD_JOINT0, HEAD_JOINT1, RARM_JOINT0, RARM_JOINT1, RARM_JOINT2, RARM_JOINT3, RARM_JOINT4,
  RARM_JOINT5, RARM_JOINT6, LARM_JOINT0, LARM_JOINT1, LARM_JOINT2, LARM_JOINT3, LARM_JOINT4,
  LARM_JOINT5, LARM_JOINT6]

CMakeLists.txt

@@ -95,7 +95,6 @@ pkg_config_use_dependency(interpreter sot-dynamic)
 
 # Stand alone embedded intepreter with a robot controller.
 rosbuild_add_executable(geometric_simu src/geometric_simu.cpp src/sot_loader.cpp)
-pkg_config_use_dependency(interpreter sot-core)
 
 add_subdirectory(src)
 

src/geometric_simu.cpp

@@ -17,31 +17,51 @@
  * with dynamic_graph_bridge.  If not, see <http://www.gnu.org/licenses/>.
  */
 #include <iostream>
+#include <boost/thread/thread.hpp>
+#include <boost/thread/condition.hpp>
 
 #include "sot_loader.hh"
 
-bool SotLoader::start_dg(std_srvs::Empty::Request& request, 
-                         std_srvs::Empty::Response& response)
+boost::condition_variable cond;
+boost::mutex mut;
+
+void workThread(SotLoader *aSotLoader)
 {
-  for(unsigned int i=0;i<500;i++)
+  {
+    boost::lock_guard<boost::mutex> lock(mut);
+  }
+  while(aSotLoader->isDynamicGraphStopped())
+    {
+      usleep(5000);
+    }  
+  while(!aSotLoader->isDynamicGraphStopped())
     {
-      oneIteration();
+      aSotLoader->oneIteration();
+      usleep(5000);
     }
+  cond.notify_all();
+  ros::waitForShutdown();
 }
 
 
 int main(int argc, char *argv[])
 {
+  
+  ros::init(argc, argv, "sot_ros_encapsulator");
+
   SotLoader aSotLoader;
   if (aSotLoader.parseOptions(argc,argv)<0)
     return -1;
   
-  ros::init(argc, argv, "start_dynamic_graph");
   ros::NodeHandle n;
-  
   ros::ServiceServer service = n.advertiseService("start_dynamic_graph", 
                                                   &SotLoader::start_dg,
                                                   &aSotLoader);
   ROS_INFO("Ready to start dynamic graph.");
+
+  boost::thread thr(workThread,&aSotLoader);
+
+  boost::unique_lock<boost::mutex> lock(mut);
+  cond.wait(lock);
   ros::spin();
 }

src/sot_loader.cpp

@@ -30,7 +30,8 @@ using namespace dynamicgraph::sot;
 namespace po = boost::program_options;
 
 SotLoader::SotLoader():
-sensorsIn_ (),
+  dynamic_graph_stopped_(true),
+  sensorsIn_ (),
   controlValues_ (),
   angleEncoder_ (),
   angleControl_ (),
@@ -40,8 +41,49 @@ sensorsIn_ (),
   accelerometer_ (3),
   gyrometer_ (3)
 {
+  readSotVectorStateParam();
+  initPublication();
 }
 
+int SotLoader::initPublication()
+{
+  ros::NodeHandle n;
+
+
+  // Prepare message to be published
+  joint_pub_ = n.advertise<sensor_msgs::JointState>("joint_states", 1);
+
+  return 0;
+}
+
+int SotLoader::readSotVectorStateParam()
+{
+  ros::NodeHandle n;
+
+  if (!ros::param::has("/sot/state_vector_map"))
+    {
+      std::cerr<< " Read Sot Vector State Param " << std::endl;
+      return 1;
+    }
+
+  n.getParam("/sot/state_vector_map", stateVectorMap_);
+  ROS_ASSERT(stateVectorMap_.getType() == XmlRpc::XmlRpcValue::TypeArray);
+  nbOfJoints_ = stateVectorMap_.size();
+
+  // Prepare joint_state according to robot description.
+  joint_state_.name.resize(nbOfJoints_);
+  joint_state_.position.resize(nbOfJoints_);
+
+  for (int32_t i = 0; i < stateVectorMap_.size(); ++i) 
+   {
+     joint_state_.name[i]= static_cast<string>(stateVectorMap_[i]);
+   }
+  angleEncoder_.resize(nbOfJoints_);
+  
+  return 0;
+}
+
+
 int SotLoader::parseOptions(int argc, char *argv[])
 { 
   po::options_description desc("Allowed options");
@@ -74,7 +116,7 @@ void SotLoader::Initialization()
   
   // Load the SotRobotBipedController library.
   void * SotRobotControllerLibrary = dlopen( dynamicLibraryName_.c_str(),
-                                             RTLD_LAZY | RTLD_LOCAL );
+                                             RTLD_LAZY | RTLD_GLOBAL );
   if (!SotRobotControllerLibrary) {
     std::cerr << "Cannot load library: " << dlerror() << '\n';
     return ;
@@ -95,8 +137,6 @@ void SotLoader::Initialization()
   
   // Create robot-controller
   sotController_ = createSot();
-  //    std::string s="libsot-hrp2-14-controller.so";
-  //  sotController_->Initialization(dynamicLibraryName_);
   cout <<"Went out from Initialization." << endl;
 }
 
@@ -110,28 +150,33 @@ SotLoader::fillSensors(map<string,dgs::SensorValues> & sensorsIn)
     angleEncoder_[i] = angleControl_[i];
   sensorsIn["joints"].setValues(angleEncoder_);
   
-  
 }
 
 void 
 SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
 {
-  //static unsigned int nbit=0;
+
   
   // Update joint values.
   angleControl_ = controlValues["joints"].getValues();
+
+  // Check if the size if coherent with the robot description.
+  if (angleControl_.size()!=(unsigned int)nbOfJoints_)
+    {
+      std::cerr << " angleControl_ and nbOfJoints are different !"
+                << std::endl;
+      exit(-1);
+    }
+
+  // Publish the data.
+  joint_state_.header.stamp = ros::Time::now();  
+  for(int i=0;i<nbOfJoints_;i++)
+    {
+      joint_state_.position[i] = angleControl_[i];
+    }
   
-  /*
-    if (nbit%100==0)
-    std::cout << "Size of angles: " << angleControl_.size() 
-    << " Size of mc->angle: " << mc->angle.length() 
-    << std::endl;
-  */
-  /* 
-     if (nbit%100==0)
-     std::cout << std::endl;
-     nbit++;
-  */
+  joint_pub_.publish(joint_state_);  
+
   
 }
 
@@ -146,7 +191,6 @@ void SotLoader::setup()
 void SotLoader::oneIteration()
 {
   fillSensors(sensorsIn_);
-  
   try 
     {
       sotController_->nominalSetSensors(sensorsIn_);
@@ -157,3 +201,18 @@ void SotLoader::oneIteration()
   readControl(controlValues_);
 }
 
+
+bool SotLoader::start_dg(std_srvs::Empty::Request& request, 
+                         std_srvs::Empty::Response& response)
+{
+  dynamic_graph_stopped_=false;    
+  return true;
+}
+
+bool SotLoader::stop_dg(std_srvs::Empty::Request& request, 
+                         std_srvs::Empty::Response& response)
+{
+  dynamic_graph_stopped_ = true;
+  return true;
+}
+

src/sot_loader.hh

@@ -22,16 +22,21 @@
 
 // System includes
 #include <iostream>
+#include <cassert>
 
 // STL includes
 #include <map>
 
 // Boost includes
 #include <boost/program_options.hpp>
+#include <boost/foreach.hpp>
+#include <boost/format.hpp>
 
 // ROS includes
 #include "ros/ros.h"
 #include "std_srvs/Empty.h"
+#include <sensor_msgs/JointState.h>
+
 // Sot Framework includes 
 #include <sot/core/debug.hh>
 #include <sot/core/abstract-sot-external-interface.hh>
@@ -39,13 +44,14 @@
 namespace po = boost::program_options;
 namespace dgs = dynamicgraph::sot;
 
-//typedef std::vector<double> SensorValues;
-//typedef std::vector<double> ControlValues;
 
 class SotLoader {
 
 protected:
 
+  // Dynamic graph is stopped.
+  bool dynamic_graph_stopped_;
+
   /// \brief the sot-hrp2 controller
   dgs::AbstractSotExternalInterface * sotController_;
 
@@ -71,19 +77,62 @@ protected:
   /// Angular velocity read by gyrometers
   std::vector <double> gyrometer_;
 
+  /// URDF string description of the robot.
+  std::string robot_desc_string_;
+
+  XmlRpc::XmlRpcValue stateVectorMap_;
+
+  // Joint state publication.
+  ros::Publisher joint_pub_;
+  
+  // Joint state to be published.
+  sensor_msgs::JointState joint_state_;
+
+  // Number of DOFs according to KDL.
+  int nbOfJoints_;
+
+
 public:
   SotLoader();
   ~SotLoader() {};
 
+  // \brief Read user input to extract the path of the SoT dynamic library.
   int parseOptions(int argc, char *argv[]);
-  
+
+  // \brief Load the SoT
   void Initialization();
+
+  // \brief Compute one iteration of control.
+  // Basically calls fillSensors, the SoT and the readControl.
   void oneIteration();
 
-  void fillSensors(std::map<std::string, dgs::SensorValues> & sensorsIn);
-  void readControl(std::map<std::string, dgs::ControlValues> & controlValues);
+  // \brief Fill the sensors value for the SoT.
+  void fillSensors(std::map<std::string, 
+                   dgs::SensorValues> & sensorsIn);
+
+  // \brief Read the control computed by the SoT framework.
+  void readControl(std::map<std::string, 
+                   dgs::ControlValues> & controlValues);
+
+  // \brief Prepare the SoT framework.
   void setup();
+
+  // \brief Callback function when starting dynamic graph.
   bool start_dg(std_srvs::Empty::Request& request, 
                 std_srvs::Empty::Response& response);
+
+  // \brief Callback function when stopping dynamic graph.
+  bool stop_dg(std_srvs::Empty::Request& request, 
+                std_srvs::Empty::Response& response);
+
+  // \brief Read the state vector description based upon the robot links.
+  int readSotVectorStateParam();
+
+  // \brief Init publication of joint states.
+  int initPublication();
+
+  // \brief Get Status of dg.
+  bool isDynamicGraphStopped()
+  { return dynamic_graph_stopped_; }
 };