Python TransformListener.canTransform方法代码示例

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
def main():
    
    rospy.init_node('tf_test')
    rospy.loginfo("Node for testing actionlib server")
 
    #from_link = '/head_color_camera_l_link'
    #to_link = '/base_link'
    
    from_link = '/base_link'
    to_link = '/map'
    
    tfl = TransformListener()
    
    rospy.sleep(5)
    
    
    t = rospy.Time(0)
    
    mpose = PoseStamped()
    
    mpose.pose.position.x = 1
    mpose.pose.position.y = 0
    mpose.pose.position.z = 0
    
    mpose.pose.orientation.x = 0
    mpose.pose.orientation.y = 0
    mpose.pose.orientation.z = 0
    mpose.pose.orientation.w = 0
    
    mpose.header.frame_id = from_link
    mpose.header.stamp = rospy.Time.now()
    
    rospy.sleep(5)
    
    mpose_transf = None
    
    rospy.loginfo('Waiting for transform for some time...')
    
    tfl.waitForTransform(to_link,from_link,t,rospy.Duration(5))
    
    if tfl.canTransform(to_link,from_link,t):
      
      mpose_transf = tfl.transformPose(to_link,mpose)
      
      print mpose_transf
      
    else:
      
      rospy.logerr('Transformation is not possible!')
      sys.exit(0)

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
class Demo:
    def __init__(self, goals):
        rospy.init_node("demo", anonymous=True)
        self.frame = rospy.get_param("~frame")
        self.pubGoal = rospy.Publisher("goal", PoseStamped, queue_size=1)
        self.listener = TransformListener()
        self.goals = goals
        self.goalIndex = 0

    def run(self):
        self.listener.waitForTransform("/world", self.frame, rospy.Time(), rospy.Duration(5.0))
        goal = PoseStamped()
        goal.header.seq = 0
        goal.header.frame_id = "world"
        while not rospy.is_shutdown():
            goal.header.seq += 1
            goal.header.stamp = rospy.Time.now()
            goal.pose.position.x = self.goals[self.goalIndex][0]
            goal.pose.position.y = self.goals[self.goalIndex][1]
            goal.pose.position.z = self.goals[self.goalIndex][2]
            quaternion = tf.transformations.quaternion_from_euler(0, 0, self.goals[self.goalIndex][3])
            goal.pose.orientation.x = quaternion[0]
            goal.pose.orientation.y = quaternion[1]
            goal.pose.orientation.z = quaternion[2]
            goal.pose.orientation.w = quaternion[3]

            self.pubGoal.publish(goal)

            t = self.listener.getLatestCommonTime("/world", self.frame)
            if self.listener.canTransform("/world", self.frame, t):
                position, quaternion = self.listener.lookupTransform("/world", self.frame, t)
                rpy = tf.transformations.euler_from_quaternion(quaternion)
                if (
                    math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.3
                    and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.3
                    and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.3
                    and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(10)
                    and self.goalIndex < len(self.goals) - 1
                ):
                    rospy.sleep(self.goals[self.goalIndex][4])
                    self.goalIndex += 1

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

