Python TransformBroadcaster.sendTransform方法代码示例

# 需要导入模块: from tf import TransformBroadcaster [as 别名]
# 或者: from tf.TransformBroadcaster import sendTransform [as 别名]
class Right_Utility_Frame():
  
    frame = 'base_footprint'
    px = py = pz = 0;
    qx = qy = qz = 0;
    qw = 1;

    def __init__(self):
        rospy.init_node('right_utilitiy_frame_source')
        
        self.updater = rospy.Service('r_utility_frame_update', FrameUpdate, self.update_frame)
        
        self.tfb = TransformBroadcaster()

    def update_frame(self, req):
        ps = req.pose
        self.frame = ps.header.frame_id
        self.px = ps.pose.position.x    
        self.py = ps.pose.position.y    
        self.pz = ps.pose.position.z    
        self.qx = ps.pose.orientation.x
        self.qy = ps.pose.orientation.y
        self.qz = ps.pose.orientation.z
        self.qw = ps.pose.orientation.w

        self.tfb.sendTransform((self.px,self.py,self.pz),(self.qx,self.qy,self.qz,self.qw), rospy.Time.now(), "rh_utility_frame", self.frame)
        rsp = Bool()
        rsp.data = True
        return rsp

# 需要导入模块: from tf import TransformBroadcaster [as 别名]
# 或者: from tf.TransformBroadcaster import sendTransform [as 别名]
class Left_Utility_Frame():
  
    frame = 'base_footprint'
    px = py = pz = 0;
    qx = qy = qz = 0;
    qw = 1;

    def __init__(self):
        rospy.init_node('left_utilitiy_frame_source')
        
        self.updater = rospy.Service('l_utility_frame_update', FrameUpdate, self.update_frame)
        
        self.tfb = TransformBroadcaster()

    def update_frame(self, req):
        ps = req.pose
        if not (math.isnan(ps.pose.orientation.x) or 
                math.isnan(ps.pose.orientation.y) or
                math.isnan(ps.pose.orientation.z) or
                math.isnan(ps.pose.orientation.w)):
            self.frame = ps.header.frame_id
            self.px = ps.pose.position.x    
            self.py = ps.pose.position.y    
            self.pz = ps.pose.position.z    
            self.qx = ps.pose.orientation.x
            self.qy = ps.pose.orientation.y
            self.qz = ps.pose.orientation.z
            self.qw = ps.pose.orientation.w
        else:
            rospy.logerr("NAN's sent to l_utility_frame_source")

        self.tfb.sendTransform((self.px,self.py,self.pz),(self.qx,self.qy,self.qz,self.qw), rospy.Time.now(), "lh_utility_frame", self.frame)
        rsp = Bool()
        rsp.data = True
        return rsp

# 需要导入模块: from tf import TransformBroadcaster [as 别名]
# 或者: from tf.TransformBroadcaster import sendTransform [as 别名]
class OptiTrackClient():
    def __init__(self, addr, port, obj_names, world, dt, rate=120):
        """
        Class tracking optitrack markers through VRPN and publishing their TF frames as well as the transformation
        from the robot's world frame to the optitrack frame
        :param addr: IP of the VRPN server
        :param port: Port of the VRPN server
        :param obj_names: Name of the tracked rigid bodies
        :param world: Name of the robot's world frame (base_link, map, base, ...)
        :param dt: Delta T in seconds whilst a marker is still considered tracked
        :param rate: Rate in Hertz of the publishing loop
        """
        self.rate = rospy.Rate(rate)
        self.obj_names = obj_names
        self.world = world
        self.dt = rospy.Duration(dt)
        self.tfb = TransformBroadcaster()

        self.trackers = []
        for obj in obj_names:
            t = vrpn.receiver.Tracker('{}@{}:{}'.format(obj, addr, port))
            t.register_change_handler(obj, self.handler, 'position')
            self.trackers.append(t)

        self.tracked_objects = {}

    @property
    def recent_tracked_objects(self):
        """ Only returns the objects that have been tracked less than 20ms ago. """
        f = lambda name: (rospy.Time.now() - self.tracked_objects[name].timestamp)
        return dict([(k, v) for k, v in self.tracked_objects.iteritems() if f(k) < self.dt])

    def handler(self, obj, data):
        self.tracked_objects[obj] = TrackedObject(data['position'],
                                                  data['quaternion'],
                                                  rospy.Time.now())

    def run(self):
        while not rospy.is_shutdown():
            for t in self.trackers:
                t.mainloop()

            # Publish the calibration matrix
            calib_matrix = rospy.get_param("/optitrack/calibration_matrix")
            self.tfb.sendTransform(calib_matrix[0], calib_matrix[1], rospy.Time.now(), "optitrack_frame", self.world)

            # Publish all other markers as frames
            for k, v in self.recent_tracked_objects.iteritems():
                self.tfb.sendTransform(v.position, v.quaternion, v.timestamp, k, "optitrack_frame")

            self.rate.sleep()

# 需要导入模块: from tf import TransformBroadcaster [as 别名]
# 或者: from tf.TransformBroadcaster import sendTransform [as 别名]
class Rebroadcaster(object):
    def __init__(self):
        self.broadcaster = TransformBroadcaster()
        self.ell_param_sub = rospy.Subscriber('ellipsoid_params', EllipsoidParams, self.ell_cb)
        self.transform = None

    def ell_cb(self, ell_msg):
        print "Got transform"
        self.transform = copy.copy(ell_msg.e_frame)

    def send_transform(self):
        print "spinning", self.transform
        if self.transform is not None:
            print "Sending frame"
            self.broadcaster.sendTransform(self.transform)

