def update_geo_path(self):
        if (self.waypoints and self.zone and self.band):
            waypoints_geo = []
            for point in self.waypoints:
                x, y = point
                geo = UTMPoint(x, y, zone=self.zone, band=self.band).toMsg()

                wp = WayPoint()
                wp.id = unique_id.toMsg(unique_id.fromRandom())
                wp.position = geo
                waypoints_geo.append(wp)

            segments_geo = []
            last_wp = waypoints_geo[0].id
            for wp in waypoints_geo:
                seg = RouteSegment()
                seg.id = unique_id.toMsg(unique_id.fromRandom())
                seg.start = last_wp
                seg.end = wp.id
                last_wp = wp.id
                segments_geo.append(seg)

            self.route_network.points = waypoints_geo
            self.route_network.segments = segments_geo

        else:
            rospy.logwarn('[update_geo_path] waypoints, zone, and/or band\n' +
                          '\thas not been set/updated.\n\tIgnoring...')
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)
Exemple #3
0
 def test_waypoint_match(self):
     p = WayPoint()
     put(p, 'another', 'anything')
     put(p, 'name', 'myself')
     prop = match(p, set(['name']))
     self.assertNotEqual(prop, None)
     k, v = prop
     self.assertEqual(k, 'name')
     self.assertEqual(v, 'myself')
Exemple #4
0
def main():
    global queue

    rospy.init_node('publish_goal_fix')

    for line in open('config/gps_waypoints.txt').xreadlines():
        latitude, longitude = [float(f) for f in line.split(",")]
        queue.append((latitude, longitude))

    pub = rospy.Publisher('waypoint', WayPoint, queue_size=10)

    s = rospy.Service('next_waypoint_srv', Trigger, next_waypoint)

    r = rospy.Rate(10)
    while not rospy.is_shutdown():
        msg = WayPoint()
        current_waypoint = queue[0]
        msg.latitude = current_waypoint.latitude
        msg.longitude = current_waypoint.longitude
        msg.altitude = 0
        pub.publish(msg)
        rospy.spin()
def main():
	global queue

	rospy.init_node('publish_goal_fix')

	for line in open('config/gps_waypoints.txt').xreadlines():
		latitude, longitude = [float(f) for f in line.split(",")]
		queue.append((latitude, longitude))

	pub = rospy.Publisher('waypoint', WayPoint, queue_size=10)
	
	s  = rospy.Service('next_waypoint_srv', Trigger, next_waypoint)

	r = rospy.Rate(10)
	while not rospy.is_shutdown():
		msg = WayPoint()
		current_waypoint = queue[0]
		msg.latitude = current_waypoint.latitude
		msg.longitude = current_waypoint.longitude
		msg.altitude = 0
		pub.publish(msg)	
		rospy.spin()
    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 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)
