diff --git a/README.md b/README.md
index 1bbb479a..03191880 100644
--- a/README.md
+++ b/README.md
@@ -97,6 +97,42 @@ This is yaml file for configuring gazebo setting.
imu_sensors_config:
imu_sensor0: {ros_name: 'ros_imu_sensor', link_name: 'LINK_NAME0', frame_id: 'LINK_NAME0'}
+#### Custom Plugins
+
+#### CranePlugin
+
+This plugin provides fake "Crane" in gazebo environment.
+
+To use this plugin, add the following lines to your URDF.
+```
+
+
+ CHEST_LINK1
+ 1.2
+ 0.5
+ 0.1
+ 0.03
+ 2500
+ 500
+ 10000
+
+
+```
+
+##### Subscribed topics
+
+- `[objname]/CranePlugin/LowerCommand` (`std_msgs::Empty`)
+
+Lower the crane to the ground.
+
+- `[objname]/CranePlugin/LiftCommand` (`std_msgs::Empty`)
+
+Lift the crane to the lift height.
+
+- `[objname]/CranePlugin/PoseCommand` (`geometry_msgs::Pose`)
+
+Place the robot to the pose.
+
[gazebo]:http://gazebosim.org
[rtmros_common]:https://github.com/start-jsk/rtmros_common
[hrpsys_gazebo_general]:https://github.com/start-jsk/rtmros_gazebo/tree/master/hrpsys_gazebo_general
diff --git a/hrpsys_gazebo_general/CMakeLists.txt b/hrpsys_gazebo_general/CMakeLists.txt
index 0146c084..eb4c9ca4 100644
--- a/hrpsys_gazebo_general/CMakeLists.txt
+++ b/hrpsys_gazebo_general/CMakeLists.txt
@@ -81,6 +81,9 @@ install(TARGETS ThermoPlugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATIO
add_library(LIPPlugin src/LIPPlugin.cpp)
add_dependencies(LIPPlugin hrpsys_gazebo_msgs_gencpp)
install(TARGETS LIPPlugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+add_library(CranePlugin src/CranePlugin.cpp)
+add_dependencies(CranePlugin hrpsys_gazebo_msgs_gencpp)
+install(TARGETS CranePlugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
## Convert robot models
include(${PROJECT_SOURCE_DIR}/cmake/compile_robot_model_for_gazebo.cmake)
diff --git a/hrpsys_gazebo_general/src/CranePlugin.cpp b/hrpsys_gazebo_general/src/CranePlugin.cpp
new file mode 100644
index 00000000..90cddc9b
--- /dev/null
+++ b/hrpsys_gazebo_general/src/CranePlugin.cpp
@@ -0,0 +1,328 @@
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+
+namespace gazebo
+{
+ class Crane : public ModelPlugin
+ {
+
+ public:
+ // Initialize
+ void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
+ {
+ // Store the pointer to the model
+ this->model = _parent;
+ this->world = this->model->GetWorld();
+
+ // read option args in sdf tags
+ this->obj_name = this->model->GetName();
+ if (_sdf->HasElement("objname")) {
+ this->obj_name = _sdf->Get("objname");
+ }
+ this->link_name = "root";
+ if (_sdf->HasElement("linkname")) {
+ this->link_name = _sdf->Get("linkname");
+ }
+ this->topic_name = "CranePlugin";
+ if (_sdf->HasElement("topicname")) {
+ this->topic_name = _sdf->Get("topicname");
+ }
+
+ // find root link
+ this->link = this->model->GetLink(this->link_name);
+ if(!this->link) {
+ gzerr << "[CranePlugin] Root link are not found. (link_name is "<< this->link_name << ")" << std::endl;
+ return;
+ }
+
+ // find pull up height
+ this->lift_height = 2.0;
+ if (_sdf->HasElement("liftheight")) {
+ this->lift_height = _sdf->Get("liftheight");
+ }
+
+ //find lift velocity
+ this->lift_velocity = 1.0;
+ if (_sdf->HasElement("liftvelocity")) {
+ this->lift_velocity = _sdf->Get("liftvelocity");
+ }
+
+ //find lower velocity
+ this->lower_velocity = 1.0;
+ if (_sdf->HasElement("lowervelocity")) {
+ this->lower_velocity = _sdf->Get("lowervelocity");
+ }
+
+ //find lower height
+ this->lower_height = 0.0;
+ if (_sdf->HasElement("lowerheight")) {
+ this->lower_height = _sdf->Get("lowerheight");
+ }
+
+ //find pgain
+ this->pgain = 10000;
+ if (_sdf->HasElement("pgain")) {
+ this->pgain = _sdf->Get("pgain");
+ }
+ //find dgain
+ this->dgain = 10;
+ if (_sdf->HasElement("dgain")) {
+ this->pgain = _sdf->Get("dgain");
+ }
+
+ //find damp
+ this->damp = 1;
+ if (_sdf->HasElement("damp")) {
+ this->damp = _sdf->Get("damp");
+ }
+
+ // initialize
+ bool liftstart = true;
+ if (_sdf->HasElement("liftstart")) {
+ liftstart = _sdf->Get("liftstart");
+ }
+ if (liftstart) {
+ std_msgs::Empty::ConstPtr msg;
+ LiftCommand(msg);
+ } else {
+ std_msgs::Empty::ConstPtr msg;
+ LowerCommand(msg);
+ }
+ this->set_pose_flag = false;
+
+ // Make sure the ROS node for Gazebo has already been initialized
+ if (!ros::isInitialized()) {
+ gzerr << "[CranePlugin] A ROS node for Gazebo has not been initialized, unable to load plugin. "
+ << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)";
+ return;
+ }
+ // ros node
+ this->rosNode = new ros::NodeHandle("");
+ // ros callback queue for processing subscription
+ this->deferredLoadThread = boost::thread(boost::bind(&Crane::DeferredLoad, this));
+ }
+
+ void DeferredLoad() {
+ // ros topic subscribtions
+ ros::SubscribeOptions LiftCommandSo =
+ ros::SubscribeOptions::create("/" + this->obj_name + "/" + this->topic_name + "/LiftCommand", 100,
+ boost::bind(&Crane::LiftCommand, this, _1),
+ ros::VoidPtr(), &this->rosQueue);
+ ros::SubscribeOptions LowerCommandSo =
+ ros::SubscribeOptions::create("/" + this->obj_name + "/" + this->topic_name + "/LowerCommand", 100,
+ boost::bind(&Crane::LowerCommand, this, _1),
+ ros::VoidPtr(), &this->rosQueue);
+ ros::SubscribeOptions PoseCommandSo =
+ ros::SubscribeOptions::create("/" + this->obj_name + "/" + this->topic_name + "/PoseCommand", 100,
+ boost::bind(&Crane::SetPoseCommand, this, _1),
+ ros::VoidPtr(), &this->rosQueue);
+ // Enable TCP_NODELAY because TCP causes bursty communication with high jitter,
+ LiftCommandSo.transport_hints = ros::TransportHints().reliable().tcpNoDelay(true);
+ this->subLiftCommand = this->rosNode->subscribe(LiftCommandSo);
+ LowerCommandSo.transport_hints = ros::TransportHints().reliable().tcpNoDelay(true);
+ this->subLowerCommand = this->rosNode->subscribe(LowerCommandSo);
+ PoseCommandSo.transport_hints = ros::TransportHints().reliable().tcpNoDelay(true);
+ this->subPoseCommand = this->rosNode->subscribe(PoseCommandSo);
+
+ // ros callback queue for processing subscription
+ this->callbackQueeuThread = boost::thread(boost::bind(&Crane::RosQueueThread, this));
+
+ // Listen to the update event.
+ this->updateConnection = event::Events::ConnectWorldUpdateEnd(boost::bind(&Crane::OnUpdate, this));
+
+ ROS_INFO("CranePlugin was loaded !");
+ }
+
+ void LiftCommand(const std_msgs::Empty::ConstPtr &_msg)
+ {
+ this->pull_up_flag = true;
+ this->pull_down_flag = false;
+#if GAZEBO_MAJOR_VERSION >= 9
+ this->target_height = this->link->WorldPose().Pos().Z();
+#else
+ this->target_height = this->link->GetWorldPose().pos.z;
+#endif
+ this->lift_fin = false;
+ // initialize update time
+#if GAZEBO_MAJOR_VERSION >= 9
+ this->lastUpdateTime = this->world->SimTime();
+#else
+ this->lastUpdateTime = this->world->GetSimTime();
+#endif
+ this->count = 0;
+ gzmsg << "[CranePlugin]subscribed LiftCommand." << std::endl;
+ }
+
+ void LowerCommand(const std_msgs::Empty::ConstPtr &_msg)
+ {
+ if(this->pull_up_flag){
+ this->pull_up_flag = false;
+ this->pull_down_flag = true;
+ // initialize update time
+#if GAZEBO_MAJOR_VERSION >= 9
+ this->lastUpdateTime = this->world->SimTime();
+#else
+ this->lastUpdateTime = this->world->GetSimTime();
+#endif
+ }
+ gzdbg << "[CranePlugin]subscribed LowerCommand." << std::endl;
+ }
+
+
+ void SetPoseCommand(const geometry_msgs::Pose::ConstPtr &_msg)
+ {
+#if GAZEBO_MAJOR_VERSION >= 9
+ this->pose.Set(ignition::math::Vector3d(_msg->position.x, _msg->position.y, _msg->position.z),
+ ignition::math::Quaterniond(_msg->orientation.w, _msg->orientation.x, _msg->orientation.y, _msg->orientation.z));
+#else
+ this->pose.Set(math::Vector3(_msg->position.x, _msg->position.y, _msg->position.z),
+ math::Quaternion(_msg->orientation.w, _msg->orientation.x, _msg->orientation.y, _msg->orientation.z));
+#endif
+ this->set_pose_flag = true;
+ this->pull_up_flag = false;
+ this->pull_down_flag = false;
+#if GAZEBO_MAJOR_VERSION >= 9
+ gzdbg << "[CranePlugin]subscribed SetPoseCommand. ( position: " << this->pose.Pos() << " orientation: " << this->pose.Rot() << " )" << std::endl;
+#else
+ gzdbg << "[CranePlugin]subscribed SetPoseCommand. ( position: " << this->pose.pos << " orientation: " << this->pose.rot << " )" << std::endl;
+#endif
+ }
+
+ // Called by the world update start event
+ void OnUpdate()
+ {
+ // get current state
+#if GAZEBO_MAJOR_VERSION >= 9
+ ignition::math::Vector3d lin = this->model->WorldLinearVel();
+ ignition::math::Vector3d ang = this->model->WorldAngularVel();
+ double height = this->link->WorldPose().Pos().Z();
+ common::Time curTime = this->world->SimTime();
+#else
+ math::Vector3 lin = this->model->GetWorldLinearVel();
+ math::Vector3 ang = this->model->GetWorldAngularVel();
+ double height = this->link->GetWorldPose().pos.z;
+ common::Time curTime = this->world->GetSimTime();
+#endif
+ double dt = (curTime - this->lastUpdateTime).Double();
+
+ if (this->pull_up_flag||this->pull_down_flag) {
+ if (this->pull_up_flag){
+ if(this->target_heightlift_height){
+ this->target_height+=lift_velocity * dt;
+ }else{
+ this->target_height=this->lift_height;
+ count++;
+ if(count%100==0){
+ this->model->SetWorldTwist(lin*0.7,ang*0.7);
+ }
+ }
+ }
+ if (this->pull_down_flag){
+ if(this->target_heightlower_height){
+ this->pull_down_flag=false;
+ return;
+ }
+ this->target_height-=lower_velocity * dt;
+ }
+ double error = this->target_height - height;
+ double derror = 0.0;
+#if GAZEBO_MAJOR_VERSION >= 9
+ if (!ignition::math::equal(dt,0.0)) derror=(error-pre_error)/dt;
+#else
+ if (!math::equal(dt,0.0)) derror=(error-pre_error)/dt;
+#endif
+ double F_z = this->pgain * error + this->dgain * derror;
+ if (F_z>0.0){
+#if GAZEBO_MAJOR_VERSION >= 9
+ ignition::math::Vector3d lin_f = - lin * this->damp;
+ ignition::math::Vector3d ang_f = - ang * this->damp;
+ lin_f.Z()=F_z;
+#else
+ math::Vector3 lin_f = - lin * this->damp;
+ math::Vector3 ang_f = - ang * this->damp;
+ lin_f.z=F_z;
+#endif
+
+ this->link->AddForce(lin_f);
+ this->link->AddTorque(ang_f);
+ }
+ this->lastUpdateTime = curTime;
+ }else if (this->set_pose_flag) {
+#if GAZEBO_MAJOR_VERSION >= 9
+ this->model->SetLinearVel(ignition::math::Vector3d(0, 0, 0));
+ this->model->SetAngularVel(ignition::math::Vector3d(0, 0, 0));
+#else
+ this->model->SetLinearVel(math::Vector3(0, 0, 0));
+ this->model->SetAngularVel(math::Vector3(0, 0, 0));
+#endif
+ this->model->SetWorldPose(this->pose);
+ this->set_pose_flag = false;
+ }
+ }
+
+ // Ros loop thread function
+ void RosQueueThread() {
+ static const double timeout = 0.01;
+
+ while (this->rosNode->ok()) {
+ this->rosQueue.callAvailable(ros::WallDuration(timeout));
+ }
+ }
+
+ private:
+ physics::WorldPtr world;
+ physics::ModelPtr model;
+ std::string obj_name;
+ std::string link_name;
+ std::string topic_name;
+ double lift_height;
+ double lower_height;
+ double target_height;
+ double lift_velocity;
+ double lower_velocity;
+ double pgain;
+ double dgain;
+ double pre_error;
+ bool lift_fin;
+ physics::LinkPtr link;
+ event::ConnectionPtr updateConnection;
+ bool pull_up_flag;
+ bool pull_down_flag;
+ bool damp;
+ common::Time lastUpdateTime;
+ int count;
+#if GAZEBO_MAJOR_VERSION >= 9
+ ignition::math::Pose3d pose;
+#else
+ math::Pose pose;
+#endif
+ bool set_pose_flag;
+
+ ros::NodeHandle* rosNode;
+ ros::CallbackQueue rosQueue;
+ ros::Subscriber subLiftCommand;
+ ros::Subscriber subLowerCommand;
+ ros::Subscriber subPoseCommand;
+ boost::thread callbackQueeuThread;
+ boost::thread deferredLoadThread;
+ };
+
+ GZ_REGISTER_MODEL_PLUGIN(Crane)
+}