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
python类is_shutdown()的实例源码
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_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 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 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)
