Python TransformBroadcaster.sendTransform方法代码示例

# 需要导入模块: from tf.broadcaster import TransformBroadcaster [as 别名]
# 或者: from tf.broadcaster.TransformBroadcaster import sendTransform [as 别名]
class DummyViconNode(object):
    """
    Example python node including reading parameters, publishers and subscribers, and timer.
    """
    def __init__(self):
        # Remember that init_node hasn't been called yet so only do stuff here that
        # doesn't require node handles etc.
        pass

    def start(self):
        rospy.init_node('python_node_example')
        self.tfb = TransformBroadcaster()
        self.init_params()
        self.init_publishers()
        self.init_timers()
        rospy.spin()
        
    def init_params(self):
        pass
#        self.param_one = rospy.get_param("~param_one", 100.0)
#        self.param_two = rospy.get_param("~param_two", False)
    
    def init_publishers(self):
        self.pub = rospy.Publisher('vicon_recv_direct/output', TransformStamped)
        
    def init_timers(self):
        self.vicon_timer = Timer(rospy.Duration(1/120.0), self.vicon_timer_callback) # Call at 10 Hz
                                          
    def vicon_timer_callback(self, event):
        msg = TransformStamped()
        msg.header.stamp = rospy.Time.now()
        msg.transform.rotation.w = 1.0
        self.pub.publish(msg)
        self.tfb.sendTransform((0,0,0), (0,0,0,1), rospy.Time.now(), '/pelican1/flyer_vicon', '/enu')

