python类Float64()的实例源码

def test_publish_to_topics(self):
        topic_ending = "desired"
        rospy.logdebug("Sleeping for 5 seconds to let ROS spin up...")
        rospy.sleep(5)
        for variable, value in self.variables:
            # Publish to each variable/desired topic to see if all of the
            # actuators come on as expected.
            topic_string = variable + "/" + topic_ending
            rospy.logdebug("Testing Publishing to " + topic_string)
            pub_desired = rospy.Publisher(topic_string,
                                               Float64, queue_size=10)
            sub_desired = rospy.Subscriber(topic_string, Float64, self.callback)
            rospy.sleep(2)
            print(self.namespace + "/" + topic_string)
            for _ in range(NUM_TIMES_TO_PUBLISH):
                pub_desired.publish(value)
                rospy.sleep(1)
            rospy.sleep(2)
            pub_desired.publish(0)

def update_telemetry(navsat_msg, compass_msg):
    """Telemetry subscription callback.

    Args:
        navsat_msg: sensor_msgs/NavSatFix message.
        compass_msg: std_msgs/Float64 message in degrees.
    """
    try:
        client.post_telemetry(navsat_msg, compass_msg)
    except (ConnectionError, Timeout) as e:
        rospy.logwarn(e)
        return
    except (ValueError, HTTPError) as e:
        rospy.logerr(e)
        return
    except Exception as e:
        rospy.logfatal(e)
        return

def test_post_telemetry(self):
        """Tests posting telemetry data through client."""
        # Set up test data.
        url = "http://interop"
        client_args = (url, "testuser", "testpass", 1.0)

        with InteroperabilityMockServer(url) as server:
            # Setup mock server.
            server.set_root_response()
            server.set_login_response()
            server.set_telemetry_response()

            # Connect client.
            client = InteroperabilityClient(*client_args)
            client.wait_for_server()
            client.login()
            client.post_telemetry(NavSatFix(), Float64())

def test_telemetry_serializer(self):
        """Tests telemetry serializer."""
        # Set up test data.
        navsat = NavSatFix()
        navsat.latitude = 38.149
        navsat.longitude = -76.432
        navsat.altitude = 30.48
        compass = Float64(90.0)

        data = serializers.TelemetrySerializer.from_msg(navsat, compass)
        altitude_msl = serializers.meters_to_feet(navsat.altitude)

        # Compare.
        self.assertEqual(data["latitude"], navsat.latitude)
        self.assertEqual(data["longitude"], navsat.longitude)
        self.assertEqual(data["altitude_msl"], altitude_msl)
        self.assertEqual(data["uas_heading"], compass.data)

def __init__(self):
        """
        Create HeadLiftJoy object.

        """
        # params
        self._head_axes = rospy.get_param('~head_axes', 3)
        self._lift_axes = rospy.get_param('~lift_axes', 3)
        self._head_button = rospy.get_param('~head_button', 4)
        self._lift_button = rospy.get_param('~lift_button', 5)

        # pubs
        self._head_pub = rospy.Publisher('head_angle', Float64, queue_size=1)
        self._lift_pub = rospy.Publisher('lift_height', Float64, queue_size=1)

        # subs
        self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_cb, queue_size=1)

def __init__(self):
        # Read Settings
        self.read_settings()

        # Init other variables
        self._num_magnetometer_reads = 0
        self._latest_bearings = np.zeros(shape = (self._number_samples_average, 1))
        self._received_enough_samples = False

        # Subscribe to magnetometer topic
        rospy.Subscriber("magnetic_field", Vector3Stamped, self.magnetic_field_callback)

        # Publishers
        self._pub_bearing_raw = rospy.Publisher(rospy.get_name() + '/bearing_raw_deg',
                                                Float64, queue_size = 10)
        self._pub_bearing_avg = rospy.Publisher(rospy.get_name() + '/bearing_avg_deg',
                                                Float64, queue_size = 10)
        self._pub_imu_bearing_avg = rospy.Publisher(rospy.get_name() + '/imu_bearing_avg',
                                                Imu, queue_size = 10)

        if self._verbose:
            self._pub_mag_corrected = rospy.Publisher(rospy.get_name() + '/mag_corrected',
                                                      Vector3Stamped, queue_size = 10)

        rospy.spin()