# 需要导入模块: from tf import TransformBroadcaster [as 别名]
# 或者: from tf.TransformBroadcaster import sendTransform [as 别名]
class FakeHuman(EventSimulator):
    '''
    Simulating the perceptual data we would get from a human.
    The human is symbolized by a red ball at roughly human size
    relative to the robot in the view.
    
    Given a schedule of poses for the human, instances of
    this class move the human through the scheduled timed poses. 
    A schedule looks like this:
    
		motionSchedule = OrderedDict();
		
		motionSchedule[3]  = {'target': [2,0], 'speed': [0.5,0.0]}; 
		motionSchedule[6]  = {'target': [3,0], 'speed': [0.5,0.0]}; 
		motionSchedule[9]  = {'target': [4,0], 'speed': [0.5,0.0]};
    
    Here the motionSchedule keys (3,6, and 9) are seconds from program start.
    the 'target' numbers are meters of distance from the robot, and the 
    two speed components are meters/sec in the x and y direction, respectively.
    
    When the scheduled pose sequence is completed, it starts over.
    Start the sequence with a call to a FakeHuman's start() method, passing
    it a schedule. Stop the motions via a call to the stop() method.
    
    '''
    
    
    
    def __init__(self):
        self.tfBroadcaster = TransformBroadcaster()
        q = transformations.quaternion_from_euler(0,0,pi)
        self.humanPose = Pose(Point(5, 0, 1.65), Quaternion(*q))
        self.markerPublisher = rospy.Publisher('visualization_marker', Marker)
        rospy.loginfo('Initialized.')
        self.startTime = time.time()
        rospy.init_node('fake_human_node', anonymous=True)

    def start(self, motionSchedule):

        self.motionSchedule = motionSchedule;
        self.motionScheduleKeys = motionSchedule.keys();
        # Current index into the motion schedule keys:
        self.motionScheduleIndex = 0;
        
        threadSchedule = OrderedDict();
        # Create a schedule that calls the callback function (i.e. self.update())
        # at 0.2 seconds, and then repeats. The None is the argument passed
        # to self.update().
        threadSchedule[0.2] = None;
        self.startTime = time.time();
        #******super(FakeHuman, self).start(threadSchedule, self.update, repeat=True, callbackInterval=FRAME_INTERVAL);
        super(FakeHuman, self).start(motionSchedule, self.update, repeat=True, callbackInterval=FRAME_INTERVAL);

    def publishTFPose(self, pose, name, parent):
        if (pose != None)  and (not rospy.is_shutdown()):
            try:
                self.tfBroadcaster.sendTransform((pose.position.x, pose.position.y, pose.position.z),
                     (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w),
                     rospy.Time.now(), name, parent)
            except rospy.ROSException:
                rospy.loginfo("Fake human topic was closed during publish. Likely due to explicit shutdown request.")

    def getPlanarDistance(self, pose):
        '''
        Compute distance of a given pose from its origin in a 2d plane.
        @param pose: pose to examine
        @type pose: Pose
        @return: linear distance
        @rType: float
        '''
        vec = numpy.array((pose.position.x, pose.position.y))
        self.planarDistance = numpy.linalg.norm(vec) 
        return self.planarDistance

    def getDiff(self, target, current, speed):
    	diff = target - current
    	if diff > speed:
    		return speed, False
    	elif diff < -speed:
    		return -speed, False
    	return diff, True
	 
    def moveTowardsTarget(self, speed):
        '''
        Called from update(). Moves the rviz representation of the human towards 
        a target position.
        @param speed: motion speed
        @type speed: float
        @return: whether fake human moved closer to the robot, or away from it
        @rtype: MotionDirection
        '''
        initialDistanceFromRobot = self.getPlanarDistance(self.humanPose);
    	xDiff, xReached = self.getDiff(self.target[0], self.humanPose.position.x, speed[0])
    	yDiff, yReached = self.getDiff(self.target[1], self.humanPose.position.y, speed[1])
    	self.humanPose.position.x += xDiff
    	self.humanPose.position.y += yDiff
        newDistanceFromRobot = self.getPlanarDistance(self.humanPose)
        if (newDistanceFromRobot - initialDistanceFromRobot) > 0:
            return MotionDirection.TOWARDS_ROBOT
        else:
#.........这里部分代码省略.........

