ros_detector.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib; roslib.load_manifest('iri_bow_object_detector')

import rospy

import numpy as np
from os.path import exists
from os import system, getenv
from sys import exit

from iri_bow_object_detector.srv import *
from iri_bow_object_detector.msg import *
from iri_perception_msgs.msg import ImagePoint
from geometry_msgs.msg import Point

from iri_bow_object_detector import config_feats
#from iri_bow_object_detector.refine_grasp_point_finddd import select_grasp_point_finddd
#from iri_bow_object_detector.refine_grasp_point_middle import select_grasp_point_middle
#from iri_bow_object_detector.refine_grasp_point_manual import select_grasp_point_manual
from iri_bow_object_detector.refine_grasp_point import select_grasp_point

from iri_bow_object_detector.bow_detector_utils import ros_ima_to_numpy, save_extra_training_data

#from iri_bow_object_detector.run_detector import gen_windows
from iri_bow_object_detector.run_detector import detect_boxes
#import ipdb
import cv, cv2
import pylab




def read_vws(geo_vw_sets):
        vws=[]
        for geo_vw_set in geo_vw_sets:
                vws.append([[geo_vw.x, geo_vw.y, geo_vw.word] for geo_vw in geo_vw_set.geo_vws])
        return vws


def handle_detector_cb(req):
        vws = read_vws(req.geo_vw_sets)

        cv_image = ros_ima_to_numpy(req.image)
        cv_mask  = ros_ima_to_numpy(req.mask)

        cv_mask_gray = cv2.cvtColor(cv_mask, cv2.COLOR_RGB2GRAY)

        #Save data for future training
        save_extra_training_data(cv_image, cv_mask_gray)

        conf = config_feats.conf
        linear_svm_file_path = rospy.get_param("~linear_svm_file")
        if not exists(linear_svm_file_path):
            print "File not found:", non_linear_svm_file
            exit(-1)
        w  = np.load(linear_svm_file_path)

        if not exists(conf.det_results_dir):
                system('mkdir '+conf.det_results_dir)

        print "Processing the algorithm."
        (win, prob) = detect_boxes(w, rospy.get_param("~non_linear_svm_file"), cv_image.copy(), cv_mask_gray, conf, vws)

        print "Finished"
        rospy.loginfo("Detection box: " + str(win) + " probs: " + str(prob));

        res = GeoVwDetectionResponse()

        for idx, sol in enumerate(win):
                point1 = ImagePoint()
                point1.x = sol[0]
                point1.y = sol[1]

                point2 = ImagePoint()
                point2.x = sol[2]
                point2.y = sol[3]

                res.posible_solutions.append(ObjectBox(point1, point2, prob[idx]))

        return res



def select_grasp_point_cb(req):
        #TO DELETE THIS BLOCK
        #return select_grasp_point_wrinkled_map(req)
        #return select_grasp_point_finddd(req, rospy.get_param("~finddd_svm_file"))
        #return select_grasp_point_middle(req)
        #return select_grasp_point_manual(req)
        #END OF TO DELETE THIS BLOCK

        ref_method = 'finddd'
        #ref_method = 'manual'
        #ref_method = 'middle'
        svm_filename = None
        if ref_method == 'finddd':
                svm_filename = rospy.get_param("~finddd_svm_file")

        return select_grasp_point(req, ref_method, svm_filename)


def add_detector_server():
        rospy.init_node('detector')
        s1 = rospy.Service('geo_vw_detector', GeoVwDetection, handle_detector_cb)
        s2 = rospy.Service('refine_grasp_point', RefineGraspPoint, select_grasp_point_cb)

        # check params
        if (not rospy.has_param("~linear_svm_file")):
                rospy.logerr("Node has no param linear_svm_file. Please set it up on launch file")
                exit
        if (not rospy.has_param("~non_linear_svm_file")):
                rospy.logerr("Node has no param non_linear_svm_file. Please set it up on launch file")
                exit
        if (not rospy.has_param("~finddd_svm_file")):
                rospy.logerr("Node has no param non_linear_svm_file. Please set it up on launch file")
                exit
        print "Detecting regions."
        rospy.spin()

if __name__ == "__main__":
        add_detector_server()


# DEPRECATED Now using finddd
#def select_grasp_point_wrinkled_map(req):
        #(XX, YY, ZZ, W) = get_auto(req.wrinkled_map, [("pos", 0), ("curv", 4)])
        #P=[(box.point1.x, box.point1.y, box.point2.x, box.point2.y, box.value ) for box in req.posible_solutions]

        #P.sort(key=lambda x:x[4], reverse=True)
        #print P

        #if len(P) < 1:
                #return Point(0,0,0)

        #xc=(P[0][0]+P[0][2])/2
        #yc=(P[0][1]+P[0][3])/2

        #offset=30
        #print xc,yc, W.shape
        #WW=W[yc-30:yc+30,xc-30:xc+30]
        #print W[yc-30:yc+30,xc-30:xc+30].max()
        #y,x=np.unravel_index(np.argmax(WW),WW.shape)
        #print np.argmax(WW), WW[y,x]
        #x=xc-30+x
        #y=yc-30+y

        #print "Solution is " + str(y) + "," + str(x)

        ## show result
        #bridge = CvBridge()
        #try:
                #image = bridge.imgmsg_to_cv(req.image, "bgr8")
        #except CvBridgeError, e:
                #print e
        #cv.Rectangle(image,(x-1,y-1), (x+1, y+1), (255,255,255))
        #cv.NamedWindow("Grasp point")
        #cv.ShowImage("Grasp point", image)
        #cv.WaitKey(10)

        #return Point(XX[y,x], YY[y,x], ZZ[y,x])