diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2a314f8abac10e11aba8f4c428e199715ce0a146..b3d0966e4b74d645ce60c4f6294e56fb57f5bc3f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -42,6 +42,7 @@ 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
)
SEARCH_FOR_EIGEN()
SEARCH_FOR_BOOST()
@@ -98,9 +99,7 @@ set_target_properties(ros_bridge PROPERTIES BUILD_WITH_INSTALL_RPATH True
macro(compile_plugin NAME)
message(lib path ${LIBRARY_OUTPUT_PATH})
file(MAKE_DIRECTORY "${LIBRARY_OUTPUT_PATH}/dynamic_graph/ros/${NAME}")
- add_library(${NAME} src/${NAME}.cpp src/${NAME}.hh)
- #pkg_config_use_dependency(${NAME} jrl-mal)
- #kg_config_use_dependency(${NAME} eigen3)
+ add_library(${NAME} SHARED src/${NAME}.cpp src/${NAME}.hh)
pkg_config_use_dependency(${NAME} dynamic-graph)
pkg_config_use_dependency(${NAME} sot-core)
#pkg_config_use_dependency(${NAME} jrl-dynamics-urdf)
@@ -108,7 +107,8 @@ macro(compile_plugin NAME)
add_dependencies(${NAME} ros_bridge)
target_link_libraries(${NAME} ros_bridge)
set_target_properties(${NAME} PROPERTIES BUILD_WITH_INSTALL_RPATH True)
- install(TARGETS ${NAME} DESTINATION lib)
+ set_target_properties(${NAME} PROPERTIES PREFIX "")
+ install(TARGETS ${NAME} DESTINATION lib/plugin)
dynamic_graph_python_module("ros/${NAME}"
@@ -165,7 +165,14 @@ pkg_config_use_dependency(interpreter dynamic_graph_bridge_msgs)
# Stand alone embedded intepreter with a robot controller.
add_executable(geometric_simu src/geometric_simu.cpp src/sot_loader.cpp)
pkg_config_use_dependency(geometric_simu roscpp)
-target_link_libraries(geometric_simu ros_bridge ${Boost_LIBRARIES} dl tf)
+target_link_libraries(geometric_simu ros_bridge ${Boost_LIBRARIES} ${CMAKE_DL_LIBS})
+
+# Sot loader library
+add_library(sot_loader src/sot_loader.cpp)
+pkg_config_use_dependency(sot_loader dynamic-graph)
+pkg_config_use_dependency(sot_loader sot-core)
+target_link_libraries(sot_loader ${Boost_LIBRARIES})
+install(TARGETS sot_loader DESTINATION lib)
add_subdirectory(src)
diff --git a/src/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
similarity index 94%
rename from src/sot_loader.hh
rename to include/dynamic_graph_bridge/sot_loader.hh
index c57f4731e20c224bea4cfbd77ca6280d131fe411..fe5b5b7dd9b4a39bf5d37688290ab3cca019364c 100644
--- a/src/sot_loader.hh
+++ b/include/dynamic_graph_bridge/sot_loader.hh
@@ -98,6 +98,9 @@ protected:
// Joint state to be published.
sensor_msgs::JointState joint_state_;
+ // \brief Start control loop
+ virtual void startControlLoop();
+
// Number of DOFs according to KDL.
int nbOfJoints_;
parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_;
@@ -117,6 +120,9 @@ public:
// \brief Load the SoT
void Initialization();
+ // \brief Create a thread for ROS.
+ void initializeRosNode(int argc, char *argv[]);
+
// \brief Compute one iteration of control.
// Basically calls fillSensors, the SoT and the readControl.
void oneIteration();
@@ -149,5 +155,9 @@ public:
// \brief Get Status of dg.
bool isDynamicGraphStopped()
{ return dynamic_graph_stopped_; }
+
+ // \brief Specify the name of the dynamic library.
+ void setDynamicLibraryName(std::string &afilename);
+
};
diff --git a/src/converter.hh b/src/converter.hh
index 79282bab6145b76e99e83b47a8bb8cffce6ae45d..d3d910a57b5a2b3b9165d383ae95584aa37d757a 100644
--- a/src/converter.hh
+++ b/src/converter.hh
@@ -117,8 +117,8 @@ namespace dynamicgraph
ROS_TO_SOT_IMPL(Matrix)
{
- //TODO: Confirm Ros Matrix Storage order.
- dst.resize (src.width, src.data.size () / src.width);
+ dst.resize (src.width, (unsigned int) src.data.size () /
+ (unsigned int)src.width);
for (unsigned i = 0; i < src.data.size (); ++i)
dst.data()[i] = src.data[i];
}
@@ -126,6 +126,7 @@ namespace dynamicgraph
// Homogeneous matrix.
SOT_TO_ROS_IMPL(sot::MatrixHomogeneous)
{
+
sot::VectorQuaternion q(src.linear());
dst.translation.x = src.translation()(0);
dst.translation.y = src.translation()(1);
@@ -135,6 +136,7 @@ namespace dynamicgraph
dst.rotation.y = q.y();
dst.rotation.z = q.z();
dst.rotation.w = q.w();
+
}
ROS_TO_SOT_IMPL(sot::MatrixHomogeneous)
@@ -297,8 +299,9 @@ namespace dynamicgraph
static ptime timeStart(date(1970,1,1));
time_duration diff = time - timeStart;
- uint32_t sec = diff.ticks ()/time_duration::rep_type::res_adjust ();
- uint32_t nsec = diff.fractional_seconds();
+ uint32_t sec = (unsigned int)diff.ticks ()/
+ (unsigned int)time_duration::rep_type::res_adjust ();
+ uint32_t nsec = (unsigned int)diff.fractional_seconds();
return ros::Time (sec, nsec);
}
diff --git a/src/geometric_simu.cpp b/src/geometric_simu.cpp
index 5a6a1ff581c50eae42a6315dc54b2b92dd7e2069..e622377aae6087e6def9b37c53749ec4e5cbc1de 100644
--- a/src/geometric_simu.cpp
+++ b/src/geometric_simu.cpp
@@ -20,28 +20,7 @@
#include <boost/thread/thread.hpp>
#include <boost/thread/condition.hpp>
-#include "sot_loader.hh"
-
-boost::condition_variable cond;
-boost::mutex mut;
-
-void workThread(SotLoader *aSotLoader)
-{
- {
- boost::lock_guard<boost::mutex> lock(mut);
- }
- while(aSotLoader->isDynamicGraphStopped())
- {
- usleep(5000);
- }
- while(!aSotLoader->isDynamicGraphStopped())
- {
- aSotLoader->oneIteration();
- usleep(5000);
- }
- cond.notify_all();
- ros::waitForShutdown();
-}
+#include <dynamic_graph_bridge/sot_loader.hh>
int main(int argc, char *argv[])
@@ -50,13 +29,11 @@ int main(int argc, char *argv[])
SotLoader aSotLoader;
if (aSotLoader.parseOptions(argc,argv)<0)
return -1;
- ros::NodeHandle n;
- ros::ServiceServer service = n.advertiseService("start_dynamic_graph",
- &SotLoader::start_dg,
- &aSotLoader);
- ROS_INFO("Ready to start dynamic graph.");
- boost::thread thr(workThread,&aSotLoader);
- boost::unique_lock<boost::mutex> lock(mut);
- cond.wait(lock);
- ros::spin();
+
+ aSotLoader.initializeRosNode(argc,argv);
+
+ while(true){
+ usleep(5000);
+ }
+
}
diff --git a/src/ros_joint_state.cpp b/src/ros_joint_state.cpp
index df04d8c4e5ff54b8922f577775a68868e0f3e0b4..c12655b136f7960b9389b9c191b4543c709d8fc9 100644
--- a/src/ros_joint_state.cpp
+++ b/src/ros_joint_state.cpp
@@ -165,7 +165,7 @@ namespace dynamicgraph
// Fill position.
jointState_.position.resize (s);
for (std::size_t i = 0; i < s; ++i)
- jointState_.position[i] = state_.access (t) (i);
+ jointState_.position[i] = state_.access (t) ((unsigned int)i);
publisher_.msg_ = jointState_;
publisher_.unlockAndPublish ();
diff --git a/src/ros_publish.hxx b/src/ros_publish.hxx
index deff4cf4a78847abb8d7f1e3f3c7655124ded856..4ff1d0154e90cc9d6c48aa57ee41043ee8d1f4df 100644
--- a/src/ros_publish.hxx
+++ b/src/ros_publish.hxx
@@ -56,7 +56,6 @@ namespace dynamicgraph
template <typename T>
void RosPublish::add (const std::string& signal, const std::string& topic)
{
- typedef typename SotToRos<T>::sot_t sot_t;
typedef typename SotToRos<T>::ros_t ros_t;
typedef typename SotToRos<T>::signalIn_t signal_t;
diff --git a/src/ros_time.cpp b/src/ros_time.cpp
index 50a23671b71ebdae4d8f2378fc59f2ef5e5274d7..0d50559a74509f4f326390c344aacf82a4a659ab 100644
--- a/src/ros_time.cpp
+++ b/src/ros_time.cpp
@@ -32,7 +32,7 @@ namespace dynamicgraph {
}
ptime&
- RosTime::update (ptime& time, const int& t)
+ RosTime::update (ptime& time, const int& )
{
time = rosTimeToPtime (ros::Time::now ());
return time;
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 10706aa28af0b2f1009ecaa8359abe141348eb68..a850624b4c135df67ff6ebea7a98f1fe85a4c8d4 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -20,15 +20,55 @@
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
-#include "sot_loader.hh"
+#include <dynamic_graph_bridge/sot_loader.hh>
+#include "dynamic_graph_bridge/ros_init.hh"
// POSIX.1-2001
#include <dlfcn.h>
+#include <boost/thread/thread.hpp>
+#include <boost/thread/condition.hpp>
+
+boost::condition_variable cond;
+boost::mutex mut;
+
using namespace std;
using namespace dynamicgraph::sot;
namespace po = boost::program_options;
+void createRosSpin(SotLoader *aSotLoader)
+{
+ ROS_INFO("createRosSpin started\n");
+ ros::NodeHandle n;
+
+ ros::ServiceServer service = n.advertiseService("start_dynamic_graph",
+ &SotLoader::start_dg,
+ aSotLoader);
+
+ ros::ServiceServer service2 = n.advertiseService("stop_dynamic_graph",
+ &SotLoader::stop_dg,
+ aSotLoader);
+
+
+ ros::waitForShutdown();
+}
+
+void workThreadLoader(SotLoader *aSotLoader)
+{
+ while(aSotLoader->isDynamicGraphStopped())
+ {
+ usleep(5000);
+ }
+
+ while(!aSotLoader->isDynamicGraphStopped())
+ {
+ aSotLoader->oneIteration();
+ usleep(5000);
+ }
+ cond.notify_all();
+ ros::waitForShutdown();
+}
+
SotLoader::SotLoader():
dynamic_graph_stopped_(true),
sensorsIn_ (),
@@ -47,8 +87,8 @@ SotLoader::SotLoader():
int SotLoader::initPublication()
{
- ros::NodeHandle n;
-
+ ros::NodeHandle & n = dynamicgraph::rosInit(false);
+
// Prepare message to be published
joint_pub_ = n.advertise<sensor_msgs::JointState>("joint_states", 1);
@@ -56,6 +96,26 @@ int SotLoader::initPublication()
return 0;
}
+void SotLoader::startControlLoop()
+{
+ boost::thread thr(workThreadLoader, this);
+}
+
+void SotLoader::initializeRosNode(int , char *[])
+{
+ ROS_INFO("Ready to start dynamic graph.");
+ boost::unique_lock<boost::mutex> lock(mut);
+ boost::thread thr2(createRosSpin,this);
+
+ startControlLoop();
+}
+
+
+void SotLoader::setDynamicLibraryName(std::string &afilename)
+{
+ dynamicLibraryName_ = afilename;
+}
+
int SotLoader::readSotVectorStateParam()
{
std::map<std::string,int> from_state_name_to_state_vector;