# 需要导入模块: from tf import TransformBroadcaster [as 别名]
# 或者: from tf.TransformBroadcaster import sendTransform [as 别名]
class PublishTf:
    def __init__(self):
        self.listener = TransformListener()
        self.br = TransformBroadcaster()
        self.pub_freq = 100.0
        self.parent_frame_id = "world"
        self.child1_frame_id = "reference1"
        self.child2_frame_id = "reference2"
        self.child3_frame_id = "reference3"
        self.child4_frame_id = "reference4"
        rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference2)
        rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference3)
        rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference4)
        rospy.sleep(1.0)

        recorder_0 = RecordingManager("all")
        recorder_1 = RecordingManager("test1")
        recorder_2 = RecordingManager("test2")
        recorder_3 = RecordingManager("test3")

        recorder_0.start()
        recorder_1.start()
        self.pub_line(length=1, time=5)
        recorder_1.stop()
        recorder_2.start()
        self.pub_quadrat(length=2, time=10)
        recorder_2.stop()
        recorder_3.start()
        self.pub_circ(radius=2, time=5)
        recorder_3.stop()
        recorder_0.stop()

    def reference2(self, event):
        self.check_for_ctrlc()
        self.pub_tf(self.child1_frame_id, self.child2_frame_id, [1, 0, 0])

    def reference3(self, event):
        self.check_for_ctrlc()
        self.pub_tf(self.child1_frame_id, self.child3_frame_id, [math.sin(rospy.Time.now().to_sec()), 0, 0])

    def reference4(self, event):
        self.check_for_ctrlc()
        self.pub_tf(self.child1_frame_id, self.child4_frame_id, [math.sin(rospy.Time.now().to_sec()),
                                                                 math.cos(rospy.Time.now().to_sec()), 0])

    def pub_tf(self, parent_frame_id, child1_frame_id, xyz=[0, 0, 0], rpy=[0, 0, 0]):
        self.check_for_ctrlc()
        self.br.sendTransform((xyz[0], xyz[1], xyz[2]), transformations.quaternion_from_euler(
            rpy[0], rpy[1], rpy[2]), rospy.Time.now(), child1_frame_id, parent_frame_id)

    def pub_line(self, length=1, time=1):
        rospy.loginfo("Line")
        rate = rospy.Rate(int(self.pub_freq))

        for i in range((int(self.pub_freq * time / 2) + 1)):
            t = i / self.pub_freq / time * 2
            self.pub_tf(self.parent_frame_id, self.child1_frame_id, [t * length, 0, 0])
            rate.sleep()
        for i in range((int(self.pub_freq * time / 2) + 1)):
            t = i / self.pub_freq / time * 2
            self.pub_tf(self.parent_frame_id, self.child1_frame_id, [(1 - t) * length, 0, 0])
            rate.sleep()

    def pub_circ(self, radius=1, time=1):
        rospy.loginfo("Circ")
        rate = rospy.Rate(int(self.pub_freq))

        for i in range(int(self.pub_freq * time) + 1):
            t = i / self.pub_freq / time
            self.pub_tf(self.parent_frame_id, self.child1_frame_id, [-radius * math.cos(2 * math.pi * t) + radius,
                                                                     -radius * math.sin(2 * math.pi * t),
                                                                     0])
            rate.sleep()

    def pub_quadrat(self, length=1, time=1):
        rospy.loginfo("Quadrat")
        rate = rospy.Rate(int(self.pub_freq))

        for i in range((int(self.pub_freq * time / 4) + 1)):
            t = i / self.pub_freq / time * 4
            self.pub_tf(self.parent_frame_id, self.child1_frame_id, [t * length, 0, 0])
            rate.sleep()
        for i in range((int(self.pub_freq * time / 4) + 1)):
            t = i / self.pub_freq / time * 4
            self.pub_tf(self.parent_frame_id, self.child1_frame_id, [length, t * length, 0])
            rate.sleep()
        for i in range((int(self.pub_freq * time / 4) + 1)):
            t = i / self.pub_freq / time * 4
            self.pub_tf(self.parent_frame_id, self.child1_frame_id, [(1 - t) * length, length, 0])
            rate.sleep()
        for i in range((int(self.pub_freq * time / 4) + 1)):
            t = i / self.pub_freq / time * 4
            self.pub_tf(self.parent_frame_id, self.child1_frame_id, [0, (1 - t) * length, 0])
            rate.sleep()

    @staticmethod
    def check_for_ctrlc():
        if rospy.is_shutdown():
            sys.exit()

# 需要导入模块: from tf import TransformBroadcaster [as 别名]
# 或者: from tf.TransformBroadcaster import sendTransform [as 别名]
class FlyerViz(object):
    """
    The main purpose of this node is to listen to telemetry from the flyer and use it to generate
    messages for the various rviz display types (in most cases, Marker or MarkerArray).
    """

    # latest_llstatus = None
    latest_odom = None
    latest_controller_status = None

    def __init__(self):
        self.init_params()
        self.init_publishers()
        self.init_vars()
        self.init_viz()
        self.init_subscribers()
        self.init_timers()
        rospy.loginfo("Initialization complete")

    def init_params(self):
        self.publish_freq = rospy.get_param("~publish_freq", 20.0)
        self.subscriber_topic_prefix = rospy.get_param("~subscriber_topic_prefix", "downlink/")

    def init_publishers(self):
        self.mpub = rospy.Publisher("flyer_viz", Marker)
        self.mapub = rospy.Publisher("flyer_viz_array", MarkerArray)
        self.tfbr = TransformBroadcaster()

    def init_vars(self):
        self.trajectory = np.empty((1000, 3))
        self.n_trajectory = 0

    def init_viz(self):
        self.mode_t = TextMarker("mode", "", (0, 0, 0))
        self.heading_marker = HeadingMarker("heading", (0, 0, 0))
        self.vlm = VerticalLineMarker("altitude", (1, 1), color=Colors.WHITE + Alpha(0.5))
        self.alt_t = TextMarker("altitude", "0.0", (0, 0, 0))
        self.pos_t = TextMarker("position", "0.0,0.0", (0, 0, 0))
        self.trail = TrailMarker("trail", [], colors=[], color=Colors.BLUE + Alpha(0.8))
        self.trail.set_max_points(500)
        self.ground_trail = TrailMarker("ground_track", [], color=Colors.WHITE + Alpha(0.2))
        self.ground_trail.set_max_points(500)
        self.boundary = PolygonMarker(
            "boundary",
            ((1.5, 1.5, 0), (-1.5, 1.5, 0), (-1.5, -1.5, 0), (1.5, -1.5, 0)),
            color=Colors.RED + Alpha(0.5),
            width=0.02,
        )
        self.mg = MarkerGroup(self.mapub)
        self.mg.add(
            self.mode_t,
            self.vlm,
            self.alt_t,
            self.pos_t,
            self.trail,
            self.ground_trail,
            self.heading_marker,
            self.boundary,
        )

    def init_subscribers(self):
        prefix = self.subscriber_topic_prefix
        # self.llstatus_sub = rospy.Subscriber(prefix+'autopilot/LL_STATUS', LLStatus, self.llstatus_callback) # AscTec specific.. how to make more generic?
        self.odom_sub = rospy.Subscriber(prefix + "estimator/output", Odometry, self.odom_callback)
        self.controller_status_sub = rospy.Subscriber(
            prefix + "controller/status", controller_status, self.controller_status_callback
        )

    def init_timers(self):
        self.publish_timer = Timer(rospy.Duration(1 / self.publish_freq), self.publish_timer_callback)

    def publish_timer_callback(self, event):
        now = rospy.Time.now()
        if self.latest_controller_status is not None:
            self.mode_t.set(text=self.latest_controller_status.active_mode)
        if self.latest_odom is not None:
            pos = self.latest_odom.pose.pose.position
            loppo = self.latest_odom.pose.pose.orientation
            ori_ypr = tft.euler_from_quaternion((loppo.x, loppo.y, loppo.z, loppo.w), "rzyx")
            self.vlm.set((pos.x, pos.y), zend=pos.z)
            self.mode_t.set(pos=(pos.x, pos.y, pos.z - 0.1))
            self.alt_t.set(text="%.3f" % -pos.z, pos=(pos.x, pos.y, pos.z / 2))
            self.pos_t.set(text="%.2f, %.2f" % (pos.x, pos.y), pos=(pos.x, pos.y, 0.02))
            self.heading_marker.set(pos=(pos.x, pos.y, pos.z), heading=degrees(ori_ypr[0]))
            self.tfbr.sendTransform(
                (pos.x, pos.y, pos.z), (loppo.x, loppo.y, loppo.z, loppo.w), now, "/viz/flyer_axes", "/ned"
            )
            self.tfbr.sendTransform((pos.y, pos.x, -pos.z), (0, 0, 0, 1), now, "/viz/chase", "/enu")
            self.tfbr.sendTransform(
                (0, 0, 0), tft.quaternion_from_euler(0, radians(180), 0, "rzyx"), now, "/viz/onboard", "/viz/flyer_axes"
            )
        self.mg.publish(now)

    #    def llstatus_callback(self, msg):
    #        self.latest_llstatus = msg

    def controller_status_callback(self, msg):
        self.latest_controller_status = msg

    def odom_callback(self, msg):
