python类get_rostime()的实例源码

def callback(self, cmdVelocity):
        self.baseVelocity.twist = cmdVelocity
        self.flag = 1

        if self.flag:
            # Publish Updated TwistStamped

             baseVelocity = TwistStamped()

             baseVelocity.twist = cmdVelocity

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

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 drive(self):
        start_time =rospy.get_rostime()
        while not rospy.is_shutdown():
            curent = rospy.get_rostime()
            if (curent - start_time) <  rospy.Duration(1,0):
                self.controller.SendTakeoff()
                self.status =0

            elif (curent - start_time) <  rospy.Duration(22,0):
                self.controller.SetCommand(pitch=1)
                self.status =1

            elif (curent - start_time) <  rospy.Duration(23,0):
                self.controller.SetCommand(pitch=0)
                self.status =2

            elif (curent - start_time) <  rospy.Duration(24,0):
                #self.controller.SetCommand(yaw_velocity=2) 
                self.turnLeft()
                self.status =3 
                rate =rospy.Rate(1)
            elif (curent - start_time) <  rospy.Duration(40.5,0):
                self.controller.SetCommand(pitch=1)  
                self.status =4
            else:
                 self.controller.SetCommand(pitch=0)
                 self.controller.SendLand()

def get_current_time(self):
        return rospy.get_rostime()

def __init__(self):
        self.lock = threading.Lock()
        rospy.init_node("roboclaw_node",log_level=rospy.DEBUG)
        rospy.on_shutdown(self.shutdown)
        rospy.loginfo("Connecting to roboclaw")

        self.wheels_speeds_pub = rospy.Publisher('/motors/commanded_speeds', Wheels_speeds, queue_size=1)
        self.motors_currents_pub = rospy.Publisher('/motors/read_currents', Motors_currents, queue_size=1)

        self.address = int(rospy.get_param("~address", "128"))
        if self.address > 0x87 or self.address < 0x80:
            rospy.logfatal("Address out of range")
            rospy.signal_shutdown("Address out of range")

        # TODO need someway to check if address is correct


        self.MAX_ABS_LINEAR_SPEED = float(rospy.get_param("~max_abs_linear_speed", "1.0"))
        self.MAX_ABS_ANGULAR_SPEED = float(rospy.get_param("~max_abs_angular_speed", "1.0"))
        self.TICKS_PER_METER = float(rospy.get_param("~ticks_per_meter", "4342.2"))
        self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.315"))
        self.ACC_LIM = float(rospy.get_param("~acc_lim", "0.1"))

        self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH)
        self.last_set_speed_time = rospy.get_rostime()

        rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=1)

        rospy.sleep(1)

        rospy.logdebug("address %d", self.address)
        rospy.logdebug("max_abs_linear_speed %f", self.MAX_ABS_LINEAR_SPEED)
        rospy.logdebug("max_abs_angular_speed %f", self.MAX_ABS_ANGULAR_SPEED)
        rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER)
        rospy.logdebug("base_width %f", self.BASE_WIDTH)

def cmd_vel_callback(self, twist):
        with self.lock:
            self.last_set_speed_time = rospy.get_rostime()

            linear_x = twist.linear.x
            angular_z = twist.angular.z
            if abs(linear_x) > self.MAX_ABS_LINEAR_SPEED:
                linear_x = copysign(self.MAX_ABS_LINEAR_SPEED, linear_x)
            if abs(angular_z) > self.MAX_ABS_ANGULAR_SPEED:
                angular_z = copysign(self.MAX_ABS_ANGULAR_SPEED, angular_z)

            vr = linear_x - angular_z * self.BASE_WIDTH / 2.0  # m/s
            vl = linear_x + angular_z * self.BASE_WIDTH / 2.0

            vr_ticks = int(vr * self.TICKS_PER_METER)  # ticks/s
            vl_ticks = int(vl * self.TICKS_PER_METER)

            v_wheels= Wheels_speeds()
            v_wheels.wheel1=vl
            v_wheels.wheel2=vr
            self.wheels_speeds_pub.publish(v_wheels)


            rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)

    # TODO: Need to make this work when more than one error is raised

