def stateCallback(state):
global learner
if learner.current_episode > learner.num_episodes:
rospy.shutdown()
learner.close()
return
print("Current step: ", learner.current_step)
if learner.current_step == 0 or learner.current_step > learner.max_steps_ep or learner.done:
reset()
observation = np.array([
state.goal_distance,
state.speed,
state.laser1,
state.laser2,
state.laser3
])
action = learner.step(observation)
learner.current_step = learner.current_step + 1
control = Control()
control.traction = action[0]
control.steering = action[1]
control.reset = 0
sendControl(control)
def stateCallback(state):
print("ciao")
if current_episode > learner.num_episodes:
rospy.shutdown()
learner.close()
return
if current_step > learner.max_steps_ep:
reset()
observation = np.array([state.goal_distance
state.speed
state.laser1
state.laser2
state.laser3
])
action = learner.step(observation)
current_step = current_step + 1
control = Control()
control.traction = action[0]
control.steering = action[1]
control.reset = 0
sendControl(control)
def evaulate_state(self):
# Check if we have data to evaluate state or not
if len(self.base_scan.ranges) is 0:
return
# We have data so we can evaluate state! Woo.
if self.state is RobotStates.MoveForward:
if self.is_blocked():
rospy.logdebug("Detected collision!")
self.state = RobotStates.Stop
self.full_stop()
else:
rospy.logdebug("Moving forward")
self.go_forward(self.get_new_velocity())
elif self.state is RobotStates.Stop:
rospy.logdebug("Stopping")
self.rotate(random.choice([-1, 1])*self.turn)
self.state = RobotStates.Rotate
elif self.state is RobotStates.Rotate:
if self.is_blocked():
pass
else:
self.state = RobotStates.MoveForward
self.full_stop()
rospy.logdebug("Rotating")
else:
# This should never happen. Panic and exit.
rospy.logfatal("Simplebot controller state is undefined. " +
"Are new states added but not managed? " +
"Killing the controller.")
self.shutdown()
rospy.shutdown()
def prepare_data(self):
self.full_data = []
self.full_labels = []
self.class_data = [[] for x in range(0, 2)]
rospy.logwarn("Normalized data : " + str(self.normalized))
rospy.logwarn("Folds : " + str(self.folds))
rospy.logwarn("Datafile : " + self.datafile)
rospy.logwarn("Batch Training : " + str(self.batch_train))
rospy.logwarn("Batch Testing : " + str(self.batch_test))
if normalized == 0:
rospy.logwarn(self.datafile + "full_data.mat")
self.full_data = scio.loadmat(self.datafile + "full_data.mat")
self.full_data = self.full_data.get("data")
else:
rospy.logwarn(self.datafile + "full_data_normalized.mat")
self.full_data = scio.loadmat(self.datafile +
"full_data_normalized.mat")
self.full_data = self.full_data.get("data")
if (self.full_data is None) or (self.full_data == []):
rospy.logerr("No data found, exiting")
rospy.shutdown()
exit()
# self.full_labels = scio.loadmat(self.datafile + "labels_annotated.mat")
self.full_labels = scio.loadmat(self.labelfile)
# print self.labelfile
# print self.full_labels
self.full_labels = self.full_labels.get("labels")
# print self.full_labels
# print self.full_data
self.full_labels = self.full_labels[0]
print(self.full_labels)
for i in range(0, len(self.full_data)):
# print self.full_labels[i]
# print self.full_data[i]
if self.full_labels[i] == 0:
self.class_data[0].append(self.full_data[i])
else:
self.class_data[1].append(self.full_data[i])
print(np.array(self.class_data[0]).shape)
print(np.array(self.class_data[1]).shape)
self.t = np.zeros((2, 2))
sum_ = 0
prev = -1
for i in range(0, len(self.full_labels)):
if prev == -1:
prev = self.full_labels[i]
self.t[prev][self.full_labels[i]] += 1.0
prev = self.full_labels[i]
sum_ += 1.0
self.t = self.t / sum_
print("Transition probabilities")
print self.t
def main(args):
ic = ImageConverter()
rospy.init_node('motion_detector_python', anonymous=True)
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
rospy.shutdown();
def main(args):
rospy.init_node('controller', anonymous=True)
command_srv = rospy.Service("voice_commands", Command, handle_command)
rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, poseDataCallBack)
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
rospy.shutdown()
def main(args):
rospy.init_node('controller', anonymous=True)
command_srv = rospy.Service("voice_commands", Command, handle_command)
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
rospy.shutdown();
def callback(data):
global recorder, count, sub
recorder.add_line(data)
print("\t%d.%d" % (data.header.stamp.secs, data.header.stamp.nsecs))
count += 1
if count == 40:
sub.unregister()
recorder.save_file()
rospy.shutdown()
def main(args):
try:
control = Controls()
# rate = rospy.Rate(10)
while (not rospy.is_shutdown()):
# rate.sleep()
control.cruizer()
except KeyboardInterrupt():
print("Shutting down..")
rospy.shutdown()
def go(self, position, angle, wait=True):
rospy.logerr('no longer supported!!!!!!!!!!!!')
rospy.shutdown()
s = Twist()
while True:
try:
(trans,rot) = self.listener.lookupTransform(self.refFrame, "/base_link", rospy.Time(0))
theta = tf.transformations.euler_from_quaternion(rot)[2]
x_diff = (position[0] - trans[0])
y_diff = (position[1] - trans[1])
alpha = atan2(y_diff, x_diff)
r = alpha - theta
if self.verbose:
print 'X: %4f, Y: %4f, angle: %4f' % (trans[0], trans[1], theta)
l = LA.norm([x_diff, y_diff])
s.linear.x = l * cos(r) * self.linearGain
s.linear.y = l * sin(r) * self.linearGain
tmp = LA.norm([s.linear.x, s.linear.y])
if tmp < self.linearTwistBound.lower:
s.linear.x = s.linear.x * (self.linearTwistBound.lower / tmp)
s.linear.y = s.linear.y * (self.linearTwistBound.lower / tmp)
if tmp > self.linearTwistBound.upper:
s.linear.x = s.linear.x * (self.linearTwistBound.upper / tmp)
s.linear.y = s.linear.y * (self.linearTwistBound.upper / tmp)
z_diff = (angle - theta)
s.angular.z = z_diff * self.angularGain
if abs(s.angular.z) > self.angularTwistBound.upper:
s.angular.z = self.angularTwistBound.upper * (s.angular.z)/abs(s.angular.z)
elif abs(s.angular.z) < self.angularTwistBound.lower:
s.angular.z = self.angularTwistBound.lower * (s.angular.z)/abs(s.angular.z)
self.base_pub.publish(s)
if LA.norm([x_diff, y_diff]) < self.posTolerance and abs(z_diff) < self.angTolerance:
if self.verbose:
print 'position and angle arrived'
return True
if not wait:
return False
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
if wait:
self.comm.sleep()
else:
return False
def main(args):
try:
control = Controls()
while (not rospy.is_shutdown()):
control.cruizer()
# control.convertor()
#control.errorcheck()
#time.sleep(0.05)
except KeyboardInterrupt():
print("Shutting down..")
rospy.shutdown()
rospy.shutdown()
def image2pred(self, image_raw, net):
# Capture/Predict frame-by-frame
image = cv2.resize(image_raw, (net.width, net.height), interpolation=cv2.INTER_AREA)
feed_pred = {net.input_node: image, net.input_shape: image_raw.shape}
pred, pred_up = net.sess.run([net.tf_pred, net.tf_pred_up],
feed_dict=feed_pred) # pred_up: ((1, 375, 1242, 1), dtype('float32'))
# Image Processing
pred_8UC1_scaled = convert2uint8(pred[0])
pred_up_8UC1_scaled = convert2uint8(pred_up[0])
pred_8UC1_66x100 = cv2.resize(pred_8UC1_scaled, (200, 66))
pred_8UC3_66x100 = cv2.cvtColor(pred_8UC1_66x100, cv2.COLOR_GRAY2RGB)
# CV2 Image -> ROS Image Message
pred_up_8UC1_msg = bridge.cv2_to_imgmsg(pred_up_8UC1_scaled, encoding="passthrough")
pred_up_32FC1_msg = bridge.cv2_to_imgmsg(pred_up[0], encoding="passthrough")
pred_up_8UC3_66x200_msg = bridge.cv2_to_imgmsg(pred_8UC1_66x100, encoding="passthrough")
print(pred_up_8UC1_msg.encoding)
print(pred_up_32FC1_msg.encoding)
# Publish!
pred_up_8UC1_msg.header = self.rec_camera_info_msg.header
pred_up_32FC1_msg.header = self.rec_camera_info_msg.header
self.pub_pred_up_8UC1.publish(pred_up_8UC1_msg)
self.pub_pred_up_32FC1.publish(pred_up_32FC1_msg)
self.pub_pred_camera_info.publish(self.rec_camera_info_msg)
self.pub_pred_up_8UC3_66x200.publish(pred_up_8UC3_66x200_msg)
rospy.loginfo("image2pred")
print('---')
# Display Images using OpenCV
cv2.imshow("image_raw", image_raw)
cv2.imshow('image', image)
cv2.imshow('pred', pred_8UC1_scaled)
cv2.imshow('pred_up', pred_up_8UC1_scaled)
cv2.imshow('pred, 66x200, gray', pred_8UC1_66x100)
cv2.imshow('pred, 66x200, rgb', pred_8UC3_66x100)
# Mandatory code for the ROS node and OpenCV structures.
self.rate.sleep()
# FIXME:
if cv2.waitKey(1) & 0xFF == ord('q'): # without waitKey() the images are not shown.
# return 0
rospy.shutdown()
def modelStatesCallback(self, data):
#First, we'll want to figure out where the car chassis is in the array.
model_name = "f1tenth"
model_index = data.name.index(model_name)
twist_x = data.twist[model_index].linear.x
twist_y = data.twist[model_index].linear.y
#We're interested in the magnitude of the velocity, here.
self.currentSpeed = math.hypot(twist_x, twist_y)
self.twist = data.twist[model_index]
self.currPos[0] = data.pose[model_index].position.x
self.currPos[1] = data.pose[model_index].position.y
self.roll, self.pitch, self.yaw = self.quatToEuler(
data.pose[model_index].orientation)
rospy.logdebug("Car Yaw:{}".format(self.yaw))
#Initialize/update checkpoint stuff.
pylon_name_gz = "pylon"
if len(self.checkpointList) == 0:
index = data.name.index(pylon_name_gz)
self.checkpointList.append(
[data.pose[index].position.x, data.pose[index].position.y])
self.currCheckpoint = self.checkpointList[0]
if len(self.checkpointList) <= 0:
rospy.logerr("No checkpoints found in this environment!")
rospy.shutdown()
#We also want to calculate the relative heading of the checkpoint.
car_goal_xdiff = self.currPos[0] - self.currCheckpoint[0]
car_goal_ydiff = self.currPos[1] - self.currCheckpoint[1]
#TODO : When you come back from Georgia, reverse the vector!
car_facing_vec = [-1 * math.cos(self.yaw), -1 * math.sin(self.yaw)]
theta_c_g = math.atan2(car_facing_vec[1],
car_facing_vec[0]) - math.atan2(
car_goal_ydiff, car_goal_xdiff)
self.relative_yaw = theta_c_g
rospy.logdebug("Rel Angle Car_Goal: {}".format(theta_c_g))
#Publish car pose info to log topic.
self.logPosition.publish(data.pose[model_index])
self.logVelocity.publish(data.twist[model_index])
def __init__(self):
rospy.init_node(NODENAME, anonymous=True)
rospy.Subscriber(rootname + subAccel, Int16, self.accelCallback)
rospy.Subscriber(rootname + subBrake, Int16, self.brakeCallback)
rospy.Subscriber(rootname + subSteer, Int16, self.steerCallback)
rospy.Subscriber(rootname + subGear, Int16, self.gearCallback)
self.canSender = can_comms_erp42.control("/dev/ttyUSB0")
print("-----Command Node Subscriber is Ready...-----")
print("-----Please Input Commands!-----")
try:
rospy.spin()
finally:
rospy.shutdown()
def init_serial():
"""
sets up serial interface with gps. This is meant to be a replacement
gpsd.
"""
global D
# start serial connection
baud = 9600
try:
D.gps_serial = serial.Serial("/dev/ttyAMA0",baud,timeout=1)
D.gps_serial.open()
D.gps_serial.write("$PMTK220,200*2C")
D.gps_serial.write("$PMTK300,200,0,0,0,0*2F")
except:
print "Failed to open serial"
rospy.shutdown("Failed to open gps serial")
def __init__(self):
rospy.init_node(NODENAME, anonymous=True)
rospy.Subscriber(rootname + subAccel, Int16, self.accelCallback)
rospy.Subscriber(rootname + subAngular, Int16, self.angularCallback)
rospy.Subscriber(rootname + subBrake, Int16, self.brakeCallback)
rospy.Subscriber(rootname + subSteer, Int16, self.steerCallback)
rospy.Subscriber(rootname + subGear, Int16, self.gearCallback)
self.canSender = can_communication.rosPub()
print("Command Node Subscriber is Ready...")
try:
rospy.spin()
finally:
rospy.shutdown()
def initlize_node():
global _ex
global _joint_positions
'''
Initilize node and spin which simply keeps python
from exiting until this node is stopped
'''
jc = JointController()
rospy.init_node('candybot_arm_control', anonymous=False)
rospy.loginfo("candybot_arm_control is now running")
jointPositionsSubscriber = rospy.Subscriber("/joint_states", JointState,
jc.refresh_joint_position)
'''
pub1 = rospy.Publisher("arm_1/gripper_controller/position_command", JointPositions)
pub2 = rospy.Publisher("/arm_1/arm_controller/position_command", JointPositions)
'''
jvp = rospy.Publisher("arm_1/arm_controller/velocity_command",
JointVelocities)
r = rospy.Rate(20)
while not rospy.is_shutdown():
if _joint_positions[5]:
jv = jc.get_joint_velocities(_joint_positions[0],
_joint_positions[1])
jvp.publish(jv)
#rospy.loginfo("WORKING")
else:
#rospy.loginfo("STOPPED BY BUTTON")
jv = jc.get_joint_velocities(0, 0)
jvp.publish(jv)
#rospy.loginfo("main cycle")
if _ex:
rospy.shutdown()
r.sleep()
#demo_class = DemoClass()
#demo_class.initilize_parameters(10, 10.0)
_ex = True
print "ex set to ", _ex
exit()
def main(args):
global pub
global connection
global velocities
error1=""
rospy.init_node('rc_create_driver_python', anonymous=True)
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
#rospy.init_node('turtle_controller', anonymous=True)
rate = rospy.Rate(40) # 10hz
rc_command_srv = rospy.Service("rc_commands",Control,handle_rc_command)
while not rospy.is_shutdown() :
if velocities is not None:
pub.publish(velocities)
try:
rate.sleep()
except KeyboardInterrupt:
print("Shutting down")
rospy.shutdown();
def waitForMoveItObject(self, name, end='scene', timeout=5):
start = rospy.get_time()
while (rospy.get_time() - start < timeout) and not rospy.is_shutdown():
in_scene = name in self.scene_commander.get_known_object_names()
attached_objects = self.scene_commander.get_attached_objects(
[name])
is_attached = len(attached_objects.keys()) > 0
if in_scene and is_attached:
rospy.logerr(
'Aww snap. An object was found both in the scene and attached to the arm.'
)
rospy.shutdown()
if end == 'scene' and in_scene:
return
rospy.logerr(
'Yikes -- timed out while adding object to MoveIt! scene. A node must have failed'
)
rospy.signal_shutdown("Failed to Generate MoveIt Object")
def callback(self, msg):
self.mini = msg.angle_min
self.increment = msg.angle_increment
currentTime = rospy.Time.now()
if bInitialized == False:
self.range1 = msg.ranges
lastTimeCalled = currentTime
self.bInitialized = True
print "FIRST_TIME:" + rospy.Time.now() + "," + len(self.range1)
elif currentTime.sec - lastTimeCalled.sec > 6:
self.range2 = msg.ranges
print "TIME_NOW:" + rospy.Time.now() + "," + len(self.range2)
for j in len(self.range2):
print "FLAG:" + rospy.Time.now() + "," + j
difference = self.range1[j] - self.range2[j]
if difference >= 2 or difference <= -2:
pos = j
value = range2[j]
print "LASER_INFO: angle_min:" + mini + "; angle_increment:" + increment + "; ranges_size:" + len(
msg.ranges) + "."
print "find object:ranges[" + pos + "]=" + value
#calculate Object position:
oX = lx + value * math.sin(mini + pos * increment)
oY = ly + value * math.cos(mini + pos * increment)
pose.append(oX)
pose.append(oY)
userdata.base_pose = pose
print "FLAG3:[" + value + ", " + position + "]"
print "Object_position:[" + oX + ", " + oY + "]"
print "POSE:" + userdata.base_pose
rospy.shutdown()
lastTimeCalled = currentTime
print "WAITING FOR NEXT CALLBACK:" + rospy.Time.now()
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
(rows, cols, channels) = cv_image.shape
if not (cols > 60 and rows > 60): # returns if data have unvalid shape
return
precise = self.stab.update(cv_image, precision_cut=1)
if self.step == 0:
self.step += 1
elif self.step == 1:
self.detectCorners(cv_image)
elif self.setp == 2:
pass
else:
rospy.shutdown()
self.step += 1
k = cv2.waitKey(30) & 0xff
if k == 27:
exit()
elif (k == ord('e')):
print("erase screen")
self.stab.clearMask() # clears all line
elif (k == ord('r')):
print("reset features")
self.stab.resetFeatures(cv_image)
try:
# self.vel_pub.publish(self.vec_vel)
# print(self.vec_vel.x)
#self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
pass
except CvBridgeError as e:
print(e)
def update_planning_scene_from_moveit(self):
""" Update the current cached scene directly from moveit.
Returns true if it the service call succeeds. False implies moveit hasn't started correctly.
"""
try:
rospy.wait_for_service("/get_planning_scene", 5)
components = PlanningSceneComponents(PlanningSceneComponents.WORLD_OBJECT_NAMES + PlanningSceneComponents.TRANSFORMS)
"""
:type ps_response :moveit_msgs.srv.GetPlanningSceneResponse
"""
ps_request = moveit_msgs.srv.GetPlanningSceneRequest(components=components)
ps_response = self.planning_scene_service.call(ps_request)
self.body_name_cache = [co.id for co in ps_response.scene.world.collision_objects]
rospy.loginfo("body name cache:%s"%(', '.join(self.body_name_cache)))
return True
except Exception as e:
raise e
rospy.logerror("Failed to find service /get_planning_scene %s and force use moveit enabled"%(e))
rospy.shutdown()
rospy.logwarn("Failed to find service /get_planning_scene %s"%(e))
return False
class cvBridgeDemo(object):
def __init__(self):
self.lower_red = np.array([0, 0, 0])
self.upper_red = np.array([25, 255, 255])
self.node_name = "cv_bridge_demo"
self.kernel = np.ones((2, 2), np.uint8)
#Initialize the ros node
rospy.init_node(self.node_name)
# What we do during shutdown
rospy.on_shutdown(self.cleanup)
self.bridge = CvBridge()
# Subscribe to the camera image and depth topics and set
# the appropriate callbacks
self.image_sub = rospy.Subscriber("/camera/rgb/image_color", Image,
self.image_callback)
rospy.loginfo("Waiting for image topics...")
# self.lower_red = np.array([0,50,50])
# self.upper_red = np.array([10,255,255])
def image_callback(self, ros_image):
# def maxContour(cnts):
# idc = 0
# max_area = 0
# counter = 0
# for n in cnts:
# a = cv2.contourArea(n)
# if a>max_area:
# max_area = a
# idc = counter
# counter+=1
# return idc,max_area
# Use cv_bridge() to convert the ROS image to OpenCV format
try:
frame = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")
except CvBridgeError, e:
print e
# Convert the image to a Numpy array since most cv2 functions
# require Numpy arrays.
# frame = np.array(frame, dtype=np.uint8)
# Process the frame using the process_image() function
# display_image = self.process_image(frame)
# Display the image.
# bboxRed = cv2.selectROI(frame)
# redObj = frame[int(bboxRed[1]):int(bboxRed[1]+bboxRed[3]), int(bboxRed[0]):int(bboxRed[0]+bboxRed[2])]
# hR, sR, vR = np.median(redObj[:,:,0]), np.median(redObj[:,:,1]), np.median(redObj[:,:,2])
# self.lower_red = np.array([hR-5, 0, vR-70])
# self.upper_red = np.array([hR+5, sR+50, vR+70])
# print('lower: ',self.lower_red)
# print('higher: ',self.upper_red)
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
hsv = cv2.bilateralFilter(frame, 9, 75, 75)
mask_red = cv2.inRange(hsv, self.lower_red, self.upper_red)
mask_red = cv2.dilate(mask_red, self.kernel, iterations=1)
# mask_red = cv2.morphologyEx(mask_red, cv2.MORPH_CLOSE, self.kernel)
cv2.imshow("red", mask_red)
_, contoursRed, hR = cv2.findContours(mask_red, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
idx, current_max, counter = 0, 0, 0
for n in contoursRed:
a = cv2.contourArea(n)
if a > current_max:
current_max = a
idx = contoursRed.index(n)
redIndex, redValue = idx, current_max
x, y, w, h = cv2.boundingRect(contoursRed[redIndex])
# centroid = (x + (x+w))/2
img = cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)
# redIndex, redValue = maxContour(contoursRed)
print(redValue)
cv2.drawContours(frame, contoursRed, redIndex, (0, 0, 255), 2)
# cv2.rectangle(frame,(x,y,w,h))
cv2.imshow(self.node_name, frame)
# Process any keyboard commands
self.keystroke = cv2.waitKey(5)
if cv2.waitKey(1) == 27:
rospy.shutdown()
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
def callback(msg_data):
rospy.loginfo(msg_data)
def listener():
rospy.init_node('my_fourth_node')
rospy.Subscriber('greetings', String, callback)
rospy.spin()
if __name__ == '__main__':
try:
listener()
except rospy.ROSInterruptException:
rospy.shutdown()
def build_dis_classifier(self):
skf = StratifiedKFold(self.full_labels, n_folds=self.folds)
classifier_array = []
stats_array = []
num_class = len(self.full_data[0])
print (num_class)
for cl in range(0, num_class):
lel = -1
tp_total = 0.0
tn_total = 0.0
fp_total = 0.0
fn_total = 0.0
tests = 0
for train_index, test_index in skf:
if lel > 0:
lel -= 1
continue
stats = []
distros = []
hmm_states = []
state_names = ['swing', 'stance']
swings = 0
stances = 0
for i in range(0, 2):
dis = MGD.from_samples(self.class_data[i])
st = State(dis, name=state_names[i])
distros.append(dis)
hmm_states.append(st)
model = HMM()
print(model.states)
model.add_states(hmm_states)
model.add_transition(model.start, hmm_states[0], 0.5)
model.add_transition(model.start, hmm_states[1], 0.5)
model.add_transition(hmm_states[1], model.end, 0.000000000000000001)
model.add_transition(hmm_states[0], model.end, 0.000000000000000001)
for i in range(0, 2):
for j in range(0, 2):
model.add_transition(hmm_states[i], hmm_states[j], self.t[i][j])
model.bake()
tp = 0.0
tn = 0.0
fp = 0.0
fn = 0.0
train_data = self.full_data[train_index, cl]
train_class = self.full_labels[train_index, cl]
test_data = self.full_data[test_index]
test_class = self.full_labels[test_index]
print(np.isfinite(train_data).all())
print(np.isfinite(test_data).all())
print(np.isnan(train_data.any()))
print(np.isinf(train_data.any()))
print(np.isnan(test_data.any()))
print(np.isinf(test_data.any()))
if (not np.isfinite(train_data.any())) or (not np.isfinite(test_data.any())) \
or (not np.isfinite(train_class.any())) or (not np.isfinite(test_data.any())):
rospy.logerr("NaN or Inf Detected")
exit()
try:
rospy.logwarn("Training model #"+str(cl)+", fold #" + str(tests))
seq = np.array(train_data)
model.fit(seq, algorithm='baum-welch', verbose='True', n_jobs=8, max_iterations=150)
except ValueError:
rospy.logwarn("Something went wrong, exiting")
rospy.shutdown()
exit()
seq = []
if self.batch_test == 1:
s = 0
# for s in range(0, len(test_data)):
while s < len(test_data):
k = 0
seq_entry = []
while k < 20 and s < len(test_data):
seq_entry.append(test_data[s])
k += 1
s += 1
seq.append(seq_entry)
else:
seq = np.array(test_data)
if seq == [] or test_data == []:
rospy.logerr("Empty testing sequence")
continue
log, path = model.viterbi(test_data)
if (len(path) - 2) != len(test_data):
rospy.logerr(len(path))
rospy.logerr(path[0][1].name)
rospy.logerr(path[len(path) - 1][1].name)
rospy.logerr(len(test_data))
exit()
tests += 1
for i in range(0, len(path) - 2):
if path[i + 1][1].name != 'Gait-start' and path[i + 1][1].name != 'Gait-end':
if path[i + 1][1].name == 'swing': # prediction is 0
swings += 1
if test_class[i] == 0: # class is 0
tn += 1.0
elif test_class[i] == 1:
fn += 1.0 # class is 1
elif path[i + 1][1].name == 'stance': # prediction is 1
stances += 1
if test_class[i] == 1: # class is 1
tp += 1.0
elif test_class[i] == 0: # class is 0
fp += 1.0
print (swings)
print (stances)
if (tp + fn) != 0.0:
rospy.logwarn("Sensitivity : " + str(tp / (tp + fn)))
# sensitivity = tp / (tp + fn)
else:
rospy.logwarn("Sensitivity : 0.0")
# sensitivity = 0.0
if (tn + fp) != 0.0:
rospy.logwarn("Specificity : " + str(tn / (tn + fp)))
# specificity = tn_total / (tn_total + fp_total)
else:
rospy.logwarn("Specificity : 0.0")
# specificity = 0.0
if (tn + tp + fn + fp) != 0.0:
rospy.logwarn("Accuracy : " + str((tn + tp) / (tn + tp + fn + fp)))
# accuracy = (tn + tp) / (tn + tp + fn + fp)
else:
rospy.logwarn("Accuracy : 0.0")
# accuracy = 0.0
tn_total += tn
tp_total += tp
fn_total += fn
fp_total += fp
tp_total /= tests
tn_total /= tests
fp_total /= tests
fn_total /= tests
rospy.logerr("TP :" + str(tp_total))
rospy.logerr("TN :" + str(tn_total))
rospy.logerr("FP :" + str(fp_total))
rospy.logerr("FN :" + str(fn_total))
rospy.logerr("Tests :" + str(tests))
if (tp_total + fn_total) != 0.0:
sensitivity = tp_total / (tp_total + fn_total)
else:
sensitivity = 0.0
if (tn_total + fp_total) != 0.0:
specificity = tn_total / (tn_total + fp_total)
else:
specificity = 0.0
if (tn_total + tp_total + fn_total + fp_total) != 0.0:
accuracy = (tn_total + tp_total) / (tn_total + tp_total + fn_total + fp_total)
else:
accuracy = 0.0
rospy.logwarn("----------------------------------------------------------")
rospy.logerr("Total accuracy: " + str(accuracy))
rospy.logerr("Total sensitivity: " + str(sensitivity))
rospy.logerr("Total specificity: " + str(specificity))
stats = [tn_total * tests, fn_total * tests, fp_total * tests, fn_total * tests, tests,
accuracy, sensitivity, specificity]
rospy.logwarn("-------------------DONE-------------------------")
classifier_array.append(model)
stats_array.append(stats)
pickle.dump(classifier_array, open(datafile + "distributed_classifiers.p", 'wb'))
pickle.dump(stats_array, open(datafile + "distributed_stats.p", 'wb'))
scio.savemat(datafile + "distributed_stats.mat", {'stats': stats_array})
def shutdown(self):
self.soundc.say("OK. I am going to shutdown!")
rospy.shutdown()
def setup():
rospy.init_node('svenzva_pick_place_demo', anonymous=False)
global qmap, fkine, joint_states, states_list
states_list = deque(maxlen=10)
record = rospy.get_param('record_points', False)
joint_states = JointState()
gripper_client = actionlib.SimpleActionClient('/revel/gripper_action',
GripperAction)
joint_states_sub = rospy.Subscriber('/joint_states',
JointState,
js_cb,
queue_size=1)
fkine = actionlib.SimpleActionClient('/svenzva_joint_action',
SvenzvaJointAction)
fkine.wait_for_server()
rospy.loginfo("Found Trajectory action server")
gripper_client.wait_for_server()
rospy.loginfo("Found Revel gripper action server")
goal = GripperGoal()
filename = "full_pick_and_place"
fname = ""
rospack = rospkg.RosPack()
path = rospack.get_path('svenzva_demo')
rospy.sleep(1)
if record:
while (not rospy.is_shutdown()):
raw_input(
"Press Enter to save the current joint state to file. Enter q/Q for exit."
)
saved_js = joint_states
name = raw_input("What to name this point: ")
if name == 'q' or name == 'Q':
rospy.shutdown()
f = open(path + "/config/" + filename + ".yaml", "a")
ar = []
ar.append(saved_js.position[0])
ar.append(saved_js.position[1])
ar.append(saved_js.position[2])
ar.append(saved_js.position[3])
ar.append(saved_js.position[4])
ar.append(saved_js.position[5])
f.write(name + ": " + str(ar) + "\n")
f.close()
else:
f = open(path + "/config/" + filename + ".yaml")
qmap = yaml.safe_load(f)
f.close()
#go to first position
js_playback(fname, "home")
while not rospy.is_shutdown():
"""
Grab an existing item from location 1
"""
gripper_client.send_goal(open_gripper())
js_playback(fname, "retract_p1_2")
js_playback(fname, "approach_p1_1")
js_playback(fname, "approach_p1_2")
js_playback(fname, "grasp_p1")
gripper_client.send_goal(close_gripper(500))
js_playback(fname, "retract_p1_1")
js_playback(fname, "retract_p1_2")
js_playback(fname, "hand_off")
rospy.sleep(1.0)
gripper_client.send_goal(feel_and_open_gripper())
rospy.sleep(0.5)
"""
Take an item from human and drop it off at location 2
"""
rospy.sleep(1.0)
gripper_client.send_goal(feel_and_close_gripper(200))
rospy.sleep(0.1)
js_playback(fname, "approach_p2")
js_playback(fname, "grasp_p2")
gripper_client.send_goal(open_gripper())
rospy.sleep(0.5)
js_playback(fname, "retract_p2_1")
js_playback(fname, "retract_p2_2")
js_playback(fname, "hand_off")
"""
Take an item from human and take to location 1
"""
rospy.sleep(1.0)
gripper_client.send_goal(feel_and_close_gripper(300))
js_playback(fname, "approach_p1_1")
js_playback(fname, "grasp_p1")
gripper_client.send_goal(open_gripper())
rospy.sleep(0.5)
js_playback(fname, "retract_p1_1")
"""
Pick up item from location 2 and hand to human
"""
js_playback(fname, "approach_p2")
js_playback(fname, "grasp_p2")
gripper_client.send_goal(close_gripper(300))
rospy.sleep(0.5)
js_playback(fname, "retract_p2_2")
js_playback(fname, "hand_off")
rospy.sleep(1.0)
gripper_client.send_goal(feel_and_open_gripper())
rospy.sleep(1.0)
#old stuff. notably, the gripping current was 400 mA for the 1.2kg box. == .7 Nm
# should delete when above script is working
"""
def __del__(self):
if not rospy.is_shutdown():
rospy.shutdown('shutdown reason')
self.wait()
