From ee435647db0b5091e7ecf521aad4a694c2751123 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 19 Apr 2018 13:48:08 +0200
Subject: [PATCH] Geometric simu uses ROS parameter /sot/dt
---
include/dynamic_graph_bridge/sot_loader.hh | 6 ++++-
src/geometric_simu.cpp | 6 ++---
src/sot_loader.cpp | 31 ++++++++++++++++++----
3 files changed, 33 insertions(+), 10 deletions(-)
diff --git a/include/dynamic_graph_bridge/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
index b740f5b..0702213 100644
--- a/include/dynamic_graph_bridge/sot_loader.hh
+++ b/include/dynamic_graph_bridge/sot_loader.hh
@@ -34,6 +34,7 @@
#include <boost/program_options.hpp>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
+#include <boost/thread/thread.hpp>
// ROS includes
#include "ros/ros.h"
@@ -82,6 +83,9 @@ protected:
std::string robot_desc_string_;
+ /// The thread running dynamic graph
+ boost::thread thread_;
+
// \brief Start control loop
virtual void startControlLoop();
@@ -92,7 +96,7 @@ protected:
public:
SotLoader();
- ~SotLoader() {};
+ ~SotLoader();
// \brief Create a thread for ROS and start the control loop.
void initializeRosNode(int argc, char *argv[]);
diff --git a/src/geometric_simu.cpp b/src/geometric_simu.cpp
index e622377..2233cf7 100644
--- a/src/geometric_simu.cpp
+++ b/src/geometric_simu.cpp
@@ -32,8 +32,6 @@ int main(int argc, char *argv[])
aSotLoader.initializeRosNode(argc,argv);
- while(true){
- usleep(5000);
- }
-
+ ros::spin();
+ return 0;
}
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 1b9071c..951d58d 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -26,7 +26,6 @@
// POSIX.1-2001
#include <dlfcn.h>
-#include <boost/thread/thread.hpp>
#include <boost/thread/condition.hpp>
boost::condition_variable cond;
@@ -39,15 +38,30 @@ namespace po = boost::program_options;
void workThreadLoader(SotLoader *aSotLoader)
{
+ unsigned period = 1000; // micro seconds
+ if (ros::param::has("/sot/dt")) {
+ double periodd;
+ ros::param::get("/sot/dt", periodd);
+ period = unsigned(1e6 * periodd);
+ }
+ DataToLog dataToLog (5000);
+
while(aSotLoader->isDynamicGraphStopped())
{
- usleep(5000);
+ usleep(period);
}
+ struct timeval start, stop;
while(!aSotLoader->isDynamicGraphStopped())
{
+ gettimeofday(&start,0);
aSotLoader->oneIteration();
- usleep(5000);
+ gettimeofday(&stop,0);
+
+ unsigned long long dt = 1000000 * (stop.tv_sec - start.tv_sec) + (stop.tv_usec - start.tv_usec);
+ if (period > dt) {
+ usleep(period - (unsigned)dt);
+ }
}
cond.notify_all();
ros::waitForShutdown();
@@ -62,15 +76,22 @@ SotLoader::SotLoader():
torques_ (),
baseAtt_ (),
accelerometer_ (3),
- gyrometer_ (3)
+ gyrometer_ (3),
+ thread_ ()
{
readSotVectorStateParam();
initPublication();
}
+SotLoader::~SotLoader()
+{
+ dynamic_graph_stopped_ = true;
+ thread_.join();
+}
+
void SotLoader::startControlLoop()
{
- boost::thread thr(workThreadLoader, this);
+ thread_ = boost::thread (workThreadLoader, this);
}
void SotLoader::initializeRosNode(int argc, char *argv[])
--
GitLab