Sot loader now includes threads for ros and real-time control.

include/dynamic_graph_bridge/sot_loader.hh

@@ -97,6 +97,9 @@ protected:
   // Joint state to be published.
   sensor_msgs::JointState joint_state_;
 
+  // \brief Start control loop
+  virtual void startControlLoop();
+
   // Number of DOFs according to KDL.
   int nbOfJoints_;
   parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_;
@@ -112,6 +115,9 @@ public:
   // \brief Load the SoT
   void Initialization();
 
+  // \brief Create a thread for ROS.
+  void initializeRosNode(int argc, char *argv[]);
+
   // \brief Compute one iteration of control.
   // Basically calls fillSensors, the SoT and the readControl.
   void oneIteration();
@@ -144,5 +150,9 @@ public:
   // \brief Get Status of dg.
   bool isDynamicGraphStopped()
   { return dynamic_graph_stopped_; }
+
+  // \brief Specify the name of the dynamic library.
+  void setDynamicLibraryName(std::string &afilename);
+
 };
 

src/converter.hh

@@ -76,7 +76,7 @@ namespace dynamicgraph
 
   ROS_TO_SOT_IMPL(ml::Vector)
   {
-    dst.resize (src.data.size ());
+    dst.resize ((int)src.data.size ());
     for (unsigned i = 0; i < src.data.size (); ++i)
       dst.elementAt (i) =  src.data[i];
   }
@@ -115,7 +115,8 @@ namespace dynamicgraph
 
   ROS_TO_SOT_IMPL(ml::Matrix)
   {
-    dst.resize (src.width, src.data.size () / src.width);
+    dst.resize (src.width, (unsigned int) src.data.size () / 
+		(unsigned int)src.width);
     for (unsigned i = 0; i < src.data.size (); ++i)
       dst.elementAt (i) =  src.data[i];
   }
@@ -127,7 +128,7 @@ namespace dynamicgraph
     btQuaternion quaternion;
     for(unsigned i = 0; i < 3; ++i)
       for(unsigned j = 0; j < 3; ++j)
-	rotation[i][j] = src (i, j);
+	rotation[i][j] = (float)src (i, j);
     rotation.getRotation (quaternion);
 
     dst.translation.x = src (0, 3);
@@ -143,7 +144,8 @@ namespace dynamicgraph
   ROS_TO_SOT_IMPL(sot::MatrixHomogeneous)
   {
     btQuaternion quaternion
-      (src.rotation.x, src.rotation.y, src.rotation.z, src.rotation.w);
+      ((float)src.rotation.x,(float)src.rotation.y, 
+       (float)src.rotation.z,(float)src.rotation.w);
     btMatrix3x3 rotation (quaternion);
 
     // Copy the rotation component.
@@ -303,8 +305,9 @@ namespace dynamicgraph
     static ptime timeStart(date(1970,1,1));
     time_duration diff = time - timeStart;
 
-    uint32_t sec = diff.ticks ()/time_duration::rep_type::res_adjust ();
-    uint32_t nsec = diff.fractional_seconds();
+    uint32_t sec = (unsigned int)diff.ticks ()/
+      (unsigned int)time_duration::rep_type::res_adjust ();
+    uint32_t nsec = (unsigned int)diff.fractional_seconds();
 
     return ros::Time (sec, nsec);
   }

src/geometric_simu.cpp

@@ -22,27 +22,6 @@
 
 #include <dynamic_graph_bridge/sot_loader.hh>
 
-boost::condition_variable cond;
-boost::mutex mut;
-
-void workThread(SotLoader *aSotLoader)
-{
-  {
-    boost::lock_guard<boost::mutex> lock(mut);
-  }
-  while(aSotLoader->isDynamicGraphStopped())
-    {
-      usleep(5000);
-    }  
-  while(!aSotLoader->isDynamicGraphStopped())
-    {
-      aSotLoader->oneIteration();
-      usleep(5000);
-    }
-  cond.notify_all();
-  ros::waitForShutdown();
-}
-
 
 int main(int argc, char *argv[])
 {
@@ -53,15 +32,10 @@ int main(int argc, char *argv[])
   if (aSotLoader.parseOptions(argc,argv)<0)
     return -1;
   
-  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);
+  aSotLoader.initializeRosNode(argc,argv);
 