def __init__(self):
        # setup
        CozmoTeleop.settings = termios.tcgetattr(sys.stdin)
        atexit.register(self.reset_terminal)

        # vars
        self.head_angle = STD_HEAD_ANGLE
        self.lift_height = STD_LIFT_HEIGHT

        # params
        self.lin_vel = rospy.get_param('~lin_vel', 0.2)
        self.ang_vel = rospy.get_param('~ang_vel', 1.5757)

        # pubs
        self._head_pub = rospy.Publisher('head_angle', Float64, queue_size=1)
        self._lift_pub = rospy.Publisher('lift_height', Float64, queue_size=1)
        self._cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)

def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # node are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('listener', anonymous=True)

    rospy.Subscriber("/turtlebot_angle", Float64, callback1)
    rospy.Subscriber("/turtlebot_posex", Float64, callback2)
    rospy.Subscriber("/turtlebot_posey", Float64, callback3)

    rospy.Subscriber("/turtlebot_follower/dist_object1", Float64, callback4)
    rospy.Subscriber("/turtlebot_follower/dist_object2", Float64, callback5)
    rospy.Subscriber("/turtlebot_follower/dist_object3", Float64, callback6)
    rospy.Subscriber("/turtlebot_follower/dist_object4", Float64, callback7)
    #rospy.Subscriber("/turtlebot_follower/dist_object", Float64, callback8)

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

def __init__(self):
        self.joint_name = rospy.get_param("~joint_name")
        self.command_pub = rospy.Publisher("command", Float64, queue_size=1)
        self.joint_sub = rospy.Subscriber("joint_state", JointState,
                                          self.joint_state_callback, queue_size=1)

def __init__(self):
        self.joint_name = rospy.get_param("~joint_name")
        self.joint_state = JointState()
        self.joint_state.name.append(self.joint_name)
        self.joint_state.position.append(0.0)
        self.joint_state.velocity.append(0.0)
        self.joint_pub = rospy.Publisher("joint_states", JointState, queue_size=1)
        self.command_sub = rospy.Subscriber("command", Float64,
                                            self.command_callback, queue_size=1)

def create_persistence_objects(
    server, environment_id, max_update_interval, min_update_interval
):
    env_var_db = server[ENVIRONMENTAL_DATA_POINT]
    for variable in ENVIRONMENT_VARIABLES.itervalues():
        variable = str(variable)
        topic = "{}/measured".format(variable)
        TopicPersistence(
            topic=topic, topic_type=Float64,
            environment=environment_id,
            variable=variable, is_desired=False,
            db=env_var_db, max_update_interval=max_update_interval,
            min_update_interval=min_update_interval
        )

def filter_topic(src_topic, dest_topic, topic_type):
    """
    Publishes a measured data point given the raw value by applying the EWMA function to the data.
    :param src_topic: String? - Source topic signal to be filtered
    :param dest_topic: String? - Output topic to publish new data to
    :param topic_type: The data type of the topic, aka Float64, Int32, etc
    :return: sub, pub : subscribed topic (raw measured value) and published topic (filtered (smoothed) value)
    """
    rospy.loginfo("Filtering topic {} to topic {}".format(
        src_topic, dest_topic
    ))
    pub = rospy.Publisher(dest_topic, topic_type, queue_size=10)
    f = EWMA(0.3)
    def callback(src_item):
        value = src_item.data
        # If the value is our magic number, leave it alone
        if value in SENTINELS:
            dest_item = value
        else:
            f(src_item.data)
            dest_item = topic_type(f.average)
        pub.publish(dest_item)
    sub = rospy.Subscriber(src_topic, topic_type, callback)
    return sub, pub