# 需要导入模块: from tf.broadcaster import TransformBroadcaster [as 别名]
# 或者: from tf.broadcaster.TransformBroadcaster import sendTransform [as 别名]
class RTbotController:
    def __init__(self, arduino):
        self.arduino = arduino
        self.stopped = False
        
        # Subscribe to rtbot_motors
        rospy.Subscriber("rtbot_motors", Int16, self.rtbotMotorsCallback)

        # Setup the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
        
    def poll(self):
        (x, y, theta) = self.arduino.rtbot_read_odometry()

        quaternion = Quaternion()
        quaternion.x = 0.0 
        quaternion.y = 0.0
        quaternion.z = sin(theta / 2.0)
        quaternion.w = cos(theta / 2.0)
    
        # Create the odometry transform frame broadcaster.
        now = rospy.Time.now()
        self.odomBroadcaster.sendTransform(
            (x, y, 0), 
            (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            now,
            "base_link",
            "odom"
            )
    
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.child_frame_id = "base_link"
        odom.header.stamp = now
        odom.pose.pose.position.x = x
        odom.pose.pose.position.y = y
        odom.pose.pose.position.z = 0
        odom.pose.pose.orientation = quaternion
        
        # Possible issue: If we were commanding the robot via Twist messages
        # then we would have some sensible values to put here.  But by setting
        # the motor commands directly, we don't have access to plausible values
        # for the forward and angular speeds.  Does this even matter???
        #odom.twist.twist.linear.x = self.forwardSpeed
        odom.twist.twist.linear.x = 0
        odom.twist.twist.linear.y = 0
        #odom.twist.twist.angular.z = self.angularSpeed
        odom.twist.twist.angular.z = 0

        self.odomPub.publish(odom)

    def stop(self):
        self.stopped = True
        self.arduino.rtbot_set_motors(0)
            
    def rtbotMotorsCallback(self, msg):
        self.arduino.rtbot_set_motors(msg.data)    

# 需要导入模块: from tf.broadcaster import TransformBroadcaster [as 别名]
# 或者: from tf.broadcaster.TransformBroadcaster import sendTransform [as 别名]
class DummyViconNode(object):
    """
    Example python node including reading parameters, publishers and subscribers, and timer.
    """
    def __init__(self):
        # Remember that init_node hasn't been called yet so only do stuff here that
        # doesn't require node handles etc.
        pass

    def start(self):
        rospy.init_node('python_node_example')
        self.tfb = TransformBroadcaster()
        self.init_params()
        self.init_publishers()
        self.init_timers()
        rospy.spin()
        
    def init_params(self):
        pass
        self.tf_ref_frame_id = rospy.get_param("~tf_ref_frame_id", "/enu")
        self.tf_tracked_frame_id = rospy.get_param("~tf_tracked_frame_id", "/pelican1/flyer_vicon")
        self.dummy_position = rospy.get_param("~dummy_position", (0.0, -0.3593, -0.05))
        self.dummy_orientation = rospy.get_param("~dummy_orientation", (0.5, 0.5, -0.5, 0.5)) # xyzw
        self.enable_tf_broadcast = rospy.get_param("~enable_tf_broadcast", False)
        
#        self.param_two = rospy.get_param("~param_two", False)
    
    def init_publishers(self):
        self.pub = rospy.Publisher('vicon_recv_direct/output', TransformStamped)
        
    def init_timers(self):
        self.vicon_timer = Timer(rospy.Duration(1/120.0), self.vicon_timer_callback) # Call at 10 Hz
                                          
    def vicon_timer_callback(self, event):
        msg = TransformStamped()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = self.tf_ref_frame_id
        msg.child_frame_id = self.tf_tracked_frame_id
        msg.transform.translation.x, msg.transform.translation.y, msg.transform.translation.z = self.dummy_position
        (msg.transform.rotation.x, msg.transform.rotation.y, 
         msg.transform.rotation.z, msg.transform.rotation.w) = self.dummy_orientation
        self.pub.publish(msg)
        if self.enable_tf_broadcast:
            self.tfb.sendTransform(self.dummy_position, self.dummy_orientation, 
                                   rospy.Time.now(), self.tf_tracked_frame_id, self.tf_ref_frame_id)

# 需要导入模块: from tf.broadcaster import TransformBroadcaster [as 别名]
# 或者: from tf.broadcaster.TransformBroadcaster import sendTransform [as 别名]

#.........这里部分代码省略.........
        scan.angle_increment =pi/180.0 
        scan.range_min = 0.020
        scan.range_max = 5.0

        #rospy.loginfo("spin: before Odometry")
        odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_footprint')

        button = Button()
        sensor = Sensor()

        # main loop of driver
        r = rospy.Rate(2)
        cmd_rate= self.CMD_RATE

        rospy.loginfo(">>> spin: before while loop <<<")

        while not rospy.is_shutdown():
            left, right = self.robot.getMotors()

            # prepare laser scan
            scan.header.stamp = rospy.Time.now()

            self.robot.requestScan()
            #rospy.loginfo("spin: loop: requestScan")
            scan.ranges = self.robot.getLdsScan()
            #if len(scan.ranges) == 0:
            #    scan.ranges = self.robot.getLdsScan()

            # now update position information
            dt = (scan.header.stamp - then).to_sec()
            then = scan.header.stamp

            d_left =  (left - encoders[0])/1000.0
            d_right =  (right - encoders[1])/1000.0
            encoders = [left, right]

            #print d_left, d_right, encoders

            dx = (d_left+d_right)/2
            dth = (d_right-d_left)/(self.robot.base_width/1000.0)

            x = cos(dth)*dx
            y = -sin(dth)*dx
            self.x += cos(self.th)*x - sin(self.th)*y
            self.y += sin(self.th)*x + cos(self.th)*y
            self.th += dth
            #print self.x,self.y,self.th

            # prepare tf from base_link to odom
            quaternion = Quaternion()
            quaternion.z = sin(self.th/2.0)
            quaternion.w = cos(self.th/2.0)

            # prepare odometry
            odom.header.stamp = rospy.Time.now()
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.twist.twist.linear.x = dx/dt
            odom.twist.twist.angular.z = dth/dt


            # sensors
            lsb, rsb, lfb, rfb = self.robot.getDigitalSensors()
            #rospy.loginfo("spin: loop: getDigitalSensors")

            # buttons
            btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down = self.robot.getButtons()


            # publish everything
            self.odomBroadcaster.sendTransform((self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z,
                                                                     quaternion.w), then, "base_footprint", "odom")
            #rospy.loginfo("spin: loop: sendTransform")
            if len(scan.ranges) > 0:
                self.scanPub.publish(scan)

            self.odomPub.publish(odom)
            button_enum = ("Soft_Button", "Up_Button", "Start_Button", "Back_Button", "Down_Button")
            sensor_enum = ("Left_Side_Bumper", "Right_Side_Bumper", "Left_Bumper", "Right_Bumper")
            for idx, b in enumerate((btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down)):
                if b == 1:
                    button.value = b
                    button.name = button_enum[idx]
                    self.buttonPub.publish(button)

            for idx, b in enumerate((lsb, rsb, lfb, rfb)):
                if b == 1:
                    sensor.value = b
                    sensor.name = sensor_enum[idx]
                    self.sensorPub.publish(sensor)
            # wait, then do it again
            #rospy.loginfo("spin: loop: before sleep()")
            r.sleep()
            # Steve: testing with longer sleep 
            #time.sleep(1)

        # shut down
        rospy.loginfo(">>>>>> Exiting <<<<<<<<")

# 需要导入模块: from tf.broadcaster import TransformBroadcaster [as 别名]
# 或者: from tf.broadcaster.TransformBroadcaster import sendTransform [as 别名]

#.........这里部分代码省略.........

        self.x = 0                  # position in xy plane
        self.y = 0
        self.th = 0
        then = rospy.Time.now()

        # things that don't ever change
        scan_link = rospy.get_param('~frame_id','base_laser_link')
        scan = LaserScan(header=rospy.Header(frame_id=scan_link)) 
        scan.angle_min = 0
        scan.angle_max = 6.26
        scan.angle_increment = 0.017437326
        scan.range_min = 0.020
        scan.range_max = 5.0
        odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_link')
    
        # main loop of driver
        r = rospy.Rate(5)
        self.robot.requestScan()
        while not rospy.is_shutdown():
            # prepare laser scan
            scan.header.stamp = rospy.Time.now()    
            #self.robot.requestScan()
            scan.ranges = self.robot.getScanRanges()

            # get motor encoder values
            left, right = self.robot.getMotors()
            
            # get analog sensors
            self.robot.getAnalogSensors()
            
            # get drop sensors
            left_drop = self.robot.state["LeftDropInMM"]
            right_drop = self.robot.state["RightDropInMM"]

            # send updated movement commands
            self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
            
            # ask for the next scan while we finish processing stuff
            self.robot.requestScan()
            
            # now update position information
            dt = (scan.header.stamp - then).to_sec()
            then = scan.header.stamp

            d_left = (left - encoders[0])/1000.0
            d_right = (right - encoders[1])/1000.0
            encoders = [left, right]
            
            dx = (d_left+d_right)/2
            dth = (d_right-d_left)/(BASE_WIDTH/1000.0)

            x = cos(dth)*dx
            y = -sin(dth)*dx
            self.x += cos(self.th)*x - sin(self.th)*y
            self.y += sin(self.th)*x + cos(self.th)*y
            self.th += dth

            # prepare tf from base_link to odom
            quaternion = Quaternion()
            quaternion.z = sin(self.th/2.0)
            quaternion.w = cos(self.th/2.0)

            # prepare odometry
            odom.header.stamp = rospy.Time.now()
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.twist.twist.linear.x = dx/dt
            odom.twist.twist.angular.z = dth/dt
            
            # prepare drop
            drop = NeatoDropSensor()
            drop.header.stamp = rospy.Time.now()
            drop.left = left_drop
            drop.right = right_drop

            # publish everything
            self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                then, "base_link", "odom" )
            self.scanPub.publish(scan)
            self.odomPub.publish(odom)
            self.dropPub.publish(drop)

            # wait, then do it again
            r.sleep()

        # shut down
        self.robot.setLDS("off")
        self.robot.setTestMode("off") 

    def cmdVelCb(self,req):
        x = req.linear.x * 1000
        th = req.angular.z * (BASE_WIDTH/2) 
        k = max(abs(x-th),abs(x+th))
        # sending commands higher than max speed will fail
        if k > MAX_SPEED:
            x = x*MAX_SPEED/k; th = th*MAX_SPEED/k
        self.cmd_vel = [ int(x-th) , int(x+th) ]