Exemple #8
0
 def test_empty_waypoint_match(self):
     p = WayPoint()
     self.assertEqual(match(p, set(['nothing', 'defined'])), None)
    def controller(self):
        rate = rospy.Rate(10)  # 10hz

        # self.x = self.polar2cartesian([earth_radius, 41.104444, 29.026997])[0] # update self.x everytime
        # self.y = self.polar2cartesian([earth_radius, 41.104444, 29.026997])[1] # update self.x everytime

        while not rospy.is_shutdown():

            # for simulating
            self.vx = self.twist.linear.x
            self.vy = self.twist.linear.y
            self.vth = self.twist.angular.z

            rospy.loginfo(rospy.get_caller_id() + "I heard %s", self.vx)

            self.current_time = rospy.Time.now()

            # We do not need this part, we are doing our own localization
            # compute odometry in a typical way given the velocities of the robot
            self.dt = (self.current_time - self.last_time).to_sec()
            self.delta_x = (self.vx * cos(self.th) -
                            self.vy * sin(self.th)) * self.dt
            self.delta_y = (self.vx * sin(self.th) +
                            self.vy * cos(self.th)) * self.dt
            self.delta_th = self.vth * self.dt

            # from GPS
            # self.target_location = [earth_radius, 41.103465, 29.027909] # publish to move_base_simple/goal
            # from magnetometer
            # self.yaw = 0

            # X, Y = current location
            # th = current yaw
            """
            self.x += self.delta_x
            self.y += self.delta_y
            self.th += self.delta_th
            """
            # since all odometry is 6DOF we'll need a quaternion created from yaw
            self.odom_quat = tf.transformations.quaternion_from_euler(
                0, 0, self.th)

            # first, we'll publish the transform over tf

            self.odom_broadcaster.sendTransform(
                (self.x, self.y, 0.), self.odom_quat, self.current_time,
                "base_link", "odom")

            # next, we'll publish the odometry message over ROS
            self.odom = Odometry()
            self.odom.header.stamp = self.current_time
            self.odom.header.frame_id = "odom"

            # set the position
            self.odom.pose.pose = Pose(Point(self.x, self.y, 0.),
                                       Quaternion(*self.odom_quat))

            # Write a tranform formula for calculating linear.x linear.y and angular.z speed
            # set the velocity
            self.odom.child_frame_id = "base_link"
            self.odom.twist.twist = Twist(Vector3(self.vx, self.vy, 0),
                                          Vector3(0, 0, self.vth))

            # publish the PoseWithCovarianceStamped
            self.pwcs = PoseWithCovarianceStamped()
            self.pwcs.header.stamp = self.odom.header.stamp
            self.pwcs.header.frame_id = "odom"
            self.pwcs.pose.pose = self.odom.pose.pose
            self.last_time = self.current_time

            # publish the NavSatFix
            # sensor_data = SC, Pressure, Temp, Roll, Pitch , Yaw, Latitude, Longitude, Accuracy, Altitude, Speed, Battery, ITU \n
            self.sensor_split = [
                'SC', '0', '0', '0', '0', '0', '0', '0', 'ITU'
            ]
            if self.sensor_data:
                self.sensor_split = self.sensor_data.split(',')
            self.gps_fix = NavSatFix()
            self.gps_fix.header.frame_id = "base_link"
            self.gps_fix.header.stamp = self.odom.header.stamp
            self.gps_fix.status.status = 0  # GPS FIXED
            self.gps_fix.status.service = 1  # GPS SERVICE = GPS
            # Buralar bizden gelecek
            self.gps_fix.latitude = float(self.sensor_split[6])  # 41.24600
            self.gps_fix.longitude = float(self.sensor_split[7])
            self.gps_fix.altitude = 0
            self.gps_fix.position_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
            self.gps_fix.position_covariance_type = 0

            # publish the NavSatFix
            self.waypoint = WayPoint()
            # self.waypoint.id.uuid = [1]
            self.waypoint.position.latitude = 40.84600
            self.waypoint.position.longitude = 29.392400
            self.waypoint.position.altitude = 0
            # self.waypoint.props.key = "key"
            # self.waypoint.props.value = "1"

            # publish the NavSatFix
            self.imuMsg = Imu()
            self.imuMsg.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
            self.imuMsg.angular_velocity_covariance = [
                0, 0, 0, 0, 0, 0, 0, 0, 0
            ]
            self.imuMsg.linear_acceleration_covariance = [
                0, 0, 0, 0, 0, 0, 0, 0, 0
            ]

            self.roll = 0
            self.pitch = 0
            self.yaw = float(self.sensor_split[5]) * pi / 180  # self.th
            self.th = self.yaw
            # Acceloremeter
            self.imuMsg.linear_acceleration.x = 0
            self.imuMsg.linear_acceleration.y = 0
            self.imuMsg.linear_acceleration.z = 0

            # Gyro
            self.imuMsg.angular_velocity.x = 0
            self.imuMsg.angular_velocity.y = 0
            self.imuMsg.angular_velocity.z = 0

            self.q = tf.transformations.quaternion_from_euler(
                self.roll, self.pitch, self.yaw)
            self.imuMsg.orientation.x = self.q[0]  #magnetometer
            self.imuMsg.orientation.y = self.q[1]
            self.imuMsg.orientation.z = self.q[2]
            self.imuMsg.orientation.w = self.q[3]

            self.imuMsg.header.frame_id = "base_link"
            self.imuMsg.header.stamp = self.odom.header.stamp
            # Subscriber(s)
            rospy.Subscriber('/husky_velocity_controller/cmd_vel', Twist,
                             self.callback)
            rospy.Subscriber('/odometry/gps', Odometry, self.gps_callback)
            # Publisher(s)
            self.odom_pub.publish(self.odom)
            self.odom_combined_pub.publish(self.pwcs)
            self.gps_pub.publish(self.gps_fix)
            self.waypoint_pub.publish(self.waypoint)
            self.imu_pub.publish(self.imuMsg)
            rate.sleep()
def makeWayPoint(id, lat, lon):
    w = WayPoint()
    w.id = UniqueID(id)
    w.position = GeoPoint(latitude=lat, longitude=lon)
    return w
Exemple #11
0
def get_osm(url, bounds):
    """Get GeographicMap from Open Street Map XML data.

    The latitude and longitude bounding box returned may differ from
    the requested bounds.

    :param url:    Uniform Resource Locator for map.
    :param bounds: Desired bounding box for map (presently ignored).
    :returns: GeographicMap message with header not filled in.
    """
    # parse the URL
    filename = ''
    if url.startswith('file:///'):
        filename = url[7:]
    elif url.startswith('package://'):
        pkg_name, slash, pkg_path = url[10:].partition('/')
        pkg_dir = roslib.packages.get_pkg_dir(pkg_name)
        filename = pkg_dir + '/' + pkg_path
    else:
        raise ValueError('unsupported URL: ' + url)

    map = GeographicMap(id=geodesy.gen_uuid.makeUniqueID(url))
    xm = None
    try:
        f = open(filename, 'r')
        xm = ElementTree.parse(f)
    except IOError:
        raise ValueError('unable to read ' + str(url))
    except ElementTree.ParseError:
        raise ValueError('XML parse failed for ' + str(url))
    osm = xm.getroot()

    # get map bounds
    for el in osm.iterfind('bounds'):
        map.bounds.min_latitude = float(get_required_attribute(el, 'minlat'))
        map.bounds.min_longitude = float(get_required_attribute(el, 'minlon'))
        map.bounds.max_latitude = float(get_required_attribute(el, 'maxlat'))
        map.bounds.max_longitude = float(get_required_attribute(el, 'maxlon'))

    # get map way-point nodes
    for el in osm.iterfind('node'):

        way = WayPoint()
        id = el.get('id')
        if id == None:
            raise ValueError('node id missing')
        way.id = makeOsmUniqueID('node', id)

        way.position.latitude = float(get_required_attribute(el, 'lat'))
        way.position.longitude = float(get_required_attribute(el, 'lon'))
        way.position.altitude = float(el.get('ele', float('nan')))

        for tag_list in el.iterfind('tag'):
            kv = get_tag(tag_list)
            if kv != None:
                way.props.append(kv)

        map.points.append(way)

    # get map paths
    for el in osm.iterfind('way'):

        feature = MapFeature()
        id = el.get('id')
        if id == None:
            raise ValueError('way id missing')
        feature.id = makeOsmUniqueID('way', id)

        for nd in el.iterfind('nd'):
            way_id = get_required_attribute(nd, 'ref')
            feature.components.append(makeOsmUniqueID('node', way_id))

        for tag_list in el.iterfind('tag'):
            kv = get_tag(tag_list)
            if kv != None:
                feature.props.append(kv)

        map.features.append(feature)

    # get relations
    for el in osm.iterfind('relation'):

        feature = MapFeature()
        id = el.get('id')
        if id == None:
            raise ValueError('relation id missing')
        feature.id = makeOsmUniqueID('relation', id)

        for mbr in el.iterfind('member'):
            mbr_type = get_required_attribute(mbr, 'type')
            if mbr_type in set(['node', 'way', 'relation']):
                mbr_id = get_required_attribute(mbr, 'ref')
                feature.components.append(makeOsmUniqueID(mbr_type, mbr_id))
            else:
                print('unknown relation member type: ' + mbr_type)

        for tag_list in el.iterfind('tag'):
            kv = get_tag(tag_list)
            if kv != None:
                feature.props.append(kv)

        map.features.append(feature)

    return map
