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)