Add fast select

Author: zhao1995

drv_brain/launch/includes/drv_v2_host.launch.xml

@@ -66,6 +66,8 @@
 
   <!-- Initialize user selection service -->
   <node ns="$(arg vision_namespace)" name="drv_user" pkg="drv_user" type="drv_user.py" output="screen">
+    <!-- If true, will select the target if only 1 candidate present -->
+    <param name="fast_select" value="true" />
   </node>
 
   <!-- Initialize color recognize service, this is used as a simplified object recognizer -->

drv_user/src/drv_user.py

@@ -12,27 +12,40 @@
 # This is used for getting selection here and in JARVIS
 param_vision_search_param_lock = '/vision/select/param/lock'
 
+# If true, will select the target if only 1 candidate present
+fast_select = True
+
 
 def handle_user_select(req):
     param_control_target_is_set = '/comm/param/control/target/is_set'
     param_control_user_selected = '/comm/param/control/target/selected'
 
+    rsp = user_selectResponse()
+    select_msg = String()
+
     num = req.select_num
+
+    if num == 1 and fast_select:
+        rsp.selected_id = 1
+        select_msg.data = 'Auto selected target No.1.'
+        print select_msg.data
+        pubInfo.publish(select_msg)
+        return rsp
+
     selected = -1
 
     info = "Please choose the one you want, if no target, enter 0."
     print info
     info_msg = String()
     info_msg.data = "CHOOSE"
-    pubInfo.publish(info_msg)
+    pubInfo.publish(info_msg)  # Send this msg to APP for display info
 
-    # broadcast the target select request
+    # Broadcast the target select request
     sr_msg = UInt32()
     sr_msg.data = num
     pubSR.publish(sr_msg)
 
     rospy.set_param(param_vision_search_param_lock, False)  # unlock param change
-    select_msg = String()
     while selected < 0:
         if rospy.has_param(param_control_target_is_set):
             if not rospy.get_param(param_control_target_is_set):
@@ -53,7 +66,6 @@ def handle_user_select(req):
             else:
                 selected = -1
 
-    rsp = user_selectResponse()
     rsp.selected_id = int(selected)
     # Reset the select parameter for next selection
     rospy.set_param(param_control_user_selected, -1)
@@ -65,6 +77,8 @@ def handle_user_select(req):
 def user_select_server():
     rospy.init_node('user_select_server')
     s = rospy.Service('drv_user', user_select, handle_user_select)
+    global fast_select
+    fast_select = rospy.get_param('fast_select', True)
     rospy.set_param(param_vision_search_param_lock, True)  # at beginning set param change locked
     print "Ready to receive user selection."
     rospy.spin()