def cmd_vel_callback(self, twist):
        with self.lock:
            self.last_set_speed_time = rospy.get_rostime()

            linear_x = twist.linear.x
            angular_z = twist.angular.z
            if abs(linear_x) > self.MAX_ABS_LINEAR_SPEED:
                linear_x = copysign(self.MAX_ABS_LINEAR_SPEED, linear_x)
            if abs(angular_z) > self.MAX_ABS_ANGULAR_SPEED:
                angular_z = copysign(self.MAX_ABS_ANGULAR_SPEED, angular_z)

            vr = linear_x - angular_z * self.BASE_WIDTH / 2.0  # m/s
            vl = linear_x + angular_z * self.BASE_WIDTH / 2.0

            vr_ticks = int(vr * self.TICKS_PER_METER)  # ticks/s
            vl_ticks = int(vl * self.TICKS_PER_METER)

            v_wheels= Wheels_speeds()
            v_wheels.wheel1=vl
            v_wheels.wheel2=vr
            self.wheels_speeds_pub.publish(v_wheels)


            rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)

            try:
                #Replaced to implement watchdog
                #roboclaw.SpeedM1M2(self.address, vr_ticks, vl_ticks)
                #Replaced to implement acc
                #roboclaw.SpeedDistanceM1M2(self.address, vr_ticks, int(abs(vr_ticks*0.04)), vl_ticks, int(abs(vl_ticks*0.04)), 1)
                #rospy.logdebug(" Acc ticks %d" % (int(self.ACC_LIM * self.TICKS_PER_METER)))
                roboclaw.SpeedAccelDistanceM1(self.address, int(self.ACC_LIM * self.TICKS_PER_METER),vr_ticks, int(abs(vr_ticks*0.04)),1)
                roboclaw.SpeedAccelDistanceM2(self.address, int(self.ACC_LIM * self.TICKS_PER_METER),vl_ticks, int(abs(vl_ticks*0.04)),1)
                #Mixed command doesn't work
                #roboclaw.SpeedAccelDistanceM1M2(self.address, int(self.ACC_LIM * self.TICKS_PER_METER),vr_ticks, int(abs(vr_ticks*0.04)), vl_ticks, int(abs(vl_ticks*0.04)), 1)

            except OSError as e:
                rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
                rospy.logdebug(e)

    # TODO: Need to make this work when more than one error is raised

def detection(self, hsv_image):
        """Check for detection in the image """
        mask = cv2.inRange(hsv_image, self.color_low, self.color_high)
        if self.baseline_cnt > 0:
            nmask = cv2.bitwise_not(mask)
            if self.baseline is None:
                rospy.loginfo("getting baseline for {}".format(self.name))
                self.baseline = nmask
            else:
                self.baseline = cv2.bitwise_or(nmask, self.baseline)
                mask = cv2.bitwise_and(mask, self.baseline)
                count = cv2.countNonZero(mask) + self.baseline_fuzzy
                self.low_count = max(self.low_count, count)
                self.high_count = self.low_count + self.baseline_fuzzy
            self.baseline_cnt -= 1
            return
        elif self.baseline is not None:
            mask = cv2.bitwise_and(mask, self.baseline)
        count = cv2.countNonZero(mask)
        if count > self.low_count and self.active is None:
            self.active = rospy.get_rostime()
            rospy.loginfo("{} ACTIVE ({})".format(self.name, count))
            self.cloud.reset()
            if self.callbacks[0] is not None:
                self.callbacks[0](self.name)
        elif self.active is not None and count < self.high_count:
            rospy.loginfo("{} GONE ({})".format(self.name, count))
            self.cloud.reset()
            self.active = None
            if self.callbacks[2] is not None:
                self.published = False
            self.report_count = 0
            if self.callbacks[1] is not None:
                self.callbacks[1](self.name)

        # DEBUGGING to see what the masked image for the request is
        if self.debug:
            cv2.namedWindow(self.name, cv2.WINDOW_NORMAL)
            if self.baseline is not None:
                cv2.namedWindow(self.name+'_baseline', cv2.WINDOW_NORMAL)
                cv2.imshow(self.name+'_baseline', self.baseline)
            cv2.imshow(self.name, mask)
            cv2.waitKey(1)

        # to to see if we notify the location callback
        if self.is_active() and self.report_count > self.min_reports:
            now = rospy.get_rostime()
            if (self.active + self.stablize_time) < now:
                self.published = True
                point = PointStamped()
                center = self.cloud.find_center()
                point.header.seq = 1
                point.header.stamp = now
                point.header.frame_id = self.frame_id
                point.point.x = center[0]
                point.point.y = center[1]
                point.point.z = center[2]
                if self.callbacks[2] is not None:
                    self.callbacks[2](self.name, point)

