Skip to content
Snippets Groups Projects
Commit 499274c9 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Merge branch 'origin/devel'

parents 2e359762 e0bdf59a
No related branches found
Tags v2.5
No related merge requests found
Pipeline #1722 failed
Stage: test
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
+ 33
− 38
View file @ 499274c9
......@@ -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()
cmake @ 52d25e05
Compare 8e7bedfc...52d25e05
Subproject commit 8e7bedfcbd8524c0401a58fd74edc07c3d4308d0
Subproject commit 52d25e05c3b5dfd70c79b3e75787fdc78c6f695e
src/interpreter.cpp deleted 100644 → 0
+ 0
− 10
View file @ 2e359762
#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 ();
}
src/ros_joint_state.cpp deleted 100644 → 0
+ 0
− 178
View file @ 2e359762
#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.
src/ros_joint_state.hh deleted 100644 → 0
+ 0
− 46
View file @ 2e359762
#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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment