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