kakepro’s blog

プログラムとかPC周りのことを書いていきます

【RaspberryPi】ボタン入力をしてみる

はじめに

・環境
Raspberrypi 4 B+
Linux raspberrypi 5.10.103-v8+
Distributor ID: Debian
Description: Debian GNU/Linux 10 (buster)
Release: 10
Codename: buster

・やろうと思ったこと
ラズパイで卓上の装置を作ろうとしたときに、GUIではなく
物理的なボタンで運転開始や停止、動作切り替えが出来るようにしたかった
とりあえずラズパイのGPIOピンとタクトスイッチを何個か接続し、
押したボタンごとに異なる動作をさせられるか確認することにした

配線

用意するのは以下の部品
・ラズパイ本体
・ジャンパーワイヤ(オスーメス)×4
・ジャンパーワイヤ(オスーオス)×3
・ブレッドボード
・タクトスイッチ×3

配線図は以下の通り

配線図

使うGPIOピンはGPIO17、18、22とGND

ソースコード

import time
import RPi.GPIO as GPIO

# Callback function
def callback(pin):
  print("callback")

# GPIO No.
SW1       = 18
SW2       = 17
SW3       = 22

# initital setup
GPIO.setmode(GPIO.BCM)
GPIO.setup(SW1, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(SW2, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(SW3, GPIO.IN, pull_up_down=GPIO.PUD_UP)

GPIO.add_event_detect(SW3, GPIO.FALLING, bouncetime=200)
GPIO.add_event_callback(SW3, callback=callback)

# MAIN PROGRAM*********************************************
try:
  print("Start program")
  while True:
    sw_status_1 = GPIO.input(SW1)
    sw_status_2 = GPIO.input(SW2)

    if sw_status_1 == 0:
      print("switch 1 is pressed!")
    elif sw_status_2 == 0:
      print("switch 2 is pressed!")

    time.sleep(0.1)

finally:
  print("cleanup")
  GPIO.cleanup()

解説

・GPIOピン17、18、22をそれぞれ入力ピンとして設定
・内部プルアップ抵抗を有効化
 上手の回路ではスイッチを押すとGNDと導通するので0Vに落ちるが、
 スイッチを押していないときはどこにも繋がっていない不定状態となるため、
 安定化させるためにプルアップ抵抗で3.3V側に引張ってやる
チャタリング対策として、whileループの中にsleep関数、割込み処理にbouncetimeを設定
 

ハマったところ

ラズパイは結構使っているので楽勝中の楽勝だと思っていたが、最初
sw_status_1 = GPIO.input(SW1)
sw_status_2 = GPIO.input(SW2)
の2文をwhileループの外に置いていたところ全く意図したとおりに動かず
30分くらい「???」状態だった
参考のページを見てようやく気が付いた

【ROS入門】ROSプログラミング:roslaunchを利用する

はじめに

・環境
Windows 10 Home 21H2
WSL2
Intel(R) Core(TM) i5-4200U CPU @ 1.60GHz

・ROS melodicのインストール
下記記事を参照のこと
kakepro.hatenablog.com

・ここでやること
複数のプログラムをrolaunchでまとめて起動する

roslaunchを利用してみる

前回の記事で簡単なROSプログラムを書いてみた
kakepro.hatenablog.com

ROSでは簡単な通信ノードを動かしてみるだけでもroscore/publish/subscribeの三つのプログラムを起動する必要があった
ROSの思想として、再利用しやすいように単一のプログラムの機能はなるべく単純にすることが推奨されているため、
複雑なことをやろうと思うとプログラムの数が非常に多くなることが想定される
そこで複数のROSノードをまとめて起動したり、まとめて設定を行うことができるroslaunchを使ってみる

launchファイルの作成

ワークスペースは前回のpubsub_wsをそのまま使うことにする
roslaunchの設定ファイルは~.launchという拡張子でlaunchフォルダに格納しておくと良い
launchファイルはxml形式で記述する

$ cd ~/pubsub_ws/
$ mkdir ./src/launch
$ vim ./src/launch/chat.launch

<launch>
    <node pkg="pubsub" name="talker" type="talker.py" />
    <node pkg="pubsub" name="listener" type="listener.py" />
</launch>

上記のように、タグの中にノードを指定する
pkg:パッケージ名
name:ノードの名前*1
type:実行ファイル名

実行方法は
$ roslaunch+パッケージ名+ファイル名
または
$ roslaunch+ファイルパス

今回の場合は
$ roslaunch ./src/launch/chat.launch

chat.launchを起動してもターミナルには特に何も表示されないが、rqt_graphを確認するとちゃんとROSノードが動いている
$ rqt_graph

roslaunch_rqt_graph

出力を確認したい場合はrqt_consoleやrostopicで確認できる
$ rostopic echo chatter

roslaunch_rostopic

roslaunch時に出力も確認したい場合は以下のように--screenオプションを付けるか
$ roslaunch pubsub chat.launch --screen
ローンチファイルに以下のように追記しても出力できる

<launch>
  <node pkg="pubsub" name="talker" type="talker.py" />
  <node pkg="pubsub" name="listener" type="listener.py" output="screen" />
</launch>

*1:roslaunchした場合、実行ファイル中のinit_nodeで指定した名前ではなくここで指定した名前で管理されることに注意

【ROS入門】ROSプログラミング:Publish/Subscribe

はじめに

・環境
Windows 10 Home 21H2
WSL2
Intel(R) Core(TM) i5-4200U CPU @ 1.60GHz

・ROS melodicのインストール
下記記事を参照のこと
kakepro.hatenablog.com

・ここでやること
ROSのTopicをPublish/Subscribe(配信/購読)する

Publish/Subscribeする

基本的なワークフローは下記参照
kakepro.hatenablog.com

まずはワークスペースの作成から

$ cd
$ mkdir -p ~/pubsub_ws/src
$ cd ~/pubsub_ws
$ catkin init
$ catkin build
$ source ./devel/setup.bash
$ cd src/
$ catkin_create_pkg pubsub rospy std_msgs

これでワークスペースの作成とpubsubパッケージを作成できた
続いてPublishするプログラムを作成していく

$ mkdir ./pubsub/scripts
$ cd ./pubsub/scripts
$ vim talker.py

#!/usr/bin/env python
import rospy
from std_msgs.msg import String
#std_msgsパッケージで定義されたString型メッセージをインポート
		
rospy.init_node('talker') #ノードの初期化
pub = rospy.Publisher('chatter', String, queue_size=10) #Publisherの作成
#chatterが名前、Stringが型
rate = rospy.Rate(10) #Rateインスタンスの作成
while not rospy.is_shutdown():
	hello_str = String() #String型メッセージの作成
	hello_str.data = "hello world %s" % rospy.get_time()
	#hello_strのメンバであるdataに変数を代入
	pub.publish(hello_str)
	#Publisherであるpubを使ってメッセージhello_strをPublishする
	rate.sleep() #10Hzだけsleep

続いてSubscribeするプログラムを作成していく

$ vim listener.py

#!/usr/bin/env python
import rospy
from std_msgs.msg import String
#std_msgsパッケージで定義されたString型メッセージをインポート
		
def callback(message):
	rospy.loginfo("I heard %s", message.data)
	#メッセージを受信したらそのメンバ変数dataを%sの部分に入れてPublishする
		
rospy.init_node('listener') #ノードの初期化
sub = rospy.Subscriber('chatter', String, callback) #Subscriberの作成
#chatterが名前、Stringが型、Topicを受信したらcallback関数を呼ぶ
rospy.spin() #無限ループ

作成したプログラムに実行権限を与えてビルドする

$ chmod +x talker.py listener.py
$ cd ~/pubsub_ws
$ catkin build

新しいターミナルでroscoreを実行

$ roscore

さらに別のターミナルで下記を実行

$ cd ~/pubsub_ws
$ source ./devel/setup.bash
$ rosrun pubsub talker.py

続いて別のターミナルで下記を実行してSubscribeする

$ cd ~/pubsub_ws
$ source ./devel/setup.bash
$ rosrun pubsub listener.py

f:id:kakepro:20220410164805p:plain
listener

talker.pyからのデータがlistener.py側に表示されたはず
なお、ROSノードのloginfoはrqt_consoleを用いることで一括で監視できる
別のターミナルから下記を実行

$ rqt_console>

f:id:kakepro:20220410164918p:plain
rqt_console

Messageの内容やNodeの名前が表示されていることが分かる
画面中央のExclude Messages...でInfoやErrorなどを押すと表示内容の切り替えも可能
ROSにはこうした便利なデバッグツールが豊富にある

【ROS入門】ROSプログラミング:ROSでHello World

はじめに

・環境
Windows 10 Home 21H2
WSL2
Intel(R) Core(TM) i5-4200U CPU @ 1.60GHz

・ROS melodicのインストール
下記記事を参照のこと
kakepro.hatenablog.com

・ここでやること
ROSでプログラムを作成する際のワークフローを理解する
PythonHello Worldする

ワークフロー

ROSの基本的なワークフローは
1.作業ディレクトリ(ワークスペース)の作成
2.ワークスペースの初期化
3.パッケージの作成
4.ビルド
である、と思う

まず、作業用ディレクトリ(ワークスペース、ws)を用意する

$ mkdir -p ~/hello_ws/src

続いてワークスペースの初期化を行う

$ cd ~/hello_ws
$ catkin init

続いてパッケージを作成する
作成方法は下記の書式の通り
$ cd catkin_create_pkg <パッケージ名> [依存パッケージ1] [依存パッケージ2]

ここではhelloというパッケージを作成してみる
$ cd src/
$ catkin_create_pkg hello rospy

f:id:kakepro:20220410163020p:plain
catkin_create_pkg

これでhelloという自作パッケージを作成できた
上で生成されたCMakeLIsts.txtはビルドシステムcatkinの設定ファイル、 package.xmlがパッケージの依存関係などを記述したファイルである

$ catkin build

ビルドしたhello_wsを有効化するため、設定ファイルの読み込みを行う
これは毎回必要

$ source ~/hello_ws/devel/setup.bash

先ほど自作したhelloパッケージで動かすソースコードをscriptディレクトリに記述する

$ mkdir ~/hello_ws/src/hello/scripts
$ vim ~/hello_ws/src/hello/scripts/hello_node.py

#Pythonのインタープリタを使用するためのシバン
#!/usr/bin/env python                                                            
#ROSでPythonを使うためのモジュールをインポート
import rospy
#ノードに"hello_node"という名前を付けて初期化
#ノードの名前は他のノードと重複してはいけない
rospy.init_node('hello_node') 
#loginfo()は引数の文字列を標準出力および/rosoutトピックとして送信する
rospy.loginfo('Hello World')  
#spin()は無限ループ(Ctrl-cで終了できる)
#これを書いておかないと/rosoutに文字列を送信する前にノードが終了してしまう
rospy.spin()

上のソースコードに実行権限を付与する

$ chmod +x ~/hello_ws/src/hello/scripts/hello_node.py

ここまで出来たら、もう一度ビルドして設定ファイルを読み込む

$ cd ~/hello_ws
$ catkin build

最後に、作成したソースコードを実行してみる
ノードの実行方法はrosrun パッケージ名 実行ファイル名である

新しいターミナルでroscoreを実行

$ roscore

さらに別のターミナルで下記を実行

$ cd ~/hello_ws
$ source ~/hello_ws/devel/setup.bash
$ rosrun hello hello_node.py

f:id:kakepro:20220410163743p:plain
HelloWorld

おしまい

【ROS入門】Gazeboでシミュレーション

はじめに

・環境
Windows 10 Home 21H2
WSL2
Intel(R) Core(TM) i5-4200U CPU @ 1.60GHz

・ROS melodicのインストール
下記記事を参照のこと
kakepro.hatenablog.com

・ここでやること
Gazeboでシミュレーションを行う上で最低限のURDFを書いてみる
set_model_stateトピックを使ってモデルを動かす

ロボットのモデリング:URDF

ROSではURDF(Unified Robot Description Format)というxml形式で記述したファイルを用いてロボットモデルを表現する
通常はツリー構造でロボットを表現する
例えば車両型のロボットなら車体をルートとして各車輪が枝となるような記述ができ、マニピュレータなら土台をルートとして上腕や前腕を枝となるように記述できる

基本的には
〇リンク:車輪や台座・上腕・前腕
〇ジョイント:関節(リンクとリンクを接続し、それらが互いに対してどう動くかを定義する)
を書けばモデルの可視化はできる
Gazeboでシミュレーションを行うにはイナーシャや干渉に関する情報も必要になる

モデルの作成

まずはワークスペースを準備する
cd
mkdir -p ./gazebo_ws/src
cd ./gazebo_ws
catkin init
cd ./src
catkin_create_pkg test_gazebo rospy
cd ../
catkin build
source ./devel/setup.bash

次にURDFを作成する
mkdir ./src/test_gazebo/urdf
cd ./src/test_gazebo/urdf
vim plate.urdf

URDFの中身は以下のような感じ

<?xml version="1.0"?>
<robot name="plate">
        <link name="base_link">
                <visual>
                        <geometry>
                                <box size="0.3 0.3 0.02"/>
                        </geometry>
                        <material name="silver">
                                <color rgba="0.75 0.75 0.75 1.0"/>
                        </material>
                </visual>
        </link>
</robot>

これをrvizで表示してみる
roslaunch urdf_tutorial display.launch model:=plate.urdf

f:id:kakepro:20220402165138p:plain
rviz

とりあえず板が出来たが、このURDFモデルはシミュレーションを行うために必要な物理的特性に関する一切の情報を持っていない

Gazeboでシミュレーション

Gazeboでのシミュレーションを行うために、collision(干渉)inertial(慣性)のタグを先ほどのURDFに追加する
干渉タグはvisualをコピペ、慣性は何の根拠もない適当な値をとりあえず入れておく
vim plate.urdf

<?xml version="1.0"?>
<robot name="plate">
        <link name="base_link">
                <visual>
                        <geometry>
                                <box size="0.3 0.3 0.02"/>
                        </geometry>
                        <material name="silver">
                                <color rgba="0.75 0.75 0.75 1.0"/>
                        </material>
                </visual>
<!-- ここから追加 -->
                <collision>
                        <geometry>
                                <box size="0.3 0.3 0.02"/>
                        </geometry>
                </collision>
                <inertial>
                        <mass value="0.5"/>
                        <inertia ixx="0.02" iyy="0.02" izz="0.02" ixy="0" iyz="0" ixz="0"/>
                </inertial>
<!-- ここまで -->
        </link>
</robot>

次はこれをGazeboに読み込むためのローンチファイルを作成する
cd ~/gazebo_ws/src/test_gazebo
vim plate.launch

<launch>
        <!-- load the urdf model  into the parameter server -->
        <param name="robot_description" textfile="$(find test_gazebo)/urdf/plate.urdf" />

        <!-- start gazebo with empty world -->
        <include file="$(find gazebo_ros)/launch/empty_world.launch" />

        <!-- spawn plate in gazebo and receive the description from the parameterserver -->
        <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model plate" />
</launch>

では実行してみる
roslaunch test_gazebo plate.launch

f:id:kakepro:20220402171213p:plain
gazebo

ちゃんとGazeboに板が読み込まれている

Gazeboが起動した状態でトピック一覧を表示してみる
別のターミナルで下記を実行
rostopic list
すると以下のように表示されるはず
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/rosout
/rosout_agg
このうちの/gazebo/model_statesを使ってモデルを動かしてみる

Gazeboを起動した状態で別のターミナルから下記を実行
rostopic pub -r 1 gazebo/set_model_state gazebo_msgs/ModelState '{model_name: plate, pose: { position: { x: 0, y: 0, z: 0.5 }, orientation: {x: 0, y: 0, z: 0, w: 0 } }, twist: { linear: { x: 1, y: 0, z: 0 }, angular: { x: 0, y: 0, z: 0} }, reference_frame: world }'

板がZ=0.5の位置から重力を受けて放物線状に落ちてくる様子が確認できる
youtu.be

参考

wiki.ros.org

【ROS入門】亀で遊ぶ

はじめに

・環境
Windows 10 Home 21H2

WSL2

Intel(R) Core(TM) i5-4200U CPU @ 1.60GHz

・ROS melodicのインストール
下記記事を参照のこと
kakepro.hatenablog.com

・ここでやること
亀で遊びながら以下のやり取りについて雰囲気を理解する
Node:ROSで動かすプロセス
Topic:Node間で送受信するデータ
Publish:Topicの送信
Subscribe:Topicの受信

亀で遊んでみる

まず、roscoreを実行する
roscore

f:id:kakepro:20220327130728p:plain
roscore

このコマンドによりマスターが起動され、ノード間での通信が可能となる
ROSノードを使用する際は必ず実行する

続いて、別のターミナルで下記コマンドを実行する
rosrun turtlesim turtlesim_node
下記の通り亀が表示される*1*2
これはROSでロボット(亀)を動かすシミュレーターである

また新たなターミナルで下記コマンドを実行する
rosrun turtlesim turtle_teleop_key
このコマンドを実行したターミナル上で矢印キーを押すと亀が動く

f:id:kakepro:20220327132501p:plain
turtle_teleop

これは
・turtlesim_teleop_keyノードがキーボード入力をPublish
・turtlesimノードがそのデータをSubscribe
することで亀を動かしている

ノード、トピックの可視化ツール(rqt_graph)でそれを確認するため、新たなターミナルで下記コマンドを実行する
rqt_graph

f:id:kakepro:20220327132720p:plain
rqt_graph

teleop_turtleノードとturtlesimノードの間がcmd_velトピックで結ばれていることが確認できる*3
cmd_velは亀の速度に関するデータ(command_velocityの略)である

ここでrostopicコマンドを使用してcmd_velの中身を確認してみる
下記コマンドを実行してから先ほどのturtle_teleop_keyで亀を動かしてみよう
rostopic echo /turtle1/cmd_vel

f:id:kakepro:20220327133127p:plain
cmd_vel

linear(直線運動に関するデータ)とangular(回転速度に関するデータ)がそれぞれ3成分ずつ格納されていることが分かる
なお、ここで先ほどのrgt_graphの更新ボタンを押すとrostopicが追加されていることを確認できる

f:id:kakepro:20220327133344p:plain
rqt_graph_2

ここまでteleop_turtleノードで亀を動かしていたが、rostopicを利用して直接値をPublishすることもできるので試してみる
まずcmd_velトピックの型を調べるため、先ほどのrostopic echoをCtrl-cで終了し、下記コマンドを実行する
rostopic type /turtle1/cmd_vel

f:id:kakepro:20220327134306p:plain
rostopic_type

geometry_msgs/Twist型であることが分かったので下記コマンドで亀を動かしてみる
rostopic pub /turtle1/cmd_vel geometry_msgs/Twist -- '[2.0,0.0,0.0]' '[0.0,0.0,3.6]'

f:id:kakepro:20220327134445p:plain
turtlesim_pub

2.0がX方向の速度[m/s]、3.6がZ軸周りの回転速度[rad/s]である

なお亀の動きはすぐに止まってしまうので、動かし続けたい場合にはトピックを送り続ける必要がある
オプション -r 1を付けると1Hzの周期でトピックを送り続けることが可能である
rostopic pub -r 1 /turtle1/cmd_vel geometry_msgs/Twist -- '[2.0,0.0,0.0]' '[0.0,0.0,3.6]'

*1:WSL2の場合は予めVcXsev等を立ち上げておく

*2:なお、rosrunコマンドは rosrun [package name] [node name] のように、パッケージ(プログラム群)とその中のノードを指定して実行する。 この場合はturtlesimパッケージからturtlesim_nodeを指定して実行している。

*3:楕円がノード、矢印がトピックを表す

【ROS入門】ROS melodicのインストール

環境

Windows 10 Home 21H2

WSL2

Intel(R) Core(TM) i5-4200U CPU @ 1.60GHz

下準備

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'

sudo apt install -y curl

curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add -

ROSのインストール

sudo apt update

sudo apt upgrade

sudo apt install -y ros-melodic-desktop-full

初期設定

sudo apt install python-rosdep

sudo rosdep init

rosdep update

環境設定・その他インストール

cd

echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc

source ~/.bashrc

sudo apt install -y python-rosinstall python-rosinstall-generator python-wstool build-essential python-catkin-tools

動作確認

roscore
で下記のような実行結果が出れば成功

f:id:kakepro:20220327130728p:plain
roscore