python类get_rostime()的实例源码

def _to_time_inst(msg, rostype, inst=None):
    # Create an instance if we haven't been provided with one
    if rostype == "time" and msg == "now":
        return rospy.get_rostime()

    if inst is None:
        if rostype == "time":
            inst = rospy.rostime.Time()
        elif rostype == "duration":
            inst = rospy.rostime.Duration()
        else:
            return None

    # Copy across the fields
    for field in ["secs", "nsecs"]:
        try:
            if field in msg:
                setattr(inst, field, msg[field])
        except TypeError:
            continue

    return inst

def init_transforms(self):
        """ Initialize the tf2 transforms """
        rospy.loginfo("Start transform calibration.")
        self.tf_buffer = tf2_ros.Buffer(rospy.Duration(300.0))
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
        while True:
            try:
                now = rospy.get_rostime()
                self.tf_buffer.lookup_transform('head',
                                                'left_camera_optical_frame',
                                                now - rospy.Duration(0.1))
            except:
                self.rate.sleep()
                continue
            break
        rospy.loginfo('transform calibration finished.')

def create_image_from_cloud(self, points):
        size = 100, 100, 1
        img = numpy.zeros(size, numpy.uint8)

        #tf = self.zarj_os.transform.lookup_transform('world', 'head', rospy.get_rostime())
        print points[0]
        #print self.zarj_os.transform.transform_from_world(points[0])
        for p in points:
            #tp = self.zarj_os.transform.transform_from_world(p)
            ipx = int((p.x * 100.0) + 50.0)
            ipy = int((p.y * 100.0) + 50.0)
            if ipx >= 0 and ipx < 100 and ipy >= 0 and ipy < 100:
                ipz = int(p.z * 100.0)
                if ipz > 255:
                    ipz = 255
                img[ipx, ipy] = ipz
        return img

def create_image_from_cloud(self, points):
        size = 100, 100, 1
        img = numpy.zeros(size, numpy.uint8)

        #tf = self.zarj_os.transform.lookup_transform('world', 'head', rospy.get_rostime())
        print points[0]
        #print self.zarj_os.transform.transform_from_world(points[0])
        for p in points:
            #tp = self.zarj_os.transform.transform_from_world(p)
            ipx = int((p.x * 100.0) + 50.0)
            ipy = int((p.y * 100.0) + 50.0)
            if ipx >= 0 and ipx < 100 and ipy >= 0 and ipy < 100:
                ipz = int(p.z * 100.0)
                if ipz > 255:
                    ipz = 255
                img[ipx, ipy] = ipz
        return img

def send_status(self):
        """ Run our code """
        next_status_send = rospy.get_rostime().to_sec()
        next_picture_send = rospy.get_rostime().to_sec()
        while True:
            rospy.sleep(0.1)
            if not self.zarj_comm.connected:
                continue

            now = rospy.get_rostime().to_sec()
            if now >= next_status_send:
                next_status_send = now + self.STATUS_INTERVAL
                zmsg = ZarjStatus(self.task, self.checkpoint, self.elapsed_time, now, self.score, self.harness)
                self.zarj_comm.push_message(zmsg)

            if self.picture_interval > 0.01 and now >= next_picture_send:
                next_picture_send = now + self.picture_interval
                self.send_left_camera()
                if self.task >= 3:
                    self.send_lidar(True)

def get_data(self, arm=TRIAL_ARM):
        """
        Request for the most recent value for data/sensor readings.
        Returns entire sample report (all available data) in sample.
        Args:
            arm: TRIAL_ARM or AUXILIARY_ARM.
        """
        request = DataRequest()
        request.id = self._get_next_seq_id()
        request.arm = arm
        request.stamp = rospy.get_rostime()
        result_msg = self._data_service.publish_and_wait(request)
        # TODO - Make IDs match, assert that they match elsewhere here.
        sample = msg_to_sample(result_msg, self)
        return sample

    # TODO - The following could be more general by being relax_actuator
    #        and reset_actuator.

def handle(req):
    global msg, pub, task
    print 'get resp class'
    response_class = get_response_class(req)
    print 'resp', response_class
    try:
        task = req
        with task_change_lock:
            pub, msg = get_message_publisher(task)
        print pub, msg
        msg.header.stamp = rospy.get_rostime()
        pub.publish(msg)
        if not offboard_and_arm():
            return response_class(success=False, message='Cannot arm or offboard the vehicle')
        return response_class(success=True)
    except Exception as e:
        return response_class(success=False, message=e.message)

