ROS 发送多gps目标点导航节点编写

writer：lIcht                      email：lIcht.gzl@gmail.com                 date:  2020.6.2

        因为项目需要，对官方gps_goal这一功能包进行修改，实现对多个目标点的连续导航。订阅的主题1: /fix 提供实时的gps经纬度信息 消息格式为：geometry_msgs::NavSatFix ；订阅节点2: /position 提供多个gps目标点序列 消息格式为：std_msgs::String。同时加入状态机，显示目前的导航状态，返回每个点导航的结果。并在导航过程中或者导航完成后追加目标点序列。

源码分享:

xxxxxxxxxx
#!/usr/bin/python
#-*-coding: UTF-8 -*-
import rospy
import math
import actionlib
import tf
import threading
​
from geographiclib.geodesic import Geodesic
from actionlib_msgs.msg import GoalStatus
from geometry_msgs.msg import PoseStamped
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from sensor_msgs.msg import NavSatFix
from tank_msgs.msg import GpsPoints
from std_msgs.msg import String
from smach import State,StateMachine,Concurrence
​
​
#创建GPS点列表(全局)
gps_list = []
current_id = 0
goal_count = 0
​
​
def DMS_to_decimal_format(lat, long):
  # 转化度分秒格式转化为十进制
  if ',' in lat:
    degrees, minutes, seconds = lat.split(',')
    degrees, minutes, seconds = float(degrees), float(minutes), float(seconds)
    if lat[0] == '-': # 负号情况
      minutes = -minutes
      seconds = -seconds
    lat = degrees + minutes/60 + seconds/3600
  if ',' in long:
    degrees, minutes, seconds = long.split(',')
    degrees, minutes, seconds = float(degrees), float(minutes), float(seconds)
    if long[0] == '-': # 负号情况
      minutes = -minutes
      seconds = -seconds
    long = degrees + minutes/60 + seconds/3600
​
  lat = float(lat)
  long = float(long)
  rospy.loginfo('Given GPS goal: lat %s, long %s.' % (lat, long))
  return lat, long
​
​
​
def get_origin_lat_long():
  # Get the lat long coordinates of our map frame's origin which must be publshed on topic /local_xy_origin. We use this to calculate our goal within the map frame.
  rospy.loginfo("Waiting for a message to initialize the origin GPS location...")
  # origin_pose = rospy.wait_for_message('local_xy_origin', PoseStamped)
  # if origin_pose is None:
  rospy.loginfo("Waiting for fix message to initialize the origin GPS location...")
  origin_pose = rospy.wait_for_message('/fix', NavSatFix)
  origin_lat = origin_pose.latitude
  origin_long = origin_pose.longitude
  # else:
  #  origin_lat = origin_pose.pose.position.y
  #  origin_long = origin_pose.pose.position.x
  rospy.loginfo('Received origin: lat %s, long %s.' % (origin_lat, origin_long))
  return origin_lat, origin_long
​
​
​
def calc_goal(origin_lat, origin_long, goal_lat, goal_long):
  # Calculate distance and azimuth between GPS points
  geod = Geodesic.WGS84  # define the WGS84 ellipsoid
  g = geod.Inverse(origin_lat, origin_long, goal_lat, goal_long) # Compute several geodesic calculations between two GPS points 
  hypotenuse = distance = g['s12'] # access distance
  rospy.loginfo("The distance from the origin to the goal is {:.3f} m.".format(distance))
  azimuth = g['azi1']
  rospy.loginfo("The azimuth from the origin to the goal is {:.3f} degrees.".format(azimuth))
​
  # Convert polar (distance and azimuth) to x,y translation in meters (needed for ROS) by finding side lenghs of a right-angle triangle
  # Convert azimuth to radians
  azimuth = math.radians(azimuth)
  x = adjacent = math.cos(azimuth) * hypotenuse
  y = opposite = math.sin(azimuth) * hypotenuse
  rospy.loginfo("The translation from the origin to the goal is (x,y) {:.3f}, {:.3f} m.".format(x, y))
​
  return x, y
​
​
​
class GpsGoal():
  def __init__(self):
    rospy.init_node('multi_gps_goal')
    self.point_id = 0
    self.waypoints = []
    self.last_data = ""
​
    rospy.Subscriber('position', String, self.gps_points_callback) # 监听gps目标点
​
    # Get the lat long coordinates of our map frame's origin which must be publshed on topic /local_xy_origin. We use this to calculate our goal within the map frame.
    self.origin_lat, self.origin_long = get_origin_lat_long()
​
​
  def point_to_goal(self, id, goal_lat, goal_long, ori_lat, ori_long, z=0, yaw=0, roll=0, pitch=0):
    # Calculate goal x and y in the frame_id given the frame's origin GPS and a goal GPS location
    x, y = calc_goal(ori_lat, ori_long, goal_lat, goal_long)
    # Create move_base goal
    quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
    return [str(id), [x, y], [quaternion[0], quaternion[1], quaternion[2], quaternion[3]]]
    # self.creat_goal(x=x, y=y, z=z, yaw=yaw, roll=roll, pitch=pitch)
