diff --git a/include/dynamic_graph_bridge/ros_interpreter.hh b/include/dynamic_graph_bridge/ros_interpreter.hh index 9988bd17fd6c8ab014ab1f9e1121ae71e3141320..88da3286542f978259ed6cc16685f11a7901dbbc 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 7ce1ce4570a661208eb839f4207ade847ae9d187..4497b6e861d96b1d5f323b31d9dcbb9a416dd332 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 25c241b5e0db503d2c3fc857b2bf29380487a690..2720158e1cb9c539b4f78004ef0e587f85996ddb 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 635c9cfcd327fa477daa5a051d0fb2e0f74c1a13..24b96e916a9cecaf616fc9144bd0da69beef5b10 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);