From 2be0ffb4598adff211b9925dbe68cebf8efa4eb7 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@gmail.com>
Date: Wed, 10 Jul 2013 16:34:13 +0200
Subject: [PATCH] Publish joint-states using the SoT
Setting up ROS:
rosparam load `rospack find hrp2_14_description`/sot/hrp2_14_reduced.yaml
rosrun robot_state_publisher state_publisher
Display the robot using the info given in
https://trac.laas.fr/gepetto/hrp2/hrp2_14_description/html/reference/
To repeat at each simulation:
rosrun dynamic_graph_bridge geometric_simu --input-file ~/devel/ros-unstable-2/install/lib/libsot-hrp2-14-controller.so
rosrun dynamic_graph_bridge run_command
>>> from dynamic_graph.sot.application.velocity.precomputed_tasks import *
>>> solver=initialize(robot)
rosservice call /start_dynamic_graph
>>> Put_whatever_you_want_in_python
The rationale is to use a state vector map provided by the robot stack.
For instance for hrp-2 we have a yaml file providing the ros parameter:
/sot/state_vector_map
which specifies the state vector:
[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]
---
CMakeLists.txt | 1 -
src/geometric_simu.cpp | 32 +++++++++++---
src/sot_loader.cpp | 95 ++++++++++++++++++++++++++++++++++--------
src/sot_loader.hh | 59 +++++++++++++++++++++++---
4 files changed, 157 insertions(+), 30 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index cad95d1..0fc69ae 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -95,7 +95,6 @@ pkg_config_use_dependency(interpreter sot-dynamic)
# Stand alone embedded intepreter with a robot controller.
rosbuild_add_executable(geometric_simu src/geometric_simu.cpp src/sot_loader.cpp)
-pkg_config_use_dependency(interpreter sot-core)
add_subdirectory(src)
diff --git a/src/geometric_simu.cpp b/src/geometric_simu.cpp
index 1c51bc0..56f31cc 100644
--- a/src/geometric_simu.cpp
+++ b/src/geometric_simu.cpp
@@ -17,31 +17,51 @@
* with dynamic_graph_bridge. If not, see <http://www.gnu.org/licenses/>.
*/
#include <iostream>
+#include <boost/thread/thread.hpp>
+#include <boost/thread/condition.hpp>
#include "sot_loader.hh"
-bool SotLoader::start_dg(std_srvs::Empty::Request& request,
- std_srvs::Empty::Response& response)
+boost::condition_variable cond;
+boost::mutex mut;
+
+void workThread(SotLoader *aSotLoader)
{
- for(unsigned int i=0;i<500;i++)
+ {
+ boost::lock_guard<boost::mutex> lock(mut);
+ }
+ while(aSotLoader->isDynamicGraphStopped())
+ {
+ usleep(5000);
+ }
+ while(!aSotLoader->isDynamicGraphStopped())
{
- oneIteration();
+ aSotLoader->oneIteration();
+ usleep(5000);
}
+ cond.notify_all();
+ ros::waitForShutdown();
}
int main(int argc, char *argv[])
{
+
+ ros::init(argc, argv, "sot_ros_encapsulator");
+
SotLoader aSotLoader;
if (aSotLoader.parseOptions(argc,argv)<0)
return -1;
- ros::init(argc, argv, "start_dynamic_graph");
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();
}
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 42ebff8..489e45f 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -30,7 +30,8 @@ using namespace dynamicgraph::sot;
namespace po = boost::program_options;
SotLoader::SotLoader():
-sensorsIn_ (),
+ dynamic_graph_stopped_(true),
+ sensorsIn_ (),
controlValues_ (),
angleEncoder_ (),
angleControl_ (),
@@ -40,8 +41,49 @@ sensorsIn_ (),
accelerometer_ (3),
gyrometer_ (3)
{
+ readSotVectorStateParam();
+ initPublication();
}
+int SotLoader::initPublication()
+{
+ ros::NodeHandle n;
+
+
+ // Prepare message to be published
+ joint_pub_ = n.advertise<sensor_msgs::JointState>("joint_states", 1);
+
+ return 0;
+}
+
+int SotLoader::readSotVectorStateParam()
+{
+ ros::NodeHandle n;
+
+ if (!ros::param::has("/sot/state_vector_map"))
+ {
+ std::cerr<< " Read Sot Vector State Param " << std::endl;
+ return 1;
+ }
+
+ n.getParam("/sot/state_vector_map", stateVectorMap_);
+ ROS_ASSERT(stateVectorMap_.getType() == XmlRpc::XmlRpcValue::TypeArray);
+ nbOfJoints_ = stateVectorMap_.size();
+
+ // Prepare joint_state according to robot description.
+ joint_state_.name.resize(nbOfJoints_);
+ joint_state_.position.resize(nbOfJoints_);
+
+ for (int32_t i = 0; i < stateVectorMap_.size(); ++i)
+ {
+ joint_state_.name[i]= static_cast<string>(stateVectorMap_[i]);
+ }
+ angleEncoder_.resize(nbOfJoints_);
+
+ return 0;
+}
+
+
int SotLoader::parseOptions(int argc, char *argv[])
{
po::options_description desc("Allowed options");
@@ -74,7 +116,7 @@ void SotLoader::Initialization()
// Load the SotRobotBipedController library.
void * SotRobotControllerLibrary = dlopen( dynamicLibraryName_.c_str(),
- RTLD_LAZY | RTLD_LOCAL );
+ RTLD_LAZY | RTLD_GLOBAL );
if (!SotRobotControllerLibrary) {
std::cerr << "Cannot load library: " << dlerror() << '\n';
return ;
@@ -95,8 +137,6 @@ void SotLoader::Initialization()
// Create robot-controller
sotController_ = createSot();
- // std::string s="libsot-hrp2-14-controller.so";
- // sotController_->Initialization(dynamicLibraryName_);
cout <<"Went out from Initialization." << endl;
}
@@ -110,28 +150,33 @@ SotLoader::fillSensors(map<string,dgs::SensorValues> & sensorsIn)
angleEncoder_[i] = angleControl_[i];
sensorsIn["joints"].setValues(angleEncoder_);
-
}
void
SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
{
- //static unsigned int nbit=0;
+
// Update joint values.
angleControl_ = controlValues["joints"].getValues();
+
+ // Check if the size if coherent with the robot description.
+ if (angleControl_.size()!=(unsigned int)nbOfJoints_)
+ {
+ std::cerr << " angleControl_ and nbOfJoints are different !"
+ << std::endl;
+ exit(-1);
+ }
+
+ // Publish the data.
+ joint_state_.header.stamp = ros::Time::now();
+ for(int i=0;i<nbOfJoints_;i++)
+ {
+ joint_state_.position[i] = angleControl_[i];
+ }
- /*
- if (nbit%100==0)
- std::cout << "Size of angles: " << angleControl_.size()
- << " Size of mc->angle: " << mc->angle.length()
- << std::endl;
- */
- /*
- if (nbit%100==0)
- std::cout << std::endl;
- nbit++;
- */
+ joint_pub_.publish(joint_state_);
+
}
@@ -146,7 +191,6 @@ void SotLoader::setup()
void SotLoader::oneIteration()
{
fillSensors(sensorsIn_);
-
try
{
sotController_->nominalSetSensors(sensorsIn_);
@@ -157,3 +201,18 @@ void SotLoader::oneIteration()
readControl(controlValues_);
}
+
+bool SotLoader::start_dg(std_srvs::Empty::Request& request,
+ std_srvs::Empty::Response& response)
+{
+ dynamic_graph_stopped_=false;
+ return true;
+}
+
+bool SotLoader::stop_dg(std_srvs::Empty::Request& request,
+ std_srvs::Empty::Response& response)
+{
+ dynamic_graph_stopped_ = true;
+ return true;
+}
+
diff --git a/src/sot_loader.hh b/src/sot_loader.hh
index 71882f3..ceb5fd2 100644
--- a/src/sot_loader.hh
+++ b/src/sot_loader.hh
@@ -22,16 +22,21 @@
// System includes
#include <iostream>
+#include <cassert>
// STL includes
#include <map>
// Boost includes
#include <boost/program_options.hpp>
+#include <boost/foreach.hpp>
+#include <boost/format.hpp>
// ROS includes
#include "ros/ros.h"
#include "std_srvs/Empty.h"
+#include <sensor_msgs/JointState.h>
+
// Sot Framework includes
#include <sot/core/debug.hh>
#include <sot/core/abstract-sot-external-interface.hh>
@@ -39,13 +44,14 @@
namespace po = boost::program_options;
namespace dgs = dynamicgraph::sot;
-//typedef std::vector<double> SensorValues;
-//typedef std::vector<double> ControlValues;
class SotLoader {
protected:
+ // Dynamic graph is stopped.
+ bool dynamic_graph_stopped_;
+
/// \brief the sot-hrp2 controller
dgs::AbstractSotExternalInterface * sotController_;
@@ -71,19 +77,62 @@ protected:
/// Angular velocity read by gyrometers
std::vector <double> gyrometer_;
+ /// URDF string description of the robot.
+ std::string robot_desc_string_;
+
+ XmlRpc::XmlRpcValue stateVectorMap_;
+
+ // Joint state publication.
+ ros::Publisher joint_pub_;
+
+ // Joint state to be published.
+ sensor_msgs::JointState joint_state_;
+
+ // Number of DOFs according to KDL.
+ int nbOfJoints_;
+
+
public:
SotLoader();
~SotLoader() {};
+ // \brief Read user input to extract the path of the SoT dynamic library.
int parseOptions(int argc, char *argv[]);
-
+
+ // \brief Load the SoT
void Initialization();
+
+ // \brief Compute one iteration of control.
+ // Basically calls fillSensors, the SoT and the readControl.
void oneIteration();
- void fillSensors(std::map<std::string, dgs::SensorValues> & sensorsIn);
- void readControl(std::map<std::string, dgs::ControlValues> & controlValues);
+ // \brief Fill the sensors value for the SoT.
+ void fillSensors(std::map<std::string,
+ dgs::SensorValues> & sensorsIn);
+
+ // \brief Read the control computed by the SoT framework.
+ void readControl(std::map<std::string,
+ dgs::ControlValues> & controlValues);
+
+ // \brief Prepare the SoT framework.
void setup();
+
+ // \brief Callback function when starting dynamic graph.
bool start_dg(std_srvs::Empty::Request& request,
std_srvs::Empty::Response& response);
+
+ // \brief Callback function when stopping dynamic graph.
+ bool stop_dg(std_srvs::Empty::Request& request,
+ std_srvs::Empty::Response& response);
+
+ // \brief Read the state vector description based upon the robot links.
+ int readSotVectorStateParam();
+
+ // \brief Init publication of joint states.
+ int initPublication();
+
+ // \brief Get Status of dg.
+ bool isDynamicGraphStopped()
+ { return dynamic_graph_stopped_; }
};
--
GitLab