#.........这里部分代码省略.........
        # TODO create particles
        self.particle_cloud = self.create_initial_particle_list(xy_theta)

        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) """
        w_sum = sum([p.w for p in self.particle_cloud])

        for particle in self.particle_cloud:
            particle.w /= w_sum
        # TODO: implement this

    def publish_particles(self, msg):

        print('publish_particles')
        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 """

        print('scan_received')
        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=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)

        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=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

        print('broadcast')
        self.tf_broadcaster.sendTransform(self.translation,
                                      self.rotation,
                                      rospy.Time.now(),
                                      self.odom_frame,
                                      self.map_frame)

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
class Collector():
    def __init__(self):
        rospy.init_node('collector', anonymous = False)                
        self.lis=TransformListener();
        self.data_out=SBC_Output();
        rospy.Subscriber("/joint_states", JointState, self.j_callback)
        rospy.Subscriber("/finger1/ContactState", KCL_ContactStateStamped, self.f_callback1)
        rospy.Subscriber("/finger2/ContactState", KCL_ContactStateStamped, self.f_callback2)
        rospy.Subscriber("/pressure", PressureControl, self.p_callback)        
        rospy.Subscriber("/prob_fail", Float64, self.prob_callback)
        self.publisher=rospy.Publisher("sbc_data", SBC_Output)
        self.point1=PointStamped()
        self.point2=PointStamped()
        self.rate=rospy.Rate(20);

    def getParams(self):     
        self.data_out.D_Gain=rospy.get_param("/bhand_pid/d_gain")     
        self.data_out.F_ref_pid=rospy.get_param("/bhand_pid/f_ref")
        self.data_out.I_Gain=rospy.get_param("/bhand_pid/i_gain")
        self.data_out.P_Gain=rospy.get_param("/bhand_pid/p_gain")
        self.data_out.freq=rospy.get_param("/pressure_reg/frequency")
        self.data_out.Beta=rospy.get_param("/bhand_sbc/beta")
        self.data_out.Delta=rospy.get_param("/bhand_sbc/delta")
        self.data_out.Eta=rospy.get_param("/bhand_sbc/eta")
        self.data_out.F_ref_sbc=rospy.get_param("/bhand_sbc/f_ref")
        
    def j_callback(self,data):
        self.joints=data;
        self.data_out.effort1=data.effort[1]
        self.data_out.effort2=data.effort[0]
        
    def f_callback1(self,data):                    
        self.data_out.Fn1=data.Fnormal;            
        ft=np.array([data.tangential_force.x,data.tangential_force.y,data.tangential_force.z])
        self.data_out.Ft1=np.sqrt(ft.dot(ft));

        self.point1=PointStamped();
        self.point1.header=data.header;
        self.point1.point=data.contact_position;
                    
        
    def f_callback2(self,data):
        self.data_out.Fn2=data.Fnormal;
        ft=np.array([data.tangential_force.x,data.tangential_force.y,data.tangential_force.z])
        self.data_out.Ft2=np.sqrt(ft.dot(ft));  
        
        self.point2=PointStamped();
        self.point2.header=data.header;
        self.point2.point=data.contact_position;

        
    def p_callback(self,data):
        self.data_out.p_demand=data.p_demand;
        self.data_out.p_measure=data.p_measure;
    def prob_callback(self,data):
        self.data_out.Pfailure=data.data;
        
    def transform_it(self,data):
        if(data.header.frame_id):
            #data.header.stamp=rospy.Time.now();
            if(self.lis.canTransform("base_link",data.header.frame_id,data.header.stamp) or True):
                #print(rospy.Time.now())     
                data.header.stamp=data.header.stamp-rospy.Duration(0.02)           
                #point=self.lis.transformPoint("base_link", data)
                try:    
                    #self.lis.waitForTransform("base_link", data.header.frame_id, data.header.stamp, rospy.Duration(1))
                  #  print(rospy.Time.now())                
                    self.point=self.lis.transformPoint("base_link", data)
                    return True
                except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                    rospy.logwarn("TF problem 2")                    
                    pass
            else:
                rospy.logwarn("Cannot Transform")
        else:
            print(data.header.frame_id)        
        return False
    
    def get_distance(self,point1,point2):  
        d=np.array([point1.x-point2.x, point1.y-point2.y, point1.z-point2.z])
        return np.sqrt(d.dot(d));
  
        
        
    def send_it(self):
        while not rospy.is_shutdown():            
            self.data_out.header.stamp=rospy.Time.now();
            self.getParams()            
            
            got_it=self.transform_it(self.point1);
            if(got_it):
                self.data_out.contact1=self.point.point
            got_it=self.transform_it(self.point2);
            if(got_it):
                self.data_out.contact2=self.point.point                   
            self.data_out.distance=self.get_distance(self.data_out.contact1,self.data_out.contact2)*100
            self.publisher.publish(self.data_out);
            self.rate.sleep();           

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [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 TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]

#.........这里部分代码省略.........
			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 = []
		for i in range(self.n_particles):
			self.particle_cloud.append(Particle(x=xy_theta[0]+gauss(0,.25),y=xy_theta[1]+gauss(0,.25),theta=xy_theta[2]+gauss(0,.25)))
		self.normalize_particles()
		self.update_robot_pose()

	""" Level 1 """
	def normalize_particles(self):
		""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
		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=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 TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]