def _check_battery_state(_battery_acpi_path):
    """
    @return LaptopChargeStatus
    """
    o = slerp(_battery_acpi_path+'/state')

    batt_info = yaml.load(o)

    rv = LaptopChargeStatus()

    state = batt_info.get('charging state', 'discharging')
    rv.charge_state = state_to_val.get(state, 0)
    rv.rate     = _strip_A(batt_info.get('present rate',        '-1 mA'))
    if rv.charge_state == LaptopChargeStatus.DISCHARGING:
        rv.rate = math.copysign(rv.rate, -1) # Need to set discharging rate to negative

    rv.charge   = _strip_Ah(batt_info.get('remaining capacity', '-1 mAh'))
    rv.voltage  = _strip_V(batt_info.get('present voltage',     '-1 mV'))
    rv.present  = batt_info.get('present', False)

    rv.header.stamp = rospy.get_rostime()

    return rv

def __init__(self):
        self.charging_state =  {0:"Not Charging",
                                1:"Reconditioning Charging",
                                2:"Full Charging",
                                3:"Trickle Charging",
                                4:"Waiting",
                                5:"Charging Fault Condition"}
        self.charging_source = {0:"None",
                                1:"Internal Charger",
                                2:"Base Dock"}
        self.digital_outputs = {0:"OFF",
                                1:"ON"}
        self.oi_mode = {1:"Passive",
                        2:"Safe",
                        3:"Full"}
        self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
        self.last_diagnostics_time = rospy.get_rostime()

def publish_position(self, pos, ps_pub, ps_cov_pub, cov):
        x, y = pos[0], pos[1]
        if len(pos) > 2:
            z = pos[2]
        else:
            z = 0
        ps = PoseStamped()
        ps_cov = PoseWithCovarianceStamped()
        ps.pose.position.x = x
        ps.pose.position.y = y
        ps.pose.position.z = z
        ps.header.frame_id = self.frame_id
        ps.header.stamp = rospy.get_rostime()
        ps_cov.header = ps.header
        ps_cov.pose.pose = ps.pose
        ps_cov.pose.covariance = cov
        ps_pub.publish(ps)
        ps_cov_pub.publish(ps_cov)

def __get_point_msg(cls, data, frame):
        """
        Deserializes a location to a message of type PointStamped.

        Args:
            data: A dictionary containing the location.
            frame: Frame for the point.

        Returns:
            A message of type PointStamped with the location.
        """
        header = Header()
        header.stamp = rospy.get_rostime()
        header.frame_id = frame

        msg = GeoPointStamped()
        msg.header = header

        msg.position.latitude = data["latitude"]
        msg.position.longitude = data["longitude"]

        return msg

def cmdVel_publish(self, cmdVelocity):

         # Publish Twist
         self.cmdVel_pub.publish(cmdVelocity)

         # Publish TwistStamped 
         self.baseVelocity.twist = cmdVelocity

         baseVelocity = TwistStamped()

         baseVelocity.twist = cmdVelocity

         now = rospy.get_rostime()
         baseVelocity.header.stamp.secs = now.secs
         baseVelocity.header.stamp.nsecs = now.nsecs
         self.cmdVelStamped_pub.publish(baseVelocity)

def get_data(self, arm=TRIAL_ARM):
        """
        Request for the most recent value for data/sensor readings.
        Returns entire sample report (all available data) in sample.
        Args:
            arm: TRIAL_ARM or AUXILIARY_ARM.
        """
        request = DataRequest()
        request.id = self._get_next_seq_id()
        request.arm = arm
        request.stamp = rospy.get_rostime()
        result_msg = self._data_service.publish_and_wait(request)
        # TODO - Make IDs match, assert that they match elsewhere here.
        sample = msg_to_sample(result_msg, self)
        return sample

    # TODO - The following could be more general by being relax_actuator
    #        and reset_actuator.

