From 8d85afd97a0ae083155ad4975d45f2d71575c5e2 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Mon, 15 Jul 2013 16:31:50 +0200
Subject: [PATCH] Make sure interpreter is started before advertizing
run_command service.
Launching an application in a roslaunch file may cause to send python
command before the interpreter has read the prologue. This commit should fix
this issue.
---
include/dynamic_graph_bridge/ros_interpreter.hh | 5 ++++-
src/ros_import.cpp | 8 +++++++-
src/ros_interpreter.cpp | 5 ++++-
src/ros_joint_state.cpp | 8 +++++++-
4 files changed, 22 insertions(+), 4 deletions(-)
diff --git a/include/dynamic_graph_bridge/ros_interpreter.hh b/include/dynamic_graph_bridge/ros_interpreter.hh
index 9988bd1..88da328 100644
--- a/include/dynamic_graph_bridge/ros_interpreter.hh
+++ b/include/dynamic_graph_bridge/ros_interpreter.hh
@@ -28,7 +28,10 @@ namespace dynamicgraph
/// \param Command string to execute, result, stdout, stderr strings.
void runCommand(const std::string & command, std::string &result,
std::string &out, std::string &err);
-
+
+ /// Initialize service run_command
+ void startRosService ();
+
protected:
/// \brief Run a Python command and return result, stderr and stdout.
bool runCommandCallback (dynamic_graph_bridge::RunCommand::Request& req,
diff --git a/src/ros_import.cpp b/src/ros_import.cpp
index 7ce1ce4..4497b6e 100644
--- a/src/ros_import.cpp
+++ b/src/ros_import.cpp
@@ -133,8 +133,14 @@ namespace dynamicgraph
sotNOSIGNAL,
MAKE_SIGNAL_STRING(name, true, "int", "trigger")),
rate_ (ROS_JOINT_STATE_PUBLISHER_RATE),
- lastPublicated_ (ros::Time::now () - rate_ - rate_)
+ lastPublicated_ ()
{
+ try {
+ lastPublicated_ = ros::Time::now ();
+ } catch (const std::exception& exc) {
+ throw std::runtime_error ("Failed to call ros::Time::now ():" +
+ std::string (exc.what ()));
+ }
signalRegistration (trigger_);
trigger_.setNeedUpdateFromAllChildren (true);
diff --git a/src/ros_interpreter.cpp b/src/ros_interpreter.cpp
index 25c241b..2720158 100644
--- a/src/ros_interpreter.cpp
+++ b/src/ros_interpreter.cpp
@@ -8,10 +8,13 @@ namespace dynamicgraph
: interpreter_ (),
nodeHandle_ (nodeHandle),
runCommandSrv_ ()
+ {
+ }
+
+ void Interpreter::startRosService ()
{
runCommandCallback_t runCommandCb =
boost::bind (&Interpreter::runCommandCallback, this, _1, _2);
-
runCommandSrv_ =
nodeHandle_.advertiseService ("run_command", runCommandCb);
}
diff --git a/src/ros_joint_state.cpp b/src/ros_joint_state.cpp
index 635c9cf..24b96e9 100644
--- a/src/ros_joint_state.cpp
+++ b/src/ros_joint_state.cpp
@@ -112,8 +112,14 @@ namespace dynamicgraph
sotNOSIGNAL,
MAKE_SIGNAL_STRING(name, true, "int", "trigger")),
rate_ (ROS_JOINT_STATE_PUBLISHER_RATE),
- lastPublicated_ (ros::Time::now () - rate_ - rate_)
+ lastPublicated_ ()
{
+ try {
+ lastPublicated_ = ros::Time::now ();
+ } catch (const std::exception& exc) {
+ throw std::runtime_error ("Failed to call ros::Time::now ():" +
+ std::string (exc.what ()));
+ }
signalRegistration (state_ << trigger_);
trigger_.setNeedUpdateFromAllChildren (true);
--
GitLab