【ROS】kobukiでバンパーに反応するようにする・プログラムを他のロボットに使用

画像元

前回
atsuyakoike.hatenablog.com

$ roslaunch kobuki_gazebo kobuki_playground.launch 

この後にrostopic listでトピックを確認します。

$ rostopic list
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/gazebo_gui/parameter_descriptions
/gazebo_gui/parameter_updates
/joint_states
/mobile_base/commands/motor_power
/mobile_base/commands/reset_odometry
/mobile_base/commands/velocity
/mobile_base/events/bumper
/mobile_base/events/cliff
/mobile_base/sensors/imu_data
/odom
/rosout
/rosout_agg
/tf
/tf_static

今回はこの中の「/mobile_base/events/bumper」を使用します。

$ vi vel_bumper.py 

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from kobuki_msgs.msg import BumperEvent

rospy.init_node( 'vel_bumper' )
pub = rospy.Publisher( '/mobile_base/commands/velocity', Twist, queue_size = 10 )

def main():
    sub = rospy.Subscriber( '/mobile_base/events/bumper', BumperEvent, callback )

    while not rospy.is_shutdown():
        vel = Twist()
        direction = raw_input( 'f: forward, b: backward, l: left, r: right >' )
        if 'f' in direction:
            vel.linear.x = 0.5
        if 'b' in direction:
            vel.linear.x = -0.5
        if 'l' in direction:
            vel.angular.z = 1.0
        if 'r' in direction:
            vel.angular.z = -1.0
        if 'q' in direction:
            break
        print vel
        pub.publish( vel )

def callback( bumper ):
    print bumper
    vel = Twist() # make Twist type message
    vel.linear.x = -1.0
    pub.publish( vel ) #Publish


if __name__ == "__main__":
    main()

これで、ロボットがブロックにぶつかるとバックするようになると思います。

parameterを使う

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from kobuki_msgs.msg import BumperEvent

rospy.init_node( 'vel_bumper' )
pub = rospy.Publisher( '/mobile_base/commands/velocity', Twist, queue_size = 10 )

# get parameter
vel_x = rospy.get_param( '~vel_x', 0.5 )
vel_rot = rospy.get_param( '~vel_rot', 1.0 )

def main():
    sub = rospy.Subscriber( '/mobile_base/events/bumper', BumperEvent, callback, queue_size = 1 )

    while not rospy.is_shutdown():
        vel = Twist()
        direction = raw_input( 'f: forward, b: backward, l: left, r:right > ' )
        if 'f' in direction:
            vel.linear.x = vel_x
        if 'b' in direction:
            vel.linear.x = -vel_x
        if 'l' in direction:
            vel.angular.z = vel_rot
        if 'r' in direction:
            vel.angular.z = -vel_rot
        if 'q' in direction:
            break
        print vel
        r = rospy.Rate(10.0)
        for i in range(10):
            pub.publish( vel )
            r.sleep()

def callback( bumper ): 
    back_vel = Twist()
    back_vel.linear.x = -vel_x
    r = rospy.Rate(10.0)
    for i in range(5):
        pub.publish( back_vel )
        r.sleep()


if __name__ == "__main__":
    main()

• コードの「~」は「プライベート・パラメータ」を表す。
• 「プライベート」はNodeの名前がつくため、「~」はノード名。
• 実行時に _vel_x:=1.0と引数を与えることで、プライベートパラメータをセットすることができる。
• vel_x = rospy.get_param( '~vel_x', 0.5 )のように0.5を入れているのは、引数として使用しなかった時の初期化用の値だという認識（間違っていたらご指摘お願いします。）

$ ./vel_bumper.py _vel_x:=1.0

launchファイルに記述する場合

<launch>
    <node pkg="ros_start" name="vel_bumper" type="vel_bumper.py">
        <param name="vel_x" value="1.0"/>
    </node>
</launch>

<param>の行に注目してください。

turtlesimを動かす

$ rosrun ros_start vel_bumper.py /mobile_base/commands/velocity:=/turtle1/cmd_vel

/mobile_base/commands/velocity:=/turtle1/cmd_velとして、vel_bumper.pyがPublishするトピックを実行時に変更

PR2に上記のプログラムを使用してみる

$ sudo apt-get install -y ros-kinetic-pr2-simulator
$ roslaunch pr2_gazebo pr2_empty_world.launch 

$ rosrun ros_start vel_bumper.py /mobile_base/commands/velocity:=/base_controller/command

型やParameterが一致していれば、様々なロボットに使用することができる