def navGoal_callb(msg):
''' reference state update. Specifically for Rviz'''
global wayPoints, state_ref
wayPoints = []
point = msg.pose.position
state_ref[0] = point.x
state_ref[1] = point.y
state_ref[2] = point.z
wp = GPS_data()
wp.x = point.x
wp.y = point.y
wayPoints.append(point)
def calculate_transect(theta_c):
global state_asv
point1 = GPS_data()
point2 = GPS_data()
distL = get_distance(theta_c + math.pi / 2, 21)
distR = get_distance(theta_c - math.pi / 2, 21)
point1.x = state_asv[0] + (distR + 10) * math.cos(theta_c - math.pi / 2)
point1.y = state_asv[1] + (distR + 10) * math.sin(theta_c - math.pi / 2)
point2.x = state_asv[0] + (distL + 10) * math.cos(theta_c + math.pi / 2)
point2.y = state_asv[1] + (distL + 10) * math.sin(theta_c + math.pi / 2)
#how should the points be saved for transect controller??
return [point1, point2]
def create_wpList():
global wayPoints
pub = rospy.Publisher('wpArray', MarkerArray, queue_size=1)
markerArray = MarkerArray()
point = GPS_data()
list = []
point.x = 15.0
point.y = 0.0
wayPoints.append(point)
point = GPS_data()
point.x = 20.0
point.y = 2.0
wayPoints.append(point)
Example:
this node should be ran with the nmea driver. Don't forget to connect the
GPS to the jetson!! If you encounter permission issue, run:
sudo chmod 666 /dev/serial/by-id/*
rosrun commands:
rosrun nmea_navsat_driver nmea_serial_driver _port:="/dev/serial/by-id/usb-Prolific_Technology_Inc._USB-Serial_Controller-if00-port0" _baud:=119200
rosrun gps_reader GPS_node.py
"""
origLat = 0.0
origLon = 0.0
gps_message = GPS_data()
pub_gps = rospy.Publisher('/GPS_coord', GPS_data, queue_size=10)
x_prev = 0.0
y_prev = 0.0
t_prev = rospy.Time()
angC_prev = 0.0
def GPS_posCallb(msg):
'''Call back function on the /fix published by the nmea driver
Reads the position in latitude and longitude. Converts it to x,y.
Publishes custom message with positions: x,y,lon,lat and velocities: lin,ang
Args:
msg (NavSatFix) message published by the /fix'''
global origLat, origLon, gps_message, pub_gps, x_vel, y_vel, ang_vel
def create_wpList():
''' Dummy waypoint generation '''
global wayPoints
pub = rospy.Publisher('wpArray', MarkerArray, queue_size=1)
markerArray = MarkerArray()
point = GPS_data()
list = []
point.x = 1.0
point.y = 0.0
wayPoints.append(point)
point = GPS_data()
point.x = 5.0
point.y = 2.0
wayPoints.append(point)
point = GPS_data()
point.x = 70.0
point.y = 60.0
wayPoints.append(point)
point = GPS_data()
point.x = 50.0
point.y = 50.0
wayPoints.append(point)
point = GPS_data()
point.x = 30.0
point.y = 20.0
wayPoints.append(point)
point = GPS_data()
point.x = 0.0
point.y = 0.0
wayPoints.append(point)
def main():
rospy.init_node('wpCreator')
point = GPS_data()
wpList = GPS_WayPoints()
pub = rospy.Publisher('ControlCenter/gps_wp', GPS_WayPoints, queue_size=10)
list = []
point.x = 2.0
point.y = 0.0
list.append(point)
point.x = 5.0
point.y = 2.0
list.append(point)
point.x = 7.0
point.y = 6.0
list.append(point)
point.x = 5.0
point.y = 5.0
list.append(point)
point.x = 3.0
point.y = 2.0
list.append(point)
point.x = 0.0
point.y = 0.0
list.append(point)
wpList = list
pub.publish(wpList)