ROS 订阅雷达/scan信息和定位信息的数据融合进行目标跟随

雷达信息scan的数据类型如下（通过rosmsg show 查看）：
代码中用到的min(range)代表离障碍物的最小距离，而且代码含有简单的过滤功能，请仔细研读！！！

Header header            # Header也是一个结构体,包含了seq,stamp,frame_id,其中seq指的是扫描顺序增加的id,stamp包含了开始扫描的时间和与开始扫描的时间差,frame_id是扫描的参考系名称.注意扫描是逆时针从正前方开始扫描的.

float32 angle_min        # 开始扫描的角度(角度)
float32 angle_max        # 结束扫描的角度(角度)
float32 angle_increment  # 每一次扫描增加的角度(角度)

float32 time_increment   # 测量的时间间隔(s)
float32 scan_time        # 扫描的时间间隔(s)

float32 range_min        # 距离最小值(m)
float32 range_max        # 距离最大值(m)

float32[] ranges         # 距离数组(长度360)
float32[] intensities    # 与设备有关,强度数组(长度360)

上面用到ros机器人的雷达定位功能，在雷达导航中也是经常雷达等避障功能，避障功能就是在这原理上面的优化。但本质相同。由于研究的是声源定位的方法，因此在声源提取方面采用的传统music算法提取方位角信息。
其数据类型为：

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
int32 count
int32 exist_src_num
hark_msgs/HarkSourceVal[] src
  int32 id
  float32 power
  float32 x
  float32 y
  float32 z
  float32 azimuth
  float32 elevation

下面代码展示为实际机器人简单的实现声源追踪和定位的一种数据融合的方法，包括tf转换的内容，关于tf坐标转换的东西会在以后给大家补充。

import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from transform_utils import quat_to_angle, normalize_angle
from math import radians, copysign, sqrt, pow, pi
from hark_msgs.msg import HarkSource
class OutAndBack():
    def __init__(self):
        # Set rospy to execute a shutdown function when exiting       
        rospy.on_shutdown(self.shutdown)

	self.LIDAR_ERR = 0.05
        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

        # How fast will we update the robot's movement?
        rate = 20

        # Set the equivalent ROS rate variable
        r = rospy.Rate(rate)

        # Set the forward linear speed to 0.2 meters per second 
        self.linear_speed = 0.2

        # Set the rotation speed in radians per second
        self.angular_speed = 1.0
                # Set the angular tolerance in degrees converted to radians
        self.angular_tolerance = radians(1.5)

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()

        # Give tf some time to fill its buffer
        rospy.sleep(2)

        # Set the odom frame
        self.odom_frame = '/odom'

        # Find out if the robot uses /base_link or /base_footprint
        try:
            self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
            self.base_frame = '/base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
                self.base_frame = '/base_link'
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
                rospy.signal_shutdown("tf Exception")  
    def callback(self, azi):

        y =  azi.src
        for i in y:
                self.target =i.azimuth*pi/180
		if self.target > 0:
                    self.target = self.target
                else:
                     self.target += 2*pi
                
		# Initialize the position variable as a Point type
		position = Point()
		
		    # Initialize the movement command
		move_cmd = Twist()
		    # Get the starting position values     
		(position, rotation) = self.get_odom()

		    # Stop the robot before the rotation
		move_cmd = Twist()
		self.cmd_vel.publish(move_cmd)
		rospy.sleep(1)

		    # Set the movement command to a rotation
		#move_cmd.angular.z = self.angular_speed

		    # Track the last angle measured
		last_angle = rotation

		    # Track how far we have turned
		turn_angle = 0
		while abs(turn_angle + self.angular_tolerance) < abs(self.target) and not rospy.is_shutdown():
			   move_cmd.angular.z = self.angular_speed
			# Publish the Twist message and sleep 1 cycle         
			   self.cmd_vel.publish(move_cmd)
			   r.sleep()
			# Get the current rotation
			   (position, rotation) = self.get_odom()
			# Compute the amount of rotation since the last loop
			   delta_angle = normalize_angle(rotation - last_angle)
			# Add to the running total
			   turn_angle += delta_angle
			   last_angle = rotation
		    # Stop the robot before the next leg
		move_cmd = Twist()
		self.cmd_vel.publish(move_cmd)
		rospy.sleep(1)
		self.get_scan()
		move_cmd.linear.x = self.linear_speed
		#while distance < self.scan_filter and not rospy.is_shutdown():
		while min(self.scan_filter) > 1.0 and not rospy.is_shutdown():
                        self.get_scan()
                        rospy.loginfo('distance of the obstacle : %f', min(self.scan_filter))
			self.cmd_vel.publish(move_cmd)
                        r.sleep()
	        self.cmd_vel.publish(Twist())

    def shutdown(self):
        # Always stop the robot when shutting down the node.
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)
if __name__ == '__main__':
       rospy.init_node('out_and_back', anonymous=False)
       outandback =  OutAndBack()
       sub = rospy.Subscriber('/HarkSource', HarkSource,outandback.callback,queue_size=1)
       r = rospy.Rate(10)
       rospy.spin()

完成跟随功能。。
以后会更新particle_filter进行跟随！