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 execute(self, userdata):
task = self.missionManager.current_task
if task is not None:
goal = manda_coverage.msg.manda_coverageGoal()
for wp in task['nav_objectives'][
task['current_nav_objective_index']]['children']:
print(wp)
gp = GeoPoint()
gp.latitude = wp['latitude']
gp.longitude = wp['longitude']
goal.area.append(gp)
goal.speed = task['default_speed']
self.task_complete = False
self.survey_area_client.wait_for_server()
self.survey_area_client.send_goal(
goal, self.survey_area_done_callback,
self.survey_area_active_callback,
self.survey_area_feedback_callback)
while True:
ret = self.missionManager.iterate('SurveyArea')
if ret is not None:
if ret == 'cancelled':
self.survey_area_client.cancel_goal()
return ret
if self.task_complete:
return 'done'
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 toMsg(self):
""":returns: corresponding `geographic_msgs/GeoPoint`_ message.
:todo: clamp message longitude to [-180..180]
"""
if not self.valid():
raise ValueError('invalid UTM point: ' + str(self))
utm_proj = pyproj.Proj(proj='utm', zone=self.zone, datum='WGS84')
msg = GeoPoint(altitude=self.altitude)
msg.longitude, msg.latitude = utm_proj(self.easting, self.northing,
inverse=True)
return msg
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)
def __get_flyzone(cls, data, frame):
"""
Deserializes flight boundary data into a FlyZoneArray message.
Args:
data: List of dictionaries.
frame: Frame id for the boundaries.
Returns:
A FlyzoneArray message type which contains an array of FlyZone
messages, which contains a polygon for the boundary, a max
altitude and a min altitude.
"""
# Generate header for all zones.
header = Header()
header.stamp = rospy.get_rostime()
header.frame_id = frame
flyzones = FlyZoneArray()
for zone in data:
flyzone = FlyZone()
flyzone.zone.header = header
flyzone.max_alt = feet_to_meters(zone["altitude_msl_max"])
flyzone.min_alt = feet_to_meters(zone["altitude_msl_min"])
# Change boundary points to ros message of type polygon.
for waypoint in zone["boundary_pts"]:
point = GeoPoint()
point.latitude = waypoint["latitude"]
point.longitude = waypoint["longitude"]
flyzone.zone.polygon.points.append(point)
flyzones.flyzones.append(flyzone)
return flyzones
def costmap_callback(data):
try:
transformation = tf_buffer.lookup_transform('map',
data.header.frame_id,
data.header.stamp,
rospy.Duration(1.0))
except Exception as e:
rospy.logwarn(str(e))
else:
origin = PoseStamped()
origin.pose = data.info.origin
origin_map = tf2_geometry_msgs.do_transform_pose(
origin, transformation)
origin_ll = toLL(origin_map.pose.position.x,
origin_map.pose.position.y,
origin_map.pose.position.z)
height_meters = data.info.resolution * data.info.height
width_meters = data.info.resolution * data.info.width
opposite_ll = toLL(origin_map.pose.position.x + height_meters,
origin_map.pose.position.y + width_meters,
origin_map.pose.position.z)
vizItem = GeoVizItem()
vizItem.id = 'occupency_grid'
plist = GeoVizPointList()
plist.color.r = 0.0
plist.color.g = 0.5
plist.color.b = 1.0
plist.color.a = 1.0
corners = [origin_ll, opposite_ll]
for i in ((0, 0), (0, 1), (1, 1), (1, 0), (0, 0)):
gp = GeoPoint()
gp.latitude = corners[i[0]].latitude
gp.longitude = corners[i[1]].longitude
plist.points.append(gp)
vizItem.lines.append(plist)
dlat = (opposite_ll.latitude - origin_ll.latitude) / float(
data.info.width)
dlong = (opposite_ll.longitude - origin_ll.longitude) / float(
data.info.height)
for row in range(data.info.height):
for col in range(data.info.width):
if data.data[row * data.info.width + col] > 0:
p = GeoVizPolygon()
p.fill_color.r = 0.1
p.fill_color.g = 0.1
p.fill_color.b = 0.1
p.fill_color.a = 0.7
for i in ((0, 0), (0, 1), (1, 1), (1, 0), (0, 0)):
gp = GeoPoint()
gp.latitude = origin_ll.latitude + dlat * (row + i[0])
gp.longitude = origin_ll.longitude + dlong * (col +
i[1])
p.outer.points.append(gp)
vizItem.polygons.append(p)
display_publisher.publish(vizItem)
#!/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