def perception_publish(self, type, frame_id):
try:
(trans, rot) = tf_listener.lookupTransform(frame_id, "base_link",
rospy.Time(0))
inFormat = pyproj.Proj("+init=EPSG:4326")
zeroMerc = pyproj.Proj(
"+proj=tmerc +lon_0={} +lat_0={} +units=m".format(
-157.8901, 21.30996))
lon, lat = pyproj.transform(zeroMerc, inFormat, trans[0], trans[1])
message = GeoPoseStamped()
message.pose.position.latitude = lat
message.pose.position.longitude = lon
message.header.frame_id = type
p_pub.publish(message)
print("PUBLISHED OBJECT")
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
inFormat = pyproj.Proj("+init=EPSG:4326")
zeroMerc = pyproj.Proj(
"+proj=tmerc +lon_0={} +lat_0={} +units=m".format(
-157.8901, 21.30996))
lon, lat = pyproj.transform(zeroMerc, inFormat, trans[0], trans[1])
message = GeoPoseStamped()
message.pose.position.latitude = lat
message.pose.position.longitude = lon
message.header.frame_id = type
p_pub.publish(message)
print("PUBLISHED OBJECT")
def publish_landmark_lla_info(self, landmark_label, landmark_pose_lla, header):
landmark_lla_msg = GeoPoseStamped()
landmark_lla_msg.header = header
landmark_lla_msg.header.frame_id = landmark_label
landmark_lla_msg.pose.position.latitude = landmark_pose_lla[0]
landmark_lla_msg.pose.position.longitude = landmark_pose_lla[1]
landmark_lla_msg.pose.position.altitude = landmark_pose_lla[2]
self.landmark_lla_pub.publish(landmark_lla_msg)
def execute(self, userdata):
task = self.missionManager.getCurrentTask()
if task is not None:
if self.missionManager.planner == 'path_follower':
goal = path_follower.msg.path_followerGoal()
elif self.missionManager.planner == 'path_planner':
goal = path_planner.msg.path_plannerGoal()
goal.path.header.stamp = rospy.Time.now()
if task['type'] == 'goto':
path = task['path']
if task['type'] == 'mission_plan':
if task['transit_path'] is not None:
path = task['transit_path']
else:
path = task['current_path']
for s in path:
#print s
gpose = GeoPoseStamped()
gpose.pose = s
goal.path.poses.append(gpose)
goal.speed = task['default_speed']
self.task_complete = False
if self.missionManager.planner == 'path_follower':
self.path_planner_client.cancel_goal()
self.path_follower_client.wait_for_server()
self.path_follower_client.send_goal(
goal, self.path_follower_done_callback,
self.path_follower_active_callback,
self.path_follower_feedback_callback)
elif self.missionManager.planner == 'path_planner':
self.path_follower_client.cancel_goal()
self.path_planner_client.wait_for_server()
self.path_planner_client.send_goal(
goal, self.path_follower_done_callback,
self.path_follower_active_callback,
self.path_follower_feedback_callback)
while True:
ret = self.missionManager.iterate('FollowPath')
if ret is not None:
if ret == 'cancelled':
if self.missionManager.planner == 'path_follower':
self.path_follower_client.cancel_goal()
elif self.missionManager.planner == 'path_planner':
self.path_planner_client.cancel_goal()
return ret
if self.task_complete:
return 'done'
def inspect_wt(self, duration=rospy.Duration(900, 0)):
"""
UAV Inspection for a WT that involves flying close and parallel
to each blade of a WT
"""
start = rospy.Time.now()
# position where drone is originated in one of the wps
original = GeoPoseStamped()
original.pose.position.latitude = self.global_pose.latitude
original.pose.position.longitude = self.global_pose.longitude
original.pose.position.altitude = self.rel_alt
original.pose.orientation = self.odometry.pose.pose.orientation
relative_altitude = self.rel_alt
rospy.loginfo("Scan first blade...")
first_blade = self.blade_inspect(original, relative_altitude,
[0.000, 0.0006, 0.0000], duration)
duration = duration - (rospy.Time.now() - start)
start = rospy.Time.now()
rospy.loginfo("Scan second blade...")
second_blade = self.blade_inspect(original, relative_altitude, [
0.000, 0.0006 * np.cos(138. / 180.0 * np.pi),
85.6 * np.sin(138. / 180.0 * np.pi)
], duration)
duration = duration - (rospy.Time.now() - start)
start = rospy.Time.now()
rospy.loginfo("Scan third blade...")
third_blade = self.blade_inspect(original, relative_altitude, [
0.000, 0.0006 * np.cos(234. / 180.0 * np.pi),
85.6 * np.sin(234. / 180.0 * np.pi)
], duration)
rospy.loginfo("Inspection is done...")
return np.min([first_blade, second_blade, third_blade])
def line_string_to_geo_path(cls, line_string: LineString) -> GeoPath:
geo_path = GeoPath()
for point in line_string.coords:
geo_pose_stamped = GeoPoseStamped()
geo_pose_stamped.pose.position = cls.tuple_to_geo_point(point)
geo_path.poses.append(geo_pose_stamped)
return geo_path
def utm_to_wgs84_pose(utm_pose, vehicle_gps_fix):
zone, band = gridZone(vehicle_gps_fix.latitude, vehicle_gps_fix.longitude)
utm_point = UTMPoint(utm_pose.pose.position.x, utm_pose.pose.position.y,
utm_pose.pose.position.z, zone, band)
geo_pose = GeoPoseStamped()
geo_pose.header.stamp = utm_pose.header.stamp
geo_pose.header.frame_id = "wgs84"
geo_pose.pose.position = utm_point.toMsg()
geo_pose.pose.orientation = utm_pose.pose.orientation
return geo_pose
def blade_inspect(self,
original,
relative_altitude,
target_position,
duration=rospy.Duration(300, 0)):
"""
Inspecting the blade given the [original] pose to return to
and end [target] position to scan
"""
start = rospy.Time.now()
reached_original = self.ACTION_FAIL
# position where drone is supposed to be
quaternion = [
self.odometry.pose.pose.orientation.x,
self.odometry.pose.pose.orientation.y,
self.odometry.pose.pose.orientation.z,
self.odometry.pose.pose.orientation.w
]
uav_orientation = tf.transformations.euler_from_quaternion(quaternion,
axes='sxyz')
latitude = self.global_pose.latitude + (
target_position[0] * np.sin(uav_orientation[2]) +
target_position[1] * np.cos(uav_orientation[2]))
longitude = self.global_pose.longitude + (
target_position[0] * np.cos(uav_orientation[2]) +
target_position[1] * np.sin(uav_orientation[2]))
altitude = self.rel_alt + target_position[2]
target = GeoPoseStamped()
target.header.seq = 1
target.header.stamp = rospy.Time.now()
target.header.frame_id = 'map'
target.pose.position.latitude = latitude
target.pose.position.longitude = longitude
target.pose.position.altitude = altitude
target.pose.orientation = self.odometry.pose.pose.orientation
duration = duration - (rospy.Time.now() - start)
start = rospy.Time.now()
reached_target = self.goto_coordinate(target, duration)
rospy.loginfo("Return to %3.5f, %3.5f position..." %
(original.pose.position.latitude,
original.pose.position.longitude))
if reached_target == self.ACTION_SUCCESS:
original.header.seq = 1
original.header.stamp = rospy.Time.now()
original.header.frame_id = 'map'
duration = duration - (rospy.Time.now() - start)
start = rospy.Time.now()
reached_original = self.goto_coordinate(original, duration)
if abs(self._rel_alt[-1] - relative_altitude) > 0.1:
original.pose.position.altitude = (
original.pose.position.altitude + relative_altitude -
self._rel_alt[-1])
reached_original = self.goto_coordinate(original, duration)
return np.min([reached_target, reached_original])
def cb(data):
for i in data.objects:
#Create a new geopose to publish
gp = GeoPoseStamped()
gp.header.stamp = rospy.Time.now()
gp.header.frame_id = allowed_strings[i.best_guess]
latlon = convertToLatLon(data.position.pose)
gp.pose.orientation = i.pose.orientation
gp.pose.position.latitude = latlon['latitude']
gp.pose.position.longitude = latlon['longitude']
gp.pose.position.altitude = latlon['altitude']
pub.publish(gp)
def testAutoOriginFromCustom(self):
rospy.init_node('test_auto_origin_from_custom')
custom_pub = rospy.Publisher('pose', GeoPoseStamped, queue_size=2, latch=True)
origin_sub = self.subscribeToOrigin()
self.test_stamp = True
custom_msg = GeoPoseStamped()
custom_msg.pose.position.latitude = swri['latitude']
custom_msg.pose.position.longitude = swri['longitude']
custom_msg.pose.position.altitude = swri['altitude']
custom_msg.header.frame_id = "/far_field"
custom_msg.header.stamp = msg_stamp
custom_pub.publish(custom_msg)
rospy.spin()
self.assertTrue(self.got_origin)
def cb(data):
for i in data.objects:
#lookup relative to map
position,q=listener.lookupTransform(i.frame_id,"map",rospy.Time(0))
#Create a new geopose to publish
gp=GeoPoseStamped()
gp.header.stamp=rospy.Time.now()
gp.header.frame_id=allowed_strings[i.best_guess]
latlon=convertToLatLon(position)# OR i.pose.position if that works without transform
gp.pose.orientation=i.pose.orientation
gp.pose.position.latitude=latlon['latitude']
gp.pose.position.longitude=latlon['longitude']
gp.pose.position.altitude=latlon['altitude']
pub.publish(gp)
def __init__(self, mav_name):
self.rate = rospy.Rate(60)
self.desired_state = ""
self.drone_pose = PoseStamped()
self.goal_pose = PoseStamped()
self.pose_target = PositionTarget()
self.goal_vel = TwistStamped()
self.drone_state = State()
self.battery = BatteryState()
self.global_pose = NavSatFix()
self.gps_target = GeoPoseStamped()
############# Services ##################
self.arm = rospy.ServiceProxy(mavros_arm, CommandBool)
self.set_mode_srv = rospy.ServiceProxy(mavros_set_mode, SetMode)
############### Publishers ##############
self.local_position_pub = rospy.Publisher(mavros_local_position_pub, PoseStamped, queue_size = 20)
self.velocity_pub = rospy.Publisher(mavros_velocity_pub, TwistStamped, queue_size=5)
self.target_pub = rospy.Publisher(mavros_pose_target_sub, PositionTarget, queue_size=5)
self.global_position_pub = rospy.Publisher(mavros_set_global_pub, GeoPoseStamped, queue_size= 20)
########## Subscribers ##################
self.local_atual = rospy.Subscriber(mavros_local_atual, PoseStamped, self.local_callback)
self.state_sub = rospy.Subscriber(mavros_state_sub, State, self.state_callback, queue_size=10)
self.battery_sub = rospy.Subscriber(mavros_battery_sub, BatteryState, self.battery_callback)
self.global_position_sub = rospy.Subscriber(mavros_global_position_sub, NavSatFix, self.global_callback)
self.extended_state_sub = rospy.Subscriber(extended_state_sub, ExtendedState, self.extended_state_callback, queue_size=2)
self.LAND_STATE = ExtendedState.LANDED_STATE_UNDEFINED # landing state
'''
LANDED_STATE_UNDEFINED = 0
LANDED_STATE_ON_GROUND = 1
LANDED_STATE_IN_AIR = 2
LANDED_STATE_TAKEOFF = 3
LANDED_STATE_LANDING = 4
'''
service_timeout = 30
rospy.loginfo("waiting for ROS services")
try:
rospy.wait_for_service('mavros/param/get', service_timeout)
rospy.wait_for_service('mavros/cmd/arming', service_timeout)
rospy.wait_for_service('mavros/set_mode', service_timeout)
rospy.loginfo("ROS services are up")
except rospy.ROSException:
rospy.logerr("failed to connect to services")
def announce_object(self, obj_id, classification, position_enu, boat_position_enu):
if classification == 'UNKNOWN':
self.send_feedback('Ignoing UNKNOWN object {}'.format(obj_id))
defer.returnValue(False)
if obj_id in self.announced:
defer.returnValue(False)
if np.linalg.norm(position_enu - boat_position_enu) < 3.5:
self.send_feedback('Ignoring {} b/c its too close'.format(obj_id))
defer.returnValue(False)
geo_point = yield self.enu_position_to_geo_point(position_enu)
msg = GeoPoseStamped()
msg.header.frame_id = classification
msg.pose.position = geo_point
self.perception_landmark.publish(msg)
defer.returnValue(True)
def setUp(self):
super(MavrosOffboardPosctl, self).setUp()
self.pos = GeoPoseStamped()
self.pos_setpoint_pub = rospy.Publisher(
'mavros/setpoint_position/global', GeoPoseStamped, queue_size=1)
# send setpoints in seperate thread to better prevent failsafe
self.pos_thread = Thread(target=self.send_pos, args=())
self.pos_thread.daemon = True
self.pos_thread.start()
self.wait_for_topics(60)
self.wait_for_landed_state(mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, 10, -1)
self.set_mode("OFFBOARD", 5)
self.set_arm(True, 5)
rospy.loginfo("setup complete")
def set_up(self, uav_id=-1):
rospy.init_node('offboard', anonymous=True)
super(MultiMavrosOffboardPosctl, self).set_up(uav_id)
if uav_id != -1:
self.namespace = 'uav' + str(uav_id) + '/'
else:
self.namespace = ''
self.pos = GeoPoseStamped()
self.pos_setpoint_pub = rospy.Publisher(
self.namespace + 'mavros/setpoint_position/global',
GeoPoseStamped,
queue_size=1)
# send setpoints in seperate thread to better prevent failsafe
self.pos_thread = Thread(target=self.send_pos, args=())
self.pos_thread.daemon = True
self.pos_thread.start()
def setUp(self):
super(MavrosOffboardPosctl, self).setUp()
# Socket receive readings from the server.
context = zmq.Context()
self.socket = context.socket(zmq.SUB, )
rospy.loginfo("connecting to server")
self.socket.connect("tcp://{}:{}".format(IP_ADDRESS, PORT))
self.socket.setsockopt(zmq.SUBSCRIBE, b"")
self.recv_attempts = 0
self.pos = GeoPoseStamped()
self.radius = 1
self.pos_setpoint_pub = rospy.Publisher(
'mavros/setpoint_position/global', GeoPoseStamped, queue_size=1)
# send setpoints in seperate thread to better prevent failsafe
self.pos_thread = Thread(target=self.send_pos, args=())
self.pos_thread.daemon = True
self.pos_thread.start()
import rospy
from geographic_msgs.msg import GeoPoseStamped
if __name__ == '__main__':
rospy.init_node('waypoint_pub')
way_pub = rospy.Publisher('/waypoints', GeoPoseStamped, queue_size = 1)
waypoint_lat = 44.566881
waypoint_lon = -123.275972
while not rospy.is_shutdown():
pose = GeoPoseStamped()
pose.header.stamp = rospy.get_rostime()
pose.header.frame_id = 'world'
pose.pose.position.latitude = waypoint_lat
pose.pose.position.longitude = waypoint_lon
pose.pose.position.altitude = float('NaN')
way_pub.publish(pose)
waypoint_lat += 0.0001
waypoint_lon += 0.0001
rospy.sleep(0.5)
def publish(self, object):
#print("perception_PUBLISHER HAS OBJECT")
message = GeoPoseStamped()
message.header.frame_id = object.best_guess
self.pub.publish(message)
print("PUBLISHED OBJECT")
import actionlib
import rospy
from geographic_msgs.msg import GeoPoseStamped
from mimree_executive.msg import FindTurbineOdomGoal, FindTurbineOdomAction
# print("FINISHED IMPORTS")
minute = 60
if __name__ == '__main__':
print("INITIATING")
rospy.init_node('find_turbine_odom_client')
client = actionlib.SimpleActionClient('find_turbine_odom',
FindTurbineOdomAction)
client.wait_for_server()
print("SERVER CONNECTED")
goal = FindTurbineOdomGoal()
goal.turbine_estimated_position = GeoPoseStamped()
# Test coordinates, flies near turbine
# latitude: 43.7993805
# longitude: 28.5936549
# altitude: 90.0
goal.turbine_estimated_position.pose.position.latitude = 43.7993805
goal.turbine_estimated_position.pose.position.longitude = 28.5936549
goal.turbine_estimated_position.pose.position.altitude = 90
goal.turbine_estimated_position.pose.orientation.z = 0.70710678
goal.turbine_estimated_position.header.frame_id = 'map'
goal.turbine_estimated_position.header.stamp.secs = 8070
goal.turbine_estimated_position.header.stamp.nsecs = 0
goal.turbine_estimated_position.header.seq = 1
goal.uav_namespace = 'hector'
print("GOAL CREATED: ", goal)
space key, s : force stop
CTRL-C to quit
"""
e = """
Communications Failed
"""
"""
global variable
"""
gps_curr = NavSatFix()
gps_pub_msg = GlobalPositionTarget()
gps_pub_geo_msg = GeoPoseStamped()
local_curr_pose = PoseStamped()
uav_state = State()
ismove = Bool()
gps_list = list()
xy_list = list()
gps_xy_file = open("/home/wzy/catkin_ws/src/gps_control/scripts/gps_xy_file.txt", "a", buffering=1000)
ismove = False
def getKey():
if os.name == 'nt':
return msvcrt.getch()
tty.setraw(sys.stdin.fileno())
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
if rlist:
def parseTask(self,task):
response = TriggerResponse()
# IF THIS IS A PLAN THAT REQUIRES SUPERVISION
if self._aware:
# if the task isn't the starting task.
if task.action!='START':
# if the energy cost mu or std = 0, then this task is buggy.
if task.cost_mu==0 or task.cost_std ==0:
rospy.logwarn("Buggy task, skipping. {}".format(task))
self.plan.skipTask()
return self.parseTask(self.plan.getTask())
# GET WAYPOINT COMPONENTS READY TO SEND TO THE GUIDANCE NODE
geo_msg = GeoPoseStamped()
geo_msg.header.frame_id="utm"
geo_msg.header.stamp = rospy.Time.now()
self.checkValidTask(task)
# Look at the action property and decide on what to do from there:
if task.action == "WP":
# waypoint task, configure for AP mode to a GPS waypoint.
if self.changeMode("/tau_com/AP"):
geo_msg.pose.position.altitude=-1
geo_msg.pose.position.latitude=task.data[0]
geo_msg.pose.position.longitude=task.data[1]
geo_msg.pose.orientation = Quaternion(0,0,0,1)
self.task_pub.publish(geo_msg)
# turn off the timing lock
self._timing_lock = False
response.success = True
response.message = "Waypoint Task Selected, executing"
return response
else:
response.success = False
response.message = "Mode not configured to AP."
return response
elif task.action == "HP":
# hold position task, configure DP mode at a GPS waypoint and orientation for a set time.
if self.changeMode("/tau_com/DP"):
geo_msg.pose.position.altitude=0
geo_msg.pose.position.latitude=task.data[0]
geo_msg.pose.position.longitude=task.data[1]
geo_msg.pose.orientation = Quaternion(*tf.quaternion_from_euler(0,0,task.data[2]))
self.hptime = task.data[3]
self.hptask = True
self.task_pub.publish(geo_msg)
# turn off the timing lock
self._timing_lock = False
response.success = True
response.message ="Hold Pose Task Selected, executing."
return response
else:
response.success = False
response.message = "Mode not configured to DP."
return response
elif task.action == "ROOT":
# root task reached, notify the operator that the mission is complete.
if self.changeMode("__none"):
self.mode="idle"
self._timing_lock = False
self.status_pub.publish("ASV currently in mode: {}".format(self.mode))
rospy.loginfo("Final task completed, now idling.")
response.success = True
response.message = "Final task completed, now idling."
return response
else:
rospy.logerr("Couldn't disable output topic, but mission complete.")
response.success = False
response.message = "Couldn't Idle for some reason?"
return response
elif task.action == "START":
# start task, is a waypoint task to move the vehicle to the starting position.
if self.changeMode("/tau_com/AP"):
self._start_flag = True
geo_msg.pose.position.altitude=-1
geo_msg.pose.position.latitude=task.data[0]
geo_msg.pose.position.longitude=task.data[1]
geo_msg.pose.orientation = Quaternion(0,0,0,1)
self.task_pub.publish(geo_msg)
self._timing_lock = False
rospy.loginfo("Moving to starting waypoint of plan.")
response.success = True
response.message = "AP mode to starting point."
return response
else:
rospy.logerr("Couldn't configure to AP mode.")
response.success = False
response.message = "Couldn't configure to AP mode."
return response
elif task.action == "HOME":
# home task, is a waypoint task to move the vehicle to the starting position.
if self.changeMode("/tau_com/AP"):
geo_msg.pose.position.altitude=-1
geo_msg.pose.position.latitude=task.data[0]
geo_msg.pose.position.longitude=task.data[1]
geo_msg.pose.orientation = Quaternion(0,0,0,1)
self.task_pub.publish(geo_msg)
self._timing_lock = False
rospy.loginfo("Moving to rendezvous point.")
response.success = True
response.message = "AP mode to home point."
return response
else:
rospy.logerr("Couldn't configure to AP mode.")
response.success = False
response.message = "Couldn't configure to AP mode."
return response
else:
rospy.logerr("Task of type {} is not supported, getting next task.".format(task.action))
self.plan.skipTask()
return self.parseTask(self.plan.getTask())
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()