def setUp(cls): if not rosgraph.is_master_online(): print("Run ROS core") #cls.kill_core_on_exit = False #_run_mqtt_node() raise Exception("ROS core not available") rospy.init_node('TestLisaRosInteraction')
def check_ros(url, verbosity): if rosgraph.is_master_online(master_uri=url): return True else: if verbosity > 0: print_flush('[clear_process.py] ROS MASTER is OFFLINE') return False
def work(vo_topic="/vins_estimator/imu_propagate"): global vo_avail, vo_position, last_vo_time, battery_votage rospy.init_node("drone_status") s_bat = rospy.Subscriber("/dji_sdk_1/dji_sdk/battery_state", BatteryState, on_battery_status) s_vo = rospy.Subscriber(vo_topic, Odometry, on_vo_msg) #Wait for dji_sdk rospy.loginfo("Wait for dji sdk.....") # rospy.wait_for_service("/dji_sdk_1/dji_sdk/set_hardsyc") rospy.loginfo("DJI SDK started") r = rospy.Rate(10) while not rospy.is_shutdown() and rosgraph.is_master_online(): prefix = "[{:7.3f}s]".format(rospy.get_time() % 1000) if vo_avail: vo_color = "green" else: vo_color = "red" vo_str = " VO {} :[{:5.3f}, {:5.3f}, {:5.3f}]".format( vo_avail, vo_position.x, vo_position.y, vo_position.z) prefix = prefix + colored(vo_str, vo_color) + " BAT:" bat_l = battery_to_percent(battery_votage) * 100 suffix = ":{:4.2f}V".format(battery_votage) printBatteryLevel(bat_l, 100, prefix, suffix, length=10) r.sleep() if rospy.get_time() - last_vo_time > 0.2: vo_avail = False print()
def getRunningNodes(self): try: if rosgraph.is_master_online(): nodes = rosnode.get_node_names() else: nodes = [] except: nodes = [] self.nodeListCtrl.delete(0, END) for n in nodes: if not "/ros" in n: index = self.nodeListCtrl.size() self.nodeListCtrl.insert(index, n) if n in self.requiredNodes: self.nodeListCtrl.itemconfigure(index, background="green") else: self.nodeListCtrl.itemconfigure(index, background="") # check required nodes and add if not running for rn in self.requiredNodes: if rn not in nodes: index = self.nodeListCtrl.size() self.nodeListCtrl.insert(index, rn) self.nodeListCtrl.itemconfigure(index, background="red")
def _init_frameworks(self): """Initialize the frameworks.""" if self.framework == "ros": import rosgraph if not rosgraph.is_master_online(): # Run roscore in a different process subprocess.Popen("roscore") time.sleep(2) elif self.framework == "ray": self._init_ray()
def start(cls): print("[BRIDGE]: starting") cont = 0 while cont < 10 and (not rosgraph.is_master_online()): time.sleep(1) cont += 1 if rosgraph.is_master_online(): rospy.init_node("rosbridge_websocket", disable_signals=True) cls.client_manager = ClientManager() #if cls.first : # rospy.init_node("rosbridge_websocket", disable_signals=True) # cls.client_manager = ClientManager() # cls.first = False print("[BRIDGE]: running") cls.master = True else: print("[BRIDGE]: master not running") cls.master = False
def __init__(self, using_ros, user_pose, robot_pose, orientation_type, user_id, target=None, targets_prob=None, w_fov=124, h_fov=60, clip_planes=None): if targets_prob is None: self._targets_prob = {'A': 0.5, 'B': 0.5} else: self._targets_prob = targets_prob if clip_planes is None: clip_planes = [1, 1000] self._target = target self._user_angle = 0.0 self._user_pose = user_pose self._rotation_type = orientation_type self._using_ros = using_ros self._user_id = user_id w_scale_factor = 1.0/(math.tan((w_fov/2.0) * (math.pi/180))) h_scale_factor = 1.0 / (math.tan((h_fov / 2.0) * (math.pi / 180))) far_plane = clip_planes[1] near_plane = clip_planes[0] self._w_fov = w_fov self._h_fov = h_fov self._clip_planes = clip_planes self._perspective_matrix = np.array([[w_scale_factor, 0, 0, 0], [0, h_scale_factor, 0, 0], [0, 0, -float(far_plane)/float(far_plane - near_plane), -1], [0, 0, -float(far_plane*near_plane)/float(far_plane - near_plane), 0]]) self._robot_pose = robot_pose if rosgraph.is_master_online(): self._ros_active = True if self._using_ros: self._tf_listener = tf.TransformListener() else: self._tf_listener = None else: self._ros_active = False self._movement_target = Point() self._trajectory = None self._transformed_trajectory = None self._trajectory_length = -1 self._trajectory_dim = 0 self._transformed_trajectory_length = -1 self._transformed_trajectory_dim = 0 self._traj_K = None self._traj_e = None
def ping(update, context): ''' keyboard = start_keyboard() reply_markup = ReplyKeyboardMarkup(keyboard) message = ( "🇨🇴 Hola! Yo soy CastorBot! \n" "Si quieres conocer mis funcionalides envía \n /help: \n\n" "También puedes utilizar mi teclado especial ⌨️!\n\n" "Intenta ahora!" ) ''' message = "Pong! 🏓 " if rosgraph.is_master_online(): message += "\nROS is running! ✅" else: message += "\nROS is not running ❌" update.message.reply_text(message, parse_mode='markdown') return
def RosVideoStart(self): if rosgraph.is_master_online( ): # Checks the master uri and results boolean (True or False) pass else: self.SendErrorMessage("No master node running") return #Subscribers rospy.init_node('ros_gui_node') for i in range(self.totalVideoChecks): exec("self.videoDetectChecks[i] = self.subscriber_box_%s" % str(i)) self.videoDetectPub[i] = rospy.Publisher(self.videoDetections[i], Bool, queue_size=1) self.videoDetectChecks[i].stateChanged.connect(self.DetectEnable) for i in range(self.totalSubscribers): self.topicSubscribers[i] = rospy.Subscriber( self.topicSubscribersText[i], String, self.SubscriberCallback, queue_size=5) self.audioThresholdSub = rospy.Subscriber("/audio/threshold_up", Bool, self.AudioThresholdCallback, queue_size=20) self.audioThresholdPub = rospy.Publisher("/audio/threshold_val", Int16, queue_size=1) for i in range(self.totalAudioChecks): self.audioPublishers[i] = rospy.Publisher(self.audioCheckTopics[i], Bool, queue_size=1) for indx in range(self.totalVideoStream): print(indx) #self.videoSubscribers[indx] = rospy.Subscriber((self.videoChannels[self.videoMenu[indx].currentIndex()]), Image, self.videoCallback, queue_size = 1) self.videoSubscribers[indx] = rospy.Subscriber( (self.videoChannels[indx]), Image, self.videoCallback, queue_size=1) return
def __init__(self, using_ros, user_pose, robot_pose, orientation_type, user_id, target=None, targets_prob=None, perspective_matrix=np.eye(3)): if targets_prob is None: self._targets_prob = {'A': 0.5, 'B': 0.5} else: self._targets_prob = targets_prob self._target = target self._user_angle = 0.0 self._user_pose = user_pose self._rotation_type = orientation_type self._using_ros = using_ros self._user_id = user_id self._perspective_matrix = perspective_matrix self._robot_pose = robot_pose if rosgraph.is_master_online(): self._ros_active = True if self._using_ros: self._tf_listener = tf.TransformListener() else: self._tf_listener = None else: self._ros_active = False self._movement_target = Point() self._trajectory = None self._transformed_trajectory = None self._trajectory_length = -1 self._trajectory_dim = 0 self._traj_K = None self._traj_e = None
def initViewer(self, viewer=None, windowName="python-pinocchio", loadModel=False, initRosNode=True): """Init RVizViewer by starting a ros node (or not) and creating an RViz window.""" from rospy import init_node, WARN from rosgraph import is_master_online from rviz import bindings as rviz from python_qt_binding.QtWidgets import QApplication if not is_master_online(): # Checks the master uri # ROS Master is offline warnings.warn( "Error while importing the viz client.\n" "Check whether ROS master (roscore) is properly started", category=UserWarning, stacklevel=2) return None if initRosNode: init_node('pinocchio_viewer', anonymous=True, log_level=WARN) if viewer == None: self.viewer = RVizVisualizer.Viewer() self.viewer.app = QApplication([]) self.viewer.viz = rviz.VisualizationFrame() self.viewer.viz.setSplashPath("") self.viewer.viz.initialize() self.viewer.viz.setWindowTitle(windowName + "[*]") self.viewer.viz_manager = self.viewer.viz.getManager() self.viewer.viz.show() else: self.viewer = viewer if loadModel: self.loadViewerModel() return self.viewer
def processFrames(self): print("Starting stream") while not rospy.is_shutdown() and rosgraph.is_master_online(): #print("Waiting for image\n") try: img = rospy.wait_for_message("/usb_cam/image_raw", Image, timeout=1) image_color = self.bridge.imgmsg_to_cv2(img, 'bgr8') except rospy.ROSException as e: print("Could not get image!!!") break # Get faces and encode face_locations = face_recognition.face_locations(image_color) face_encodings = face_recognition.face_encodings( image_color, face_locations) labels = [] for face_encoding in face_encodings: matches = face_recognition.compare_faces( self.Faces, face_encoding, self.tolerance) name = "Unknown" if True in matches: first_match_index = matches.index(True) name = self.names[first_match_index] face_distances = face_recognition.face_distance( self.Faces, face_encoding) face_distance = face_distances[first_match_index] face_distance = 1 - round(face_distance, 2) # Why? labels.append(name) self.showDetections(image_color, labels, face_locations) cv2.waitKey(1)
def __init__(self, using_ros, user_pose, rotation_type, user_id, perspective_matrix=np.eye(3), robot_pose=Pose()): self._user_angle = 0.0 self._user_pose = user_pose self._rotation_type = rotation_type self._using_ros = using_ros self._user_id = user_id self._perspective_matrix = perspective_matrix self._robot_pose = robot_pose if rosgraph.is_master_online(): self._ros_active = True if self._using_ros: self._tf_listener = tf.TransformListener() else: self._tf_listener = None else: self._ros_active = False
def main(): rospy.init_node('face_recognition_lib_node') rate = rospy.Rate(30) rospack = rospkg.RosPack() global train_dir #train_dir = rospack.get_path('face_recog') + "/faces" train_dir = rospy.get_param("~train_dir") global labels global process_this_frame global distances global Faces global names global bridge global face_locations global face_encodings global verbose global tolerance global clear_face_id global clear_bd global face_clear_id clear_face_id = False clear_bd = False face_clear_id = "" face_recog_available = True try: if not os.path.exists(train_dir): os.makedirs(train_dir) except OSError: print("Creation of the directory %s failed" % train_dir) else: print("Successfully created the directory %s " % train_dir) Faces = [] names = [] process_this_frame = True verbose = True tolerance = 0.55 load_Images() bridge = CvBridge() s = rospy.Service('face_recognizer/faces', FaceRecognition, face_recognition_callback) st = rospy.Service('face_recognizer/train_face', FaceRecognition, train_faces_callback) rospy.Subscriber("face_recognizer/clear_faces", Empty, clear_faces_callback) rospy.Subscriber("face_recognizer/clear_face_id", String, clear_face_id_callback) #rospy.Subscriber("face_recog/train_online", String, callbackTrain) while not rospy.is_shutdown() and rosgraph.is_master_online(): print("Waiting for image\n") try: img = rospy.wait_for_message("/usb_cam/image_raw", Image, timeout=1) image_color = CvBridge().imgmsg_to_cv2(img, 'bgr8') if process_this_frame: face_locations = face_recognition.face_locations(image_color) face_encodings = face_recognition.face_encodings( image_color, face_locations) labels = [] distances = [] for face_encoding in face_encodings: matches = face_recognition.compare_faces( Faces, face_encoding, tolerance) name = "Unknown" if True in matches: first_match_index = matches.index(True) name = names[first_match_index] face_distances = face_recognition.face_distance( Faces, face_encoding) face_distance = face_distances[first_match_index] face_distance = 1 - round(face_distance, 2) labels.append(name) process_this_frame = not process_this_frame for (top, right, bottom, left), name in zip(face_locations, labels): #Draw a box around the face cv2.rectangle(image_color, (left, top), (right, bottom), (0, 0, 255), 2) #Draw a label with a name below the face cv2.rectangle(image_color, (left, bottom - 35), (right, bottom), (0, 0, 255), cv2.FILLED) font = cv2.FONT_HERSHEY_DUPLEX cv2.putText(image_color, name, (left + 6, bottom - 6), font, 1.0, (255, 255, 255), 1) cv2.putText(image_color, str(face_distance), (right - 50, bottom - 6), font, 1.0, (255, 255, 255), 1) # Display the resulting image cv2.imshow('Face Recognition', image_color) if cv2.waitKey(1) & 0xFF == ord('q'): break except rospy.ROSException as e: pass if clear_bd or clear_face_id: print('Trying to clean data base') if os.path.exists(train_dir): if clear_bd: shutil.rmtree(train_dir, ignore_errors=True) Faces = [] names = [] if clear_face_id: person_dir = train_dir + "/" + face_clear_id if os.path.exists(person_dir): shutil.rmtree(person_dir, ignore_errors=True) index = 0 while index < len(names): if names[index] == face_clear_id: names.pop(index) Faces.pop(index) else: index = index + 1 clear_bd = False clear_face_id = False rate.sleep()
def main(args): global bridge global facenetGraph global face_recognition global classifier_file global training_dir global threshold_reco global image_size global training_dir global batch_size global face_recog_available global classifier_mode global clear_face_id global clear_bd global face_clear_id global s global sa global st global rate global sub_clear_faces global sub_clear_face_id clear_face_id = False clear_bd = False face_clear_id = "" face_recog_available = True initRos = False print args model_file = args.model_file classifier_file = args.classifier_file image_size = args.image_size margin = args.margin gpu_memory_fraction = args.gpu_memory_fraction detect_multiple_faces = args.detect_multiple_faces threshold_reco = args.threshold_reco training_dir = args.training_dir classifier_mode = args.classifier_mode batch_size = args.batch_size bridge = CvBridge() facenetGraph = tf.Graph() with facenetGraph.as_default(): if classifier_mode: face_recognition = faceC.Recognition( facenet_model=model_file, classifier_model=classifier_file, face_crop_size=image_size, threshold=[0.7, 0.8, 0.8], factor=0.709, face_crop_margin=margin, gpu_memory_fraction=gpu_memory_fraction, detect_multiple_faces=detect_multiple_faces) else: face_recognition = face.Recognition( facenet_model=model_file, data_dir=training_dir, face_crop_size=image_size, threshold=[0.7, 0.8, 0.8], factor=0.709, face_crop_margin=margin, gpu_memory_fraction=gpu_memory_fraction, detect_multiple_faces=detect_multiple_faces) while 1: if rospy.is_shutdown(): break if rosgraph.is_master_online(): if not initRos: print 'Creating the ros node and service' rospy.init_node('facenet_node', anonymous=True) rospack = RosPack() rate = rospy.Rate(30) #30Hz s = rospy.Service('/vision/face_recognizer/faces', FaceRecognition, face_recognition_callback) sa = rospy.Service('/vision/face_recognizer/train_face', FaceRecognition, add_face_callback) if classifier_mode: st = rospy.Service('/vision/face_recognizer/train_flush', EmptySrv, train_faces_callback) sub_clear_faces = rospy.Subscriber( "/vision/face_recognizer/clear_faces", Empty, clear_faces_callback) sub_clear_face_id = rospy.Subscriber( "/vision/face_recognizer/clear_face_id", String, clear_face_id_callback) try: if not os.path.exists(training_dir): os.makedirs(training_dir) except OSError: print("Creation of the directory %s failed" % training_dir) else: print("Successfully created the directory %s " % training_dir) if classifier_mode: try: if not os.path.exists(training_dir + "/Unknown/defualt.png"): package_path = rospack.get_path('facenet_ros') image_path = package_path + "/default.png" img_def = cv2.imread(image_path, 1) if not os.path.exists(training_dir + "/Unknown"): os.makedirs(training_dir + "/Unknown") misc.imsave( training_dir + "/Unknown/default.png", cv2.cvtColor(img_def, cv2.COLOR_BGR2RGB)) except OSError: print("Creation of the directory %s failed" % training_dir) else: print("Successfully created the directory %s " % training_dir) else: try: if os.path.exists(training_dir + "/Unknown/"): shutil.rmtree(training_dir + "/Unknown/") except OSError: pass initRos = True else: if clear_bd or clear_face_id: print('Try to clean a data base') if os.path.exists(training_dir): if clear_bd: for the_file in os.listdir(training_dir): file_path = os.path.join( training_dir, the_file) try: if os.path.isfile(file_path): os.unlink(file_path) elif os.path.isdir(file_path) and ( not "Unknown" in os.path.basename(file_path) or not classifier_mode): shutil.rmtree(file_path) except Exception as e: print(e) if classifier_mode: train_faces_classifier() else: face_recognition.identifier.class_names = [] face_recognition.identifier.labels = [] if len(face_recognition.identifier.db_emb) > 0: face_recognition.identifier.db_emb = np.empty( (0, len(face_recognition.identifier. db_emb[0]))) if clear_face_id: person_dir = training_dir + "/" + face_clear_id if os.path.exists(person_dir): shutil.rmtree(person_dir) if classifier_mode: train_faces_classifier() if not classifier_mode: try: index_class = face_recognition.identifier.class_names.index( face_clear_id) face_recognition.identifier.class_names.remove( face_clear_id) indices = [ i for i, x in enumerate( face_recognition.identifier.labels) if x == index_class ] indices.sort() removes = 0 if len(indices) > 0: for index in range(len(indices)): del face_recognition.identifier.labels[ indices[index] + removes] removes -= 1 face_recognition.identifier.db_emb = np.delete( face_recognition.identifier.db_emb, indices, 0) except ValueError: pass clear_bd = False clear_face_id = False rate.sleep() else: print 'Waiting for the ros master' if initRos: print 'Shudowning the ros node' s.shutdown() sa.shutdown() if classifier_mode: st.shutdown() sub_clear_faces.unregister() sub_clear_face_id.unregister() #rospy.signal_shutdown('Ros master is shutdown') initRos = False else: time.sleep(1)
import sys import os from subprocess import Popen import signal import time import rosgraph def run(cmd): return Popen(cmd, shell=False, preexec_fn=os.setsid) def start_process(cmd, type): return run(cmd) print("Output from Python") p_ros = start_process('/opt/ros/kinetic/bin/roscore', 'ros') if rosgraph.is_master_online(): print("ROS is online") print("PGID ROS: ", os.getpgid(p_ros.pid)) else: print("Start ROS core give argument yes or y")
if edge(u, v) not in fin: log("Timing point " + str(u) + " -> " + str(v) + " ...") t = time_point(coords[u], coords[v]) while t is False: print("Failed to time point, retiming!") t = time_point(coords[u], coords[v]) log("Took: " + str(t) + " !") log("Distance was: " + str(dist(coords[u], coords[v]))) done += 1 fin.add(edge(u, v)) log("Finished %s out of %s" % (done, tot)) if __name__ == '__main__': if not rosgraph.is_master_online(): roscore() rospy.init_node('star_publisher') # Start simulation process = start_roslaunch_process("bwi_launch", "simulation_v2.launch") pub = rospy.Publisher('/move_base_interruptable_simple/goal', PoseStamped, queue_size=3) sub_end = rospy.Subscriber('/move_base_interruptable/result', MoveBaseActionResult, runEnded) odom = rospy.Subscriber('/odom', Odometry, odometer) vel = rospy.Subscriber('/cmd_vel', Twist, speedometer) clearTimes() rospy.sleep(20)
def flash_firmware( firmware_path: Optional[str] = None, board_type: Optional[BoardType] = None, check_version: bool = True, ): write_flush("--> Checking if stm32loader is installed.. ") if is_tool("stm32loader"): print("YES") else: print("NO") print("ERROR: Cannot find the stm32loader tool. " "Make sure the python3-stm32loader package is installed.") return ##################################################### write_flush("--> Checking if ROS Master is online.. ") if rosgraph.is_master_online(): print("YES") master_online = True else: print("NO") master_online = False if check_version: print("ROS Master is not running. " "Will not be able to check the current firmware version.") if not query_yes_no("Are you sure you want to continue?", default="no"): return ##################################################### if master_online: write_flush("--> Initializing ROS node.. ") rospy.init_node("firmware_flasher", anonymous=True) print("DONE") ##################################################### if master_online: write_flush("--> Checking if rosserial node is active.. ") if rospy.resolve_name("serial_node") in rosnode.get_node_names(): print("YES") serial_node_active = True else: print("NO") serial_node_active = False if check_version: print( "Rosserial node is not active. " "Will not be able to check the current firmware version.") if not query_yes_no("Are you sure you want to continue?", default="no"): return ##################################################### if master_online and serial_node_active and board_type is None: write_flush("--> Trying to determine board type.. ") board_type = determine_board() if board_type is not None: print("SUCCESS") else: print("FAIL") ##################################################### current_firmware_version = "<unknown>" if check_version and master_online and serial_node_active: write_flush("--> Trying to check the current firmware version.. ") current_firmware_version = check_firmware_version() if current_firmware_version != "<unknown>": print("SUCCESS") else: print("FAIL") ##################################################### if master_online and serial_node_active: write_flush("--> Checking if rosmon service is available.. ") if rospy.resolve_name( "rosmon/start_stop") in rosservice.get_service_list(): start_stop = rospy.ServiceProxy("rosmon/start_stop", StartStop) print("YES") rosmon_available = True else: print("NO") rosmon_available = False ##################################################### if board_type is None: print( "Was not able to determine the board type. Choose the board manually: " ) board_type = prompt_options([ ("LeoCore", BoardType.LEOCORE), ("Husarion CORE2", BoardType.CORE2), ]) ##################################################### if firmware_path is not None: firmware_version = "<unknown>" else: if board_type == BoardType.CORE2: firmware_version = "1.2.0" elif board_type == BoardType.LEOCORE: firmware_version = "1.0.2" print(f"Current firmware version: {current_firmware_version}") print(f"Version of the firmware to flash: {firmware_version}") if not query_yes_no("Flash the firmware?"): return ##################################################### if master_online and serial_node_active: if rosmon_available: write_flush("--> Stopping the rosserial node.. ") start_stop("serial_node", rospy.get_namespace().rstrip("/"), StartStopRequest.STOP) rospy.sleep(1) print("DONE") else: print("WARNING: rosserial node is active, but rosmon service " "is not available. You have to manually stop rosserial node " "before flashing the firmware.") if not query_yes_no("Continue?", default="no"): return ##################################################### if board_type == BoardType.CORE2: bootloader_path = os.path.join( rospkg.RosPack().get_path("leo_fw"), "firmware/bootloader_1_0_0_core2.bin", ) if firmware_path is None: firmware_path = os.path.join( rospkg.RosPack().get_path("leo_fw"), "firmware/core2_firmware.bin", ) flash_core2(bootloader_path, firmware_path) elif board_type == BoardType.LEOCORE: if firmware_path is None: firmware_path = os.path.join( rospkg.RosPack().get_path("leo_fw"), "firmware/leocore_firmware.bin", ) flash_leocore(firmware_path) ##################################################### if master_online and serial_node_active: if rosmon_available: write_flush("--> Starting the rosserial node.. ") start_stop("serial_node", rospy.get_namespace().rstrip("/"), StartStopRequest.START) print("DONE") else: print("You can now start the rosserial node.")
def is_master_disconnected(self): try: rospy.get_master().getSystemState() except socket.error: return True return not rosgraph.is_master_online()
def __init__(self): #clean up previous process os.system("killall gzserver gzclient") if rosgraph.is_master_online( ): # Checks the master uri and results boolean (True or False) print 'ROS MASTER is active' nodes = rosnode.get_node_names() if "/rviz" in nodes: print("Rviz active") rvizflag = " rviz:=false" else: rvizflag = " rviz:=true" #start ros impedance controller uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) self.launch = roslaunch.parent.ROSLaunchParent(uuid, [ os.environ['LOCOSIM_DIR'] + "/ros_impedance_controller/launch/ros_impedance_controller.launch" ]) #only available in ros lunar # roslaunch_args=rvizflag # self.launch = roslaunch.parent.ROSLaunchParent(uuid, [os.environ['LOCOSIM_DIR'] + "/ros_impedance_controller/launch/ros_impedance_controller_stdalone.launch"],roslaunch_args=[roslaunch_args]) self.launch.start() ros.sleep(4.0) threading.Thread.__init__(self) # instantiate graphic utils self.ros_pub = RosPub(True) self.joint_names = "" self.u = Utils() self.comPoseW = np.zeros(6) self.baseTwistW = np.zeros(6) self.stance_legs = np.array([True, True, True, True]) self.q = np.zeros(12) self.qd = np.zeros(12) self.tau = np.zeros(12) self.q_des = np.zeros(12) self.qd_des = np.zeros(12) self.tau_ffwd = np.zeros(12) self.b_R_w = np.eye(3) self.grForcesW = np.zeros(12) self.basePoseW = np.zeros(6) self.J = [np.eye(3)] * 4 self.wJ = [np.eye(3)] * 4 self.robot_name = ros.get_param('/robot_name') self.sub_contact = ros.Subscriber("/" + self.robot_name + "/contacts_state", ContactsState, callback=self._receive_contact, queue_size=100) self.sub_pose = ros.Subscriber("/" + self.robot_name + "/ground_truth", Odometry, callback=self._receive_pose, queue_size=1) self.sub_jstate = ros.Subscriber("/" + self.robot_name + "/joint_states", JointState, callback=self._receive_jstate, queue_size=1) self.pub_des_jstate = ros.Publisher("/command", JointState, queue_size=1) # freeze base and pause simulation service self.reset_world = ros.ServiceProxy('/gazebo/set_model_state', SetModelState) self.reset_gravity = ros.ServiceProxy('/gazebo/set_physics_properties', SetPhysicsProperties) self.pause_physics_client = ros.ServiceProxy('/gazebo/pause_physics', Empty) self.unpause_physics_client = ros.ServiceProxy( '/gazebo/unpause_physics', Empty) # Loading a robot model of HyQ (Pinocchio) self.robot = getRobotModel("hyq") #send data to param server self.verbose = conf.verbose self.u.putIntoGlobalParamServer("verbose", self.verbose)
def update_fw(): write_flush("--> Checking if stm32loader is installed.. ") if is_tool("stm32loader"): print("YES") else: print("NO") print("ERROR: Cannot find the stm32loader tool. " "Make sure the python3-stm32loader package is installed.") return write_flush("--> Checking if ROS Master is online.. ") if rosgraph.is_master_online(): print("YES") master_online = True else: print("NO") master_online = False print("ROS Master is not running. " "Will not be able to check the current firmware version.") if not query_yes_no("Are you sure you want to continue?", default="no"): return if master_online: write_flush("--> Initializing ROS node.. ") rospy.init_node("firmware_updater", anonymous=True) print("DONE") if master_online: write_flush("--> Checking if rosserial node is active.. ") if "/serial_node" in rosnode.get_node_names(): print("YES") serial_node_active = True else: print("NO") serial_node_active = False print("Rosserial node is not active. " "Will not be able to check the current firmware version.") if not query_yes_no("Are you sure you want to continue?", default="no"): return current_firmware_version = "<unknown>" if master_online and serial_node_active: write_flush("--> Checking the current firmware version.. ") if "/core2/get_firmware_version" in rosservice.get_service_list(): get_firmware_version = rospy.ServiceProxy( "/core2/get_firmware_version", Trigger) current_firmware_version = get_firmware_version().message print("OK") else: print("FAIL") print("WARNING: Could not get the current firmware version: " "/core2/get_firmware_version service is not available.") if master_online and serial_node_active: write_flush("--> Checking if rosmon service is available.. ") if "/rosmon/start_stop" in rosservice.get_service_list(): start_stop = rospy.ServiceProxy("/rosmon/start_stop", StartStop) print("YES") rosmon_available = True else: print("NO") rosmon_available = False print("Current firmware version: {}".format(current_firmware_version)) print("Version of the firmware to flash: {}".format(FIRMWARE_VERSION)) if not query_yes_no("Flash the firmware?"): return if master_online and serial_node_active: if rosmon_available: write_flush("--> Stopping the rosserial node.. ") start_stop("serial_node", "", StartStopRequest.STOP) rospy.sleep(1) print("DONE") else: print("WARNING: rosserial node is active, but rosmon service " "is not available. You have to manually stop rosserial node " "before flashing the firmware.") if not query_yes_no("Continue?", default="no"): return bootloader_path = os.path.join(rospkg.RosPack().get_path("leo_fw"), "firmware", BOOTLOADER_BINARY) firmware_path = os.path.join(rospkg.RosPack().get_path("leo_fw"), "firmware", FIRMWARE_BINARY) print("--> Disabling flash write protection") subprocess.check_call("stm32loader -c rpi -f F4 -W", shell=True) print("--> Erasing flash and flashing bootloader") subprocess.check_call( "stm32loader -c rpi -f F4 -e -w -v {}".format(bootloader_path), shell=True) print("--> Flashing firmware") subprocess.check_call( "stm32loader -c rpi -f F4 -a 0x08010000 -w -v {}".format( firmware_path), shell=True) print("Flashing completed!") if master_online and serial_node_active: if rosmon_available: write_flush("--> Starting the rosserial node.. ") start_stop("serial_node", "", StartStopRequest.START) print("DONE") else: print("You can now start the rosserial node.")
rospy.loginfo('Exiting the program...') launch_tracking.shutdown() exit() print('[INFO] Starting calibration program...') launch_calibration = roslaunch.parent.ROSLaunchParent( uuid, [os.path.join(script_path, '..', 'launch', 'my_calibrate.launch')]) launch_calibration.start() calibration_result_file = os.path.join( os.getenv('HOME'), '.ros/easy_handeye/my_eye_on_base.yaml') # todo: this loop condition is not the most appropriate, better using ros node info... file_ind = 0 while rosgraph.is_master_online(): rospy.sleep(2) # check if the calibration result has been saved # if so, move the file to ../results/ if os.path.exists(calibration_result_file): if not os.path.exists(os.path.join(script_path, '..', 'results')): os.mkdir(os.path.join(script_path, '..', 'results')) shutil.move( calibration_result_file, os.path.join(script_path, '..', 'results', 'Extrinsics' + str(file_ind) + '.yaml')) file_ind += 1 launch_tracking.shutdown() launch_calibration.shutdown() exit()
def main(): if not rosgraph.is_master_online(): print('Error: ROS master not running') exit(1) rospy.init_node('pick_and_place_state_machine') bf = BlockFactory() # Create a SMACH state machine sm = smach.StateMachine(outcomes=['finished', 'failed']) # Open the container with sm: # Add states smach.StateMachine.add('SELECT', Select(bf), transitions={ 'selected': 'PICKANDPLACE', 'out_of_blocks': 'REST' }) # Submachine for pick-and-place sm_pickplace = smach.StateMachine( outcomes=['placed', 'failed'], input_keys=['start_pose', 'end_pose']) with sm_pickplace: def pick_goal_cb(userdata, goal): pick_goal = PickGoal() pick_goal.obj_pose = userdata.start_pose return pick_goal def pick_result_cb(userdata, status, result): if status == GoalStatus.SUCCEEDED: userdata.target_pose = userdata.end_pose return 'succeeded' elif status == GoalStatus.PREEMPTED: return 'preempted' else: return 'aborted' smach.StateMachine.add('PICK', smach_ros.SimpleActionState( 'pick_action_server', PickAction, goal_cb=pick_goal_cb, result_cb=pick_result_cb, input_keys=['start_pose', 'end_pose'], output_keys=['target_pose']), transitions={ 'succeeded': 'PLACE', 'preempted': 'failed', 'aborted': 'failed' }) def place_goal_cb(userdata, goal): place_goal = PlaceGoal() place_goal.target_pose = userdata.target_pose return place_goal smach.StateMachine.add('PLACE', smach_ros.SimpleActionState( 'place_action_server', PlaceAction, goal_cb=place_goal_cb, input_keys=['target_pose']), transitions={ 'succeeded': 'placed', 'preempted': 'failed', 'aborted': 'failed' }) smach.StateMachine.add('PICKANDPLACE', sm_pickplace, transitions={ 'placed': 'SELECT', 'failed': 'failed' }) def rest_goal_cb(userdata, goal): rest_goal = RestGoal() return rest_goal smach.StateMachine.add('REST', smach_ros.SimpleActionState( 'rest_action_server', RestAction, goal_cb=rest_goal_cb), transitions={ 'succeeded': 'finished', 'preempted': 'failed', 'aborted': 'failed' }) # Create and start the introspection server sis = smach_ros.IntrospectionServer('sm_introspection_server', sm, '/SM_ROOT') sis.start() # Execute SMACH plan outcome = sm.execute() print("Outcome: " + outcome) # Wait for ctrl-c to stop the application rospy.spin() sis.stop()
def do_autorun(test_args, cwssim_args, save_name=None, autosort=False, test_id=None, test_variable=None): """ runs the simulate torf launch file 'repeats' times with the logfiles saved in the default location (~/.ros). The procedure is repeated in all of the required worlds as defined in the test_args Namespace :param repeats: how many times each test is repeated :return: """ normal_exit_summary = np.zeros(test_args.repeats) for world in test_args.worlds: for jdx in range(test_args.repeats): rospy.loginfo( 'running main test with index {}, inside a while loop until shutdown detected' .format(jdx)) run_id = get_uuid() print('starting roscore') parent = roslaunch.parent.ROSLaunchParent(run_id, [], is_core=True) if rosgraph.is_master_online( ): # Checks the master uri and results boolean (True or False) print('ROS MASTER is already online no need to restart') else: print('ROS MASTER is offline, starting new roscore') parent.start() print('roscore started') print('Launching torf simulate ') if save_name: bag_prefix = save_name + '_' + world[0:-6] + '_r_' + str( jdx ) + '.bag' # todo - make world optional - perhaps create subfolder for each test run elif autosort: if test_variable: bag_path = os.path.join(ROSLOG, test_id, world[0:-6], str(test_variable)) if not os.path.exists(bag_path): try: os.makedirs(bag_path) except OSError: # if e.errno != errno.EEXIST: raise OSError() bag_prefix = os.path.join(bag_path, 'r_' + str(jdx)) else: bag_prefix = os.path.join(ROSLOG, test_id, world[0:-6], 'r_' + str(jdx)) # raise IOError("the 'test_variable' parameter must be passed in autosorting mode") else: bag_prefix = 'sweep_mission_' + world[0:-6] + '_r_' + str(jdx) print('saving too {}'.format(bag_prefix)) # print (bag_path) print(ROSLOG) print(ROSLAUNCH) lf = Cwssim_launch_node( run_id=run_id, cwssim_args=cwssim_args, world=world, vehicle=test_args.vehicle, gui=test_args.gui, bag_prefix=bag_prefix, save_images=test_args.save_images, ) lf.spin_up() # set the test complete parameter to False cx_test_complete_param_str = 'cwssim_test_complete' rospy.set_param(cx_test_complete_param_str, False) try: rospy.wait_for_message('cwssim_node/cwssim_status', Cwssim_status, 60) except Exception as e: rospy.logwarn(e) rospy.loginfo( 'timed out waiting for torf status - will try next bit anyway' ) try: rospy.loginfo( 'All nodes initiliased succesfully - starting our test loop' ) wd = WatchdogTimer() while True: if rospy.is_shutdown(): rospy.logerr( ('ROS shutdown request - qutting auto mission')) normal_exit_summary[jdx] = 2.0 break mish_comp = rospy.get_param(cx_test_complete_param_str, False) if mish_comp: rospy.logwarn( 'terminating mission as we think its finished') normal_exit_summary[jdx] = 1.0 break if wd.timed_out: rospy.logerr('Watchdog timer elapsed - quitting') break rospy.logwarn_throttle( 20, 'running main test with index {}, inside a while loop until shutdown detected' .format(jdx)) rospy.sleep(1.0) except Exception: rospy.logerr('Problem with loop {} in auto test'.format(jdx)) rospy.loginfo('waiting 10s before cleanup') time.sleep(10) rospy.loginfo('cleaning up learning flight') try: lf.launch.shutdown() except Exception: rospy.logerr( 'not cleaning up lf node - suspect this has already terminated' ) rospy.loginfo('shutting parent down') parent.shutdown() rospy.loginfo('waiting 10s before next loop') time.sleep(10) return normal_exit_summary
def main(args): rospy.init_node('facenet_node', anonymous=True) rospack = RosPack() rate = rospy.Rate(30) #30Hz global bridge global facenetGraph global face_recognition global classifier_file global training_dir global threshold_reco global image_size global training_dir global batch_size global face_recog_available global classifier_mode global clear_face_id global clear_bd global face_clear_id clear_face_id = False clear_bd = False face_clear_id = "" face_recog_available = True model_file = rospy.get_param("~model_file") classifier_file = rospy.get_param("~classifier_file") image_size = rospy.get_param("~image_size") margin = rospy.get_param("~margin") gpu_memory_fraction = rospy.get_param("~gpu_memory_fraction") detect_multiple_faces = rospy.get_param("~detect_multiple_faces") threshold_reco = rospy.get_param("~threshold_reco") training_dir = rospy.get_param("~training_dir") classifier_mode = rospy.get_param("~classifier_mode") batch_size = rospy.get_param("~batch_size") try: if not os.path.exists(training_dir): os.makedirs(training_dir) except OSError: print("Creation of the directory %s failed" % training_dir) else: print("Successfully created the directory %s " % training_dir) if classifier_mode: try: if not os.path.exists(training_dir + "/Unknown/defualt.png"): package_path = rospack.get_path('facenet_ros') image_path = package_path + "/default.png" img_def = cv2.imread(image_path, 1) if not os.path.exists(training_dir + "/Unknown"): os.makedirs(training_dir + "/Unknown") misc.imsave(training_dir + "/Unknown/default.png", cv2.cvtColor(img_def, cv2.COLOR_BGR2RGB)) except OSError: print("Creation of the directory %s failed" % training_dir) else: print("Successfully created the directory %s " % training_dir) else: try: if os.path.exists(training_dir + "/Unknown/"): shutil.rmtree(training_dir + "/Unknown/") except OSError: pass bridge = CvBridge() facenetGraph = tf.Graph() with facenetGraph.as_default(): if classifier_mode: face_recognition = faceC.Recognition( facenet_model=model_file, classifier_model=classifier_file, face_crop_size=image_size, threshold=[0.7, 0.8, 0.8], factor=0.709, face_crop_margin=margin, gpu_memory_fraction=gpu_memory_fraction, detect_multiple_faces=detect_multiple_faces) else: face_recognition = face.Recognition( facenet_model=model_file, data_dir=training_dir, face_crop_size=image_size, threshold=[0.7, 0.8, 0.8], factor=0.709, face_crop_margin=margin, gpu_memory_fraction=gpu_memory_fraction, detect_multiple_faces=detect_multiple_faces) s = rospy.Service('face_recognizer/faces', FaceRecognition, face_recognition_callback) sa = rospy.Service('face_recognizer/train_face', FaceRecognition, add_face_callback) if classifier_mode: st = rospy.Service('face_recognizer/train_flush', EmptySrv, train_faces_callback) rospy.Subscriber("face_recognizer/clear_faces", Empty, clear_faces_callback) rospy.Subscriber("face_recognizer/clear_face_id", String, clear_face_id_callback) while not rospy.is_shutdown() and rosgraph.is_master_online(): print("Waiting for image\n") try: img = rospy.wait_for_message("/usb_cam/image_raw/compressed", CompressedImage, timeout=1) img_np_arr = np.fromstring(img.data, np.uint8) encoded_img = cv2.imdecode(img_np_arr, 1) encoded_img = cv2.flip(encoded_img, 1) with facenetGraph.as_default(): with face_recognition.encoder.sess.as_default(): faces = face_recognition.identify(encoded_img) add_overlays(encoded_img, faces) cv2.imshow('Face Recognition', encoded_img) if cv2.waitKey(1) & 0xFF == ord('q'): return except rospy.ROSException as e: pass if clear_bd or clear_face_id: print('Try to clean a data base') if os.path.exists(training_dir): if clear_bd: for the_file in os.listdir(training_dir): file_path = os.path.join(training_dir, the_file) try: if os.path.isfile(file_path): os.unlink(file_path) elif os.path.isdir(file_path) and ( not "Unknown" in os.path.basename(file_path) or not classifier_mode): shutil.rmtree(file_path) except Exception as e: print(e) if classifier_mode: train_faces_classifier() else: face_recognition.identifier.class_names = [] face_recognition.identifier.labels = [] if len(face_recognition.identifier.db_emb) > 0: face_recognition.identifier.db_emb = np.empty( (0, len(face_recognition.identifier.db_emb[0]))) if clear_face_id: person_dir = training_dir + "/" + face_clear_id if os.path.exists(person_dir): shutil.rmtree(person_dir) if classifier_mode: train_faces_classifier() if not classifier_mode: try: index_class = face_recognition.identifier.class_names.index( face_clear_id) face_recognition.identifier.class_names.remove( face_clear_id) indices = [ i for i, x in enumerate( face_recognition.identifier.labels) if x == index_class ] indices.sort() removes = 0 if len(indices) > 0: for index in range(len(indices)): del face_recognition.identifier.labels[ indices[index] + removes] removes -= 1 face_recognition.identifier.db_emb = np.delete( face_recognition.identifier.db_emb, indices, 0) except ValueError: pass clear_bd = False clear_face_id = False rate.sleep()
#!/usr/bin/env python # Reference: # https://answers.ros.org/question/10499/best-way-to-check-if-ros-is-running/ import rosgraph if rosgraph.is_master_online(): print 'ROS MASTER is Online' else: print 'ROS MASTER is Offline'
def is_alive(self): if rosgraph.is_master_online(): return True
def isROS(): with timeout(1): return rosgraph.is_master_online()