def PointCloudCall(self,data): # 20 Hz As this function has highest frequency, good for update
pointCloud_ = PointCloud2()
pointCloud_ = data
self.graphslam_PointCloud_pub.publish(pointCloud_) # for Graph slam
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS)
self.nlosExclusionF_.nlosExclusion_(self.posArr, self.GNSS_, 'statManu', self.excluSatLis) # statManu
self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal()
if((len(self.GNSSNRAWlosDel.GNSS_Raws) > 1) and (self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time != self.GNSSNRAWlosDelTimeFlag)): # only if there is a change, there will conduct the calculation
# print 'self.GNSSNRAWlosDel',len(self.GNSSNRAWlosDel.GNSS_Raws)
self.puGNSSPosCalF_prop.iterPosCal(self.GNSSNRAWlosDel, 'WLSGNSS')
self.GNSSNRAWlosDelTimeFlag = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time
navfix_ = NavSatFix()
llh_ = []
llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_prop.ecef_)
navfix_.header.stamp.secs = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time
navfix_.latitude = float(llh_[0])
navfix_.longitude = float(llh_[1])
navfix_.altitude = float(llh_[2])
self.propGNSS_Navfix_pub.publish(navfix_)
# for Graph slam
graphNavfix_ = NavSatFix()
graphNavfix_ = navfix_
graphNavfix_.header = pointCloud_.header
self.graphslam_Navfix_pub.publish(graphNavfix_)
# for Graph slam
geopoint = GeoPointStamped()
geopoint.header = graphNavfix_.header
geopoint.position.latitude = float(llh_[0])
geopoint.position.longitude = float(llh_[1])
geopoint.position.altitude = float(llh_[2])
self.graphslam_GeoPoint_pub.publish(geopoint)
def gpsCallback(self, data):
gps = GeoPointStamped()
gps.header = data.header
gps.position.latitude = data.latitude
gps.position.longitude = data.longitude
gps.position.altitude = data.altitude
self.position_pub.publish(gps)
def PointCloudCall(
self, data
): # 20 Hz As this function has highest frequency, good for update
self.pointCloud_ = PointCloud2()
self.pointCloud_ = data
# self.graphslam_PointCloud_pub.publish(self.pointCloud_) # for Graph slam
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS)
self.nlosExclusionF_.nlosExclusion_(self.posArr, self.GNSS_,
'statManu_ExclusionOpenLoop',
self.excluSatLis) # statManu
self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal()
if ((len(self.GNSSNRAWlosDel.GNSS_Raws) > 1)
and (self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time !=
self.GNSSNRAWlosDelTimeFlag)
): # only if there is a change, there will conduct the calculation
# print 'self.GNSSNRAWlosDel',len(self.GNSSNRAWlosDel.GNSS_Raws)
self.puGNSSPosCalF_prop.iterPosCal(self.GNSSNRAWlosDel, 'WLSGNSS')
self.GNSSNRAWlosDelTimeFlag = self.GNSSNRAWlosDel.GNSS_Raws[
-1].GNSS_time
navfix_ = NavSatFix()
llh_ = []
llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_prop.ecef_)
navfix_.header.stamp.secs = self.GNSSNRAWlosDel.GNSS_Raws[
-1].GNSS_time
navfix_.latitude = float(llh_[0])
navfix_.longitude = float(llh_[1])
navfix_.altitude = float(llh_[2])
self.propGNSS_Navfix_pub.publish(navfix_)
# for Graph slam
graphNavfix_ = NavSatFix()
graphNavfix_ = navfix_
graphNavfix_.header = self.pointCloud_.header
self.graphslam_Navfix_pub.publish(graphNavfix_)
# for Graph slam
geopoint = GeoPointStamped()
geopoint.header = graphNavfix_.header
# calculate the ENU coordiantes initialLLH
enu_ = pugeomath.transformEcefToEnu(self.initialLLH,
self.puGNSSPosCalF_prop.ecef_)
print 'enu_ ->: ', enu_
geopoint.position.latitude = float(enu_[0])
geopoint.position.longitude = float(enu_[1])
# geopoint.position.altitude = float(llh_[2]) # use this to save the covariance in altitude component
geopoint.position.altitude = self.GNSSNRAWlosDel.GNSS_Raws[
-1].snr * self.HDOPProp # save the covariance
self.graphslam_GeoPoint_pub.publish(geopoint)
def __init__(self):
rospy.init_node('MissionManager')
self.Mission = MissionPlan.missionplan.Mission()
#print self.Mission;
# Variable for waypoint
self.waypoint = PointStamped()
# Variable for current position of boat--to be set in position_callback
self.currPosBoat_LatLon = GeoPointStamped()
self.currPosBoat_ECEF = PointStamped()
self.currPosBoat_Map = PointStamped()
# Set minimum distance to waypoint
self.threshold = 3.0
# Receive up-to-date UTM coordinates of boat
rospy.Subscriber('/position_utm', PoseStamped, self.position_utm_callback, queue_size = 1)
# Receive up-to-date lat/lon coordinates of boat
rospy.Subscriber('/position', GeoPointStamped, self.position_callback, queue_size = 1)
# To publish waypoint updates to MOOS
self.wpt_updates = rospy.Publisher('/moos/wpt_updates', String, queue_size = 10)
# TODO:
# Subscribe to waypoints/behaviors from AutonomousMissionPlanner. Not sure what topic.
pass
def update(self, event):
if self.last_command_timestamp is None or event.current_real - self.last_command_timestamp > rospy.Duration.from_sec(
0.5 * self.time_factor):
self.throttle = 0.0
self.rudder = 0.0
self.dynamics.update(self.throttle, self.rudder, event.current_real)
gps = GeoPointStamped()
gps.header.stamp = self.dynamics.last_update
gps.position.latitude = math.degrees(self.dynamics.latitude)
gps.position.longitude = math.degrees(self.dynamics.longitude)
self.position_publisher.publish(gps)
ts = TwistStamped()
ts.header.stamp = self.dynamics.last_update
# TODO should this be split in x and y components when heading != course?
ts.twist.linear.x = self.dynamics.sog
self.speed_publisher.publish(ts)
#p.basic_position.cog = self.dynamics.cog
#p.basic_position.sog = self.dynamics.sog
#p.header.stamp = self.dynamics.last_update
h = NavEulerStamped()
h.header.stamp = self.dynamics.last_update
h.orientation.heading = math.degrees(self.dynamics.heading)
self.heading_publisher.publish(h)
def __init__(self):
rospy.init_node('MissionManager')
# This will be used to read mission.txt file later...
self.Mission = MissionPlan.missionplan.Mission()
self.missionReader = MissionReader.MissionReader()
# Variable for waypoint - in map / robot coordinates
self.waypoint = PointStamped()
# Variable for current position of boat--to be set in position_callback
self.currPosBoat_LatLon = GeoPointStamped()
self.currPosBoat_ECEF = PointStamped()
self.currPosBoat_Map = PointStamped()
# TODO: Change depth type when we find out what it should be.
self.depth = Float32
self.wptIndex = Int32
# TODO: Threshold possibly set thresholds in parameters?
# Set minimum distance to waypoint
self.shallowWaterDanger = False
self.waypointThreshold = 3.0
self.depthThreshold = 4.0
self.shallowWaterNegativeCount = 0
self.shallowWaterPositiveCount = 0
# If ASV receives this many consecutive negative shallow warnings,
# reset positive shallowWater counts
self.shallowWaterNegativeCountThreshold = 5
# If ASV receives this many consecutive positive shallow warnings,
# deviate path and reset negative shallowWater counts
self.shallowWaterPositiveCountThreshold = 5
#self.xIndex = 1
self.moos_wptIndex_Update = False
self.shallowWaterUpdate = False
self.incrementedLine = False
# Timer used to print status
self.everyTwoSeconds = rospy.Duration(0.5)
# Receive up-to-date UTM coordinates of ASV
rospy.Subscriber('/position_utm', PoseStamped, self.position_utm_callback, queue_size = 1)
# Receive up-to-date lat/lon coordinates of ASV
rospy.Subscriber('/position', GeoPointStamped, self.position_callback, queue_size = 1)
# Receive up-to-date depth
rospy.Subscriber('/depth', Float32, self.depth_callback, queue_size = 1)
#Receive up-to-date warnings from Don't Run Aground, Ben! behavior.
rospy.Subscriber('/shallow_water_warning', String, self.shallow_water_callback, queue_size = 1)
# Receive up-to-date waypoint indices
rospy.Subscriber('/moos/wpt_index', Int32, self.waypoint_index_callback, queue_size = 1)
# Receive up-to-date waypoint updates
rospy.Subscriber('/moos/wpt_updates', String, self.waypoint_updates_callback, queue_size = 1)
# To publish waypoint updates to MOOS
self.wpt_index_publisher = rospy.Publisher('/moos/wpt_index', Int32, queue_size = 10)
self.wpt_updates_publisher = rospy.Publisher('/moos/wpt_updates', String, queue_size = 10)
self.behavior_ctrl = rospy.Publisher('/behavior_ctrl', BehaviorControl, queue_size = 10)
def latlong_to_GeoPointStamped(self,LAT,LONG): ''' Assigning the given waypoint (LAT,LONG) to a GeoPointStamped variable ''' # because the message type of the input to the service 'wgs84_to_earth' LatLongToEarth.srv is GeoPointStamp wpt_latlong = GeoPointStamped() #latlong_location.header.frame_id = 'earth' # <-- Don't need it wpt_latlong.position.latitude = LAT wpt_latlong.position.longitude = LONG #print "Given WPT - LAT,LONG :" #print wpt_latlong return wpt_latlong
def __get_point_msg(cls, data, frame):
"""
Deserializes a location to a message of type PointStamped.
Args:
data: A dictionary containing the location.
frame: Frame for the point.
Returns:
A message of type PointStamped with the location.
"""
header = Header()
header.stamp = rospy.get_rostime()
header.frame_id = frame
msg = GeoPointStamped()
msg.header = header
msg.position.latitude = data["latitude"]
msg.position.longitude = data["longitude"]
return msg
def init_uav(self, arm=False, takeoff=False, verbose=False):
if verbose:
print("Starting init_uav(). Atm we have to manualy accept every new command, but just remove the `input()` from the code to do this faster.")
res_setMode = self.setmode_srv(0,'GUIDED')
if verbose:
print("Res of setmode:", res_setMode)
res_set_ref_mode = self.set_ref_frame_srv(1) # FRAME_BODY_NED = 8 works for velocities
if verbose:
print("Res of set_ref_frame", res_set_ref_mode)
rospy.sleep(1)
print("Waiting for ekf initialisation")
origin = GeoPointStamped()
origin.header = Header()
origin.header.stamp = rospy.Time.now()
origin.position = GeoPoint()
origin.position.latitude = self.OPERATION_POSITION[0]
origin.position.longitude = self.OPERATION_POSITION[1]
origin.position.altitude = self.OPERATION_POSITION[2]
self.set_origin_pub.publish(origin)
if verbose:
print("Published ",origin)
while(True):
res = self.arm()
if res.success:
break
rospy.sleep(1)
self.disarm()
# input("Ekf initialized?")
#time.sleep(40) # For some reason, it does not work if we use rospy.sleep(...)
if arm or takeoff:
self.arm()
if takeoff:
self.takeoff()
print("Uav is now initialized. We are done with waiting.")
def gps2navsat(filename, bag):
with open(filename, 'r') as file:
try:
while True:
data = struct.unpack('qddd', file.read(8 * 4))
time = data[0]
local = data[1:]
lat_lon_el_theta = struct.unpack('dddd', file.read(8 * 4))
gps_cov = struct.unpack('dddddddddddddddd', file.read(8 * 16))
if abs(lat_lon_el_theta[0]) < 1e-1:
continue
navsat = NavSatFix()
navsat.header.frame_id = 'gps'
navsat.header.stamp = rospy.Time.from_sec(time * 1e-6)
navsat.status.status = NavSatStatus.STATUS_FIX
navsat.status.service = NavSatStatus.SERVICE_GPS
navsat.latitude = lat_lon_el_theta[0]
navsat.longitude = lat_lon_el_theta[1]
navsat.altitude = lat_lon_el_theta[2]
navsat.position_covariance = numpy.array(gps_cov).reshape(
4, 4)[:3, :3].flatten().tolist()
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN
bag.write('/gps/fix', navsat, navsat.header.stamp)
geopoint = GeoPointStamped()
geopoint.header = navsat.header
geopoint.position.latitude = lat_lon_el_theta[0]
geopoint.position.longitude = lat_lon_el_theta[1]
geopoint.position.altitude = lat_lon_el_theta[2]
bag.write('/gps/geopoint', geopoint, geopoint.header.stamp)
except:
print 'done'
def extractPosition(self, wpt):
'''Helper method to retrieve position from input mission file'''
position = GeoPointStamped()
position.position.latitude = wpt["nav"]["position"]["latitude"]
position.position.longitude = wpt["nav"]["position"]["longitude"]
# Altitude may be "None" -- GeoPointStamped requires float
if type(wpt["nav"]["position"]["altitude"]) is float:
position.position.altitude = wpt["nav"]["position"]["altitude"]
# Return GeoPointStamped message
return position
def gps2navsat(filename, bag):
with open(filename, 'r') as file:
try:
while True:
data = struct.unpack('qddd', file.read(8*4))
time = data[0]
local = data[1:]
lat_lon_el_theta = struct.unpack('dddd', file.read(8*4))
gps_cov = struct.unpack('dddddddddddddddd', file.read(8*16))
if abs(lat_lon_el_theta[0]) < 1e-1:
continue
navsat = NavSatFix()
navsat.header.frame_id = 'gps'
navsat.header.stamp = rospy.Time.from_sec(time * 1e-6)
navsat.status.status = NavSatStatus.STATUS_FIX
navsat.status.service = NavSatStatus.SERVICE_GPS
navsat.latitude = lat_lon_el_theta[0]
navsat.longitude = lat_lon_el_theta[1]
navsat.altitude = lat_lon_el_theta[2]
navsat.position_covariance = numpy.array(gps_cov).reshape(4, 4)[:3, :3].flatten().tolist()
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN
bag.write('/gps/fix', navsat, navsat.header.stamp)
geopoint = GeoPointStamped()
geopoint.header = navsat.header
geopoint.position.latitude = lat_lon_el_theta[0]
geopoint.position.longitude = lat_lon_el_theta[1]
geopoint.position.altitude = lat_lon_el_theta[2]
bag.write('/gps/geopoint', geopoint, geopoint.header.stamp)
except:
print 'done'
def setEKFOrigin(self):
gp_origin_pub = rospy.Publisher(
'/mavros/global_position/set_gp_origin',
GeoPointStamped,
queue_size=1,
latch=True)
global_origin = GeoPointStamped()
global_origin.header.stamp = rospy.Time.now()
global_origin.header.seq = 0
global_origin.header.frame_id = ''
global_origin.position.latitude = 13.736444
global_origin.position.longitude = 100.533986
global_origin.position.altitude = 0
gp_origin_pub.publish(global_origin)
def run(self):
rospy.init_node('zboat_gps')
sog_pub = rospy.Publisher('/sog', TwistStamped, queue_size=5)
h_pub = rospy.Publisher('/heading', NavEulerStamped, queue_size=5)
pos_pub = rospy.Publisher('/position', GeoPointStamped, queue_size=5)
while not rospy.is_shutdown():
line = self.gps.read()
#print line
if line and line.startswith('$GPVTG'):
parts = line.split(',')
#print parts
if len(parts) >= 8:
ms = float(parts[7]) * 1000 / 3600.0
#print ms
ts = TwistStamped()
ts.twist.linear.x = ms
sog_pub.publish(ts)
if line and line.startswith('$GPHDT'):
parts = line.split(',')
if len(parts) >= 1:
h = float(parts[1]) #*2*3.14/360.0
nes = NavEulerStamped()
nes.orientation.heading = h
h_pub.publish(nes)
if line and line.startswith('$GPGGA'):
parts = line.split(',')
if len(parts) >= 6:
lat = float(parts[2][:2]) + float(parts[2][2:]) / 60.0
if parts[3] == 'S':
lat = -lat
lon = float(parts[4][:3]) + float(parts[4][3:]) / 60.0
if parts[5] == 'W':
lon = -lon
gps = GeoPointStamped()
gps.position.latitude = lat
gps.position.longitude = lon
pos_pub.publish(gps)
def update(self, event):
if self.last_command_timestamp is None or event.current_real - self.last_command_timestamp > rospy.Duration.from_sec(
0.5 * self.time_factor):
self.throttle = 0.0
self.rudder = 0.0
diag = self.dynamics.update(self.throttle, self.rudder,
event.current_real)
gps = GeoPointStamped()
gps.header.stamp = self.dynamics.last_update
gps.position.latitude = math.degrees(self.dynamics.latitude)
gps.position.longitude = math.degrees(self.dynamics.longitude)
self.position_publisher.publish(gps)
ts = TwistStamped()
ts.header.stamp = self.dynamics.last_update
# TODO should this be split in x and y components when heading != course?
ts.twist.linear.x = self.dynamics.sog
self.speed_publisher.publish(ts)
#p.basic_position.cog = self.dynamics.cog
#p.basic_position.sog = self.dynamics.sog
#p.header.stamp = self.dynamics.last_update
h = NavEulerStamped()
h.header.stamp = self.dynamics.last_update
h.orientation.heading = math.degrees(self.dynamics.heading)
self.heading_publisher.publish(h)
for key, value in diag.items():
if not key in self.diag_publishers:
self.diag_publishers[key] = rospy.Publisher(
'/asv_sim/diagnostics/' + key, Float64, queue_size=5)
self.diag_publishers[key].publish(value)
def update(self, event):
if self.last_command_timestamp is None or event.current_real - self.last_command_timestamp > rospy.Duration.from_sec(
0.5 * self.time_factor):
self.throttle = 0.0
self.rudder = 0.0
self.dynamics.update(self.throttle, self.rudder, event.current_real)
gps = GeoPointStamped()
gps.header.stamp = self.dynamics.last_update
gps.position.latitude = math.degrees(self.dynamics.latitude)
gps.position.longitude = math.degrees(self.dynamics.longitude)
self.position_publisher.publish(gps)
#p.basic_position.cog = self.dynamics.cog
#p.basic_position.sog = self.dynamics.sog
#p.header.stamp = self.dynamics.last_update
h = NavEulerStamped()
h.header.stamp = self.dynamics.last_update
h.orientation.heading = math.degrees(self.dynamics.heading)
self.heading_publisher.publish(h)
def __init__(self):
# Create instance of missionplan
# Used to read mission.txt file or /mission_plan subscriber...
# And store mission plan.
self.Mission = MissionPlan.missionplan.Mission()
self.missionPlanData = False
# Array to contain waypoints and behaviors.
self.waypointBehaviorArray = []
# Variable for waypoint - in map / robot coordinates
#self.waypoint = PointStamped()
# Variable for current position of boat--to be set in position_callback
self.currPosBoat_LatLon = GeoPointStamped()
self.currPosBoat_ECEF = PointStamped()
self.currPosBoat_Map = PointStamped()
# Receive up-to-date mission from Autonomous Mission Planner
rospy.Subscriber('/mission_plan',
String,
self.mission_plan_callback,
queue_size=1)
def run_test(self, filename):
lines = []
index = 0
obstacles = []
map_file = ""
start = []
self.stats["time_limit"] = self.default_time_limit
parameter_file_names = []
try:
with open(filename, "r") as testfile:
for line in testfile:
line = line.strip()
if line.startswith("line"):
lines.append([float(f) for f in line.split(" ")[1:]])
print("Read line ", lines[-1])
assert len(lines[-1]
) == 4 # should be startX startY endX endY
elif line.startswith("start"):
start = [float(f) for f in line.split(" ")[1:]]
start[2] = math.radians(start[2])
# assert len(start) == 4 # x y heading speed # assume speed is zero?
elif line.startswith("obstacle"):
obs = [float(f) for f in line.split(" ")[1:]]
if len(obs) == 4:
obs.append(5)
obs.append(20)
obstacles.append(obs)
# ignore test-defined time limit (deprecated)
# elif line.startswith("time_limit"):
# self.stats["time_limit"] = float(line[10:])
elif line.startswith("map_file"):
map_file = line[8:].strip()
elif line.startswith("parameter_file"):
parameter_file_names.append(line[15:])
except IOError as err:
print("Couldn't find file: " + filename)
return
try:
# Convert to lat long
lines = [self.convert_line(line) for line in lines]
except rospy.ServiceException as exc:
print("Map to LatLong service did not process request: " +
str(exc))
# # wait until clock is initialized
# while rospy.get_time() == 0:
# pass
if rospy.get_time() == 0:
print("Simulation does not appear to be running yet. Exiting.")
return
# load parameters and update dynamic reconfigure
self.load_parameters(parameter_file_names)
# load map file, if any
if map_file:
current_path = os.getcwd()
self.path_planner_parameter_client.update_configuration(
{"planner_geotiff_map": current_path + "/" + map_file})
else:
self.path_planner_parameter_client.update_configuration(
{"planner_geotiff_map": ""})
self.test_name = filename
# create results directory
now = datetime.now()
results_dir_path = "../results/" + now.strftime("%Y_%m_%d/%H:%M:%S_" +
self.test_name)
if not os.path.exists(results_dir_path):
os.makedirs(results_dir_path)
# initialize bag
self.bag_lock.acquire()
self.bag = rosbag.Bag(results_dir_path + "/logs.bag", 'w')
self.bag_lock.release()
self.piloting_mode_publisher.publish("autonomous")
# Set up set_pose request
if start:
gps = GeoPointStamped()
gps.position = self.convert_point(start[0], start[1])
h = NavEulerStamped()
h.orientation.heading = start[2]
self.set_pose(gps, h)
else:
print("Warning: no start state read; using default start state")
self.reset_publisher.publish(True)
rospy.sleep(0.2) # Let simulator reset
# finish setting up obstacles
for o in obstacles:
o.append(rospy.get_time())
o[2] = math.radians(o[2])
self.start_time = rospy.get_time()
self.end_time = self.start_time + self.stats["time_limit"]
# set up both path_planner goal and display for lines
goal = path_planner.msg.path_plannerGoal()
goal.path.header.stamp = rospy.Time.now()
display_item = GeoVizItem()
display_item.id = "current_path"
for line in lines:
# now sending all lines at once, so points are to be read in pairs
display_points = GeoVizPointList()
display_points.color.r = 1.0
display_points.color.a = 1.0
display_points.size = 5.0
for p in line:
pose = GeoPoseStamped()
pose.pose.position = p
goal.path.poses.append(pose)
display_points.points.append(p)
# TODO! -- goal.speed?
display_item.lines.append(display_points)
self.display_publisher.publish(display_item)
self.test_running = True
self.path_planner_client.wait_for_server()
self.path_planner_client.send_goal(goal, self.done_callback,
self.active_callback,
self.feedback_callback)
# Spin and publish obstacle updates every 0.5s
self.spin_until_done(obstacles)
print("Test %s complete in %s seconds." %
(self.test_name, (rospy.get_time() - self.start_time)))
# remove line from display
if display_item:
display_item.lines = []
self.display_publisher.publish(display_item)
self.piloting_mode_publisher.publish("standby")
# reporting
self.write_results(results_dir_path)
self.reset_stats()
self.bag_lock.acquire()
self.bag.close()
self.bag = None
self.bag_lock.release()
########## Define Subscribers
state_sub = rospy.Subscriber(mavros.get_topic('state'), State, state_cb)
local_pos_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), PoseStamped, position_cb)
alt_sub = rospy.Subscriber(mavros.get_topic('altitude'), Altitude, alt_cb)
imu_sub = rospy.Subscriber(mavros.get_topic('imu', 'data'), Imu, imu_cb)
input_sub = rospy.Subscriber('UserInput', String, input_cb)
laser_sub = rospy.Subscriber('scan', LaserScan, rplidar_cb)
CVA_sub = rospy.Subscriber('command_info_topic', command_info, command_info_cb)
########## Define Services
arming_client = rospy.ServiceProxy(mavros.get_topic('cmd', 'arming'), CommandBool)
set_mode_client = rospy.ServiceProxy(mavros.get_topic('set_mode'), SetMode)
targetHeight = 0.8 # Fly 1 meter high
geo_pos = GeoPointStamped()
geo_pos.position.latitude = 0
geo_pos.position.longitude = 0
geo_pos.position.altitude = 0
initial_pose = PoseStamped()
initial_pose.pose.position.x = 0
initial_pose.pose.position.y = 0
initial_pose.pose.position.z = 0
danger_zone = 1.0 # Dangerous Obstacle Distance
input_str = "ready" # Initialize input command
os.system('clear') # Clear screen
print('Connecting...')
def main():
# Load File
folder_path = '/home/dank-engine/3d_ws/json2_merger/'
path = list(pathlib.Path(folder_path).glob('*.json'))
id = 0
k = 0
num = 20
arr_rx = np.zeros(num)
arr_ry = np.zeros(num)
arr_lx = np.zeros(num)
arr_ly = np.zeros(num)
arr_cx = np.zeros(num)
arr_cy = np.zeros(num)
for i in range(100, 552):
filename = folder_path + '{i:0{width}}'.format(i=i + 1,
width=4) + '.json'
with open(filename, 'r') as f:
print(filename)
data = json.load(f)
# GPS Topic
gps_coor = GeoPointStamped()
gps_map = NavSatFix()
gps_pub = rospy.Publisher('/gps', GeoPointStamped, queue_size=10)
gps_map_pub = rospy.Publisher('/gps/fix', NavSatFix, queue_size=10)
#IMU Topic
imu_coor = Imu()
imu_pub = rospy.Publisher('/imu', Imu, queue_size=10)
#PointCloud Topic
cloud = PointCloud2()
pc_data = rospy.Publisher('/velodyne_points',
PointCloud2,
queue_size=32)
rospy.init_node('converter')
# Publish GPS Data in ROS
gps_coor.header.frame_id = "gps"
gps_coor.position.latitude = data["INS_SWIFT"]["Latitude"]
gps_coor.position.longitude = data["INS_SWIFT"]["Longitude"]
gps_coor.position.altitude = float('nan')
gps_map.header.frame_id = "base_link"
gps_map.latitude = data["INS_SWIFT"]["Latitude"]
gps_map.longitude = data["INS_SWIFT"]["Longitude"]
gps_map.altitude = data["INS_SWIFT"]["Altitude"]
gps_pub.publish(gps_coor)
gps_map_pub.publish(gps_map)
# Publish IMU Data in ROS
roll = data["INS_SWIFT"]["Roll"]
pitch = data["INS_SWIFT"]["Pitch"]
yaw = data["INS_SWIFT"]["Yaw"]
quaternion = tf.transformations.quaternion_from_euler(
roll, pitch, yaw)
imu_coor.header.frame_id = "imu"
imu_coor.header.stamp = rospy.get_rostime()
imu_coor.orientation.x = quaternion[0]
imu_coor.orientation.y = quaternion[1]
imu_coor.orientation.z = quaternion[2]
imu_coor.orientation.w = quaternion[3]
imu_coor.orientation_covariance = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
imu_pub.publish(imu_coor)
# Publish Point Cloud Data
point = []
scan = data["PointCloud"]
for i in range(len(scan)):
if (scan[i]["x"] != None):
xyzi = (scan[i]["x"], scan[i]["y"], scan[i]["z"],
scan[i]["intensity"])
point.append(xyzi)
else:
xyzi = (np.nan, np.nan, np.nan, 0)
point.append(xyzi)
cloud = xyzi_array_to_pointcloud2(point, rospy.get_rostime(),
"velodyne")
pc_data.publish(cloud)
lane_mark = rospy.Publisher('/marker_lane', Marker, queue_size=100)
marker_l = Marker()
marker_l.header.frame_id = "base_link"
marker_l.type = marker_l.TEXT_VIEW_FACING
marker_l.action = marker_l.ADD
marker_l.scale.x = 5
marker_l.scale.y = 5
marker_l.scale.z = 5
marker_l.color.a = 1.0
marker_l.color.r = 1.0
marker_l.color.g = 1.0
marker_l.color.b = 0.0
marker_l.pose.position.x = 0
marker_l.pose.position.y = 30
marker_l.pose.position.z = 1
marker_l.pose.orientation.w = 1.0
marker_l.text = "Lanes: " + str(
data["Lanes"]) + '\n' + "Width: " + str(
data["Width"]) + '\n' + "Slope_Hoizontal: " + str(
data["Slope"]["Slope_Hoizontal"])
marker_l.id = 0
lane_mark.publish(marker_l)
try:
r = rospy.Rate(1) # 10hz
lane_r = data["Right_Marker"]
lane_l = data["Left_Marker"]
lane_c = data["Center_Marker"]
publisher = rospy.Publisher('/drive_region',
MarkerArray,
queue_size=100)
arr_rx[k % num] = lane_r["x"]
arr_ry[k % num] = lane_r["y"]
arr_lx[k % num] = lane_l["x"]
arr_ly[k % num] = lane_l["y"]
arr_cx[k % num] = lane_c["x"]
arr_cy[k % num] = lane_c["y"]
# if k == 10:
markerArray = MarkerArray()
marker = Marker()
marker.header.frame_id = "base_link"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 2
marker.scale.y = 2
marker.scale.z = 2
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = np.average(arr_rx)
marker.pose.position.y = np.average(arr_ry)
marker.pose.position.z = 0
marker1 = Marker()
marker1.header.frame_id = "base_link"
marker1.type = marker.SPHERE
marker1.action = marker.ADD
marker1.scale.x = 2
marker1.scale.y = 2
marker1.scale.z = 2
marker1.color.a = 1.0
marker1.color.r = 1.0
marker1.color.g = 1.0
marker1.color.b = 0.0
marker1.pose.orientation.w = 1.0
marker1.pose.position.x = np.average(arr_lx)
marker1.pose.position.y = np.average(arr_ly)
marker1.pose.position.z = 0
marker2 = Marker()
marker2.header.frame_id = "base_link"
marker2.type = marker.SPHERE
marker2.action = marker.ADD
marker2.scale.x = 2
marker2.scale.y = 2
marker2.scale.z = 2
marker2.color.a = 1.0
marker2.color.r = 0.0
marker2.color.g = 0.0
marker2.color.b = 1.0
marker2.pose.orientation.w = 1.0
marker2.pose.position.x = np.average(arr_cx)
marker2.pose.position.y = np.average(arr_cy)
marker2.pose.position.z = 0
markerArray.markers.append(marker)
markerArray.markers.append(marker1)
markerArray.markers.append(marker2)
# Renumber the marker IDs
for m in markerArray.markers:
m.id = id
id += 1
# Publish the MarkerArray
publisher.publish(markerArray)
# k = 0
k = k + 1
r.sleep()
except KeyError:
r = rospy.Rate(1) # 10hz
r.sleep()