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)
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')
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)
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
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
def makeWayPoint(id, lat, lon):
w = WayPoint()
w.id = UniqueID(id)
w.position = GeoPoint(latitude=lat, longitude=lon)
return w
