python类has_param()的实例源码

def _read_unit_offsets(self):
        if not rospy.has_param('~num_of_units'):
            rospy.logwarn("No unit offset parameters found!")
        num_of_units = rospy.get_param('~num_of_units', 0)
        self._unit_offsets = np.zeros((num_of_units, 3))
        self._unit_coefficients = np.zeros((num_of_units, 2))
        for i in xrange(num_of_units):
            unit_params = rospy.get_param('~unit_{}'.format(i))
            x = unit_params['x']
            y = unit_params['y']
            z = unit_params['z']
            self._unit_offsets[i, :] = [x, y, z]
            p0 = unit_params['p0']
            p1 = unit_params['p1']
            self._unit_coefficients[i, :] = [p0, p1]
        rospy.loginfo("Unit offsets: {}".format(self._unit_offsets))
        rospy.loginfo("Unit coefficients: {}".format(self._unit_coefficients))

def __init__(self):

        if rospy.has_param('~orientation_offset'):
            # Orientation offset as quaterion q = [x,y,z,w].
            self.orientation_offset = rospy.get_param('~orientation_offset')
        else:
            yaw_offset_deg = rospy.get_param('~yaw_offset_deg', 0.0)
            self.orientation_offset = tf.quaternion_from_euler(0.0, 0.0, math.radians(yaw_offset_deg))

        rospy.Subscriber(rospy.get_name() + "/imu_in", Imu, self.imu_callback)

        self.pub_imu_out = rospy.Publisher(rospy.get_name() + '/imu_out',
                                           Imu, queue_size=10)

        rospy.spin()

def _read_unit_offsets(self):
        if not rospy.has_param('~num_of_units'):
            rospy.logwarn("No unit offset parameters found!")
        num_of_units = rospy.get_param('~num_of_units', 0)
        self._unit_offsets = np.zeros((num_of_units, 3))
        self._unit_coefficients = np.zeros((num_of_units, 2))
        for i in xrange(num_of_units):
            unit_params = rospy.get_param('~unit_{}'.format(i))
            x = unit_params['x']
            y = unit_params['y']
            z = unit_params['z']
            self._unit_offsets[i, :] = [x, y, z]
            p0 = unit_params['p0']
            p1 = unit_params['p1']
            self._unit_coefficients[i, :] = [p0, p1]
        rospy.loginfo("Unit offsets: {}".format(self._unit_offsets))
        rospy.loginfo("Unit coefficients: {}".format(self._unit_coefficients))

def __init__(self, name):
        rospy.init_node(name)

        self.start_rotation = None

        self.WIDTH = 320
        self.HEIGHT = 240

        self.set_x_range(-30.0, 30.0)
        self.set_y_range(-30.0, 30.0)
        self.set_z_range(-30.0, 30.0)

        self.completed_image = None

        if not rospy.has_param('/ihmc_ros/robot_name'):
            rospy.logerr("Cannot run team_zarj_main.py, missing parameters!")
            rospy.logerr("Missing parameter '/ihmc_ros/robot_name'")

        else:
            self.zarj = ZarjOS()

def lookup_feet(name, tf_buffer):
    global lf_start_position
    global lf_start_orientation
    global rf_start_position
    global rf_start_orientation

    lfp = "/ihmc_ros/{0}/left_foot_frame_name".format(name)
    rfp = "/ihmc_ros/{0}/right_foot_frame_name".format(name)

    if rospy.has_param(rfp) and rospy.has_param(lfp):
        lfname = rospy.get_param(lfp)
        rfname = rospy.get_param(rfp)

        leftFootWorld = tf_buffer.lookup_transform('world', lfname, rospy.Time())
        lf_start_orientation = leftFootWorld.transform.rotation
        lf_start_position = leftFootWorld.transform.translation

        rightFootWorld = tf_buffer.lookup_transform('world', rfname, rospy.Time())
        rf_start_orientation = rightFootWorld.transform.rotation
        rf_start_position = rightFootWorld.transform.translation