#.........这里部分代码省略.........

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

#.........这里部分代码省略.........
            
	if xy_theta == None:
            xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.robot_pose)
            print self.robot_pose
            print xy_theta
            # raw_input()
        self.particle_cloud = []
        # TODO create particles

	#create a normal distribution of particles around starting position
	#then normalize and update pose accordingly
        x_vals = np.random.normal(xy_theta[0], self.lin_spread, self.n_particles)
        y_vals = np.random.normal(xy_theta[1], self.lin_spread, self.n_particles)
        t_vals = np.random.normal(xy_theta[2], self.ang_spread, self.n_particles)
        self.particle_cloud = [Particle(x_vals[i], y_vals[i], t_vals[i], 1)
                               for i in xrange(self.n_particles)]
        
        self.normalize_particles()
        self.update_robot_pose()
        # TODO(mary): create particles
        

    def normalize_particles(self):
        """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
        total_weight = sum([particle.w for particle in self.particle_cloud]) * 1.0
        for particle in self.particle_cloud:
          particle.w /= total_weight 
      
    def publish_particles(self, msg):
        particles_conv = []
        for p in self.particle_cloud:
            particles_conv.append(p.as_pose())
        # actually send the message so that we can view it in rviz
        self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),frame_id=self.map_frame),poses=particles_conv))

    def scan_received(self, msg):
        self.scan_count +=1
        """ This is the default logic for what to do when processing scan data.  Feel free to modify this, however,
            I hope it will provide a good guide.  The input msg is an object of type sensor_msgs/LaserScan """
        if not(self.initialized):
            # wait for initialization to complete
            return

        if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)):
            # need to know how to transform the laser to the base frame
            # this will be given by either Gazebo or neato_node
            return

        if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)):
            # need to know how to transform between base and odometric frames
            # this will eventually be published by either Gazebo or neato_node
            return

        # calculate pose of laser relative ot the robot base
        p = PoseStamped(header=Header(stamp=rospy.Time(0),frame_id=msg.header.frame_id))
        self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)

        # find out where the robot thinks it is based on its odometry
        p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id=self.base_frame), pose=Pose())
        self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
        # store the the odometry pose in a more convenient format (x,y,theta)
        new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose)
        

        if not(self.particle_cloud):
            # now that we have all of the necessary transforms we can update the particle cloud
            self.initialize_particle_cloud()
            # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
            self.current_odom_xy_theta = new_odom_xy_theta
            # update our map to odom transform now that the particles are initialized
            self.fix_map_to_odom_transform(msg)
                    # if self.test or self.scan_count % 1 is 0:
            print "updated pose:"
            print self.robot_pose
        elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
              math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
              math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
            
            # we have moved far enough to do an update!
            self.update_particles_with_odom(msg)    # update based on odometry
            self.update_particles_with_laser(msg)   # update based on laser scan
            self.update_robot_pose()                # update robot's pose
            self.resample_particles()               # resample particles to focus on areas of high density
            self.fix_map_to_odom_transform(msg)     # update map to odom transform now that we have new particles
        # publish particles (so things like rviz can see them)
        self.publish_particles(msg)

    def fix_map_to_odom_transform(self, msg):
        """ Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """
        (translation, rotation) = TransformHelpers.convert_pose_inverse_transform(self.robot_pose)
        p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=msg.header.stamp,frame_id=self.base_frame))
        self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
        (self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_map.pose)

    def broadcast_last_transform(self):
        """ Make sure that we are always broadcasting the last map to odom transformation.
            This is necessary so things like move_base can work properly. """
        if not(hasattr(self,'translation') and hasattr(self,'rotation')):
            return
        self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)

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

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

        if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,rospy.Time(0))):
            # need to know how to transform the laser to the base frame
            # this will be given by either Gazebo or neato_node
            print 2
            return

        if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,rospy.Time(0))):
            # need to know how to transform between base and odometric frames
            # this will eventually be published by either Gazebo or neato_node
            print 3
            return
        # calculate pose of laser relative ot the robot base
        p = PoseStamped(header=Header(stamp=rospy.Time(0),
                                      frame_id=msg.header.frame_id))
        self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)

        # find out where the robot thinks it is based on its odometry
        p = PoseStamped(header=Header(stamp=rospy.Time(0),
                                      frame_id=self.base_frame),
                                      pose=Pose())
        self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
        # store the the odometry pose in a more convenient format (x,y,theta)
        new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)

        print(self.current_odom_xy_theta) # Essa list não está sendo "refeita" / preenchida
        print(new_odom_xy_theta)

        if not(self.particle_cloud):
            # now that we have all of the necessary transforms we can update the particle cloud
            self.initialize_particle_cloud()
            # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
            self.current_odom_xy_theta = new_odom_xy_theta
            # update our map to odom transform now that the particles are initialized
            self.fix_map_to_odom_transform(msg)
            print(math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]), "hi")

        elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
              math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
              math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
            # we have moved far enough to do an update!

            '''

                É AQUI!!!!

            '''
            print('jorge')
            self.update_particles_with_odom(msg)    # update based on odometry - FAZER
            self.update_particles_with_laser(msg)   # update based on laser scan - FAZER
            self.update_robot_pose()                # update robot's pose
            """ abaixo """
            self.resample_particles()               # resample particles to focus on areas of high density - FAZER
            self.fix_map_to_odom_transform(msg)     # update map to odom transform now that we have new particles
        # publish particles (so things like rviz can see them)
        self.publish_particles(msg)

    def fix_map_to_odom_transform(self, msg):
        """ This method constantly updates the offset of the map and
            odometry coordinate systems based on the latest results from
            the localizer """
        (translation, rotation) = convert_pose_inverse_transform(self.robot_pose)
        p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation),
                        header=Header(stamp=rospy.Time(0),frame_id=self.base_frame))
        self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
        (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)

    def broadcast_last_transform(self):
        """ Make sure that we are always broadcasting the last map
            to odom transformation.  This is necessary so things like
            move_base can work properly. """
        if not(hasattr(self,'translation') and hasattr(self,'rotation')):
            return
        self.tf_broadcaster.sendTransform(self.translation,
                                          self.rotation,
                                          rospy.Time.now(),
                                          self.odom_frame,
                                          self.map_frame)


    def initial_list_builder(self, xy_theta):
        '''
        Creates the initial particles list,
        using the super advanced methods
        provided to us by the one and only
        John
        Cena
        '''
        initial_particles = []

        for i in range(self.n_particles):
            p = Particle()
            p.x = gauss(xy_theta[0], 1)
            p.y = gauss(xy_theta[1], 1)
            p.theta = gauss(xy_theta[2], (math.pi / 2))
            p.w = 1.0 / self.n_particles
            initial_particles.append(p)

        return initial_particles

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

#.........这里部分代码省略.........
	@staticmethod
	def weighted_values(values, probabilities, size):
		""" Return a random sample of size elements form the set values with the specified probabilities
			values: the values to sample from (numpy.ndarray)
			probabilities: the probability of selecting each element in values (numpy.ndarray)
			size: the number of samples
		"""
		bins = np.add.accumulate(probabilities)
		return values[np.digitize(random_sample(size), bins)]

	def update_initial_pose(self, msg):
		""" Callback function to handle re-initializing the particle filter based on a pose estimate.
			These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
		xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(msg.pose.pose)
		self.initialize_particle_cloud(xy_theta)
		self.fix_map_to_odom_transform(msg)

	def initialize_particle_cloud(self, xy_theta=None):
		""" Initialize the particle cloud.
			Arguments
			xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
					  particle cloud around.  If this input is ommitted, the odometry will be used """
		if xy_theta == None:
			xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose)
		self.particle_cloud = []
		# TODO create particles

		self.normalize_particles()
		self.update_robot_pose()

	def normalize_particles(self):
		""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
		# TODO: implement this

	def publish_particles(self, msg):
		particles_conv = []
		for p in self.particle_cloud:
			particles_conv.append(p.as_pose())
		# actually send the message so that we can view it in rviz
		self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),frame_id=self.map_frame),poses=particles_conv))

	def scan_received(self, msg):
		""" This is the default logic for what to do when processing scan data.  Feel free to modify this, however,
			I hope it will provide a good guide.  The input msg is an object of type sensor_msgs/LaserScan """
		if not(self.initialized):
			# wait for initialization to complete
			return

		if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)):
			# need to know how to transform the laser to the base frame
			# this will be given by either Gazebo or neato_node
			return

		if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)):
			# need to know how to transform between base and odometric frames
			# this will eventually be published by either Gazebo or neato_node
			return

		# calculate pose of laser relative ot the robot base
		p = PoseStamped(header=Header(stamp=rospy.Time(0),frame_id=msg.header.frame_id))
		self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)

		# find out where the robot thinks it is based on its odometry
		p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id=self.base_frame), pose=Pose())
		self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
		# store the the odometry pose in a more convenient format (x,y,theta)
		new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose)

		if not(self.particle_cloud):
			# now that we have all of the necessary transforms we can update the particle cloud
			self.initialize_particle_cloud()
			# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
			self.current_odom_xy_theta = new_odom_xy_theta
			# update our map to odom transform now that the particles are initialized
			self.fix_map_to_odom_transform(msg)
		elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
			  math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
			  math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
			# we have moved far enough to do an update!
			self.update_particles_with_odom(msg)	# update based on odometry
			self.update_particles_with_laser(msg)	# update based on laser scan
			self.resample_particles()				# resample particles to focus on areas of high density
			self.update_robot_pose()				# update robot's pose
			self.fix_map_to_odom_transform(msg)		# update map to odom transform now that we have new particles
		# publish particles (so things like rviz can see them)
		self.publish_particles(msg)

	def fix_map_to_odom_transform(self, msg):
		""" Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """
		(translation, rotation) = TransformHelpers.convert_pose_inverse_transform(self.robot_pose)
		p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=msg.header.stamp,frame_id=self.base_frame))
		self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
		(self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_map.pose)

	def broadcast_last_transform(self):
		""" Make sure that we are always broadcasting the last map to odom transformation.
			This is necessary so things like move_base can work properly. """
		if not(hasattr(self,'translation') and hasattr(self,'rotation')):
			return
		self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)