def IK(self, T_goal):
        req = moveit_msgs.srv.GetPositionIKRequest()
        req.ik_request.group_name = self.group_name
        req.ik_request.robot_state = moveit_msgs.msg.RobotState()
        req.ik_request.robot_state.joint_state = self.joint_state
        req.ik_request.avoid_collisions = True
        req.ik_request.pose_stamped = geometry_msgs.msg.PoseStamped()
        req.ik_request.pose_stamped.header.frame_id = "world_link"
        req.ik_request.pose_stamped.header.stamp = rospy.get_rostime()
        req.ik_request.pose_stamped.pose = convert_to_message(T_goal)
        req.ik_request.timeout = rospy.Duration(3.0)
        res = self.ik_service(req)
        q = []
        if res.error_code.val == res.error_code.SUCCESS:
            q = self.q_from_joint_state(res.solution.joint_state)
        return q

def handle_multi_range_message(self, multi_range_msg):
        """Handle a ROS multi-range message by updating and publishing the state.

        Args:
             multi_range_msg (uwb.msg.UWBMultiRangeWithOffsets): ROS multi-range message.
        """
        # Update tracker position based on time-of-flight measurements
        new_estimate = self.update_estimate(multi_range_msg)
        if new_estimate is None:
            rospy.logwarn('Could not compute initial estimate: address={}, remote_address={}'.format(
                multi_range_msg.address, multi_range_msg.remote_address))
        else:
            # Publish tracker message
            ros_msg = uwb.msg.UWBTracker()
            ros_msg.header.stamp = rospy.get_rostime()
            ros_msg.address = multi_range_msg.address
            ros_msg.remote_address = multi_range_msg.remote_address
            ros_msg.state = new_estimate.state
            ros_msg.covariance = np.ravel(new_estimate.covariance)
            self.uwb_pub.publish(ros_msg)

            # Publish target transform (rotation is identity)
            self.tf_broadcaster.sendTransform(
                (new_estimate.state[0], new_estimate.state[1], new_estimate.state[2]),
                tf.transformations.quaternion_from_euler(0, 0, 0),
                rospy.get_rostime(),
                self.target_frame,
                self.tracker_frame
            )

def _to_object_inst(msg, rostype, roottype, inst, stack):
    # Typecheck the msg
    if type(msg) is not dict:
        raise FieldTypeMismatchException(roottype, stack, rostype, type(msg))

    # Substitute the correct time if we're an std_msgs/Header
    if rostype in ros_header_types:
        inst.stamp = rospy.get_rostime()

    inst_fields = dict(zip(inst.__slots__, inst._slot_types))

    for field_name in msg:
        # Add this field to the field stack
        field_stack = stack + [field_name]

        # Raise an exception if the msg contains a bad field
        if not field_name in inst_fields:
            raise NonexistentFieldException(roottype, field_stack)

        field_rostype = inst_fields[field_name]
        field_inst = getattr(inst, field_name)

        field_value = _to_inst(msg[field_name], field_rostype,
                    roottype, field_inst, field_stack)

        setattr(inst, field_name, field_value)

    return inst

def handle_multi_range_message(self, multi_range_msg):
        """Handle a ROS multi-range message by updating and publishing the state.

        Args:
             multi_range_msg (uwb.msg.UWBMultiRangeWithOffsets): ROS multi-range message.
        """
        # Update tracker position based on time-of-flight measurements
        new_estimate = self.update_estimate(multi_range_msg)
        if new_estimate is None:
            rospy.logwarn('Could not compute initial estimate: address={}, remote_address={}'.format(
                multi_range_msg.address, multi_range_msg.remote_address))
        else:
            # Publish tracker message
            ros_msg = uwb.msg.UWBTracker()
            ros_msg.header.stamp = rospy.get_rostime()
            ros_msg.address = multi_range_msg.address
            ros_msg.remote_address = multi_range_msg.remote_address
            ros_msg.state = new_estimate.state
            ros_msg.covariance = np.ravel(new_estimate.covariance)
            self.uwb_pub.publish(ros_msg)

            # Publish target transform (rotation is identity)
            self.tf_broadcaster.sendTransform(
                (new_estimate.state[0], new_estimate.state[1], new_estimate.state[2]),
                tf.transformations.quaternion_from_euler(0, 0, 0),
                rospy.get_rostime(),
                self.target_frame,
                self.tracker_frame
            )

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

        # Todo - publish the spin logic...
        self.laser_publisher = rospy.Publisher("/multisense/lidar_cloud", PointCloud, queue_size=10)
        self.scan_service = rospy.ServiceProxy('assemble_scans', AssembleScans)
        #self.bridge = CvBridge()

        #self.zarj_os.zps.look_down()

        while True:
            begin = rospy.get_rostime()
            rospy.sleep(3.0)
            resp = self.scan_service(begin, rospy.get_rostime())
            self.laser_publisher.publish(resp.cloud)
            #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 run(self):
        rospy.loginfo('start task {} checkpoint {}.'.format(self.task.task_id, self.checkpoint_id))
        self.start_time = rospy.get_rostime()
        self.setup()
        while not self.stopped or not self.is_done():
            self.periodic()
            rospy.sleep(0.01)
        self.cleanup()

        rospy.loginfo('end task {} checkpoint {}.'.format(self.task.task_id, self.checkpoint_id))