def lookupFeet(name, tfBuffer):
    global lf_start_position
    global lf_start_orientation
    global rf_start_position
    global rf_start_orientation

    lfp = "/ihmc_ros/{0}/left_foot_frame_name".format(name)
    rfp = "/ihmc_ros/{0}/right_foot_frame_name".format(name)

    if rospy.has_param(rfp) and rospy.has_param(lfp):
        lfname = rospy.get_param(lfp)
        rfname = rospy.get_param(rfp)

        leftFootWorld = tfBuffer.lookup_transform('world', lfname, rospy.Time())
        lf_start_orientation = leftFootWorld.transform.rotation
        lf_start_position = leftFootWorld.transform.translation

        rightFootWorld = tfBuffer.lookup_transform('world', rfname, rospy.Time())
        rf_start_orientation = rightFootWorld.transform.rotation
        rf_start_position = rightFootWorld.transform.translation

def lookupFeet(name, tfBuffer):
    global lf_start_position
    global lf_start_orientation
    global rf_start_position
    global rf_start_orientation

    lfp = "/ihmc_ros/{0}/left_foot_frame_name".format(name)
    rfp = "/ihmc_ros/{0}/right_foot_frame_name".format(name)

    if rospy.has_param(rfp) and rospy.has_param(lfp):
        lfname = rospy.get_param(lfp)
        rfname = rospy.get_param(rfp)

        leftFootWorld = tfBuffer.lookup_transform('world', lfname, rospy.Time())
        lf_start_orientation = leftFootWorld.transform.rotation
        lf_start_position = leftFootWorld.transform.translation

        rightFootWorld = tfBuffer.lookup_transform('world', rfname, rospy.Time())
        rf_start_orientation = rightFootWorld.transform.rotation
        rf_start_position = rightFootWorld.transform.translation

def lookupFeet(name, tfBuffer):
    global lf_start_position
    global lf_start_orientation
    global rf_start_position
    global rf_start_orientation

    lfp = "/ihmc_ros/{0}/left_foot_frame_name".format(name)
    rfp = "/ihmc_ros/{0}/right_foot_frame_name".format(name)

    if rospy.has_param(rfp) and rospy.has_param(lfp):
        lfname = rospy.get_param(lfp)
        rfname = rospy.get_param(rfp)

        leftFootWorld = tfBuffer.lookup_transform('world', lfname, rospy.Time())
        lf_start_orientation = leftFootWorld.transform.rotation
        lf_start_position = leftFootWorld.transform.translation

        rightFootWorld = tfBuffer.lookup_transform('world', rfname, rospy.Time())
        rf_start_orientation = rightFootWorld.transform.rotation
        rf_start_position = rightFootWorld.transform.translation

def get_param(param_name):
    """
    GET /api/<version>/param/<param_name>

    GET the value for a specific parameter at param_name in the ROS
    Parameter Server.

    Parameter value is in the "result" field of the JSON response body. If
    parameter does not exist, a JSON document with ERROR field will
    be returned.
    """
    if not rospy.has_param(param_name):
        return error("No such parameter exists")
    return jsonify({"result": str(rospy.get_param(param_name))})