#.........这里部分代码省略.........
            y = random_sample()* map_info.height * map_info.resolution * 0.1 
            if random_sample() > 0.5:
                y = -y
            theta = random_sample() * math.pi*2
            self.particle_cloud.append(Particle(x, y, theta))

        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) """
        sum = 0
        for particle in self.particle_cloud:
            sum += particle.w
        for particle in self.particle_cloud:
            particle.w /= sum

    def publish_particles(self, pub):
        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
        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, 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
            rospy.logwarn("can't transform to laser scan")
            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
            rospy.logwarn("can't transform to base frame")
            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))
        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

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

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


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

        #Soma total das probabilidades das particulas
        w_sum = sum([p.w for p in self.particle_cloud])

        #Dividir cada probabilidade de uma particula pela soma total
        for particle in self.particle_cloud:
            particle.w /= w_sum
        # TODO: implement this

    def publish_particles(self, msg):
        print("Publicar Particulas.")
        print(len(self.particle_cloud))
        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):
            print("Not Initialized")
            # wait for initialization to complete
            return

        if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,rospy.Time(0))):
            print("Not 2")
            # 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,rospy.Time(0))):
            print("Not 3")
            # 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 = 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("PASSOU")
        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

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

#.........这里部分代码省略.........
            xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
        self.particle_cloud = []

        for i in range(self.n_particles):
            self.particle_cloud.append(Particle(x=xy_theta[0]+gauss(0,0.25),y=xy_theta[1]+gauss(0,0.25),theta=xy_theta[2]+gauss(0,0.25)))        
        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) """
        w = [deepcopy(p.w) for p in self.particle_cloud]
        z = sum(w)
        print(z)
        if z > 0:
            for i in range(len(self.particle_cloud)):
                self.particle_cloud[i].w = w[i] / z
        else:
            for i in range(len(self.particle_cloud)):
                self.particle_cloud[i].w = 1/len(self.particle_cloud)
        print(sum([p.w for p in self.particle_cloud]))

    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)

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

