以下記事のようにwifi経由でROS接続する場合、画像データは圧縮して扱った方が良さそうです。
生データを使うと、通信帯域が狭いときに通信遅延が起こる場合がありました。
JetsonNano側での画像データ受信テスト
ROS_MASTER側で画像の生データ(/front_camera/image_raw
)を0.1秒毎に送信した上で、これをROS接続したマシン側で受信するコード
subscribe_image_raw_test.py
#!/usr/bin/env python
import rospy
import time
from sensor_msgs.msg import Image
import cv2
from cv_bridge import CvBridge, CvBridgeError
now = time.time()
pre = time.time()
is_use_exp=False
bridge = CvBridge()
def image_view_callback_raw(data):
image_view_callback(data)
def image_view_callback_exp(data):
image_view_callback(data)
def image_view_callback(data):
global pre
global now
global bridge
now = time.time()
print ("subscribe image time:{0}".format((now - pre)) + "[sec]")
pre = now
try:
cv_image = bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
cv2.imshow("Origin Image", cv_image)
cv2.waitKey(3)
def prepare():
rospy.init_node('prepare', anonymous=True)
rospy.Subscriber("/front_camera/image_raw", Image, image_view_callback, queue_size=1)
#rospy.Subscriber("/front_camera/image_exp", Image, image_view_callback, queue_size=1)
r = rospy.Rate(10)
rospy.spin()
if __name__ == '__main__':
try:
prepare()
except rospy.ROSInterruptException:
pass
0.1秒毎に送信しているにも関わらず、受信は0.2秒以上掛かっている
(通信帯域が8Mbps程の場合)
$ rosrun workspace subscribe_image_raw_test.py
subscribe image time:0.850562095642[sec]
subscribe image time:0.246268987656[sec]
subscribe image time:0.122575998306[sec]
subscribe image time:0.248157024384[sec]
subscribe image time:0.225661993027[sec]
subscribe image time:0.207190036774[sec]
subscribe image time:0.178869009018[sec]
subscribe image time:0.286194801331[sec]
subscribe image time:0.106919050217[sec]
subscribe image time:0.155977010727[sec]
...
画像は、rawデータではなく圧縮データをrawデータに変換して扱う
以下記事を参考にさせて頂き、画像は圧縮データを扱う
必要なパッケージをインストール
sudo apt-get install -y ros-melodic-image-transport
sudo apt-get install -y ros-melodic-image-transport-plugins
受信側で圧縮データをrawデータに変換するroslaunchファイル
image_republish.launch
<?xml version="1.0"?>
<launch>
<arg name="input_image_topic" default="/front_camera/image_raw" />
<arg name="output_image_topic" default="/front_camera/image_exp" />
<arg name="use_compressed" default="true" />
<arg name="use_image_view" default="true" />
<group if="$(arg use_compressed)">
<node name="image_republish" pkg="image_transport" type="republish" args="compressed raw">
<remap from="in" to="$(arg input_image_topic)" />
<remap from="out" to="$(arg output_image_topic)" />
</node>
<group if="$(eval use_image_view==true)">
<node name="image_view" pkg="image_view" type="image_view" >
<remap from="image" to="$(arg output_image_topic)"/>
</node>
</group>
</group>
<group unless="$(arg use_compressed)">
<group if="$(eval use_image_view==true)">
<node name="image_view" pkg="image_view" type="image_view" >
<remap from="image" to="$(arg input_image_topic)"/>
</node>
</group>
</group>
</launch>
以下の通りlaunchする
$ roslaunch workspace image_republish.launch
圧縮データをrawデータに変換し、変換した方の画像送受信用トピック(/front_camera/image_exp
)をsubscribeすると、0.1秒間隔で取得できていた
$ rosrun workspace subscribe_image_raw_test.py
subscribe image time:0.417353153229[sec]
subscribe image time:0.206629991531[sec]
subscribe image time:0.0283079147339[sec]
subscribe image time:0.0864000320435[sec]
subscribe image time:0.0865058898926[sec]
subscribe image time:0.118394136429[sec]
subscribe image time:0.0980160236359[sec]
subscribe image time:0.0875458717346[sec]
subscribe image time:0.114013910294[sec]
subscribe image time:0.0870351791382[sec]
subscribe image time:0.089733839035[sec]
subscribe image time:0.110941171646[sec]
...
生データを無線で0.1秒間隔で送るのは無理がありそうです。
(有線で50Mbps程出せば可能かもしれないが、、無線環境ではかなり厳しい)
備考
image_republish実行時に以下オプションを与えて、image_viewを非表示に設定できるようにしている
$ roslaunch workspace image_republish.launch use_image_view:=false