def filter_all_variable_topics(variables):
    """
    Given an iterator publishers, where each publisher is a two-tuple
    `(topic, type)`, publishes a filtered topic endpoint.
    """
    for env_var in variables:
        src_topic = "{}/raw".format(env_var)
        dest_topic = "{}/measured".format(env_var)
        # Ignore type associated with environmental variable type and
        # coerce to Float64

        # @FIXME this is a short-term fix for preventing boolean values from
        # being filtered by the EWMA filter.
        #
        # Explanation: right now all topics under `/environment/<id>` are
        # float64 type, with Boolean being 1 or 0. This was judged to be a
        # simpler architecture at the time. Values from sensors may be any
        # type, but are coerced to Float64. The same is true for actuators.
        # However, this assumption breaks down for filtering boolean values,
        # since the EWMA will produce fractional numbers that don't coerce
        # correctly back to boolean.
        #
        # In future, we should change the architecture of the system to support
        # standard ros types under `/environment/<id>`.
        if env_var.type is None or 'boolean' in env_var.type.lower():
            forward_topic(src_topic, dest_topic, Float64)
        else:
            filter_topic(src_topic, dest_topic, Float64)

def on_data(self, item):
        curr_time = time.time()
        value = item.data
        if value is None or value == self.last_value:
            return
        # This is kind of a hack to correctly interpret UInt8MultiArray
        # messages. There should be a better way to do this
        if item._slot_types[item.__slots__.index('data')] == "uint8[]":
            value = [ord(x) for x in value]
        # Throttle updates by value only (not time)
        if self.topic_type == Float64 and \
           self.last_value is not None and \
           self.last_value != 0.0:
            delta_val = value - self.last_value
            if abs(delta_val / self.last_value) <= 0.01:
                return
        # Save the data point
        point = EnvironmentalDataPoint({
            "environment": self.environment,
            "variable": self.variable,
            "is_desired": self.is_desired,
            "value": value,
            "timestamp": curr_time
        })
        point_id = gen_doc_id(curr_time)
        self.db[point_id] = point
        self.last_value = value

def run(self):
        """ Run our code """
        rospy.loginfo("Start laser test code")

        #self.blank_image()
        #for x in range(0, 99):
        #    self.add_point(x * 1.0, x * 1.0, 10)

        #cv2.imshow("Hackme", self.img)
        #cv2.waitKey(0)

        self.joint_subscriber = rospy.Subscriber("/multisense/joint_states", JointState, self.recv_joint_state)
        self.scan_subscriber = rospy.Subscriber("/multisense/lidar_scan", LaserScan, self.recv_scan)
        self.spindle = rospy.Publisher("/multisense/set_spindle_speed", Float64, queue_size=10)

        self.set_x_range(-1.0, 1.0)
        self.set_y_range(-1.0, 1.0)
        self.set_z_range(0.0, 2.0)

        rospy.sleep(0.1)
        rospy.loginfo("Setting spindle going")
        self.spindle.publish(Float64(1.0))

        #self.zarj.zps.look_down()

        rospy.loginfo("Spin forever, hopefully receiving data...")
        while True:
            rospy.sleep(1.0)
            #print "Got cloud {}x{}".format(resp.cloud.height, resp.cloud.width)
            #img = self.create_image_from_cloud(resp.cloud.points)
            #cv2.destroyAllWindows()
            #print "New image"
            #cv2.imshow("My image", img)
            #cv2.waitKey(1)
            #print resp
            #cv_image = self.bridge.imgmsg_to_cv2(resp.cloud, "bgr8")
            #cv2.imshow("Point cloud", cv_image)

def __init__(self, P=1/30000):
        self.limb_name = 'left'
        self.other_limb_name = 'right'
        self.limb = baxter_interface.Limb(self.limb_name)
        self.err_pub = rospy.Publisher('/error', Float64, queue_size=1)
        self.P = P
        self.kinematics = baxter_kinematics(self.limb_name)
        self.jinv = self.kinematics.jacobian_pseudo_inverse()

