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