# 需要导入模块: from tf import TransformBroadcaster [as 别名]
# 或者: from tf.TransformBroadcaster import sendTransform [as 别名]
class TransformLearner(object):
    def __init__(self):
        rospy.init_node("learn_transform")

        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()

        self.possible_base_link_poses = []
        self.baselink_averages = []

        self.rate = rospy.Rate(20)

        self.markers = { "/PATTERN_1": Pose( [-0.56297, 0.11, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] ),
                         "/PATTERN_2": Pose( [-0.45097, 0.11, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] ),
                         "/PATTERN_3": Pose( [-0.34097, 0.0, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] ),
                         "/PATTERN_4": Pose( [-0.34097, -0.11, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] ),
                         "/PATTERN_5": Pose( [-0.45097, -0.11, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] ),
                         "/PATTERN_6": Pose( [-0.56297, 0.0, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] )
                        }

        self.run()

    def run(self):
        calibration_complete = False
        while ( not rospy.is_shutdown()) and (not calibration_complete):
            for index, marker in enumerate( self.markers.items() ):
                pose_tmp = self.publish_and_get_pose( marker[0], marker[1], index )
                if pose_tmp != None:
                    self.possible_base_link_poses.append( pose_tmp )

            if len( self.possible_base_link_poses ) != 0:
                best_guess_base_link_to_camera = self.average_poses()
                self.baselink_averages.append( best_guess_base_link_to_camera )
                calibration_complete = self.check_end_of_calibration()

                #print best_guess_base_link_to_camera
                print str(best_guess_base_link_to_camera.position.x) + " " + str(best_guess_base_link_to_camera.position.y) + " " +str(best_guess_base_link_to_camera.position.z) + " " + str(best_guess_base_link_to_camera.orientation.x) + " " + str(best_guess_base_link_to_camera.orientation.y) + " " + str(best_guess_base_link_to_camera.orientation.z) + " " + str(best_guess_base_link_to_camera.orientation.w)
                

                self.tf_broadcaster.sendTransform( (best_guess_base_link_to_camera.position.x, best_guess_base_link_to_camera.position.y,
                                                    best_guess_base_link_to_camera.position.z),
                                                   (best_guess_base_link_to_camera.orientation.x, best_guess_base_link_to_camera.orientation.y,
                                                    best_guess_base_link_to_camera.orientation.z, best_guess_base_link_to_camera.orientation.w),
                                                   rospy.Time.now(),
                                                   "/camera_link",
                                                   "/computed_base_link" )

            self.rate.sleep()

    def average_poses(self):
        """
        Average the list of poses for the different guesses of where the kinect
        is compared to the base_link
        """
        averaged_pose = Pose()
        for pose in self.possible_base_link_poses:
            averaged_pose.position.x += pose.position.x
            averaged_pose.position.y += pose.position.y
            averaged_pose.position.z += pose.position.z

            averaged_pose.orientation.x += pose.orientation.x
            averaged_pose.orientation.y += pose.orientation.y
            averaged_pose.orientation.z += pose.orientation.z
            averaged_pose.orientation.w += pose.orientation.w

        size = len( self.possible_base_link_poses )
        averaged_pose.position.x /= size
        averaged_pose.position.y /= size
        averaged_pose.position.z /= size

        averaged_pose.orientation.x /= size
        averaged_pose.orientation.y /= size
        averaged_pose.orientation.z /= size
        averaged_pose.orientation.w /= size

        return averaged_pose



    def publish_and_get_pose(self, marker_name, transform_marker_to_base_link, index):
        """
        Publishes a transform between the given marker and the base link, then
        listen for the transform from /base_link to /camera_link.

        The transform between the marker and the base link is known.

        """
        trans = None
        rot = None

        #try to get the pose of the given link_name
        # in the base_link frame.
        success = False

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

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

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

	@staticmethod
	def weighted_values(values, probabilities, size):
		bins = np.add.accumulate(probabilities)
		return values[np.digitize(random_sample(size), bins)]

	@staticmethod
	def convert_pose_to_xy_and_theta(pose):
		orientation_tuple = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
		angles = euler_from_quaternion(orientation_tuple)
		return (pose.position.x, pose.position.y, angles[2])

	def initialize_particle_cloud(self):
		self.particle_cloud_initialized = True
		(x,y,theta) = MyAMCL.convert_pose_to_xy_and_theta(self.odom_pose.pose)

		for i in range(self.n_particles):
			self.particle_cloud.append(Particle(x=x+gauss(0,.25),y=y+gauss(0,.25),theta=theta+gauss(0,.25)))
		self.normalize_particles()

	def normalize_particles(self):
		z = 0.0
		for p in self.particle_cloud:
			z += p.w
		for i in range(len(self.particle_cloud)):
			self.particle_cloud[i].w /= z

	def publish_particles(self, msg):
		particles_conv = []
		for p in self.particle_cloud:
			particles_conv.append(p.as_pose())
		# actually send the message so that we can view it in rviz
		self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),frame_id="map"),poses=particles_conv))

	def scan_received(self, msg):
		if not(self.initialized):
			return

		if not(self.tf_listener.canTransform("base_footprint",msg.header.frame_id,msg.header.stamp)):
			return

		if not(self.tf_listener.canTransform("base_footprint","odom",msg.header.stamp)):
			return

		p = PoseStamped(header=Header(stamp=rospy.Time(0),frame_id=msg.header.frame_id))
		self.laser_pose = self.tf_listener.transformPose("base_footprint",p)

		p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id="base_footprint"), pose=Pose())
		#p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id="base_footprint"), pose=Pose(position=Point(x=0.0,y=0.0,z=0.0),orientation=Quaternion(x=0.0,y=0.0,z=0.0,w=0.0)))
		self.odom_pose = self.tf_listener.transformPose("odom", p)
		new_odom_xy_theta = MyAMCL.convert_pose_to_xy_and_theta(self.odom_pose.pose)

		if not(self.particle_cloud_initialized):
			self.initialize_particle_cloud()
			self.update_robot_pose()
			self.current_odom_xy_theta = new_odom_xy_theta
			self.fix_map_to_odom_transform(msg)
		else:
			delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
			if math.fabs(delta[0]) > self.d_thresh or math.fabs(delta[1]) > self.d_thresh or math.fabs(delta[2]) > self.a_thresh:
				self.update_particles_with_odom(msg)
				self.update_robot_pose()
				self.update_particles_with_laser(msg)
				self.resample_particles()
				self.update_robot_pose()
				self.fix_map_to_odom_transform(msg)
			else:
				self.fix_map_to_odom_transform(msg, False)

		self.publish_particles(msg)

	def fix_map_to_odom_transform(self, msg, recompute_odom_to_map=True):
		if recompute_odom_to_map:
			(translation, rotation) = MyAMCL.convert_pose_inverse_transform(self.robot_pose)
			p = PoseStamped(pose=MyAMCL.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=msg.header.stamp,frame_id="base_footprint"))
			self.odom_to_map = self.tf_listener.transformPose("odom", p)
		(translation, rotation) = MyAMCL.convert_pose_inverse_transform(self.odom_to_map.pose)
		self.tf_broadcaster.sendTransform(translation, rotation, msg.header.stamp+rospy.Duration(1.0), "odom", "map")

	@staticmethod
	def convert_translation_rotation_to_pose(translation,rotation):
		return Pose(position=Point(x=translation[0],y=translation[1],z=translation[2]), orientation=Quaternion(x=rotation[0],y=rotation[1],z=rotation[2],w=rotation[3]))

	@staticmethod
	def convert_pose_inverse_transform(pose):
		translation = np.zeros((4,1))
		translation[0] = -pose.position.x
		translation[1] = -pose.position.y
		translation[2] = -pose.position.z
		translation[3] = 1.0

		rotation = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
		euler_angle = euler_from_quaternion(rotation)
		rotation = np.transpose(rotation_matrix(euler_angle[2], [0,0,1]))		# the angle is a yaw
		transformed_translation = rotation.dot(translation)

		translation = (transformed_translation[0], transformed_translation[1], transformed_translation[2])
		rotation = quaternion_from_matrix(rotation)
		return (translation, rotation)

