diff --git a/include/dynamic_graph_bridge/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
index 64d5ed33cc8fb9c19cb368a6581f422ab32369ec..2c1faf158b04d5283c046689fec331e9f51dc877 100644
--- a/include/dynamic_graph_bridge/sot_loader.hh
+++ b/include/dynamic_graph_bridge/sot_loader.hh
@@ -97,6 +97,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_;
@@ -112,6 +115,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();
@@ -144,5 +150,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);
+
};
diff --git a/src/converter.hh b/src/converter.hh
index e53cbf52f1a359e88c8b0c1c9ad62ce3696e9714..0668d40dec66baf5d242f2b3daecaa8a1a60d10b 100644
--- a/src/converter.hh
+++ b/src/converter.hh
@@ -76,7 +76,7 @@ namespace dynamicgraph
ROS_TO_SOT_IMPL(ml::Vector)
{
- dst.resize (src.data.size ());
+ dst.resize ((int)src.data.size ());
for (unsigned i = 0; i < src.data.size (); ++i)
dst.elementAt (i) = src.data[i];
}
@@ -115,7 +115,8 @@ namespace dynamicgraph
ROS_TO_SOT_IMPL(ml::Matrix)
{
- 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.elementAt (i) = src.data[i];
}
@@ -127,7 +128,7 @@ namespace dynamicgraph
btQuaternion quaternion;
for(unsigned i = 0; i < 3; ++i)
for(unsigned j = 0; j < 3; ++j)
- rotation[i][j] = src (i, j);
+ rotation[i][j] = (float)src (i, j);
rotation.getRotation (quaternion);
dst.translation.x = src (0, 3);
@@ -143,7 +144,8 @@ namespace dynamicgraph
ROS_TO_SOT_IMPL(sot::MatrixHomogeneous)
{
btQuaternion quaternion
- (src.rotation.x, src.rotation.y, src.rotation.z, src.rotation.w);
+ ((float)src.rotation.x,(float)src.rotation.y,
+ (float)src.rotation.z,(float)src.rotation.w);
btMatrix3x3 rotation (quaternion);
// Copy the rotation component.
@@ -303,8 +305,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);
}
diff --git a/src/geometric_simu.cpp b/src/geometric_simu.cpp
index f6cfffb40d1c338fa6c471072674bbb69ae06f7b..a9981133d569ab2d992d14fc079b88df4acfae4b 100644
--- a/src/geometric_simu.cpp
+++ b/src/geometric_simu.cpp
@@ -22,27 +22,6 @@
#include <dynamic_graph_bridge/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();
-}
-
int main(int argc, char *argv[])
{
@@ -53,15 +32,10 @@ int main(int argc, char *argv[])
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);
+ aSotLoader.initializeRosNode(argc,argv);
- boost::unique_lock<boost::mutex> lock(mut);
- cond.wait(lock);
- ros::spin();
+ while(true){
+ usleep(5000);
+ }
+
}
diff --git a/src/ros_joint_state.cpp b/src/ros_joint_state.cpp
index dccc85ed93b645cd20d3bc00173ec69b2ea362f3..77b91408c155e953745773b84b95d9f961b08117 100644
--- a/src/ros_joint_state.cpp
+++ b/src/ros_joint_state.cpp
@@ -172,7 +172,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 ();
diff --git a/src/ros_publish.hxx b/src/ros_publish.hxx
index 34f45fce29f1e58695c820af30c9b4995d2ba840..fd217385fa86e5f8c880e1d1930131c62b4e8e77 100644
--- a/src/ros_publish.hxx
+++ b/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;
diff --git a/src/ros_time.cpp b/src/ros_time.cpp
index 50a23671b71ebdae4d8f2378fc59f2ef5e5274d7..0d50559a74509f4f326390c344aacf82a4a659ab 100644
--- a/src/ros_time.cpp
+++ b/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;
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index cc63d43ad6f92857741afd233c3af5355a953948..575f804c9b74773c82e533f7fcfaccc2db732dd6 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -21,14 +21,54 @@
/* -------------------------------------------------------------------------- */
#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;