def publish(ac):
    pub = rospy.Publisher("/aide/temperature", AirConditionerMessage, queue_size=42)
    rate = rospy.Rate(2)
    while not rospy.is_shutdown():
        pub.publish(AirConditionerMessage(ac.temperature, "simulatedRoom0", rospy.get_rostime()))
        rate.sleep()

def callback3(self, msg):
        now = rospy.get_rostime()
        self.time3.append(now.secs)
        self.values.append(msg.data)

def handle_sensor(self, msg):
        # TODO maybe time stamp sensor values with header
        # TODO rospy.get_rostime() vs rospy.Time.now()?
        # print(rospy.get_rostime(), rospy.Time.now())  # They're different
        # self.sensor_t.append(rospy.Time())
        self.sensor_values.append(msg.data)
        #print (self.sensor_values)

def relax_arm(self, arm):
        """
        Relax one of the arms of the robot.
        Args:
            arm: Either TRIAL_ARM or AUXILIARY_ARM.
        """
        relax_command = RelaxCommand()
        relax_command.id = self._get_next_seq_id()
        relax_command.stamp = rospy.get_rostime()
        relax_command.arm = arm
        self._relax_service.publish_and_wait(relax_command)

def update(self):
        with self._mutex:
            diag = DiagnosticArray()
            diag.header.stamp = rospy.get_rostime()

            info_update_ok  = rospy.get_time() - self._last_info_update  < 5.0 / self._batt_info_rate
            state_update_ok = rospy.get_time() - self._last_state_update < 5.0 / self._batt_state_rate

            if info_update_ok:
                self._msg.design_capacity = self._batt_design_capacity
                self._msg.capacity        = self._batt_last_full_capacity
            else:
                self._msg.design_capacity = 0.0
                self._msg.capacity        = 0.0

            if info_update_ok and state_update_ok and self._msg.capacity != 0:
                self._msg.percentage = int(self._msg.charge / self._msg.capacity * 100.0)

            diag_stat = _laptop_charge_to_diag(self._msg)
            if not info_update_ok or not state_update_ok:
                diag_stat.level   = DiagnosticStatus.ERROR
                diag_stat.message = 'Laptop battery data stale'

            diag.status.append(diag_stat)

            self._diag_pub.publish(diag)
            self._power_pub.publish(self._msg)

def ekf_pub(self, ranges, vel_data, yaw, alt):
        z = np.array([])
        new_pose = Odometry()
        ps_cov = PoseWithCovarianceStamped()
        for tag_name in self.tag_order:
            measurement = ranges[tag_name]
            z = np.append(z, measurement)

        if self.last_time is None:
            self.last_time = rospy.Time.now().to_sec()
        else:
            dt = rospy.Time.now().to_sec() - self.last_time
            self.predict(dt)
            self.update(z, vel_data, yaw, alt)
            self.last_time = rospy.Time.now().to_sec()

            new_pose.header.stamp = rospy.get_rostime()
            new_pose.header.frame_id = self.frame_id
            new_pose.pose.pose.position.x = self.x[0]
            new_pose.pose.pose.position.y = self.x[1]
            new_pose.pose.pose.position.z = self.x[2]
            cov = self.P.flatten().tolist()
            new_pose.pose.covariance = cov
            new_pose.twist.twist.linear.x = self.x[3]
            new_pose.twist.twist.linear.y = self.x[4]
            new_pose.twist.twist.linear.z = self.x[5]

        return new_pose

    # @n.publisher(EKF_COV_TOPIC, PoseWithCovarianceStamped)
    # def cov_pub(self, ps_cov):
    #     return ps_cov

