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