# 需要导入模块: from tf import TransformBroadcaster [as 别名]
# 或者: from tf.TransformBroadcaster import sendTransform [as 别名]
class MarkerProcessor(object):
    def __init__(self, use_dummy_transform=False):
        rospy.init_node('star_center_positioning_node')
        if use_dummy_transform:
            self.odom_frame_name = "odom_dummy"
        else:
            self.odom_frame_name = "odom"

        self.marker_locators = {}
        self.add_marker_locator(MarkerLocator(0,(0.0,0.0),0))
        self.add_marker_locator(MarkerLocator(1,(1.4/1.1,2.0/1.1),0))

        self.marker_sub = rospy.Subscriber("ar_pose_marker",
                                           ARMarkers,
                                           self.process_markers)
        self.odom_sub = rospy.Subscriber("odom", Odometry, self.process_odom, queue_size=10)
        self.star_pose_pub = rospy.Publisher("STAR_pose",PoseStamped,queue_size=10)
        self.continuous_pose = rospy.Publisher("STAR_pose_continuous",PoseStamped,queue_size=10)
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()

    def add_marker_locator(self, marker_locator):
        self.marker_locators[marker_locator.id] = marker_locator

    def process_odom(self, msg):
        p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=self.odom_frame_name),
                        pose=msg.pose.pose)
        try:
            STAR_pose = self.tf_listener.transformPose("STAR", p)
            STAR_pose.header.stamp = msg.header.stamp
            self.continuous_pose.publish(STAR_pose)
        except Exception as inst:
            print "error is", inst

    def process_markers(self, msg):
        for marker in msg.markers:
            # do some filtering basd on prior knowledge
            # we know the approximate z coordinate and that all angles but yaw should be close to zero
            euler_angles = euler_from_quaternion((marker.pose.pose.orientation.x,
                                                  marker.pose.pose.orientation.y,
                                                  marker.pose.pose.orientation.z,
                                                  marker.pose.pose.orientation.w))
            angle_diffs = TransformHelpers.angle_diff(euler_angles[0],pi), TransformHelpers.angle_diff(euler_angles[1],0)
            if (marker.id in self.marker_locators and
                2.4 <= marker.pose.pose.position.z <= 2.6 and
                fabs(angle_diffs[0]) <= .2 and
                fabs(angle_diffs[1]) <= .2):
                locator = self.marker_locators[marker.id]
                xy_yaw = locator.get_camera_position(marker)
                orientation_tuple = quaternion_from_euler(0,0,xy_yaw[2])
                pose = Pose(position=Point(x=xy_yaw[0],y=xy_yaw[1],z=0),
                            orientation=Quaternion(x=orientation_tuple[0], y=orientation_tuple[1], z=orientation_tuple[2], w=orientation_tuple[3]))
                # TODO: use markers timestamp instead of now() (unfortunately, not populated currently by ar_pose)
                pose_stamped = PoseStamped(header=Header(stamp=rospy.Time.now(),frame_id="STAR"),pose=pose)
                try:
                    offset, quaternion = self.tf_listener.lookupTransform("/base_link", "/base_laser_link", rospy.Time(0))
                except Exception as inst:
                    print "Error", inst
                    return
                # TODO: use frame timestamp instead of now()
                pose_stamped_corrected = deepcopy(pose_stamped)
                pose_stamped_corrected.pose.position.x -= offset[0]*cos(xy_yaw[2])
                pose_stamped_corrected.pose.position.y -= offset[0]*sin(xy_yaw[2])
                self.star_pose_pub.publish(pose_stamped_corrected)
                self.fix_STAR_to_odom_transform(pose_stamped_corrected)

    def fix_STAR_to_odom_transform(self, msg):
        """ Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """
        (translation, rotation) = TransformHelpers.convert_pose_inverse_transform(msg.pose)
        p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=rospy.Time(),frame_id="base_link"))
        try:
            self.tf_listener.waitForTransform("odom","base_link",rospy.Time(),rospy.Duration(1.0))
        except Exception as inst:
            print "whoops", inst
            return
        print "got transform"
        self.odom_to_STAR = self.tf_listener.transformPose("odom", p)
        (self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_STAR.pose)

    def broadcast_last_transform(self):
        """ Make sure that we are always broadcasting the last map to odom transformation.
            This is necessary so things like move_base can work properly. """
        if not(hasattr(self,'translation') and hasattr(self,'rotation')):
            return
        self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame_name, "STAR")

    def run(self):
        r = rospy.Rate(10)
        while not rospy.is_shutdown():
            self.broadcast_last_transform()
            r.sleep()

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

