diff --git a/CMakeLists.txt b/CMakeLists.txt
index cad95d1f39368a2df85c6bf4b66c271bef88ccde..0fc69ae30f9b85b8e93615e9df12a5ed13da5a08 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -95,7 +95,6 @@ pkg_config_use_dependency(interpreter sot-dynamic)
# Stand alone embedded intepreter with a robot controller.
rosbuild_add_executable(geometric_simu src/geometric_simu.cpp src/sot_loader.cpp)
-pkg_config_use_dependency(interpreter sot-core)
add_subdirectory(src)
diff --git a/src/geometric_simu.cpp b/src/geometric_simu.cpp
index 1c51bc08126dfa01e41ce4f7a84d86fecd17c9b1..56f31cc8cecdb2336714964fd835804b94dba368 100644
--- a/src/geometric_simu.cpp
+++ b/src/geometric_simu.cpp
@@ -17,31 +17,51 @@
* with dynamic_graph_bridge. If not, see <http://www.gnu.org/licenses/>.
*/
#include <iostream>
+#include <boost/thread/thread.hpp>
+#include <boost/thread/condition.hpp>
#include "sot_loader.hh"
-bool SotLoader::start_dg(std_srvs::Empty::Request& request,
- std_srvs::Empty::Response& response)
+boost::condition_variable cond;
+boost::mutex mut;
+
+void workThread(SotLoader *aSotLoader)
{
- for(unsigned int i=0;i<500;i++)
+ {
+ boost::lock_guard<boost::mutex> lock(mut);
+ }
+ while(aSotLoader->isDynamicGraphStopped())
+ {
+ usleep(5000);
+ }
+ while(!aSotLoader->isDynamicGraphStopped())
{
- oneIteration();
+ aSotLoader->oneIteration();
+ usleep(5000);
}
+ cond.notify_all();
+ ros::waitForShutdown();
}
int main(int argc, char *argv[])
{
+
+ ros::init(argc, argv, "sot_ros_encapsulator");
+
SotLoader aSotLoader;
if (aSotLoader.parseOptions(argc,argv)<0)
return -1;
- ros::init(argc, argv, "start_dynamic_graph");
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();
}
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 42ebff851789f1c5c2aae15b65711c298af90a23..489e45fbda5de82c184b7f1a6011da40cf7058d3 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -30,7 +30,8 @@ using namespace dynamicgraph::sot;
namespace po = boost::program_options;
SotLoader::SotLoader():
-sensorsIn_ (),
+ dynamic_graph_stopped_(true),
+ sensorsIn_ (),
controlValues_ (),
angleEncoder_ (),
angleControl_ (),
@@ -40,8 +41,49 @@ sensorsIn_ (),
accelerometer_ (3),
gyrometer_ (3)
{
+ readSotVectorStateParam();
+ initPublication();
}
+int SotLoader::initPublication()
+{
+ ros::NodeHandle n;
+
+
+ // Prepare message to be published
+ joint_pub_ = n.advertise<sensor_msgs::JointState>("joint_states", 1);
+
+ return 0;
+}
+
+int SotLoader::readSotVectorStateParam()
+{
+ ros::NodeHandle n;
+
+ if (!ros::param::has("/sot/state_vector_map"))
+ {
+ std::cerr<< " Read Sot Vector State Param " << std::endl;
+ return 1;
+ }
+
+ n.getParam("/sot/state_vector_map", stateVectorMap_);
+ ROS_ASSERT(stateVectorMap_.getType() == XmlRpc::XmlRpcValue::TypeArray);
+ nbOfJoints_ = stateVectorMap_.size();
+
+ // Prepare joint_state according to robot description.
+ joint_state_.name.resize(nbOfJoints_);
+ joint_state_.position.resize(nbOfJoints_);
+
+ for (int32_t i = 0; i < stateVectorMap_.size(); ++i)
+ {
+ joint_state_.name[i]= static_cast<string>(stateVectorMap_[i]);
+ }
+ angleEncoder_.resize(nbOfJoints_);
+
+ return 0;
+}
+
+
int SotLoader::parseOptions(int argc, char *argv[])
{
po::options_description desc("Allowed options");
@@ -74,7 +116,7 @@ void SotLoader::Initialization()
// Load the SotRobotBipedController library.
void * SotRobotControllerLibrary = dlopen( dynamicLibraryName_.c_str(),
- RTLD_LAZY | RTLD_LOCAL );
+ RTLD_LAZY | RTLD_GLOBAL );
if (!SotRobotControllerLibrary) {
std::cerr << "Cannot load library: " << dlerror() << '\n';
return ;
@@ -95,8 +137,6 @@ void SotLoader::Initialization()
// Create robot-controller
sotController_ = createSot();
- // std::string s="libsot-hrp2-14-controller.so";
- // sotController_->Initialization(dynamicLibraryName_);
cout <<"Went out from Initialization." << endl;
}
@@ -110,28 +150,33 @@ SotLoader::fillSensors(map<string,dgs::SensorValues> & sensorsIn)
angleEncoder_[i] = angleControl_[i];
sensorsIn["joints"].setValues(angleEncoder_);
-
}
void
SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
{
- //static unsigned int nbit=0;
+
// Update joint values.
angleControl_ = controlValues["joints"].getValues();
+
+ // Check if the size if coherent with the robot description.
+ if (angleControl_.size()!=(unsigned int)nbOfJoints_)
+ {
+ std::cerr << " angleControl_ and nbOfJoints are different !"
+ << std::endl;
+ exit(-1);
+ }
+
+ // Publish the data.
+ joint_state_.header.stamp = ros::Time::now();
+ for(int i=0;i<nbOfJoints_;i++)
+ {
+ joint_state_.position[i] = angleControl_[i];
+ }
- /*
- if (nbit%100==0)
- std::cout << "Size of angles: " << angleControl_.size()
- << " Size of mc->angle: " << mc->angle.length()
- << std::endl;
- */
- /*
- if (nbit%100==0)
- std::cout << std::endl;
- nbit++;
- */
+ joint_pub_.publish(joint_state_);
+
}
@@ -146,7 +191,6 @@ void SotLoader::setup()
void SotLoader::oneIteration()
{
fillSensors(sensorsIn_);
-
try
{
sotController_->nominalSetSensors(sensorsIn_);
@@ -157,3 +201,18 @@ void SotLoader::oneIteration()
readControl(controlValues_);
}
+
+bool SotLoader::start_dg(std_srvs::Empty::Request& request,
+ std_srvs::Empty::Response& response)
+{
+ dynamic_graph_stopped_=false;
+ return true;
+}
+
+bool SotLoader::stop_dg(std_srvs::Empty::Request& request,
+ std_srvs::Empty::Response& response)
+{
+ dynamic_graph_stopped_ = true;
+ return true;
+}
+
diff --git a/src/sot_loader.hh b/src/sot_loader.hh
index 71882f33e8507ecd62cff20b6d7ddf10334be6cb..ceb5fd28758e68b21917ab376fa84babf2e2bf25 100644
--- a/src/sot_loader.hh
+++ b/src/sot_loader.hh
@@ -22,16 +22,21 @@
// System includes
#include <iostream>
+#include <cassert>
// STL includes
#include <map>
// Boost includes
#include <boost/program_options.hpp>
+#include <boost/foreach.hpp>
+#include <boost/format.hpp>
// ROS includes
#include "ros/ros.h"
#include "std_srvs/Empty.h"
+#include <sensor_msgs/JointState.h>
+
// Sot Framework includes
#include <sot/core/debug.hh>
#include <sot/core/abstract-sot-external-interface.hh>
@@ -39,13 +44,14 @@
namespace po = boost::program_options;
namespace dgs = dynamicgraph::sot;
-//typedef std::vector<double> SensorValues;
-//typedef std::vector<double> ControlValues;
class SotLoader {
protected:
+ // Dynamic graph is stopped.
+ bool dynamic_graph_stopped_;
+
/// \brief the sot-hrp2 controller
dgs::AbstractSotExternalInterface * sotController_;
@@ -71,19 +77,62 @@ protected:
/// Angular velocity read by gyrometers
std::vector <double> gyrometer_;
+ /// URDF string description of the robot.
+ std::string robot_desc_string_;
+
+ XmlRpc::XmlRpcValue stateVectorMap_;
+
+ // Joint state publication.
+ ros::Publisher joint_pub_;
+
+ // Joint state to be published.
+ sensor_msgs::JointState joint_state_;
+
+ // Number of DOFs according to KDL.
+ int nbOfJoints_;
+
+
public:
SotLoader();
~SotLoader() {};
+ // \brief Read user input to extract the path of the SoT dynamic library.
int parseOptions(int argc, char *argv[]);
-
+
+ // \brief Load the SoT
void Initialization();
+
+ // \brief Compute one iteration of control.
+ // Basically calls fillSensors, the SoT and the readControl.
void oneIteration();
- void fillSensors(std::map<std::string, dgs::SensorValues> & sensorsIn);
- void readControl(std::map<std::string, dgs::ControlValues> & controlValues);
+ // \brief Fill the sensors value for the SoT.
+ void fillSensors(std::map<std::string,
+ dgs::SensorValues> & sensorsIn);
+
+ // \brief Read the control computed by the SoT framework.
+ void readControl(std::map<std::string,
+ dgs::ControlValues> & controlValues);
+
+ // \brief Prepare the SoT framework.
void setup();
+
+ // \brief Callback function when starting dynamic graph.
bool start_dg(std_srvs::Empty::Request& request,
std_srvs::Empty::Response& response);
+
+ // \brief Callback function when stopping dynamic graph.
+ bool stop_dg(std_srvs::Empty::Request& request,
+ std_srvs::Empty::Response& response);
+
+ // \brief Read the state vector description based upon the robot links.
+ int readSotVectorStateParam();
+
+ // \brief Init publication of joint states.
+ int initPublication();
+
+ // \brief Get Status of dg.
+ bool isDynamicGraphStopped()
+ { return dynamic_graph_stopped_; }
};