# 需要导入模块: from tf.broadcaster import TransformBroadcaster [as 别名]
# 或者: from tf.broadcaster.TransformBroadcaster import sendTransform [as 别名]
class ARbotController:
    def __init__(self, arduino):
        self.arduino = arduino
        self.stopped = False
        
        self.cmd = 1

        self.forwardSpeed = 0
        self.angularSpeed = 0
                 
        # Subscribe to cmd_vel
        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
        rospy.Subscriber("cmd_vel2", Twist, self.cmdVel2Callback)
        s = rospy.Service("pick_cmd_vel", PickCmdVel, self.pickCmdVel)

        # Subscribe to arbot_play_led and arbot_advance_led
        rospy.Subscriber("arbot_play_led", Bool, self.arbotPlayLedCallback)
        rospy.Subscriber("arbot_advance_led", Bool, self.arbotAdvanceLedCallback)
        
        # Setup the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
        
        # Setup blob publisher
#        self.blob_publisher = rospy.Publisher('blobs', Blobs, queue_size=10)
        rospy.loginfo("Started ARbot controller.")
        
    def poll(self):
        (x, y, theta) = self.arduino.arbot_read_odometry()

        quaternion = Quaternion()
        quaternion.x = 0.0 
        quaternion.y = 0.0
        quaternion.z = sin(theta / 2.0)
        quaternion.w = cos(theta / 2.0)
    
        # Create the odometry transform frame broadcaster.
        now = rospy.Time.now()
        self.odomBroadcaster.sendTransform(
            (x, y, 0), 
            (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            now,
            "base_link",
            "odom"
            )
    
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.child_frame_id = "base_link"
        odom.header.stamp = now
        odom.pose.pose.position.x = x
        odom.pose.pose.position.y = y
        odom.pose.pose.position.z = 0
        odom.pose.pose.orientation = quaternion
        
        odom.twist.twist.linear.x = self.forwardSpeed
        odom.twist.twist.linear.y = 0
        odom.twist.twist.angular.z = self.angularSpeed

        self.arduino.arbot_set_velocity(self.forwardSpeed, self.angularSpeed)

        self.odomPub.publish(odom)

#        blobs = self.arduino.get_blobs()
#        blobs.header.stamp = rospy.Time.now()
#        self.blob_publisher.publish(blobs)
            
    def stop(self):
        self.stopped = True
        self.forwardSpeed = 0
        self.angularSpeed = 0
        self.arduino.arbot_set_velocity(0, 0)
            
    def cmdVelCallback(self, cmd_vel):
        if self.cmd == 2:
            return
        self.forwardSpeed = cmd_vel.linear.x         # m/s
        self.angularSpeed = cmd_vel.angular.z        # rad/s

    def cmdVel2Callback(self, cmd_vel2):
        if self.cmd == 1:
            return
        self.forwardSpeed = cmd_vel2.linear.x
        self.angularSpeed = cmd_vel2.angular.z

    def pickCmdVel(self, req):
        self.cmd = req.cmd_vel
        #print "got cmd_vel: {0}".format(req.cmd_vel)
        return PickCmdVelResponse(req.cmd_vel)
        
    def arbotPlayLedCallback(self, msg):
        self.arduino.arbot_set_play_led(mgs.data)    
    def arbotAdvanceLedCallback(self, msg):
        self.arduino.arbot_set_advance_led(msg.data)

# 需要导入模块: from tf.broadcaster import TransformBroadcaster [as 别名]
# 或者: from tf.broadcaster.TransformBroadcaster import sendTransform [as 别名]
class base_controller(Thread):
    """ Controller to handle movement & odometry feedback for a differential drive mobile base. """
    def __init__(self, name):
        Thread.__init__ (self)
        self.finished = Event()

        # Parameters
        self.rate = float(rospy.get_param("~base_controller_rate", 10))
        now = rospy.Time.now()
        
        self.t_delta = rospy.Duration(1.0/self.rate)
        self.t_next = now + self.t_delta
        self.then = now # time for determining dx/dy

        # internal data        
        self.x = 0.                  # position in xy plane
        self.y = 0.
        self.th = 0.                 # rotation in radians

        # Set up the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry)
        self.odomBroadcaster = TransformBroadcaster()
        
        rospy.loginfo("Started Odometry simmulator " + name )

    def run(self):
        rosRate = rospy.Rate(self.rate)
        rospy.loginfo("Publishing Odometry data at: " + str(self.rate) + " Hz")

        vx = 0.0
        vy = 0.0
        vth = 0.0

        while not rospy.is_shutdown() and not self.finished.isSet():
            now = rospy.Time.now()         
            if now > self.t_next:
                dt = now - self.then
                self.then = now
                dt = dt.to_sec()

                delta_x = (vx * cos(self.th) - vy * sin(self.th)) * dt
                delta_y = (vx * sin(self.th) + vy * cos(self.th)) * dt
                delta_th = vth * dt

                self.x += delta_x
                self.y += delta_y
                self.th += delta_th


                quaternion = Quaternion()
                quaternion.x = 0.0 
                quaternion.y = 0.0
                quaternion.z = sin(self.th / 2.0)
                quaternion.w = cos(self.th / 2.0)
    
                # Create the odometry transform frame broadcaster.
                self.odomBroadcaster.sendTransform(
                    (self.x, self.y, 0), 
                    (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                    rospy.Time.now(),
                    "base_link",
                    "odom"
                    )
    
                odom = Odometry()
                odom.header.frame_id = "odom"
                odom.child_frame_id = "base_link"
                odom.header.stamp = now
                odom.pose.pose.position.x = self.x
                odom.pose.pose.position.y = self.y
                odom.pose.pose.position.z = 0
                odom.pose.pose.orientation = quaternion
                odom.twist.twist.linear.x = vx
                odom.twist.twist.linear.y = 0
                odom.twist.twist.angular.z = vth
    
                self.odomPub.publish(odom)
                
                self.t_next = now + self.t_delta
                
            rosRate.sleep()

    def stop(self):
        print "Shutting down base odom_sim"
        self.finished.set()
        self.join()

# 需要导入模块: from tf.broadcaster import TransformBroadcaster [as 别名]
# 或者: from tf.broadcaster.TransformBroadcaster import sendTransform [as 别名]

#.........这里部分代码省略.........
            self.enc_left = self.left
            self.enc_right = self.right
           
            # distance traveled is the average of the two wheels 
            d = ( d_left + d_right ) / 2
            # this approximation works (in radians) for small angles
            th = ( d_right - d_left ) / self.base_width
            # calculate velocities
            self.dx = d / elapsed
            self.dr = th / elapsed
           
             
            if (d != 0):
                # calculate distance traveled in x and y
                x = cos( th ) * d
                y = -sin( th ) * d
                # calculate the final position of the robot
                self.x = self.x + ( cos( self.th ) * x - sin( self.th ) * y )
                self.y = self.y + ( sin( self.th ) * x + cos( self.th ) * y )
            if( th != 0):
                self.th = self.th + th
            
            #->??? Need to check about race condition with callback?????? later.
            #       it's _possible_ that self.orientation _MIGHT_ change between
            #       references. if it does, it's hopeful that the dt is very very tiny.
            copy_of_orientation = self.orientation

# emg - this is the section that needs to be updated to
#       bring in the IMU orientation
# http://answers.ros.org/question/38099/how-to-subscribe-to-a-topic-at-willon-demand/
# http://answers.ros.org/question/37869/subscribing-and-publishing/
#   in particular, the second one. it explains using spinOnce to
#       block until a message is received and then do other stuff,
#       and then go back to the top of a loop or something. which
#       is essentially what you've done.
# can have two subscribers - one for the wheels, and one for imu.
#   store the imu message in some part of the class and refer to
#   it here.
            # publish the odom information
# emg           quaternion = Quaternion()
# emg           quaternion.x = 0.0
# emg           quaternion.y = 0.0
# emg           quaternion.z = sin( self.th / 2 )
# emg           quaternion.w = cos( self.th / 2 )
            self.odomBroadcaster.sendTransform(
                (self.x, self.y, 0),
# emg               (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                copy_of_orientation,   # emg - the self-real orientation from /IMU_data
                rospy.Time.now(),
                self.base_frame_id,
                self.odom_frame_id
                )
            
            odom = Odometry()
            odom.header.stamp = now
            odom.header.frame_id = self.odom_frame_id
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
# emg           odom.pose.pose.orientation = quaternion
            odom.pose.pose.orientation = copy_of_orientation   # emg - again
            odom.child_frame_id = self.base_frame_id
            odom.twist.twist.linear.x = self.dx
            odom.twist.twist.linear.y = 0
            odom.twist.twist.angular.z = self.dr
            self.odomPub.publish(odom)
            
            

    #############################################################################
    def orientationCallback(self, msg):
    # emg - catch a wild quaternion
    #############################################################################
        self.orientation = msg

    #############################################################################
    def lwheelCallback(self, msg):
    #############################################################################
        enc = msg.data
        if (enc < self.encoder_low_wrap and self.prev_lencoder > self.encoder_high_wrap):
            self.lmult = self.lmult + 1
            
        if (enc > self.encoder_high_wrap and self.prev_lencoder < self.encoder_low_wrap):
            self.lmult = self.lmult - 1
            
        self.left = 1.0 * (enc + self.lmult * (self.encoder_max - self.encoder_min)) 
        self.prev_lencoder = enc
        
    #############################################################################
    def rwheelCallback(self, msg):
    #############################################################################
        enc = msg.data
        if(enc < self.encoder_low_wrap and self.prev_rencoder > self.encoder_high_wrap):
            self.rmult = self.rmult + 1
        
        if(enc > self.encoder_high_wrap and self.prev_rencoder < self.encoder_low_wrap):
            self.rmult = self.rmult - 1
            
        self.right = 1.0 * (enc + self.rmult * (self.encoder_max - self.encoder_min))
        self.prev_rencoder = enc

# 需要导入模块: from tf.broadcaster import TransformBroadcaster [as 别名]
# 或者: from tf.broadcaster.TransformBroadcaster import sendTransform [as 别名]
class NeatoNode:

    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('neato')

        self.tf_prefix = rospy.get_param('~tf_prefix', "")
        self.port = rospy.get_param('~port', "/dev/ttyUSB0")
        rospy.loginfo("Using port: %s"%(self.port))

        self.robot = xv11(self.port)

        rospy.Subscriber("pi_cmd",String,self.pi_command)

        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
        self.scanPub = rospy.Publisher('scan', LaserScan)
        self.odomPub = rospy.Publisher('odom',Odometry)
        self.odomBroadcaster = TransformBroadcaster()

        self.cmd_to_send = None

        self.cmd_vel = [0,0]

    def pi_command(self,msg):
        self.cmd_to_send = msg

    def spin(self):        
        encoders = [0,0]

        self.x = 0                  # position in xy plane
        self.y = 0
        self.th = 0

        # things that don't ever change
        scan_link = rospy.get_param('~frame_id','base_laser_link')
        scan = LaserScan(header=rospy.Header(frame_id=scan_link)) 
        scan.angle_min = 0
        scan.angle_max = 6.26
        scan.angle_increment = 0.017437326
        scan.range_min = 0.020
        scan.range_max = 5.0
        odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_link')
    
        # main loop of driver
        r = rospy.Rate(5)
        rospy.sleep(4)
        self.robot.requestScan()
        scan.header.stamp = rospy.Time.now()
        last_motor_time = rospy.Time.now()
        total_dth = 0.0
        while not rospy.is_shutdown():
            if self.cmd_to_send != None:
                self.robot.send_command(self.cmd_to_send)
                self.cmd_to_send = None

            t_start = time.time()
            (scan.ranges, scan.intensities) = self.robot.getScanRanges()
            print 'Got scan ranges %f' % (time.time() - t_start)

            # get motor encoder values
            curr_motor_time = rospy.Time.now()
            t_start = time.time()
            try:
                left, right = self.robot.getMotors()
                # now update position information
                dt = (curr_motor_time - last_motor_time).to_sec()
                last_motor_time = curr_motor_time

                d_left = (left - encoders[0])/1000.0
                d_right = (right - encoders[1])/1000.0

                encoders = [left, right]
                dx = (d_left+d_right)/2
                dth = (d_right-d_left)/(BASE_WIDTH/1000.0)
                total_dth += dth

                x = cos(dth)*dx
                y = -sin(dth)*dx

                self.x += cos(self.th)*x - sin(self.th)*y
                self.y += sin(self.th)*x + cos(self.th)*y
                self.th += dth

                # prepare tf from base_link to odom
                quaternion = Quaternion()
                quaternion.z = sin(self.th/2.0)
                quaternion.w = cos(self.th/2.0)

                # prepare odometry
                odom.header.stamp = curr_motor_time
                odom.pose.pose.position.x = self.x
                odom.pose.pose.position.y = self.y
                odom.pose.pose.position.z = 0
                odom.pose.pose.orientation = quaternion
                odom.twist.twist.linear.x = dx/dt
                odom.twist.twist.angular.z = dth/dt
                self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), curr_motor_time, self.tf_prefix + "/base_link", self.tf_prefix + "/odom" )
                self.odomPub.publish(odom)
                print 'Got motors %f' % (time.time() - t_start)
            except:
