Merge branch 'master' of https://github.com/stack-of-tasks/dynamic_graph_bridge

Conflicts:
	CMakeLists.txt
	package.xml
	src/converter.hh
	src/geometric_simu.cpp

CMakeLists.txt

@@ -42,6 +42,7 @@ set(CUSTOM_HEADER_DIR dynamic_graph_bridge)
 set(${PROJECT_NAME}_HEADERS
   include/dynamic_graph_bridge/ros_init.hh
   include/dynamic_graph_bridge/ros_interpreter.hh
+  include/dynamic_graph_bridge/sot_loader.hh
   )
 SEARCH_FOR_EIGEN()
 SEARCH_FOR_BOOST()
@@ -98,9 +99,7 @@ set_target_properties(ros_bridge PROPERTIES BUILD_WITH_INSTALL_RPATH True
 macro(compile_plugin NAME)
   message(lib path ${LIBRARY_OUTPUT_PATH})
   file(MAKE_DIRECTORY "${LIBRARY_OUTPUT_PATH}/dynamic_graph/ros/${NAME}")
-  add_library(${NAME} src/${NAME}.cpp src/${NAME}.hh)
-  #pkg_config_use_dependency(${NAME} jrl-mal)
-  #kg_config_use_dependency(${NAME} eigen3)
+  add_library(${NAME} SHARED src/${NAME}.cpp src/${NAME}.hh)
   pkg_config_use_dependency(${NAME} dynamic-graph)
   pkg_config_use_dependency(${NAME} sot-core)
   #pkg_config_use_dependency(${NAME} jrl-dynamics-urdf)
@@ -108,7 +107,8 @@ macro(compile_plugin NAME)
   add_dependencies(${NAME} ros_bridge)
   target_link_libraries(${NAME} ros_bridge)
   set_target_properties(${NAME} PROPERTIES BUILD_WITH_INSTALL_RPATH True)
-  install(TARGETS ${NAME} DESTINATION lib)
+  set_target_properties(${NAME} PROPERTIES PREFIX "")
+  install(TARGETS ${NAME} DESTINATION lib/plugin)
   
 
   dynamic_graph_python_module("ros/${NAME}"
@@ -165,7 +165,14 @@ pkg_config_use_dependency(interpreter dynamic_graph_bridge_msgs)
 # Stand alone embedded intepreter with a robot controller.
 add_executable(geometric_simu src/geometric_simu.cpp src/sot_loader.cpp)
 pkg_config_use_dependency(geometric_simu roscpp)
-target_link_libraries(geometric_simu  ros_bridge ${Boost_LIBRARIES} dl tf)
+target_link_libraries(geometric_simu  ros_bridge ${Boost_LIBRARIES} ${CMAKE_DL_LIBS})
+
+# Sot loader library
+add_library(sot_loader src/sot_loader.cpp)
+pkg_config_use_dependency(sot_loader dynamic-graph)
+pkg_config_use_dependency(sot_loader sot-core)
+target_link_libraries(sot_loader ${Boost_LIBRARIES})
+install(TARGETS sot_loader DESTINATION lib)
 
 add_subdirectory(src)
 

src/sot_loader.hh → include/dynamic_graph_bridge/sot_loader.hh

@@ -98,6 +98,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_;
@@ -117,6 +120,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();
@@ -149,5 +155,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

@@ -117,8 +117,8 @@ namespace dynamicgraph
   
   ROS_TO_SOT_IMPL(Matrix)
   {
-    //TODO: Confirm Ros Matrix Storage order.
-    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.data()[i] =  src.data[i];
   }
@@ -126,6 +126,7 @@ namespace dynamicgraph
   // Homogeneous matrix.
   SOT_TO_ROS_IMPL(sot::MatrixHomogeneous)
   {
+
     sot::VectorQuaternion q(src.linear());
     dst.translation.x = src.translation()(0);
     dst.translation.y = src.translation()(1);
@@ -135,6 +136,7 @@ namespace dynamicgraph
     dst.rotation.y = q.y();
     dst.rotation.z = q.z();
     dst.rotation.w = q.w();
+
   }
 
   ROS_TO_SOT_IMPL(sot::MatrixHomogeneous)
@@ -297,8 +299,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

@@ -20,28 +20,7 @@
 #include <boost/thread/thread.hpp>
 #include <boost/thread/condition.hpp>
 
-#include "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();
-}
+#include <dynamic_graph_bridge/sot_loader.hh>
 
 
 int main(int argc, char *argv[])
@@ -50,13 +29,11 @@ int main(int argc, char *argv[])
   SotLoader aSotLoader;
   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);
-  boost::unique_lock<boost::mutex> lock(mut);
-  cond.wait(lock);
-  ros::spin();
+  
+  aSotLoader.initializeRosNode(argc,argv);
+
+  while(true){
+    usleep(5000);
+  }
+ 
 }

src/ros_joint_state.cpp

@@ -165,7 +165,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

@@ -20,15 +20,55 @@
 /* --- INCLUDES ------------------------------------------------------------- */
 /* -------------------------------------------------------------------------- */
 
-#include "sot_loader.hh"
+#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;