diff --git a/CMakeLists.txt b/CMakeLists.txt
index f6db65a8e3607f51a009135d094a4a41489afd6f..9afa0843fac00f79b5423c204212fa0ebdeb101b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -59,10 +59,15 @@ set(CXX_DISABLE_WERROR False)
set(CUSTOM_HEADER_DIR dynamic_graph_bridge)
set(${PROJECT_NAME}_HEADERS
include/dynamic_graph_bridge/ros_init.hh
- include/dynamic_graph_bridge/ros_interpreter.hh
include/dynamic_graph_bridge/sot_loader.hh
include/dynamic_graph_bridge/sot_loader_basic.hh
)
+
+IF(BUILD_PYTHON_INTERFACE)
+ set(${PROJECT_NAME}_HEADERS ${${PROJECT_NAME}_HEADERS}
+ include/dynamic_graph_bridge/ros_interpreter.hh )
+ENDIF(BUILD_PYTHON_INTERFACE)
+
SEARCH_FOR_EIGEN()
SEARCH_FOR_BOOST()
@@ -130,8 +135,19 @@ macro(compile_plugin NAME)
set_target_properties(${NAME} PROPERTIES BUILD_WITH_INSTALL_RPATH True)
set_target_properties(${NAME} PROPERTIES PREFIX "")
install(TARGETS ${NAME} DESTINATION lib/plugin)
+endmacro()
+
+# Build Sot Entities
+set(listplugins ros_publish ros_subscribe ros_queued_subscribe ros_tf_listener ros_time)
+
+foreach(aplugin ${listplugins})
+ compile_plugin(${aplugin})
+endforeach()
+
+target_link_libraries(ros_publish ros_bridge)
- IF(BUILD_PYTHON_INTERFACE)
+IF(BUILD_PYTHON_INTERFACE)
+ foreach(NAME ${listplugins})
dynamic_graph_python_module("ros/${NAME}"
${NAME}
ros/${NAME}/wrap
@@ -141,25 +157,10 @@ macro(compile_plugin NAME)
PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap dynamic_graph)
PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap sot-core)
PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap dynamic_graph_bridge_msgs)
- ENDIF(BUILD_PYTHON_INTERFACE)
-endmacro()
-
-# Build Sot Entities
-compile_plugin(ros_publish)
-compile_plugin(ros_subscribe)
-compile_plugin(ros_queued_subscribe)
-compile_plugin(ros_tf_listener)
-compile_plugin(ros_time)
-compile_plugin(ros_joint_state)
-pkg_config_use_dependency(ros_joint_state pinocchio)
-
-target_link_libraries(ros_joint_state "${DYNAMIC_GRAPH_PLUGINDIR}/dp-dynamic.so")
-target_link_libraries(ros_publish ros_bridge)
+ endforeach()
-#compile_plugin(robot_model)
+ # ros_interperter library.
-# ros_interperter library.
-IF(BUILD_PYTHON_INTERFACE)
add_library(ros_interpreter src/ros_interpreter.cpp)
pkg_config_use_dependency(ros_interpreter dynamic-graph)
pkg_config_use_dependency(ros_interpreter sot-core)
@@ -173,16 +174,6 @@ IF(BUILD_PYTHON_INTERFACE)
message(cmakeinstalllibdir " is ${CMAKE_INSTALL_LIBDIR} ")
install(TARGETS ros_interpreter DESTINATION lib)
- # Stand alone remote dynamic-graph Python interpreter.
- add_executable(interpreter src/interpreter.cpp)
- add_dependencies(interpreter ros_interpreter)
- target_link_libraries(interpreter ros_interpreter)
- pkg_config_use_dependency(interpreter dynamic-graph)
- pkg_config_use_dependency(interpreter sot-core)
- pkg_config_use_dependency(interpreter sot-dynamic-pinocchio)
- pkg_config_use_dependency(interpreter dynamic_graph_bridge_msgs)
- # set_target_properties(interpreter PROPERTIES BUILD_WITH_INSTALL_RPATH True)
- #install(TARGETS interpreter DESTINATION bin)
ENDIF(BUILD_PYTHON_INTERFACE)
# Stand alone embedded intepreter with a robot controller.
@@ -213,23 +204,27 @@ catkin_package(CATKIN_DEPENDS message_runtime roscpp realtime_tools tf2_bullet $
# Add libraries in pc file generated by cmake submodule
PKG_CONFIG_APPEND_LIBS(ros_bridge sot_loader)
+
IF(BUILD_PYTHON_INTERFACE)
PKG_CONFIG_APPEND_LIBS(ros_interpreter)
+
+ #install ros executables
+ install(PROGRAMS
+ ${CMAKE_SOURCE_DIR}/scripts/robot_pose_publisher
+ ${CMAKE_SOURCE_DIR}/scripts/run_command
+ ${CMAKE_SOURCE_DIR}/scripts/tf_publisher
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ )
+
+ # Service file.
+ install(FILES ./srv/RunPythonFile.srv DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/srv)
+
ENDIF(BUILD_PYTHON_INTERFACE)
-#install ros executables
-install(PROGRAMS
- ${CMAKE_SOURCE_DIR}/scripts/robot_pose_publisher
- ${CMAKE_SOURCE_DIR}/scripts/run_command
- ${CMAKE_SOURCE_DIR}/scripts/tf_publisher
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
- )
message(cmake_install_bindir " is ${CMAKE_INSTALL_BINDIR} ")
install(TARGETS geometric_simu DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(FILES manifest.xml DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/)
-# Service file.
-install(FILES ./srv/RunPythonFile.srv DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/srv)
SETUP_PROJECT_FINALIZE()
diff --git a/cmake b/cmake
index 8e7bedfcbd8524c0401a58fd74edc07c3d4308d0..52d25e05c3b5dfd70c79b3e75787fdc78c6f695e 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit 8e7bedfcbd8524c0401a58fd74edc07c3d4308d0
+Subproject commit 52d25e05c3b5dfd70c79b3e75787fdc78c6f695e
diff --git a/src/interpreter.cpp b/src/interpreter.cpp
deleted file mode 100644
index db9efb67707d33d9f42b57771dae3f3b4d183245..0000000000000000000000000000000000000000
--- a/src/interpreter.cpp
+++ /dev/null
@@ -1,10 +0,0 @@
-#include <dynamic_graph_bridge/ros_init.hh>
-#include <dynamic_graph_bridge/ros_interpreter.hh>
-
-int main ()
-{
- // we spin explicitly so we do not need an async spinner here.
- ros::NodeHandle& nodeHandle = dynamicgraph::rosInit (false);
- dynamicgraph::Interpreter interpreter (nodeHandle);
- ros::spin ();
-}
diff --git a/src/ros_joint_state.cpp b/src/ros_joint_state.cpp
deleted file mode 100644
index d2e1ba928c4a03b3586cbccd7e33b75635e33a3c..0000000000000000000000000000000000000000
--- a/src/ros_joint_state.cpp
+++ /dev/null
@@ -1,178 +0,0 @@
-#include <boost/assign.hpp>
-#include <boost/bind.hpp>
-#include <boost/format.hpp>
-
-#include <dynamic-graph/command.h>
-#include <dynamic-graph/factory.h>
-#include <dynamic-graph/pool.h>
-#include <sot-dynamic-pinocchio/dynamic-pinocchio.h>
-
-#include "dynamic_graph_bridge/ros_init.hh"
-#include "ros_joint_state.hh"
-#include "sot_to_ros.hh"
-
-namespace dynamicgraph
-{
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosJointState, "RosJointState");
- const double RosJointState::ROS_JOINT_STATE_PUBLISHER_RATE = 0.01;
-
- namespace command
- {
- using ::dynamicgraph::command::Command;
- using ::dynamicgraph::command::Value;
-
- class RetrieveJointNames : public Command
- {
- public:
- RetrieveJointNames (RosJointState& entity,
- const std::string& docstring);
- virtual Value doExecute ();
- };
-
- RetrieveJointNames::RetrieveJointNames
- (RosJointState& entity, const std::string& docstring)
- : Command (entity, boost::assign::list_of (Value::STRING), docstring)
- {
-}
-
- namespace {
- void buildJointNames (sensor_msgs::JointState& jointState, se3::Model* robot_model) {
- int cnt = 0;
- for (int i=1;i<robot_model->njoints;i++) {
- // Ignore anchors.
- if (se3::nv(robot_model->joints[i]) != 0) {
- // If we only have one dof, the dof name is the joint name.
- if (se3::nv(robot_model->joints[i]) == 1) {
- jointState.name[cnt] = robot_model->names[i];
- cnt++;
- }
- else {
- // ...otherwise, the dof name is the joint name on which
- // the dof id is appended.
- int joint_dof = se3::nv(robot_model->joints[i]);
- for(int j = 0; j<joint_dof; j++) {
- boost::format fmt("%1%_%2%");
- fmt % robot_model->names[i];
- fmt % j;
- jointState.name[cnt + j] = fmt.str();
- }
- cnt+=joint_dof;
- }
- }
- }
- }
- } // end of anonymous namespace
-
- Value RetrieveJointNames::doExecute ()
- {
- RosJointState& entity = static_cast<RosJointState&> (owner ());
-
- std::vector<Value> values = getParameterValues ();
- std::string name = values[0].value ();
-
- if (!dynamicgraph::PoolStorage::getInstance ()->existEntity (name))
- {
- std::cerr << "invalid entity name" << std::endl;
- return Value ();
- }
-
- dynamicgraph::sot::DynamicPinocchio* dynamic =
- dynamic_cast<dynamicgraph::sot::DynamicPinocchio*>
- (&dynamicgraph::PoolStorage::getInstance ()->getEntity (name));
- if (!dynamic)
- {
- std::cerr << "entity is not a DynamicPinocchio entity" << std::endl;
- return Value ();
- }
-
- se3::Model* robot_model = dynamic->m_model;
- if (robot_model->njoints == 1)
- {
- std::cerr << "no robot in the dynamic entity" << std::endl;
- return Value ();
- }
-
- entity.jointState ().name.resize (robot_model->nv);
- buildJointNames (entity.jointState (), robot_model);
- return Value ();
- }
- } // end of namespace command.
-
- RosJointState::RosJointState (const std::string& n)
- : Entity (n),
- // do not use callbacks, so do not create a useless spinner
- nh_ (rosInit (false)),
- state_ (0, MAKE_SIGNAL_STRING(name, true, "Vector", "state")),
- publisher_ (nh_, "dynamic_graph/joint_states", 5),
- jointState_ (),
- trigger_ (boost::bind (&RosJointState::trigger, this, _1, _2),
- sotNOSIGNAL,
- MAKE_SIGNAL_STRING(name, true, "int", "trigger")),
- rate_ (ROS_JOINT_STATE_PUBLISHER_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);
-
- // Fill header.
- jointState_.header.seq = 0;
- jointState_.header.stamp.sec = 0;
- jointState_.header.stamp.nsec = 0;
- jointState_.header.frame_id = "";
-
- std::string docstring =
- "\n"
- " Retrieve joint names using robot model contained in a Dynamic entity\n"
- "\n"
- " Input:\n"
- " - dynamic entity name (i.e. robot.dynamic.name)\n"
- "\n";
- addCommand ("retrieveJointNames",
- new command::RetrieveJointNames (*this, docstring));
- }
-
- RosJointState::~RosJointState ()
- {}
-
- int&
- RosJointState::trigger (int& dummy, int t)
- {
- ros::Duration dt = ros::Time::now () - lastPublicated_;
- if (dt > rate_ && publisher_.trylock ())
- {
- lastPublicated_ = ros::Time::now ();
-
- // State size without the free floating.
- std::size_t s = state_.access (t).size ();
-
- // Safety check: if data are inconsistent, clear
- // the joint names to avoid sending erroneous data.
- // This should not happen unless you change
- // the robot model at run-time.
- if (s != jointState_.name.size())
- jointState_.name.clear();
-
- // Update header.
- ++jointState_.header.seq;
-
- ros::Time now = ros::Time::now ();
- jointState_.header.stamp.sec = now.sec;
- jointState_.header.stamp.nsec = now.nsec;
-
- // Fill position.
- jointState_.position.resize (s);
- for (std::size_t i = 0; i < s; ++i)
- jointState_.position[i] = state_.access (t) ((unsigned int)i);
-
- publisher_.msg_ = jointState_;
- publisher_.unlockAndPublish ();
- }
- return dummy;
- }
-} // end of namespace dynamicgraph.
diff --git a/src/ros_joint_state.hh b/src/ros_joint_state.hh
deleted file mode 100644
index 10ee53acf9ea47494aaacef2c864bbab64fe4b7b..0000000000000000000000000000000000000000
--- a/src/ros_joint_state.hh
+++ /dev/null
@@ -1,46 +0,0 @@
-#ifndef DYNAMIC_GRAPH_JOINT_STATE_HH
-# define DYNAMIC_GRAPH_JOINT_STATE_HH
-# include <dynamic-graph/entity.h>
-# include <dynamic-graph/signal-ptr.h>
-# include <dynamic-graph/signal-time-dependent.h>
-
-# include <ros/ros.h>
-# include <realtime_tools/realtime_publisher.h>
-# include <sensor_msgs/JointState.h>
-
-# include "converter.hh"
-# include "sot_to_ros.hh"
-
-namespace dynamicgraph
-{
- /// \brief Publish current robot configuration to ROS.
- class RosJointState : public dynamicgraph::Entity
- {
- DYNAMIC_GRAPH_ENTITY_DECL();
- public:
- /// \brief Vector input signal.
- typedef SignalPtr<Vector, int> signalVectorIn_t;
-
- static const double ROS_JOINT_STATE_PUBLISHER_RATE;
-
- RosJointState (const std::string& n);
- virtual ~RosJointState ();
-
- int& trigger (int&, int);
-
- sensor_msgs::JointState& jointState ()
- {
- return jointState_;
- }
- private:
- ros::NodeHandle& nh_;
- signalVectorIn_t state_;
- realtime_tools::RealtimePublisher<sensor_msgs::JointState> publisher_;
- sensor_msgs::JointState jointState_;
- dynamicgraph::SignalTimeDependent<int,int> trigger_;
- ros::Duration rate_;
- ros::Time lastPublicated_;
- };
-} // end of namespace dynamicgraph.
-
-#endif //! DYNAMIC_GRAPH_JOINT_STATE_HH