[wip/dynamic-graph-bridge3][PATCH] add patch file

dynamic-graph-bridge3/patches/patch-ab

+move robot pose publishing from python script to cpp
+
+diff --git CMakeLists.txt CMakeLists.txt
+index 47df55e..a863658 100644
+--- CMakeLists.txt
++++ CMakeLists.txt
+@@ -162,7 +162,7 @@ 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)
++target_link_libraries(geometric_simu  ros_bridge ${Boost_LIBRARIES} dl tf)
+ 
+ add_subdirectory(src)
+ 
+diff --git scripts/robot_pose_publisher scripts/robot_pose_publisher
+index fc34c31..f742b2a 100755
+--- scripts/robot_pose_publisher
++++ scripts/robot_pose_publisher
+@@ -11,6 +11,7 @@ import sensor_msgs.msg
+ frame = ''
+ childFrame = ''
+ 
++#DEPRECATED. Robot Pose is already being published
+ def pose_broadcaster(msg):
+     translation = msg.position[0:3];
+     rotation = tf.transformations.quaternion_from_euler(msg.position[3], msg.position[4], msg.position[5])
+diff --git src/robot_model.cpp src/robot_model.cpp
+index b183bc3..b94f61c 100644
+--- src/robot_model.cpp
++++ src/robot_model.cpp
+@@ -77,7 +77,7 @@ void RosRobotModel::loadUrdf (const std::string& filename)
+   
+   XmlRpc::XmlRpcValue JointsNamesByRank_;
+   JointsNamesByRank_.setSize(m_model.names.size());
+-  std::vector<std::string>::const_iterator it = m_model.names.begin()+1; //the first name is universe
++  std::vector<std::string>::const_iterator it = m_model.names.begin()+2; //first joint is universe, second is freeflyer
+   for (int i=0;it!=m_model.names.end();++it, ++i)  JointsNamesByRank_[i]= (*it);
+   nh.setParam(jointsParameterName_, JointsNamesByRank_);
+ }
+@@ -119,7 +119,7 @@ void RosRobotModel::loadFromParameterServer()
+     
+     XmlRpc::XmlRpcValue JointsNamesByRank_;
+     JointsNamesByRank_.setSize(m_model.names.size());
+-    std::vector<std::string>::const_iterator it = m_model.names.begin();
++    std::vector<std::string>::const_iterator it = m_model.names.begin()+2; //first joint is universe, second is freeflyer
+     for (int i=0;it!=m_model.names.end();++it, ++i) JointsNamesByRank_[i]= (*it);
+     nh.setParam(jointsParameterName_, JointsNamesByRank_);
+ }
+@@ -138,16 +138,13 @@ Vector RosRobotModel::curConf() const
+         nh.getParam(param_name, ffpose);
+         ROS_ASSERT(ffpose.getType() == XmlRpc::XmlRpcValue::TypeArray);
+         ROS_ASSERT(ffpose.size() == 6);
+-        for (int32_t i = 0; i < ffpose.size(); ++i)
+-        {
++    for (int32_t i = 0; i < ffpose.size(); ++i) {
+             ROS_ASSERT(ffpose[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
+         }
+     }
+-    else
+-    {
++  else {
+         ffpose.setSize(6);
+-        for (int32_t i = 0; i < ffpose.size(); ++i)
+-            ffpose[i] = 0.0;
++    for (int32_t i = 0; i < ffpose.size(); ++i)  ffpose[i] = 0.0;
+     }
+ 
+     if (!m_data )
+@@ -155,12 +152,10 @@ Vector RosRobotModel::curConf() const
+     else {
+       //TODO: confirm accesscopy is for asynchronous commands
+       Vector currConf = jointPositionSIN.accessCopy();
+-      
+       for (int32_t i = 0; i < ffpose.size(); ++i)
+ 	currConf(i) = static_cast<double>(ffpose[i]);
+       
+       return currConf;
+-     
+     }
+ }
+ 
+diff --git src/sot_loader.cpp src/sot_loader.cpp
+index 3d0661d..10706aa 100644
+--- src/sot_loader.cpp
++++ src/sot_loader.cpp
+@@ -212,6 +212,19 @@ SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
+   // Update joint values.
+   angleControl_ = controlValues["joints"].getValues();
+ 
++  //Debug
++  std::map<std::string,dgs::ControlValues>::iterator it = controlValues.begin();
++  sotDEBUG (30)<<"ControlValues to be broadcasted:"<<std::endl;
++  for(;it!=controlValues.end(); it++){
++    sotDEBUG (30)<<it->first<<":";
++    std::vector<double> ctrlValues_ = it->second.getValues();
++    std::vector<double>::iterator it_d = ctrlValues_.begin();
++    for(;it_d!=ctrlValues_.end();it_d++) sotDEBUG (30)<<*it_d<<" ";
++    sotDEBUG (30)<<std::endl;
++  }
++  sotDEBUG (30)<<"End ControlValues"<<std::endl;
++
++
+   // Check if the size if coherent with the robot description.
+   if (angleControl_.size()!=(unsigned int)nbOfJoints_)
+     {
+@@ -235,6 +248,24 @@ SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
+ 
+   joint_pub_.publish(joint_state_);  
+   
++  //Publish robot pose
++  //get the robot pose values
++  std::vector<double> poseValue_ = controlValues["baseff"].getValues();
++
++  freeFlyerPose_.setOrigin(tf::Vector3(poseValue_[0],
++				       poseValue_[1],
++				       poseValue_[2]));
++  tf::Quaternion poseQ_(poseValue_[4],
++			poseValue_[5],
++			poseValue_[6],
++			poseValue_[3]);
++  freeFlyerPose_.setRotation(poseQ_);
++  //Publish
++  freeFlyerPublisher_.sendTransform(tf::StampedTransform(freeFlyerPose_,
++							 ros::Time::now(),
++							 "odom",
++							 "base_link"));
++  
+ }
+ 
+ void SotLoader::setup()
+diff --git src/sot_loader.hh src/sot_loader.hh
+index 64d5ed3..c57f473 100644
+--- src/sot_loader.hh
++++ src/sot_loader.hh
+@@ -36,6 +36,7 @@
+ #include "ros/ros.h"
+ #include "std_srvs/Empty.h"
+ #include <sensor_msgs/JointState.h>
++#include <tf/transform_broadcaster.h>
+ 
+ // Sot Framework includes 
+ #include <sot/core/debug.hh>
+@@ -101,6 +102,10 @@ protected:
+   int nbOfJoints_;
+   parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_;
+ 
++  //Robot Pose Publisher
++  tf::TransformBroadcaster freeFlyerPublisher_;
++  tf::Transform freeFlyerPose_;
++
+ 
+ public:
+   SotLoader();