[WIP] [Depends on #239] Add indigo & kinetic tests

.travis.yml

@@ -5,33 +5,44 @@ python:
   - "2.7"
 compiler:
   - gcc
+cache:
+  apt: true
+  pip: true
+  directories:
+    - $HOME/.ccache
+    - $HOME/.cache/pip
+    - $HOME/apt-cacher-ng
+dist: trusty
+services:
+  - docker
 env:
-  #- ROS_DISTRO=groovy ROSWS=rosws  BUILDER=rosbuild  USE_DEB=true
-  #- ROS_DISTRO=groovy ROSWS=rosws  BUILDER=rosbuild  USE_DEB=false
-  #- ROS_DISTRO=groovy ROSWS=wstool BUILDER=catkin    USE_DEB=true
-  #- ROS_DISTRO=groovy ROSWS=wstool BUILDER=catkin    USE_DEB=false
-  - ROS_DISTRO=hydro  ROSWS=wstool BUILDER=catkin    USE_DEB=true   USE_JENKINS="true"
-  - ROS_DISTRO=hydro  ROSWS=wstool BUILDER=catkin    USE_DEB=false   USE_JENKINS="true"
-  #- ROS_DISTRO=indigo  ROSWS=wstool BUILDER=catkin   USE_DEB=true
-  #- ROS_DISTRO=indigo  ROSWS=wstool BUILDER=catkin   USE_DEB=false
+  global:
+    - USE_DOCKER=true
+    - USE_TRAVIS=true
+    - ROS_PARALLEL_JOBS="-j1 -l1"
+  matrix:
+    #- ROS_DISTRO=groovy ROSWS=rosws BUILDER=rosbuild USE_DEB=true
+    #- ROS_DISTRO=groovy ROSWS=rosws BUILDER=rosbuild USE_DEB=false
+    #- ROS_DISTRO=groovy ROSWS=wstool BUILDER=catkin USE_DEB=true
+    #- ROS_DISTRO=groovy ROSWS=wstool BUILDER=catkin USE_DEB=false
+    - ROS_DISTRO=hydro ROSWS=wstool BUILDER=catkin USE_DEB=true
+    - ROS_DISTRO=hydro ROSWS=wstool BUILDER=catkin USE_DEB=false
+    - ROS_DISTRO=indigo ROSWS=wstool BUILDER=catkin USE_DEB=true
+    - ROS_DISTRO=indigo ROSWS=wstool BUILDER=catkin USE_DEB=false
+    - ROS_DISTRO=kinetic ROSWS=wstool BUILDER=catkin USE_DEB=true
+    - ROS_DISTRO=kinetic ROSWS=wstool BUILDER=catkin USE_DEB=false
 matrix:
   allow_failures:
-    - env: ROS_DISTRO=indigo  ROSWS=wstool BUILDER=catkin   USE_DEB=true
-    - env: ROS_DISTRO=indigo  ROSWS=wstool BUILDER=catkin   USE_DEB=false
-    - env: ROS_DISTRO=hydro  ROSWS=wstool BUILDER=catkin    USE_DEB=false   USE_JENKINS="true"
+    - env: ROS_DISTRO=hydro ROSWS=wstool BUILDER=catkin USE_DEB=false
+    - env: ROS_DISTRO=indigo ROSWS=wstool BUILDER=catkin USE_DEB=false
+    - env: ROS_DISTRO=kinetic ROSWS=wstool BUILDER=catkin USE_DEB=false
 before_install:
-  # add osrf
-  - sudo sh -c 'echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'
-  - sudo sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu precise main" > /etc/apt/sources.list.d/drc-latest.list'
-  - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-  - wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add -
+  # Install openrtm_aist & add osrf
+  - export BEFORE_SCRIPT="sudo apt-get install -qq -y ros-${ROS_DISTRO}-openrtm-aist; sudo -E sh -c 'echo \"deb http://packages.osrfoundation.org/drc/ubuntu \`lsb_release -cs\` main\" > /etc/apt/sources.list.d/drc-latest.list'; wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add -; sudo apt-get update -qq"
+  # On kinetic, drcsim is not released
+  - if [ ${ROS_DISTRO} != "kinetic" ] ; then export BEFORE_SCRIPT="${BEFORE_SCRIPT}; sudo apt-get install -qq -y drcsim"; fi
   - if [ $USE_DEB == true ] ; then mkdir -p ~/ros/ws_rtmros_gazebo/src; fi
   - if [ $USE_DEB == true ] ; then git clone https://github.com/start-jsk/rtmros_tutorials ~/ros/ws_rtmros_gazebo/src/rtmros_tutorials; fi
-install:
-  - sudo apt-get update -qq
-  - sudo apt-get install -qq drcsim-hydro
-  - sudo apt-get install -qq ros-hydro-openrtm-aist
-  - export ROS_PARALLEL_JOBS="-j1 -l1"
 script: source .travis/travis.sh
 notifications:
   email:

hrpsys_gazebo_atlas/iob/CMakeLists.txt

@@ -21,7 +21,7 @@ set_target_properties(RobotHardware_atlas PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${
 set_target_properties(RobotHardware_atlas PROPERTIES OUTPUT_NAME RobotHardware)
 
 add_executable(RobotHardwareComp_atlas ${ROBOTHARDWARE_SOURCE}/RobotHardwareComp.cpp ${comp_source})
-target_link_libraries(RobotHardwareComp_atlas ${libs} ${omniorb_LIBRARIES} ${omnidynamic_LIBRARIES} RTC coil)
+target_link_libraries(RobotHardwareComp_atlas ${libs} ${omniorb_LIBRARIES} ${omnidynamic_LIBRARIES} RTC coil dl pthread)
 set_target_properties(RobotHardwareComp_atlas PROPERTIES OUTPUT_NAME RobotHardwareComp)
 
 install(TARGETS RobotHardwareComp_atlas RobotHardware_atlas hrpIo_atlas

hrpsys_gazebo_atlas/iob/iob.cpp

@@ -4,7 +4,7 @@
 #include <cstdlib>
 #include <cstring>
 #include <vector>
-#include "io/iob.h"
+#include "hrpsys/io/iob.h"
 
 #include <ros/ros.h>
 #include <boost/algorithm/string.hpp>

hrpsys_gazebo_general/CMakeLists.txt

@@ -18,16 +18,25 @@ if(NOT CMAKE_BUILD_TYPE)
     FORCE)
 endif()
 
+# Gazebo only supports C++11 from version 5
+# http://answers.gazebosim.org/question/13869/ros-enabled-plugin-examples-use-c11/
+include (FindPkgConfig)
+if (PKG_CONFIG_FOUND)
+  pkg_check_modules(GAZEBO gazebo)
+else()
+  message(FATAL_ERROR "pkg-config is required; please install it")
+endif()
+if(${GAZEBO_VERSION} VERSION_LESS "5.0.0")
+else()
+  set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
+endif()
+
 ## Build only gazebo iob
 pkg_check_modules(omniorb omniORB4 REQUIRED)
 pkg_check_modules(omnidynamic omniDynamic4 REQUIRED)
 pkg_check_modules(openrtm_aist openrtm-aist REQUIRED)
 pkg_check_modules(openhrp3 openhrp3.1 REQUIRED)
 pkg_check_modules(hrpsys hrpsys-base REQUIRED)
-### hotfix for https://github.com/fkanehiro/hrpsys-base/pull/803
-list(GET hrpsys_INCLUDE_DIRS 0 hrpsys_first_incdir)
-list(APPEND hrpsys_INCLUDE_DIRS ${hrpsys_first_incdir}/hrpsys)
-list(APPEND hrpsys_INCLUDE_DIRS ${hrpsys_first_incdir}/hrpsys/idl)
 if(EXISTS ${hrpsys_SOURCE_DIR})
   set(ROBOTHARDWARE_SOURCE ${hrpsys_SOURCE_DIR}/src/rtc/RobotHardware)
   set(HRPEC_SOURCE         ${hrpsys_SOURCE_DIR}/src/ec/hrpEC)
@@ -46,13 +55,6 @@ add_custom_target(hrpsys_gazebo_general_iob ALL DEPENDS RobotHardware_gazebo)
 add_dependencies(hrpsys_gazebo_general_iob hrpsys_gazebo_msgs_gencpp)
 
 ## Gazebo plugins
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
-  pkg_check_modules(GAZEBO gazebo)
-else()
-  message(FATAL_ERROR "pkg-config is required; please install it")
-endif()
-
 include_directories( ${GAZEBO_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${openrtm_aist_INCLUDE_DIRS} ${openhrp3_INCLUDE_DIRS})
 link_directories( ${GAZEBO_LIBRARY_DIRS} ${openhrp3_LIBRARY_DIRS})
 

hrpsys_gazebo_general/iob/iob.cpp

@@ -3,7 +3,7 @@
 #include <cstdlib>
 #include <cstring>
 #include <vector>
-#include "io/iob.h"
+#include "hrpsys/io/iob.h"
 
 #include <ros/ros.h>
 #include <boost/algorithm/string.hpp>

hrpsys_gazebo_general/src/IOBPlugin.cpp

@@ -7,6 +7,13 @@
 
 #include "IOBPlugin.h"
 
+#if __cplusplus >= 201103L
+#include <memory>
+using std::dynamic_pointer_cast;
+#else
+using boost::dynamic_pointer_cast;
+#endif
+
 namespace gazebo
 {
 GZ_REGISTER_MODEL_PLUGIN(IOBPlugin);
@@ -240,10 +247,10 @@ void IOBPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
             ROS_ERROR("Force-Torque sensor: %s has invalid configuration", sensor_name.c_str());
           }
           // setup force sensor publishers
-          boost::shared_ptr<std::vector<boost::shared_ptr<geometry_msgs::WrenchStamped> > > forceValQueue(new std::vector<boost::shared_ptr<geometry_msgs::WrenchStamped> >);
+          shared_ptr<std::vector<shared_ptr<geometry_msgs::WrenchStamped> > > forceValQueue(new std::vector<shared_ptr<geometry_msgs::WrenchStamped> >);
           // forceValQueue->resize(this->force_sensor_average_window_size);
           for ( int i=0; i<this->force_sensor_average_window_size; i++ ){
-            boost::shared_ptr<geometry_msgs::WrenchStamped> fbuf(new geometry_msgs::WrenchStamped);
+            shared_ptr<geometry_msgs::WrenchStamped> fbuf(new geometry_msgs::WrenchStamped);
             forceValQueue->push_back(fbuf);
           }
           this->forceValQueueMap[sensor_name] = forceValQueue;
@@ -275,7 +282,7 @@ void IOBPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
               gzerr << ln << " not found\n";
             } else {
               // Get imu sensors
-              msi.sensor = boost::dynamic_pointer_cast<sensors::ImuSensor>
+              msi.sensor = dynamic_pointer_cast<sensors::ImuSensor>
                 (sensors::SensorManager::Instance()->GetSensor
                  (this->world->GetName() + "::" + msi.link->GetScopedName() + "::" + msi.sensor_name));
 
@@ -377,7 +384,7 @@ void IOBPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
     // effort average
     effortValQueue.resize(0);
     for(int i = 0; i < this->effort_average_window_size; i++) {
-      boost::shared_ptr<std::vector<double> > vbuf(new std::vector<double> (this->joints.size()));
+      shared_ptr<std::vector<double> > vbuf(new std::vector<double> (this->joints.size()));
       effortValQueue.push_back(vbuf);
     }
     // for reference
@@ -779,7 +786,7 @@ void IOBPlugin::GetRobotStates(const common::Time &_curTime){
   // populate robotState from robot
   this->robotState.header.stamp = ros::Time(_curTime.sec, _curTime.nsec);
 
-  boost::shared_ptr<std::vector<double > > vbuf = effortValQueue.at(this->effort_average_cnt);
+  shared_ptr<std::vector<double > > vbuf = effortValQueue.at(this->effort_average_cnt);
   // joint states
   for (unsigned int i = 0; i < this->joints.size(); ++i) {
     this->robotState.position[i] = this->joints[i]->GetAngle(0).Radian();
@@ -809,7 +816,7 @@ void IOBPlugin::GetRobotStates(const common::Time &_curTime){
   }
   if (this->use_joint_effort) {
     for (int j = 0; j < effortValQueue.size(); j++) {
-      boost::shared_ptr<std::vector<double > > vbuf = effortValQueue.at(j);
+      shared_ptr<std::vector<double > > vbuf = effortValQueue.at(j);
       for (int i = 0; i < this->joints.size(); i++) {
         this->robotState.effort[i] += vbuf->at(i);
       }
@@ -828,8 +835,8 @@ void IOBPlugin::GetRobotStates(const common::Time &_curTime){
   this->robotState.sensors.resize(this->forceSensorNames.size());
   for (unsigned int i = 0; i < this->forceSensorNames.size(); i++) {
     forceSensorMap::iterator it = this->forceSensors.find(this->forceSensorNames[i]);
-    boost::shared_ptr<std::vector<boost::shared_ptr<geometry_msgs::WrenchStamped> > > forceValQueue = this->forceValQueueMap.find(this->forceSensorNames[i])->second;
-    boost::shared_ptr<geometry_msgs::WrenchStamped> forceVal = forceValQueue->at(this->force_sensor_average_cnt);
+    shared_ptr<std::vector<shared_ptr<geometry_msgs::WrenchStamped> > > forceValQueue = this->forceValQueueMap.find(this->forceSensorNames[i])->second;
+    shared_ptr<geometry_msgs::WrenchStamped> forceVal = forceValQueue->at(this->force_sensor_average_cnt);
     if(it != this->forceSensors.end()) {
       physics::JointPtr jt = it->second.joint;
       if (!!jt) {
@@ -868,7 +875,7 @@ void IOBPlugin::GetRobotStates(const common::Time &_curTime){
     this->robotState.sensors[i].torque.y = 0;
     this->robotState.sensors[i].torque.z = 0;
     for ( int j=0; j<forceValQueue->size() ; j++ ){
-      boost::shared_ptr<geometry_msgs::WrenchStamped> forceValBuf = forceValQueue->at(j);
+      shared_ptr<geometry_msgs::WrenchStamped> forceValBuf = forceValQueue->at(j);
       this->robotState.sensors[i].force.x += forceValBuf->wrench.force.x;
       this->robotState.sensors[i].force.y += forceValBuf->wrench.force.y;
       this->robotState.sensors[i].force.z += forceValBuf->wrench.force.z;
@@ -897,9 +904,15 @@ void IOBPlugin::GetRobotStates(const common::Time &_curTime){
     if(!!sp) {
       this->robotState.Imus[i].name = this->imuSensorNames[i];
       this->robotState.Imus[i].frame_id = it->second.frame_id;
+#if __cplusplus >= 201103L
+      math::Vector3 wLocal = sp->AngularVelocity();
+      math::Vector3 accel = sp->LinearAcceleration();
+      math::Quaternion imuRot = sp->Orientation();
+#else
       math::Vector3 wLocal = sp->GetAngularVelocity();
       math::Vector3 accel = sp->GetLinearAcceleration();
       math::Quaternion imuRot = sp->GetOrientation();
+#endif
       this->robotState.Imus[i].angular_velocity.x = wLocal.x;
       this->robotState.Imus[i].angular_velocity.y = wLocal.y;
       this->robotState.Imus[i].angular_velocity.z = wLocal.z;
@@ -948,7 +961,11 @@ void IOBPlugin::UpdatePID_Velocity_Control(double _dt) {
       static_cast<double>(this->robotState.kpv_velocity[i]) * this->errorTerms[i].qd_p;
 
     // update max force
+#if __cplusplus >= 201103L
+    this->joints[i]->SetParam("max_force", 0, this->joints[i]->GetEffortLimit(0));
+#else
     this->joints[i]->SetMaxForce(0, this->joints[i]->GetEffortLimit(0));
+#endif
     // clamp max velocity
     j_velocity = math::clamp(j_velocity, -max_vel, max_vel);
 #if 0

hrpsys_gazebo_general/src/IOBPlugin.h

@@ -42,12 +42,19 @@
 
 #include "PubQueue.h"
 
+#if __cplusplus >= 201103L
+#include <memory>
+using std::shared_ptr;
+#else
+using boost::shared_ptr;
+#endif
+
 namespace gazebo
 {
-  typedef boost::shared_ptr< sensors::ImuSensor > ImuSensorPtr;
+  typedef shared_ptr< sensors::ImuSensor > ImuSensorPtr;
   typedef hrpsys_gazebo_msgs::JointCommand JointCommand;
   typedef hrpsys_gazebo_msgs::RobotState RobotState;
-  typedef boost::shared_ptr< math::Pose > PosePtr;
+  typedef shared_ptr< math::Pose > PosePtr;
 
   class IOBPlugin : public ModelPlugin
   {
@@ -197,11 +204,11 @@ namespace gazebo
     // force sensor averaging
     int force_sensor_average_window_size;
     int force_sensor_average_cnt;
-    std::map<std::string, boost::shared_ptr<std::vector<boost::shared_ptr<geometry_msgs::WrenchStamped> > > > forceValQueueMap;
+    std::map<std::string, shared_ptr<std::vector<shared_ptr<geometry_msgs::WrenchStamped> > > > forceValQueueMap;
     // effort averaging
     int effort_average_cnt;
     int effort_average_window_size;
-    std::vector< boost::shared_ptr<std::vector<double> > > effortValQueue;
+    std::vector< shared_ptr<std::vector<double> > > effortValQueue;
     // stepping data publish cycle
     int publish_count;
     int publish_step;

hrpsys_gazebo_general/src/LIPPlugin.cpp

@@ -133,7 +133,11 @@ namespace gazebo
       this->root_y_joint_->SetVelocity(0, 0);
       this->linear_joint_->SetVelocity(0, 0);
       // set position and velocity
+#if __cplusplus >= 201103L
+      this->model_->SetWorldPose(base_pose);
+#else
       this->model_->SetWorldPose(base_pose, this->base_link_);
+#endif
       this->mass_link_->SetWorldPose(mass_pose);
       this->mass_link_->SetLinearVel(mass_velocity);
 
@@ -157,7 +161,11 @@ namespace gazebo
       this->root_y_joint_->SetVelocity(0, 0);
       this->linear_joint_->SetVelocity(0, 0);
       // set position and velocity
+#if __cplusplus >= 201103L
+      this->model_->SetWorldPose(base_pose);
+#else
       this->model_->SetWorldPose(base_pose, this->base_link_);
+#endif
       this->mass_link_->SetWorldPose(mass_pose);
       this->mass_link_->SetLinearVel(mass_velocity);
 

hrpsys_gazebo_general/src/PubQueue.h

@@ -27,6 +27,12 @@
 
 #include <ros/ros.h>
 
+#if __cplusplus >= 201103L
+using std::shared_ptr;
+#else
+using boost::shared_ptr;
+#endif
+
 
 /// \brief Container for a (ROS publisher, outgoing message) pair.  
 /// We'll have queues of these.  Templated on a ROS message type.
@@ -49,21 +55,21 @@ template<class T>
 class PubQueue
 {
   public:
-    typedef boost::shared_ptr<std::deque<boost::shared_ptr<
+    typedef shared_ptr<std::deque<shared_ptr<
       PubMessagePair<T> > > > QueuePtr;
-    typedef boost::shared_ptr<PubQueue<T> > Ptr;
+    typedef shared_ptr<PubQueue<T> > Ptr;
 
   private:
     /// \brief Our queue of outgoing messages.
     QueuePtr queue_;
     /// \brief Mutex to control access to the queue.
-    boost::shared_ptr<boost::mutex> queue_lock_;
+    shared_ptr<boost::mutex> queue_lock_;
     /// \brief Function that will be called when a new message is pushed on.
     boost::function<void()> notify_func_;
 
   public:
     PubQueue(QueuePtr queue, 
-             boost::shared_ptr<boost::mutex> queue_lock,
+             shared_ptr<boost::mutex> queue_lock,
              boost::function<void()> notify_func) :
       queue_(queue), queue_lock_(queue_lock), notify_func_(notify_func) {}
     ~PubQueue() {}
@@ -73,15 +79,15 @@ class PubQueue
     /// \param[in] pub The ROS publisher to use to publish the message
     void push(T& msg, ros::Publisher& pub)
     {
-      boost::shared_ptr<PubMessagePair<T> > el(new PubMessagePair<T>(msg, pub));
+      shared_ptr<PubMessagePair<T> > el(new PubMessagePair<T>(msg, pub));
       boost::mutex::scoped_lock lock(*queue_lock_);
       queue_->push_back(el);
       notify_func_();
     }
 
     /// \brief Pop all waiting messages off the queue.
     /// \param[out] els Place to store the popped messages
-    void pop(std::vector<boost::shared_ptr<PubMessagePair<T> > >& els)
+    void pop(std::vector<shared_ptr<PubMessagePair<T> > >& els)
     {
       boost::mutex::scoped_lock lock(*queue_lock_);
       while(!queue_->empty())
@@ -113,11 +119,11 @@ class PubMultiQueue
     /// \brief Service a given queue by popping outgoing message off it and
     /// publishing them.
     template <class T> 
-    void serviceFunc(boost::shared_ptr<PubQueue<T> > pq)
+    void serviceFunc(shared_ptr<PubQueue<T> > pq)
     {
-      std::vector<boost::shared_ptr<PubMessagePair<T> > > els;
+      std::vector<shared_ptr<PubMessagePair<T> > > els;
       pq->pop(els);
-      for(typename std::vector<boost::shared_ptr<PubMessagePair<T> > >::iterator it = els.begin();
+      for(typename std::vector<shared_ptr<PubMessagePair<T> > >::iterator it = els.begin();
           it != els.end();
           ++it)
       {
@@ -141,11 +147,11 @@ class PubMultiQueue
     /// least each type of publish message).
     /// \return Pointer to the newly created queue, good for calling push() on.
     template <class T>
-    boost::shared_ptr<PubQueue<T> > addPub()
+    shared_ptr<PubQueue<T> > addPub()
     {
-      typename PubQueue<T>::QueuePtr queue(new std::deque<boost::shared_ptr<PubMessagePair<T> > >);
-      boost::shared_ptr<boost::mutex> queue_lock(new boost::mutex);
-      boost::shared_ptr<PubQueue<T> > pq(new PubQueue<T>(queue, queue_lock, boost::bind(&PubMultiQueue::notifyServiceThread, this)));
+      typename PubQueue<T>::QueuePtr queue(new std::deque<shared_ptr<PubMessagePair<T> > >);
+      shared_ptr<boost::mutex> queue_lock(new boost::mutex);
+      shared_ptr<PubQueue<T> > pq(new PubQueue<T>(queue, queue_lock, boost::bind(&PubMultiQueue::notifyServiceThread, this)));
       boost::function<void()> f = boost::bind(&PubMultiQueue::serviceFunc<T>, this, pq);
       {
         boost::mutex::scoped_lock lock(service_funcs_lock_);