[on #242] add Craneplugin

[Naoki-Hiraoka]

gazeboでクレーンを使用するためのプラグインです。
gazeboでロボットを地面に置きたい時に、従来は地面より少し高い位置を指定してロボットをワープさせて落とすという手法をとっていましたが、
このプラグインを使うことで①落とすことなく、②正確なZ方向位置を指定することなく、ロボットを地面に置くことが可能になります。

URDFの以下に以下のようなタグを加えることで使用できます。

 <gazebo>                                                                      
    <plugin filename="libCranePlugin.so" name="crane_plugin">                   
      <objname>HRP2JSKNTS</objname>                                             
      <linkname>CHEST_LINK1</linkname>                                          
      <liftheight>1.2</liftheight>                                              
      <lowerheight>0.5</lowerheight>                                            
      <liftvelocity>0.1</liftvelocity>                                          
      <lowervelocity>0.03</lowervelocity>                                       
      <pgain>2500</pgain>                                                       
      <dgain>500</dgain>                                                        
      <damp>10000</damp>                                                        
    </plugin>                                                                   
  </gazebo>   

linknameのリンクに対し、liftheightの高さになるようにpgain dgainの値を用いてPD制御で外力を加えます。
紐で吊るされた状態を想定しているため、鉛直上向きにのみ外力を加え、下向きには外力を加えません。

objname/name/LowerCommandにstd_msgs/Emptyトピックを送ると、目標高さがlowerheightまでlowervelocityの速さで徐々に下がり、目標高さがlowerheightに到達するとこのプラグインが無効になります。

objname/name/LiftCommandにstd_msgs/Emptyトピックを送ると、このプラグインが再度有効になり、目標高さがliftheightまでliftvelocityの速さで徐々に上がります。

[k-okada]

nice! can you add description of plugins at https://github.com/start-jsk/rtmros_gazebo/blob/master/README.md ?

after Environment variables used in iob.cpp

Custom Plugins

Crameplugin
This plugin provides fake "Crame" in gazebo environment

<gazebo>                                                                      
    <plugin filename="libCranePlugin.so" name="crane_plugin">                   
      <objname>HRP2JSKNTS</objname>                                             
      <linkname>CHEST_LINK1</linkname>                                          
      <liftheight>1.2</liftheight>                                              
      <lowerheight>0.5</lowerheight>                                            
      <liftvelocity>0.1</liftvelocity>                                          
      <lowervelocity>0.03</lowervelocity>                                       
      <pgain>2500</pgain>                                                       
      <dgain>500</dgain>                                                        
      <damp>10000</damp>                                                        
    </plugin>                                                                   
  </gazebo>   

Subscribed topics

objname/name/LowerCommand : put robot on the ground

objname/name/LiftCommand : lift the robot to the lift height

Cc: @pazeshun

[pazeshun]

@Naoki-Hiraoka Kindly ping

[k-okada]

Please add description of plugins at https://github.com/start-jsk/rtmros_gazebo/blob/master/README.md ?

[Naoki-Hiraoka]

I'm sorry for my late response.
I added description of plugins.

[pazeshun]

@k-okada Could you review this?