def init_config( self ):
        # mandatory configurations to be set
        self.config['frame_rate'] = rospy.get_param('~frame_rate')
        self.config['source'] = rospy.get_param('~source')
        self.config['resolution'] = rospy.get_param('~resolution')
        self.config['color_space'] = rospy.get_param('~color_space')

        # optional for camera frames
        # top camera with default
        if rospy.has_param('~camera_top_frame'):
            self.config['camera_top_frame'] = rospy.get_param('~camera_top_frame')
        else:
            self.config['camera_top_frame'] = "/CameraTop_optical_frame"
        # bottom camera with default
        if rospy.has_param('~camera_bottom_frame'):
            self.config['camera_bottom_frame'] = rospy.get_param('~camera_bottom_frame')
        else:
            self.config['camera_bottom_frame'] = "/CameraBottom_optical_frame"
        # depth camera with default
        if rospy.has_param('~camera_depth_frame'):
            self.config['camera_depth_frame'] = rospy.get_param('~camera_depth_frame')
        else:
            self.config['camera_depth_frame'] = "/CameraDepth_optical_frame"

        #load calibration files
        if rospy.has_param('~calibration_file_top'):
            self.config['calibration_file_top'] = rospy.get_param('~calibration_file_top')
        else:
            rospy.loginfo('no camera calibration for top camera found')

        if rospy.has_param('~calibration_file_bottom'):
            self.config['calibration_file_bottom'] = rospy.get_param('~calibration_file_bottom')
        else:
            rospy.loginfo('no camera calibration for bottom camera found')

        # set time reference
        if rospy.has_param('~use_ros_time'):
            self.config['use_ros_time'] = rospy.get_param('~use_ros_time')
        else:
            self.config['use_ros_time'] = False

def set_param(self, param_name, val):
        # Set private var value
        setattr(self, '_{}'.format(param_name), val)
        if val == None:
            full_param_name = '/needybot/blackboard/{}'.format(param_name)
            if rospy.has_param(full_param_name):
                rospy.delete_param(full_param_name)
            self.publish("delete_{}".format(param_name))
        else:
            rospy.set_param('/needybot/blackboard/{}'.format(param_name), val)
            self.publish("set_{}".format(param_name))

def lookup_frame(self, frame):
        """ helper to lookup frames"""
        source = "/ihmc_ros/{0}/{1}".format(self.robot_name, frame)
        if rospy.has_param(source):
            return rospy.get_param(source)
        return None

def __init__(self, name):
        rospy.init_node(name)

        if not rospy.has_param('/ihmc_ros/robot_name'):
            rospy.logerr("Cannot run team_zarj_main.py, missing parameters!")
            rospy.logerr("Missing parameter '/ihmc_ros/robot_name'")

        else:
            self.zarj_os = ZarjOS()

def __init__(self, name):
        rospy.init_node(name)
        self.zarj_os = ZarjOS()
        if not rospy.has_param('/ihmc_ros/robot_name'):
            rospy.logerr("Cannot run team_zarj_main.py, missing parameters!")
            rospy.logerr("Missing parameter '/ihmc_ros/robot_name'")
        else:
            self.zarj_os = ZarjOS()

def __init__(self, name):
        rospy.init_node(name)

        if not rospy.has_param('/ihmc_ros/robot_name'):
            rospy.logerr("Cannot run team_zarj_main.py, missing parameters!")
            rospy.logerr("Missing parameter '/ihmc_ros/robot_name'")

        else:
            self.zarj_os = ZarjOS()

def __init__(self, name):
        rospy.init_node(name)

        if not rospy.has_param('/ihmc_ros/robot_name'):
            rospy.logerr("Cannot run team_zarj_main.py, missing parameters!")
            rospy.logerr("Missing parameter '/ihmc_ros/robot_name'")
        else:
            self.zarj = zarj.ZarjOS()

def __init__(self, name):
        rospy.init_node(name)

        if not rospy.has_param('/ihmc_ros/robot_name'):
            rospy.logerr("Cannot run team_zarj_main.py, missing parameters!")
            rospy.logerr("Missing parameter '/ihmc_ros/robot_name'")
        else:
            self.zarj = zarj.ZarjOS()

def __init__(self, name):
        rospy.init_node(name)

        if not rospy.has_param('/ihmc_ros/robot_name'):
            rospy.logerr("Cannot run team_zarj_main.py, missing parameters!")
            rospy.logerr("Missing parameter '/ihmc_ros/robot_name'")

        else:
            self.zarj = ZarjOS()
            #self.hand_trajectory_publisher = rospy.Publisher(
            #    "/ihmc_ros/{0}/control/hand_trajectory".format(self.zarj.robot_name),
            #    HandTrajectoryRosMessage, queue_size=10)

