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_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 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 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 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 fromLatLong(lat, lon, alt=float('nan')):
"""Generate WayPoint from latitude, longitude and (optional) altitude.
:returns: minimal WayPoint object just for test cases.
"""
geo_pt = GeoPoint(latitude=lat, longitude=lon, altitude=alt)
return WayPoint(position=geo_pt)
def _parse_plan(self):
# TODO this parser only accepts a set of goto commands
"""A plan has been requested by neptus to start. Parse the plan here and send to controller."""
geopoints = []
speeds = []
for maneuver in self._current_plan.plan_spec.maneuvers:
geopoints.append(
GeoPoint(maneuver.maneuver.lat * 180.0 / np.pi,
maneuver.maneuver.lon * 180.0 / np.pi,
maneuver.maneuver.z))
speeds.append(maneuver.maneuver.speed)
if self._datum is not None:
geopoints.append(self._datum)
req = ConvertGeoPointsRequest()
req.geopoints = geopoints
points = self._geo_converter.call(req).utmpoints
wps = []
# If datum is available, then reference frame is ENU World
if self._datum is not None:
for point, speed in zip(points[:-1], speeds):
wp = Waypoint()
wp.point = Point(point.x - points[-1].x,
point.y - points[-1].y,
-point.z - points[-1].z)
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = "world"
wp.radius_of_acceptance = 3.0
wp.max_forward_speed = speed
wp.use_fixed_heading = False
wp.heading_offset = 0.0
wps.append(wp)
# If datum is not given, then reference frame is ENU UTM TODO: LOOKUP TRANSFORM
else:
for point, speed in zip(points, speeds):
wp = Waypoint()
wp.point.x = point.x
wp.point.y = point.y
wp.point.z = -point.z
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = "utm"
wp.radius_of_acceptance = 3.0
wp.max_forward_speed = speed
wp.use_fixed_heading = False
wp.heading_offset = 0.0
wps.append(wp)
self.STATE = PlanControlState.READY
req = InitWaypointSetRequest()
req.start_time = Time(rospy.Time.from_sec(0))
req.start_now = True
req.waypoints = wps
req.max_forward_speed = max(speeds)
req.interpolator.data = "lipb"
req.heading_offset = 0
self._waypoint_setter.wait_for_service()
if self._waypoint_setter(Time(rospy.Time.from_sec(0.0)), True, wps,
max(speeds), 0.0, String("lipb")):
self.STATE = PlanControlState.EXECUTING
else:
self.STATE = PlanControlState.FAILURE
def parse_request(mis, utm_frame_id="utm", radius_of_acceptance=5.0):
with open(mis, "r") as f:
while not f.readline().startswith("START"):
pass
geopoints = []
headings = []
speeds = []
line = f.readline().rstrip()
while not line.startswith("END"):
values = line.split(";")
values = [value.strip() for value in values]
params = parse_iver_format(values[5])
zvar = -float(params[0][1:]) * 0.3048 if params[0][
0] == 'D' else float(params[0][1:]) * 0.3048
geopoints.append(GeoPoint(float(values[1]), float(values[2]),
zvar))
headings.append(float(values[4]))
speeds.append(float(params[-1][1:]) * 0.5144)
line = f.readline().rstrip()
# while not f.readline().startswith("START VEHICLE"):
# pass
# line = f.readline().rstrip()
# while not line.startswith("END VEHICLE"):
# if line.startswith("UVC=WPRadius"):
# roas.append(float(line.split("=")[-1]))
# break
req = ConvertGeoPointsRequest()
req.geopoints = geopoints
p = rospy.ServiceProxy("convert_points", ConvertGeoPoints)
p.wait_for_service()
res = p(req)
wps = []
for point, heading, speed in zip(res.utmpoints, headings, speeds):
wp = Waypoint()
wp.point.x = point.x
wp.point.y = point.y
wp.point.z = point.z
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = utm_frame_id
wp.radius_of_acceptance = radius_of_acceptance
wp.max_forward_speed = speed
wp.use_fixed_heading = False
wp.heading_offset = 0.0
wps.append(wp)
req = InitWaypointSetRequest()
req.start_time = Time(rospy.Time.from_sec(0))
req.start_now = True
req.waypoints = wps
req.max_forward_speed = max(speeds)
req.interpolator = String("lipb")
req.heading_offset = 0
req.max_forward_speed = max(speeds)
req.waypoints = wps
return req
def Global2Local(self, polygons_geo):
polygons_local = {}
origin_geo = polygons_geo["Origin"][0]
new_coordinate_geo = GeoPoint(*origin_geo)
for key in self.polygons_geo.keys():
vertex_local_list = []
for new_geo in polygons_geo[key]:
new_geo = GeoPoint(*new_geo)
geo_local_pose = GeoLocalPose(new_geo)
local_pose = geo_local_pose.geoToUtmToCartesian(
new_coordinate_geo)
vertex_local_list.append(
[-local_pose.x, -local_pose.y, local_pose.z])
polygons_local[key] = vertex_local_list
return polygons_local
def test_three_point_set(self):
# test data
uuids = [
'da7c242f-2efe-5175-9961-49cc621b80b9',
'812f1c08-a34b-5a21-92b9-18b2b0cf4950',
'6f0606f6-a776-5940-b5ea-5e889b32c712'
]
latitudes = [30.3840168, 30.3857290, 30.3866750]
longitudes = [-97.7282100, -97.7316754, -97.7270967]
eastings = [622191.124, 621856.023, 622294.785]
northings = [3362024.764, 3362210.790, 3362320.569]
points = []
for i in range(len(uuids)):
latlon = GeoPoint(latitude=latitudes[i], longitude=longitudes[i])
points.append(WayPoint(id=UniqueID(uuid=uuids[i]),
position=latlon))
# test iterator
wupts = WuPointSet(points)
i = 0
for w in wupts:
self.assertEqual(w.uuid(), uuids[i])
self.assertEqual(wupts[uuids[i]].uuid(), uuids[i])
self.assertAlmostEqual(w.utm.easting, eastings[i], places=3)
self.assertAlmostEqual(w.utm.northing, northings[i], places=3)
point_xy = w.toPointXY()
self.assertAlmostEqual(point_xy.x, eastings[i], places=3)
self.assertAlmostEqual(point_xy.y, northings[i], places=3)
self.assertAlmostEqual(point_xy.z, 0.0, places=3)
i += 1
self.assertEqual(i, 3)
self.assertEqual(len(wupts), 3)
bogus = '00000000-c433-5c42-be2e-fbd97ddff9ac'
self.assertFalse(bogus in wupts)
self.assertEqual(wupts.get(bogus), None)
uu = uuids[1]
self.assertTrue(uu in wupts)
wpt = wupts[uu]
self.assertEqual(wpt.uuid(), uu)
self.assertNotEqual(wupts.get(uu), None)
self.assertEqual(wupts.get(uu).uuid(), uu)
# test index() function
for i in xrange(len(uuids)):
self.assertEqual(wupts.index(uuids[i]), i)
self.assertEqual(wupts.points[i].id.uuid, uuids[i])
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 test_real_point(self):
ll = GeoPoint(latitude=30.385315, longitude=-97.728524, altitude=209.0)
msg = WayPoint(position=ll)
pt = WuPoint(msg)
self.assertEqual(pt.toWayPoint(), msg)
self.assertEqual(str(pt.utm),
'UTM: [622159.338, 3362168.303, 209.000, 14R]')
point_xyz = pt.toPoint()
self.assertAlmostEqual(point_xyz.x, 622159.338, places=3)
self.assertAlmostEqual(point_xyz.y, 3362168.303, places=3)
self.assertAlmostEqual(point_xyz.z, 209.0, places=3)
point_xy = pt.toPointXY()
self.assertAlmostEqual(point_xy.x, 622159.338, places=3)
self.assertAlmostEqual(point_xy.y, 3362168.303, places=3)
self.assertAlmostEqual(point_xy.z, 0.0, places=3)
def __init__(self):
self.STATE = PlanControlState.NONE
self._plan_db = dict()
self._current_plan = PlanDB()
datum = rospy.get_param("~datum", [])
datum = map(
float,
datum.strip('][').split(',')) if type(datum) is str else datum
self._datum = None if len(datum) == 0 else GeoPoint(*map(float, datum))
rospy.loginfo("Datum Reference: " + str(self._datum))
self._estimated_state_pub = rospy.Publisher("imc/estimated_state",
EstimatedState,
queue_size=10)
self._estimated_state_msg = EstimatedState()
self._plan_control_state_pub = rospy.Publisher(
"imc/plan_control_state", PlanControlState, queue_size=10)
self._plan_control_state_msg = PlanControlState()
self._plan_control_state_msg.state = self.STATE
self._plan_db_pub = rospy.Publisher("imc/plan_db",
PlanDB,
queue_size=10)
self._geo_converter = rospy.ServiceProxy("convert_points",
ConvertGeoPoints)
self._waypoint_setter = rospy.ServiceProxy("start_waypoint_list",
InitWaypointSet)
self._waypoint_forwarder = rospy.Service("load_waypoints",
InitWaypointSet,
self._send_waypoints)
self._hold_position = rospy.ServiceProxy("hold_vehicle", Hold)
self._tf_buffer = Buffer()
TransformListener(self._tf_buffer)
rospy.Subscriber("gps", NavSatFix, self._handle_gps)
rospy.Subscriber("pose_gt", Odometry, self._handle_pose)
rospy.Subscriber("imc/goto_waypoint", Pose, self._handle_goto)
rospy.Subscriber("imc/plan_control", PlanControl,
self._handle_plan_control)
rospy.Subscriber("imc/abort", Empty, self._handle_abort)
rospy.Subscriber("imc/imc_heartbeat", Empty,
self._handle_imc_heartbeat)
rospy.Subscriber("imc/plan_db", PlanDB, self._handle_plan_db)
rospy.Timer(rospy.Duration(1, 0), self._send_estimated_state)
rospy.Timer(rospy.Duration(1, 0), self._send_plan_control_state)
def list_to_msg(type, array, add=None, override=None):
array = copy(array)
if add is not None:
assert len(array) == len(add)
array += add
if override is not None:
assert len(array) == len(override)
for i in range(len(array)):
if not np.isnan(override[i]):
array[i] = override[i]
if type == 'Pose':
msg = Pose()
msg.position.x = array[0]
msg.position.y = array[1]
msg.position.z = array[2]
if len(array) == 4:
msg.orientation = Quaternion(
*quaternion_from_euler(0, 0, radians(array[3])))
else:
msg.orientation.w = 1
elif type == 'GeoPoint':
msg = GeoPoint()
msg.latitude = array[0]
msg.longitude = array[1]
msg.altitude = array[2]
elif type == 'GeoPose':
msg = GeoPose()
msg.position.latitude = array[0]
msg.position.longitude = array[1]
msg.position.altitude = array[2]
if len(array) == 4:
msg.orientation = Quaternion(
*quaternion_from_euler(0, 0, radians(array[3])))
else:
msg.orientation.w = 1
else:
raise ValueError('Unknown type: {}'.format(type))
return msg
def publish_data(self):
# information about the variables https://mavlink.io/en/messages/common.html#HIL_GPS
# default paremeter the EKF take https://docs.px4.io/master/en/advanced_config/parameter_reference.html#EKF2_REQ_EPH
# more info https://docs.px4.io/master/en/advanced_config/tuning_the_ecl_ekf.html
#self.h.stamp = rospy.Time.now()
#lat1 = self.gps_out_lat
#lon1 = self.gps_out_lon
#alt1 = self.gps_out_alt
lat1 = self.lat_inc
lon1 = self.lon_inc
alt1 = self.lat_inc
geo1 = GeoPoint(latitude=lat1, longitude=lon1, altitude=alt1)
self.pub_spoofing_signal.publish(geo1)
self.count += 1
print("data PUB ", self.count, "runs= ", self.runs, "dt= ", self.dt)
print('lat_off= ', lat1)
print('lon_off= ', lon1)
def init_uav(self, arm=False, takeoff=False, verbose=False):
if verbose:
print("Starting init_uav(). Atm we have to manualy accept every new command, but just remove the `input()` from the code to do this faster.")
res_setMode = self.setmode_srv(0,'GUIDED')
if verbose:
print("Res of setmode:", res_setMode)
res_set_ref_mode = self.set_ref_frame_srv(1) # FRAME_BODY_NED = 8 works for velocities
if verbose:
print("Res of set_ref_frame", res_set_ref_mode)
rospy.sleep(1)
print("Waiting for ekf initialisation")
origin = GeoPointStamped()
origin.header = Header()
origin.header.stamp = rospy.Time.now()
origin.position = GeoPoint()
origin.position.latitude = self.OPERATION_POSITION[0]
origin.position.longitude = self.OPERATION_POSITION[1]
origin.position.altitude = self.OPERATION_POSITION[2]
self.set_origin_pub.publish(origin)
if verbose:
print("Published ",origin)
while(True):
res = self.arm()
if res.success:
break
rospy.sleep(1)
self.disarm()
# input("Ekf initialized?")
#time.sleep(40) # For some reason, it does not work if we use rospy.sleep(...)
if arm or takeoff:
self.arm()
if takeoff:
self.takeoff()
print("Uav is now initialized. We are done with waiting.")
def _parse_plan(self):
# TODO this parser only accepts a set of goto commands
"""A plan has been requested by neptus to start. Parse the plan here and send to controller."""
geopoints = []
speeds = []
is_depths = []
for maneuver in self._current_plan.plan_spec.maneuvers:
geopoints.append(
GeoPoint(maneuver.maneuver.lat * 180.0 / np.pi,
maneuver.maneuver.lon * 180.0 / np.pi,
maneuver.maneuver.z))
speeds.append(maneuver.maneuver.speed)
is_depths.append(maneuver.maneuver.z_units == 1)
req = ConvertGeoPointsRequest()
req.geopoints = geopoints
points = self._geo_converter.call(req).utmpoints
wps = []
# If datum is available, then reference frame is ENU World
for point, speed, is_depth in zip(points, speeds, is_depths):
wp = Waypoint()
wp.point.x = point.x
wp.point.y = point.y
wp.point.z = -abs(point.z) if is_depth else abs(point.z)
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = "utm"
wp.radius_of_acceptance = 1.0
wp.max_forward_speed = speed
wp.use_fixed_heading = False
wp.heading_offset = 0.0
wps.append(wp)
req = InitWaypointSetRequest()
req.start_time = Time(rospy.Time.from_sec(0))
req.start_now = True
req.waypoints = wps
req.max_forward_speed = max(speeds)
req.interpolator.data = "lipb"
req.heading_offset = 0
self._send_goal(req)
def iterate(self,data):
#calculate fov for current position
self.calculateFOV()
#update VizItem
self.fovVizItem.polygons[0] = self.geoPathToGeoVizPoly(self.fovPath)
#query enc
layers = ['BCNCAR', 'BCNISD', 'BCNLAT', 'BCNSAW', 'BCNSPP', 'BOYCAR', 'BOYINB', 'BOYISD', 'BOYLAT', 'BOYSAW', 'BOYSPP', 'CGUSTA', 'CTRPNT', 'CURENT', 'DAYMAR', 'DISMAR', 'FOGSIG', 'LIGHTS', 'LITFLT', 'LITVES', 'PILPNT', 'RADRFL', 'RADSTA', 'RDOSTA', 'RETRFL', 'RSCSTA', 'RTPBCN', 'SISTAT', 'SISTAW', 'SOUNDG', 'SPRING', 'TOPMAR', 'UWTROC']
t= time.time()
enc_ret = self.enc_query(layers,self.fovPath.poses)
t = time.time() - t
self.timelog[0] += t
self.timelog[1] += 1
rospy.loginfo("FOVQUERY TIME:" + str(t))
rospy.loginfo("TIMELOG:" + str(self.timelog))
#draw points in view
inViewList = GeoVizPointList()
inViewList.color = ColorRGBA(0,1,0,1)
inViewList.size = 20
self.featureViz = GeoVizItem()
self.featureViz.point_groups = [None]
# rospy.loginfo(enc_ret.featuresInView)
for enc_feat in enc_ret.featuresInView:
long = enc_feat.longitude
lat=enc_feat.latitude
# rospy.loginfo(str(long) +" "+ str(lat))
feat = GeoPoint(lat,long,0)
inViewList.points.append(feat)
self.featureViz.point_groups[0] = inViewList
# #pubish updates
geoVizItem_pub.publish(self.fovVizItem)
geoVizItem_pub.publish(self.featureViz)
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)
#!/usr/bin/env python
import rospy
from tf2_ros import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
from geographic_msgs.msg import GeoPoint
from geodesy import utm
if __name__=="__main__":
rospy.init_node("utm_to_world")
datum = map(float, rospy.get_param("~datum", []).strip('][').split(','))
datum = GeoPoint(*map(float, datum))
utmpoint = utm.fromMsg(datum)
broadcaster = StaticTransformBroadcaster()
tf = TransformStamped()
tf.header.stamp = rospy.Time.now()
tf.header.frame_id = "utm"
tf.child_frame_id = "world"
tf.transform.translation.x = utmpoint.easting
tf.transform.translation.y = utmpoint.northing
tf.transform.translation.z = utmpoint.altitude
tf.transform.rotation.w = 1.0
broadcaster.sendTransform(tf)
rospy.spin()
def makeWayPoint(id, lat, lon):
w = WayPoint()
w.id = UniqueID(id)
w.position = GeoPoint(latitude=lat, longitude=lon)
return w
# [,0.0],
# [,0.0],
# [,0.0],
# [,0.0],
# [,0.0],
# [,0.0],
# [,0.0],
# [,0.0],
# [,0.0]
# ]
polygons_local = {}
origin_geo = polygons_geo["Origin"][0]
new_coordinate_geo = GeoPoint(*origin_geo)
limits = [[99999,-99999],[99999,-99999]]
for key in polygons_geo.keys():
vertex_local_list = []
for new_geo in polygons_geo[key]:
new_geo = GeoPoint(*new_geo)
geo_local_pose = GeoLocalPose(new_geo)
local_pose = geo_local_pose.geoToUtmToCartesian(new_coordinate_geo)
vertex_local_list.append([local_pose.x,local_pose.y,local_pose.z])
if local_pose.x < limits[0][0]:
limits[0][0] = local_pose.x
if local_pose.x > limits[0][1]:
limits[0][1] = local_pose.x
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
