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()