def publish_tf(self):
        model_cache = {}
        poses = {'gazebo_world': identity_matrix()}
        for (link_idx, link_name) in enumerate(self.link_states_msg.name):
            poses[link_name] = pysdf.pose_msg2homogeneous(self.link_states_msg.pose[link_idx])
            # print('%s:\n%s' % (link_name, poses[link_name]))

        for (link_idx, link_name) in enumerate(self.link_states_msg.name):
            # print(link_idx, link_name)
            modelinstance_name = link_name.split('::')[0]
            # print('modelinstance_name:', modelinstance_name)
            model_name = pysdf.name2modelname(modelinstance_name)
            # print('model_name:', model_name)
            if not model_name in model_cache:
                sdf = pysdf.SDF(model=model_name)
                model_cache[model_name] = sdf.world.models[0] if len(sdf.world.models) >= 1 else None
                if not model_cache[model_name]:
                    print('Unable to load model: %s' % model_name)
            model = model_cache[model_name]
            link_name_in_model = link_name.replace(modelinstance_name + '::', '')
            if model:
                link = model.get_link(link_name_in_model)

                if link.tree_parent_joint:
                    parent_link = link.tree_parent_joint.tree_parent_link
                    parent_link_name = parent_link.get_full_name()
                    # print('parent:', parent_link_name)
                    parentinstance_link_name = parent_link_name.replace(model_name, modelinstance_name, 1)
                else:  # direct child of world
                    parentinstance_link_name = 'gazebo_world'
            else:  # Not an SDF model
                parentinstance_link_name = 'gazebo_world'
            # print('parentinstance:', parentinstance_link_name)
            pose = poses[link_name]
            #parent_pose = pysdf.pose_msg2homogeneous(self.model_states_msg.pose[1])
            parent_pose = poses[parentinstance_link_name]
            rel_tf = concatenate_matrices(inverse_matrix(parent_pose), pose)
            translation, quaternion = pysdf.homogeneous2translation_quaternion(rel_tf)
            # print('Publishing TF %s -> %s: t=%s q=%s' % (pysdf.sdf2tfname(parentinstance_link_name), pysdf.sdf2tfname(link_name), translation, quaternion))
            self.tfBroadcaster.sendTransform(translation, quaternion, rospy.get_rostime(), pysdf.sdf2tfname(link_name),
                                        pysdf.sdf2tfname(parentinstance_link_name))

# Main function.

def run(self):
        rospy.loginfo("Starting motor drive")
        r_time = rospy.Rate(30)
        while not rospy.is_shutdown():
            with self.lock:
                if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 1:
                    rospy.loginfo("Did not get comand for 1 second, stopping")
                    try:
                        roboclaw.ForwardM1(self.address, 0)
                        roboclaw.ForwardM2(self.address, 0)
                    except OSError as e:
                        rospy.logerr("Could not stop")
                        rospy.logdebug(e)

                # TODO need find solution to the OSError11 looks like sync problem with serial
                status1, enc1, crc1 = None, None, None
                status2, enc2, crc2 = None, None, None
                statusC, amp1, amp2 = None, None, None

                try:
                    status1, enc1, crc1 = roboclaw.ReadEncM1(self.address)
                except ValueError:
                    pass

                try:
                    status2, enc2, crc2 = roboclaw.ReadEncM2(self.address)
                except ValueError:
                    pass
                try:
                    status1c, amp1, amp2 = roboclaw.ReadCurrents(self.address)
                except ValueError:
                    pass

                if (enc1 != None) & (enc2 != None):
                    rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
                    self.encodm.update_publish(enc1, enc2)
                    self.updater.update()
                else:
                    rospy.logdebug("Error Reading enc")

                if (amp1 != None) & (amp2 != None):
                    rospy.logdebug(" Currents %d %d" % (amp1, amp2))
                    amps=Motors_currents()
                    amps.motor1=float(amp1)/100
                    amps.motor2=float(amp2)/100
                    self.motors_currents_pub.publish(amps)
                else:
                    rospy.logdebug("Error Reading Currents")

            r_time.sleep()