Gazebo turtlebot3 burger Depth Camera Program

サンプルプログラムもグーグルで検索すると、
https://www.programcreek.com/python/example/99841/sensor_msgs.msg.PointCloud2 がありました。
Example 6 辺りが使えそうです。
これは、どうやら、ros_numpy パッケージの抜粋みたいです。
早速、パッケージをインストールしてみます。
$ sudo apt install ros-melodic-ros-numpy

2. 作成
上記プログラムを参考に、早速テストプログラムを作ってみます。
1. ~/catkin_ws/src/ros_start/scripts/pointcloud2_test2.py
#!/usr/bin/env python ''' pointcloud2_test2.py https://www.programcreek.com/python/example/99841/sensor_msgs.msg.PointCloud2 http://docs.ros.org/en/jade/api/ros_numpy/html/point__cloud2_8py_source.html ''' import rospy import numpy as np import math from geometry_msgs.msg import Twist from sensor_msgs.msg import PointCloud2 from ros_numpy.point_cloud2 import pointcloud2_to_array,fields_to_dtype,DUMMY_FIELD_PREFIX # Example 6 def pointcloud2_to_array_ex6(cloud_msg, squeeze=True): ''' Converts a rospy PointCloud2 message to a numpy recordarray Reshapes the returned array to have shape (height, width), even if the height is 1. The reason for using np.frombuffer rather than struct.unpack is speed... especially for large point clouds, this will be <much> faster. ''' # construct a numpy record type equivalent to the point type of this cloud dtype_list = fields_to_dtype(cloud_msg.fields, cloud_msg.point_step) # parse the cloud into an array cloud_arr = np.frombuffer(cloud_msg.data, dtype_list) # remove the dummy fields that were added cloud_arr = cloud_arr[ [fname for fname, _type in dtype_list if not (fname[:len(DUMMY_FIELD_PREFIX)] == DUMMY_FIELD_PREFIX)]] if squeeze and cloud_msg.height == 1: return np.reshape(cloud_arr, (cloud_msg.width,)) else: return np.reshape(cloud_arr, (cloud_msg.height, cloud_msg.width)) def get_max_min(dt): x_min = 1000.0 x_max = -1000.0 x_min_y=0 x_max_y=0 y_min = 1000.0 y_max = -1000.0 z_min=1000.0 z_max=-1000.0 for j in range(640): (x,y,z,rgb) = dt[j] if np.isnan(x) != True and np.isnan(y) != True and np.isnan(z) != True: if x >= x_max: x_max =x x_max_y=y if x <= x_min: x_min =x x_min_y=y if y >= y_max: y_max =y if y <= y_min: y_min =y if z >= z_max: z_max =z if z <= z_min: z_min =z print 'x_min=',x_min,' ,x_max=',x_max,' ,x_min_y=',x_min_y,' ,x_max_y=',x_max_y print 'y_min=',y_min,' ,y_max=',y_max print 'z_min=',z_min,' ,z_max=',z_max def main(): rospy.init_node('pointcloud2_test1') cloud_msg = rospy.wait_for_message('camera/depth/points', PointCloud2, timeout=5) #np_data = pointcloud2_to_array(cloud_msg) np_data = pointcloud2_to_array_ex6(cloud_msg) #print np_data print np_data.shape #(480,640) #print np_data[-1,0:10] #(x,y,z,rgb) = np_data[-1,0] #print x,y,z,rgb #line240 = np_data[240] print '-----' key_s =[0,30,60,90,120,150,180,210,240,270,300,330,360,390,420,450,479] key_s.sort() for i in key_s: print '--- ',i,' ---' get_max_min(np_data[i]) if __name__ == '__main__': main()

3. 実行は、
$ roscore
$ roslaunch turtlebot3_gazebo turtlebot3_world_nav_nishi.launch

$ roscd ros_start/scripts
$ python pointcloud2_test2.py

結果は、
.... --- 210 --- x_min= -0.90156 ,x_max= 0.712119 ,x_min_y= -0.0832426 ,x_max_y= -0.0657512 y_min= -0.0832426 ,y_max= -0.0657512 z_min= 1.23535 ,z_max= 1.56399 --- 240 --- x_min= -0.90199 ,x_max= 0.712388 ,x_min_y= 0.00141157 ,x_max_y= 0.00111485 y_min= 0.00111485 ,y_max= 0.00141157 z_min= 1.23582 ,z_max= 1.56473 --- 270 --- x_min= -0.902421 ,x_max= 0.712657 ,x_min_y= 0.0861466 ,x_max_y= 0.0680314 y_min= 0.0680314 ,y_max= 0.0861466 z_min= 1.23629 ,z_max= 1.56548 --- 300 --- x_min= -0.634559 ,x_max= 0.634662 ,x_min_y= 0.120159 ,x_max_y= 0.120178 y_min= 0.120159 ,y_max= 0.120179 z_min= 1.1008 ,z_max= 1.10098 ... --- 479 --- x_min= -0.168647 ,x_max= 0.168657 ,x_min_y= 0.126419 ,x_max_y= 0.126427 y_min= 0.126419 ,y_max= 0.126427 z_min= 0.29256 ,z_max= 0.29258

理解するのに、おんちゃんは、時間がかかりました。
どうやら、
x が、 カメラの左右軸 (x0 から) の値(M)。
y が、カメラの上下軸 (y0 から) の値(M)。
z が、カメラの前方軸 (z0 から) の値(M)。
中心座標 (x0,y0,z0) = (0,0,0) は、カメラの中心点の様です。
受信データで言えば、行:列 = 240:320 が 中心座標 の (x,y,z,rgb) になる。
ただし、 y は値が逆転しているのか?

