def loadPaths(self):
lat_long_points = np.genfromtxt(self.path_file_name, delimiter = ',')
lat_long_safety_points = np.genfromtxt(self.safety_path_file_name, delimiter = ',')
rospy.wait_for_service('fromLL')
transformation_method = rospy.ServiceProxy('fromLL', FromLL)
try:
for gps_point in lat_long_points:
lat_long = GeoPoint()
lat_long.latitude = gps_point[0]
lat_long.longitude = gps_point[1]
lat_long.altitude = 0.0
response = transformation_method(lat_long)
self.path.append([response.map_point.x,response.map_point.y])
self.control_status_publisher.publish('Path loaded.')
self.path=np.array(self.path)
for gps_point in lat_long_safety_points:
lat_long = GeoPoint()
lat_long.latitude = gps_point[0]
lat_long.longitude = gps_point[1]
lat_long.altitude = 0.0
response = transformation_method(lat_long)
self.safety_path.append([response.map_point.x,response.map_point.y])
self.control_status_publisher.publish('Safety path loaded.')
self.safety_path=np.array(self.path)
except rospy.ServiceException as e:
print("Service call failed: %s"%e)
def get_marker(self, ns, id, fix, r, g, b):
pt = GeoPoint()
pt.latitude = fix[0]
pt.longitude = fix[1]
pt.altitude = fix[2]
resp = self.from_ll(pt)
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
marker.ns = ns
marker.id = id
marker.type = marker.SPHERE
marker.pose.position.x = resp.map_point.x
marker.pose.position.y = resp.map_point.y
marker.pose.position.z = resp.map_point.z
marker.pose.orientation.w = 1.0
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 0.5
marker.color.r = r
marker.color.g = g
marker.color.b = b
marker.color.a = 1.0
marker.frame_locked = True
return marker
def __get_search_grid(cls, data, frame):
"""
Deserializes a the search grid into a polygon message.
Args:
data: List of dictionaries corresponding to the search grid points.
frame: Frame for the polygon.
Returns:
Message of type PolygonStamped with the bounds of the search grid.
"""
header = Header()
header.stamp = rospy.get_rostime()
header.frame_id = frame
search_grid = GeoPolygonStamped()
search_grid.header = header
for point in data:
boundary_pnt = GeoPoint()
altitude = feet_to_meters(point["altitude_msl"])
boundary_pnt.latitude = point["latitude"]
boundary_pnt.longitude = point["longitude"]
boundary_pnt.altitude = altitude
search_grid.polygon.points.append(boundary_pnt)
return search_grid
def __get_waypoints(cls, data, frame):
"""
Deserializes a list of waypoints into a marker message.
Args:
data: List of dictionaries corresponding to waypoints.
frame: Frame of the markers.
Returns:
A marker message of type Points, with a list of points in order
corresponding to each waypoint.
"""
header = Header()
header.stamp = rospy.get_rostime()
header.frame_id = frame
waypoints = WayPoints()
waypoints.header = header
# Ensure there is no rotation by setting w to 1.
for point in data:
waypoint = GeoPoint()
altitude = feet_to_meters(point["altitude_msl"])
waypoint.latitude = point["latitude"]
waypoint.longitude = point["longitude"]
waypoint.altitude = altitude
waypoints.waypoints.append(waypoint)
return waypoints
def loadPath(self):
lat_long_points = np.genfromtxt(self.pathFileName, delimiter = ',')
rospy.wait_for_service('fromLL')
transformation_method = rospy.ServiceProxy('fromLL', FromLL)
try:
for gps_point in lat_long_points:
lat_long = GeoPoint()
lat_long.latitude = gps_point[0]
lat_long.longitude = gps_point[1]
lat_long.altitude = 0.0
response = transformation_method(lat_long) # maybe we need to pass a special message
self.path.append([response.map_point.x,response.map_point.y])
self.control_status_publisher.publish('Path loaded')
except rospy.ServiceException as e:
print("Service call failed: %s"%e)
#Callback function implementing the pose value received
def callback(self, data):
self.robot_pos = data
self.robot_pos.pose.pose.position.x = round(self.robot_pos.pose.pose.position.x, 4)
self.robot_pos.pose.pose.position.y = round(self.robot_pos.pose.pose.position.y, 4)
def run(self):
p1 = 0
p2 = 1
while p2 < len(self.path):
robot_pos = [self.robot_pos.pose.pose.position.x, self.robot_pos.pose.pose.position.y, self.robot_pos.pose.pose.orientation.x]
start_point = self.path[p1]
end_point = self.path[p2]
v,omega = purePursuitController(start_point, end_point, self.velocity, robot_pos,self.lookahead)
# v,omega = self.pidController(end_point, self.velocity, robot_pos)
vel_msg = Twist()
#linear velocity in the x-axis:
vel_msg.linear.x = self.velocity
vel_msg.linear.y, vel_msg.linear.z = 0,0
#angular velocity in the z-axis:
vel_msg.angular.z = omega
vel_msg.angular.x, vel_msg.angular.y = 0,0
#Publishing our vel_msg
self.velocity_publisher.publish(vel_msg)
self.control_status_publisher.publish('On path.')
self.rate.sleep()
# If at the goal point, change the points fed to the controller.
if np.linalg.norm(robot_pos[:2] - self.path[p2]) < 0.1:
p1 += 1
p2 += 1
# When finished, stop the robot.
vel_msg.linear.x = 0
vel_msg.angular.z = 0
self.velocity_publisher.publish(vel_msg)
self.control_status_publisher.publish('Finished at goal point.')
rospy.spin()
def __init__(self):
global geoVizItem
geoPoint = GeoPoint()
geoPoint.longitude = -70.75598
geoPoint.latitude = 43.08110
geoPoint.altitude = 1000
pointList = GeoVizPointList()
pointList.points = [geoPoint]
pointList.color = ColorRGBA(0, 1, 0, 1)
pointList.size = 100
geoVizItem.point_groups = [pointList]
geoVizItem.id = "THOMAS TEST"
def latlon_to_utm(lat, lon, z, latlontoutm_service_name):
rospy.loginfo("Waiting for latlontoutm service "+str(latlontoutm_service_name))
rospy.wait_for_service(latlontoutm_service_name)
rospy.loginfo("Got latlontoutm service")
try:
latlontoutm_service = rospy.ServiceProxy(latlontoutm_service_name,
LatLonToUTM)
gp = GeoPoint()
gp.latitude = np.degrees(lat)
gp.longitude = np.degrees(lon)
gp.altitude = z
res = latlontoutm_service(gp)
return (res.utm_point.x, res.utm_point.y)
except rospy.service.ServiceException:
rospy.logerr_throttle_identical(5, "LatLon to UTM service failed! namespace:{}".format(latlontoutm_service_name))
return (None, None)
#!/usr/bin/env python
import rospy
from dji_sdk_demo.srv import *
from geographic_msgs.msg import GeoPoint
def run_mission_srv(wp_list):
rospy.wait_for_service('/inspector/run_mission')
try:
run_mission = rospy.ServiceProxy('/inspector/run_mission', RunMission)
res = run_mission(wp_list)
return res.ack
except rospy.ServiceException as e:
print("Service call failed: %s"%e)
if __name__ == "__main__":
wp_list = []
n_wp = raw_input('Num wp?')
for i in range(int(n_wp)):
print 'WP '+ str(i+1)
wp = GeoPoint()
wp.latitude = float(raw_input('Lat: '))
wp.longitude = float(raw_input('Lon: '))
wp.altitude = float(raw_input('Alt: '))
wp_list.append(wp)
print wp_list
print "ACK: " + str(run_mission_srv(wp_list))
def geoToUtmToCartesian(self, new_coordinate_geo):
# Conversion from geographic coordinates to UTM.
new_coordinate_UTM = geodesy.utm.fromMsg(new_coordinate_geo)
# new_coordinate_geo_second = new_coordinate_UTM.toMsg()
# print(new_coordinate_geo_second)
# Conversion from geographic coordinates to UTM.
new_coordinate_cartesian = Point32(
) # Cartesian coordinate that this function will return.
# The problem with UTM is that if there are coordinates in more than one zone, it's difficult to merge the coordinates of the different zones.
# This is because each zone has 6 degrees of longitude, and the width in meters of the zone is variable from equator to the poles.
# Something similar when the coordinates are in different hemispheres, separated by the equator. Each hemisphere has a different origin for the y axis.
# These are the reasons why the x and y assignation for the Cartesian conversion isn't a simple UTM substraction.
# Assignating "new_coordinate_cartesian.x" is tricky when the actual coordinate and the origin of the Cartesian coordinates are on different zones.
if int(
new_coordinate_UTM.zone
) == 60 and self.origin_UTM.zone == 1: # Coordinate and origin separated by the +- 180 degrees longitude
geo_180_w = GeoPoint()
geo_180_e = GeoPoint()
geo_180_w.longitude = 179.9999999
geo_180_w.latitude = new_coordinate_geo.latitude
geo_180_w.altitude = new_coordinate_geo.altitude
geo_180_e.longitude = -179.9999999
geo_180_e.latitude = new_coordinate_geo.latitude
geo_180_e.altitude = new_coordinate_geo.altitude
utm_180_w = UTMPoint((geo_180_w))
utm_180_e = UTMPoint((geo_180_e))
new_coordinate_cartesian.x = new_coordinate_UTM.easting - utm_180_w.easting + utm_180_e.easting - self.origin_UTM.easting # Transformation of the x coordinate taking into account the different zones. " - utm_180_w.easting + utm_180_e.easting " computes the x step in both sides of the border longitude.
elif self.origin_UTM.zone == 60 and int(
new_coordinate_UTM.zone
) == 1: # Coordinate and origin separated by the +-180 degrees longitude
geo_180_w = GeoPoint()
geo_180_e = GeoPoint()
geo_180_w.longitude = 179.9999999
geo_180_w.latitude = new_coordinate_geo.latitude
geo_180_w.altitude = new_coordinate_geo.altitude
geo_180_e.longitude = -179.9999999
geo_180_e.latitude = new_coordinate_geo.latitude
geo_180_e.altitude = new_coordinate_geo.altitude
utm_180_w = UTMPoint((geo_180_w))
utm_180_e = UTMPoint((geo_180_e))
new_coordinate_cartesian.x = new_coordinate_UTM.easting - utm_180_e.easting + utm_180_w.easting - self.origin_UTM.easting # Transformation of the x coordinate taking into account the different zones. " - utm_180_w.easting + utm_180_e.easting " computes the x step in both sides of the border longitude.
elif int(new_coordinate_UTM.zone) < self.origin_UTM.zone:
quotient_from_int_division = int(
np.max(np.abs(new_coordinate_geo.longitude),
self.origin_geo.longitude)) / 6
border_longitude = quotient_from_int_division * 6.0 * (
-1)**np.sigbit(
np.max(new_coordinate_geo.longitude, origin_geo.longitude))
geo_w = GeoPoint()
geo_e = GeoPoint()
geo_w.longitude = border_longitude - 0.0000001
geo_w.latitude = new_coordinate_geo.latitude
geo_w.altitude = new_coordinate_geo.altitude
geo_e.longitude = border_longitude + 0.0000001
geo_e.latitude = new_coordinate_geo.latitude
geo_e.altitude = new_coordinate_geo.altitude
utm_w = UTMPoint((geo_w))
utm_e = UTMPoint((geo_e))
new_coordinate_cartesian.x = new_coordinate_UTM.easting - utm_w.easting + utm_e.easting - self.origin_UTM.easting # Transformation of the x coordinate taking into account the different zones. " - utm_w.easting + utm_e.easting " computes the x step in both sides of the border longitude.
elif self.origin_UTM.zone < int(new_coordinate_UTM.zone):
quotient_from_int_division = int(
np.max(np.abs(new_coordinate_geo.longitude),
self.origin_geo.longitude)) / 6
border_longitude = quotient_from_int_division * 6.0 * (
-1)**np.sigbit(
np.max(new_coordinate_geo.longitude,
self.origin_geo.longitude))
geo_w = GeoPoint()
geo_e = GeoPoint()
geo_w.longitude = border_longitude - 0.0000001
geo_w.latitude = new_coordinate_geo.latitude
geo_w.altitude = new_coordinate_geo.altitude
geo_e.longitude = border_longitude + 0.0000001
geo_e.latitude = new_coordinate_geo.latitude
geo_e.altitude = new_coordinate_geo.altitude
utm_w = UTMPoint((geo_w))
utm_e = UTMPoint((geo_e))
new_coordinate_cartesian.x = new_coordinate_UTM.easting - utm_e.easting + utm_w.easting - self.origin_UTM.easting # Transformation of the x coordinate taking into account the different zones. " - utm_w.easting + utm_e.easting " computes the x step in both sides of the border longitude.
else:
# The actual coordinate is in the same zone that the origin (the first station). This is the normal situation, the assigntation of the x axis value is trivial (simple subtraction).
new_coordinate_cartesian.x = new_coordinate_UTM.easting - self.origin_UTM.easting
# Assignating "new_coordinate_cartesian.y" is also tricky when the actual coordinate and the origin of the Cartesian coordinates are on different hemispheres:
if self.origin_UTM.band == 'N' and new_coordinate_UTM.band == 'M': # Coordinate and origin separated by the 0 degrees latitude
geo_0_n = GeoPoint()
geo_0_s = GeoPoint()
geo_0_n.longitude = new_coordinate_geo.longitude
geo_0_n.latitude = 0.0000001
geo_0_n.altitude = new_coordinate_geo.altitude
geo_0_s.longitude = new_coordinate_geo.longitude
geo_0_s.latitude = -0.0000001
geo_0_s.altitude = new_coordinate_geo.altitude
utm_0_n = UTMPoint((geo_0_n))
utm_0_s = UTMPoint((geo_0_s))
new_coordinate_cartesian.y = new_coordinate_UTM.northing - utm_0_s.northing + utm_0_n.northing - self.origin_UTM.northing # Transformation of the y coordinate taking into account the different hemispheres. " - utm_0_s.northing + utm_0_n.northing " computes the y step in both sides of the equator.
elif new_coordinate_UTM.band == 'N' and self.origin_UTM.band == 'M': # Coordinate and origin separated by the 0 degrees latitude
geo_0_n = GeoPoint()
geo_0_s = GeoPoint()
geo_0_n.longitude = new_coordinate_geo.longitude
geo_0_n.latitude = 0.0000001
geo_0_n.altitude = new_coordinate_geo.altitude
geo_0_s.longitude = new_coordinate_geo.longitude
geo_0_s.latitude = -0.0000001
geo_0_s.altitude = new_coordinate_geo.altitude
utm_0_n = UTMPoint((geo_0_n))
utm_0_s = UTMPoint((geo_0_s))
new_coordinate_cartesian.y = new_coordinate_UTM.northing - utm_0_n.northing + utm_0_s.northing - self.origin_UTM.northing # Transformation of the y coordinate taking into account the different hemispheres. " - utm_0_n.northing + utm_0_s.northing " computes the y step in both sides of the equator.
else:
# The actual coordinate is in the same hemisphere that the origin (the first station). This is the normal situation, the assigntation of the y axis value is trivial (simple subtraction).
new_coordinate_cartesian.y = new_coordinate_UTM.northing - self.origin_UTM.northing
# Assignating "new_coordinate_cartesian.z" is trivial always, simple subtraction.
new_coordinate_cartesian.z = new_coordinate_UTM.altitude - self.origin_UTM.altitude
return new_coordinate_cartesian #, new_coordinate_UTM
def track_waypoints():
# Parse arguments
parser = argparse.ArgumentParser(
description='Track waypoints defined in a yaml file')
parser.add_argument(
'-plan_package',
type=str,
default='handy_tools',
help='Name of the package where plan to track is stored')
parser.add_argument('-plan_file',
type=str,
default='wp_default.yaml',
help='Name of the file inside plan_package/plans')
parser.add_argument('-wait_for',
type=str,
default='path',
help='Wait for human response: [none]/[path]/[wp]')
args, unknown = parser.parse_known_args()
# utils.check_unknown_args(unknown)
rospy.init_node('waypoint_tracker')
file_name = args.plan_file
# Autocomplete file extension
if not file_name.endswith('.yaml'):
file_name = file_name + '.yaml'
file_url = rospkg.RosPack().get_path(
args.plan_package) + '/plans/' + file_name
with open(file_url, 'r') as wp_file:
wp_data = yaml.load(wp_file)
if 'frame_id' not in wp_data:
rospy.logerr(
"Must specify frame_id in waypoints file") # TODO: default to ''?
return
wp_list = []
for wp_id in range(1000):
# for wp_id in range(int(n_wp)):
if 'wp_' + str(wp_id) in wp_data:
print 'WP ' + str(wp_id + 1)
wp_raw = wp_data['wp_' + str(wp_id)]
wp = GeoPoint()
wp.latitude = wp_raw[0]
wp.longitude = wp_raw[1]
wp.altitude = wp_raw[2]
print wp
wp_list.append(wp)
rospy.wait_for_service('/inspector/run_mission')
try:
run_mission = rospy.ServiceProxy('/inspector/run_mission', RunMission)
# TODO: Check we're flying!
print "Ready to track " + str(
len(wp_list)) + " waypoints from " + file_url
res = run_mission(wp_list)
return res.ack
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
def tuple_to_geo_point(cls, point_tuple: Tuple) -> GeoPoint:
geo_point = GeoPoint()
geo_point.longitude = point_tuple[0]
geo_point.latitude = point_tuple[1]
geo_point.altitude = point_tuple[2]
return geo_point
def point_to_geo_point(cls, point: Point) -> GeoPoint:
geo_point = GeoPoint()
geo_point.longitude = point.x
geo_point.latitude = point.y
geo_point.altitude = point.z
return geo_point