From d80dd4dedebce538bf29cc9d08df1cff11ba38b5 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Wed, 6 Feb 2019 19:43:18 +0100
Subject: [PATCH] Add gettimeofday for time computation. Revert "Remove useless
thread to start and stop dynamic graph"
This reverts commit 924c30135eef1cbda05f367b110b7af20de92c63.
---
src/ros_publish.cpp | 62 +++++++++++++++++++++++++++++++++++++++------
src/ros_publish.hh | 3 +++
2 files changed, 57 insertions(+), 8 deletions(-)
diff --git a/src/ros_publish.cpp b/src/ros_publish.cpp
index 05585b6..ee721c8 100644
--- a/src/ros_publish.cpp
+++ b/src/ros_publish.cpp
@@ -18,8 +18,12 @@
#include "dynamic_graph_bridge/ros_init.hh"
#include "ros_publish.hh"
+#define ENABLE_RT_LOG
+#include <dynamic-graph/real-time-logger.h>
+
namespace dynamicgraph
{
+
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosPublish, "RosPublish");
const double RosPublish::ROS_JOINT_STATE_PUBLISHER_RATE = 0.01;
@@ -142,12 +146,22 @@ namespace dynamicgraph
trigger_ (boost::bind (&RosPublish::trigger, this, _1, _2),
sotNOSIGNAL,
MAKE_SIGNAL_STRING(name, true, "int", "trigger")),
- rate_ (ROS_JOINT_STATE_PUBLISHER_RATE),
+ rate_ (0,10000000),
nextPublication_ ()
{
-
+ aofs_.open("/tmp/ros_publish.txt");
+ dgADD_OSTREAM_TO_RTLOG (aofs_);
+
try {
- nextPublication_ = ros::Time::now ();
+ if (ros::Time::isSimTime())
+ nextPublication_ = ros::Time::now ();
+ else
+ {
+ struct timeval timeofday;
+ gettimeofday(&timeofday,NULL);
+ nextPublicationRT_.tv_sec = timeofday.tv_sec;
+ nextPublicationRT_.tv_usec = timeofday.tv_usec;
+ }
} catch (const std::exception& exc) {
throw std::runtime_error ("Failed to call ros::Time::now ():" +
std::string (exc.what ()));
@@ -201,6 +215,7 @@ namespace dynamicgraph
RosPublish::~RosPublish ()
{
+ aofs_.close();
}
void RosPublish::display (std::ostream& os) const
@@ -256,13 +271,44 @@ namespace dynamicgraph
int& RosPublish::trigger (int& dummy, int t)
{
+
typedef std::map<std::string, bindedSignal_t>::iterator iterator_t;
+ ros::Time aTime;
+ struct timeval aTimeRT;
+ if (ros::Time::isSimTime())
+ {
+ aTime= ros::Time::now();
+ dgRTLOG() << "nextPublication_:"
+ << " " << nextPublication_.sec
+ << " " << nextPublication_.nsec
+ << " " << ros::Time::isValid()
+ << " " << ros::Time::isSimTime();
+
+ if (aTime <= nextPublication_)
+ return dummy;
+
+ nextPublication_ = aTime + rate_;
+ }
+ else
+ {
+ struct timeval aTimeRT;
+ gettimeofday(&aTimeRT,NULL);
+ // dgRTLOG() << "nextPublicationRT_:"
+ // << " " << nextPublicationRT_.tv_sec
+ // << " " << nextPublicationRT_.tv_usec
+ // << " " << ros::Time::isValid()
+ // << " " << ros::Time::isSimTime()
+ // << std::endl;
+ nextPublicationRT_.tv_sec = aTimeRT.tv_sec + rate_.sec;
+ nextPublicationRT_.tv_usec = aTimeRT.tv_usec + rate_.nsec/1000;
+ if (nextPublicationRT_.tv_usec>1000000)
+ {
+ nextPublicationRT_.tv_usec-=1000000;
+ nextPublicationRT_.tv_sec+=1;
+ }
+ }
- if (ros::Time::now() <= nextPublication_)
- return dummy;
-
- nextPublication_ = ros::Time::now() + rate_;
-
+ // dgRTLOG() << "after nextPublication_" << std::endl;
boost::mutex::scoped_lock lock (mutex_);
for (iterator_t it = bindedSignal_.begin ();
diff --git a/src/ros_publish.hh b/src/ros_publish.hh
index bd9f81b..5574016 100644
--- a/src/ros_publish.hh
+++ b/src/ros_publish.hh
@@ -16,6 +16,7 @@
# include "converter.hh"
# include "sot_to_ros.hh"
+# include <fstream>
namespace dynamicgraph
{
@@ -95,6 +96,8 @@ namespace dynamicgraph
ros::Duration rate_;
ros::Time nextPublication_;
boost::mutex mutex_;
+ std::ofstream aofs_;
+ struct timeval nextPublicationRT_;
};
} // end of namespace dynamicgraph.
--
GitLab