ロボクメモ Robojimamemo

ロボットを研究する、ある学生のメモ。日々、勉強したことをメモ代わりに残して共有します(ROS,C++,python,linux,etc...)

ROS 画像からoccupancy grid を任意の値でpublishしたい

画像からros occupancy grid mapをpublish

既存のmap_server(http://wiki.ros.org/map_server)では画像からでは0(free), 100(occupied), -1(unknown)の3つの状態しかえらぶことができない。 将来のバージョンではグラデーションができるようになるみたいだが、すぐこの機能が欲しかったので実装してみた。

コード

img2navgrid.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import numpy as np
import sys
import rospy
import yaml

from PIL import Image

from pyquaternion import Quaternion
#sudo pip install pyquaternion

from nav_msgs.msg import OccupancyGrid

if __name__== '__main__':

args = sys.argv
if(len(args) != 2):
print("Usage: python img2gridmap.py <yaml>")

#yaml file名
yaml_file = args[1]
with open("map_reorigin.bt.yaml", "rt") as fp:
text = fp.read()
map_info = yaml.safe_load(text)

#read img
im = np.array(Image.open(map_info['image']), dtype='int8')

print('##########image info############')
print('name: %s' % map_info["image"])
print('size: %s' % str(im.shape))
print('origin[x,y,yaw]: %s' % str(map_info["origin"]))
print('resolution: %s' % str(map_info["resolution"]))
print('################################')

# set ros node
publish_gridmap = rospy.Publisher("/map_py", OccupancyGrid, queue_size=10)
rospy.init_node("map_server_py", anonymous=True)

# set msg_info
_map = OccupancyGrid()
_map.header.stamp = rospy.Time.now()
_map.header.frame_id = "/map"
_map.info.resolution = map_info["resolution"]
_map.info.width = im.shape[1]
_map.info.height = im.shape[0]
_map.info.origin.position.x = map_info["origin"][0]
_map.info.origin.position.y = map_info["origin"][1]

rad = map_info["origin"][2]
q = Quaternion(axis=[0,0,1], angle=rad).elements
_map.info.origin.orientation.x = q[1]
_map.info.origin.orientation.y = q[2]
_map.info.origin.orientation.z = q[3]
_map.info.origin.orientation.w = q[0]

#change occupancy data
im_py = np.flip(im,0).reshape(-1,1)
_map.data = im_py

r = rospy.Rate(1)
while not rospy.is_shutdown():
publish_gridmap.publish(_map)
r.sleep()
print("publish")

使い方

python img2navgrid [yaml file]

便利

linux google driveと同期 

Ubuntu でメモの管理を一箇所でまとめてやりたい 

自分はエディタとしてVimを使用している。
メモなどをふと書こうと思った時に、端末上で書いてしまいどのPCで書いたか忘れてしまうことが多々ある。

そんなとき発見した記事
解決方法、Google Driveを同期してその中でメモを取る。

参考:
Ubuntu(他Linux)で Googleドライブ を使う方法アレコレまとめ | mogi2fruitsどっとねっと

google-drive-ocamlfuse のダウンロード

$sudo add-apt-repository ppa:alessandro-strada/ppa  
$sudo apt-get update  
$sudo apt-get install google-drive-ocamlfuse  

1. アカウント認証
$ google-drive-ocamlfuse

2. フォルダの作成、マウント

$ mkdir ~/TOARU_google_drive  
$ google-drive-ocamlfuse ~/TOARU_google_drive

こんな感じになる.

f:id:shun0612:20181128195017p:plain
ディレクトリ画面

3. 使用し終わったらアンマウント
$fusermount -u ~/TOARU_google_drive

便利

vim で clipboardが無効だけどコピーアンドペーストしたい

vim で 書いたコードを他の環境に貼り付けたい時

クリップボードを有効なものをインストールする
・再ビルド
等、いろいろあるが最近見つけた方法 xselの使用. www.mk-mode.com

1. xselのインストール
$ sudo apt install xsel

2. vimrcに書く設定

vnoremap <C-c> :w !xsel -bi<CR><CR>

こんな感じにするとCtrl-Cでコピーすることができる。

vim quickrun でpython,シェルスクリプトの開発

vim の端末で実行しながら開発

簡単なpython , シェルスクリプトを作成するとき、実行結果を確認しながら開発できる、 他にもvim の中で簡単にコマンドが実行できる。

参考:
d.hatena.ne.jp

1. インストール https://github.com/thinca/vim-quickrun .vimrcに以下を追加
NeoBundle 'thinca/vim-quickrun'

2. vimprocのインストール
時間のかかる作業をまわすときは非同期で編集をすることができるといい。 https://github.com/Shougo/vimproc.vim

3. 設定を.vimrcに書き込む
私は下の狭いwindowで結果を確認するように設定している.

let g:quickrun_config = {
\"_" : {
\ "runner" : "vimproc",
\ "runner/vimproc/updatetime" : 60,
\ "outputter/buffer/split" : "botright 8sp",
\ "outputter/buffer/close_on_empty" : 1
\ },
\}

byobu 1コマンドで複数のコマンドを実行 screen 編

byobu はロボットやリモートでの作業に便利

以前紹介したbyobuでの複数コマンド実行はbackendがtmuxの人にしか使うことができない。

memo-methods-in-researching.hatenablog.com

ちなみにbackendの変更は
$byobu-select-backend
で変更可能。
screenはそれぞれのwindowに対してユニークな名前がつけられてわかりやすい。 screenコマンドの場合はbyobuに入らなくてもガンガンコマンドをバックプロセスに投げられる。 もう少し工夫した方法はないか。

1. windowの名前の変更 スクリーン内のウィンドウの名前はこのコマンドで変更することができる。
$ echo -ne "\ekhello\e\\"

2. セッション名、window名を指定してコマンドを実行
$ byobu -r <Session> -p <Window> -X stuff "<command>^M"

#!/bin/bash
command_0='echo -ne "\ekhello\e\\\\" && screen && echo robot'
command_1='echo -ne "\ekhello2\e\\\\" && screen && echo research'

session="toaru"
window_bash="bash"

byobu -d -m -S ${session} 
byobu -r ${session} -X stuff "${command_0}^M" 
sleep 1s
byobu -r ${session} -p $window_bash -X stuff "${command_1}^M" 

echo "all done ... "

f:id:shun0612:20181123165225p:plain:w300 f:id:shun0612:20181123165236p:plain:w300

便利

byobu 1コマンドで複数のコマンドを実行 tmux編

byobuでコマンドを一つにして簡単に

ロボットやリモートでプロセスを回すときにはbyobuを使うとネットワークが突然切れても大丈夫。 ssh 先で複数の端末を開いて作業することができる。  http://byobu.org/about.html 

windowのlayoutを保存して復元

1. paneを分ける
縦割り:Shift + F2
横割り:Ctrl + F2
大きさ調整:Alt + Shift + 方向キー

2. 保存
$byobu-layout save <NAME>

f:id:shun0612:20181123161453p:plain:w300

コマンドを指定したpaneに振り分ける

1. 保存したwindowを復元 $ byobu-layout restore <NAME>

2. コマンドの振り分け

#!/bin/bash
window_name="toaru"
byobu-layout restore <NAME> #1.の内容
byobu rename-window $window_name # toaruがそのwindowの名前

byobu send-keys -t $window_name.0 'echo toaru' C-m
byobu send-keys -t $window_name.1 'echo gakusei' C-m
byobu send-keys -t $window_name.2 'echo robot' C-m
byobu send-keys -t $window_name.3 'echo research' C-m  

f:id:shun0612:20181123161525p:plain:w300

便利 screen でやる方法はこちら

memo-methods-in-researching.hatenablog.com