#.........这里部分代码省略.........

# 需要导入模块: from tf.broadcaster import TransformBroadcaster [as 别名]
# 或者: from tf.broadcaster.TransformBroadcaster import sendTransform [as 别名]

#.........这里部分代码省略.........

        marker.name = 'camera_marker'
        marker.description = 'Camera 6-DOF pose control'

        # X axis rotation
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 1
        control.orientation.y = 0
        control.orientation.z = 0
        control.name = "rotate_x"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        marker.controls.append(control)
        # X axis traslation
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 1
        control.orientation.y = 0
        control.orientation.z = 0
        control.name = "move_x"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        marker.controls.append(control)
        # Y axis rotation
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 1
        control.orientation.z = 0
        control.name = "rotate_y"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        marker.controls.append(control)
        # Y axis traslation
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 1
        control.orientation.z = 0
        control.name = "move_y"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        marker.controls.append(control)
        # Z axis rotation
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 0
        control.orientation.z = 1
        control.name = "rotate_z"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        marker.controls.append(control)
        # Z axis traslation
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 0
        control.orientation.z = 1
        control.name = "move_z"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        marker.controls.append(control)
        # Add marker to server
        self.server.insert(marker, self.marker_feedback)
        self.server.applyChanges()

    def publish_transform(self, timer_event):
        time = rospy.Time.now()
        self.pose_mutex.acquire()
        self.tf.sendTransform(self.marker_position, self.marker_orientation,
            time, 'sensor_base', 'map')
        self.pose_mutex.release()

    def marker_feedback(self, feedback):
        rospy.loginfo('Feedback from ' + feedback.marker_name)
        # Check event
        if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
            rospy.loginfo( 'Pose changed')
            # Update marker position
            self.pose_mutex.acquire()
            self.marker_position = (feedback.pose.position.x,
                feedback.pose.position.y, feedback.pose.position.z)
            # Update marker orientation
            self.marker_orientation = (feedback.pose.orientation.x,
                feedback.pose.orientation.y, feedback.pose.orientation.z,
                feedback.pose.orientation.w)
            self.pose_mutex.release()

    def shutdown(self):
        data = {
            'init_position' :
            {
                'x': 0.0,
                'y': 0.0,
                'z': 0.0,
                'roll': 0.0,
                'pitch': 0.0,
                'yaw': 0.0,
            }

        }
        rospy.loginfo('Writing current position')
        with open(self.filename, 'w') as outfile:
            outfile.write(yaml.dump(data, default_flow_style=True))