#.........这里部分代码省略.........
            #Button 5
            if delta[4] == 1:
                
		self.square_go()
		#self.targetX = square[0][0]
                #self.targetY = square[0][1]
		#self.targetZ = square[0][2]
                #self.des_angle = square[0][3]
                #print(self.targetZ)
                #self.power += 100.0
                #print(self.power)
                self.state = Controller.Automatic
            #Button 6
            if delta[5] == 1:
		
		self.square_start = False
                self.targetX = 0.0
                self.targetY = 0.0
		self.targetZ = 0.5
                self.des_angle = 0.0
                #print(self.targetZ)
                #self.power -= 100.0
                #print(self.power)
                self.state = Controller.Automatic
        self.lastJoy = data


    def run(self):
        thrust = 0
        print("jello")
        while not rospy.is_shutdown():
            if self.state == Controller.TakeOff:
                t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark_Gon4")
		print(t,self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t))
                if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t):
                    position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4", t)
                    print(position[0],position[1],position[2])			
                    #if position[2] > 2.0 or thrust > 54000:
		    if thrust > 55000:
                        self.pidReset()
                        self.pidZ.integral = thrust / self.pidZ.ki
                        #self.targetZ = 0.5
                        self.state = Controller.Automatic
                        thrust = 0
                    else:
                        thrust += 500
                        #self.power = thrust
                        msg = self.cmd_vel_telop
                        msg.linear.z = thrust
                        self.pubNav.publish(msg)

	    if self.state == Controller.Land:
                t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark_Gon4")
                if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t):
                    position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4", t)
		    if position[2] > 0.05:
			msg_land = self.cmd_vel_telop
			self.power -=100
			msg_land.linear.z = self.power
			self.pubNav.publish(msg_land)
		    else:
			msg_land = self.cmd_vel_telop
			msg_land.linear.z = 0
			self.pubNav.publish(msg_land)

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

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

    def normalize_particles(self):
        """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
        tot_weight = sum([particle.w for particle in self.particle_cloud]) or 1
        for particle in self.particle_cloud:
            particle.w = particle.w/tot_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))

        marker_array = []
        for index, particle in enumerate(self.particle_cloud):
            marker = Marker(header=Header(stamp=rospy.Time.now(),
                                          frame_id=self.map_frame),
                                  pose=particle.as_pose(),
                                  type=0,
                                  scale=Vector3(x=particle.w*2,y=particle.w*1,z=particle.w*5),
                                  id=index,
                                  color=ColorRGBA(r=1,a=1))
            marker_array.append(marker)

        self.marker_pub.publish(MarkerArray(markers=marker_array))

    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)

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
class Controller:
    Idle = 0
    Automatic = 1
    TakingOff = 2
    Landing = 3

    def __init__(self, frame):
        self.frame = frame
        self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1)
        self.listener = TransformListener()
        self.pidX = PID(35, 10, 0.0, -20, 20, "x")
        self.pidY = PID(-35, -10, -0.0, -20, 20, "y")
        self.pidZ = PID(4000, 3000.0, 2000.0, 10000, 60000, "z")
        self.pidYaw = PID(-50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
        self.state = Controller.Idle
        self.goal = Pose()
        rospy.Subscriber("goal", Pose, self._poseChanged)
        rospy.Service("takeoff", std_srvs.srv.Empty, self._takeoff)
        rospy.Service("land", std_srvs.srv.Empty, self._land)

    def getTransform(self, source_frame, target_frame):
        now = rospy.Time.now()
        success = False
        if self.listener.canTransform(source_frame, target_frame, rospy.Time(0)):
            t = self.listener.getLatestCommonTime(source_frame, target_frame)
            if self.listener.canTransform(source_frame, target_frame, t):
                position, quaternion = self.listener.lookupTransform(source_frame, target_frame, t)
                success = True
            delta = (now - t).to_sec() * 1000 #ms
            if delta > 25:
                rospy.logwarn("Latency: %f ms.", delta)
            #     self.listener.clear()
            #     rospy.sleep(0.02)
        if success:
            return position, quaternion, t

    def pidReset(self):
        self.pidX.reset()
        self.pidZ.reset()
        self.pidZ.reset()
        self.pidYaw.reset()

    def _poseChanged(self, data):
        self.goal = data

    def _takeoff(self, req):
        rospy.loginfo("Takeoff requested!")
        self.state = Controller.TakingOff
        return std_srvs.srv.EmptyResponse()

    def _land(self, req):
        rospy.loginfo("Landing requested!")
        self.state = Controller.Landing
        return std_srvs.srv.EmptyResponse()

    def run(self):
        thrust = 0
        while not rospy.is_shutdown():
            now = rospy.Time.now()
            if self.state == Controller.TakingOff:
                r = self.getTransform("/world", self.frame)
                if r:
                    position, quaternion, t = r

                    if position[2] > 0.05 or thrust > 50000:
                        self.pidReset()
                        self.pidZ.integral = thrust / self.pidZ.ki
                        self.targetZ = 0.5
                        self.state = Controller.Automatic
                        thrust = 0
                    else:
                        thrust += 100
                        msg = Twist()
                        msg.linear.z = thrust
                        self.pubNav.publish(msg)
                else:
                    rospy.logerr("Could not transform from /world to %s.", self.frame)

            if self.state == Controller.Landing:
                self.goal.position.z = 0.05
                r = self.getTransform("/world", self.frame)
                if r:
                    position, quaternion, t = r
                    if position[2] <= 0.1:
                        self.state = Controller.Idle
                        msg = Twist()
                        self.pubNav.publish(msg)
                else:
                    rospy.logerr("Could not transform from /world to %s.", self.frame)

            if self.state == Controller.Automatic or self.state == Controller.Landing:
                # transform target world coordinates into local coordinates
                r = self.getTransform("/world", self.frame)
                if r:
                    position, quaternion, t = r
                    targetWorld = PoseStamped()
                    targetWorld.header.stamp = t
                    targetWorld.header.frame_id = "world"
                    targetWorld.pose = self.goal

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

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

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

        for i in range(self.n_particles):
            self.particle_cloud.append(Particle(randn(), randn(),randn()))     #add robot location guess to each number

        self.normalize_particles()
        self.update_robot_pose()
        #print self.particle_cloud

    def normalize_particles(self):
        """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
        pass
        # TODO: implement this
        #add up all weights of all particles, divide each particle by this number 
        sumWeight = 0
        for particle in self.particle_cloud:
          sumWeight += particle.w
        
        for particle in self.particle_cloud:
          particle.w /=sumWeight

    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)

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [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)

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [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)