diff --git a/CMakeLists.txt b/CMakeLists.txt
index 47df55e6fca28c1056c51f5123239509054ba76a..a8636587e78a06c6b34b88aceb2ed39af91f2b9e 100644
--- a/CMakeLists.txt
+++ b/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 a/scripts/robot_pose_publisher b/scripts/robot_pose_publisher
index fc34c316a4e4957c6fe0e07274f428a2f848f5e1..f742b2ac40e3db646688ff6451b254adfdfb3de2 100755
--- a/scripts/robot_pose_publisher
+++ b/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 a/src/robot_model.cpp b/src/robot_model.cpp
index b183bc34bf306ce18b3835419bf8bae29034778b..b94f61cab40376d5e50b9871d5c6a088f5bda974 100644
--- a/src/robot_model.cpp
+++ b/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_);
}
@@ -127,43 +127,38 @@ void RosRobotModel::loadFromParameterServer()
Vector RosRobotModel::curConf() const
{
- // The first 6 dofs are associated to the Freeflyer frame
- // Freeflyer reference frame should be the same as global
- // frame so that operational point positions correspond to
- // position in freeflyer frame.
- XmlRpc::XmlRpcValue ffpose;
- ros::NodeHandle nh(ns_);
- std::string param_name = "ffpose";
- if (nh.hasParam(param_name)){
- 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)
- {
- ROS_ASSERT(ffpose[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
- }
- }
- else
- {
- ffpose.setSize(6);
- for (int32_t i = 0; i < ffpose.size(); ++i)
- ffpose[i] = 0.0;
- }
-
- if (!m_data )
- throw std::runtime_error ("no robot loaded");
- 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;
-
+ // The first 6 dofs are associated to the Freeflyer frame
+ // Freeflyer reference frame should be the same as global
+ // frame so that operational point positions correspond to
+ // position in freeflyer frame.
+ XmlRpc::XmlRpcValue ffpose;
+ ros::NodeHandle nh(ns_);
+ std::string param_name = "ffpose";
+ if (nh.hasParam(param_name)){
+ 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) {
+ ROS_ASSERT(ffpose[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
}
+ }
+ else {
+ ffpose.setSize(6);
+ for (int32_t i = 0; i < ffpose.size(); ++i) ffpose[i] = 0.0;
+ }
+
+ if (!m_data )
+ throw std::runtime_error ("no robot loaded");
+ 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;
+ }
}
-
+
void
RosRobotModel::addJointMapping(const std::string &link, const std::string &repName)
{
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 3d0661d65e2e3961def065673378bdc705b16742..10706aa28af0b2f1009ecaa8359abe141348eb68 100644
--- a/src/sot_loader.cpp
+++ b/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_)
{
@@ -233,7 +246,25 @@ SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
coefficient_parallel_joints_[i]*angleControl_[parallel_joints_to_state_vector_[i]];
}
- joint_pub_.publish(joint_state_);
+ 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"));
}
diff --git a/src/sot_loader.hh b/src/sot_loader.hh
index 64d5ed33cc8fb9c19cb368a6581f422ab32369ec..c57f4731e20c224bea4cfbd77ca6280d131fe411 100644
--- a/src/sot_loader.hh
+++ b/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();