# 需要导入模块: from tf.broadcaster import TransformBroadcaster [as 别名]
# 或者: from tf.broadcaster.TransformBroadcaster import sendTransform [as 别名]

#.........这里部分代码省略.........
    def on_aseba_button_event(self, button):
        publisher = rospy.Publisher('buttons/' + button, Bool, queue_size=1)

        def callback(msg):
            bool_msg = Bool(msg.data[0])
            publisher.publish(bool_msg)
        return callback

    # ======== we send the speed to the aseba running on the robot  ========
    def set_speed(self, values):
        self.aseba_pub.publish(AsebaEvent(rospy.get_rostime(), 0, values))

    # ======== stop the robot safely ========
    def shutdown(self):
        self.set_speed([0, 0])

    def on_aseba_buttons_event(self, data):
        self.buttons.header.stamp = rospy.Time.now()
        self.buttons.buttons = data.data
        self.buttons_pub.publish(self.buttons)

    # ======== processing odometry events received from the robot ========
    def on_aseba_odometry_event(self, data):
        now = data.stamp
        dt = (now - self.then).to_sec()
        self.then = now

        m_l, m_r = data.data
        if abs(m_l) < self.motor_speed_deadband:
            m_l = 0
        if abs(m_r) < self.motor_speed_deadband:
            m_r = 0

        vl = self.left_wheel_speed(m_l)
        vr = self.right_wheel_speed(m_r)

        # wheel joint states
        left_wheel_angular_speed = vl / WHEEL_RADIUS
        right_wheel_angular_speed = vr / WHEEL_RADIUS

        self.left_wheel_angle += dt * left_wheel_angular_speed
        self.right_wheel_angle += dt * right_wheel_angular_speed

        dsl = vl * dt  # left wheel delta in m
        dsr = vr * dt  # right wheel delta in m

        # robot traveled distance in meters
        ds = ((dsl + dsr) / 2.0)
        dth = (dsr - dsl) / self.axis  # turn angle

        self.x += ds * cos(self.th + dth / 2.0)
        self.y += ds * sin(self.th + dth / 2.0)
        self.th += dth

        # We publish odometry, tf, and wheel joint state only at a maximal rate:
        if self.odom_min_period > (now - self.odom_msg.header.stamp).to_sec():
            return

        self.wheel_state_msg.header.stamp = rospy.Time.now()
        self.wheel_state_msg.position = [self.left_wheel_angle, self.right_wheel_angle]
        self.wheel_state_msg.velocity = [left_wheel_angular_speed, right_wheel_angular_speed]
        self.wheel_state_pub.publish(self.wheel_state_msg)

        # prepare tf from base_link to odom
        quaternion = Quaternion()
        quaternion.z = sin(self.th / 2.0)
        quaternion.w = cos(self.th / 2.0)

        # prepare odometry
        self.odom_msg.header.stamp = rospy.Time.now()  # OR TO TAKE ONE FROM THE EVENT?
        self.odom_msg.pose.pose.position.x = self.x
        self.odom_msg.pose.pose.position.y = self.y
        self.odom_msg.pose.pose.orientation = quaternion

        if(dt > 0):
            self.odom_msg.twist.twist.linear.x = ds / dt
            self.odom_msg.twist.twist.angular.z = dth / dt

        # publish odometry
        self.odom_broadcaster.sendTransform(
            (self.x, self.y, 0),
            (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            self.then, self.robot_frame, self.odom_frame)
        self.odom_pub.publish(self.odom_msg)

    def set_linear_angular_speed(self, speed, ang_speed):
        left_wheel_speed = speed - ang_speed * 0.5 * self.axis
        right_wheel_speed = speed + ang_speed * 0.5 * self.axis

        left_motor_speed = round(self.left_wheel_motor_speed(left_wheel_speed))
        right_motor_speed = round(self.right_wheel_motor_speed(right_wheel_speed))
        max_motor_speed = max(abs(left_motor_speed), abs(right_motor_speed))
        if max_motor_speed > MAX_SPEED:
            return self.set_linear_angular_speed(speed * MAX_SPEED / max_motor_speed,
                                                 ang_speed * MAX_SPEED / max_motor_speed)

        self.set_speed([left_motor_speed, right_motor_speed])

    def on_cmd_vel(self, data):
        self.set_linear_angular_speed(data.linear.x, data.angular.z)

# 需要导入模块: from tf.broadcaster import TransformBroadcaster [as 别名]
# 或者: from tf.broadcaster.TransformBroadcaster import sendTransform [as 别名]

#.........这里部分代码省略.........
		    # send updated movement commands
		    #if self.cmd_vel != self.old_vel or self.cmd_vel == [0,0]:
                    # max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
		    #self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], (abs(self.cmd_vel[0])+abs(self.cmd_vel[1]))/2)
		    self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
		    cmd_rate = self.CMD_RATE

            self.old_vel = self.cmd_vel

            # prepare laser scan
            scan.header.stamp = rospy.Time.now()
           
            self.robot.requestScan()
            scan.ranges = self.robot.getScanRanges()

            # now update position information
            dt = (scan.header.stamp - then).to_sec()
            then = scan.header.stamp

            d_left =  (left - encoders[0])/1000.0
            d_right =  (right - encoders[1])/1000.0
            encoders = [left, right]

	    #print d_left, d_right, encoders

            dx = (d_left+d_right)/2
            dth = (d_right-d_left)/(self.robot.base_width/1000.0)

            x = cos(dth)*dx
            y = -sin(dth)*dx
            self.x += cos(self.th)*x - sin(self.th)*y
            self.y += sin(self.th)*x + cos(self.th)*y
            self.th += dth
            #print self.x,self.y,self.th

            # prepare tf from base_link to odom
            quaternion = Quaternion()
            quaternion.z = sin(self.th/2.0)
            quaternion.w = cos(self.th/2.0)

            # prepare odometry
            odom.header.stamp = rospy.Time.now()
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.twist.twist.linear.x = dx/dt
            odom.twist.twist.angular.z = dth/dt


            # sensors
            lsb, rsb, lfb, rfb = self.robot.getDigitalSensors()

            # buttons
            btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down = self.robot.getButtons()


            # publish everything
            self.odomBroadcaster.sendTransform((self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z,
                                                                     quaternion.w), then, "base_footprint", "odom")
            self.scanPub.publish(scan)
            self.odomPub.publish(odom)
            button_enum = ("Soft_Button", "Up_Button", "Start_Button", "Back_Button", "Down_Button")
            sensor_enum = ("Left_Side_Bumper", "Right_Side_Bumper", "Left_Bumper", "Right_Bumper")
            for idx, b in enumerate((btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down)):
                if b == 1:
                    button.value = b
                    button.name = button_enum[idx]
                    self.buttonPub.publish(button)

            for idx, b in enumerate((lsb, rsb, lfb, rfb)):
                if b == 1:
                    sensor.value = b
                    sensor.name = sensor_enum[idx]
                    self.sensorPub.publish(sensor)
            # wait, then do it again
            r.sleep()

        # shut down
        self.robot.setBacklight(0)
        self.robot.setLED("Off")
        self.robot.setLDS("off")
        self.robot.setTestMode("off")

    def sign(self,a):
        if a>=0:
		return 1
	else:
		return-1

    def cmdVelCb(self,req):
        x = req.linear.x * 1000
        th = req.angular.z * (self.robot.base_width/2)
        k = max(abs(x-th),abs(x+th))
        # sending commands higher than max speed will fail

        if k > self.robot.max_speed:
            x = x*self.robot.max_speed/k; th = th*self.robot.max_speed/k

        self.cmd_vel = [int(x-th), int(x+th)]

# 需要导入模块: from tf.broadcaster import TransformBroadcaster [as 别名]
# 或者: from tf.broadcaster.TransformBroadcaster import sendTransform [as 别名]
class SMALdogROS:

    def __init__(self, execute=False):
        self.robot = SMALdog()
        if execute:
            self.conn = EthBridge()
        else:
            self.conn = None

        self.x = 0
        self.y = 0

        self.joint_state_pub = rospy.Publisher('joint_states', JointState)
        self.odom_broadcaster = TransformBroadcaster()
        rospy.Subscriber("cmd_vel", Twist, self.cmdCb)
    
    def run(self):
        controller = MuybridgeGaitController(self.robot, self.robot.DEFAULT_STANCE)
        old_pose = self.robot.getIK(self.robot.DEFAULT_STANCE)
        old_pose["x"] = 0.0
        old_pose["y"] = 0.0
        old_pose["r"] = 0.0

        draw = StabilityVisualization(self.robot)

        r = rospy.Rate(50)
        while not rospy.is_shutdown():
            # get stance from controller TODO: this really should be a motion plan
            new_stance = controller.next(self.x, 0, 0) # TODO: add y/r
            t = new_stance[0]
            #draw.draw(new_stance[1], controller)

            # do IK
            new_pose = self.robot.getIK(new_stance[1])
            new_pose["x"] = controller.x
            new_pose["y"] = controller.y
            new_pose["r"] = controller.r

            # interpolate
            for pose in self.interpolate(old_pose, new_pose, int(t/0.02)):
                # publish
                msg = JointState()
                msg.header.stamp = rospy.Time.now()
                for name in self.robot.names:
                    msg.name.append(name)
                    msg.position.append(pose[name])
                    if pose[name] == float('nan'):
                        print "WARN", name, "is nan"
                self.joint_state_pub.publish(msg)

                # TF
                quaternion = Quaternion()
                quaternion.x = 0.0
                quaternion.y = 0.0
                quaternion.z = sin(pose["r"]/2)
                quaternion.w = cos(pose["r"]/2)
                self.odom_broadcaster.sendTransform(
                    (pose["x"], pose["y"], 0.095),
                    (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                    rospy.Time.now(),
                    "body_link",
                    "world"
                    )

                if self.conn:
                    packet = makeSyncWritePacket(convertToAX12(pose, self.robot))
                    self.conn.send(self.conn.makePacket(254, ax12.AX_SYNC_WRITE, packet))

                r.sleep()

            old_pose = new_pose

    def interpolate(self, start_pose, end_pose, iterations):
        diffs = dict()
        for name in start_pose.keys():
            diffs[name] = (end_pose[name] - start_pose[name])/iterations
        for i in range(iterations):
            pose = dict()
            for name in start_pose.keys():
                pose[name] = start_pose[name] + (diffs[name]*i)
            yield pose

    def cmdCb(self, msg):
        # TODO: add y/r
        if msg.linear.x > 0.075:
            self.x = 0.075
        else:
            self.x = msg.linear.x

# 需要导入模块: from tf.broadcaster import TransformBroadcaster [as 别名]
# 或者: from tf.broadcaster.TransformBroadcaster import sendTransform [as 别名]

#.........这里部分代码省略.........
                        return -float('inf')
                if raw<800:
                        return float('inf')
                return -0.0736*log(raw)+0.632 
        
        def on_aseba_proximity_event(self,msg):
                data=msg.data
                values=[self.proximity2range(d) for d in data]
                for sensor,value  in zip(self.proximity_sensors,values):
                        sensor['msg'].range=value
                        sensor['msg'].header.stamp=rospy.Time.now()
                        sensor['publisher'].publish(sensor['msg'])

                self.proximityToLaser.ranges=[]
                self.proximityToLaser.intensities=[]
                self.proximityToLaser.header.stamp=rospy.Time.now()
                for dist,raw in zip(values,data)[4::-1]:
                        if dist>0.14:
                                dist=0.14
                        if dist<0.0215:
                                dist=0.0215
                        self.proximityToLaser.ranges.append(dist+0.08)
                        self.proximityToLaser.intensities.append(raw)
                    
                        
                self.proximityToLaserPublisher.publish(self.proximityToLaser)
                

        def on_aseba_button_event(self,button):
                publisher=rospy.Publisher('buttons/'+button,Bool,queue_size=1)
                def callback(msg):
                        bool_msg=Bool(msg.data[0])
                        publisher.publish(bool_msg)
                return callback

	# ======== we send the speed to the aseba running on the robot  ======== 
	def set_speed(self,values):
		self.aseba_pub.publish(AsebaEvent(rospy.get_rostime(),0,values))
	
	# ======== stop the robot safely ======== 
	def shutdown(self):
		self.set_speed([0,0])
                

        def on_aseba_buttons_event(self,data):
                self.buttons.header.stamp=rospy.Time.now()
                self.buttons.buttons=data.data
                self.buttons_pub.publish(self.buttons)

	# ======== processing odometry events received from the robot ======== 
	def on_aseba_odometry_event(self,data): 
		now = data.stamp
		dt = (now-self.then).to_sec()
		self.then = now
		dsl = (data.data[0]*dt)/SPEED_COEF # left wheel delta in mm
		dsr = (data.data[1]*dt)/SPEED_COEF # right wheel delta in mm
		ds = ((dsl+dsr)/2.0)/1000.0      # robot traveled distance in meters
		dth = (dsr-dsl)/BASE_WIDTH  # turn angle

		self.x += ds*cos(self.th+dth/2.0)
		self.y += ds*sin(self.th+dth/2.0)
		self.th+= dth

		# prepare tf from base_link to odom 
		quaternion = Quaternion()
		quaternion.z = sin(self.th/2.0)
		quaternion.w = cos(self.th/2.0)

		# prepare odometry
		self.odom.header.stamp = rospy.Time.now() # OR TO TAKE ONE FROM THE EVENT?
		self.odom.pose.pose.position.x = self.x
		self.odom.pose.pose.position.y = self.y
		self.odom.pose.pose.position.z = 0
		self.odom.pose.pose.orientation = quaternion

                if(dt>0):
                        self.odom.twist.twist.linear.x = ds/dt
                        self.odom.twist.twist.angular.z = dth/dt

		# publish odometry
		self.odom_broadcaster.sendTransform((self.x,self.y,0),(quaternion.x,quaternion.y,quaternion.z,quaternion.w),self.then,"base_link","odom")
		self.odom_pub.publish(self.odom)
	
	# ======== processing events received from the robot  ======== 
	def on_cmd_vel(self,data):
		x = data.linear.x * 1000.0 # from meters to millimeters 
		x = x * SPEED_COEF # to thymio units
		th = data.angular.z * (BASE_WIDTH/2) # in mm
		th = th * SPEED_COEF # in thymio units
		k = max(abs(x-th),abs(x+th))
		# sending commands higher than max speed will fail
		if k > MAX_SPEED:
			x = x*MAX_SPEED/k; th = th*MAX_SPEED/k
		self.set_speed([int(x-th),int(x+th)])

	# ======== ======== ======== ======== ======== ======== ========      
	def control_loop(self):	
		rospy.on_shutdown(self.shutdown) # on shutdown hook
		while not rospy.is_shutdown():
			rospy.spin()

# 需要导入模块: from tf.broadcaster import TransformBroadcaster [as 别名]
# 或者: from tf.broadcaster.TransformBroadcaster import sendTransform [as 别名]

#.........这里部分代码省略.........
            # Calculate odometry
            if self.enc_left == None:
                dright = 0
                dleft = 0
            else:
                dright = (right_enc - self.enc_right) / self.ticks_per_meter
                dleft = (left_enc - self.enc_left) / self.ticks_per_meter

            self.enc_right = right_enc
            self.enc_left = left_enc
            
            dxy_ave = self.odom_linear_scale_correction * (dright + dleft) / 2.0
            dth = self.odom_angular_scale_correction * (dright - dleft) / float(self.wheel_track)
            vxy = dxy_ave / dt
            vth = dth / dt
                
            if (dxy_ave != 0):
                dx = cos(dth) * dxy_ave
                dy = -sin(dth) * dxy_ave
                self.x += (cos(self.th) * dx - sin(self.th) * dy)
                self.y += (sin(self.th) * dx + cos(self.th) * dy)
    
            if (dth != 0):
                self.th += dth 
    
            quaternion = Quaternion()
            quaternion.x = 0.0 
            quaternion.y = 0.0
            quaternion.z = sin(self.th / 2.0)
            quaternion.w = cos(self.th / 2.0)
    
            # Create the odometry transform frame broadcaster.
            if self.publish_odom_base_transform:
                self.odomBroadcaster.sendTransform(
                    (self.x, self.y, 0), 
                    (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                    rospy.Time.now(),
                    self.base_frame,
                    "odom"
                    )
    
            odom = Odometry()
            odom.header.frame_id = "odom"
            odom.child_frame_id = self.base_frame
            odom.header.stamp = now
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.twist.twist.linear.x = vxy
            odom.twist.twist.linear.y = 0
            odom.twist.twist.angular.z = vth
            
            self.current_speed = Twist()
            self.current_speed.linear.x = vxy
            self.current_speed.angular.z = vth

            """
            Covariance values taken from Kobuki node odometry.cpp at:
            https://github.com/yujinrobot/kobuki/blob/indigo/kobuki_node/src/library/odometry.cpp
            
            Pose covariance (required by robot_pose_ekf) TODO: publish realistic values
            Odometry yaw covariance must be much bigger than the covariance provided
            by the imu, as the later takes much better measures
            """
            odom.pose.covariance[0]  = 0.1