def __init__(self, name):
        rospy.init_node(name)

        self.harness = None

        if not rospy.has_param('/ihmc_ros/robot_name'):
            rospy.logerr("Cannot run, missing /ihmc_ros/robot_name")

def start(self, name):
        """ Start the field computer """
        rospy.init_node(name)
        self.oclog('Getting robot_name')
        while not rospy.has_param('/ihmc_ros/robot_name'):
            print "-------------------------------------------------"
            print "Cannot run team_zarj_main.py, missing parameters!"
            print "Missing parameter '/ihmc_ros/robot_name'"
            print "Likely connection to ROS not present. Retry in 1 second."
            print "-------------------------------------------------"
            time.sleep(1)

        self.oclog('Booting ZarjOS')
        self.zarj = ZarjOS()

        self.start_task_service = rospy.ServiceProxy(
            "/srcsim/finals/start_task", StartTask)
        self.task_subscriber = rospy.Subscriber(
            "/srcsim/finals/task", Task, self.task_status)
        self.harness_subscriber = rospy.Subscriber(
            "/srcsim/finals/harness", Harness, self.harness_status)
        if HAS_SCORE:
            self.score_subscriber = rospy.Subscriber(
                "/srcsim/finals/score", Score, self.score_status)

        rate = rospy.Rate(10) # 10hz
        if self.task_subscriber.get_num_connections() == 0:
            self.oclog('waiting for task publisher...')
            while self.task_subscriber.get_num_connections() == 0:
                rate.sleep()
            if self.harness_subscriber.get_num_connections() == 0:
                self.oclog('waiting for harness publisher...')
                while self.harness_subscriber.get_num_connections() == 0:
                    rate.sleep()
            self.oclog('...got it, field computer is operational!')

def fetch_param(name, default):
  if rospy.has_param(name):
    return rospy.get_param(name)
  else:
    print "parameter [%s] not defined. Defaulting to %.3f" % (name, default)
    return default

def define(self):
  self.say=rospy.Publisher('speak_string', String, queue_size=1)
  if not rospy.has_param('~words'):
   rospy.set_param('~words','???????')

  if not rospy.has_param('~SpeakerSubTopic'):
   rospy.set_param('~SpeakerSubTopic', 'stop_flag')

  self.words=rospy.get_param('~words')
  self.topic=rospy.get_param('~SpeakerSubTopic')

def shutdown_hook():

    #check if there is a file to save to
    if rospy.has_param("~layer_file"):          
        layer_file = rospy.get_param("~layer_file")

        #extract layer data for saving
        layer_data = {}
        global layer_objects
        for name in layer_objects:
            layer_data[name] = layer_objects[name].get_layer_data()

        #save data
        pickle.dump( layer_data, open( layer_file, "wb" ) )
        rospy.loginfo("Layer file saved  "+layer_file)

def get_param(name, value=None):
    private = "~%s" % name
    if rospy.has_param(private):
        return rospy.get_param(private)
    elif rospy.has_param(name):
        return rospy.get_param(name)
    else:
        return value

def get_param(name, value=None):
    private = "~%s" % name
    if rospy.has_param(private):
        return rospy.get_param(private)
    elif rospy.has_param(name):
        return rospy.get_param(name)
    else:
        return value

