diff --git a/.travis b/.travis index 39be9c1d..6edf53ca 160000 --- a/.travis +++ b/.travis @@ -1 +1 @@ -Subproject commit 39be9c1d739bf54cb4e2e54c37d13292c0005fc8 +Subproject commit 6edf53caf5b8e2b4a392673b7e652916408ec9c7 diff --git a/.travis.yml b/.travis.yml index 58ba1336..0a23a453 100644 --- a/.travis.yml +++ b/.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: diff --git a/hrpsys_gazebo_atlas/iob/CMakeLists.txt b/hrpsys_gazebo_atlas/iob/CMakeLists.txt index 6bcbb8ef..891afcab 100644 --- a/hrpsys_gazebo_atlas/iob/CMakeLists.txt +++ b/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 diff --git a/hrpsys_gazebo_atlas/iob/iob.cpp b/hrpsys_gazebo_atlas/iob/iob.cpp index 427b639f..44ddc569 100644 --- a/hrpsys_gazebo_atlas/iob/iob.cpp +++ b/hrpsys_gazebo_atlas/iob/iob.cpp @@ -4,7 +4,7 @@ #include #include #include -#include "io/iob.h" +#include "hrpsys/io/iob.h" #include #include diff --git a/hrpsys_gazebo_general/CMakeLists.txt b/hrpsys_gazebo_general/CMakeLists.txt index a217a8d5..0146c084 100644 --- a/hrpsys_gazebo_general/CMakeLists.txt +++ b/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}) diff --git a/hrpsys_gazebo_general/iob/iob.cpp b/hrpsys_gazebo_general/iob/iob.cpp index b06981a4..a53afc4f 100644 --- a/hrpsys_gazebo_general/iob/iob.cpp +++ b/hrpsys_gazebo_general/iob/iob.cpp @@ -3,7 +3,7 @@ #include #include #include -#include "io/iob.h" +#include "hrpsys/io/iob.h" #include #include diff --git a/hrpsys_gazebo_general/src/IOBPlugin.cpp b/hrpsys_gazebo_general/src/IOBPlugin.cpp index abbdff74..af2d86ea 100644 --- a/hrpsys_gazebo_general/src/IOBPlugin.cpp +++ b/hrpsys_gazebo_general/src/IOBPlugin.cpp @@ -7,6 +7,13 @@ #include "IOBPlugin.h" +#if __cplusplus >= 201103L +#include +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 > > forceValQueue(new std::vector >); + shared_ptr > > forceValQueue(new std::vector >); // forceValQueue->resize(this->force_sensor_average_window_size); for ( int i=0; iforce_sensor_average_window_size; i++ ){ - boost::shared_ptr fbuf(new geometry_msgs::WrenchStamped); + shared_ptr 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 + msi.sensor = dynamic_pointer_cast (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 > vbuf(new std::vector (this->joints.size())); + shared_ptr > vbuf(new std::vector (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 > vbuf = effortValQueue.at(this->effort_average_cnt); + shared_ptr > 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 > vbuf = effortValQueue.at(j); + shared_ptr > 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 > > forceValQueue = this->forceValQueueMap.find(this->forceSensorNames[i])->second; - boost::shared_ptr forceVal = forceValQueue->at(this->force_sensor_average_cnt); + shared_ptr > > forceValQueue = this->forceValQueueMap.find(this->forceSensorNames[i])->second; + shared_ptr 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; jsize() ; j++ ){ - boost::shared_ptr forceValBuf = forceValQueue->at(j); + shared_ptr 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(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 diff --git a/hrpsys_gazebo_general/src/IOBPlugin.h b/hrpsys_gazebo_general/src/IOBPlugin.h index b8fafbd4..3cb725e5 100644 --- a/hrpsys_gazebo_general/src/IOBPlugin.h +++ b/hrpsys_gazebo_general/src/IOBPlugin.h @@ -42,12 +42,19 @@ #include "PubQueue.h" +#if __cplusplus >= 201103L +#include +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 > > > forceValQueueMap; + std::map > > > forceValQueueMap; // effort averaging int effort_average_cnt; int effort_average_window_size; - std::vector< boost::shared_ptr > > effortValQueue; + std::vector< shared_ptr > > effortValQueue; // stepping data publish cycle int publish_count; int publish_step; diff --git a/hrpsys_gazebo_general/src/LIPPlugin.cpp b/hrpsys_gazebo_general/src/LIPPlugin.cpp index 49a201b8..3879699e 100644 --- a/hrpsys_gazebo_general/src/LIPPlugin.cpp +++ b/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); diff --git a/hrpsys_gazebo_general/src/PubQueue.h b/hrpsys_gazebo_general/src/PubQueue.h index 48acec3c..29051cba 100644 --- a/hrpsys_gazebo_general/src/PubQueue.h +++ b/hrpsys_gazebo_general/src/PubQueue.h @@ -27,6 +27,12 @@ #include +#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 PubQueue { public: - typedef boost::shared_ptr > > > QueuePtr; - typedef boost::shared_ptr > Ptr; + typedef shared_ptr > Ptr; private: /// \brief Our queue of outgoing messages. QueuePtr queue_; /// \brief Mutex to control access to the queue. - boost::shared_ptr queue_lock_; + shared_ptr queue_lock_; /// \brief Function that will be called when a new message is pushed on. boost::function notify_func_; public: PubQueue(QueuePtr queue, - boost::shared_ptr queue_lock, + shared_ptr queue_lock, boost::function notify_func) : queue_(queue), queue_lock_(queue_lock), notify_func_(notify_func) {} ~PubQueue() {} @@ -73,7 +79,7 @@ class PubQueue /// \param[in] pub The ROS publisher to use to publish the message void push(T& msg, ros::Publisher& pub) { - boost::shared_ptr > el(new PubMessagePair(msg, pub)); + shared_ptr > el(new PubMessagePair(msg, pub)); boost::mutex::scoped_lock lock(*queue_lock_); queue_->push_back(el); notify_func_(); @@ -81,7 +87,7 @@ class PubQueue /// \brief Pop all waiting messages off the queue. /// \param[out] els Place to store the popped messages - void pop(std::vector > >& els) + void pop(std::vector > >& 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 - void serviceFunc(boost::shared_ptr > pq) + void serviceFunc(shared_ptr > pq) { - std::vector > > els; + std::vector > > els; pq->pop(els); - for(typename std::vector > >::iterator it = els.begin(); + for(typename std::vector > >::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 - boost::shared_ptr > addPub() + shared_ptr > addPub() { - typename PubQueue::QueuePtr queue(new std::deque > >); - boost::shared_ptr queue_lock(new boost::mutex); - boost::shared_ptr > pq(new PubQueue(queue, queue_lock, boost::bind(&PubMultiQueue::notifyServiceThread, this))); + typename PubQueue::QueuePtr queue(new std::deque > >); + shared_ptr queue_lock(new boost::mutex); + shared_ptr > pq(new PubQueue(queue, queue_lock, boost::bind(&PubMultiQueue::notifyServiceThread, this))); boost::function f = boost::bind(&PubMultiQueue::serviceFunc, this, pq); { boost::mutex::scoped_lock lock(service_funcs_lock_); diff --git a/hrpsys_gazebo_general/src/SetVelPlugin.cpp b/hrpsys_gazebo_general/src/SetVelPlugin.cpp index a37b81dd..5255773c 100644 --- a/hrpsys_gazebo_general/src/SetVelPlugin.cpp +++ b/hrpsys_gazebo_general/src/SetVelPlugin.cpp @@ -120,7 +120,11 @@ namespace gazebo gzdbg << "subscribed SetPoseCommand. ( position: " << this->pose.pos << " orientation: " << this->pose.rot << " )" << std::endl; if (!this->apply_in_gazebo_loop) { // this->model->SetLinkWorldPose(this->pose, this->link); +#if __cplusplus >= 201103L + this->model->SetWorldPose(this->pose); +#else this->model->SetWorldPose(this->pose, this->link); +#endif } } @@ -129,7 +133,11 @@ namespace gazebo { if (this->apply_in_gazebo_loop) { if (this->set_pose_flag) { +#if __cplusplus >= 201103L + this->model->SetWorldPose(this->pose); +#else this->model->SetWorldPose(this->pose, this->link); +#endif } if (this->set_vel_flag) { this->model->SetLinearVel(this->linear_vel);