我们从Python开源项目中，提取了以下47个代码示例，用于说明如何使用rospy.signal_shutdown()。
def run(self): cv2.namedWindow("display", cv2.WINDOW_NORMAL) cv2.setMouseCallback("display", self.opencv_calibration_node.on_mouse) cv2.createTrackbar("scale", "display", 0, 100, self.opencv_calibration_node.on_scale) if self.extra_queue: cv2.namedWindow("extra", cv2.WINDOW_NORMAL) while True: # wait for an image (could happen at the very beginning when the queue is still empty) while len(self.queue) == 0: time.sleep(0.1) im = self.queue[0] cv2.imshow("display", im) k = cv2.waitKey(6) & 0xFF if k in [27, ord('q')]: rospy.signal_shutdown('Quit') elif k == ord('s'): self.opencv_calibration_node.screendump(im) if self.extra_queue: if len(self.extra_queue): extra_img = self.extra_queue[0] cv2.imshow("extra", extra_img)
def on_mouse(self, event, x, y, flags, param): if event == cv2.EVENT_LBUTTONDOWN and self.displaywidth < x: if self.c.goodenough: if 180 <= y < 280: self.c.do_calibration() if 280 <= y < 380: self.c.do_save() if self.c.calibrated: if 380 <= y < 480: # Only shut down if we set camera info correctly, #3993 if self.do_upload(): rospy.signal_shutdown('Quit')
def __init__(self, limb, joint_names): self._joint_names = joint_names ns = 'robot/limb/' + limb + '/' self._client = actionlib.SimpleActionClient( ns + "follow_joint_trajectory", FollowJointTrajectoryAction, ) self._goal = FollowJointTrajectoryGoal() self._goal_time_tolerance = rospy.Time(0.1) self._goal.goal_time_tolerance = self._goal_time_tolerance server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0)) if not server_up: rospy.logerr("Timed out waiting for Joint Trajectory" " Action Server to connect. Start the action server" " before running example.") rospy.signal_shutdown("Timed out waiting for Action Server") sys.exit(1) self.clear(limb)
def wobble(self): self.set_neutral() """ Performs the wobbling """ command_rate = rospy.Rate(1) control_rate = rospy.Rate(100) start = rospy.get_time() while not rospy.is_shutdown() and (rospy.get_time() - start < 10.0): angle = random.uniform(-2.0, 0.95) while (not rospy.is_shutdown() and not (abs(self._head.pan() - angle) <= intera_interface.HEAD_PAN_ANGLE_TOLERANCE)): self._head.set_pan(angle, speed=0.3, timeout=0) control_rate.sleep() command_rate.sleep() self._done = True rospy.signal_shutdown("Example finished.")
def __init__(self, limb="right"): """ @param limb: Limb to run CalibrateArm on arm side. """ self._limb=limb self._client = actionlib.SimpleActionClient('/calibration_command', CalibrationCommandAction) # Waits until the action server has started up and started # listening for goals. server_up = self._client.wait_for_server(rospy.Duration(10.0)) if not server_up: rospy.logerr("Timed out waiting for Calibration" " Server to connect. Check your ROS networking" " and/or reboot the robot.") rospy.signal_shutdown("Timed out waiting for Action Server") sys.exit(1)
def __init__(self): # ROS initialization: rospy.init_node('pose_manager') self.poseLibrary = dict() self.readInPoses() self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction, execute_cb=self.executeBodyPose, auto_start=False) self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction) if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)): try: rospy.wait_for_service("stop_walk_srv", timeout=2.0) self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty) except: rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. " +"This is normal if there is no nao_walker running.") self.stopWalkSrv = None self.poseServer.start() rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys()); else: rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?"); rospy.signal_shutdown("Required component missing");
def main(): rospy.init_node("evaluation_ac") ac = ACControllerSimulator() rospy.Subscriber("/aide/ac_control", Bool, callback=lambda msg: ac.increase_ac() if msg.data else ac.decrease_ac()) console = Console() console.preprocess = lambda source: source[3:] atexit.register(loginfo, "Going down by user-input.") ac_thread = Thread(target=ac.simulate) ac_thread.daemon = True pub_thread = Thread(target=publish, args=(ac, )) pub_thread.daemon = True pub_thread.start() while not rospy.is_shutdown(): try: command = console.raw_input("ac> ") if command == "start": ac_thread.start() if command == "end": return except EOFError: print("") ac.finished = True rospy.signal_shutdown("Going down.")
def img_callback(self, image): try: inImg = self.bridge.imgmsg_to_cv2(image, 'bgr8') except CvBridgeError, e: print e inImgarr = np.array(inImg) self.outImg = self.process_image(inImgarr) # self.train_img_pub.publish(self.bridge.cv2_to_imgmsg(self.outImg, "bgr8")) # cv2.namedWindow("Capture Face") cv2.imshow('Capture Face', self.outImg) cv2.waitKey(3) if self.count == 100*self.cp_rate: rospy.loginfo("Data Captured!") rospy.loginfo("Training Data...") self.fisher_train_data() rospy.signal_shutdown('done')
def __init__(self, limb): ns = 'robot/limb/' + limb + '/' self._client = actionlib.SimpleActionClient( ns + "follow_joint_trajectory", FollowJointTrajectoryAction, ) self._goal = FollowJointTrajectoryGoal() self._goal_time_tolerance = rospy.Time(0.1) self._goal.goal_time_tolerance = self._goal_time_tolerance server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0)) if not server_up: rospy.logerr("Timed out waiting for Joint Trajectory" " Action Server to connect. Start the action server" " before running example.") rospy.signal_shutdown("Timed out waiting for Action Server") sys.exit(1) self.clear(limb)
def __init__(self, limb): ns = 'robot/limb/' + limb + '/' self._client = actionlib.SimpleActionClient( ns + "follow_joint_trajectory", FollowJointTrajectoryAction, ) self._goal = FollowJointTrajectoryGoal() self._goal_time_tolerance = rospy.Time(0.1) self._goal.goal_time_tolerance = self._goal_time_tolerance server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0)) if not server_up: rospy.logerr("Timed out waiting for Joint Trajectory" " Action Server to connect. Start the action server" " before running example.") rospy.signal_shutdown("Timed out waiting for Action Server") sys.exit(1) self._sub_path = rospy.Subscriber('joint_path', numpy_msg(Float32MultiArray), self.path_cb) self._sub_traj = rospy.Subscriber('joint_traj', numpy_msg(Float32MultiArray), self.traj_cb) self._limb_interface = baxter_interface.limb.Limb('right') self._q_start = np.array([-0.22281071, -0.36470393, 0.36163597, 1.71920897, -0.82719914, -1.16889336, -0.90888362]) self.clear(limb)
def stop(self): if self.show_plots: import pyqtgraph pyqtgraph.QtGui.QApplication.quit() else: rospy.signal_shutdown('User request')
def stop(self): rospy.signal_shutdown('User request')
def display(win_name, img): cv2.namedWindow(win_name, cv2.WINDOW_NORMAL) cv2.imshow( win_name, numpy.asarray( img[:,:] )) k=-1 while k ==-1: k=waitkey() cv2.destroyWindow(win_name) if k in [27, ord('q')]: rospy.signal_shutdown('Quit')
def on_closing(): if msg.askokcancel("Warning", "Do you want to quit?"): background.destroy() rospy.signal_shutdown("")
def handleCmdVel(self, data): rospy.logdebug("Walk cmd_vel: %f %f %f, frequency %f", data.linear.x, data.linear.y, data.angular.z, self.stepFrequency) if data.linear.x != 0 or data.linear.y != 0 or data.angular.z != 0: self.gotoStartWalkPose() try: eps = 1e-3 # maybe 0,0,0 is a special command in motionProxy... if abs(data.linear.x)<eps and abs(data.linear.y)<eps and abs(data.angular.z)<eps: self.motionProxy.moveToward(0,0,0,[["Frequency",0.5]]) else: if data.linear.x>=eps and abs(data.linear.y)<eps and abs(data.angular.z)<eps: self.motionProxy.angleInterpolationWithSpeed("HeadPitch", self.headPitchAngle, 0.5) self.motionProxy.moveToward(data.linear.x, data.linear.y, data.angular.z, [["Frequency", self.stepFrequency]] + self.footGaitConfig) rospy.loginfo('case forward') elif data.linear.x<=-eps and abs(data.linear.y)<eps and abs(data.angular.z)<eps: self.motionProxy.angleInterpolationWithSpeed("HeadPitch", self.headPitchAngleBack, 0.5) self.motionProxy.moveToward(data.linear.x, data.linear.y, data.angular.z, [["Frequency", self.stepFrequencyBack]] + self.footGaitConfigBack) rospy.loginfo('case backward') elif abs(data.linear.x)<eps and abs(data.linear.y)<eps and data.angular.z>=-eps: self.motionProxy.angleInterpolationWithSpeed("HeadPitch", self.headPitchAngleLeft, 0.5) self.motionProxy.moveToward(data.linear.x, data.linear.y, data.angular.z, [["Frequency", self.stepFrequencyLeft]] + self.footGaitConfigLeft) rospy.loginfo('case left') elif abs(data.linear.x)<eps and abs(data.linear.y)<eps and data.angular.z<=eps: self.motionProxy.angleInterpolationWithSpeed("HeadPitch", self.headPitchAngleRight, 0.5) self.motionProxy.moveToward(data.linear.x, data.linear.y, data.angular.z, [["Frequency", self.stepFrequencyRight]] + self.footGaitConfigRight) rospy.loginfo('case right') else: self.motionProxy.angleInterpolationWithSpeed("HeadPitch", self.headPitchAngle, 0.5) self.motionProxy.moveToward(data.linear.x, data.linear.y, data.angular.z, [["Frequency", self.stepFrequency]] + self.footGaitConfig) rospy.loginfo('case else') except RuntimeError,e: # We have to assume there's no NaoQI running anymore => exit! rospy.logerr("Exception caught in handleCmdVel:\n%s", e) rospy.signal_shutdown("No NaoQI available anymore")
def make_model(self): # create the base pre-trained model if self.model_architecture == 'vgg16': from keras.applications.vgg16 import VGG16 self.base_model = VGG16(weights='imagenet', include_top=False) elif self.model_architecture == 'resnet': from keras.applications.resnet50 import ResNet50 self.base_model = ResNet50(weights='imagenet', include_top=False) elif self.model_architecture == 'inception': from keras.applications.inception_v3 import InceptionV3 self.base_model = InceptionV3(weights='imagenet', include_top=False) else: print 'Model architecture parameter unknown. Options are: vgg16, resnet, and inception' rospy.signal_shutdown("Model architecture unknown.") # now we add a new dense layer to the end of the network inplace of the old layers x = self.base_model.output x = GlobalAveragePooling2D()(x) x = Dense(1024, activation='relu')(x) # add the outplut layer predictions = Dense(len(self.categories.keys()), activation='softmax')(x) # create new model composed of pre-trained network and new final layers # if you want to change the input size, you can do this with the input parameter below self.model = Model(input=self.base_model.input, output=predictions) # now we go through and freeze all of the layers that were pretrained for layer in self.base_model.layers: layer.trainable = False if self.verbose: print 'compiling model ... ' start_time = time.time() # in finetuning, these parameters can matter a lot, it is wise to observe # how well your model is learning for this to work well self.model.compile(optimizer=self.optimizer, loss='categorical_crossentropy', metrics=['accuracy']) if self.verbose: end_time = time.time() self.print_time(start_time,end_time,'compiling model')
def process_scan(self, m): """ This function is the callback for our laser subscriber. """ currDist = m.ranges[270] # check if the neato has passed the spot. If not, move forward if self.timestamp2 is None: self.twist = FORWARD # if thers is no distance to neato stored, store the lidar reading # as distance to neato if self.dist2Neato is None: self.dist2Neato = currDist print "dist2Neato: ", self.dist2Neato # else check whether the current lidar reading is within the range of wall distance elif currDist > (self.dist2Neato + LENGTH_OF_SPOT - .05): # if so, update radius and set the first timestamp self.dist2Wall = currDist self.radius = (self.dist2Wall/2.0) if self.timestamp1 is None: self.timestamp1 = rospy.Time.now() print "TIME1: ", self.timestamp1 # if the first timestamp has been set, check if the current lidar reading is # close enough to dis2neato, we assume the neato has passed the spot if abs(currDist - self.dist2Neato) <= 0.2 and self.timestamp1 is not None: self.timestamp2 = rospy.Time.now() print "TIME2: ", self.timestamp2 self.twist = STOP # If we have both timestamps, we can determine the width of the spot, and begin parking. elif self.timestamp1 is not None and self.timestamp2 is not None: self.adjustment = 0.2 if self.is_parallel else -0.1 self.widthOfSpot = SPEED * (self.timestamp2.secs - self.timestamp1.secs) if self.dist2Neato >= 0.3 and not self.isAligned: #determine later what the threshold should be self.align_with_origin() self.park() rospy.signal_shutdown("Done parking.") elif self.dist2Neato < 0.3: print "Neato was too close to park!"
def done(self): self.sub.unregister() rospy.signal_shutdown("done")
def main(): rospy.init_node("input_reader") pub = rospy.Publisher("/aide/console_input", String, queue_size=42) console = HistoryConsole() console.preprocess = lambda source: source[7:] atexit.register(loginfo, "Going down by user-input.") while not rospy.is_shutdown(): try: command = console.raw_input("aide> ") pub.publish(command) except EOFError: print("") rospy.signal_shutdown("Going down.")
def main(): rospy.init_node("evaluation_building") building = SmartBuildingSimulator() rospy.Subscriber("/aide/building_control", String, callback=lambda msg: building.handle_incoming(msg.data)) console = Console() console.preprocess = lambda source: source[3:] atexit.register(loginfo, "Going down by user-input.") building_thread = Thread(target=building.start_simulation) building_thread.daemon = True pub_thread = Thread(target=publish, args=(building,)) pub_thread.daemon = True pub_thread.start() while not rospy.is_shutdown(): try: command = console.raw_input("sb> ") if command == "start": building_thread.start() if command.startswith("fire"): room = command.split(" ")[1] building.set_on_fire(int(room)) if command == "rescue": building.fire_department_arrived = True if command == "end": return except EOFError: print("") building.finished = True rospy.signal_shutdown("Going down.")
def shutdown_handler(self, signum, frame): print ('smp_thread: Signal handler called with signal', signum) self.isrunning = False # for sub in self.sub: # self.sub.shutdown() # # print sub.unregister() # for pub in self.pub: # self.pub.shutdown() # rospy.signal_shutdown("ending") # sys.exit(0)
def collect_goal_image(self, ind=0): savedir = self.recording_dir + '/goalimage' if not os.path.exists(savedir): os.makedirs(savedir) done = False print("Press g to take goalimage!") while not done and not rospy.is_shutdown(): c = intera_external_devices.getch() if c: # catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Example finished.") if c == 'g': print 'taking goalimage' imagemain = self.recorder.ltob.img_cropped cv2.imwrite( savedir+ "/goal_main{}.png".format(ind), imagemain, [cv2.IMWRITE_PNG_STRATEGY_DEFAULT, 1]) state = self.get_endeffector_pos() with open(savedir + '/goalim{}.pkl'.format(ind), 'wb') as f: cPickle.dump({'main': imagemain, 'state': state}, f) break else: print 'wrong key!' print 'place object in different location!' pdb.set_trace()
def sensor_node(): pub = rospy.Publisher('/sensor_values', Int32MultiArray, queue_size=1) rospy.init_node('sensor_node') rate = rospy.Rate(50) while not rospy.is_shutdown(): with serial.Serial(PORT_NAME, BAUD_RATE, timeout=0.1) as ser: data_canditate = ser.readline().decode('utf-8') # print (data_candidate) data_candidate = ser.readline().decode('utf-8') data_candidate = np.fromstring(data_candidate,sep=' ',dtype=np.uint32) if data_candidate.shape[0] == NUM_SENSORS: values = data_candidate msg = Int32MultiArray( MultiArrayLayout( [MultiArrayDimension('sensor data', NUM_SENSORS, 1)],1),values) # CHANGE the second arg to no. of sensor values read values) pub.publish(msg) else: rospy.signal_shutdown("\n"+"No data available on serial port. Shutting down!"+"\n") print ("\n"+"No data available on serial port. Shutting down!"+"\n") rate.sleep()
def map_keyboard(self): # initialize interfaces print("Getting robot state... ") rs = baxter_intercontrollerface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled limb_0 = baxter_interface.Gripper(self.limb_name, CHECK_VERSION) done = False print("Enabling robot... ") rs.enable() print("Controlling grippers. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: if c in ['\x1b', '\x03']: done = True elif c == 'a': rospy.loginfo("%s is pressed" %c) self.move((0,-1,0),0.05) elif c == 'd': rospy.loginfo("%s is pressed" %c) self.move((0,1,0),0.05) elif c == 'w': rospy.loginfo("%s is pressed" %c) self.move((-1,0,0),0.05) elif c == 's': rospy.loginfo("%s is pressed" %c) self.move((1,0,0),0.05) elif c == 'z': rospy.loginfo("%s is pressed" %c) self.move((0,0,-1),0.05) elif c == 'x': rospy.loginfo("%s is pressed" %c) self.move((0,0,1),0.05) else: print("Not implement it yet...") rospy.signal_shutdown("Move.py finished.")
def keycb(self, tdat): # check if there was a key pressed, and if so, check it's value and # toggle flag: if self.kb.kbhit(): c = self.kb.getch() if ord(c) == 27 or c == 'q': rospy.signal_shutdown("Escape pressed!") else: print c if c == 'c': rospy.loginfo("You pressed 'c'.... calibrating!") self.calibrate_count = 0 self.calibrate_flag = True self.calibrated = False self.trans_arr = np.zeros((self.count, 3)) self.quat_arr = np.zeros((self.count, 4)) self.trans_ave = np.zeros(3) self.quat_ave = np.zeros(3) elif c == 'w': if self.calibrated: rospy.loginfo("Writing launch file") self.write_launch_file() rospy.loginfo("Done writing launch file") else: rospy.logwarn("Not calibrated!") rospy.loginfo("Press 'c' to calibrate") elif c == 'h': print "Press 'c' to calibrate" print "Once successfully calibrated, press 'w' to write to a file" self.kb.flush() # if calibrated, send transform: if self.calibrated: self.send_tranform() return
def callback(data): rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data) rospy.signal_shutdown("Success")
def write_final_tracklet_xml(self): if (not self.tracklet_generated): self.tracklet_saver.write_tracklet() self.tracklet_generated = True print '---------tracklet exported------------------' rospy.signal_shutdown('Tracklet exported')
def finish(): """ Close ROS node """ print("\nFinishing ROS Node \n") rospy.signal_shutdown("Finished from RL-ROBOT") return
def stop_ros(reason): rospy.signal_shutdown(reason) roscpp_shutdown()
def publish_map(self): """ Publish the map. """ grid_msg = self._map.to_message() self._map_data_pub.publish(grid_msg.info) self._map_pub.publish(grid_msg) # rospy.signal_shutdown("stop spin")
def watchdog(F_max, lmb): rospy.signal_shutdown('cleaning') rospy.init_node('watchdog') while True: try: Fz = baxter_interface.limb.Limb(lmb).endpoint_effort()['force'].z print Fz if abs(Fz) > F_max: rospy.signal_shutdown('Fmax exceeded') except KeyError: pass
def goer(z_offset, lmb): rospy.signal_shutdown('cleaning') rospy.init_node('goer') go_relative(lmb, dz=z_offset) rospy.signal_shutdown('done moving')
def __init__(self): # create subscribers, timers, clients, etc. try: rospy.wait_for_service("/check_state_validity", timeout=5) except ROSException: rospy.logwarn("[check_collisions_node] Done waiting for /check_state_validity service... service not found") rospy.logwarn("shutting down...") rospy.signal_shutdown("service unavailable") except ROSInterruptException: pass self.coll_client = rospy.ServiceProxy("check_state_validity", GetStateValidity) self.js_sub = rospy.Subscriber("joint_state_check", numpy_msg(Float32MultiArray), self.js_cb) self.js_pub = rospy.Publisher("collision_check", Int16, queue_size = 10) return
def __init__(self): self.lock = threading.Lock() rospy.init_node("roboclaw_node",log_level=rospy.DEBUG) rospy.on_shutdown(self.shutdown) rospy.loginfo("Connecting to roboclaw") self.wheels_speeds_pub = rospy.Publisher('/motors/commanded_speeds', Wheels_speeds, queue_size=1) self.motors_currents_pub = rospy.Publisher('/motors/read_currents', Motors_currents, queue_size=1) self.address = int(rospy.get_param("~address", "128")) if self.address > 0x87 or self.address < 0x80: rospy.logfatal("Address out of range") rospy.signal_shutdown("Address out of range") # TODO need someway to check if address is correct self.MAX_ABS_LINEAR_SPEED = float(rospy.get_param("~max_abs_linear_speed", "1.0")) self.MAX_ABS_ANGULAR_SPEED = float(rospy.get_param("~max_abs_angular_speed", "1.0")) self.TICKS_PER_METER = float(rospy.get_param("~ticks_per_meter", "4342.2")) self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.315")) self.ACC_LIM = float(rospy.get_param("~acc_lim", "0.1")) self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH) self.last_set_speed_time = rospy.get_rostime() rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=1) rospy.sleep(1) rospy.logdebug("address %d", self.address) rospy.logdebug("max_abs_linear_speed %f", self.MAX_ABS_LINEAR_SPEED) rospy.logdebug("max_abs_angular_speed %f", self.MAX_ABS_ANGULAR_SPEED) rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER) rospy.logdebug("base_width %f", self.BASE_WIDTH)
def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0, pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0): if service_check: # assume any non-default service names have been set. Wait for the service to become ready for svcname in ["camera", "left_camera", "right_camera"]: remapped = rospy.remap_name(svcname) if remapped != svcname: fullservicename = "%s/set_camera_info" % remapped print("Waiting for service", fullservicename, "...") try: rospy.wait_for_service(fullservicename, 5) print("OK") except rospy.ROSException: print("Service not found") rospy.signal_shutdown('Quit') self._boards = boards self._calib_flags = flags self._checkerboard_flags = checkerboard_flags self._pattern = pattern self._camera_name = camera_name lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image) rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image) ts = synchronizer([lsub, rsub], 4) ts.registerCallback(self.queue_stereo) msub = message_filters.Subscriber('image', sensor_msgs.msg.Image) msub.registerCallback(self.queue_monocular) self.set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo) self.set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo) self.set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo) self.q_mono = deque([], 1) self.q_stereo = deque([], 1) self.c = None mth = ConsumerThread(self.q_mono, self.handle_monocular) mth.setDaemon(True) mth.start() sth = ConsumerThread(self.q_stereo, self.handle_stereo) sth.setDaemon(True) sth.start()
def map_keyboard(limb): # initialize interfaces print("Getting robot state...") rs = intera_interface.RobotEnable(CHECK_VERSION) init_state = rs.state() try: gripper = intera_interface.Gripper(limb) except ValueError: rospy.logerr("Could not detect a gripper attached to the robot.") return def clean_shutdown(): print("Exiting example.") rospy.on_shutdown(clean_shutdown) def offset_position(offset): current = gripper.get_position() gripper.set_position(current + offset) def offset_holding(offset): current = gripper.get_force() gripper.set_holding_force(current + offset) num_steps = 10.0 thirty_percent_velocity = 0.3*(gripper.MAX_VELOCITY - gripper.MIN_VELOCITY) + gripper.MIN_VELOCITY bindings = { # key: (function, args, description) 'r': (gripper.reboot, [], "reboot"), 'c': (gripper.calibrate, [], "calibrate"), 'q': (gripper.close, [], "close"), 'o': (gripper.open, [], "open"), '+': (gripper.set_velocity, [gripper.MAX_VELOCITY], "set 100% velocity"), '-': (gripper.set_velocity, [thirty_percent_velocity], "set 30% velocity"), 's': (gripper.stop, [], "stop"), 'h': (offset_holding, [-(gripper.MAX_FORCE / num_steps)], "decrease holding force"), 'j': (offset_holding, [gripper.MAX_FORCE / num_steps], "increase holding force"), 'u': (offset_position, [-(gripper.MAX_POSITION / num_steps)], "decrease position"), 'i': (offset_position, [gripper.MAX_POSITION / num_steps], "increase position"), } done = False rospy.loginfo("Enabling robot...") rs.enable() print("Controlling grippers. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = intera_external_devices.getch() if c: if c in ['\x1b', '\x03']: done = True elif c in bindings: cmd = bindings[c] cmd[0](*cmd[1]) print("command: %s" % (cmd[2],)) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(bindings.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2])) # force shutdown call if caught by key handler rospy.signal_shutdown("Example finished.")
def __init__(self, limb="right"): #create our action server clients self._limb_client = actionlib.SimpleActionClient( 'robot/limb/right/follow_joint_trajectory', FollowJointTrajectoryAction, ) #verify joint trajectory action servers are available is_server_up = self._limb_client.wait_for_server(rospy.Duration(10.0)) if not is_server_up: msg = ("Action server not available." " Verify action server availability.") rospy.logerr(msg) rospy.signal_shutdown(msg) sys.exit(1) #create our goal request self.goal = FollowJointTrajectoryGoal() #limb interface - current angles needed for start move self.arm = intera_interface.Limb(limb) self.limb = limb self.gripper_name = '_'.join([limb, 'gripper']) #gripper interface - for gripper command playback try: self.gripper = intera_interface.Gripper(limb) except: self.gripper = None rospy.loginfo("Did not detect a connected electric gripper.") #flag to signify the arm trajectories have begun executing self._arm_trajectory_started = False #reentrant lock to prevent same-thread lockout self._lock = threading.RLock() # Verify Grippers Have No Errors and are Calibrated if self.gripper: if self.gripper.has_error(): self.gripper.reboot() if not self.gripper.is_calibrated(): self.gripper.calibrate() #gripper goal trajectories self.grip = FollowJointTrajectoryGoal() #gripper control rate self._gripper_rate = 20.0 # Hz # Timing offset to prevent gripper playback before trajectory has started self._slow_move_offset = 0.0 self._trajectory_start_offset = rospy.Duration(0.0) self._trajectory_actual_offset = rospy.Duration(0.0) #param namespace self._param_ns = '/rsdk_joint_trajectory_action_server/'
def run(self): """Generic run method for learners""" print("starting") while self.isrunning: # print "smp_thread: running" # call any local computations self.local_hooks() # prepare input for local conditions self.prepare_inputs() # execute network / controller # FIXME: callback count based learning control: washout, learning, testing, # dynamic switching and learning rate modulation if self.cnt_main > self.cfg.lag: (z, zn) = self.controller() # (z, zn) = (0., 0.) else: z = np.zeros_like(self.iosm.z) zn = np.zeros_like(self.iosm.z) # print "z/zn.shape", self.iosm.z.shape, self.iosm.zn.shape # local: adjust generic network output to local conditions self.prepare_output(z, zn) # post hooks self.local_post_hooks() # write to memory self.memory_pushback() # publish all state data self.pub_all() # count self.cnt_main += 1 # incr counter # check if finished if self.cnt_main == self.cfg.len_episode: # self.savelogs() self.isrunning = False # generates problem with batch mode rospy.signal_shutdown("ending") print("ending") # time.sleep(self.loop_time) self.rate.sleep() # class Terminator(object): # def __init__(self): # signal.signal(signal.SIGINT, self.handler) # # pass # def handler(self, signum, frame): # print ('class Signal handler called with signal', signum) # # al.savelogs() # l.isrunning = False # rospy.signal_shutdown("ending") # sys.exit(0) # # raise IOError("Couldn't open device!")
def __init__(self, direction): if direction not in [RIGHT, LEFT]: rospy.loginfo("incorect %s wall selected. choose left or right") rospy.signal_shutdown() self.direction = direction if SHOW_VIS: self.viz = DynamicPlot() self.viz.initialize() self.pub = rospy.Publisher("/vesc/high_level/ackermann_cmd_mux/input/nav_0",\ AckermannDriveStamped, queue_size =1 ) self.sub = rospy.Subscriber("/scan", LaserScan, self.lidarCB, queue_size=1) if PUBLISH_LINE: self.line_pub = rospy.Publisher("/viz/line_fit", PolygonStamped, queue_size =1 ) # computed control instructions self.control = None self.steering_hist = CircularArray(HISTORY_SIZE) # containers for laser scanner related data self.data = None self.xs = None self.ys = None self.m = 0 self.c = 0 # flag to indicate the first laser scan has been received self.received_data = False # cached constants self.min_angle = None self.max_angle = None self.direction_muliplier = 0 self.laser_angles = None self.drive_thread = Thread(target=self.apply_control) self.drive_thread.start() if SHOW_VIS: while not rospy.is_shutdown(): self.viz_loop() rospy.sleep(0.1)
def callback(self, data): """ Called every time a new image is received from the camera. Find the target object in the image, mark it, and display the annotated image. Listen for user input. When directed, move to the target object, pick it up, and bring it back to a preset location. """ # Convert ROS image message to opencv format try: cv_img = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError, e: print e # Find target object [(cx,cy),approx,self.angle] = self.finder.find_thing(cv_img) if cx != -1: # Annotate camera image purple = [150, 0, 150] red = [0, 0, 255] blue = [255, 0, 0] cv2.drawContours(cv_img, approx, -1, purple, 8) cv2.circle(cv_img, (cx, cy), 5, red, -1) cv2.circle(cv_img, (self.gx, self.gy), 5, blue, -8) # Over the course of multiple loops through, # move gripper to be centered over the object, # then pick up and move the object self.move_center(cx, cy) # Briefly display annotated camera image cv2.imshow("Object search", cv_img) key = cv2.waitKey(3) & 0xFF if key == ord('x'): # Exit rospy.signal_shutdown('User shutdown') elif key == ord('n'): # Find and move object again self.head.command_nod(0) time.sleep(0.5) self.finished = 0 self.angle_corrected = False self.moving = False elif key == ord('d'): # Refind distance to table self.head.command_nod(0) time.sleep(0.5) self.head.command_nod(0) self.dist = float(baxter_interface.analog_io.AnalogIO( arm + '_hand_range').state() / 1000.0)
def __init__(self): # Give the node a name rospy.init_node('quat_to_angle', anonymous=False) # Publisher to control the robot's speed self.turtlebot_angle = rospy.Publisher('/turtlebot_angle', Float64, queue_size=5) self.turtlebot_posex = rospy.Publisher('/turtlebot_posex', Float64, queue_size=5) self.turtlebot_posey = rospy.Publisher('/turtlebot_posey', Float64, queue_size=5) #goal.target_pose.pose = Pose(Point(float(data["point"]["x"]), float(data["point"]["y"]), float(data["point"]["z"])), Quaternion(float(data["quat"]["x"]), float(data["quat"]["y"]), float(data["quat"]["z"]), float(data["quat"]["w"]))) # How fast will we update the robot's movement? rate = 20 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Set the odom frame self.odom_frame = '/odom' # Find out if the robot uses /base_link or /base_footprint try: self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_footprint' except (tf.Exception, tf.ConnectivityException, tf.LookupException): try: self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_link' except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") rospy.signal_shutdown("tf Exception") # Initialize the position variable as a Point type position = Point() while not rospy.is_shutdown(): (position, rotation) = self.get_odom() print(position) self.turtlebot_angle.publish(rotation) #print(str(position).split('x: ')[1].split('\ny:')[0]) x = float(str(position).split('x: ')[1].split('\ny:')[0]) y = float(str(position).split('y: ')[1].split('\nz:')[0]) self.turtlebot_posex.publish(x) self.turtlebot_posey.publish(y) #print(rotation) rospy.sleep(5)
