ROS 移動ロボットの走行経路を設定する
この記事の目的
ROSで移動ロボットの経路を設定する方法についてまとめる
1. move_baseにおけるロボットの移動経路
前回の投稿でROSシミュレーション上で手動で設定した速度・角速度でロボットを動かすことができたので、次はロボットが指定した経路を追従するシミュレーションについて検討する。ROSのmove_baseでは、ロボットが走行する経路はglobal_plannerを用いて計算する。
global_plannerは地図データから障害物のある確率を『コストマップ』として認識し、通行可能な領域からゴールまでの移動コストが小さくなる経路を探索的に求める。 アルゴリズムとしては、A* search、ダイクストラ法などが用いられる。
得られた経路は『Path』というデータ構造でROS Topicとしてpublishされる。保持している情報は下記のようになる。
| 要素 | 説明 |
|---|---|
| header | タイムスタンプ、座標系を保持する |
| PoseStamped | header情報とpose情報 |
前回のシミュレーション環境で地図データにglobal_plannerを用いて経路を求めることもできるが、直線的な経路が選ばれるなどユーザーが狙った経路をうまく導出できない場合が多い。 今回はユーザーが手動で操作した結果をそのまま走行経路として設定する方法について検討する。
2. ロボットの走行結果を保存する
前回の投稿で作成したユーザーがシミュレーション上で車両を動かすシミュレーションの結果をcsvファイルに保存する機能を追加する。 もともと各時刻における位置座標・速度・角速度を計算しているのでcsvに保存する処理を下記のように加えるだけで対応できる。
class Simple_odom_simulator(): ################## # Initialization # ################## def __init__(self): #中略 if self.save_path_as_csv == True: self.path_dict = {} #中略 def update_odom(self): #中略 #Save path_plan if self.save_path_as_csv == True: addRow = [0,self.sim_pose.position.x,self.sim_pose.position.y,0,updated_quaternion[0],updated_quaternion[1],updated_quaternion[2],updated_quaternion[3], self.sim_twist.linear.x,self.sim_twist.linear.y,self.sim_twist.linear.z,0,0,self.sim_twist.angular.z] self.path_dict[len(self.path_dict)] = addRow pass #中略 def save_csv(self): # Save CSV path file cols = ["time", "x", "y", "z", "w0", "w1", "w2", "w3", "vx", "vy", "vz", "roll", "pitch", "yaw"] df = pd.DataFrame.from_dict(self.path_dict, orient='index',columns=cols) df.to_csv("path_data.csv", index=False)
上記では各時刻のposeの値をdict型データに保存し、ユーザー操作終了時にpandasの関数でcsvファイルに保存している。
3. ロボットの走行結果を走行経路(Path)としてPublishする
次に、保存したcsvファイルを読み込みPathを出力するスクリプトは下記のように記述できる。
#!/usr/bin/env python # -*- coding: utf-8 -*- import pandas as pd # import for ros function import rospy from geometry_msgs.msg import PoseStamped from std_msgs.msg import Header from nav_msgs.msg import Path ######################### # Simple Path publisher # ######################### class Simple_path_simulator(): ################## # Initialization # ################## def __init__(self): rospy.init_node('Simple_Path_Publisher', anonymous=True) self.r = rospy.Rate(50) # 50hz #Initialize odometry header self.path_header = Header() self.path_header.seq = 0 self.path_header.stamp = rospy.Time.now() self.path_header.frame_id = "map" self.path = Path() self.path.header = self.path_header #get pose data from csv self.csv_path_data = pd.read_csv("path_data.csv") pose_list = self.get_poses_from_csvdata() self.path.poses =pose_list #initialize publisher self.path_pub = rospy.Publisher("/path", Path, queue_size=50) ############################################# # Update odometry form User request cmd_vel # ############################################# def get_poses_from_csvdata(self): #Get poses from csv data poses_list = [] for indx in range(len(self.csv_path_data)): temp_pose = PoseStamped() temp_pose.pose.position.x = self.csv_path_data["x"][indx] temp_pose.pose.position.y = self.csv_path_data["y"][indx] temp_pose.pose.position.z = self.csv_path_data["z"][indx] temp_pose.pose.orientation.x = self.csv_path_data["w0"][indx] temp_pose.pose.orientation.y = self.csv_path_data["w1"][indx] temp_pose.pose.orientation.z = self.csv_path_data["w2"][indx] temp_pose.pose.orientation.w = self.csv_path_data["w3"][indx] temp_pose.pose.orientation.w = self.csv_path_data["w3"][indx] temp_pose.header = self.path_header temp_pose.header.seq = indx poses_list.append(temp_pose) return poses_list def publish_path_topic(self): self.path_pub.publish(self.path) self.r.sleep() if __name__ == '__main__': print('Path Publisher is Started...') test = Simple_path_simulator() try: while not rospy.is_shutdown(): test.publish_path_topic() except KeyboardInterrupt: print("finished!")
上記スクリプトでは、csvファイルのposeを読み込みROS Navigation Stackの『Path』データ形式で出力している。 実行すると下記のようにロボットの走行経路を設定することができる。
ROS シミュレーション上でロボットを移動させる
この記事の目的
ROSシミュレーションでロボットを移動させるスクリプトの作り方をまとめる。(mapserver/Odometry/TFの基礎)
1. ROSで移動ロボットを扱うための仕組み
ROSではロボットを自律移動させるためのパッケージが『Navigation Stack』として提供されている。
Navigation Stackの機能は多岐にわたり、自己位置推定やパスプランニング、SLAMを使った地図生成などのパッケージを設定して使う必要がある。
本記事ではNavigationスタックの基礎として、ROSのシミュレーション上で、地図上を車両ロボットをユーザーが操作した速度/角速度で移動させるシミュレーションを構築する。
2. 地図情報の読み込み(mapserver)
ROS の Navigation Stackではmapserverというパッケージを使って地図をROS上で読み込む。
mapサーバーを使うためには下記コマンドでインストールする。
ros-kinetic-map-server
mapserverは地図情報をyamlファイルと画像データ(白黒画像)のセットとして保持する。 yamlファイルは下記の情報を保持する。
| 設定 | 意味 |
|---|---|
| image | 画像ファイルぺのパス(画像はPNG/BMPなど) |
| resolution | 解像度(1pixelを何mとするか)) |
| origin | 原点座標(x,y,yaw) |
| occupied_thresh | 障害物があると判定する閾値 |
| free_thresh | 障害物がないと判定する閾値 |
| negate | 白黒反転設定 |
下記のサンプルファイルを参考として作成するとよい。 github.com
ファイルを用意したあと、下記のtest_mapserver.launchファイルを作成する。(yamlファイルのパスは実行環境に応じて変更する。)
<launch> <arg name="map_file" default="/home/ubuntu/catkin_ws/src/mapserver_test/launch/glbmap.yaml"/> <group ns="map_server"> <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" /> </group> </launch>
このtest_mapserver.launchファイルを実行すると、ROSでmap情報を読み込むことができる。
roslaunch testmapserver.launch
3. 現在位置(Odometry)の生成
ROSのNavigation Stackではロボットの自己位置を"Odometry"というTopicで保持する。 Odometryは下記の信号から構成されている。
| 設定 | 意味 |
|---|---|
| header | 自己位置の計算されたタイムスタンプ/フレームID |
| child_frame_id | オドメトリに関連付けるフレームID |
| pose | ロボットの姿勢 (x[m],y[m],z[m],クォータニオン) |
| twist | ロボットの推定速度(vx[m/s],vy[m/s],vz[m/s],roll[rad/s],pitch[rad/s],yaw[rad/s]) |
ユーザーがtkinterで指定した車両の前進速度[m/s]と角速度[m/s]を指定し、それに従ってOdometryを計算して発行するpythonスクリプトは下記のように記述できる。
#!/usr/bin/env python # -*- coding: utf-8 -*- import math import signal # import for ros function import rospy import tf from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3 from std_msgs.msg import Header from Tkinter import * from nav_msgs.msg import Odometry ############################# # Simple Odometry simulator # ############################# class Simple_odom_simulator(): ################## # Initialization # ################## def __init__(self): rospy.init_node('Simple_Odometry_Simlator', anonymous=True) r = rospy.Rate(50) # 50hz #Initialize odometry header self.odom_header = Header() self.odom_header.seq = 0 self.odom_header.stamp = rospy.Time.now() self.odom_header.frame_id = "map" # Initialize pose info self.sim_pose = Pose() self.sim_pose.position.x = 0.0 self.sim_pose.position.y = 0.0 self.sim_pose.position.z = 0.0 self.sim_pose.orientation.x = 0.0 self.sim_pose.orientation.y = 0.0 self.sim_pose.orientation.z = 0.0 # initialize twist info self.sim_twist = Twist() # Initialize odometry info self.sim_odom = Odometry() self.sim_odom.header = self.odom_header self.sim_odom.child_frame_id = "base_link" self.sim_odom.pose.pose = self.sim_pose self.sim_odom.twist.twist = self.sim_twist #configuration for tkinter self.root = Tk() self.root.option_add('*font', ('FixedSys', 14)) self.buffer = StringVar() self.buffer.set('') Label(self.root, text='Simple Odometry Simulator').pack() self.a = Label(self.root, textvariable=self.buffer) self.a.pack() self.a.bind('<Any-KeyPress>', self.cb_keyevent) #initialize publisher self.emu_odom_pub = rospy.Publisher("/odom", Odometry, queue_size=50) #initialize TF self.odom_broadcaster = tf.TransformBroadcaster() self.map_broadcaster = tf.TransformBroadcaster() #set callback for ctrl and c signal.signal(signal.SIGINT, self.ctr_c_interruption) ########################### # Bind Key board callback # ########################### def cb_keyevent(self,event): key = event.keysym #Update User request Speed from Key board if key=="Up": print("Up") self.sim_twist.linear.x = self.sim_twist.linear.x + 0.1 #m/sec pass elif key=="Down": print("Down") self.sim_twist.linear.x = self.sim_twist.linear.x - 0.1 #m/sec pass elif key == "Left": self.sim_twist.angular.z = self.sim_twist.angular.z + 0.1 #rad/sec print("Left") pass elif key == "Right": self.sim_twist.angular.z = self.sim_twist.angular.z - 0.1 #rad/sec print("Right") pass elif key == "Escape": self.sim_twist.linear.x = 0.0 # m/sec self.sim_twist.angular.z = 0.0 #rad/sec print("Escape") #Min-Max GUARD if self.sim_twist.linear.x < -5.0: self.sim_twist.linear.x = -5.0 elif self.sim_twist.linear.x > 5.0: self.sim_twist.linear.x = 5.0 if self.sim_twist.angular.z < -3.14: self.sim_twist.angular.z = -3.14 elif self.sim_twist.angular.z > 3.14 self.sim_twist.angular.z = 3.14 ############################################# # Update odometry form User request cmd_vel # ############################################# def update_odom(self): #Update Vehicle Pose sampletime = 0.1 #calculate by 100msec e = tf.transformations.euler_from_quaternion((self.sim_pose.orientation.x, self.sim_pose.orientation.y, self.sim_pose.orientation.z, self.sim_pose.orientation.w)) yaw_euler = e[2] #update pose self.sim_pose.position.x = self.sim_pose.position.x + self.sim_twist.linear.x*sampletime*math.cos(yaw_euler) self.sim_pose.position.y = self.sim_pose.position.y + self.sim_twist.linear.x*sampletime*math.sin(yaw_euler) updated_yaw = e[2] +self.sim_twist.angular.z*sampletime updated_quaternion =tf.transformations.quaternion_from_euler(0, 0, updated_yaw) self.sim_pose.orientation.x = updated_quaternion[0] self.sim_pose.orientation.y = updated_quaternion[1] self.sim_pose.orientation.z = updated_quaternion[2] self.sim_pose.orientation.w = updated_quaternion[3] #for GUI out_str = '' vel_kmh = "{0:.3f}".format(self.sim_twist.linear.x * 3600.0 / 1000.0) yawrate = "{0:.3f}".format(self.sim_twist.angular.z) pose_x = "{0:.3f}".format(self.sim_pose.position.x) pose_y = "{0:.3f}".format(self.sim_pose.position.y) orientation = "{0:.3f}".format(self.sim_pose.orientation.z) out_str = out_str + "\n velocity[km/h]:" + vel_kmh out_str = out_str + "\n yawrate[rad/s]:" + yawrate out_str = out_str + "\n position[m]:(" + pose_x +"," + pose_y + ")" out_str = out_str + "\n orientation[rad]:" + orientation self.buffer.set(out_str) #update timestamp self.odom_header.seq =self.odom_header.seq + 1 self.odom_header.stamp = rospy.Time.now() self.sim_odom.header = self.odom_header self.sim_odom.pose.pose = self.sim_pose self.sim_odom.twist.twist = self.sim_twist self.emu_odom_pub.publish(self.sim_odom) #update TF self.map_broadcaster.sendTransform( (self.sim_odom.pose.pose.position.x, self.sim_odom.pose.pose.position.y, self.sim_odom.pose.pose.position.z), updated_quaternion, rospy.Time.now(), "odom", "map" ) base_link_quat = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0) self.odom_broadcaster.sendTransform( (0.0, 0.0, 0.0), base_link_quat, rospy.Time.now(), # "base_link", "base_link", "odom" ) self.root.after(100, self.update_odom) #interval for gui update (100msec) ####################### # ctrl and c callabck # ####################### def ctr_c_interruption(self, signum, frame): self.root.quit() self.root.update() print("finish") pass if __name__ == '__main__': print('Simple Odometry Simulator is Started...') test = Simple_odom_simulator() test.a.focus_set() test.update_odom() test.root.mainloop() pass
上記例ではtkinterのUIを100msec毎に動作させてユーザーの速度/角速度目標値を取得する。 そして、目標値に基づいて自車の位置座標(速度、角度)を計算しなおし、Odometryの値を更新してトピックを発行している。
4. 座標変換情報の設定
Odometryを計算する際に、座標系についても設定する必要があるため補足して説明する。 ROSでは複数の座標系(frame)を持つことができ、各フレーム間を座標変換行列で関連付けて設定することができる。 この機能はTFというパッケージで実現される。
今回のシミュレーション実装例では3つの座標系を利用している。
| 設定 | 意味 |
|---|---|
| map | 地図座標 |
| odom | 自己位置 |
| baselink | 車体中心 |
シミュレーションで自己位置として車体中心座標が得られるという前提でodomとbaselinkが同じ座標になるようにTFを発行している。 実際にはロボットのセンサーがOdometryを発行するので、Odomとbaselinkは自己位置認識の方法やデバイスによって変更する必要がある。
5. シミュレーションの実行
コードを下記のgithubにまとめる。(地図ファイルは環境事にパス修正が必要なので注意)
下記コマンドを実行するとtkinterで操作した速度・角速度に従ってロボットが移動することををrviz上で確認できる。
roslaunch dispaly_car_model.launch roslaunch test_mapserver.launch python Simple_odom_simulator.py rviz
ROS URDFファイルによるモデル作成
この記事の目的
ROSでURDFファイルを記述してロボットモデルを作成する方法についてまとめる
1. URDF(Unified Robot Description Format)ファイルについて
ROSではロボットの構造(車輪や関節、各種センサの位置関係や可動範囲など)をURDFというXML形式で規定されたフォーマットで記述する。 URDFでモデルを記述することで、rvizやGazebooでロボットモデルを画面上で可視化しながら動作確認ができ開発がしやすくなる。 URDFファイルの記述方法についてはROS公式チュートリアルに記載されている。 wiki.ros.org
本記事では、1/10サイズのラジコンにkinect v1センサを搭載している車両を例にURDFファイルの記述方法を説明する。 www.tamiya.com
●シャーシ長385mm､シャーシ幅187mm 厚さ 50mm (タイヤ中心が車体と接続していると仮定) ●ホイールベース257mm ●タイヤ幅/径=前後とも24/64mm ●Kinectセンサ(サイズは幅275×奥行き6.0×高さ8.0mm)は車体最前方に取り付け
2. URDFの記述
URDFファイルの最も基本的なサンプルは公式チュートリアルに下記のように示されている。
<robot name="test_robot"> <link name="link1" /> <link name="link2" /> <link name="link3" /> <link name="link4" /> <joint name="joint1" type="continuous"> <parent link="link1"/> <child link="link2"/> </joint> <joint name="joint2" type="continuous"> <parent link="link1"/> <child link="link3"/> </joint> <joint name="joint3" type="continuous"> <parent link="link3"/> <child link="link4"/> </joint> </robot>
上記において、robot nameはロボット名(任意)を設定する。 linkはロボットの剛体部分、ジョイントはlinkを結合する関節部分を設定する。
jointのtypeは下記の意味を持つ。今回描画しようとしている例ではタイヤはcontinuous、センサーはfixedを使う。
| 設定 | 意味 |
|---|---|
| continuous | 軸方向に制限なく回転する |
| revolute | 軸方向に回転(上下限制限あり) |
| prismatic | 軸方向にスライドする機構 |
| floating | 6自由度(回転+スライド) |
| planar | 軸の上下前後に動く |
| fixed | 固定されている |
joint内部のparent、childは親子関係を設定する。(車体を親、センサー・タイヤを子に設定すればよい)
次に、linkの形状・色の設定について説明する。車体を立方体として記述する場合は下記の様に設定する。
<?xml version="1.0"?> <robot name="my_robo"> <link name="base_link"> <visual> <geometry> <box size="0.385 0.187 0.05"/> </geometry> <origin xyz="0 0 0.034" rpy="0 0 0"/> <material name="gray"> <color rgba="0.8 0.8 0.8 1"/> </material> </visual> <collision> <geometry> <box size="0.385 0.187 0.05"/> </geometry> </collision> </link> </robot>
link_name "base_link"はROSでロボットの車体中心に設定する名称である。 geometryのboxsizeに立方体のx y z方向の大きさを設定する。 (xが進行方向 = 縦方向であることに注意) origin xyzが立方体の中心座標を設定する。 また、materialとしてrvizで表示される色をRGB + 透明度として設定している。 その下ではcollisionとして衝突判定に使う領域サイズを設定できる。
上記に左前輪のタイヤを設定するには下記のように記述する。
<link name="left_front_wheel_link"> <visual> <geometry> <cylinder radius="0.034" length="0.024"/> </geometry> <origin xyz="0 0 0" rpy="0 0 0"/> <material name="black"> <color rgba="0.1 0.1 0.1 1"/> </material> </visual> </link> <joint name="left_front_wheel_joint" type="fixed"> <parent link="base_link"/> <child link="left_front_wheel_link"/> <origin rpy="-1.5707 0 0" xyz="0.128 0.105 0.034"/> <axis xyz="0 0 1"/> </joint> </robot>
円柱はcylinderとして設定し、半径と高さ をタイヤに合わせて設定する。 jointで車体にタイヤを取り付ける角度をrpyと位置をxyzで指定する。 車体に縦方向に回転させたいので -PI/2をroll角に設定し、xにはホイールベースの半分、yはシャシー幅+タイヤ幅で設定した。 同様に4つ分のタイヤとセンサーを設定すると下記のようになる。
<robot name="my_robo"> <link name="base_link"> <visual> <geometry> <box size="0.385 0.187 0.05"/> </geometry> <origin xyz="0 0 0.034" rpy="0 0 0"/> <material name="gray"> <color rgba="0.8 0.8 0.8 1"/> </material> </visual> <collision> <geometry> <box size="0.385 0.187 0.05"/> </geometry> </collision> </link> <link name="left_front_wheel_link"> <visual> <geometry> <cylinder radius="0.034" length="0.024"/> </geometry> <origin xyz="0 0 0" rpy="0 0 0"/> <material name="black"> <color rgba="0.1 0.1 0.1 1"/> </material> </visual> </link> <joint name="left_front_wheel_joint" type="fixed"> <parent link="base_link"/> <child link="left_front_wheel_link"/> <origin rpy="-1.5707 0 0" xyz="0.128 0.105 0.034"/> <axis xyz="0 0 1"/> </joint> <link name="left_rear_wheel_link"> <visual> <geometry> <cylinder radius="0.034" length="0.024"/> </geometry> <origin xyz="0 0 0" rpy="0 0 0"/> <material name="black"> <color rgba="0.1 0.1 0.1 1"/> </material> </visual> </link> <joint name="left_rear_wheel_joint" type="fixed"> <parent link="base_link"/> <child link="left_rear_wheel_link"/> <origin rpy="-1.5707 0 0" xyz="-0.128 0.105 0.034"/> <axis xyz="0 0 1"/> </joint> <link name="right_front_wheel_link"> <visual> <geometry> <cylinder radius="0.034" length="0.024"/> </geometry> <origin xyz="0 0 0" rpy="0 0 0"/> <material name="black"> <color rgba="0.1 0.1 0.1 1"/> </material> </visual> </link> <joint name="right_front_wheel_joint" type="fixed"> <parent link="base_link"/> <child link="right_front_wheel_link"/> <origin rpy="1.5707 0 0" xyz="0.128 -0.105 0.034"/> <axis xyz="0 0 1"/> </joint> <link name="right_rear_wheel_link"> <visual> <geometry> <cylinder radius="0.034" length="0.024"/> </geometry> <origin xyz="0 0 0" rpy="0 0 0"/> <material name="black"> <color rgba="0.1 0.1 0.1 1"/> </material> </visual> </link> <joint name="right_rear_wheel_joint" type="fixed"> <parent link="base_link"/> <child link="right_rear_wheel_link"/> <origin rpy="1.5707 0 0" xyz="-0.128 -0.105 0.034"/> <axis xyz="0 0 1"/> </joint> <link name="kinect_vis"> <visual> <geometry> <box size="0.060 0.275 0.08"/> </geometry> <origin xyz="0 0 0" rpy="0 0 0"/> <material name="blue"> <color rgba="0.0 0.0 0.8 1"/> </material> </visual> </link> <joint name="kinect_vis" type="fixed"> <parent link="base_link"/> <child link="kinect_vis"/> <origin rpy="0 0 0" xyz="0.180 0.0 0.098"/> <axis xyz="0 0 1"/> </joint> </robot>
3. TFの発行とrvizでの可視化
rosで上記のurdfファイルを読み込みrvizで可視化することができる。
そのために、下記ツールが必要となるためapt-get でインストールする
sudo apt-get install ros-melodic-joint-state-publisher-gui
その後、下記のlaunchファイルを記述する。
<launch> <arg name="model" default="$(find vehivle_model_description)/car_model.urdf"/> <arg name="gui" default="False"/> <arg name="rvizconfig" default="$(find vehivle_model_description)/urdf.rviz"/> <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)"/> <param name="use_gui" value="$(arg gui)"/> <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/> <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true"/> </launch>
このファイルでは、記述したurdf (car_model.urdfというファイル名で保存した)を読み込み、 rvizの起動とrosノード joint_state_publisher・robot_state_publisherの立ち上げを行っている。 joint_state_publisherとrobot_state_publisherはいずれもロボットの関節と座標系のtfを配信してrvizで表示するときに必要なツールである。
上記を実行後にrvizで座標系をbaselink に設定してDisplaysにtfとrobotmodelを選択すると正しく表示されることが確認できる。
matplotlibのアニメーション描画
この記事の目的
pythonのmatplotlibを使ってアニメーションデータを表示/gif形式で保存する機能についてまとめる。
1. 時系列データの可視化について
組込み機器(特に自動車や移動ロボット)の開発で、時系列データを解析する際に位置座標と時系列情報を一緒に確認したいことがある。 時系列毎にX,Y座標とセンサーデータが取得できているという前提でシークバーを用いて時間軸をずらしながら位置座標を確認したり、位置座標が時間ごとに どのように変化したかをgifファイルに保存するpythonスクリプトを実装する。
本記事では下記のようなデータを想定してアニメーションによる可視化を検討する。
2. matplotlib可視化ライブラリ
Matplotlibにはanimation用のAPIが用意されており、一定時間ごとに異なるプロット内容を表示することでアニメーションを描画できる。 この機能については下記サイトなどで解説されている。
単純にアニメーション描画をしたい場合には上記機能でも十分だが組込み機器の時系列データ確認では時系列を変化させながら位置座標やセンサー値の変化を確認したいことが多い。 そのために、描画したグラフの時間軸を変化させて波形を確認できる animatplotというライブラリを導入する。animatplotは下記のpipコマンドでインストールできる。
pip install animatplot
3. animatplotを使ったデータの可視化
animatplotでは、描画するデータの2次元座標X,Yデータを時間軸毎の配列として付与することでアニメーションデータを描画する。 例えば、0.1sec間隔で5秒間取得したデータを1秒づつ描画したい場合は、X軸のデータを10点ずつまとめたデータを開始時点をずらして0～4.0秒目分用意する。 pythonコードでは下記のように実装できる。
ref_df = pd.read_csv(csv_file_path, encoding="utf-8-sig") #日本語データ(Shift-Jis)を含む場合を想定 x_np = np.array(ref_df["x"]) y_np = np.array(ref_df["y"]) Xs_log =np.asarray([x_np[t:t+10] for t in range(len(time_data_np)-10)]) #X軸データ × 時間軸 分の配列 Ys_log =[y_np[t:t+10] for t in range(len(time_data_np)-10)] #Y軸データ × 時間軸 分の配列
上記のデータを用意したあとで、animatplotのblocksというクラスの描画関数Scatter(散布図)もしくはLines(線グラフ)を使ってデータを描画する。 描画したデータを関数Animationでアニメーションの対象として設定して、plot.showするとアニメーション表示される。 また、anim.controls()で時間軸の制御コントロールバーを利用することができ、save_gifで結果をgifファイルに保存する。
anim = amp.Animation([block,block2,block3,block4])
anim.controls()
anim.save_gif("result")
plt.show()
上記で基本的なグラフの描画はできるが、animatplotはmatplotのsubplot機能が利用できるので複数のグラフを1つの画面で表示できる。 (ただし、すべてのデータの時間軸を合わせておく必要があるので注意する。) subplotを使ってXYのデータとセンサーの時間軸データを1画面に表示する例を示す。
#!/usr/bin/env python # -*- coding: utf-8 -*- import os import numpy as np import pandas as pd import matplotlib.pyplot as plt import animatplot as amp #グラフの描画 def plot_animation(ref_df): #X軸・Y軸のデータ取得 X_data = 0 Y_data = 0 #refの経路描画 time_data_np = np.array(ref_df["time"]) x_np = np.array(ref_df["x"]) y_np = np.array(ref_df["y"]) sensor_1_np = np.array(ref_df["sensor1"]) sensor_2_np = np.array(ref_df["sensor2"]) sensor_3_np = np.array(ref_df["sensor3"]) Xs_log =np.asarray([x_np[t:t+10] for t in range(len(time_data_np)-10)]) #X軸データ × 時間軸 分の配列 Ys_log =[y_np[t:t+10] for t in range(len(time_data_np)-10)] #Y軸データ × 時間軸 分の配列 sensor_1_log =[sensor_1_np[t:t+10] for t in range(len(time_data_np)-10)] sensor_2_log =[sensor_2_np[t:t+10] for t in range(len(time_data_np)-10)] sensor_3_log =[sensor_3_np[t:t+10] for t in range(len(time_data_np)-10)] Time_log =np.asarray([time_data_np[t:t+10] for t in range(len(time_data_np)-10)]) #subplotの描画 (X-Yの情報を3行分の画面で表示) ax1 = plt.subplot2grid((3,2), (0,0), rowspan=3) ax2 = plt.subplot2grid((3,2), (0,1)) ax3 = plt.subplot2grid((3,2), (1,1)) ax4 = plt.subplot2grid((3,2), (2,1)) ax1.set_xlim([x_np.min(), x_np.max()]) #描画範囲の設定 ax1.set_ylim([y_np.min(),y_np.max()]) #描画範囲の設定 block = amp.blocks.Scatter(Xs_log, Ys_log,label="X_Y",ax=ax1) block2 = amp.blocks.Line(Time_log, sensor_1_log, label="sensor1",ax=ax2) block3 = amp.blocks.Line(Time_log, sensor_2_log, label="sensor2",ax=ax3) block4 = amp.blocks.Line(Time_log, sensor_3_log, label="sensor3",ax=ax4) ax2.set_xlim([time_data_np.min(), time_data_np.max()]) #描画範囲の設定 ax2.set_ylim([sensor_1_np.min(),sensor_1_np.max()]) #描画範囲の設定 ax3.set_xlim([time_data_np.min(), time_data_np.max()]) #描画範囲の設定 ax3.set_ylim([sensor_1_np.min(),sensor_1_np.max()]) #描画範囲の設定 ax4.set_xlim([time_data_np.min(), time_data_np.max()]) #描画範囲の設定 ax4.set_ylim([sensor_1_np.min(),sensor_1_np.max()]) #描画範囲の設定 ax1.legend() ax2.legend() ax3.legend() ax4.legend() plt.subplots_adjust(wspace=0.4, hspace=0.6) anim = amp.Animation([block,block2,block3,block4]) anim.controls() anim.save_gif("result") plt.show() if __name__ == '__main__': csv_file_path = "data\\plotdata.csv" #CSVの読み込み ref_df = pd.read_csv(csv_file_path, encoding="utf-8-sig") #日本語データ(Shift-Jis)を含む場合を想定 plot_animation(ref_df) print("finished!")
Virtual Box (Ubuntu) の導入
この記事の目的
Windows 10 PCにVirtual Box + Ubuntu 16を導入する方法をまとめる (グラフィック関係のトラブルシューティングと共有フォルダ・コピペ設定などを含む)
1. 仮想PCツールについて
VmwareとVirtualboxではどちらがレスポンスが上？。低スペックでもとりあえず使える最近のVM | アロエのおうち
代表的な仮想PCツールとしてVirtual BoxとVMWareがある。
上述の記事のように性能的にはややVMWareが優勢。 ただし、Virtual Boxは拡張機能をインストールしなければ商用でも無料で利用できる。 上記を考慮してVirtual Boxをインストールする。
2. VirtualBox導入方法
1. OSのISOファイルの取得
通常のUbuntuのインストールと一緒 (日本語版のISOを下記からダウンロード) www.ubuntulinux.jp
2. VirtualBoxのダウンロードとインストール
下記からWindows用のインストーラーをダウンロードしてインストール
メモリ容量などはPCスペックに応じて設定する
3. Ubuntuのインストール
上記でダウンロードしたISOを下記から設定して読み込ませる。
仮想PCを起動し、通常のUbuntuと同様にインストールする。 インストール後再起動するときには、ISOファイルの設定を元に戻しておく。
画面が正しく表示されない場合は下記コマンドを実行
sudo apt-get update sudo apt-get upgrade
再起動後、low-graphic mode error が出た場合はCtrl+Alt+F1でコンソールモードに移行して下記コマンドを実行
sudo apt -y purge ubuntu-desktop apt -y purge lightdm sudo apt-get install tasksel sudo tasksel install ubuntu-desktop
4. ネットワーク設定
インストール後はWindows PCのネットワークが設定されている場合はUbuntuからもネット接続できる。 ただし、Proxyがある場合はシステム設定から別途設定する必要がある。
5. Windows/Ubuntu間の共有フォルダの設定
WindowsとUbuntuでファイルの受け渡しができるように共有フォルダを設定できる。 まず、共有フォルダの場所をVirtual Boxの下記画面で設定する。
次に、Ubuntuに追加のドライバをインストールする。Virtual Boxの『Guest Addtions CDイメージの挿入』を選択する
ディスク挿入時にインストールを促されるので、ドライバをインストールする。
その後下記コマンドを入力し、共有フォルダをマウントする。
sudo su - sudo gpasswd -a [アカウント名] vboxsf # (アカウント名は users コマンドで確認できる)
再起動後にWindowsとUbuntuで共有フォルダが設定できていることを確認できる。
6. コピーアンドペーストの設定
WindowsとUbuntu間でコピーアンドペーストを許可する設定はVirtualBoxの設定⇒一般⇒高度で変更できる。
Raspberry pi でWebカメラ映像を処理する方法(OpenCV)
この記事の目的
Raspberry pi でWebカメラを用いて撮影したカメラ動画をストリーミング配信して、他端末からの画像の確認/データ処理する方法をまとめる
1. Webカメラ動画のストリーミング配信
Raspberry pi に接続したWebカメラの映像を他デバイスに配信することで低コストで監視カメラや移動ロボット用カメラとして利用することができる。 本記事では、下記の環境で動画のストリーミングを行った。
| 項目 | バージョン |
|---|---|
| Raspberry pi | Raspberry pi 3 |
| OS | Ubuntu Mate 16.04 |
| Camera | Logicool C615 |
| OpenCV | 3.1 (Python) |
Webカメラのストリーミングはmjpg-streamerというソフトを利用する。下記のコマンドでインストールできる。
sudo apt-get update sudo apt-get install -y cmake libv4l-dev libjpeg-dev imagemagick #mjpg-streamerに必要なパッケージのインストール svn co https://svn.code.sf.net/p/mjpg-streamer/code/mjpg-streamer mjpg-streamer #mjpg-streamerのダウンロード cd mjpg-streamer #ディレクトリの移動 sudo make install #makeとインストールの実施
次に、Webカメラの起動とストリーミング配信を行う。Webカメラが正しく接続されているかを下記コマンドで確認する。
lsusb #正しくWebカメラが認識できていれば下記のようにカメラ名が表示される #Bus 001 Device 005: ID XXXXXXXXX Logitech, Inc. Webcam C615
上記で正しくWebカメラが認識できていた場合、下記コマンドでストリーミングの開始ができる
sudo ./mjpg_streamer -i "./input_uvc.so -f 10 -r 320x240 -d /dev/video0 -y -n" -o "./output_http.so -w ./www -p 8080" #フレームレート10fps サイズ 320×240 の画像をポート8080から配信
この際、ストリーミングのオプションを下記のように設定することができる。
| 引数 | オプションの内容 | 設定例 |
|---|---|---|
| -d | デバイス名(複数接続時の識別用) | /dev/video0 |
| -r | ビデオ解像度 | 320x240 |
| -f | フレームレート | 10 |
| -y | YUVフォーマットの有効・無効 | 追記時に有効 |
| -q | ポート番号 | 8080 |
| -p | 画質YUVフォーマット時のみ有効 | 80 |
繰り返し使う処理とする場合には、下記サイトに紹介されているようにshファイルとして作成しておくと管理しやすい
正しく実行できたら、LAN上の他端末でブラウザからRaspberry pi のアドレス:ポート番号のurlにアクセスできる。
ストリーミングを終了するには、起動させたターミナルでctrl + c を押す。
2. Webカメラの画像をOpenCVで処理する
上記によりRaspberry pi からWebカメラの画像を配信することはできたが、実際にはこれらの画像を入力として各種の画像処理(物体検出など)を加えたい場合も多い。 そこで、Raspberry pi 上でOpenCVで画像を処理する場合とPCでストリーミングした画像を取得してOpenCVで処理する2パターンについて実装する。 物体検出などの計算量の多い処理をさせるのであれば、Raspberry pi はWebカメラで撮影データをストリーミングさせるだけにしておいた方が良い。
2.1 Raspberry pi 上でWebカメラ画像を処理する
OpenCVのWebカメラにアクセスする機能(VideoCapture)を用いて画像を処理する。
サンプルコードは下記のようになる。
#!/usr/bin/env python # -*- coding: utf-8 -*- import cv2 cap = cv2.VideoCapture(0) # VideoCaptureのインスタンスを作成する。(複数のカメラがあるときは引数で識別) while True: ret, frame = cap.read() # 1フレーム読み込む # 画像を表示する cv2.imshow('Image', frame) k = cv2.waitKey(1) #引数は待ち時間(ms) if k == 27: #Esc入力時は終了 break # 終了処理 cap.release() cv2.destroyAllWindows()
2.2 ストリーミングした画像を取得してOpenCVで処理をする
ストリーミングされた動画を別PCで受信して処理する方法
#!/usr/bin/env python # -*- coding: utf-8 -*- import cv2 #mjpg-streamerを動作させているPC・ポートを入力 URL = "http://192.168.0.10:8080/?action=stream" s_video = cv2.VideoCapture(URL) while True: ret, img = s_video.read() cv2.imshow("WebCamera form Raspberry pi",img) key = cv2.waitKey(1) if k == 27: #Esc入力時は終了 break
ROS端末間のTopic通信 (デスクトップPCとRaspberry pi )
この記事の目的
ROS 端末間 (デスクトップPCとRaspberry pi )のTopic 通信についてまとめる
1. デスクトップ PC とRaspberry pi 間のTopic 通信
ROSでは複数の端末間でのTopic通信を行う機能が提供されている。(下記参考サイト参照)
上記2つ目のサイトに説明があるようにROSは『Master』端末が『roscore』を実行し、それ以外の『Slave 』端末がMaster からの調停に従ってTopicを送受信する。(複数のSlaveを存在させることも可能) 一般的にはMasterをPC (システム全体を管理するPC)、Slaveを移動ロボット、小型機器などで構成することが多い。
同一のLANにROSを搭載したデスクトップPCとRaspberry pi が接続されている場合は下記の手順でROS Topic通信を行うことが可能である。
今回利用している環境
- Master PC (Desktop) : Ubuntu 18.04 + ROS melodic (有線接続)
- Slave PC (Raspberry pi) : Ubuntu Mate 16.04 + ROS kinetic (無線接続)
1.1 IPアドレスの確認/設定
最初に接続対象となる機器のIPアドレスを確認する必要がある。IPアドレスはターミナルを起動して下記コマンドを実行すると確認できる。
ifconfig #出力される文字列の "inet "の値を確認
LANの設定によってはPCのIPアドレスが毎回変動してしまい以降のROS設定を毎回修正する必要があるため固定IPとしておくと良い。 IPの固定はUbuntuのネットワーク設定から可能である。(下記はUbuntu18.04における設定画面とUbuntu Mate 16.04における設定画面)
1.2 SSHの設定
Slave端末を移動ロボットに搭載して通信する場合はSSHの設定をしてMaster端末からアクセスできるようにしておくと便利である。 SSHの導入は下記コマンドから行う。(Master/Slave共に実施)
sudo apt-get install openssh-server
その後、SlaveのRaspberry pi で下記設定を実施してPort 22を開放する。
sudo ufw allow 22 #Port 22のFireWall許可 sudo /etc/init.d/ssh restart # sshの再起動 sudo systemctl enable ssh # 起動時にsshが起動するように設定
上記設定後、ターミナルに下記コマンドを入力するとMasterからSlave PCの処理を実施できるようになる。
ssh -p 22 ユーザー名@ローカルアドレス名
1.3 ROS設定の修正
ROSで端末間通信を行うために、MasterとSlaveでそれぞれ下記コマンドを実行する。
- Master PC (下記は IPアドレスが 192.168.0.2の場合)
export ROS_MASTER_URI=http://192.168.0.2:11311 export ROS_IP=192.168.0.2
- Slave PC (下記は IPアドレスが 192.168.0.3の場合)
export ROS_MASTER_URI=http://192.168.0.2:11311 export ROS_IP=192.168.0.3
いずれも利用時に毎回書くのは面倒なので.bashrcに記載しておくと良い。
Ubuntuにファイアフォール(ufw)を導入済みの場合下記コマンドで無効化しておく。
sudo ufw allow 11311 sudo ufw reload
1.4 ROSノードの起動
上記設定を実施した状態でROSノードを立ち上げ、TopicをPublishすると相互にデータがやり取りできる。
（動作確認には下記記事のtalker.pyとlistener.pyなどを用いると良い)
正しく動作するとSlaveで起動したtalker.pyのtopicをMasterで取得(もしくはその逆)できることが確認できる。
