From 876acbcc31a979d09c4fc52e2c6f2f9ed0482314 Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Wed, 14 Mar 2012 14:59:56 +0100
Subject: [PATCH] Read the joint names in the robot model to fill the
joint_states message.
Previous version of abstract-robot-dynamics did not contain the joint
name. Therefore, we had no way to forward the joint names to ROS.
This patch changes the ros_joint_state entity to add a
retrieveJointNames command. This command uses the pool to retrieve an
instance of the Dynamic entity (i.e. robot.dynamic) by using the
entity name. It then iterates on the robot structure to fill the joint
names in the message.
This command is automatically called in the Ros class so this change
is transparent for the end-user.
However, no more assumptions are made on the robot structure so the
sent message is exactly matching the robot structure loaded in the
dynamic entity. In particular, the free floating DOFs are sent and the
hard coded hand values have been removed.
ROS, in fact, requires DOF names and not joint names. Therefore, the
names are computed as follow:
- 0 dof: ignored,
- 1 dof: dof name is the joint name,
- >1 dof: <joint name>_<dof id>
---
CMakeLists.txt | 154 +++++++++++++--------------------
src/dynamic_graph/ros/ros.py | 1 +
src/ros_joint_state.cpp | 159 ++++++++++++++++++++++++-----------
src/ros_joint_state.hh | 5 ++
4 files changed, 174 insertions(+), 145 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f9a4ac4..24e0fa8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,7 +1,7 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-include(FindPkgConfig)
+INCLUDE(cmake/base.cmake)
set(ROS_BUILD_TYPE RelWithDebInfo)
@@ -15,10 +15,17 @@ rosbuild_gensrv()
rosbuild_add_boost_directories()
-pkg_check_modules(JRL_MAL REQUIRED jrl-mal)
-pkg_check_modules(DYNAMIC_GRAPH REQUIRED dynamic-graph)
-pkg_check_modules(DYNAMIC_GRAPH_PYTHON REQUIRED dynamic-graph-python)
-pkg_check_modules(SOT_CORE REQUIRED sot-core)
+SET(PKG_CONFIG_ADDITIONAL_VARIABLES
+ ${PKG_CONFIG_ADDITIONAL_VARIABLES}
+ plugindirname
+ plugindir
+ )
+
+ADD_REQUIRED_DEPENDENCY(jrl-mal)
+ADD_REQUIRED_DEPENDENCY(dynamic-graph)
+ADD_REQUIRED_DEPENDENCY(dynamic-graph-python)
+ADD_REQUIRED_DEPENDENCY(sot-core)
+ADD_REQUIRED_DEPENDENCY(sot-dynamic)
include_directories(include)
@@ -36,105 +43,58 @@ rosbuild_add_library(ros_bridge
# are not installed.
set_target_properties(ros_bridge PROPERTIES BUILD_WITH_INSTALL_RPATH True)
-rosbuild_add_library(ros_import src/ros_import.cpp src/ros_import.hh)
-rosbuild_add_compile_flags(ros_import ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS}
- ${SOT_CORE_CFLAGS})
-rosbuild_add_link_flags(ros_import ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS}
- ${SOT_CORE_LDFLAGS})
-target_link_libraries(ros_import ros_bridge)
-target_link_libraries(ros_import
- ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES})
-set_target_properties(ros_import PROPERTIES BUILD_WITH_INSTALL_RPATH True)
-
-rosbuild_add_library(ros_export src/ros_export.cpp src/ros_export.hh)
-rosbuild_add_compile_flags(ros_export ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS}
- ${SOT_CORE_CFLAGS})
-rosbuild_add_link_flags(ros_export ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS}
- ${SOT_CORE_LDFLAGS})
-target_link_libraries(ros_export ros_bridge)
-target_link_libraries(ros_export
- ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES})
-set_target_properties(ros_export PROPERTIES BUILD_WITH_INSTALL_RPATH True)
-
-rosbuild_add_library(ros_joint_state
- src/ros_joint_state.cpp src/ros_joint_state)
-rosbuild_add_compile_flags(ros_joint_state
- ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS}
- ${SOT_CORE_CFLAGS})
-rosbuild_add_link_flags(ros_joint_state ${JRL_MAL_LDFLAGS}
- ${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS})
-target_link_libraries(ros_joint_state ros_bridge)
-target_link_libraries(ros_joint_state
- ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES})
-set_target_properties(ros_joint_state PROPERTIES BUILD_WITH_INSTALL_RPATH True)
-
-rosbuild_add_library(ros_interpreter
- src/ros_interpreter.cpp)
-rosbuild_add_compile_flags(ros_interpreter
- ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} ${DYNAMIC_GRAPH_PYTHON_CFLAGS}
- ${SOT_CORE_CFLAGS})
-rosbuild_add_link_flags(ros_interpreter ${JRL_MAL_LDFLAGS}
- ${DYNAMIC_GRAPH_LDFLAGS} ${DYNAMIC_GRAPH_PYTHON_LDFLAGS} ${SOT_CORE_LDFLAGS})
-target_link_libraries(ros_interpreter ros_bridge)
-target_link_libraries(ros_interpreter
- ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES}
- ${DYNAMIC_GRAPH_PYTHON_LIBRARIES} ${SOT_CORE_LIBRARIES})
-set_target_properties(ros_interpreter PROPERTIES BUILD_WITH_INSTALL_RPATH True)
-
-# Stand alone remote dynamic-graph Python interpreter.
-rosbuild_add_executable(interpreter src/interpreter.cpp)
-target_link_libraries(interpreter ros_interpreter)
-rosbuild_add_compile_flags(interpreter ${SOT_CORE_CFLAGS})
-rosbuild_add_link_flags(interpreter ${JRL_MAL_LDFLAGS}
- ${DYNAMIC_GRAPH_LDFLAGS} ${DYNAMIC_GRAPH_PYTHON_LDFLAGS} ${SOT_CORE_LDFLAGS})
-set_target_properties(interpreter PROPERTIES BUILD_WITH_INSTALL_RPATH True)
-
-INSTALL(TARGETS ros_bridge DESTINATION lib)
-INSTALL(TARGETS ros_import DESTINATION lib)
-INSTALL(TARGETS ros_export DESTINATION lib)
-INSTALL(TARGETS ros_joint_state DESTINATION lib)
-INSTALL(TARGETS ros_interpreter DESTINATION lib)
-INSTALL(TARGETS interpreter DESTINATION bin)
+MACRO(COMPILE_PLUGIN NAME)
+ rosbuild_add_library(${NAME} src/${NAME}.cpp src/${NAME}.hh)
+ PKG_CONFIG_USE_DEPENDENCY(${NAME} jrl-mal)
+ PKG_CONFIG_USE_DEPENDENCY(${NAME} dynamic-graph)
+ PKG_CONFIG_USE_DEPENDENCY(${NAME} sot-core)
+ target_link_libraries(${NAME} ros_bridge)
+ set_target_properties(${NAME} PROPERTIES BUILD_WITH_INSTALL_RPATH True)
+ INSTALL(TARGETS ${NAME} DESTINATION lib)
+
+ DYNAMIC_GRAPH_PYTHON_MODULE("ros/${NAME}"
+ ${NAME}
+ ros/${NAME}/wrap
+ )
+
+ SET_TARGET_PROPERTIES(ros/${NAME}/wrap
+ PROPERTIES
+ COMPILE_FLAGS
+ ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} ${SOT_CORE_CFLAGS}
+ LINK_FLAGS
+ ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS}
+ )
+ENDMACRO()
INCLUDE(cmake/python.cmake)
INCLUDE_DIRECTORIES(${DYNAMIC_GRAPH_INCLUDE_DIRS})
LINK_DIRECTORIES(${DYNAMIC_GRAPH_LIBRARY_DIRS})
-DYNAMIC_GRAPH_PYTHON_MODULE("ros/ros_import"
- ros_import
- ros/ros_import/wrap
- )
-SET_TARGET_PROPERTIES(ros/ros_import/wrap
- PROPERTIES
- COMPILE_FLAGS
- ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} ${SOT_CORE_CFLAGS}
- LINK_FLAGS
- ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS}
- )
+COMPILE_PLUGIN(ros_import)
+COMPILE_PLUGIN(ros_export)
+COMPILE_PLUGIN(ros_export)
-DYNAMIC_GRAPH_PYTHON_MODULE("ros/ros_export"
- ros_export
- ros/ros_export/wrap
- )
-SET_TARGET_PROPERTIES(ros/ros_export/wrap
- PROPERTIES
- COMPILE_FLAGS
- ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} ${SOT_CORE_CFLAGS}
- LINK_FLAGS
- ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS}
- )
+COMPILE_PLUGIN(ros_joint_state)
+target_link_libraries(ros_joint_state "${DYNAMIC_GRAPH_PLUGINDIR}/dynamic.so")
-DYNAMIC_GRAPH_PYTHON_MODULE("ros/ros_joint_state"
- ros_joint_state
- ros/ros_joint_state/wrap
- )
-SET_TARGET_PROPERTIES(ros/ros_joint_state/wrap
- PROPERTIES
- COMPILE_FLAGS
- ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} ${SOT_CORE_CFLAGS}
- LINK_FLAGS
- ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS}
- )
+# ros_interperter library.
+rosbuild_add_library(ros_interpreter src/ros_interpreter.cpp)
+PKG_CONFIG_USE_DEPENDENCY(ros_interpreter jrl-mal)
+PKG_CONFIG_USE_DEPENDENCY(ros_interpreter dynamic-graph)
+PKG_CONFIG_USE_DEPENDENCY(ros_interpreter sot-core)
+target_link_libraries(ros_interpreter ros_bridge)
+set_target_properties(ros_interpreter PROPERTIES BUILD_WITH_INSTALL_RPATH True)
+INSTALL(TARGETS ros_interpreter DESTINATION lib)
+
+# Stand alone remote dynamic-graph Python interpreter.
+rosbuild_add_executable(interpreter src/interpreter.cpp)
+target_link_libraries(interpreter ros_interpreter)
+PKG_CONFIG_USE_DEPENDENCY(interpreter jrl-mal)
+PKG_CONFIG_USE_DEPENDENCY(interpreter dynamic-graph)
+PKG_CONFIG_USE_DEPENDENCY(interpreter sot-core)
+PKG_CONFIG_USE_DEPENDENCY(interpreter sot-dynamic)
+set_target_properties(interpreter PROPERTIES BUILD_WITH_INSTALL_RPATH True)
+INSTALL(TARGETS interpreter DESTINATION bin)
ADD_SUBDIRECTORY(src)
diff --git a/src/dynamic_graph/ros/ros.py b/src/dynamic_graph/ros/ros.py
index e0a8210..edfcead 100644
--- a/src/dynamic_graph/ros/ros.py
+++ b/src/dynamic_graph/ros/ros.py
@@ -15,6 +15,7 @@ class Ros(object):
self.rosImport = RosImport('rosImport{0}'.format(suffix))
self.rosExport = RosExport('rosExport{0}'.format(suffix))
self.rosJointState = RosJointState('rosJointState{0}'.format(suffix))
+ self.rosJointState.retrieveJointNames(self.robot.dynamic.name)
plug(self.robot.device.state, self.rosJointState.state)
self.robot.device.after.addSignal('{0}.trigger'.format(self.rosImport.name))
diff --git a/src/ros_joint_state.cpp b/src/ros_joint_state.cpp
index f6a78b5..7779137 100644
--- a/src/ros_joint_state.cpp
+++ b/src/ros_joint_state.cpp
@@ -1,46 +1,106 @@
+#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/dynamic.h>
#include "dynamic_graph_bridge/ros_init.hh"
#include "ros_joint_state.hh"
#include "sot_to_ros.hh"
-//FIXME: this is very very wrong but we need to update abstract-robot-dynamics
-// to publish joint names.
-static const char* dof_names[] =
- {
- "RLEG_JOINT0", "RLEG_JOINT1", "RLEG_JOINT2",
- "RLEG_JOINT3", "RLEG_JOINT4", "RLEG_JOINT5",
- "LLEG_JOINT0", "LLEG_JOINT1", "LLEG_JOINT2",
- "LLEG_JOINT3", "LLEG_JOINT4", "LLEG_JOINT5",
- "CHEST_JOINT0", "CHEST_JOINT1",
- "HEAD_JOINT0", "HEAD_JOINT1",
- "RARM_JOINT0", "RARM_JOINT1", "RARM_JOINT2",
- "RARM_JOINT3", "RARM_JOINT4", "RARM_JOINT5", "RARM_JOINT6",
- "LARM_JOINT0", "LARM_JOINT1", "LARM_JOINT2",
- "LARM_JOINT3", "LARM_JOINT4", "LARM_JOINT5", "LARM_JOINT6",
- "RHAND_JOINT0", "RHAND_JOINT1",
- "RHAND_JOINT2", "RHAND_JOINT3", "RHAND_JOINT4",
- "LHAND_JOINT0", "LHAND_JOINT1",
- "LHAND_JOINT2", "LHAND_JOINT3", "LHAND_JOINT4",
- 0
- };
-
-// Number of dofs in left and right robot hands.
-//
-// This part is very poorly written: currently, dynamic-graph uses a
-// lighter robot model without the hands. This model do not have the
-// dofs corresponding to the robot hand dofs. Therefore we put them
-// manually to zero to please ROS. If this is not the case, some
-// packages such as rviz will behave badly.
-static const std::size_t handsDofsCount = 10;
-
namespace dynamicgraph
{
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosJointState, "RosJointState");
+ 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, CjrlJoint* joint)
+ {
+ if (!joint)
+ return;
+ // Ignore anchors.
+ if (joint->numberDof() != 0)
+ {
+ // If we only have one dof, the dof name is the joint name.
+ if (joint->numberDof() == 1)
+ {
+ jointState.name[joint->rankInConfiguration()] =
+ joint->getName();
+ }
+ // ...otherwise, the dof name is the joint name on which
+ // the dof id is appended.
+ else
+ for (unsigned i = 0; i < joint->numberDof(); ++i)
+ {
+ boost::format fmt("%1%_%2%");
+ fmt % joint->getName();
+ fmt % i;
+ jointState.name[joint->rankInConfiguration() + i] =
+ fmt.str();
+ }
+ }
+ for (unsigned i = 0; i < joint->countChildJoints (); ++i)
+ buildJointNames (jointState, joint->childJoint (i));
+ }
+ } // 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::Dynamic* dynamic =
+ dynamic_cast<dynamicgraph::sot::Dynamic*>
+ (&dynamicgraph::PoolStorage::getInstance ()->getEntity (name));
+ if (!dynamic)
+ {
+ std::cerr << "entity is not a Dynamic entity" << std::endl;
+ return Value ();
+ }
+
+ CjrlHumanoidDynamicRobot* robot = dynamic->m_HDR;
+ if (!robot)
+ {
+ std::cerr << "no robot in the dynamic entity" << std::endl;
+ return Value ();
+ }
+
+ entity.jointState ().name.resize (robot->numberDof());
+ buildJointNames (entity.jointState (), robot->rootJoint());
+ 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
@@ -61,7 +121,17 @@ namespace dynamicgraph
jointState_.header.seq = 0;
jointState_.header.stamp.sec = 0;
jointState_.header.stamp.nsec = 0;
- jointState_.header.frame_id = "odom";
+ 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 ()
@@ -76,7 +146,14 @@ namespace dynamicgraph
lastPublicated_ = ros::Time::now ();
// State size without the free floating.
- std::size_t s = state_.access (t).size () - 6;
+ 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;
@@ -85,24 +162,10 @@ namespace dynamicgraph
jointState_.header.stamp.sec = now.sec;
jointState_.header.stamp.nsec = now.nsec;
- // Fill names if needed.
- if (jointState_.name.size () != s + handsDofsCount)
- {
- jointState_.name.resize (s + handsDofsCount);
- for (std::size_t i = 0; i < s + handsDofsCount; ++i)
- {
- if (dof_names[i] == 0)
- break;
- jointState_.name[i] = dof_names[i];
- }
- }
-
// Fill position.
- jointState_.position.resize (s + handsDofsCount);
+ jointState_.position.resize (s);
for (std::size_t i = 0; i < s; ++i)
- jointState_.position[i] = state_.access (t) (i + 6);
- for (std::size_t i = 0; i < handsDofsCount; ++i)
- jointState_.position[i + s] = 0.;
+ jointState_.position[i] = state_.access (t) (i);
publisher_.msg_ = jointState_;
publisher_.unlockAndPublish ();
diff --git a/src/ros_joint_state.hh b/src/ros_joint_state.hh
index 9d9ff2b..6ce379c 100644
--- a/src/ros_joint_state.hh
+++ b/src/ros_joint_state.hh
@@ -27,6 +27,11 @@ namespace dynamicgraph
virtual ~RosJointState ();
int& trigger (int&, int);
+
+ sensor_msgs::JointState& jointState ()
+ {
+ return jointState_;
+ }
private:
ros::NodeHandle& nh_;
signalVectorIn_t state_;
--
GitLab