def execute(self, userdata):
if not self.d:
f = open( self.fname, 'r' )
self.d = yaml.load( f )
self.size = len( self.d['servo_cap']['captures'] )
self.ind = 0
f.close()
if self.ind < self.size:
rospy.logout( 'YamlProc: issuing %d of %d' % (self.ind+1, self.size))
ps = PoseStamped()
ps.header.frame_id = '/map'
ps.header.stamp = rospy.Time(0)
ps.pose.position.x = self.d['servo_cap']['captures'][ self.ind ][0]
ps.pose.position.y = self.d['servo_cap']['captures'][ self.ind ][1]
ang = self.d['servo_cap']['captures'][ self.ind ][2]
q = Quaternion( *tft.quaternion_from_euler( 0.0, 0.0, math.radians( ang )))
ps.pose.orientation = q
self.ind += 1
userdata.next_move_pose = ps
userdata.tagid = self.d['servo_cap']['tagid']
userdata.bagfile_name = self.d['servo_cap']['bagfile_path'] + str(int(rospy.Time.now().to_sec()))
userdata.bagfile_topics = self.d['servo_cap']['bagfile_topics']
return 'succeeded'
else:
return 'aborted' # done!
def __init__(self):
rospy.init_node("handoff", anonymous=True)
rospy.logout("handoff_node: Have run hrl_pr2_gains/change_gains.sh yet?")
self.robot = hrl_pr2.HRL_PR2()
if not (os.environ.has_key("ROBOT") and os.environ["ROBOT"] == "sim"):
self.ts = tsen.TactileSensor()
self.arm = "right_arm"
self.start_ja = [
0.040304940763152608,
1.2398003444166741,
-1.2204088251845415,
-1.9324078526157087,
-31.197472992401149,
-1.7430222641585842,
-1.5358378047038517,
]
# self.target_ja = [0.35891507126604916, 0.13778228113494312, -0.01277662779292843, -1.4992538841561938, -28.605807802842136, -0.96590944225972863, -3.0950669743130161]
self.target_ja = [0.0818, 0.377, -0.860, -2.144, -3.975, -1.479, 3.907]
self._sh = rospy.Service("/rfid_handoff/handoff", HandoffSrv, self.handoff)
self._si = rospy.Service("/rfid_handoff/initialize", HandoffSrv, self.initialize)
self._sj = rospy.Service("/rfid_handoff/handoff_pos", arm_srv, self.handoff_pos)
self._swave = rospy.Service("/rfid_handoff/wave", HandoffSrv, self.wave)
# self.initialize() # Prefer to do this manually... (rosservice call /rfid_handoff/initialize)
rospy.logout("handoff_node: Waiting for service calls.")
def order_by_rostime(dat):
# dat is [[trial, obj], ... ]
# I'm too lazy to figure out how to reset time and prevent "TF_OLD_DATA" errors / warnings.
# Instead, we're just going to order the bag playback in wall-clock order.
def build_fname(t, o):
# woot_150_6_tag_BlueHairBrus_headpost.bag
fname = 'search_aware_home/woot_150_' + str(
t) + '_tag_' + ahe.tdb[o][0].replace(' ', '') + '_headpost.bag'
return fname
dat = [[t, o] + [build_fname(t, o)] for t, o in dat]
dat = [d for d in dat if glob.glob(d[-1]) != []]
rospy.logout('Ordering the bagfiles in increasing order of start time.')
def gettime(fname):
print fname
# returns the timestamp of the first message
b = rosbag.Bag(fname)
msg = b.read_messages().next()
tt = msg[-1].to_sec()
b.close()
return tt
start_times = [gettime(d[-1]) for d in dat]
rospy.logout('Done ordering.')
return [[dat[ind][0], dat[ind][1]] for ind in np.argsort(start_times)]
def run( self ):
while self.should_run and not rospy.is_shutdown():
[ s.update_server() for s in self.ros_servers ]
time.sleep(0.001)
for n in self.names:
rospy.logout( 'ROS_Robotis_Servo: Shutting Down /robotis/servo_' + n + ' on ' + self.dev_name )
def __init__(self, name = 'reader1', readPwr = 2300,
portStr = '/dev/robot/RFIDreader',
antFuncs = [], callbacks = []):
Thread.__init__(self)
self.should_run = True
try:
rospy.init_node( 'rfid_m5e_' + name )
except rospy.ROSException:
pass
self.mode = ''
self.name = name + '_reader'
rospy.logout( 'ROS_M5e: Launching RFID Reader' )
rospy.logout( 'ROS_M5e: Please check out our related work @ http://www.hsi.gatech.edu/hrl/project_rfid.shtml' )
rospy.logout( 'ROS_M5e: '+self.name+' Building & Connecting to reader' )
def prin( x ): rospy.logout( 'ROS_M5e: lib_M5e: ' + x ) # use rospy.logout in underlying lib's output
self.reader = M5e(readPwr=readPwr, portSTR = portStr, verbosity_func = prin)
self.antFuncs = antFuncs
self.callbacks = callbacks + [self.broadcast]
rospy.logout( 'ROS_M5e: publishing RFID reader with type RFIDread to channel /rfid/'+name+'_reader' )
self.channel = rospy.Publisher('/rfid/'+name+'_reader', RFIDread)
self.pub_arr = rospy.Publisher('/rfid/'+name+'_reader_arr', RFIDreadArr)
self._mode_service_obj = rospy.Service('/rfid/'+name+'_mode',
RfidSrv, self._mode_service)
rospy.logout( 'ROS_M5e: '+self.name+' Inialized and awaiting instructions' )
self.start() # Thread: calls self.run()
def __init__( self ):
Thread.__init__( self )
self.should_run = True
rospy.logout('servo_node: Initializing')
rospy.init_node('servo_node')
# Unfold "ears" to servo positions
p_left = rr.ROS_Robotis_Client( 'left_pan' )
t_left = rr.ROS_Robotis_Client( 'left_tilt' )
p_left.move_angle( math.radians(-40), math.radians(10), blocking = False )
t_left.move_angle( 0.0, math.radians(10), blocking = False )
p_right = rr.ROS_Robotis_Client( 'right_pan' )
t_right = rr.ROS_Robotis_Client( 'right_tilt' )
p_right.move_angle( math.radians(40), math.radians(10), blocking = False )
t_right.move_angle( 0.0, math.radians(10), blocking = False )
self.reader = rmc.ROS_M5e_Client('ears')
self.reader.track_mode('person ')
# Use assisted teleop as pseudo-callback to see if valid movement:
self.check_pub = rospy.Publisher( 'rfid_cmd_vel_check', Twist )
self.sub = rospy.Subscriber( "assisted_teleop_response", Twist, self.callback )
self.command_pub = rospy.Publisher( "rfid_cmd_vel", Twist )
self.moving = True
time.sleep(3.0) # give antennas time to settle
self.start()
def doCycle(self):
#make sure that all adapters have legal states
if self.legalStates():
if self.state == "nav":
"""
State Transitions from nav:
1) nav --- robot is inactive initially, reaches a goal, or timeout is reached ---> nav
2) nav --- robot is inactive and should be pursuing the current goal (move_base crashed) ---> nav
"""
if self.navigator.goalReached() or (
not self.navigator.active() and self.current_goal
== None) or self.navigator.timeUp():
self.current_goal = self.goals[random.randint(
0,
len(self.goals) - 1)]
self.navigator.sendGoal(self.current_goal, "/map")
print "nav --> nav"
elif not self.navigator.active() and self.current_goal != None:
self.navigator.sendGoal(self.current_goal, "/map")
print "nav --> nav"
else:
if not self.navigator.legalState():
print("Waiting on %s to be published" %
(self.navigator.state_topic))
rospy.logout("Waiting on %s to be published" %
(self.navigator.state_topic))
def orient(self, request):
tagid = request.data
if not self.data.has_key(tagid):
rospy.logout('Tag id \'%s\' not found during last scan.' % tagid)
return String_Int32Response(int(False))
arr = np.array(self.data[tagid]).T
arr = arr[:, np.argsort(arr[0])]
h, bins = np.histogram(arr[0], 36, (-np.pi, np.pi))
ind = np.sum(arr[0][:, np.newaxis] > bins,
axis=1) - 1 # Gives indices for data into bins
bin_centers = (bins[:-1] + bins[1:]) / 2.0
best_dir = 0.0
best_rssi = 0.0
for i in np.unique(ind):
avg_rssi = np.mean(arr[1, np.argwhere(ind == i)])
if avg_rssi > best_rssi:
best_rssi = avg_rssi
best_dir = bin_centers[i]
rospy.logout(
'orient_node: Best dir (deg): %2.2f with avg rssi: %2.1f' %
(math.degrees(best_dir), best_rssi))
self.rotate_backup_client.rotate_backup(best_dir, 0.0)
return String_Int32Response(int(True))
def __init__( self ):
rospy.init_node( 'handoff', anonymous = True )
rospy.logout( 'handoff_node: Have run hrl_pr2_gains/change_gains.sh yet?' )
self.robot = hrl_pr2.HRL_PR2()
if not (os.environ.has_key('ROBOT') and os.environ['ROBOT'] == 'sim'):
self.ts = tsen.TactileSensor()
self.arm = 'right_arm'
self.start_ja = [0.040304940763152608, 1.2398003444166741, -1.2204088251845415, -1.9324078526157087, -31.197472992401149, -1.7430222641585842, -1.5358378047038517]
#self.target_ja = [0.35891507126604916, 0.13778228113494312, -0.01277662779292843, -1.4992538841561938, -28.605807802842136, -0.96590944225972863, -3.0950669743130161]
self.target_ja = [0.0818, 0.377, -0.860, -2.144, -3.975, -1.479, 3.907]
self.grasp_ja = [ -1.57263428749, -0.347376409246, -1.58724516843, -1.61707941489, -51.4022142048, -1.36894875484, -5.9965378332 ]
self.stowgrasp_ja = [-0.130, 1.18, -1.410, -1.638, -141.06, -1.695, 48.616 ]
self._sh = rospy.Service( '/rfid_handoff/handoff' , HandoffSrv, self.handoff )
self._si = rospy.Service( '/rfid_handoff/initialize' , HandoffSrv, self.initialize )
self._sj = rospy.Service( '/rfid_handoff/handoff_pos' , ArmSrv, self.handoff_pos )
self._ss = rospy.Service( '/rfid_handoff/stow' , HandoffSrv, self.stow )
self._sp = rospy.Service( '/rfid_handoff/pre_stow' , HandoffSrv, self.pre_stow )
self._sg = rospy.Service( '/rfid_handoff/grasp' , HandoffSrv, self.grasp )
self._senough = rospy.Service( '/rfid_handoff/stow_grasp' , HandoffSrv, self.stow_grasp )
self._swave = rospy.Service( '/rfid_handoff/wave' , HandoffSrv, self.wave )
# self.initialize() # Prefer to do this manually... (rosservice call /rfid_handoff/initialize)
rospy.logout( 'handoff_node: Waiting for service calls.' )
def run(self, total_iter=1000):
for i in xrange(total_iter):
if self.mode == self.QUERY_MODE:
for aF in self.antFuncs:
antennaName = aF(
self.reader
) # let current antFunc make appropriate changes
results = self.reader.QueryEnvironment()
if len(results) == 0:
datum = [antennaName, '', -1]
[cF(datum) for cF in self.callbacks]
for tagid, rssi in results:
datum = [antennaName, tagid, rssi]
[cF(datum) for cF in self.callbacks]
elif self.mode == self.TRACK_MODE:
for aF in self.antFuncs:
antennaName = aF(
self.reader
) # let current antFunc make appropriate changes
tagid = self.tag_to_track
rssi = self.reader.TrackSingleTag(tagid,
timeout=10,
safe_response=False)
#if rssi != -1:
datum = [antennaName, tagid, rssi]
[cF(datum) for cF in self.callbacks]
else:
time.sleep(0.005)
rospy.logout('ROS_M5e: ' + self.name + ' Shutting down reader')
def rotate_robot_loop(self):
if self.rotate_mode == False:
return
angle = self.rotate_angle
angle = angle / 180.0 * math.pi
if angle > 0:
z = 0.4
else:
z = -0.4
self.twist.angular.z = z
self.vel_pub.publish(self.twist)
euler = self.quaternion_to_euler(self.odom.pose.pose.orientation)
temp = euler.z - self.start_rotate_angle.z
if angle > 0:
if temp < 0:
temp = 2 * math.pi + temp
else:
if temp > 0:
temp = -2 * math.pi + temp
if abs(temp) < 350 / 180 * math.pi:
if abs(temp) >= abs(angle):
self.rotate_mode = False
rospy.logout("rotate loop " + str(angle / math.pi * 180) + " " +
str(temp / math.pi * 180))
if self.rotate_mode == False:
self.twist.angular.z = 0
self.vel_pub.publish(self.twist)
rospy.logout("rotate finish")
def fetch_tool(self, duration=5.):
rospy.logout("Moving the "+self.arm+" to fetch the object")
self.switch_arm()
self.arm_controller.reset_ep()
ep_cur = self.arm_controller.get_ep()
ep1 = copy.deepcopy(self.ar_ep)
ep1[0][2]=ep_cur[0][2]+.1
self.epc_move_arm(self.arm_controller, ep_cur, ep1, duration)
self.gripper.Open(self.arm)
time.sleep(2.)
self.tool_ep = copy.deepcopy(self.ar_ep)
# Kinect on Monty has not been calibrated!
#Offset due to Kinect error
self.tool_ep[0][0]-= .05
# self.tool_ep[0][1]-= .01
# self.tool_ep[0][2]-= .03
self.tool_ep[0][2]-= .04
self.epc_move_arm(self.arm_controller, ep1, self.tool_ep, duration)
self.gripper.Close(self.arm)
time.sleep(2.)
self.epc_move_arm(self.arm_controller, self.tool_ep, ep1, duration)
self.found_tag = False
self.search_tag = False
self.pr2_init = False
self.grasp_object = True
def execute(self, userdata):
if not self.d:
f = open( self.fname, 'r' )
self.d = yaml.load( f )
self.size = len( self.d['rad_cap']['captures'] )
self.ind = 0
f.close()
self.t0 = rospy.Time.now().to_sec()
if self.ind < self.size:
rospy.logout( 'YamlProcPoses: issuing %d of %d' % (self.ind+1, self.size))
rospy.logout( 'YamlProcPoses: Time to capture %2.2f' % ( rospy.Time.now().to_sec() - self.t0 ))
self.t0 = rospy.Time.now().to_sec()
ps = PoseStamped()
ps.header.frame_id = '/map'
ps.header.stamp = rospy.Time(0)
ps.pose.position.x = self.d['rad_cap']['captures'][ self.ind ][0]
ps.pose.position.y = self.d['rad_cap']['captures'][ self.ind ][1]
ang = self.d['rad_cap']['captures'][ self.ind ][2]
q = Quaternion( *tft.quaternion_from_euler( 0.0, 0.0, math.radians( ang )))
ps.pose.orientation = q
self.ind += 1
userdata.next_move_pose = ps
return 'succeeded'
else:
return 'aborted' # done!
def __init__(self, ft=False, audio=False, kinematics=False, vision=False, pps=False, skin=False, \
subject=None, task=None, data_pub= False, verbose=False):
rospy.logout('ADLs_log node subscribing..')
self.subject = subject
self.task = task
self.data_pub = data_pub
self.verbose = verbose
self.initParams()
self.audio = kinect_audio() if audio else None
self.kinematics = robot_kinematics() if kinematics else None
self.ft = tool_ft() if ft else None
self.vision = artag_vision(False, viz=False) if vision else None
self.pps_skin = pps_skin(True) if pps else None
self.fabric_skin = fabric_skin(True) if skin else None
self.waitForReady()
self.initComms()
if self.data_pub:
t = threading.Thread(target=self.runDataPub)
t.setDaemon(True)
t.start()
def __init__( self ):
rospy.logout('servo_node: Initializing')
try:
rospy.init_node('servo_node')
except: # Node probably already initialized elsewhere
pass
# Create servos.
self.p_left = rr.ROS_Robotis_Client( 'left_pan' )
self.t_left = rr.ROS_Robotis_Client( 'left_tilt' )
self.p_right = rr.ROS_Robotis_Client( 'right_pan' )
self.t_right = rr.ROS_Robotis_Client( 'right_tilt' )
# Create Costmap Services obj
self.cs = costmap.CostmapServices( accum = 3 )
# Note: After moving, this will require accum * -1's before stopping.
# Publish move_base command
self._pub = rospy.Publisher( 'rfid_cmd_vel', Twist )
# Alterative ways to call servoing using ROS services / actionlib
self._service_servo = rospy.Service( '/rfid_servo/servo', ServoSrv, self.service_request )
self._as = actionlib.SimpleActionServer( '/rfid_servo/servo_act',
ServoAction, execute_cb = self.action_request )
self._as.start()
rospy.logout( 'servo_node: Service ready and waiting' )
def processARtag( self, msg ):
if msg.markers != []:
frame = msg.markers[0].header.frame_id
p = msg.markers[0].pose.pose.position
pt = PointStamped()
pt.header.frame_id = frame
pt.point.x = p.x
pt.point.y = p.y
pt.point.z = p.z
pt.header.stamp = rospy.Time(0)
try:
pt_bl = self.listener.transformPoint( '/torso_lift_link', pt )
pbl = pt_bl.point
# pointing_frame manual offset (should actually be on p.y,
# but this is a good approximation with less noise in robot frame).
pbl.z = pbl.z - 0.2
self.lock.acquire()
self.last_pos = [ pbl.x, pbl.y, pbl.z ]
self.found_tag = True
self.lock.release()
except:
rospy.logout( 'ARtagDetect: Transform failed' )
def __init__(self, ft_sensor_node_name):
self.init_time = 0.
self.counter = 0
self.counter_prev = 0
self.force = np.matrix([0., 0., 0.]).T
self.force_raw = np.matrix([0., 0., 0.]).T
self.torque = np.matrix([0., 0., 0.]).T
self.torque_raw = np.matrix([0., 0., 0.]).T
self.torque_bias = np.matrix([0., 0., 0.]).T
self.time_data = []
self.force_data = []
self.force_raw_data = []
self.torque_data = []
self.torque_raw_data = []
#capture the force on the tool tip
self.force_sub = rospy.Subscriber(ft_sensor_node_name+\
'/wrench_zeroed', WrenchStamped, self.force_cb)
#raw ft values from the NetFT
self.force_raw_sub = rospy.Subscriber('pr2_netft/wrench_raw',\
WrenchStamped, self.force_raw_cb)
self.force_zero = rospy.Publisher(ft_sensor_node_name+\
'/rezero_wrench', Bool)
rospy.logout('Done subscribing to '+ft_sensor_node_name+\
'/rezero_wrench topic')
def __init__(self, service_name='/rotate_backup'):
rospy.logout('rotate_backup_client: Waiting for service: \'%s\'' %
service_name)
rospy.wait_for_service(service_name)
rospy.logout('rotate_backup_client: Service ready.')
self._rb_service = rospy.ServiceProxy(service_name, FloatFloat_Int32)
def execute(self, userdata):
rospy.logout( 'Executing PrintStr: %s' % self.ins )
f = raw_input()
if string.find( f, 'yes' ) == -1:
return 'aborted'
else:
return 'succeeded'
def __init__(self):
Thread.__init__(self)
self.should_run = True
rospy.logout('servo_node: Initializing')
rospy.init_node('servo_node')
# Unfold "ears" to servo positions
p_left = rr.ROS_Robotis_Client('left_pan')
t_left = rr.ROS_Robotis_Client('left_tilt')
p_left.move_angle(math.radians(-40), math.radians(10), blocking=False)
t_left.move_angle(0.0, math.radians(10), blocking=False)
p_right = rr.ROS_Robotis_Client('right_pan')
t_right = rr.ROS_Robotis_Client('right_tilt')
p_right.move_angle(math.radians(40), math.radians(10), blocking=False)
t_right.move_angle(0.0, math.radians(10), blocking=False)
self.reader = rmc.ROS_M5e_Client('ears')
self.reader.track_mode('person ')
# Use assisted teleop as pseudo-callback to see if valid movement:
self.check_pub = rospy.Publisher('rfid_cmd_vel_check', Twist)
self.sub = rospy.Subscriber("assisted_teleop_response", Twist,
self.callback)
self.command_pub = rospy.Publisher("rfid_cmd_vel", Twist)
self.moving = True
time.sleep(3.0) # give antennas time to settle
self.start()
def __init__(self, robot):
try:
rospy.init_node('ZenitherClient')
rospy.logout('ZenitherServer: Initialized Node')
except rospy.ROSException:
pass
if robot not in zc.calib:
raise RuntimeError('unknown robot')
self.calib = zc.calib[robot]
srv = '/zenither/move_position'
rospy.wait_for_service(srv)
self.move_position = rospy.ServiceProxy(srv, Float_Int)
srv = '/zenither/stop'
rospy.wait_for_service(srv)
self.stop = rospy.ServiceProxy(srv, Float_Int)
srv = '/zenither/apply_torque'
rospy.wait_for_service(srv)
self.apply_torque = rospy.ServiceProxy(srv, Float_Int)
srv = '/zenither/torque_move_position'
rospy.wait_for_service(srv)
self.torque_move_position = rospy.ServiceProxy(srv, Float_Int)
zenither_pose_topic = 'zenither_pose'
self.h = None
self.lock = RLock()
rospy.Subscriber(zenither_pose_topic, FloatArray, self.pose_cb)
def joy_process( self, msg ):
if msg.buttons[11] == 1 and time.time() - self.last_button > 1.0:
if msg.buttons[12] == 1: # tri + R1
rospy.logout( 'demo_node: joy_process: Calling demo service' )
p = CmdProcess( ['rosservice', 'call', '/rfid_demo/demo'] )
p.run()
self.last_button = time.time()
elif msg.buttons[13] == 1: # circle + R1
rospy.logout( 'demo_node: joy_process: Calling handoff service' )
p = CmdProcess( ['rosservice', 'call', '/rfid_handoff/handoff'] )
p.run()
self.last_button = time.time()
elif msg.buttons[14] == 1: # X + R1
rospy.logout( 'demo_node: joy_process: Calling servo toggle service' )
p = CmdProcess( ['rosservice', 'call', '/rfid_servo/stop_next_obs', '1'] )
p.run()
self.last_button = time.time()
elif msg.buttons[15] == 1: # square + R1
rospy.logout( 'demo_node: joy_process: Calling handoff initialization service' )
p = CmdProcess( ['rosservice', 'call', '/rfid_handoff/initialize'] )
p.run()
self.last_button = time.time()
elif msg.buttons[9] == 1: # R2 + R1
rospy.logout( 'demo_node: joy_process: Calling handoff wave service' )
p = CmdProcess( ['rosservice', 'call', '/rfid_handoff/wave'] )
p.run()
self.last_button = time.time()
def slct_obj_srv():
rospy.init_node('select_object_server')
# Create Service for object info retrieving
s = rospy.Service('slct_obj', RecognizeObject, handle_slt_obj)
rospy.logout("Service select_object_server waiting")
os.system("rosrun object_recognition_ros object_information_server")
rospy.spin()
def __init__(self, robot):
try:
rospy.init_node('ZenitherClient')
rospy.logout('ZenitherServer: Initialized Node')
except rospy.ROSException:
pass
if robot not in zc.calib:
raise RuntimeError('unknown robot')
self.calib = zc.calib[robot]
srv = '/zenither/move_position'
rospy.wait_for_service(srv)
self.move_position = rospy.ServiceProxy(srv, Float_Int)
srv = '/zenither/stop'
rospy.wait_for_service(srv)
self.stop = rospy.ServiceProxy(srv, Float_Int)
srv = '/zenither/apply_torque'
rospy.wait_for_service(srv)
self.apply_torque = rospy.ServiceProxy(srv, Float_Int)
srv = '/zenither/torque_move_position'
rospy.wait_for_service(srv)
self.torque_move_position = rospy.ServiceProxy(srv, Float_Int)
zenither_pose_topic = 'zenither_pose'
self.h = None
self.lock = RLock()
rospy.Subscriber(zenither_pose_topic, FloatArray, self.pose_cb)
def joy_process(self, msg):
if msg.buttons[11] == 1 and time.time() - self.last_button > 1.0:
if msg.buttons[12] == 1: # tri + R1
rospy.logout('demo_node: joy_process: Calling demo service')
p = CmdProcess(['rosservice', 'call', '/rfid_demo/demo'])
p.run()
self.last_button = time.time()
elif msg.buttons[13] == 1: # circle + R1
rospy.logout('demo_node: joy_process: Calling handoff service')
p = CmdProcess(['rosservice', 'call', '/rfid_handoff/handoff'])
p.run()
self.last_button = time.time()
elif msg.buttons[14] == 1: # X + R1
rospy.logout(
'demo_node: joy_process: Calling servo toggle service')
p = CmdProcess(
['rosservice', 'call', '/rfid_servo/stop_next_obs', '1'])
p.run()
self.last_button = time.time()
elif msg.buttons[15] == 1: # square + R1
rospy.logout(
'demo_node: joy_process: Calling handoff initialization service'
)
p = CmdProcess(
['rosservice', 'call', '/rfid_handoff/initialize'])
p.run()
self.last_button = time.time()
elif msg.buttons[9] == 1: # R2 + R1
rospy.logout(
'demo_node: joy_process: Calling handoff wave service')
p = CmdProcess(['rosservice', 'call', '/rfid_handoff/wave'])
p.run()
self.last_button = time.time()
def shutdown():
global loaded, unload_controller_service, load_controller_service, switch_controller_service
rospy.loginfo(
"Shutting down spawner. Stopping and unloading controllers...")
try:
# unloader
unload_controller = rospy.ServiceProxy(unload_controller_service,
UnloadController)
# switcher
switch_controller = rospy.ServiceProxy(switch_controller_service,
SwitchController)
rospy.loginfo("Stopping all controllers...")
switch_controller([], loaded, SwitchControllerRequest.STRICT)
rospy.loginfo("Unloading all loaded controllers...")
for name in reversed(loaded):
rospy.logout("Trying to unload %s" % name)
unload_controller(name)
rospy.logout("Succeeded in unloading %s" % name)
except (rospy.ServiceException, rospy.exceptions.ROSException) as exc:
rospy.logwarn(
"Controller Spawner error while taking down controllers: %s" %
(exc))
def current_robot_position( self ):
ps = PoseStamped()
ps.header.stamp = rospy.Time(0)
ps.header.frame_id = '/base_link'
ps.pose.orientation.w = 1.0
try:
ps_map = self.listener.transformPose( '/map', ps )
except:
rospy.logout( 'room_explore: Transform failed' )
ps_map = PoseStamped()
ps_map.header.stamp = rospy.Time.now()
ps_map.header.frame_id = '/map'
ps_map.pose.orientation = new_quat
roll, pitch, yaw = tf.transformations.euler_from_quaternion(( ps_map.pose.orientation.x,
ps_map.pose.orientation.y,
ps_map.pose.orientation.z,
ps_map.pose.orientation.w ))
rv = [ ps_map.pose.position.x,
ps_map.pose.position.y,
ps_map.pose.position.z,
roll,
pitch,
yaw ]
#print 'RV: ', rv
return rv
def __init__(self):
super(tool_audio, self).__init__()
self.daemon = True
self.cancelled = False
self.init_time = 0.
self.noise_freq_l = None
self.noise_band = 150.0
self.noise_amp_num = 0 #20 #10
self.noise_amp_thres = 0.0
self.noise_amp_mult = 2.0
self.noise_bias = 0.0
# self.audio_freq = np.fft.fftfreq(self.CHUNK, self.UNIT_SAMPLE_TIME)
self.audio_data = []
self.audio_amp = []
self.audio_data_raw = []
self.time_data = []
self.b, self.a = self.butter_bandpass(1, 400, self.RATE, order=6)
self.p = pyaudio.PyAudio()
deviceIndex = self.find_input_device()
print 'Audio device:', deviceIndex
self.stream = self.p.open(format=self.FORMAT, channels=self.CHANNEL, rate=self.RATE, input=True, frames_per_buffer=self.CHUNK, input_device_index=deviceIndex)
rospy.logout('Done subscribing audio')
print 'Done subscribing audio'
def __init__(self,ft_sensor_topic_name):
super(tool_ft, self).__init__()
self.daemon = True
self.cancelled = False
self.init_time = 0.
self.counter = 0
self.counter_prev = 0
self.force = np.matrix([0.,0.,0.]).T
self.force_raw = np.matrix([0.,0.,0.]).T
self.torque = np.matrix([0.,0.,0.]).T
self.torque_raw = np.matrix([0.,0.,0.]).T
self.torque_bias = np.matrix([0.,0.,0.]).T
self.time_data = []
self.force_data = []
self.force_raw_data = []
self.torque_data = []
self.torque_raw_data = []
# capture the force on the tool tip
# self.force_sub = rospy.Subscriber(ft_sensor_topic_name, WrenchStamped, self.force_cb)
# raw ft values from the NetFT
self.force_raw_sub = rospy.Subscriber(ft_sensor_topic_name, WrenchStamped, self.force_raw_cb)
# self.force_zero = rospy.Publisher('/tool_netft_zeroer/rezero_wrench', Bool)
rospy.logout('Done subscribing to '+ft_sensor_topic_name+' topic')
def __init__(self, ft_sensor_topic_name):
super(tool_ft, self).__init__()
self.daemon = True
self.cancelled = False
self.init_time = 0.
self.counter = 0
self.counter_prev = 0
self.force = np.matrix([0., 0., 0.]).T
self.force_raw = np.matrix([0., 0., 0.]).T
self.torque = np.matrix([0., 0., 0.]).T
self.torque_raw = np.matrix([0., 0., 0.]).T
self.torque_bias = np.matrix([0., 0., 0.]).T
self.time_data = []
self.force_data = []
self.force_raw_data = []
self.torque_data = []
self.torque_raw_data = []
# capture the force on the tool tip
# self.force_sub = rospy.Subscriber(ft_sensor_topic_name, WrenchStamped, self.force_cb)
# raw ft values from the NetFT
self.force_raw_sub = rospy.Subscriber(ft_sensor_topic_name,
WrenchStamped, self.force_raw_cb)
# self.force_zero = rospy.Publisher('/tool_netft_zeroer/rezero_wrench', Bool)
rospy.logout('Done subscribing to ' + ft_sensor_topic_name + ' topic')
def __init__(self):
self.pub = rospy.Publisher("/ros_switch", Bool)
rospy.init_node("shaver_pwr_pub", anonymous=True)
rospy.logout("shaver_pwr_pub node publishing..")
self.state = False
self.pwr_on = "Power is on"
self.pwr_off = "Power is off"
def __init__(self):
rospy.logout('servo_node: Initializing')
try:
rospy.init_node('servo_node')
except:
pass
# Unfold "ears" to servo positions
self.p_left = rr.ROS_Robotis_Client('left_pan')
self.t_left = rr.ROS_Robotis_Client('left_tilt')
self.p_right = rr.ROS_Robotis_Client('right_pan')
self.t_right = rr.ROS_Robotis_Client('right_tilt')
# Use assisted teleop as pseudo-callback to see if valid movement:
self.scores = deque()
self.og_pub = rospy.Publisher('assisted_teleop_check', Twist)
self.og_sub = rospy.Subscriber('assisted_teleop_score', Float64,
self.og_score_cb)
self.command_pub = rospy.Publisher('rfid_cmd_vel', Twist)
self._service_servo = rospy.Service('/rfid_servo/servo',
StringInt32_Int32, self.run)
self._service_stop = rospy.Service('/rfid_servo/stop', Empty,
self.servo_stop)
self._service_stop = rospy.Service('/rfid_servo/abort', Empty,
self.servo_abort)
self._service_stop_next_obs = rospy.Service(
'/rfid_servo/stop_next_obs', Int32_Int32, self.stop_request)
self.stop_next_obs = False # run forever
self.should_run = True
self.abort = False
rospy.logout('servo_node: Service ready and waiting')
def __init__( self, topic = '/pressure/r_gripper_motor',
mag_func = default_mag_func ):
rospy.logout( 'tactile_sensor: Initializing' )
try:
rospy.init_node( 'tactile_sensor' )
except:
pass
self.mag_func = mag_func
self.left = None
self.right = None
self.l_bias = None
self.r_bias = None
self.topic = topic
self.pub = rospy.Publisher( '/readings/' + string.replace(topic,'/','_') + '_mag', Float64 )
self.service = rospy.Service( '/readings/' + string.replace(topic,'/','_') + '_serv',
FloatFloat_Int32,
self.thresh_service )
self.reg_sensor( )
while self.left == None or self.right == None:
time.sleep( 0.1 )
self.unreg_sensor()
rospy.logout( 'tactile_sensor: Ready' )
def __init__(self):
rospy.init_node('handoff', anonymous=True)
rospy.logout(
'handoff_node: Have run hrl_pr2_gains/change_gains.sh yet?')
self.robot = hrl_pr2.HRL_PR2()
if not (os.environ.has_key('ROBOT') and os.environ['ROBOT'] == 'sim'):
self.ts = tsen.TactileSensor()
self.arm = 'right_arm'
self.start_ja = [
0.040304940763152608, 1.2398003444166741, -1.2204088251845415,
-1.9324078526157087, -31.197472992401149, -1.7430222641585842,
-1.5358378047038517
]
#self.target_ja = [0.35891507126604916, 0.13778228113494312, -0.01277662779292843, -1.4992538841561938, -28.605807802842136, -0.96590944225972863, -3.0950669743130161]
self.target_ja = [0.0818, 0.377, -0.860, -2.144, -3.975, -1.479, 3.907]
self._sh = rospy.Service('/rfid_handoff/handoff', HandoffSrv,
self.handoff)
self._si = rospy.Service('/rfid_handoff/initialize', HandoffSrv,
self.initialize)
self._sj = rospy.Service('/rfid_handoff/handoff_pos', arm_srv,
self.handoff_pos)
self._swave = rospy.Service('/rfid_handoff/wave', HandoffSrv,
self.wave)
# self.initialize() # Prefer to do this manually... (rosservice call /rfid_handoff/initialize)
rospy.logout('handoff_node: Waiting for service calls.')
def bias( self ):
self.reg_sensor()
rospy.logout( 'tactile_sensor: Biasing' )
self.l_bias = np.copy( self.left )
self.r_bias = np.copy( self.right )
self.unreg_sensor()
return True
def move(self, request):
r = request.rotate
d = request.displace
rospy.logout('rotate_backup: Asked to rotate: %3.2f (deg)' %
math.degrees(r))
rospy.logout(
'rotate_backup: Asked to translate (forward-backward): %3.2f (m)' %
d)
self.non_nav_backup(d)
self.non_nav_rotate(r)
# success, pose, orient = self.get_pose()
# if not success:
# return FloatFloat_Int32Response( int(False) )
# new_point = Point( pose[0], pose[1], pose[2] )
# old_rx, old_ry, old_rz = orient
# new_orient = tft.quaternion_from_euler( old_rx, old_ry, old_rz + r )
# new_quat = Quaternion( *new_orient )
# new_ps = PoseStamped()
# new_ps.header.stamp = rospy.Time(0)
# new_ps.header.frame_id = '/odom_combined'
# new_ps.pose.position = new_point
# new_ps.pose.orientation = new_quat
# self.pub.publish( new_ps )
# self.wait_for_stop()
# rospy.logout( 'rotate_backup: Done with call.' )
return FloatFloat_Int32Response(int(True))
def scan(self, tag_id):
# Note: action_request does pretty much the same thing, but with preemption.
rospy.logout('ARservoScan: Scan requested for tagid \'%s\'' % tag_id)
# Subscribe to RFID reader to figure out where it wants to go.
zc = servo.ZedCalc(filt_len=5, tag_id=tag_id)
def preempt_func():
# Note: I believe the variables are scoped at time of func def
zed_next = zc.next_pub() # Where does RFID want to go?
costmap_rv = self.cs.scoreTraj_NegHyst(0.1, 0.0, zed_next)
#print zed_next
#print costmap_rv
if costmap_rv == -1.0:
#rospy.logout( 'Obstacle still present.' )
return False # Still keep scaning
else:
rospy.logout('ARservoScan: Obstacle Gone... PREEMPT!')
return True # Whatever was in way is gone. Should go back to servoing.
success, found_tag, pos, frame = self.scanner.scan(
preempt_func=preempt_func)
# Success will be false if it was preempted (eg. preempt_func returns True)
zc.stop() # Stop the RFID reader...
if success and found_tag:
found_tag, pos, frame = self.scanner.settle(pos, frame)
rospy.logout('ARservoScan: Scan completed.')
#print 'scan result: ', success, found_tag, pos, frame
return (success, found_tag, pos, frame)
def fetch_tool(self, duration=5.):
rospy.logout("Moving the " + self.arm + " to fetch the object")
self.switch_arm()
self.arm_controller.wait_for_ep()
ep_cur = self.arm_controller.get_ep()
ep1 = copy.deepcopy(self.ar_ep)
ep1[0][2] = ep_cur[0][2] + .1
self.epc_move_arm(self.arm_controller, ep_cur, ep1, 10)
self.gripper.Open(self.arm)
time.sleep(2.)
self.tool_ep = copy.deepcopy(self.ar_ep)
# self.tool_ep[1] = np.mat(tf_trans.euler_matrix(0, np.pi/2, 0))[:3,:3]
# Kinect on Monty has not been calibrated!
#Offset due to Kinect error
self.tool_ep[0][0] -= .02
# self.tool_ep[0][1]+= .02
self.tool_ep[0][2] += .025
# self.tool_ep[0][2]-= .05
self.epc_move_arm(self.arm_controller, ep1, self.tool_ep, 15)
self.gripper.Close(self.arm)
time.sleep(2.)
self.epc_move_arm(self.arm_controller, self.tool_ep, ep1, 15)
self.found_tag = False
self.search_tag = False
self.pr2_init = False
self.grasp_object = True
def current_robot_position(self):
ps = PoseStamped()
ps.header.stamp = rospy.Time(0)
ps.header.frame_id = '/base_link'
ps.pose.orientation.w = 1.0
try:
ps_map = self.listener.transformPose('/map', ps)
except:
rospy.logout('room_explore: Transform failed')
ps_map = PoseStamped()
ps_map.header.stamp = rospy.Time.now()
ps_map.header.frame_id = '/map'
ps_map.pose.orientation = new_quat
roll, pitch, yaw = tf.transformations.euler_from_quaternion(
(ps_map.pose.orientation.x, ps_map.pose.orientation.y,
ps_map.pose.orientation.z, ps_map.pose.orientation.w))
rv = [
ps_map.pose.position.x, ps_map.pose.position.y,
ps_map.pose.position.z, roll, pitch, yaw
]
#print 'RV: ', rv
return rv
def __init__( self ):
rospy.logout('servo_node: Initializing')
try:
rospy.init_node('servo_node')
except:
pass
# Unfold "ears" to servo positions
self.p_left = rr.ROS_Robotis_Client( 'left_pan' )
self.t_left = rr.ROS_Robotis_Client( 'left_tilt' )
self.p_right = rr.ROS_Robotis_Client( 'right_pan' )
self.t_right = rr.ROS_Robotis_Client( 'right_tilt' )
# Use assisted teleop as pseudo-callback to see if valid movement:
self.scores = deque()
self.og_pub = rospy.Publisher( 'assisted_teleop_check', Twist )
self.og_sub = rospy.Subscriber( 'assisted_teleop_score', Float64, self.og_score_cb )
self.command_pub = rospy.Publisher( 'rfid_cmd_vel', Twist )
self._service_servo = rospy.Service('/rfid_servo/servo',
StringInt32_Int32,
self.run )
self._service_stop = rospy.Service('/rfid_servo/stop', Empty, self.servo_stop )
self._service_stop = rospy.Service('/rfid_servo/abort', Empty, self.servo_abort )
self._service_stop_next_obs = rospy.Service('/rfid_servo/stop_next_obs', Int32_Int32, self.stop_request )
self.stop_next_obs = False # run forever
self.should_run = True
self.abort = False
rospy.logout( 'servo_node: Service ready and waiting' )
def __init__(self, df, room, start_rad=1.5):
Thread.__init__(self)
try:
rospy.init_node('RoomExplore')
except:
pass
self.should_run = True
self.frame = '/' + room
self.height = df[room]['height']
self.width = df[room]['width']
self.listener = tf.TransformListener()
self.listener.waitForTransform('/base_link',
'/map',
rospy.Time(0),
timeout=rospy.Duration(100))
self.setup_poses(radius=start_rad) # also initializes radius
self.pub = rospy.Publisher('visarr', Marker)
self._as = actionlib.SimpleActionServer('/explore',
ExploreAction,
execute_cb=self.action_request)
self._as.start()
rospy.logout('Action should be started.')
self.start()
def __init__(self):
super(tool_audio, self).__init__()
self.daemon = True
self.cancelled = False
self.init_time = 0.
self.noise_freq_l = None
self.noise_band = 150.0
self.noise_amp_num = 0 #20 #10
self.noise_amp_thres = 0.0
self.noise_amp_mult = 2.0
self.noise_bias = 0.0
# self.audio_freq = np.fft.fftfreq(self.CHUNK, self.UNIT_SAMPLE_TIME)
self.audio_data = []
self.audio_amp = []
self.audio_data_raw = []
self.time_data = []
self.b, self.a = self.butter_bandpass(1, 400, self.RATE, order=6)
self.p = pyaudio.PyAudio()
deviceIndex = self.find_input_device()
print 'Audio device:', deviceIndex
self.stream = self.p.open(format=self.FORMAT,
channels=self.CHANNEL,
rate=self.RATE,
input=True,
frames_per_buffer=self.CHUNK,
input_device_index=deviceIndex)
rospy.logout('Done subscribing audio')
print 'Done subscribing audio'
def __init__(self,
topic='/pressure/r_gripper_motor',
mag_func=default_mag_func):
rospy.logout('tactile_sensor: Initializing')
try:
rospy.init_node('tactile_sensor')
except:
pass
self.mag_func = mag_func
self.left = None
self.right = None
self.l_bias = None
self.r_bias = None
self.topic = topic
self.pub = rospy.Publisher(
'/readings/' + string.replace(topic, '/', '_') + '_mag', Float64)
self.service = rospy.Service(
'/readings/' + string.replace(topic, '/', '_') + '_serv',
FloatFloat_Int32, self.thresh_service)
self.reg_sensor()
while self.left == None or self.right == None:
time.sleep(0.1)
self.unreg_sensor()
rospy.logout('tactile_sensor: Ready')
def __init__( self, name, fname, arm = 'right' ):
self.arm = arm
self.name = name
rospy.logout( 'TrajPlayback: Initializing (%s)' % self.name )
try:
rospy.init_node('traj_playback_'+self.name)
except:
pass
dat = hrl_util.load_pickle( fname )
self.q = np.mat( dat[0].T )
# subsample
self.q = self.q[:,0::30]
# smooth
self.t = np.linspace( 1.3, 1.3 + (self.q.shape[1]-1)*0.3, self.q.shape[1] )
self.vel = self.q.copy()
self.vel[:,1:] -= self.q[:,0:-1]
self.vel[:,0] = 0
self.vel /= 0.3
self.pr2 = PR2()
self.__service = rospy.Service( 'traj_playback/' + name,
TrajPlaybackSrv,
self.process_service )
rospy.logout( 'TrajPlayback: Ready for service requests (%s)' % self.name )
def bias(self):
self.reg_sensor()
rospy.logout('tactile_sensor: Biasing')
self.l_bias = np.copy(self.left)
self.r_bias = np.copy(self.right)
self.unreg_sensor()
return True
def __init__(self, ft=True, audio=False, kinematics=False, subject=None, task=None):
self.init_time = 0
self.tool_tracker_name, self.ft_sensor_topic_name = log_parse()
self.tf_listener = tf.TransformListener()
rospy.logout('ADLs_log node subscribing..')
self.isScooping = True if task == 's' else False
# self.audio = rospy.ServiceProxy('/audio_server', String_String) if audio else None
# self.audioTrialName = rospy.ServiceProxy('/audio_server_trial_name', String_String) if audio else None
self.ft = tool_ft('/netft_data') if ft else None
self.audio = tool_audio_slim() if audio else None
self.kinematics = tool_kinematics(self.tf_listener, targetFrame='/torso_lift_link', isScooping=self.isScooping) if kinematics else None
# File saving
self.iteration = 0
self.subject = subject.replace(' ', '')
self.task = 'scooping' if task == 's' else 'feeding'
directory = os.path.join(os.path.dirname(__file__), '../recordings/')
if not os.path.exists(directory):
os.makedirs(directory)
sensors = ''
if self.ft is not None: sensors += 'f'
if self.audio is not None: sensors += 'a'
if self.kinematics is not None: sensors += 'k'
self.record_root_path = directory
self.folderName = os.path.join(directory, self.subject + '_' + self.task)
## self.folderName = os.path.join(directory, self.subject + '_' + self.task + '_' + sensors + '_' + time.strftime('%m-%d-%Y_%H-%M-%S/'))
self.scooping_steps_times = []
self.scoopingStepsService = rospy.Service('/scooping_steps_service', None_Bool, self.scoopingStepsTimesCallback)
def enable_timer(self, enabled=True):
if enabled:
rospy.logout("Timer enabled")
self._update_plot_timer.start(self._redraw_interval)
else:
rospy.logout("Timer stopped")
self._update_plot_timer.stop()
def __init__(self):
#conv_model_name = "processed_2.0m_7vc_barrett_18_grasp_types_5_layer_170x170_1_12_12_41"
#conv_model_name = "processed_out_condensed_5_layer_170x170_2_6_16_55"
#conv_model_name = "random_5_layer_170x170_2_6_16_43"
#self.conv_model_name = "processed_sweet_guava_5_layer_72x72_2_13_10_29"
#this is just the all bottle
#self.conv_model_name = "processed_man-2_18_16_46_condensed72_5_layer_72x72_small_dset_2_18_17_29"
self.conv_model_name = 'processed_gazebo_contact_and_potential_grasps-2_11_18_20_8_grasp_types72_5_layer_72x72_2_20_17_59'
conv_model_filepath = paths.MODEL_DIR + self.conv_model_name + "/cnn_model.pkl"
dataset_file = str(int(round(time.time() * 1000)))
self.input_dset, raw_rgbd_filepath = init_rgbd_file(dataset_file)
self.input_dset.create_dataset("rgbd", (1, 480, 640, 4))
self.save_dset, save_filepath = init_save_file(dataset_file, self.conv_model_name)
rospy.logout(raw_rgbd_filepath)
rospy.logout(save_filepath)
self.pipeline = BarrettGraspClassificationPipeline(save_filepath, raw_rgbd_filepath, conv_model_filepath, input_key="rgbd")
self.pipeline.run()
self.service = rospy.Service('calculate_grasps_service', CalculateGraspsService, self.service_request_handler)
def fetch_tool(self, duration=5.):
rospy.logout("Moving the "+self.arm+" to fetch the object")
self.switch_arm()
self.arm_controller.wait_for_ep()
ep_cur = self.arm_controller.get_ep()
ep1 = copy.deepcopy(self.ar_ep)
ep1[0][2]=ep_cur[0][2]+.1
self.epc_move_arm(self.arm_controller, ep_cur, ep1, 10)
self.gripper.Open(self.arm)
time.sleep(2.)
self.tool_ep = copy.deepcopy(self.ar_ep)
# self.tool_ep[1] = np.mat(tf_trans.euler_matrix(0, np.pi/2, 0))[:3,:3]
# Kinect on Monty has not been calibrated!
#Offset due to Kinect error
self.tool_ep[0][0]-= .02
# self.tool_ep[0][1]+= .02
self.tool_ep[0][2]+= .025
# self.tool_ep[0][2]-= .05
self.epc_move_arm(self.arm_controller, ep1, self.tool_ep, 15)
self.gripper.Close(self.arm)
time.sleep(2.)
self.epc_move_arm(self.arm_controller, self.tool_ep, ep1, 15)
self.found_tag = False
self.search_tag = False
self.pr2_init = False
self.grasp_object = True
def __init__( self, df, room, start_rad = 1.5, markers_update = lambda x: True ):
try:
rospy.init_node( 'SnakingExplore' )
except:
pass
self.frame = '/' + room
self.markers_update = markers_update
self.length = df[room]['length']
self.width = df[room]['width']
rospy.logout( 'snaking_explore: Initializing' )
self.listener = tf.TransformListener()
self.listener.waitForTransform( '/base_link', '/map', rospy.Time(0), timeout = rospy.Duration(100))
self.listener.waitForTransform( self.frame, '/map', rospy.Time(0), timeout = rospy.Duration(100))
self.setup_poses( radius = start_rad )
self.client = actionlib.SimpleActionClient( 'move_base', MoveBaseAction )
self.client.wait_for_server()
self._service = rospy.Service( '/explore/explore' , ExploreRoomSrv, self.service_request )
self._as = actionlib.SimpleActionServer( '/explore', ExploreAction, execute_cb = self.action_request )
self._as.start()
rospy.logout( 'snaking_explore: Ready and waiting.' )
def processARtag(self, msg):
if msg.markers != []:
frame = msg.markers[0].header.frame_id
p = msg.markers[0].pose.pose.position
pt = PointStamped()
pt.header.frame_id = frame
pt.point.x = p.x
pt.point.y = p.y
pt.point.z = p.z
pt.header.stamp = rospy.Time(0)
try:
pt_bl = self.listener.transformPoint("/torso_lift_link", pt)
pbl = pt_bl.point
# pointing_frame manual offset (should actually be on p.y,
# but this is a good approximation with less noise in robot frame).
pbl.z = pbl.z - 0.2
self.last_pos = [pbl.x, pbl.y, pbl.z]
if not self.found_tag:
self.hc.cancel_all_goals() # stop searching
self.found_tag = True
except:
print pt
rospy.logout("ARtagDetect: Transform failed")
return False
else:
if not self.found_tag and ctime() - self.last_logout > 3.0:
rospy.logout("ARtag still undetected...")
self.last_logout = ctime()
def __init__(self, servo = None, name = '' ):
if servo == None:
raise RuntimeError( 'ROS_Robotis_Servo: No servo specified for server.\n' )
self.servo = servo
self.name = name
try:
rospy.init_node( 'robotis_servo_' + self.name )
except rospy.ROSException:
pass
#self.servo.disable_torque()
rospy.logout( 'ROS_Robotis_Servo: Starting Server /robotis/servo_' + self.name )
self.channel = rospy.Publisher('/robotis/servo_' + self.name, Float64)
self.__service_ang = rospy.Service('/robotis/servo_' + name + '_readangle',
None_Float, self.__cb_readangle)
self.__service_ismove = rospy.Service('/robotis/servo_' + name + '_ismoving',
None_Int32, self.__cb_ismoving)
self.__service_moveang = rospy.Service('/robotis/servo_' + name + '_moveangle',
MoveAng, self.__cb_moveangle)
def move(self, request):
r = request.rotate
d = request.displace
rospy.logout("rotate_backup: Asked to rotate: %3.2f (deg)" % math.degrees(r))
rospy.logout("rotate_backup: Asked to translate (forward-backward): %3.2f (m)" % d)
self.non_nav_backup(d)
self.non_nav_rotate(r)
# success, pose, orient = self.get_pose()
# if not success:
# return FloatFloat_Int32Response( int(False) )
# new_point = Point( pose[0], pose[1], pose[2] )
# old_rx, old_ry, old_rz = orient
# new_orient = tft.quaternion_from_euler( old_rx, old_ry, old_rz + r )
# new_quat = Quaternion( *new_orient )
# new_ps = PoseStamped()
# new_ps.header.stamp = rospy.Time(0)
# new_ps.header.frame_id = '/odom_combined'
# new_ps.pose.position = new_point
# new_ps.pose.orientation = new_quat
# self.pub.publish( new_ps )
# self.wait_for_stop()
# rospy.logout( 'rotate_backup: Done with call.' )
return FloatFloat_Int32Response(int(True))
def scan(self, tag_id):
# Note: action_request does pretty much the same thing, but with preemption.
rospy.logout("ARservoScan: Scan requested for tagid '%s'" % tag_id)
# Subscribe to RFID reader to figure out where it wants to go.
zc = servo.ZedCalc(filt_len=5, tag_id=tag_id)
def preempt_func():
# Note: I believe the variables are scoped at time of func def
zed_next = zc.next_pub() # Where does RFID want to go?
costmap_rv = self.cs.scoreTraj_NegHyst(0.1, 0.0, zed_next)
# print zed_next
# print costmap_rv
if costmap_rv == -1.0:
# rospy.logout( 'Obstacle still present.' )
return False # Still keep scaning
else:
rospy.logout("ARservoScan: Obstacle Gone... PREEMPT!")
return True # Whatever was in way is gone. Should go back to servoing.
success, found_tag, pos, frame = self.scanner.scan(preempt_func=preempt_func)
# Success will be false if it was preempted (eg. preempt_func returns True)
zc.stop() # Stop the RFID reader...
if success and found_tag:
found_tag, pos, frame = self.scanner.settle(pos, frame)
rospy.logout("ARservoScan: Scan completed.")
# print 'scan result: ', success, found_tag, pos, frame
return (success, found_tag, pos, frame)
def __init__(self, df, room, start_rad=1.5):
try:
rospy.init_node("RoomExplore")
except:
pass
self.should_run = True
self.frame = "/" + room
self.old_markers = []
self.mid = 1
self.height = df[room]["height"]
self.width = df[room]["width"]
self.listener = tf.TransformListener()
self.listener.waitForTransform("/base_link", "/map", rospy.Time(0), timeout=rospy.Duration(100))
self.pub = rospy.Publisher("visarr", Marker)
self.setup_poses(radius=start_rad) # also initializes radius
print "Len: ", len(self.poses), " Publishing."
self.publish_markers()
self._as = actionlib.SimpleActionServer("/explore", ExploreAction, execute_cb=self.action_request)
self._as.start()
self.publish_markers()
rospy.logout("Action should be started.")