def __init__(self, zarj_os):
        log("Zarj walk initialization begin")
        self.zarj = zarj_os
        self.steps = 0
        self.publishers = []
        self.planned_steps = 0
        self.stride = self.DEFAULT_STRIDE
        self.transfer_time = self.DEFAULT_TRANSFER_TIME
        self.swing_time = self.DEFAULT_SWING_TIME
        self.stance_width = self.DEFAULT_STANCE_WIDTH
        self.lf_start_position = Vector3()
        self.lf_start_orientation = Quaternion()
        self.rf_start_position = Vector3()
        self.rf_start_orientation = Quaternion()
        self.behavior = 0
        self.start_walk = None
        self.walk_failure = None
        self.last_footstep = None

        lfp = "/ihmc_ros/{0}/left_foot_frame_name".format(self.zarj.robot_name)
        rfp = "/ihmc_ros/{0}/right_foot_frame_name".format(self.zarj.robot_name)

        if rospy.has_param(rfp) and rospy.has_param(lfp):
            self.lfname = rospy.get_param(lfp)
            self.rfname = rospy.get_param(rfp)

        if self.lfname == None or self.rfname == None:
            log("Zarj could not find left or right foot name")
            raise RuntimeError

        self.footstep_list_publisher = rospy.Publisher(
            "/ihmc_ros/{0}/control/footstep_list".format(self.zarj.robot_name),
            FootstepDataListRosMessage, queue_size=10)
        self.publishers.append(self.footstep_list_publisher)

        self.footstep_status_subscriber = rospy.Subscriber(
            "/ihmc_ros/{0}/output/footstep_status".format(self.zarj.robot_name),
            FootstepStatusRosMessage, self.receive_footstep_status)

        self.behavior_subscriber = rospy.Subscriber(
            "/ihmc_ros/{0}/output/behavior".format(self.zarj.robot_name),
            Int32, self.receive_behavior)

        self.abort_publisher = rospy.Publisher(
            "/ihmc_ros/{0}/control/abort_walking".format(self.zarj.robot_name),
            AbortWalkingRosMessage, queue_size=1)
        self.publishers.append(self.abort_publisher)

        self.lookup_feet()
        self.zarj.wait_for_publishers(self.publishers)

        log("Zarj walk initialization completed")

def main(limb_name, reset):
    """
    Parameters
    ----------
    limb : str
        Which limb to use. Choices are {'left', 'right'}
    reset : bool
        Whether to use previously saved picking and placing poses or
        to save new ones by using 0g mode and the OK cuff buttons.
    """
    # Initialise ros node
    rospy.init_node("pick_and_place", anonymous=False)
    #pick_pose = []
    #place_pose = []
    #for ii in range(3):
    if reset or not rospy.has_param('~pick_and_place_poses'):
        rospy.loginfo(
            'Saving picking pose for %s limb' % limb_name)
        pick_pose = limb_pose(limb_name)
        rospy.sleep(1)
        place_pose = limb_pose(limb_name)
        rospy.set_param('~pick_and_place_poses',
                        {'pick': pick_pose.tolist(),
                         'place': place_pose.tolist()})
        #rospy.loginfo('pick_pose is %s' % pick_pose)
        #rospy.loginfo('place_pose is %s' % place_pose)
    else:
        pick_pose = rospy.get_param('~pick_and_place_poses/pick')
        place_pose =  rospy.get_param('~pick_and_place_poses/place')

    b = Baxter(limb_name)

    c1 = Controller()

    f = FilterValues()
    f.start_recording()
    #for ii in range(3):
    b.pick(pick_pose, controller=c1)
    b.place(place_pose)

    c1.save_centeringerr()

    f.stop_recording()
    f.convertandsave() #convert to numpy and save the recoreded data
    f.filter()
    f.plot()

def main(limb_name, reset):
    rospy.init_node("pick_and_place", anonymous=True)
    if reset or not rospy.has_param('~pick_and_place_poses'):
        #rospy.loginfo(
            #'Saving picking pose for %s limb' % limb_name)
        pick_pose = limb_pose(limb_name)
        rospy.sleep(1)
        place_pose = limb_pose(limb_name)
        rospy.set_param('~pick_and_place_poses',
                        {'pick': pick_pose.tolist(),
                         'place': place_pose.tolist()})
        #rospy.loginfo('pick_pose is %s' % pick_pose)
        #rospy.loginfo('place_pose is %s' % place_pose)
    else:
        pick_pose = rospy.get_param('~pick_and_place_poses/pick')
        place_pose = rospy.get_param('~pick_and_place_poses/place')

    b = Baxter(limb_name)
    while True:
        ser.write(b'm')
        s=''
        line = ser.readline()
        line = re.sub(' +',',',line)
        line = re.sub('\r\n','',line)
        pre = line.split(",")
        err = [(int(pre[j+8])-int(pre[j])) for j in range(1,8)]
        sum1 = 0
        for i in range(0,7,1):
            sum1 = sum1 + err[i]
        avg = sum1/7
        print(avg)

        endeffvel = baxter_interface.Limb(limb_name).endpoint_velocity()
        print(endeffvel)

        jacobianinv = baxter_kinematics(limb_name).jacobian_pseudo_inverse()
        print(jacobianinv)

        if avg>50:
            b.map_keyboardR()
        elif avg<-50:
            b.map_keyboardL()
        else:
            break