#.........这里部分代码省略.........
                rospy.logerr("Exception during transform.")
                return pose
        else:
            rospy.logwarn("One of the frame objects might not exist: " + from_frame + " or " + to_frame)
            return pose

    @staticmethod
    def pose_to_string(pose):
        """For printing a pose to stdout"""
        return (
            "Position: "
            + str(pose.position.x)
            + ", "
            + str(pose.position.y)
            + ", "
            + str(pose.position.z)
            + "\n"
            + "Orientation: "
            + str(pose.orientation.x)
            + ", "
            + str(pose.orientation.y)
            + ", "
            + str(pose.orientation.z)
            + ", "
            + str(pose.orientation.w)
            + "\n"
        )

    def _publish_tf_pose(self, pose, name, parent):
        """ Publishes a TF for object pose"""
        if pose != None:
            pos = (pose.position.x, pose.position.y, pose.position.z)
            rot = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
            self._tf_broadcaster.sendTransform(pos, rot, rospy.Time.now(), name, parent)

    def update_object_pose(self):
        """ Function to externally update an object pose"""
        Response.perform_gaze_action(GazeGoal.LOOK_DOWN)
        while (
            Response.gaze_client.get_state() == GoalStatus.PENDING
            or Response.gaze_client.get_state() == GoalStatus.ACTIVE
        ):
            time.sleep(0.1)

        if Response.gaze_client.get_state() != GoalStatus.SUCCEEDED:
            rospy.logerr("Could not look down to take table snapshot")
            return False

        rospy.loginfo("Looking at table now.")
        goal = UserCommandGoal(UserCommandGoal.RESET, False)
        self._object_action_client.send_goal(goal)
        while (
            self._object_action_client.get_state() == GoalStatus.ACTIVE
            or self._object_action_client.get_state() == GoalStatus.PENDING
        ):
            time.sleep(0.1)
        rospy.loginfo("Object recognition has been reset.")
        rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text())
        self._reset_objects()

        if self._object_action_client.get_state() != GoalStatus.SUCCEEDED:
            rospy.logerr("Could not reset recognition.")
            return False

        # Do segmentation
        goal = UserCommandGoal(UserCommandGoal.SEGMENT, False)

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