def compute_faii(self):
        filtered_acc = signal.lfilter(self.b1, self.a1, self.acc, axis=0)
        self.faii = np.sqrt((filtered_acc**2).sum(axis=1))
        # Publish just the last value
        val = self.faii[-1]
        self.faii_pub.publish(Float64(val))
        if self.recording:
            t = time()
            self.data['faii'].append((t, val))

def __init__(self):
        rospy.init_node('gripper_controller')


        self.r_pub = rospy.Publisher('gripper_controller/command', Float64, queue_size=10)

        # subscribe to command and then spin
        self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
        self.server.start()
        rospy.spin()

def actionCb(self, goal):
        """ Take an input command of width to open gripper. """
        rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position)
        command = goal.command.position

        # publish msgs

        rmsg = Float64(command)

        self.r_pub.publish(rmsg)
        rospy.sleep(5.0)
        self.server.set_succeeded()
        rospy.loginfo('Gripper Controller: Done.')

def __init__(self):
        rospy.init_node('gripper_controller')


        self.r_pub = rospy.Publisher('gripper_controller/command', Float64, queue_size=10)

        # subscribe to command and then spin
        self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
        self.server.start()
        rospy.spin()

def actionCb(self, goal):
        """ Take an input command of width to open gripper. """
        rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position)
        command = goal.command.position

        # publish msgs

        rmsg = Float64(command)

        self.r_pub.publish(rmsg)
        rospy.sleep(5.0)
        self.server.set_succeeded()
        rospy.loginfo('Gripper Controller: Done.')

def __init__(self):
        self.joint_names = ['base_joint', 'shoulder_joint', 'elbow_joint', 'wrist_flex_joint', 'wrist_rot_joint',
                            'finger_joint1', 'finger_joint2']
        # subscriber for setting joints position
        self.states_sub = rospy.Subscriber("/as_arm/set_joints_states", JointState, self.callback,
                                           queue_size=2)
        # a list of publisher
        self.joint_pub = dict()
        self.joint_pose = dict()
        for idx, name in enumerate(self.joint_names):
            pub = rospy.Publisher("/as_arm/joint%d_position_controller/command" % (idx + 1), Float64, queue_size=3)
            self.joint_pub[name] = pub
            self.joint_pose[name] = Float64()

def __init__(self, max_random_start,
               observation_dims, data_format, display, use_cumulated_reward):

    self.max_random_start = max_random_start
    self.action_size = 28

    self.use_cumulated_reward = use_cumulated_reward
    self.display = display
    self.data_format = data_format
    self.observation_dims = observation_dims

    self.screen = np.zeros((self.observation_dims[0],self.observation_dims[1]), np.uint8)

    #self.dirToTarget = 0
    self.robotPose = Pose()
    self.goalPose = Pose()
    self.robotRot = 0.0
    self.prevDist = 0.0
    self.numWins = 0
    self.ep_reward = 0.0
    self.readyForNewData = True
    self.terminal = 0
    self.sendTerminal = 0
    self.minFrontDist = 6

    self.r = 1
    self.ang = 0

    rospy.wait_for_service('reset_positions')
    self.resetStage = rospy.ServiceProxy('reset_positions', EmptySrv)


    # Subscribers:
    rospy.Subscriber('/input_data', stage_message, self.stageCB, queue_size=10)

    # publishers:
    self.pub_vel_ = rospy.Publisher('cmd_vel', Twist, queue_size = 10)
    self.pub_rew_ = rospy.Publisher("lastreward",Float64,queue_size=10)
    self.pub_goal_ = rospy.Publisher("target", Pose, queue_size = 15)

