Beispiel #1
0
    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!
Beispiel #2
0
    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.")
Beispiel #3
0
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)]
Beispiel #4
0
    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 )
Beispiel #5
0
    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()
Beispiel #6
0
    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))
Beispiel #8
0
    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))
Beispiel #9
0
    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.' )
Beispiel #10
0
    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')
Beispiel #11
0
    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
Beispiel #13
0
    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!
Beispiel #14
0
    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()
Beispiel #15
0
    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' )
Beispiel #16
0
    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' )
Beispiel #17
0
    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)
Beispiel #19
0
 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'
Beispiel #20
0
    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()
Beispiel #21
0
    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)
Beispiel #22
0
 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()
Beispiel #23
0
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()
Beispiel #24
0
    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)
Beispiel #25
0
 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()
Beispiel #26
0
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))
Beispiel #27
0
    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
Beispiel #28
0
    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'
Beispiel #29
0
    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')
Beispiel #30
0
    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')
Beispiel #31
0
 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"
Beispiel #32
0
    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')
Beispiel #33
0
    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' )
Beispiel #34
0
    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.')
Beispiel #35
0
 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))
Beispiel #37
0
    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)
Beispiel #38
0
    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
Beispiel #39
0
    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
Beispiel #40
0
    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' )
Beispiel #41
0
    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()
Beispiel #42
0
    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'
Beispiel #43
0
    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')
Beispiel #44
0
    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 )
Beispiel #45
0
 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
Beispiel #46
0
    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)
Beispiel #47
0
 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)
Beispiel #49
0
    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
Beispiel #50
0
    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.' )
Beispiel #51
0
    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()
Beispiel #52
0
    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)
Beispiel #53
0
    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))
Beispiel #54
0
    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)
Beispiel #55
0
    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.")