我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用rospy.is_shutdown()。
def talker(): pub = rospy.Publisher('chatter', String, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): hello_str = "hello world %s" % rospy.get_time() rospy.loginfo(hello_str) pub.publish(hello_str) rate.sleep() # rate = rospy.Rate(10) # hello_str = "hello world" # rospy.loginfo(hello_str) # pub.publish(hello_str) # rospy.spin() # exit(0)
def main(): rospy.init_node("whatsapp_service") cred = credentials.WHATSAPP stackBuilder = YowStackBuilder() stack = (stackBuilder .pushDefaultLayers(True) .push(AideRosLayer) .build()) loginfo("Stack built...") stack.setCredentials(cred) stack.broadcastEvent(YowLayerEvent(YowNetworkLayer.EVENT_STATE_CONNECT)) # sending the connect signal loginfo("Connected...") atexit.register(lambda: stack.broadcastEvent(YowLayerEvent(YowNetworkLayer.EVENT_STATE_DISCONNECT))) th = threading.Thread(target=stack.loop) th.daemon = True th.start() loginfo("Running in background.") loginfo("All done. spinning.") while not rospy.is_shutdown(): rospy.spin()
def keyboard_loop(self): while not rospy.is_shutdown(): acc = 0 yaw = 0 keys = pygame.key.get_pressed() for event in pygame.event.get(): if event.type==pygame.QUIT: sys.exit() if(keys[pygame.K_UP]): acc = self.acc elif(keys[pygame.K_DOWN]): acc = -self.acc if(keys[pygame.K_LEFT]): yaw = self.yaw elif(keys[pygame.K_RIGHT]): yaw = -self.yaw self.send_control(acc, yaw) self.rate.sleep()
def record_motion(self): for countdown in ['ready?', 3, 2, 1, "go"]: self.say('{}'.format(countdown), blocking=False) rospy.sleep(1) self.arm.recorder.start(10) rospy.loginfo("Recording...") choice = "" while choice != 'stop' and not rospy.is_shutdown(): choice = self.read_user_input(['stop']) joints, eef = self.arm.recorder.stop() self.say('Recorded', blocking=True) if len(joints.joint_trajectory.points) == 0: self.say('This demo is empty, please retry') else: target_id = self.promp.add_demonstration(joints, eef) self.say('Added to Pro MP {}'.format(target_id), blocking=False)
def main(args): rospy.init_node("publish_calib_file", args) files = glob.glob("left-*.png"); files.sort() print("All {} files".format(len(files))) image_pub = rospy.Publisher("image",Image, queue_size=10) bridge = CvBridge(); for f in files: if rospy.is_shutdown(): break raw_input("Publish {}?".format(f)) img = cv2.imread(f, 0) image_pub.publish(bridge.cv2_to_imgmsg(img, "mono8"))
def attach_springs(self): """ Switches to joint torque mode and attached joint springs to current joint positions. """ # record initial joint angles self._start_angles = self._limb.joint_angles() # set control rate control_rate = rospy.Rate(self._rate) # for safety purposes, set the control rate command timeout. # if the specified number of command cycles are missed, the robot # will timeout and return to Position Control Mode self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds) # loop at specified rate commanding new joint torques while not rospy.is_shutdown(): if not self._rs.state().enabled: rospy.logerr("Joint torque example failed to meet " "specified control rate timeout.") break self._update_forces() control_rate.sleep()
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 revalidate(self, timeout, invalidate_state=True, invalidate_config=True): """ invalidate the state and config topics, then wait up to timeout seconds for them to become valid again. return true if both the state and config topic data are valid """ if invalidate_state: self.invalidate_state() if invalidate_config: self.invalidate_config() timeout_time = rospy.Time.now() + rospy.Duration(timeout) while not self.is_state_valid() and not rospy.is_shutdown(): rospy.sleep(0.1) if timeout_time < rospy.Time.now(): rospy.logwarn("Timed out waiting for node interface valid...") return False return True
def robot_listener(self): ''' rospy.wait_for_service('spawn') spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) spawner(4, 2, 0, 'turtle2') ''' robot_vel_pub = rospy.Publisher('robot_0/cmd_vel', geometry_msgs.msg.Twist, queue_size=1) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: (trans,rot) = self.listener.lookupTransform('/robot_3', '/robot_0', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue angular = 4 * math.atan2(trans[1], trans[0]) linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2) cmd = geometry_msgs.msg.Twist() cmd.linear.x = linear cmd.angular.z = angular robot_vel_pub.publish(cmd) rate.sleep()
def compute_static_stability_thread(): rate = rospy.Rate(60) global kron_com_vertices while not rospy.is_shutdown(): if 'COM-static' in support_area_handles \ and not gui.show_com_support_area: delete_support_area_display('COM-static') if not motion_plan or not motion_plan.started \ or 'COM-static' in support_area_handles \ or not gui.show_com_support_area: rate.sleep() continue color = (0.1, 0.1, 0.1, 0.5) req = contact_stability.srv.StaticAreaRequest( contacts=convert_contacts_to_ros(motion_plan.cur_stance.contacts), mass=robot.mass, z_out=motion_plan.com_height) try: res = compute_com_area(req) vertices = [array([v.x, v.y, v.z]) for v in res.vertices] update_support_area_display('COM-static', vertices, [], color) except rospy.ServiceException: delete_support_area_display('COM-static') rate.sleep()
def monitor(self): # Wait until subscriber on instruct message is present notified = False while not rospy.is_shutdown(): _, subscribers, _ = Master('/needybot').getSystemState() if dict(subscribers).get(nb_channels.Messages.instruct.value) is not None: if self.is_connected == False: self.connected_pub.publish() self.is_connected = True else: if self.is_connected or not notified: notified = True self.disconnected_pub.publish() self.is_connected = False rospy.sleep(0.1)
def get_cylinder_status(self): self.cylinder_laser_client.wait_for_service() self.cylinder_opencv_client.wait_for_service() flag = 0 r = rospy.Rate(2) while not rospy.is_shutdown() and flag != 1: res_opencv = self.cylinder_opencv_client() res_laser = self.cylinder_laser_client() x_laser = res_laser.x theta_laser = res_laser.theta theta_opencv = res_opencv.theta if abs(theta_laser - theta_opencv) < 0.01: flag = 1 break r.sleep() return (x_laser,theta_laser)
def go_to(self, radius, angular, mode): # ?????????? symbol_y,symbol_w = self.MODE[mode] ang_has_moved = 0.0 self.reach_goal = False move_vel = g_msgs.Twist() start_yaw = self.get_position.get_robot_current_w() while not rospy.is_shutdown() and self.reach_goal != True: current_yaw = self.get_position.get_robot_current_w() ang_has_moved += abs(abs(current_yaw) - abs(start_yaw)) start_yaw = current_yaw if abs(ang_has_moved - abs(angular)) <= self.stop_tolerance: self.reach_goal = True break move_vel.linear.y = self.move_speed*symbol_y # ???????, ???? dw*dt = dt*atan2(dv*dt/2.0, radius) move_vel.angular.z = self.rate*atan2(self.move_speed/self.rate,radius)*symbol_w print ang_has_moved self.move_cmd_pub.publish(move_vel) self.R.sleep() self.brake() print ang_has_moved
def correct_angle(self): # rospy.loginfo("sadasdafasf") (x,theta,if_close_line) = self.close_line_cmd.find_line() r = rospy.Rate(50) move_velocity = g_msgs.Twist() if theta > 0: move_velocity.angular.z = -0.10 else: move_velocity.angular.z = 0.10 while not rospy.is_shutdown(): (x,theta,if_close_line) = self.close_line_cmd.find_line() self.cmd_move_pub.publish(move_velocity) #???????????????? if theta < 0.02 and theta > -0.02: rospy.loginfo("will Stop!!!!!!!!!!") self.brake() break r.sleep() #??3??????
def go_to_home(self): (x,theta,if_close_line) = self.close_line_cmd.find_line() r = rospy.Rate(50) move_velocity = g_msgs.Twist() while not rospy.is_shutdown(): (x,theta,if_close_line) = self.close_line_cmd.find_line() move_velocity.linear.y = -0.3 self.cmd_move_pub.publish(move_velocity) rospy.loginfo("python: y = %s",move_velocity.linear.y) if if_close_line != 0: rospy.loginfo("will Stop!!!!!!!!!!") self.brake() break r.sleep() #????????
def turn(self, goal_angular): # ???????????????,?????????????? # ????,???? rospy.loginfo('[robot_move_pkg]->move_in_robot.turn will turn %s'%goal_angular) current_angular = start_angular = self.robot_state.get_robot_current_w()#?????????? r = rospy.Rate(100) delta_angular = current_angular - start_angular move_velocity = g_msgs.Twist() while not rospy.is_shutdown() : if abs(delta_angular - abs(goal_angular)) < self.turn_move_stop_tolerance: break current_angular = self.robot_state.get_robot_current_w() move_velocity.angular.z = math.copysign(self.w_speed, goal_angular) delta_angular += abs(abs(current_angular) - abs(start_angular) ) start_angular = current_angular self.cmd_move_pub.publish(move_velocity) #??????????? r.sleep() self.brake()
def turn_to(self,goal_angular): rospy.on_shutdown(self.brake) #??????????? current_angular = start_angular = self.robot_state.get_robot_current_w()#?????????? is_arrive_goal = False r = rospy.Rate(100) delta_angular = current_angular - start_angular delta_upper_limit = abs(goal_angular) + 0.02 #???? delta_lower_limit = abs(goal_angular) - 0.02 #???? move_velocity = g_msgs.Twist() while not rospy.is_shutdown() and not is_arrive_goal: if abs(delta_angular)<=delta_upper_limit and abs(delta_angular) >= delta_lower_limit: #???? self.brake() is_arrive_goal = True break current_angular = self.robot_state.get_robot_current_w() if goal_angular > 0: move_velocity.angular.z = 0.1 else: move_velocity.angular.z = -0.1 delta_angular += abs(abs(current_angular) - abs(start_angular) ) start_angular = current_angular self.cmd_move_pub.publish(move_velocity) #??????????? r.sleep() self.brake()
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 waitForInitialPose(self, next_topic, timeout=None): counter = 0 while not rospy.is_shutdown(): counter = counter + 1 if timeout and counter >= timeout: return False try: self.marker_lock.acquire() self.initialize_poses = True topic_suffix = next_topic.split("/")[-1] if self.initial_poses.has_key(topic_suffix): self.pre_pose = PoseStamped(pose=self.initial_poses[topic_suffix]) self.initialize_poses = False return True else: rospy.logdebug(self.initial_poses.keys()) rospy.loginfo("Waiting for pose topic of '%s' to be initialized", topic_suffix) rospy.sleep(1) finally: self.marker_lock.release()
def attach_springs(self): """ Switches to joint torque mode and attached joint springs to current joint positions. """ # record initial joint angles self._des_angles = self._limb.joint_angles() # set control rate control_rate = rospy.Rate(self._rate) # for safety purposes, set the control rate command timeout. # if the specified number of command cycles are missed, the robot # will timeout and disable self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds) # loop at specified rate commanding new joint torques while not rospy.is_shutdown(): if not self._rs.state().enabled: rospy.logerr("Joint torque example failed to meet " "specified control rate timeout.") break self._update_forces() control_rate.sleep()
def __init__(self): State.__init__(self, outcomes=['success'], input_keys=['waypoints'], output_keys=['waypoints']) # Create publsher to publish waypoints as pose array so that you can see them in rviz, etc. self.poseArray_publisher = rospy.Publisher('/waypoints', PoseArray, queue_size=1) # Start thread to listen for reset messages to clear the waypoint queue def wait_for_path_reset(): """thread worker function""" global waypoints while not rospy.is_shutdown(): data = rospy.wait_for_message('/path_reset', Empty) rospy.loginfo('Recieved path RESET message') self.initialize_path_queue() rospy.sleep(3) # Wait 3 seconds because `rostopic echo` latches # for three seconds and wait_for_message() in a # loop will see it again. reset_thread = threading.Thread(target=wait_for_path_reset) reset_thread.start()
def zero_sensor(self): rospy.loginfo("Zeroing sensor...") # Wait a little bit until we get a message from the sensor rospy.sleep(1) self.tip_offset, self.inside_offset = (np.zeros_like(self.tip), np.zeros_like(self.inside)) inside_vals, tip_vals = [], [] r = rospy.Rate(10) while not rospy.is_shutdown() and len(inside_vals) < 10: inside, tip = self.inside, self.tip # If there are zero values (most likely becase a message # has not yet been received), skip that. We could also # initialize them with nans to find out if there's a # problem if all(inside) and all(tip): inside_vals.append(inside) tip_vals.append(tip) r.sleep() # Center around 5000, so ranges are similar to when not centering self.inside_offset = np.min(inside_vals, axis=0) - 5000 self.tip_offset = np.min(tip_vals, axis=0) - 5000 rospy.loginfo("Zeroing finished")
def limb_pose(limb_name): """Get limb pose at time of OK cuff button press.""" button = CuffOKButton(limb_name) rate = rospy.Rate(20) # rate at which we check whether button was # pressed or not rospy.loginfo( 'Waiting for %s OK cuff button press to save pose' % limb_name) while not button.pressed and not rospy.is_shutdown(): rate.sleep() joint_pose = baxter_interface.Limb(limb_name).joint_angles() # Now convert joint coordinates to end effector cartesian # coordinates using forward kinematics. kinematics = baxter_kinematics(limb_name) endpoint_pose = kinematics.forward_position_kinematics(joint_pose) # How is this different from # baxter_interface.Limb(limb_name).endpoint_pose() print() print('baxter_interface endpoint pose:') print(baxter_interface.Limb(limb_name).endpoint_pose()) print('pykdl forward kinematics endpoint pose:') print(endpoint_pose) print() return endpoint_pose
def attach_springs(self): self._start_angles = self._get_cmd_positions() control_rate = rospy.Rate(self._rate) # for safety purposes, set the control rate command timeout. # if the specified number of command cycles are missed, the robot # will timeout and disable self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds) error = [self._limb.joint_angles()[key] - self._start_angles[key] for key in self._limb.joint_angles().keys()] self.sum_sqr_error = sum([error[i]**2 for i in range(len(error))]) print ('-------new set of joint position---------') print (self.sum_sqr_error) tolerance = 0.020 # loop at specified rate commanding new joint torques while self.sum_sqr_error>tolerance and not rospy.is_shutdown(): if not self._rs.state().enabled: rospy.logerr("Joint torque example failed to meet " "specified control rate timeout.") break self._update_forces() control_rate.sleep()
def limb_pose(limb_name): """Get limb pose at time of OK cuff button press.""" button = CuffOKButton(limb_name) rate = rospy.Rate(20) # rate at which we check whether button was # pressed or not rospy.loginfo( 'Waiting for %s OK cuff button press to save pose' % limb_name) while not button.pressed and not rospy.is_shutdown(): rate.sleep() joint_pose = baxter_interface.Limb(limb_name).joint_angles() # Now convert joint coordinates to end effector cartesian # coordinates using forward kinematics. kinematics = baxter_kinematics(limb_name) endpoint_pose = kinematics.forward_position_kinematics(joint_pose) #print (endpoint_pose) return endpoint_pose
def _execute_gripper_commands(self): start_time = rospy.get_time() - self._trajectory_actual_offset.to_sec() r_cmd = self._r_grip.trajectory.points l_cmd = self._l_grip.trajectory.points pnt_times = [pnt.time_from_start.to_sec() for pnt in r_cmd] end_time = pnt_times[-1] rate = rospy.Rate(self._gripper_rate) now_from_start = rospy.get_time() - start_time while(now_from_start < end_time + (1.0 / self._gripper_rate) and not rospy.is_shutdown()): idx = bisect(pnt_times, now_from_start) - 1 if self._r_gripper.type() != 'custom': self._r_gripper.command_position(r_cmd[idx].positions[0]) if self._l_gripper.type() != 'custom': self._l_gripper.command_position(l_cmd[idx].positions[0]) rate.sleep() now_from_start = rospy.get_time() - start_time
def goto_cup(self): # self.calibrate_obj_det_pub.publish(True) # # while self.calibrated == False: # pass # # print("Finger Sensors calibrated") rate = rospy.Rate(100) while not rospy.is_shutdown(): try: trans = self.tfBuffer.lookup_transform('root', 'teaCup_position', rospy.Time()) break except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rate.sleep() # continue translation = [trans.transform.translation.x, trans.transform.translation.y, trans.transform.translation.z] rotation = [trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w] pose_value = translation + rotation #second arg=0 (absolute movement), arg = '-r' (relative movement) self.cmmnd_CartesianPosition(pose_value, 0)
def cmmd_touchBlock(self,current_finger_position): ii = 0 rate = rospy.Rate(100) while self.touch_finger_1 != True and not rospy.is_shutdown(): current_finger_position[0] += 5 # slowly close finger_1 until contact is made # print (current_finger_position[0]) self.cmmnd_FingerPosition([current_finger_position[0], current_finger_position[1], current_finger_position[2]]) rate.sleep() self.touch_finger_1 = False while self.touch_finger_2 != True and not rospy.is_shutdown(): current_finger_position[1] += 5 # slowly close finger_1 until contact is made # print (current_finger_position[0]) self.cmmnd_FingerPosition([current_finger_position[0], current_finger_position[1], current_finger_position[2]]) rate.sleep() self.touch_finger_2 = False return current_finger_position
def main(): rospy.init_node('issue_com') pub = rospy.Publisher(POS_COM_TOPIC, PositionCommand, queue_size=10) test_pub = rospy.Publisher(TEST_TOPIC, Empty, queue_size=10) sub = rospy.Subscriber(POS_COM_TOPIC, PositionCommand, listen) #sub = rospy.Subscriber('/joint_states', JointState, listen) pc = PositionCommand() pc.mode = JOINT_SPACE #pc.arm = PositionCommand.LEFT_ARM pc.arm = 1#PositionCommand.RIGHT_ARM pc.data = np.zeros(7) r = rospy.Rate(1) #while not rospy.is_shutdown(): # pub.publish(pc) # r.sleep() # print 'published!' r.sleep() test_pub.publish(Empty()) pub.publish(pc)
def wait_for_human_interaction(self, arm_threshold=1, joystick_threshold=0.15): rospy.loginfo("We are waiting for human interaction...") def detect_arm_variation(): new_effort = np.array(self.topics.torso_l_j.effort) delta = np.absolute(effort - new_effort) return np.amax(delta) > arm_threshold def detect_joy_variation(): return np.amax(np.abs(self.topics.joy1.axes)) > joystick_threshold effort = np.array(self.topics.torso_l_j.effort) rate = rospy.Rate(50) is_joystick_demo = None while not rospy.is_shutdown(): if detect_arm_variation(): is_joystick_demo = False break elif detect_joy_variation(): is_joystick_demo = True break rate.sleep() return is_joystick_demo ################################# Service callbacks
def wait_for_server(self): """Waits until interoperability server is reachable.""" reachable = False rate = rospy.Rate(1) while not reachable and not rospy.is_shutdown(): try: response = requests.get( self.url, timeout=self.timeout, verify=self.verify) response.raise_for_status() reachable = response.ok except requests.ConnectionError: rospy.logwarn_throttle(5.0, "Waiting for server: {}".format( self.url)) except Exception as e: rospy.logerr_throttle( 5.0, "Unexpected error waiting for server: {}, {}".format( self.url, e)) if not reachable: rate.sleep()
def spin(self): # @todo: is this excessively hitting the master? r = rospy.Rate(10.0) while not rospy.is_shutdown(): for srv in self._local_services: srv_uri = self._local_manager.lookup_service(srv) if srv_uri: self._foreign_manager.advertise_service(srv, srv_uri) else: self._foreign_manager.unadvertise_service(srv) for srv in self._foreign_services: srv_uri = self._foreign_manager.lookup_service(srv) if srv_uri: self._local_manager.advertise_service(srv, srv_uri) else: self._local_manager.unadvertise_service(srv) r.sleep() if self._local_manager: self._local_manager.unsubscribe_all() if self._foreign_manager: self._foreign_manager.unsubscribe_all()
def spin(self): reconnect_delay = 1.0 while not rospy.is_shutdown(): try: rospy.loginfo("Connecting to SwiftNav Piksi on port %s" % self.piksi_port) self.connect_piksi() while not rospy.is_shutdown(): rospy.sleep(0.05) if not self.piksi.is_alive(): raise IOError self.diag_updater.update() self.check_timeouts() break # should only happen if rospy is trying to shut down except IOError as e: rospy.logerr("IOError") self.disconnect_piksi() except SystemExit as e: rospy.logerr("Unable to connect to Piksi on port %s" % self.piksi_port) self.disconnect_piksi() except: # catch *all* exceptions e = sys.exc_info()[0] rospy.logerr("Uncaught error: %s" % repr(e)) self.disconnect_piksi() rospy.loginfo("Attempting to reconnect in %fs" % reconnect_delay) rospy.sleep(reconnect_delay)
def speed_converter(): rospy.init_node('wheel_speed', anonymous=True) pub1 = rospy.Publisher('lwheel_vtarget', Float32, queue_size=10) pub2 = rospy.Publisher('rwheel_vtarget', Float32, queue_size=10) rate = rospy.Rate(10) while not rospy.is_shutdown(): rospy.Subscriber('cmd_vel', Twist, callback) s1 = "The left wheel's target speed is %f m/s." % lv s2 = "The right wheel's target speed is %f m/s." % rv rospy.loginfo(s1) rospy.loginfo(s2) pub1.publish(lv) pub2.publish(rv) rate.sleep()
def loop(self): self.r = rospy.Rate(self.rate) while not rospy.is_shutdown(): self.r.sleep()
def spin(self): self.r = rospy.Rate(self.rate) while not rospy.is_shutdown(): # self.l_speed() # self.r_speed() self.r.sleep()
def run(self): while not rospy.is_shutdown(): # publish the speed for each robot if self.start_game: for i in self.robot_pubs: vel = self.update_vel(self.robot_pubs[i]['laneid']) yaw = 0 self.send_control(self.robot_pubs[i]['pub'], vel, yaw) self.rate.sleep()
def run(self): while not rospy.is_shutdown(): if self.start_game: self.updatAccVel() ctr = Twist() ctr.linear.x = self.vel self.robot_pub.publish(ctr) sleep(0.0001)
def run(self): while not rospy.is_shutdown(): if self.start_game: vel = self.update_vel() yaw = self.yaw self.send_control(vel, yaw) self.rate.sleep()
def keyboard_loop(self): while not rospy.is_shutdown(): acc = 0 yaw = 0 keys = pygame.key.get_pressed() for event in pygame.event.get(): if event.type==pygame.QUIT:sys.exit() if(keys[pygame.K_s]): self.send_highway_start(1) if(keys[pygame.K_t]): self.send_highway_start(2) if(keys[pygame.K_UP]): acc = self.acc elif(keys[pygame.K_DOWN]): acc = -self.acc if(keys[pygame.K_LEFT]): yaw = self.yaw elif(keys[pygame.K_RIGHT]): yaw = -self.yaw if(keys[pygame.K_r]): state = 1 self.send_record_state(state) elif(keys[pygame.K_q]): state = 2 self.send_record_state(state) elif(keys[pygame.K_p]): state = 0 self.send_record_state(state) self.send_control(acc, yaw) self.rate.sleep()
def run(self): rate = rospy.Rate(self.hz) while not rospy.is_shutdown(): if self.record_state == 2: print '!! finishes recording path!' self.write2File() break rate.sleep()
def run(self): rate = rospy.Rate(self.hz) while not rospy.is_shutdown(): speed = self.getSpeed() self.sendSpeed(speed) rate.sleep()
def run(self): rate = rospy.Rate(self.hz) while not rospy.is_shutdown(): self.pure_pub.publish(self.msg) rate.sleep()
def main(): rospy.init_node("talker") pub = rospy.Publisher("/chatter_topic", String, queue_size=1) rate = rospy.Rate(10) # 10 Hertz ile çal???yor while not rospy.is_shutdown(): message = "Naber Dünya? %s" % (rospy.get_time()) rospy.loginfo("Mesaj haz?rland?: %s" % message) pub.publish(message) rate.sleep()
def set_goal(self): if self.promp.num_primitives > 0: self.say('Move the robot and say ready to set the goal') choice = "" while choice != 'ready' and not rospy.is_shutdown(): choice = self.read_user_input(['ready']) eef = self.arm.endpoint_pose() state = self.arm.get_current_state() goal_set = self.promp.set_goal(eef, state) for result in self.promp.goal_log: rospy.loginfo(result) if goal_set: self.say('I can reach this object, let me demonstrate', blocking=False) self.arm.move_to_controlled(self.init) self.arm.open() trajectory = self.promp.generate_trajectory() self.arm.execute(trajectory) self.arm.close() self.arm.translate_to_cartesian([0, 0, 0.2], 'base', 2) if self.arm.gripping(): self.say('Take it!') self.arm.wait_for_human_grasp(ignore_gripping=False) self.arm.open() else: self.say("I don't know how to reach this object. {}".format(self.promp.status_writing)) else: self.say('There is no demonstration yet, please record at least one demo')
def run(self): try: while not rospy.is_shutdown(): self.arm.move_to_controlled(self.init) if len(self.promp.need_demonstrations()) > 0 or self.promp.num_primitives == 0: needs_demo = 0 if self.promp.num_primitives == 0 else self.promp.need_demonstrations().keys()[0] self.say("Record a demo for Pro MP {}".format(needs_demo)) if self.promp.num_demos == 0: self.say("Say stop to finish") self.record_motion() else: self.say('Do you want to record a motion or set a new goal?') choice = self.read_user_input(['record', 'goal']) if choice == 'record': self.record_motion() elif choice == 'goal': self.set_goal() if not rospy.is_shutdown(): self.say('There are {} primitive{} and {} demonstration{}'.format(self.promp.num_primitives, 's' if self.promp.num_primitives > 1 else '', self.promp.num_demos, 's' if self.promp.num_demos > 1 else '')) finally: self.promp.plot_demos() self.promp.close() print("Your dataset has ID {} at {}".format(self.promp.id, self.promp.dataset_path))
def run(self): while not rospy.is_shutdown(): while not rospy.is_shutdown(): m = self.queue.get() if self.queue.empty(): break self.function(m)
