def to_message(self):
wp = WaypointMessage()
wp.point.x = self._x
wp.point.y = self._y
wp.point.z = self._z
wp.max_forward_speed = self._max_forward_speed
wp.use_fixed_heading = self._use_fixed_heading
wp.heading_offset = self._heading_offset
return wp
def to_message(self):
wp = WaypointMessage()
wp.point.x = self._x
wp.point.y = self._y
wp.point.z = self._z
wp.max_forward_speed = self._max_forward_speed
wp.use_fixed_heading = self._use_fixed_heading
wp.heading_offset = self._heading_offset
wp.header.frame_id = self._inertial_frame_id
return wp
def to_message(self):
wp = WaypointMessage()
wp.point.x = self._x
wp.point.y = self._y
wp.point.z = self._z
wp.max_forward_speed = self._max_forward_speed
wp.use_fixed_heading = self._use_fixed_heading
wp.heading_offset = self._heading_offset
wp.header.frame_id = self._inertial_frame_id
wp.radius_of_acceptance = self._radius_acceptance
return wp
def to_message(self):
"""Convert waypoint to `uuv_control_msgs/Waypoint` message
> *Returns*
`uuv_control_msgs/Waypoint` message
"""
wp = WaypointMessage()
wp.point.x = self._x
wp.point.y = self._y
wp.point.z = self._z
wp.max_forward_speed = self._max_forward_speed
wp.use_fixed_heading = self._use_fixed_heading
wp.heading_offset = self._heading_offset
wp.header.frame_id = self._inertial_frame_id
wp.radius_of_acceptance = self._radius_acceptance
return wp
def self_pose_callback(data):
global wp_list
if len(wp_list) > 1:
# reached the first waypoint in the waypoint list
# going to the next waypoint
if sqrt(
pow(data.pose.pose.position.x - wp_list[0].point.x, 2) +
pow(data.pose.pose.position.y - wp_list[0].point.y, 2) +
pow(data.pose.pose.position.z - wp_list[0].point.z, 2)) <= 0.1:
wp_list.pop(0)
try:
go_to = rospy.ServiceProxy('go_to', GoTo)
except rospy.ServiceException as e:
raise rospy.ROSException(
'Service call for go to failed, error=%s', str(e))
wp = wp_list[0]
response = go_to(wp, max_forward_speed, interpolator)
if not response:
rospy.loginfo('Failed to start a new trajectory')
else:
rospy.loginfo('Started a new trajectory')
# try:
# init_wp = rospy.ServiceProxy(
# '/%s/start_waypoint_list' % uuv_name,
# InitWaypointSet)
# except rospy.ServiceException as e:
# raise rospy.ROSException('Service call for initialize waypoint set failed, error=%s', str(e))
# success = init_wp(Time(rospy.Time.from_sec(rospy.Time.now().to_sec())),
# True,
# wp_list,
# max_forward_speed,
# heading_offset,
# String(interpolator))
# if success:
# rospy.loginfo(len(wp_list))
# else:
# rospy.loginfo('Failed to remove an old waypoint')
elif len(wp_list) == 0:
# have an empty waypoint list
# want to first add the initial position of the uuv
wp = Waypoint()
wp.max_forward_speed = max_forward_speed
wp.heading_offset = heading_offset
wp.use_fixed_heading = use_fixed_heading
wp.radius_of_acceptance = radius_of_acceptance
wp.header.frame_id = 'world'
wp.point = data.pose.pose.position
wp_list.append(wp)
def parse_request(mis, utm_frame_id="utm", radius_of_acceptance=5.0):
with open(mis, "r") as f:
while not f.readline().startswith("START"):
pass
geopoints = []
headings = []
speeds = []
line = f.readline().rstrip()
while not line.startswith("END"):
values = line.split(";")
values = [value.strip() for value in values]
params = parse_iver_format(values[5])
zvar = -float(params[0][1:]) * 0.3048 if params[0][
0] == 'D' else float(params[0][1:]) * 0.3048
geopoints.append(GeoPoint(float(values[1]), float(values[2]),
zvar))
headings.append(float(values[4]))
speeds.append(float(params[-1][1:]) * 0.5144)
line = f.readline().rstrip()
# while not f.readline().startswith("START VEHICLE"):
# pass
# line = f.readline().rstrip()
# while not line.startswith("END VEHICLE"):
# if line.startswith("UVC=WPRadius"):
# roas.append(float(line.split("=")[-1]))
# break
req = ConvertGeoPointsRequest()
req.geopoints = geopoints
p = rospy.ServiceProxy("convert_points", ConvertGeoPoints)
p.wait_for_service()
res = p(req)
wps = []
for point, heading, speed in zip(res.utmpoints, headings, speeds):
wp = Waypoint()
wp.point.x = point.x
wp.point.y = point.y
wp.point.z = point.z
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = utm_frame_id
wp.radius_of_acceptance = radius_of_acceptance
wp.max_forward_speed = speed
wp.use_fixed_heading = False
wp.heading_offset = 0.0
wps.append(wp)
req = InitWaypointSetRequest()
req.start_time = Time(rospy.Time.from_sec(0))
req.start_now = True
req.waypoints = wps
req.max_forward_speed = max(speeds)
req.interpolator = String("lipb")
req.heading_offset = 0
req.max_forward_speed = max(speeds)
req.waypoints = wps
return req
def pose_delay_callback(data, event):
global wp_list
wp = Waypoint()
wp.max_forward_speed = max_forward_speed
wp.heading_offset = heading_offset
wp.use_fixed_heading = use_fixed_heading
wp.radius_of_acceptance = radius_of_acceptance
wp.header.frame_id = 'world'
wp.point = data.pose.pose.position
wp.point.x += x_shift
wp.point.y += y_shift
wp.point.z += z_shift
wp_list.append(wp)
def pose_delay_callback(data, event):
wp = Waypoint()
wp.max_forward_speed = max_forward_speed
wp.heading_offset = heading_offset
wp.use_fixed_heading = use_fixed_heading
wp.radius_of_acceptance = radius_of_acceptance
wp.header.frame_id = 'world'
wp.point = data.pose.pose.position
try:
go_to = rospy.ServiceProxy('go_to', GoTo)
except rospy.ServiceException as e:
raise rospy.ROSException('Service call for go to failed, error=%s',
str(e))
response = go_to(wp, max_forward_speed, interpolator)
if not response:
rospy.loginfo('Failed to start a new trajectory')
else:
rospy.loginfo('Started a new trajectory')
def _parse_plan(self):
# TODO this parser only accepts a set of goto commands
"""A plan has been requested by neptus to start. Parse the plan here and send to controller."""
geopoints = []
speeds = []
is_depths = []
for maneuver in self._current_plan.plan_spec.maneuvers:
geopoints.append(
GeoPoint(maneuver.maneuver.lat * 180.0 / np.pi,
maneuver.maneuver.lon * 180.0 / np.pi,
maneuver.maneuver.z))
speeds.append(maneuver.maneuver.speed)
is_depths.append(maneuver.maneuver.z_units == 1)
req = ConvertGeoPointsRequest()
req.geopoints = geopoints
points = self._geo_converter.call(req).utmpoints
wps = []
# If datum is available, then reference frame is ENU World
for point, speed, is_depth in zip(points, speeds, is_depths):
wp = Waypoint()
wp.point.x = point.x
wp.point.y = point.y
wp.point.z = -abs(point.z) if is_depth else abs(point.z)
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = "utm"
wp.radius_of_acceptance = 1.0
wp.max_forward_speed = speed
wp.use_fixed_heading = False
wp.heading_offset = 0.0
wps.append(wp)
req = InitWaypointSetRequest()
req.start_time = Time(rospy.Time.from_sec(0))
req.start_now = True
req.waypoints = wps
req.max_forward_speed = max(speeds)
req.interpolator.data = "lipb"
req.heading_offset = 0
self._send_goal(req)
def sender(p):
try:
rospy.wait_for_service(rospy.get_namespace() + 'go_to', timeout=2)
except rospy.ROSException:
rospy.ROSException('Service not available! Closing node...')
try:
init_wp = rospy.ServiceProxy(rospy.get_namespace() + '/go_to', GoTo)
except rospy.ServiceException, e:
rospy.ROSException('Service call failed, error=' + e)
p.z = -20
# don't bump into boxbox
success = init_wp(Waypoint(p, 0.4, 0, False))
if success:
print 'Waypoint command successfully sent'
else:
print 'Failed'
if __name__ == '__main__':
print 'Send a waypoint, namespace=', rospy.get_namespace()
rospy.init_node('move_to_resource.py')
if rospy.is_shutdown():
print 'ROS master not running!'
sys.exit(-1)
def _parse_plan(self):
# TODO this parser only accepts a set of goto commands
"""A plan has been requested by neptus to start. Parse the plan here and send to controller."""
geopoints = []
speeds = []
for maneuver in self._current_plan.plan_spec.maneuvers:
geopoints.append(
GeoPoint(maneuver.maneuver.lat * 180.0 / np.pi,
maneuver.maneuver.lon * 180.0 / np.pi,
maneuver.maneuver.z))
speeds.append(maneuver.maneuver.speed)
if self._datum is not None:
geopoints.append(self._datum)
req = ConvertGeoPointsRequest()
req.geopoints = geopoints
points = self._geo_converter.call(req).utmpoints
wps = []
# If datum is available, then reference frame is ENU World
if self._datum is not None:
for point, speed in zip(points[:-1], speeds):
wp = Waypoint()
wp.point = Point(point.x - points[-1].x,
point.y - points[-1].y,
-point.z - points[-1].z)
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = "world"
wp.radius_of_acceptance = 3.0
wp.max_forward_speed = speed
wp.use_fixed_heading = False
wp.heading_offset = 0.0
wps.append(wp)
# If datum is not given, then reference frame is ENU UTM TODO: LOOKUP TRANSFORM
else:
for point, speed in zip(points, speeds):
wp = Waypoint()
wp.point.x = point.x
wp.point.y = point.y
wp.point.z = -point.z
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = "utm"
wp.radius_of_acceptance = 3.0
wp.max_forward_speed = speed
wp.use_fixed_heading = False
wp.heading_offset = 0.0
wps.append(wp)
self.STATE = PlanControlState.READY
req = InitWaypointSetRequest()
req.start_time = Time(rospy.Time.from_sec(0))
req.start_now = True
req.waypoints = wps
req.max_forward_speed = max(speeds)
req.interpolator.data = "lipb"
req.heading_offset = 0
self._waypoint_setter.wait_for_service()
if self._waypoint_setter(Time(rospy.Time.from_sec(0.0)), True, wps,
max(speeds), 0.0, String("lipb")):
self.STATE = PlanControlState.EXECUTING
else:
self.STATE = PlanControlState.FAILURE