#.........这里部分代码省略.........
        if xy_theta == None:
            xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
        self.particle_cloud = []
        self.particle_cloud.append(Particle(xy_theta[0],xy_theta[1],xy_theta[2]))
        # Initialize particle cloud with a decent amount of noise
        for i in range (0,self.number_of_particles):
            self.particle_cloud.append(Particle(xy_theta[0]+np.random.randn()*.5,xy_theta[1]+np.random.randn()*.5,xy_theta[2]+np.random.randn()*.5))
        self.normalize_particles()
        self.update_robot_pose()

    def normalize_particles(self):
        """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
        particle_sum = 0
        # Sum up particle weights to divide by for normalization
        for particle in self.particle_cloud:
            particle_sum += particle.w
        # Make all particle weights add to 1
        for particle in self.particle_cloud:
            particle.w /= particle_sum

    def publish_particles(self, msg):
        particles_conv = []
        for p in self.particle_cloud:
            particles_conv.append(p.as_pose())
        # actually send the message so that we can view it in rviz
        self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),
                                            frame_id=self.map_frame),
                                  poses=particles_conv))

    def scan_received(self, msg):
        """ This is the default logic for what to do when processing scan data.
            Feel free to modify this, however, I hope it will provide a good
            guide.  The input msg is an object of type sensor_msgs/LaserScan """
        if not(self.initialized):
            # wait for initialization to complete
            return

        if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)):
            # need to know how to transform the laser to the base frame
            # this will be given by either Gazebo or neato_node
            return

        if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)):
            # need to know how to transform between base and odometric frames
            # this will eventually be published by either Gazebo or neato_node
            return

        # calculate pose of laser relative ot the robot base
        p = PoseStamped(header=Header(stamp=rospy.Time(0),
                                      frame_id=msg.header.frame_id))
        self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)

        # find out where the robot thinks it is based on its odometry
        p = PoseStamped(header=Header(stamp=msg.header.stamp,
                                      frame_id=self.base_frame),
                        pose=Pose())
        self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
        # store the the odometry pose in a more convenient format (x,y,theta)
        new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)

        if not(self.particle_cloud):
            # now that we have all of the necessary transforms we can update the particle cloud
            self.initialize_particle_cloud()
            # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
            self.current_odom_xy_theta = new_odom_xy_theta
            # update our map to odom transform now that the particles are initialized
            self.fix_map_to_odom_transform(msg)
        elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
              math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
              math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
            # we have moved far enough to do an update!
            self.update_particles_with_odom(msg)    # update based on odometry
            self.update_particles_with_laser(msg)   # update based on laser scan
            self.update_robot_pose()                # update robot's pose
            self.resample_particles()               # resample particles to focus on areas of high density
            self.fix_map_to_odom_transform(msg)     # update map to odom transform now that we have new particles
        # publish particles (so things like rviz can see them)
        self.publish_particles(msg)

    def fix_map_to_odom_transform(self, msg):
        """ This method constantly updates the offset of the map and 
            odometry coordinate systems based on the latest results from
            the localizer """
        (translation, rotation) = convert_pose_inverse_transform(self.robot_pose)
        p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation),
                        header=Header(stamp=msg.header.stamp,frame_id=self.base_frame))
        self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
        (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)

    def broadcast_last_transform(self):
        """ Make sure that we are always broadcasting the last map
            to odom transformation.  This is necessary so things like
            move_base can work properly. """
        if not(hasattr(self,'translation') and hasattr(self,'rotation')):
            return
        self.tf_broadcaster.sendTransform(self.translation,
                                          self.rotation,
                                          rospy.get_rostime(),
                                          self.odom_frame,
                                          self.map_frame)