def talker(zero_utm):
pub = rospy.Publisher('/crw_geopose_pub', GeoPose, queue_size=10)
rospy.init_node('simple_simulator', anonymous=True)
rate = rospy.Rate(10) # 10hz
pp = GeoPose()
while not rospy.is_shutdown():
zero_utm.northing -= 0.1
pp.position = zero_utm.toMsg()
pub.publish(pp)
rate.sleep()
def generatePaths(self, task):
path = []
print(task['nav_objectives'])
for p in task['nav_objectives'][
task['current_nav_objective_index']]['waypoints']:
#path.append((p['position']['latitude'],p['position']['longitude']))
gp = GeoPose()
gp.position.latitude = p['latitude']
gp.position.longitude = p['longitude']
path.append(gp)
task['current_path'] = path
# decide if we transit or start line
task['transit_path'] = None
if len(task['current_path']) > 1:
start_point = task['current_path'][0]
next_point = task['current_path'][1]
if task['do_transit'] and self.missionManager.distanceTo(
start_point.position.latitude,
start_point.position.longitude
) > self.missionManager.waypointThreshold and self.missionManager.planner == 'path_follower':
#transit
transit_path = self.missionManager.generatePathFromVehicle(
start_point.position.latitude,
start_point.position.longitude,
self.missionManager.segmentHeading(
start_point.position.latitude,
start_point.position.longitude,
next_point.position.latitude,
next_point.position.longitude))
task['transit_path'] = transit_path
task['do_transit'] = True
def cb_set_datum(request):
"""
service set_datum callback.
This method will collect currnt location's gps signal to set the DATUM.
And reset the ekf_node initial pose. This means the datum will be the origin of map.
"""
global DATUM
SERVICE_INIT_STATE.call()
lati_avg, long_avg = utility.get_avg_gps(topic=NAME_TOPIC_GPS)
DATUM = (lati_avg, long_avg, 0.0)
# -- Use robot_localization navsat_transform_node service to set datum.
# -- Note: Changing existing parameter ~datum doesn't work
geo_pose = GeoPose()
geo_pose.position.latitude = DATUM[0]
geo_pose.position.longitude = DATUM[1]
geo_pose.position.altitude = 0.0
geo_pose.orientation.x = 0.0
geo_pose.orientation.y = 0.0
geo_pose.orientation.z = 0.0
geo_pose.orientation.w = 1.0
SERVICE_NAVSAT_DATUM(geo_pose) # navsat_transform_node datum
PUB.publish(geo_pose)
rospy.loginfo("gps_nav: DATUM = {}, {}".format(DATUM[0], DATUM[1]))
return EmptyResponse()
def __init__(self):
self.wind_sub = rospy.Subscriber('/crw_wind_pub', Vector3,
self.wind_callback)
self.pose_sub = rospy.Subscriber('/crw_geopose_pub', GeoPose,
self.pose_callback)
self.wind_pub = rospy.Publisher('/wind_converted',
Vector3,
queue_size=100)
self.current_pose = GeoPose()
def set_datum(datum_lat, datum_lon):
rospy.loginfo('Waiting for service datum')
rospy.wait_for_service('datum')
try:
datum_srv = rospy.ServiceProxy('datum', SetDatum)
datum_pose = GeoPose()
datum_pose.position.latitude = datum_lat
datum_pose.position.longitude = datum_lon
datum_pose.position.altitude = 0
datum_pose.orientation.x = 0
datum_pose.orientation.y = 0
datum_pose.orientation.z = 0
datum_pose.orientation.w = 1
rospy.loginfo('Setting datum to ({}, {})'.format(datum_lat, datum_lon))
resp = datum_srv(datum_pose)
rospy.loginfo('SetDatum Response: {}'.format(resp))
except rospy.ServiceException as se:
rospy.logerror('Service call failed: {}'.format(se))
def __init__(self,distance):
self.distance = distance
self.timelog = [0,0]
# init path and add points
self.fovPath = GeoPath()
for i in range(9):
self.fovPath.poses.append(GeoPose())
#init VizItem and add polygon
self.fovVizItem = GeoVizItem()
self.fovVizItem.polygons.append(GeoPath())
self.fovVizItem.id = "FOV"
try:
rospy.wait_for_service('enc_query_node')
self.enc_query = rospy.ServiceProxy('enc_query_node', enc_query_srv)
except rospy.ServiceException as e:
rospy.loginfo("ERROR IN SERVICE INITIALIZATION")
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
rospy.wait_for_service('enc_query_node')
print("Done waiting for service...")
try:
enc_query = rospy.ServiceProxy('enc_query_node', enc_query_srv)
# rosparam.set_param('enc_root', '/home/thomas/Downloads/ENC_ROOT')
# rosparam.set_param('catalog_location', 'https://www.charts.noaa.gov/ENCs/NH_ENCProdCat_19115.xml')
resp1 = enc_query(layers, points)
print(resp1)
return resp1
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
if __name__ == "__main__":
layers = ['boylat']
p1 = GeoPose()
p1.position.latitude = 43.123
p1.position.longitude = -70.863
p2 = GeoPose()
p2.position.latitude = 43.12
p2.position.longitude = -70.855
p3 = GeoPose()
p3.position.latitude = 43.12
p3.position.longitude = -70.863
p4 = GeoPose()
p4.position.latitude = 43.123
p4.position.longitude = -70.863
points = [p1, p2, p3, p4]
enc_query_client(layers, points)
def clicked_callback(self, msg):
gpose = self.get_utm_coords([msg.point.x, msg.point.y])
outmsg = GeoPose()
outmsg.position = gpose.toMsg()
self.wp_pub_.publish(outmsg)