def add(self, msg, my_queue, my_queue_index=None):
        """Adds a message to the current queue, and matches them accordingly.

        Args:
            msg: Message.
            my_queue: Current message queue map from ROS Time to ROS message.
            my_queue_index: Unused.
        """
        # Store when this message was received.
        received = rospy.get_rostime()

        # Acquire lock.
        self.lock.acquire()

        # Add to queue.
        my_queue[received] = msg
        while len(my_queue) > self.queue_size:
            del my_queue[min(my_queue)]

        # Signal by approximate time, as per ros_comm source code.
        # Credits to Willow Garage, Inc.
        # TODO: Clean this up.
        for vv in itertools.product(*[list(q.keys()) for q in self.queues]):
            qt = zip(self.queues, vv)
            if (((max(vv) - min(vv)) < self.slop) and
                (len([1 for q, t in qt if t not in q]) == 0)):
                msgs = [q[t] for q, t in qt]
                self.signalMessage(*msgs)
                for q, t in qt:
                    del q[t]

        # Unlock.
        self.lock.release()

def __get_flyzone(cls, data, frame):
        """
        Deserializes flight boundary data into a FlyZoneArray message.

        Args:
            data: List of dictionaries.
            frame: Frame id for the boundaries.

        Returns:
            A FlyzoneArray message type which contains an array of FlyZone
            messages, which contains a polygon for the boundary, a max
            altitude and a min altitude.
        """
        # Generate header for all zones.
        header = Header()
        header.stamp = rospy.get_rostime()
        header.frame_id = frame

        flyzones = FlyZoneArray()
        for zone in data:
            flyzone = FlyZone()
            flyzone.zone.header = header

            flyzone.max_alt = feet_to_meters(zone["altitude_msl_max"])
            flyzone.min_alt = feet_to_meters(zone["altitude_msl_min"])

            # Change boundary points to ros message of type polygon.
            for waypoint in zone["boundary_pts"]:
                point = GeoPoint()
                point.latitude = waypoint["latitude"]
                point.longitude = waypoint["longitude"]
                flyzone.zone.polygon.points.append(point)

            flyzones.flyzones.append(flyzone)

        return flyzones

def __get_waypoints(cls, data, frame):
        """
        Deserializes a list of waypoints into a marker message.

        Args:
            data: List of dictionaries corresponding to waypoints.
            frame: Frame of the markers.

        Returns:
            A marker message of type Points, with a list of points in order
            corresponding to each waypoint.
        """
        header = Header()
        header.stamp = rospy.get_rostime()
        header.frame_id = frame

        waypoints = WayPoints()
        waypoints.header = header

        # Ensure there is no rotation by setting w to 1.
        for point in data:
            waypoint = GeoPoint()

            altitude = feet_to_meters(point["altitude_msl"])

            waypoint.latitude = point["latitude"]
            waypoint.longitude = point["longitude"]
            waypoint.altitude = altitude

            waypoints.waypoints.append(waypoint)

        return waypoints

def __get_search_grid(cls, data, frame):
        """
        Deserializes a the search grid into a polygon message.

        Args:
            data: List of dictionaries corresponding to the search grid points.
            frame: Frame for the polygon.

        Returns:
            Message of type PolygonStamped with the bounds of the search grid.
        """
        header = Header()
        header.stamp = rospy.get_rostime()
        header.frame_id = frame

        search_grid = GeoPolygonStamped()
        search_grid.header = header

        for point in data:
            boundary_pnt = GeoPoint()

            altitude = feet_to_meters(point["altitude_msl"])

            boundary_pnt.latitude = point["latitude"]
            boundary_pnt.longitude = point["longitude"]
            boundary_pnt.altitude = altitude

            search_grid.polygon.points.append(boundary_pnt)

        return search_grid

def publishCmdVel(cmdvel, velPublisher, velPublisherStamped):

     velPublisher.publish(cmdvel)

     baseVelocity = TwistStamped()

     baseVelocity.twist = cmdvel

     now = rospy.get_rostime()
     baseVelocity.header.stamp.secs = now.secs
     baseVelocity.header.stamp.nsecs = now.nsecs
     velPublisherStamped.publish(baseVelocity)