​
​
  def string_to_list(self, data):
    res = []
    list1 = data.split(";")
    for i in list1:
      if i:
        rospy.loginfo('%s' % i)
        list2 = i.split(",")
        # rospy.loginfo('add %d point lat: %s, lon: %s' % (self.point_id,list2[0],list2[1]))
        temp_list = (self.point_id, float(list2[0]), float(list2[1]))
        res.append(tuple(temp_list))
        self.point_id += 1
        rospy.loginfo('add point %d lat:%s, lon:%s' % (temp_list[0],temp_list[1],temp_list[2]))
    self.point_id = 0
    return res
​
​
  def gps_points_callback(self, data):
    global gps_list
    if data.data == self.last_data:
      return
    self.last_data = data.data
    gps_list = self.string_to_list(data.data)
​
    if gps_list:
      rospy.loginfo("get %d points !" % len(gps_list))
      waypoints = self.get_waypoint()
      # statemachine(waypoints)
      add_thread = threading.Thread(target = statemachine, args=(waypoints,))
      add_thread.start()
      gps_list=[]
      self.waypoints = []
      return
    return
​
​
  def get_waypoint(self):
    if not gps_list:
      rospy.loginfo("no point in the point_list")
      return
    temp_goal = []
    # 由获得的经纬度目标点序列转化为姿态序列
    for i in range(0,len(gps_list)):
      if i == 0:
        temp_goal = self.point_to_goal(gps_list[i][0], gps_list[i][1], gps_list[i][2], self.origin_lat, self.origin_long)
        self.waypoints.append(tuple(temp_goal))
        rospy.loginfo("add the goal %d: lat=%f, lon=%f" % (gps_list[i][0], gps_list[i][1], gps_list[i][2]))
      else:
        temp_goal = self.point_to_goal(gps_list[i][0], gps_list[i][1], gps_list[i][2], gps_list[i-1][1], gps_list[i-1][2])
        self.waypoints.append(tuple(temp_goal))
        rospy.loginfo("add the goal %d: lat=%f, lon=%f" % (gps_list[i][0], gps_list[i][1], gps_list[i][2]))
    self.point_id = 0
    return self.waypoints
​
​
# 状态机---------------------------------------------------------
​
def statemachine(waypoints):
  global current_id
  rospy.loginfo('start with state %d ' % current_id)
  goal_count = len(waypoints)
  rospy.loginfo('waypoint count: %d' % goal_count)
  if waypoints:
    patrol = StateMachine(outcomes=['finished','failed'])
    with patrol:
      for i,w in enumerate(waypoints):
        if i+current_id < len(waypoints)+current_id-1:
          next_status = str(i+current_id+1)
        elif i+current_id == len(waypoints)+current_id-1:
          next_status = 'finished'
        StateMachine.add(str(i+current_id),
                         Waypoint(w[1], w[2]),
                         transitions={'success':next_status,
                                      'fail':'failed'})
    patrol.execute()
    waypoints = []
    print('done!!!!!!!!!!')
    return
​
​
class Waypoint(State):
  def __init__(self, position, orientation):
    State.__init__(self, outcomes=['success','fail'])
​
    # Get an action client
    self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
    self.client.wait_for_server()
​
    # Define the goal
    self.goal = MoveBaseGoal()
    self.goal.target_pose.header.frame_id = 'odom'
    self.goal.target_pose.pose.position.x = position[0]
    self.goal.target_pose.pose.position.y = position[1]
    self.goal.target_pose.pose.position.z = 0.0
    self.goal.target_pose.pose.orientation.x = orientation[0]
    self.goal.target_pose.pose.orientation.y = orientation[1]
    self.goal.target_pose.pose.orientation.z = orientation[2]
    self.goal.target_pose.pose.orientation.w = orientation[3]
​
​
  def execute(self, userdata):
    global gps_list
    global current_id
    global goal_count
    self.client.send_goal(self.goal)
    current_id += 1
​
    rospy.loginfo('Inital goal status: %s' % GoalStatus.to_string(self.client.get_state()))
    status = self.client.get_goal_status_text()
    if status:
      rospy.loginfo(status)
​
    self.client.wait_for_result()
    res = GoalStatus.to_string(self.client.get_state())
    rospy.loginfo('Final goal status: %s' % res)
    status = self.client.get_goal_status_text()
    if status:
      rospy.loginfo(status)
​
    if res == 'SUCCEEDED':
      return 'success'
    else:
      return 'fail'
​
​
def ros_main():
  # while True:
  gpsgoal = GpsGoal()
  rospy.loginfo("Waiting for goal points")
  rospy.wait_for_message('/position', String)
  rospy.sleep(2)
  rospy.loginfo('*********************************************')
  rospy.spin()
​
​
if __name__ == '__main__':
  ros_main()
​