control.py
Go to the documentation of this file.
1 #!/usr/bin/python2
2 
3 #Copyright (c) 2016, Allgeyer Tobias, Aumann Florian, Borella Jocelyn, Karrenbauer Oliver, Marek Felix, Meissner Pascal, Stroh Daniel, Trautmann Jeremias
4 #All rights reserved.
5 #
6 #Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
7 #
8 #1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
9 #
10 #2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other #materials provided with the distribution.
11 #
12 #3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific #prior written permission.
13 #
14 #THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED #WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, #INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR #PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) #ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
15 
16 import os
17 from time import sleep
18 import rospy
19 import tf
20 import re
21 from tmux import *
22 from string_util import *
23 import subprocess
24 import rlcompleter
25 import readline
26 import time
27 import imp
28 import time
29 import psutil
30 import shutil
31 from nbv import *
32 from general_ros import *
33 from configuration import *
34 from move import *
35 from personal_stuff import *
36 from asr_world_model.srv import GetMissingObjectList
37 from asr_msgs.msg import AsrObject
38 
39 # tab completion
40 readline.parse_and_bind("tab: complete")
41 
42 # ros node
44 rospy.init_node("general_configuration", anonymous = True)
45 
46 # import stuff from state machine
47 ObjectDetectorsManager = imp.load_source("ObjectDetectorsManager", scene_exploration_path + "/src/common/object_detectors_manager.py").ObjectDetectorsManager()
48 
49 # obj detection
50 global detectedObjects
51 detectedObjects = {}
53  global detectedObjects
54  if data.type not in detectedObjects:
55  detectedObjects[data.type] = 1
56  else:
57  detectedObjects[data.type] += 1
58 
59 def startRecognition(recognizers = [], waitTime = 2):
60  global detectedObjects
61  detectedObjects = {}
62  if type(recognizers) is not list:
63  recognizers = [recognizers]
64  if len(recognizers) == 0:
65  rospy.wait_for_service("/env/asr_world_model/get_missing_object_list", timeout=5)
66  get_missing_object_list = rospy.ServiceProxy("/env/asr_world_model/get_missing_object_list", GetMissingObjectList)
67  missing_pbd_typeAndId = get_missing_object_list().missingObjects
68  print(missing_pbd_typeAndId)
69  for obj in missing_pbd_typeAndId:
70  recognizers.append(str(obj.type))
71  ObjectDetectorsManager.start_recognizers(recognizers)
72  future = time.time() + waitTime
73  sub = rospy.Subscriber("/stereo/objects", AsrObject, detection_callback)
74  while (time.time() < future):
75  pass
76  # unregister, otherwise the callbacks are still called
77  sub.unregister()
78  ObjectDetectorsManager.stop_recognizers(recognizers)
79  return detectedObjects
80 
81 # tmux
82 def tmuxStart():
83  subprocess.call("tmux new-session -d", shell=True)
84  subprocess.call("tmux send-keys \"roscd asr_resources_for_active_scene_recognition/launch/\" C-m", shell=True)
85  subprocess.call("tmux send-keys \"./start_modules_sim.sh\" C-m", shell=True)
86 
88  tmuxKill()
89  tmuxStart()
90 
92  # since tmuxkill does not always terminate everything
93  processNames = ["gzserver", "roslaunch", "asr_world_model", "rosout", "visualization", "rp_ism_node", "roscore", "move_base", "asr_mild_base_fake_driving", "state_publisher", "grid_action_server", "asr_object_database", "asr_fake_object_recognition", "asr_flir_ptu_driver", "PTUController", "map_server", "asr_next_best_view", "next_best_view_robot_model", "rosbag", "record", "rviz", "static_transform_publisher", "rosmaster", "gdb"]
94  for proc in psutil.process_iter():
95  if proc.name in processNames or ("python" in proc.name and proc.pid != os.getpid()):
96  print("terminating " + proc.name)
97  proc.kill()
98 
99 # sm
100 def restartSM():
101  restartPanes(smPane)
102 
103 def stopSM():
104  terminatePanes(smPane)
105 
107  result = getSMResult()
108  return result != False
109  # return "State machine terminating" in getLastLinesFrom(getTmuxOut("state_machine", 0), -2)
110 
112  while not isSMFinished():
113  time.sleep(1)
114 
115 def getLogSMResults(logdir):
116  if isLogFolder(logdir):
117  content = getFile(logdir + "/state_machine.log")
118  return str(getSMResultFromFileContent(content))
119 
120 def startSM():
121  selectWindow("state_machine")
122  while "indirect_search" not in getLastLinesFrom(getTmuxOut("state_machine", 0), -2):
123  time.sleep(1)
124  curPane.send_keys("", enter=True)
125 
127  result = re.findall(".*State machine terminating '.*(?:SEARCH|SCENE_RECOGNITION).*':'(.*)':'.*", smLog)
128  return result[0] if type(result) == list and len(result) > 0 else False
129 
131  result = re.findall(".*State machine terminating '.*(?:SEARCH|SCENE_RECOGNITION).*':'(.*)':'.*", getFile(logDir + "/state_machine.log"))
132  return result[0] if type(result) == list and len(result) > 0 else False
133 
134 def startAll(basePos, ptuPos):
135  tmuxRestart()
136  if not waitForAllModules():
137  tmuxKill()
138  return False
139  setBase(*basePos)
140  setPTU(*ptuPos)
141  startSM()
142  return True
143 
145  startAll(positions[0][0], orientations[0])
146 
147 def start():
148  waitForAllModules(True)
149  setDefaultPos()
150  startSM()
151 
152 def restart():
153  restartPanes(allPanes)
155  setDefaultPos()
def getLogSMResults(logdir)
Definition: control.py:115
def restartPanes(windowAndPanes)
Definition: tmux.py:130
def startSM()
Definition: control.py:120
def terminateAllModules()
Definition: control.py:91
def tmuxRestart()
Definition: control.py:87
def getSMResult()
Definition: control.py:130
def tmuxKill()
Definition: tmux.py:156
def startRecognition(recognizers=[], waitTime=2)
Definition: control.py:59
def getSMResultFromFileContent(smLog)
Definition: control.py:126
def detection_callback(data)
Definition: control.py:52
def getTmuxOut(windowNameOrId, paneId)
Definition: tmux.py:32
def startAllDefault()
Definition: control.py:144
def stopSM()
Definition: control.py:103
def terminatePanes(windowAndPanes)
Definition: tmux.py:141
def setBase(x, y, rotation)
Definition: move.py:84
def tmuxStart()
Definition: control.py:82
def isLogFolder(folder)
def getFile(fileLoc)
def restart()
Definition: control.py:152
ObjectDetectorsManager
Definition: control.py:47
def waitForAllModules(silence=False)
Definition: general_ros.py:11
def selectWindow(windowNameOrId)
Definition: tmux.py:52
def setPTU(pan, tilt)
Definition: move.py:50
def start()
Definition: control.py:147
def restartSM()
Definition: control.py:100
def waitSMFinished()
Definition: control.py:111
def getLastLinesFrom(str, pos)
Definition: string_util.py:18
def isSMFinished()
Definition: control.py:106
def startAll(basePos, ptuPos)
Definition: control.py:134


asr_resources_for_active_scene_recognition
Author(s): Allgeyer Tobias, Aumann Florian, Borella Jocelyn, Karrenbauer Oliver, Marek Felix, Mei├čner Pascal, Stroh Daniel, Trautmann Jeremias
autogenerated on Tue Feb 18 2020 03:14:15