-  boost::unique_lock<boost::mutex> lock(mut);
-  cond.wait(lock);
-  ros::spin();
+  while(true){
+    usleep(5000);
+  }
+  
 }

src/ros_joint_state.cpp

@@ -172,7 +172,7 @@ namespace dynamicgraph
 	// Fill position.
 	jointState_.position.resize (s);
 	for (std::size_t i = 0; i < s; ++i)
-	  jointState_.position[i] = state_.access (t) (i);
+	  jointState_.position[i] = state_.access (t) ((unsigned int)i);
 
 	publisher_.msg_ = jointState_;
 	publisher_.unlockAndPublish ();

src/ros_publish.hxx

@@ -56,7 +56,6 @@ namespace dynamicgraph
   template <typename T>
   void RosPublish::add (const std::string& signal, const std::string& topic)
   {
-    typedef typename SotToRos<T>::sot_t sot_t;
     typedef typename SotToRos<T>::ros_t ros_t;
     typedef typename SotToRos<T>::signalIn_t signal_t;
 

src/ros_time.cpp

@@ -32,7 +32,7 @@ namespace dynamicgraph {
   }
 
   ptime&
-  RosTime::update (ptime& time, const int& t)
+  RosTime::update (ptime& time, const int& )
   {
     time = rosTimeToPtime (ros::Time::now ());
     return time;

src/sot_loader.cpp

@@ -21,14 +21,54 @@
 /* -------------------------------------------------------------------------- */
 
 #include <dynamic_graph_bridge/sot_loader.hh>
+#include "dynamic_graph_bridge/ros_init.hh"
 
 // POSIX.1-2001
 #include <dlfcn.h>
 
+#include <boost/thread/thread.hpp>
+#include <boost/thread/condition.hpp>
+
+boost::condition_variable cond;
+boost::mutex mut;
+
 using namespace std;
 using namespace dynamicgraph::sot; 
 namespace po = boost::program_options;
 
+void createRosSpin(SotLoader *aSotLoader)
+{
+  ROS_INFO("createRosSpin started\n");
+  ros::NodeHandle n;
+
+  ros::ServiceServer service = n.advertiseService("start_dynamic_graph",
+                                                  &SotLoader::start_dg,
+                                                  aSotLoader);
+
+  ros::ServiceServer service2 = n.advertiseService("stop_dynamic_graph",
+                                                  &SotLoader::stop_dg,
+                                                  aSotLoader);
+
+
+  ros::waitForShutdown();
+}
+
+void workThreadLoader(SotLoader *aSotLoader)
+{
+  while(aSotLoader->isDynamicGraphStopped())
+    {
+      usleep(5000);
+    }
+
+  while(!aSotLoader->isDynamicGraphStopped())
+    {
+      aSotLoader->oneIteration();
+      usleep(5000);
+    }
+  cond.notify_all();
+  ros::waitForShutdown();
+}
+
 SotLoader::SotLoader():
   dynamic_graph_stopped_(true),
   sensorsIn_ (),
@@ -47,8 +87,8 @@ SotLoader::SotLoader():
 
 int SotLoader::initPublication()
 {
-  ros::NodeHandle n;
-
+  ros::NodeHandle & n = dynamicgraph::rosInit(false);
+ 
 
   // Prepare message to be published
   joint_pub_ = n.advertise<sensor_msgs::JointState>("joint_states", 1);
@@ -56,6 +96,26 @@ int SotLoader::initPublication()
   return 0;
 }
 
+void SotLoader::startControlLoop()
+{
+  boost::thread thr(workThreadLoader, this);
+}
+
+void SotLoader::initializeRosNode(int , char *[])
+{
+  ROS_INFO("Ready to start dynamic graph.");
+  boost::unique_lock<boost::mutex> lock(mut);
+  boost::thread thr2(createRosSpin,this);
+
+  startControlLoop();
+}   
+
+
+void SotLoader::setDynamicLibraryName(std::string &afilename)
+{
+  dynamicLibraryName_ = afilename;
+}
+
 int SotLoader::readSotVectorStateParam()
 {
   std::map<std::string,int> from_state_name_to_state_vector;