ここまで、わかれば、Gazebo / Turtlebot3 バーガーを、動かすプログラムができそうです。

4. 本題のプログラム
Gazebo / Turtlebot3 バーガー に、 深度カメラ(rgbd カメラ: openni kinect) プラグインを載せて、
トピックで送られてくる、 /camera/depth/points (sensor_msgs/PointCloud2) を解析して、
バーガーが壁から 35cm 近づいたら、直進をやめて、向きを変えるプログラムを作ります。

測定は、行=240 のデータのみ使います。
y=0.0 なので、カメラの高さ位置の距離データのみ利用します。
列のデータは、[0,40, 80, 120, 160, 200, 240, 280, 320, 360, 400, 440, 480, 520, 560, 600,639] の所のみチェックします。
該当箇所の x,z から、 カメラ(x0,z0) からの距離をそれぞれ計算して、 0.35 [M] 以内か判定します。

5. 作成2
1. ~/catkin_ws/src/ros_start/scripts/tbot3_rgbd_camera1.py
#!/usr/bin/env python ''' tbot3_rgbd_camera1.py https://www.programcreek.com/python/example/99841/sensor_msgs.msg.PointCloud2 http://docs.ros.org/en/jade/api/ros_numpy/html/point__cloud2_8py_source.html ''' import rospy import numpy as np import math from geometry_msgs.msg import Twist from sensor_msgs.msg import PointCloud2 from ros_numpy.point_cloud2 import pointcloud2_to_array,fields_to_dtype,DUMMY_FIELD_PREFIX class Rgbd_Camera_Anly(object): def __init__(self): self._cloud_msg = None self._height0 = int(480/2) self._width = 640 self._x_front = int(self._width/2) def get_sdata(self): self._cloud_msg = None self._closely = None while self._cloud_msg is None: try: self._cloud_msg = rospy.wait_for_message('camera/depth/points', PointCloud2, timeout=5) except: pass self._np_data = pointcloud2_to_array(self._cloud_msg) self.angle_div() return self._sc_data def angle_div(self): self._sc_data = {} self._out_of_range=1 # get 1 row data of camera height level dt = self._np_data[self._height0] # set horizontal check point [0,40, 80, 120, 160, 200, 240, 280, 320, 360, 400, 440, 480, 520, 560, 600,639] angle = range(0,self._width,40) angle.append(639) angle.sort() for i in angle: self._sc_data[i] = None (x,y,z,rgb) = dt[i] # x,y,z data is alive if np.isnan(x) != True and np.isnan(y) != True and np.isnan(z) != True: self._out_of_range=0 l = math.sqrt(x**2 + z**2) if l <= 0.35: self._sc_data[i] = l if self._out_of_range == 0: self.check_closely() else: self._closely=1 self._right_closely=1 self._left_closely=0 def check_closely(self): self._closely=0 self._right_closely=0 self._left_closely=0 # [0,40, 80, 120, 160, 200, 240, 280] angle_left = range(0,self._x_front,40) # [360, 400, 440, 480, 520, 560, 600, 639] angle_right = range(self._x_front+40,self._width,40) angle_right.append(639) #left side check for i in angle_left: if self._sc_data[i] != None: self._closely=1 self._left_closely += 1.0/self._sc_data[i] # right side check for i in angle_right: if self._sc_data[i] != None: self._closely=1 self._right_closely += 1.0/self._sc_data[i] # fornt if self._sc_data[320] != None : self._closely=1 def myhook(): print "shutdown time!" def main(): rospy.init_node('tbot3_3d_camera1') #rospy.on_shutdown(myhook) pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) # sleep need rospy.sleep(0.5) cnt=200 move=0 sts=0 rgbd_camera_anly=Rgbd_Camera_Anly() while not rospy.is_shutdown(): vel = Twist() data = rgbd_camera_anly.get_sdata() if sts == 0: if rgbd_camera_anly._closely == 0 : vel.linear.x = 0.05 print 'forward' sts=1 #print vel pub.publish(vel) else: sts=2 elif sts == 1: if rgbd_camera_anly._closely != 0: print 'stop' vel.linear.x = 0.0 #print vel pub.publish(vel) sts=2 elif sts==2: if rgbd_camera_anly._right_closely >= rgbd_camera_anly._left_closely: print 'turn left' vel.angular.z = 0.2 else: print 'turn right' vel.angular.z = -0.2 pub.publish(vel) sts=3 elif sts==3: if rgbd_camera_anly._closely == 0 : print 'turn stop' vel.angular.z = 0.0 pub.publish(vel) sts=0 cnt -=1 if cnt <= 0: print 'end' vel.linear.x = 0.0 vel.angular.z = 0.0 #print vel pub.publish(vel) rospy.sleep(2.0) break rospy.sleep(1.0) #rospy.spin() if __name__ == '__main__': main()

5. 実行は、
$ roscore

$ export TURTLEBOT3_MODEL=burger
$ roslaunch turtlebot3_gazebo turtlebot3_world_nav_nishi.launch

$rviz
   Fixed Frame : odom
   Add > RobotModel
   Add > ByTopic > /camera/depth/image_raw/image
   Add > ByTopic > /camera/depth/points/PointCloud2

$ roscd ros_start/scripts
$ python tbot3_rgbd_camera1.py

どうぞね!
少しは、バーガーが、自動で、動くかね!