def stop_recording_bags():
_logger.info('Stopping bag recording')
nodes_list = rosnode.get_node_names()
for node_name in nodes_list:
if node_name.find('record') != -1:
rosnode.kill_nodes([node_name])
time.sleep(2)
def restart_motor_node(self):
rospy.logwarn("Restarting motor node...")
rospy.sleep(0.5) # Aucune idee si c'est necessaire...
rosnode.kill_nodes(['animatics_smart_motor'])
rospy.sleep(2)
subprocess.Popen('roslaunch animatics_smart_motor SmartMotor.launch',
shell=True)
def save_all_data(self, data):
save_data(
self.all_coverage_data, '{}/coverage_{}_{}_{}_{}_{}.pickle'.format(
self.method, self.environment, self.robot_count, self.run,
self.termination_metric, self.max_target_info_ratio))
count = self.robot_count
if self.method == 'recurrent_connectivity':
count += 1
sleep(10)
self.check_kill_process(self.environment)
all_nodes = []
count = self.robot_count
if self.method == 'recurrent_connectivity':
count += 1
for i in range(count):
all_nodes += [
'/robot_{}/GetMap'.format(i), '/robot_{}/Mapper'.format(i),
'/robot_{}/map_align'.format(i),
'/robot_{}/navigator'.format(i),
'/robot_{}/operator'.format(i), '/robot_{}/SetGoal'.format(i)
]
if self.method != "gvgexploration":
all_nodes += [
'/robot_{}/explore_client'.format(i),
'/robot_{}/graph'.format(i)
]
all_nodes += ['/rosout', '/RVIZ', '/Stage', '/rostopic*']
rosnode.kill_nodes(all_nodes)
rospy.signal_shutdown("Exploration complete! Shutting down")
def _kill(self):
if self._process is not None:
# kill attached process directly
self._process._kill()
self._process = None
else:
# TODO: enhance roslaunch API to allow more direct kill
rosnode.kill_nodes([self._name])
def kill_rosnodes(self, data):
rosnode.kill_nodes(data.get('rosnodes'))
# Note: The launched rosnode-name does not appear the soon after roslaunch is executed.
# Therefore, sleep is neccessary to wait it finishes to launch.
time.sleep(2)
msg = {'uuid': self.id, 'rosnodes': rosnode.get_node_names()}
self.sio.emit('update_rosnodes', json.dumps(msg), namespace=self.nms)
print('killed')
def call_tsp():
print rospy.get_name()
t0 = time.time()
global clusters
global metacluster
global boat_location
while not rospy.is_shutdown():
if metacluster.position.x == 0 and metacluster.position.y == 0:
time.sleep(1)
else:
rospy.wait_for_service('/traveling_salesman_service')
try:
tsp_service = rospy.ServiceProxy('/traveling_salesman_service',
TSP)
path = tsp_service(boat_location, clusters)
runtime = []
runtime.append([time.time() - t0, 0])
delay = r_delay(0.05, random.random())
runtime.append([delay, 0])
runtime.append([-1, 0])
path_arr = []
for loc in path.path.poses:
path_arr.append([loc.position.x, loc.position.y])
with open(
"/DataStorage/asv_path" +
rospy.get_namespace().replace("/", '') + ".csv",
"wb") as f:
writer = csv.writer(f)
writer.writerows(path_arr)
with open(
"/DataStorage/asv_runtime" +
rospy.get_namespace().replace("/", '') + ".csv",
"wb") as f:
print runtime
writer = csv.writer(f)
writer.writerows(runtime)
time.sleep(delay)
'''server = smtplib.SMTP('smtp.gmail.com', 587)
server.starttls()
server.login("*****@*****.**", "doctoralrobot_ticsstudent")
msg = ''.join(str(e) for e in path_arr)
print msg
server.sendmail("*****@*****.**", "*****@*****.**", msg)
server.quit()
time.sleep(1)
server = smtplib.SMTP('smtp.gmail.com', 587)
server.starttls()
server.login("*****@*****.**", "doctoralrobot_ticsstudent")
msg = MIMEText("ASV Runtime: " + str(runtime))
server.sendmail("*****@*****.**", "*****@*****.**", msg.as_string())
server.quit()'''
rosnode.kill_nodes([rospy.get_name()])
break
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def cbShutdownNode(self,params): rospy.loginfo(params) nodes = rosnode.get_node_names() if params.data in nodes: rosnode.kill_nodes([params.data]) time.sleep(0.5) return SetStringResponse(u'关闭中') else: return SetStringResponse(u'节点未启动')
def stop_recording(self, req):
if not self.recording:
rospy.logerr('Not Recording')
return TriggerResponse(False, 'Not Recording')
rosnode.kill_nodes(['/eurobench_rosbag_recorder_node'])
self.recording_process = None
self.recording = False
rospy.loginfo('Stopped Recording')
return TriggerResponse(True, 'Stopped Recording')
def handle_map_req(self, req):
rospy.loginfo("Received request to switch to %s map" % req.new_map)
reply = SwitchMapResponse()
if req.new_map not in self.maps:
reply.success = False
rospy.logerr(
"This map was not specified in the config file. Aborting...")
return reply
if not self.wormholes.has_key(req.entry_wormhole):
reply.success = False
rospy.logerr(
"This wormhole was not specified in the config file. Aborting..."
)
return reply
rosnode.kill_nodes('map_server')
rospy.sleep(1)
map_name = req.new_map
wormhole = req.entry_wormhole
#TODO robustly handle directories /
args = join(self.map_dir, self.building_name, map_name + '/map.yaml')
node = roslaunch.core.Node(self.package,
self.executable,
name='map_server',
args=args)
self.map_server = self.launch.launch(node)
reply.success = True
reply.estimated_pose.pose.pose.position.x = self.wormholes[wormhole][
'connected_locations'][map_name]['position'][0]
reply.estimated_pose.pose.pose.position.y = self.wormholes[wormhole][
'connected_locations'][map_name]['position'][1]
reply.estimated_pose.pose.pose.position.z = 0.0
orientation = tf.transformations.quaternion_from_euler(
0.0, 0.0, self.wormholes[wormhole]['connected_locations'][map_name]
['orientation'])
reply.estimated_pose.pose.pose.orientation.x = orientation[0]
reply.estimated_pose.pose.pose.orientation.y = orientation[1]
reply.estimated_pose.pose.pose.orientation.z = orientation[2]
reply.estimated_pose.pose.pose.orientation.w = orientation[3]
reply.estimated_pose.pose.covariance = [0.0] * 36
return reply
def stop_recording(self, req):
if not self.recording:
rospy.logerr('Not Recording')
return TriggerResponse(False, 'Not Recording')
rosnode.kill_nodes(['/data_recording_myrecorder'])
self.process = None
self.recording = False
self.timeout_timer.shutdown()
rospy.loginfo('Stopped Recording')
return TriggerResponse(True, 'Stopped Recording')
def run(self):
"""
run specific case
specify the stdout and stderr
privides running conf
"""
if not os.path.exists(self.case_dir):
logging.error("%s not found in cases"% self.case_dir)
return
else:
logging.info(" %s Begin to run %s %s"% ("=" * 10, self.case_dir, "=" * 10))
# load environment and param
rosmap_cmd = "roslaunch aw_hdmap hdmap_runtime_env.launch"
rosevaluation_cmd = "roslaunch aw_evaluation start_evaluation.launch"
rossim_cmd = "roslaunch launch/run_case_simulation_by_env.launch"
casename = os.path.basename(self.case_dir)
rosrecord_cmd = "rosbag record /aw/planning_info -o %s/%s"%(self.record_dir, casename)
self.__loadparam_main()
env = os.environ
rosmap = AWProcess(rosmap_cmd, None, self.stdout, self.stderr, env)
rossim = AWProcess(rossim_cmd, None, self.stdout, self.stderr, env)
rosevaluation = AWProcess(rosevaluation_cmd, None, self.stdout, self.stderr, env)
rosrecord = AWProcess(rosrecord_cmd, None, self.stdout, self.stderr, env)
rosmap.run()
rospy.sleep(3)
## launch evaluation ahead in case something missed at beginning
rosevaluation.run()
rospy.sleep(3)
rossim.run()
if self.record:
rosrecord.run()
ctrl = RuntimeManager(autopause=False, outputpath=None, dtask=True,
autoexit=True, casename=self.case_dir, timeout=None,runid=None, enable_keyboard=False)
try:
ctrl.initialize(rossim)
ctrl.spin_til_exit()
except KeyboardInterrupt:
rospy.loginfo(ColorStrBuilder.notice("Keyboradinterrupt caught in main."))
else:
rospy.loginfo("Normal Exit.")
rospy.sleep(3)
finally:
print(ColorStrBuilder.notice("Cleaning up..."))
rossim.terminate()
rosmap.terminate()
rosevaluation.terminate()
if self.record:
rosrecord.terminate()
rosnode.kill_nodes(['/hdmap_node','/hdmap','/evaluation_node'])
def run_case(self, case_dir, vehicle, ctrl, stdout, stderr):
"""
run specific case
specify the stdout and stderr
privides running conf
"""
if not os.path.exists(case_dir):
logging.error("%s not found in cases" % case_dir)
return
else:
logging.info(" %s Begin to run %s %s" %
("=" * 10, case_dir, "=" * 10))
# load environment and param
rosmap_cmd = "roslaunch aw_hdmap hdmap_runtime_env.launch"
rosevaluation_cmd = "roslaunch aw_evaluation start_evaluation.launch"
rossim_cmd = "roslaunch launch/run_case_simulation_by_env.launch"
self.__loadparam_main(case_dir, vehicle)
env = os.environ
rosmap = AWProcess(rosmap_cmd, None, stdout, stderr, env)
rossim = AWProcess(rossim_cmd, None, stdout, stderr, env)
rosevaluation = AWProcess(rosevaluation_cmd, None, stdout, stderr, env)
if rospy.is_shutdown():
rospy.loginfo("shuting down already")
rosmap.run()
rospy.sleep(5)
## launch evaluation ahead in case something missed at beginning
rosevaluation.run()
rospy.sleep(5)
rossim.run()
try:
ctrl.initialize(rossim)
ctrl.spin_til_exit()
except KeyboardInterrupt:
rospy.loginfo(
ColorStrBuilder.notice("Keyboradinterrupt caught in main."))
else:
rospy.loginfo("Normal Exit.")
rospy.sleep(3)
finally:
print(ColorStrBuilder.notice("Cleaning up..."))
rosnode.kill_nodes(['/hdmap_node', '/hdmap'])
rossim.terminate()
rosmap.terminate()
rosevaluation.terminate()
def main():
rospy.Subscriber(stopExternal, stop, readEmergencyStop)
counter = 0
rospy.loginfo("[mast emStop init]")
while (not rospy.is_shutdown()) and (emStop == 0):
if not emStop == 0:
rospy.logerr("main emstop isn't zero")
writeEmergencyStop()
updateRate.sleep()
counter += 1
rospy.logwarn("[Shut down in 5 sec]")
writeEmergencyStop()
sleep(5)
nodes = rosnode.get_node_names()
rosnode.kill_nodes(nodes)
def shutdown_ros(self):
self.sub.unregister()
neo = neopixel()
neo.execute()
print("---start killing nodes---")
nodes_list = rosnode.get_node_names()
nodes_list.remove('/' + self.node_name)
rosnode.kill_nodes(nodes_list)
print("---other nodes killed---")
self.bringup.shutdown()
print("---core shutdown---")
rosnode.kill_nodes(['/' + self.node_name])
print("---self node killed---")
self.shutdown_finished = True
def _restart_robin_node(cls, node_path): #TODO try to simplify
"""Restarts robin bridge node."""
# if node alive
if rosnode.rosnode_ping(node_path, max_count=3):
# kill node
if node_path not in rosnode.kill_nodes([node_path])[0]:
raise RuntimeError(
"Failed to kill robin node '{}'.".format(node_path))
else:
master = rosgraph.Master(rosnode.ID)
rosnode.cleanup_master_blacklist(master, [node_path])
node_name = node_path.split('/')[-1]
cls._wait_for(lambda: cls._get_node_path(node_name) == '')
namespace = '/'.join(node_path.split('/')[:-1])
cmd = '''bash -c "
cd {} &&
. *devel*/setup.bash &&
rosrun robin_bridge_generated robin_node __ns:={} &
" > /dev/null 2>&1'''.format(catkin_ws, namespace)
# " > /dev/null 2>&1'''.format(catkin_ws, node_path[:-len('/' + node_name)])
if os.system(cmd) != 0:
raise RuntimeError('Failed to rerun robin node.')
cls._wait_for(lambda: cls._get_node_path(node_name) != '', timeout=10)
def relaunch(self, req):
killed,failed = rosnode.kill_nodes(['move_base', 'slam_gmapping'])
result = 'move_base' in killed and 'slam_gmapping' in killed
if not result:
rospy.logwarn('NavLauncher failed to kill %s' % failed)
process = subprocess.Popen(['roslaunch', 'vln_agent', 'nav_stack.launch'])
return TriggerResponse(success=result)
def exit_handler(self):
'''
This function is called on exit.
- Cleans up all child processes
'''
child_pids = [str(p.pid) for p in self.process_list]
# Kill all child nodes
print("Nodes to kill: " + str(self.anon_node_list))
rosnode.kill_nodes(self.anon_node_list)
# Join all child processes
print("Children to be destroyed: %s" % (child_pids))
for p in self.process_list:
print("Trying to destroy %s" % (str(p.pid)))
p.join()
print("cleaned up process %s" % (str(p.pid)))
def kill_node(self, node, specific_node=None):
if node not in ['amcl', 'mrpt', 'aruco']:
return False, "Illegal node passed in for killing."
nodes = self.NODE_MAP[node]
if nodes is None or len(nodes) == 0:
return False, "Nothing to kill"
if specific_node is None:
rosnode.kill_nodes(nodes)
return True, None
elif specific_node in nodes:
rosnode.kill_nodes(specific_node)
return True, None
else:
return False, "Could not kill %s" % specific_node
def test_deadlock(self):
# Prepare condition (for safe preemption)
self.condition = threading.Condition()
self.last_execution_time = None
# Prepare Simple Action Server
self.action_server = actionlib.SimpleActionServer(
Constants.topic,
TestAction,
execute_cb=self.execute_callback,
auto_start=False)
self.action_server.register_preempt_callback(self.preempt_callback)
self.action_server.start()
# Sleep for the amount specified
rospy.sleep(Constants.deadlock_timeout)
# Start actual tests
running_nodes = set(rosnode.get_node_names())
required_nodes = {
"/deadlock_companion_1", "/deadlock_companion_2",
"/deadlock_companion_3", "/deadlock_companion_4",
"/deadlock_companion_5"
}
self.assertTrue(required_nodes.issubset(running_nodes),
"Required companion nodes are not currently running")
# Shutdown companions so that we can exit nicely
termination_time = rospy.Time.now()
rosnode.kill_nodes(required_nodes)
rospy.sleep(Constants.shutdown_timeout)
# Check last execution wasn't too long ago...
self.assertIsNotNone(self.last_execution_time is None,
"Execute Callback was never executed")
time_since_last_execution = (termination_time -
self.last_execution_time).to_sec()
self.assertTrue(
time_since_last_execution < 2 * Constants.max_action_duration,
"Too long since last goal was executed; likely due to a deadlock")
def stop(self):
if not self.process:
rospy.logerr('No rosbag running...')
return
self.process = None
nodes = []
for node_name in rosnode.get_node_names():
if self.node_name in node_name:
nodes.append(node_name)
success_list, _ = rosnode.kill_nodes(nodes)
return len(success_list) > 0
def test_published(self):
"""Test topics are published and messages come"""
use_sim_time = rospy.get_param('/use_sim_time', False)
t_start = time.time()
while not rospy.is_shutdown() and \
use_sim_time and (rospy.Time.now() == rospy.Time(0)):
rospy.logwarn('/use_sim_time is specified and rostime is 0, '
'/clock is published?')
if time.time() - t_start > 10:
self.fail('Timed out (10s) of /clock publication.')
time.sleep(1)
self._check_topic_pubilshed()
if self.check_after_kill_node:
rospy.logwarn(
'Check topic published after killing nodes ({})'.format(
self.target_node_names))
rosnode.kill_nodes(self.target_node_names)
time.sleep(5.0)
self._check_topic_pubilshed()
def execute(self):
rospy.loginfo('Executing node kill item')
msg = String()
msg.data = "LIT; EXEC:%s" % (self.name)
self.parent.display_publisher.publish(msg)
if (self.pre_delay > 0):
rospy.loginfo("Waiting for %r seconds prior to execution." %
(self.pre_delay))
rospy.sleep(self.pre_delay)
self.activate()
rosnode.kill_nodes(self.node_names)
if (self.post_delay > 0):
rospy.loginfo("Waiting for %r seconds after execution." %
(self.post_delay))
rospy.sleep(self.post_delay)
self.deactivate()
def drone_localize_in_map(self):
# Get to Right Height
self.height_up = self.localization.localization_height
while not self.fly_to('Go_UP'):
pass
# Search for Tag
self.localization.search_for_tag_flag()
# Wait Till the Two Trees are Joint in One
while not self.localization.localized:
pass
# Kill the Ar Pose Thread to save CPU Usage!
kill_nodes(['/ar_pose'])
# Get to Exploration Height
self.height_up = self.localization.exploration_height
while not self.fly_to('Go_UP'):
pass
print('\x1b[33;1m' + 'Drone Localized in Map' + '\x1b[0m')
return
def kill_node(taype_name="/map_server"):
success, fail = rosnode.kill_nodes([taype_name])
rospy.loginfo("success = " + str(success) + "fail = " + str(fail))
if fail != []:
rospy.logwarn("node " + taype_name + " is still alive")
return False
elif success != []:
rospy.loginfo("node " + taype_name + " died")
return True
else:
rospy.logwarn("something went wrong, with killing the node: " +
taype_name)
return False
def image_cb( img ):
global pt1, pt2
cv2.namedWindow("img")
cv2.setMouseCallback("img", mouse_event)
img = np.frombuffer(img.data, dtype=np.uint8).reshape(img.height, img.width, -1)
img_display = cv2.cvtColor( img, cv2.COLOR_RGB2BGR )
if pt1!=None and pt2!=None:
cv2.rectangle( img_display, pt1, pt2, (0,0,255), 1 )
cv2.imshow("img", img_display)
c = cv2.waitKey(10)
if c==115: # s
if pt1!=None and pt2!=None:
save_img( cv2.cvtColor( img, cv2.COLOR_RGB2BGR ), pt1, pt2 )
else:
print("画像をドラッグして保存範囲を選択してください")
elif c==113: # q
cv2.destroyAllWindows()
rosnode.kill_nodes([rospy.get_name() ])
def stop_node(self, node_id):
"""
Stops the node with the given id.
Returns a message about operation's success.
:param node_id: id of the node to be stopped.
:type node_id: String.
:returns: String
"""
success, fail = rosnode.kill_nodes([node_id])
if node_id in success:
return 'Node %s successfully stopped' % node_id
else:
return 'Node %s could not be stopped' % node_id
def stop_node(self, node_id):
"""
Stops the node with the given id.
Returns a message about operation's success.
:param node_id: id of the node to be stopped.
:type node_id: String.
:returns: String
"""
success, fail = rosnode.kill_nodes([node_id])
if node_id in success:
return 'Node %s successfully stopped' % node_id
else:
return 'Node %s could not be stopped' % node_id
def kill_all_ns_nodes(self):
names = rosnode.get_node_names(self.robot_ns)
if len(names) == 0:
# Something probably went wrong - warn the user.
rospy.logwarn("Couldn't find any nodes under %s, while shutting down nodes." % self.robot_ns)
return
# Notify that you're about to shutdown each node,
# to indicate that the next user initiated shutdown of a node with that name came from here.
# Following this, a [ WARN ] message should appear, saying:
# [ WARN] [secs, nsecs]: Shutdown request received.
# [ WARN] [secs, nsecs]: Reason given for shutdown: [user request]
for node_name in names:
rospy.loginfo("Sending shutdown command to node '%s'" % node_name)
success, fail = rosnode.kill_nodes([node_name])
if len(fail) > 0:
rospy.logwarn("Failed to shutdown node '%s'" % node_name)
def kill_nodes(self, *nodes):
rosnode.kill_nodes(nodes)
def kill(self):
rospy.loginfo("Killing /%s" % self.name)
rosnode.kill_nodes([self.name])
def restart_motor_node(self):
rospy.logwarn("Restarting motor node...")
rospy.sleep(0.5) # Aucune idee si c'est necessaire...
rosnode.kill_nodes(['animatics_smart_motor'])
rospy.sleep(2)
subprocess.Popen('roslaunch animatics_smart_motor SmartMotor.launch', shell=True)
def kill_node(self, node_name):
success, fail = rosnode.kill_nodes([node_name])
return node_name in success
def restart_camera(self):
# Just kill it here, will be restarted by
# roslaunch respawn
rosnode.kill_nodes([self.camera_node_name])
self.try_to_set_recordingmedia()
self.start_restart_timer()
def keyPressEvent(self, event):
QGraphicsItem.keyPressEvent(self, event)
if event.key() == Qt.Key_Delete:
res = rosnode.kill_nodes([self.name])
def kill(self):
self._setenv()
rosnode.kill_nodes(['gazebo'])
time.sleep(1)
self.proc.terminate()
def kill(self):
self._setenv()
rosnode.kill_nodes(['gazebo'])
time.sleep(1)
self.proc.terminate()
def kill(self):
rospy.loginfo("Killing /%s" % self.name)
rosnode.kill_nodes([self.name])
def kill_nodes(self, node_name): self.nodes_to_kill.append(node_name) rosnode.kill_nodes(self.nodes_to_kill) self.print_kill_list(self.nodes_to_kill) self.nodes_to_kill = []
def __terminate_node(self):
if self.listeners_initialised:
rosnode.kill_nodes(self.node_handle_name)
self.node_thread.terminate()
def shutdown_manops(exit_str):
"""Kill the key_publisher node
"""
rospy.logdebug('Attempting to kill keyboard_driver')
rosnode.kill_nodes(['key_publisher'])
rospy.sleep(0.1)
#!/usr/bin/env python
import rospy, rosnode
import hsrb_interface
from tmc_navigation_msgs.msg import OccupancyGridUint
import numpy as np
if __name__ == '__main__':
rospy.init_node('bullseye')
robot = hsrb_interface.Robot()
base = robot.try_get("omni_base")
rosnode.kill_nodes(['/obstacle_grid_mapper'])
rate = rospy.Rate(1)
n = 1
msg = OccupancyGridUint()
msg.data = tuple(np.zeros((140 * 140), dtype=np.uint8))
msg.header.frame_id = "/map"
msg.header.stamp = rospy.Time.now()
msg.info.map_load_time = rospy.Time(0)
msg.info.resolution = 0.05
msg.info.width = 140
msg.info.height = 140
msg.info.origin.orientation.x = 0
msg.info.origin.orientation.y = 0
msg.info.origin.orientation.z = 0
msg.info.origin.orientation.w = 1
while not rospy.is_shutdown():
msg.header.seq = n
pose = base.pose
def test_deadlock(self):
# Prepare condition (for safe preemption)
self.condition = threading.Condition()
self.last_execution_time = None
# Prepare Simple Action Server
self.action_server = actionlib.SimpleActionServer(
Constants.topic,
TestAction,
execute_cb=self.execute_callback,
auto_start=False)
self.action_server.register_preempt_callback(self.preempt_callback)
self.action_server.start()
# Sleep for the amount specified
rospy.sleep(Constants.deadlock_timeout)
# Start actual tests
running_nodes = set(rosnode.get_node_names())
required_nodes = {
"/deadlock_companion_1",
"/deadlock_companion_2",
"/deadlock_companion_3",
"/deadlock_companion_4",
"/deadlock_companion_5"}
self.assertTrue(required_nodes.issubset(running_nodes),
"Required companion nodes are not currently running")
# Shutdown companions so that we can exit nicely
termination_time = rospy.Time.now()
rosnode.kill_nodes(required_nodes)
rospy.sleep(Constants.shutdown_timeout)
# Check last execution wasn't too long ago...
self.assertIsNotNone(self.last_execution_time is None,
"Execute Callback was never executed")
time_since_last_execution = (
termination_time - self.last_execution_time).to_sec()
self.assertTrue(
time_since_last_execution < 2 * Constants.max_action_duration,
"Too long since last goal was executed; likely due to a deadlock")
