ROS x Depthセンサで遊んでみる
やりたいこと
Depthセンサで取得したデータで遊んでみる
xtionやkinectで使えるros package: rgbd_launch - ROS Wiki
今回はsoftkineticを用いる
設定
RGBDの情報を使って亀を制御してみる
ピンクと黄色のボールで亀を制御
ピンクのボールをカメラに近づけたり遠ざけたりすることで前進、後退を制御
黄色のボールをカメラに近づけたり遠ざけたりすることで右回転、左回転を制御
鬼畜の操作方法...操作鬼難しい笑
ざっくりとした流れ
色相抽出
短形検出
短形部分の距離計算
色ごとの距離で制御
構成
├── CMakeLists.txt
├── launch
│ └── depth_turtlesim.launch
├── package.xml
└── scripts
└── teleop_depth.py
teleop_depth.py
完全に深夜のテンションで書いたとんでもないくらい適当なコード
#!/usr/bin/env python # -*- coding: utf-8 -*- import sys import rospy import message_filters import numpy as np from geometry_msgs.msg import Twist from sensor_msgs.msg import Image from sensor_msgs.msg import CameraInfo import cv2 from cv_bridge import CvBridge, CvBridgeError class DepthTeleopTurtle: def __init__(self): self.bridge = CvBridge() rgb_sub = message_filters.Subscriber("/softkinetic_camera/rgb/image_color", Image) depth_sub = message_filters.Subscriber("/softkinetic_camera/depth/image_raw", Image) # # for extract x,y,z positoin from depth image # camera_info = rospy.wait_for_message("/softkinetic_camera/depth/camera_info", CameraInfo) # self.invK = np.linalg.inv(np.array(camera_info.K).reshape(3,3)) # xyz_point = np.matmul(self.invK, image_point) * z # z = depth_image[x, y] self.mf = message_filters.ApproximateTimeSynchronizer([rgb_sub, depth_sub], 30, 0.5) self.mf.registerCallback(self.callback) self.pub = rospy.Publisher('turtle1/cmd_vel', Twist, queue_size=10) def pink_yellow_mask(self, frame): hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) lower_light_pink = np.array([168, 100, 100]) upper_light_pink = np.array([188, 255, 255]) mask_pink = cv2.inRange(hsv, lower_light_pink, upper_light_pink) lower_light_yellow = np.array([20, 50, 50]) upper_light_yellow = np.array([100, 255, 255]) mask_yellow = cv2.inRange(hsv, lower_light_yellow, upper_light_yellow) return mask_pink, mask_yellow def detect_rect(self, mask): image, contours, hierarchy = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) rects = [] for contour in contours: approx = cv2.convexHull(contour) rect = cv2.boundingRect(approx) rects.append(np.array(rect)) if len(rects) > 0: rect = max(rects, key=(lambda x: x[2] * x[3])) # w x hの一番大きいリストを抽出 return rect def disp(self, frame, rect_pink, rect_yellow): if rect_pink is not None and rect_yellow is not None: cv2.rectangle(frame, tuple(rect_pink[0:2]), tuple(rect_pink[0:2] + rect_pink[2:4]), (0, 0, 255), thickness=2) cv2.rectangle(frame, tuple(rect_yellow[0:2]), tuple(rect_yellow[0:2] + rect_yellow[2:4]), (0, 255, 0), thickness=2) return frame def callback(self, rgb_data , depth_data): twist = Twist() try: rgb_image = self.bridge.imgmsg_to_cv2(rgb_data, 'passthrough') depth_image = self.bridge.imgmsg_to_cv2(depth_data, 'passthrough') except CvBridgeError, e: rospy.logerr(e) depth_array = np.array(depth_image, dtype=np.float32) mask_pink, mask_yellow = self.pink_yellow_mask(rgb_image) rect_pink = self.detect_rect(mask_pink) rect_yellow = self.detect_rect(mask_yellow) # for debug frame = self.disp(rgb_image, rect_pink, rect_yellow) cv2.imshow('frame',frame) k = cv2.waitKey(5) & 0xFF if k == 27: sys.exit() low_weight = 0.8 high_weight = 1.2 if rect_pink is not None and rect_yellow is not None: # trim depth trim_depth_pink = depth_array[int(rect_pink[0]/2.0*low_weight):int((rect_pink[0]+rect_pink[2])/2.0*high_weight), int(rect_pink[1]/2.0*low_weight):int((rect_pink[1]+rect_pink[3])/2.0*high_weight)] # reject zero and non valid_pink = trim_depth_pink[np.nonzero(trim_depth_pink)] valid_pink = [x for x in valid_pink if not np.isnan(x)] # trim depth trim_depth_yellow = depth_array[int(rect_yellow[0]/2.0*low_weight):int((rect_yellow[0]+rect_yellow[2])/2.0*high_weight), int(rect_yellow[1]/2.0*low_weight):int((rect_yellow[1]+rect_yellow[3])/2.0*high_weight)] # reject zero and non valid_yellow = trim_depth_yellow[np.nonzero(trim_depth_yellow)] valid_yellow = [x for x in valid_yellow if not np.isnan(x)] # teleop turtle # if you close pink ball, turtle back # if you close yellow ball, turtle turn right if len(valid_pink) != 0 and len(valid_yellow) != 0: z_pink = min(valid_pink) print "pink_go:{}".format(z_pink) if z_pink > 0.30: twist.linear.x = 0.0 elif z_pink > 0.20: twist.linear.x = 1.5 elif z_pink > 0.0: twist.linear.x = -1.5 else: twist.linear.x = 0.0 z_yellow = min(valid_yellow) print "yellow_rot:{}".format(z_yellow) if z_yellow > 0.30: twist.angular.z = 0.0 elif z_yellow > 0.20: twist.angular.z = 1.5 elif z_yellow > 0.0: twist.angular.z = -1.5 else: twist.angular.z = 0.0 self.pub.publish(twist) def main(): rospy.init_node('depth_teleop_turtle') DepthTeleopTurtle() rospy.spin() if __name__ == "__main__": main()
参考：
pythonで最大値、最小値を求める色々 - BlankTar
launch
<launch> <!-- Turtlesim Node --> <node pkg="turtlesim" type="turtlesim_node" name="sim"/> <!-- depth node --> <include file="$(find softkinetic_camera)/launch/softkinetic_camera_demo.launch" /> <node pkg="teleop_depth" type="teleop_depth.py" name="teleop_depth" launch-prefix="xterm -font r14 -bg darkblue -geometry 113x30+503+80 -e" required="true" /> </launch>
CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3) project(teleop_depth) find_package(catkin REQUIRED COMPONENTS rospy std_msgs geometry_msgs sensor_msgs ) catkin_package( CATKIN_DEPENDS std_msgs geometry_msgs sensor_msgs rospy )
package.xml
<buildtool_depend>catkin</buildtool_depend> <build_depend>geometry_msgs</build_depend> <build_depend>rospy</build_depend> <build_depend>std_msgs</build_depend> <build_depend>sensor_msgs</build_depend> <run_depend>geometry_msgs</run_depend> <run_depend>rospy</run_depend> <run_depend>std_msgs</run_depend> <run_depend>sensor_msgs</run_depend>
次のアクション
softkineticのdepthセンサ情報でOpen3Dを動かしてみよう〜楽しみ♪
参考
配布されるTopic
/softkinetic_camera/depth/camera_info /softkinetic_camera/depth/image_raw /softkinetic_camera/depth/image_raw/compressed /softkinetic_camera/depth/image_raw/compressed/parameter_descriptions /softkinetic_camera/depth/image_raw/compressed/parameter_updates /softkinetic_camera/depth/image_raw/compressedDepth /softkinetic_camera/depth/image_raw/compressedDepth/parameter_descriptions /softkinetic_camera/depth/image_raw/compressedDepth/parameter_updates /softkinetic_camera/depth/image_raw/theora /softkinetic_camera/depth/image_raw/theora/parameter_descriptions /softkinetic_camera/depth/image_raw/theora/parameter_updates /softkinetic_camera/depth/points /softkinetic_camera/parameter_descriptions /softkinetic_camera/parameter_updates /softkinetic_camera/rgb/camera_info /softkinetic_camera/rgb/image_color /softkinetic_camera/rgb/image_color/compressed /softkinetic_camera/rgb/image_color/compressed/parameter_descriptions /softkinetic_camera/rgb/image_color/compressed/parameter_updates /softkinetic_camera/rgb/image_color/compressedDepth /softkinetic_camera/rgb/image_color/compressedDepth/parameter_descriptions /softkinetic_camera/rgb/image_color/compressedDepth/parameter_updates /softkinetic_camera/rgb/image_color/theora /softkinetic_camera/rgb/image_color/theora/parameter_descriptions /softkinetic_camera/rgb/image_color/theora/parameter_updates /softkinetic_camera/rgb/image_mono /softkinetic_camera/rgb/image_mono/compressed /softkinetic_camera/rgb/image_mono/compressed/parameter_descriptions /softkinetic_camera/rgb/image_mono/compressed/parameter_updates /softkinetic_camera/rgb/image_mono/compressedDepth /softkinetic_camera/rgb/image_mono/compressedDepth/parameter_descriptions /softkinetic_camera/rgb/image_mono/compressedDepth/parameter_updates /softkinetic_camera/rgb/image_mono/theora /softkinetic_camera/rgb/image_mono/theora/parameter_descriptions /softkinetic_camera/rgb/image_mono/theora/parameter_updates
提供するサービス
/softkinect_rviz/get_loggers /softkinect_rviz/reload_shaders /softkinect_rviz/set_logger_level /softkinect_tf/get_loggers /softkinect_tf/set_logger_level /softkinetic_camera/depth/image_raw/compressed/set_parameters /softkinetic_camera/depth/image_raw/compressedDepth/set_parameters /softkinetic_camera/depth/image_raw/theora/set_parameters /softkinetic_camera/get_loggers /softkinetic_camera/rgb/image_color/compressed/set_parameters /softkinetic_camera/rgb/image_color/compressedDepth/set_parameters /softkinetic_camera/rgb/image_color/theora/set_parameters /softkinetic_camera/rgb/image_mono/compressed/set_parameters /softkinetic_camera/rgb/image_mono/compressedDepth/set_parameters /softkinetic_camera/rgb/image_mono/theora/set_parameters /softkinetic_camera/set_logger_level /softkinetic_camera/set_parameters
利用できるパラメータ
/softkinetic_camera/camera_link /softkinetic_camera/color_compression /softkinetic_camera/color_frame_format /softkinetic_camera/color_frame_rate /softkinetic_camera/confidence_threshold /softkinetic_camera/depth/image_raw/compressed/format /softkinetic_camera/depth/image_raw/compressed/jpeg_quality /softkinetic_camera/depth/image_raw/compressed/png_level /softkinetic_camera/depth/image_raw/compressedDepth/depth_max /softkinetic_camera/depth/image_raw/compressedDepth/depth_quantization /softkinetic_camera/depth/image_raw/compressedDepth/png_level /softkinetic_camera/depth/image_raw/theora/keyframe_frequency /softkinetic_camera/depth/image_raw/theora/optimize_for /softkinetic_camera/depth/image_raw/theora/quality /softkinetic_camera/depth/image_raw/theora/target_bitrate /softkinetic_camera/depth_frame_format /softkinetic_camera/depth_frame_rate /softkinetic_camera/depth_mode /softkinetic_camera/depth_optical_frame /softkinetic_camera/enable_color /softkinetic_camera/enable_depth /softkinetic_camera/far_plane /softkinetic_camera/hfov /softkinetic_camera/limit_max /softkinetic_camera/limit_min /softkinetic_camera/min_neighbours /softkinetic_camera/near_plane /softkinetic_camera/rgb/image_color/compressed/format /softkinetic_camera/rgb/image_color/compressed/jpeg_quality /softkinetic_camera/rgb/image_color/compressed/png_level /softkinetic_camera/rgb/image_color/compressedDepth/depth_max /softkinetic_camera/rgb/image_color/compressedDepth/depth_quantization /softkinetic_camera/rgb/image_color/compressedDepth/png_level /softkinetic_camera/rgb/image_color/theora/keyframe_frequency /softkinetic_camera/rgb/image_color/theora/optimize_for /softkinetic_camera/rgb/image_color/theora/quality /softkinetic_camera/rgb/image_color/theora/target_bitrate /softkinetic_camera/rgb/image_mono/compressed/format /softkinetic_camera/rgb/image_mono/compressed/jpeg_quality /softkinetic_camera/rgb/image_mono/compressed/png_level /softkinetic_camera/rgb/image_mono/compressedDepth/depth_max /softkinetic_camera/rgb/image_mono/compressedDepth/depth_quantization /softkinetic_camera/rgb/image_mono/compressedDepth/png_level /softkinetic_camera/rgb/image_mono/theora/keyframe_frequency /softkinetic_camera/rgb/image_mono/theora/optimize_for /softkinetic_camera/rgb/image_mono/theora/quality /softkinetic_camera/rgb/image_mono/theora/target_bitrate /softkinetic_camera/rgb_calibration_file /softkinetic_camera/rgb_optical_frame /softkinetic_camera/search_radius /softkinetic_camera/serial /softkinetic_camera/use_frustum_culling_filter /softkinetic_camera/use_passthrough_filter /softkinetic_camera/use_radius_outlier_filter /softkinetic_camera/use_serial /softkinetic_camera/use_voxel_grid_filter /softkinetic_camera/vfov /softkinetic_camera/voxel_grid_size