def __init__(self, max_random_start,
               observation_dims, data_format, display, use_cumulated_reward):

    self.max_random_start = max_random_start
    self.action_size = 28

    self.use_cumulated_reward = use_cumulated_reward
    self.display = display
    self.data_format = data_format
    self.observation_dims = observation_dims

    self.screen = np.zeros((self.observation_dims[0],self.observation_dims[1]), np.uint8)

    #self.dirToTarget = 0
    self.robotPose = Pose()
    self.goalPose = Pose()
    self.robotRot = 0.0
    self.prevDist = 0.0
    self.numWins = 0
    self.ep_reward = 0.0
    self.readyForNewData = True
    self.terminal = 0
    self.sendTerminal = 0
    self.minFrontDist = 6

    rospy.wait_for_service('reset_positions')
    self.resetStage = rospy.ServiceProxy('reset_positions', EmptySrv)


    # Subscribers:
    rospy.Subscriber('/input_data', stage_message, self.stageCB, queue_size=10)

    # publishers:
    self.pub_vel_ = rospy.Publisher('cmd_vel', Twist, queue_size = 10)
    self.pub_rew_ = rospy.Publisher("lastreward",Float64,queue_size=10)
    self.pub_goal_ = rospy.Publisher("target", Pose, queue_size = 15)

def __init__(self):
        self.joint_name = rospy.get_param("~joint_name")
        self.joint_state = JointState()
        self.joint_state.name.append(self.joint_name)
        self.joint_state.position.append(0.0)
        self.joint_state.velocity.append(0.0)
        self.joint_pub = rospy.Publisher("joint_states", JointState, queue_size=1)
        self.command_sub = rospy.Subscriber("command", Float64,
                                            self.command_callback, queue_size=1)

def __init__(self,side,ik=True):
        limb.Limb.__init__(self,side)
        self.side=side

        self.DEFAULT_BAXTER_SPEED=0.3

        if not side in ["left","right"]:
            raise BaxterException,"Error non existing side: %s, please provide left or right"%side

        self.post=Post(self)
        self.__stop = False
        self.simple=self
        self._moving=False
        self.moving_lock=Lock()

        self.ik=ik        
        if self.ik:            
            self.ns = "/ExternalTools/%s/PositionKinematicsNode/IKService"%self.side    
            rospy.loginfo("Waiting for inverse kinematics service on %s..."%self.ns)
            rospy.wait_for_service(self.ns)
            self.iksvc = rospy.ServiceProxy(self.ns, SolvePositionIK)
            rospy.loginfo("Waiting for inverse kinematics service DONE")
        else:
            rospy.loginfo("Skipping inverse kinematics service loading")

        #self.simple_limb_srv = rospy.Service("/simple_limb/"+side,LimbPose,self.__cbLimbPose)

        self._pub_speed=rospy.Publisher("/robot/limb/%s/set_speed_ratio"%self.side,Float64,queue_size=1)
        while not rospy.is_shutdown() and self._pub_speed.get_num_connections() < 1:
            rospy.sleep(0.1)
        #self.setSpeed(3)


#     def __cbLimbPose(self,msg):
#         cmd = msg.command
#         if cmd == "goToPose":
#             resp = self.goToPose(msg.pose, msg.speed, msg.position_tolerance, msg.orientation_tolerance, msg.joint_tolerance, msg.speed_tolerance, msg.timeout, cartesian, msg.path_tolerance)
#         elif cmd=="goToAngles":
#             resp = self.goToAngles(msg.angles, msg.speed, msg.joint_tolerance_plan, msg.joint_tolerance, msg.speed_tolerance, msg.timeout, msg.path_tolerance)
#         return LimbPoseResponse(resp)

def setSpeed(self,speed):
        finalspeed=self.DEFAULT_BAXTER_SPEED*speed
        if finalspeed>1: finalspeed=1
        if finalspeed<0: finalspeed=0
        try:
            self._pub_speed.publish(Float64(finalspeed))
        except:
            pass