def main(limb_name, reset):
    """
    Parameters
    ----------
    limb : str
        Which limb to use. Choices are {'left', 'right'}
    reset : bool
        Whether to use previously saved picking and placing poses or
        to save new ones by using 0g mode and the OK cuff buttons.
    """
    # Initialise ros node
    rospy.init_node("pick_and_place", anonymous=False)

    # Either load picking and placing poses from the parameter server
    # or save them using the 0g mode and the circular buttons on
    # baxter's cuffs
    if reset or not rospy.has_param('~pick_and_place_poses'):
        rospy.loginfo(
            'Saving picking pose for %s limb' % limb_name)
        pick_pose = limb_pose(limb_name)
        rospy.sleep(1)
        place_pose = limb_pose(limb_name)
        # Parameter server can't store numpy arrays, so make sure
        # they're lists of Python floats (specifically not
        # numpy.float64's). I feel that's a bug in rospy.set_param.
        rospy.set_param('~pick_and_place_poses',
                        {'pick': pick_pose.tolist(),
                         'place': place_pose.tolist()})
        #rospy.loginfo('pick_pose is %s' % pick_pose)
        #rospy.loginfo('place_pose is %s' % place_pose)
    else:
        pick_pose = rospy.get_param('~pick_and_place_poses/pick')
        place_pose = rospy.get_param('~pick_and_place_poses/place')

    b = Baxter(limb_name)

    c = Controller()
    c1 = Controller_1()
    c2 = Controller_2()
    #f = FilterValues()
    #f.start_recording()
    for i in range(20):
        print ('this iss the intial pick pose')
        pick_pose[1]= 0.25286245 #change this for every new exp
        print (pick_pose)
        #pick_pose[1] = 0.30986200091872873
        pick_pose[1] += random.uniform(-1,1)*0.00 ##introduce error in endposition (y axis)
        print ('ERROR introduced the intial pick pose')
        print (pick_pose)
        b.pick(pick_pose, controller=c, controller_1=None, controller_2 = c2)
        b.place(place_pose)
    #f.stop_recording()
    #f.filter()
    #f.plot()
    rospy.spin()

def layer_server():

    #start node
    rospy.init_node('map_layer_server')
    rospy.on_shutdown(shutdown_hook)

    #init data names
    global layer_objects
    layer_objects = {}
    layer_file = None
    layer_data = None

    #check if there is a file name
    if rospy.has_param("~layer_file"):
        layer_file = rospy.get_param("~layer_file")
        rospy.loginfo("Map server set to load and save data")

        #check if this file exists, if so, load from it
        if os.path.isfile(layer_file):
            rospy.loginfo("Layer file loaded from "+layer_file)
            layer_data = pickle.load( open( layer_file, "rb" ) )        

    #load parameters
    layer_names = rospy.get_param("~layer_names")
    layer_name_list = layer_names.split()

    #set up layers
    for name in layer_name_list:

        #load layer configuration parameters
        layer_cfg = rospy.get_param("~"+name)

        #create layers with given configuration
        #reload saved data if available
        if layer_data and layer_data[name]:
            layer_objects[name] = layer_instance(name,layer_cfg,layer_data=layer_data[name])
        else:
            layer_objects[name] = layer_instance(name,layer_cfg)    

    #set up service hanlder
    s = rospy.Service("map_layer_simple_lookup",SimpleLookup,SimpleLookupCallback)
    rospy.loginfo("Map Layer Server ready")

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()