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

CMakeLists.txt

@@ -136,6 +136,7 @@ compile_plugin(ros_time)
 compile_plugin(ros_joint_state)
 
 target_link_libraries(ros_joint_state "${DYNAMIC_GRAPH_PLUGINDIR}/dp-dynamic.so")
+target_link_libraries(ros_publish ros_bridge)
 
 #compile_plugin(robot_model)
 
@@ -173,7 +174,7 @@ target_link_libraries(geometric_simu  ros_bridge tf ${Boost_LIBRARIES} ${CMAKE_D
 add_library(sot_loader src/sot_loader.cpp src/sot_loader_basic.cpp)
 pkg_config_use_dependency(sot_loader dynamic-graph)
 pkg_config_use_dependency(sot_loader sot-core)
-target_link_libraries(sot_loader ${Boost_LIBRARIES} roscpp ros_bridge)
+target_link_libraries(sot_loader ${Boost_LIBRARIES} roscpp ros_bridge tf)
 install(TARGETS sot_loader DESTINATION lib)
 
 add_subdirectory(src)

src/sot_loader.cpp

@@ -76,15 +76,20 @@ void SotLoader::startControlLoop()
 void SotLoader::initializeRosNode(int argc, char *argv[])
 {
   SotLoaderBasic::initializeRosNode(argc, argv);
-  angleEncoder_.resize(nbOfJoints_);
+  //Temporary fix. TODO: where should nbOfJoints_ be initialized from?                         
+  if (ros::param::has("/sot/state_vector_map")) {
+    angleEncoder_.resize(nbOfJoints_);
+  }
+
   startControlLoop();
 }
 
 void 
 SotLoader::fillSensors(map<string,dgs::SensorValues> & sensorsIn)
 {
-  
   // Update joint values.w
+  assert(angleControl_.size() == angleEncoder_.size());
+
   sensorsIn["joints"].setName("angle");
   for(unsigned int i=0;i<angleControl_.size();i++)
     angleEncoder_[i] = angleControl_[i];
@@ -97,7 +102,7 @@ SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
 
   
   // Update joint values.
-  angleControl_ = controlValues["joints"].getValues();
+  angleControl_ = controlValues["control"].getValues();
 
   //Debug
   std::map<std::string,dgs::ControlValues>::iterator it = controlValues.begin();