def __init__(self, node_name, sub_topic, sub_base_throttle_topic, pub_topic, pub_setpoint_topic, pub_state_topic, pub_throttle_topic, reset_service):
        self.bridge = CvBridge()
        self.img_prep = ImagePreparator()
        self.ipm = InversePerspectiveMapping()

        # Publisher
        self.image_pub = rospy.Publisher(pub_topic, Image, queue_size=QUEUE_SIZE)
        self.setpoint_pub = rospy.Publisher(pub_setpoint_topic, Float64, queue_size=QUEUE_SIZE)
        self.state_pub = rospy.Publisher(pub_state_topic, Float64, queue_size=QUEUE_SIZE)
        self.throttle_pub = rospy.Publisher(pub_throttle_topic, Float64, queue_size=QUEUE_SIZE)

        self.reset_srv = rospy.Service(reset_service, Empty, self.reset_callback)
        self.reset_tracking = False

        rospy.init_node(node_name, anonymous=True)

        self.image_sub = rospy.Subscriber(sub_topic, Image, self.callback)
        self.base_throttle_sub = rospy.Subscriber(sub_base_throttle_topic, Float64, self.callbackBaseThrottle)

        # Base Throttle
        self.base_throttle = rospy.get_param("/autonomous_driving/lane_detection_node/base_throttle", 0.6)

        # Crop Parameters
        self.above_value = rospy.get_param("/autonomous_driving/lane_detection_node/above", 0.58)
        self.below_value = rospy.get_param("/autonomous_driving/lane_detection_node/below", 0.1)
        self.side_value = rospy.get_param("/autonomous_driving/lane_detection_node/side", 0.3)

        # Lane Tracking Parameters
        self.deviation = rospy.get_param("/autonomous_driving/lane_detection_node/deviation", 5)
        self.border = rospy.get_param("/autonomous_driving/lane_detection_node/border", 0)

        # Canny Parameters
        self.threshold_low = rospy.get_param("/autonomous_driving/lane_detection_node/threshold_low", 50)
        self.threshold_high = rospy.get_param("/autonomous_driving/lane_detection_node/threshold_high", 150)
        self.aperture = rospy.get_param("/autonomous_driving/lane_detection_node/aperture", 3)

        # Lane Tracking
        self.init_lanemodel()

        rospy.spin()

def __init__(self, node_name, sub_topic, pub_topic, pub_setpoint_topic, pub_state_topic, reset_service):
        self.bridge = CvBridge()
        self.init_lanemodel()
        self.image_pub = rospy.Publisher(pub_topic, Image, queue_size=QUEUE_SIZE)
        self.setpoint_pub = rospy.Publisher(pub_setpoint_topic, Float64, queue_size=QUEUE_SIZE)
        self.state_pub = rospy.Publisher(pub_state_topic, Float64, queue_size=QUEUE_SIZE)

        self.reset_srv = rospy.Service(reset_service, Empty, self.reset_callback)
        self.reset_tracking = False

        rospy.init_node(node_name, anonymous=True)

        self.image_sub = rospy.Subscriber(sub_topic, Image, self.callback)

        rospy.spin()

def __init__(self, sub_topic, pub_steering_topic, pub_throttle_topic):
        self.bridge = CvBridge()
        self.base_throttle = rospy.get_param("/autonomous_driving/object_detection_node/base_throttle", DEFAULT_BASE_THROTTLE)
        self.b_calc_steering = rospy.get_param("/autonomous_driving/object_detection_node/b_calc_steering", DEFAULT_B_CALC_STEERING)
        self.debug_flag = rospy.get_param("/autonomous_driving/object_detection_node/debug_flag", DEBUG_FLAG)
        self.image_sub = rospy.Subscriber(sub_topic, Image, self.callback)
        self.steering_pub = rospy.Publisher(pub_steering_topic, Float64, queue_size=1)
        self.throttle_pub = rospy.Publisher(pub_throttle_topic, Float64, queue_size=1)
        self.roadmask = None
        rospy.spin()