def get_osm(url, bounds):
    """Get `geographic_msgs/GeographicMap`_ from Open Street Map XML data.

    The latitude and longitude of the bounding box returned may differ
    from the requested bounds.

    :param url:    Uniform Resource Locator for map.
    :param bounds: Desired `geographic_msgs/BoundingBox`_ for map (presently ignored).
    :returns: `geographic_msgs/GeographicMap`_ message (header not filled in).
    """
    # parse the URL
    filename = ''
    if url.startswith('file:///'):
        filename = url[7:]
    elif url.startswith('package://'):
        pkg_name, slash, pkg_path = url[10:].partition('/')
        pkg_dir = roslib.packages.get_pkg_dir(pkg_name)
        filename = pkg_dir + '/' + pkg_path
    else:
        raise ValueError('unsupported URL: ' + url)

    gmap = GeographicMap(id = geodesy.gen_uuid.makeUniqueID(url))
    xm = None
    try:
        f = open(filename, 'r')
        xm = ElementTree.parse(f)
    except IOError:
        raise ValueError('unable to read ' + str(url))
    except ElementTree.ParseError:
        raise ValueError('XML parse failed for ' + str(url))
    osm = xm.getroot()

    # get map bounds
    for el in osm.iterfind('bounds'):
        minlat = float(get_required_attribute(el, 'minlat'))
        minlon = float(get_required_attribute(el, 'minlon'))
        maxlat = float(get_required_attribute(el, 'maxlat'))
        maxlon = float(get_required_attribute(el, 'maxlon'))
        gmap.bounds = bounding_box.makeBounds2D(minlat, minlon, maxlat, maxlon)

    # get map way-point nodes
    for el in osm.iterfind('node'):

        way = WayPoint()
        el_id = el.get('id')
        if el_id is None:
            raise ValueError('node id missing')
        way.id = makeOsmUniqueID('node', el_id)

        way.position.latitude = float(get_required_attribute(el, 'lat'))
        way.position.longitude = float(get_required_attribute(el, 'lon'))
        way.position.altitude = float(el.get('ele', float('nan')))

        for tag_list in el.iterfind('tag'):
            kv = get_tag(tag_list)
            if kv != None:
                way.props.append(kv)

        gmap.points.append(way)

    # get map paths
    for el in osm.iterfind('way'):

        feature = MapFeature()
        el_id = el.get('id')
        if el_id is None:
            raise ValueError('way id missing')
        feature.id = makeOsmUniqueID('way', el_id)

        for nd in el.iterfind('nd'):
            way_id = get_required_attribute(nd, 'ref')
            feature.components.append(makeOsmUniqueID('node', way_id))

        for tag_list in el.iterfind('tag'):
            kv = get_tag(tag_list)
            if kv != None:
                feature.props.append(kv)

        gmap.features.append(feature)

    # get relations
    for el in osm.iterfind('relation'):

        feature = MapFeature()
        el_id = el.get('id')
        if el_id is None:
            raise ValueError('relation id missing')
        feature.id = makeOsmUniqueID('relation', el_id)

        for mbr in el.iterfind('member'):
            mbr_type = get_required_attribute(mbr, 'type')
            if mbr_type in set(['node', 'way', 'relation']):
                mbr_id = get_required_attribute(mbr, 'ref')
                feature.components.append(makeOsmUniqueID(mbr_type, mbr_id))
            else:
                print('unknown relation member type: ' + mbr_type)

        for tag_list in el.iterfind('tag'):
            kv = get_tag(tag_list)
            if kv != None:
                feature.props.append(kv)

        gmap.features.append(feature)

    return gmap
Exemple #13
0
def makeWayPoint(id, lat, lon):
    w = WayPoint()
    w.id = UniqueID(id)
    w.position = GeoPoint(latitude=lat, longitude=lon)
    return w