Beispiel #1
0
def send_position_goal(stateRefPub, position):

    stateRef = NavSts()
    stateRef.header.seq = 0
    stateRef.header.stamp.secs = 0
    stateRef.header.stamp.nsecs = 0
    stateRef.global_position.latitude = 0.0
    stateRef.global_position.longitude = 0.0
    stateRef.origin.latitude = 0.0
    stateRef.origin.longitude = 0.0
    stateRef.position.north = position.north
    stateRef.position.east = position.east
    stateRef.position.depth = position.depth
    stateRef.altitude = 0.0
    stateRef.body_velocity.x = 0
    stateRef.body_velocity.y = 0
    stateRef.body_velocity.z = 0
    stateRef.gbody_velocity.x = 0
    stateRef.gbody_velocity.y = 0
    stateRef.gbody_velocity.z = 0
    stateRef.orientation.roll = 0
    stateRef.orientation.pitch = 0
    stateRef.orientation.yaw = 0
    stateRef.orientation_rate.roll = 0
    stateRef.orientation_rate.pitch = 0
    stateRef.orientation_rate.yaw = 0
    stateRef.position_variance.north = 0
    stateRef.position_variance.east = 0
    stateRef.position_variance.depth = 0
    stateRef.orientation_variance.roll = 0
    stateRef.orientation_variance.pitch = 0
    stateRef.orientation_variance.yaw = 0
    stateRef.status = 0

    send_state_ref(stateRefPub, stateRef)
Beispiel #2
0
    def odometry_cb(self, odom):
        quaternion = (odom.pose.pose.orientation.x,
                      odom.pose.pose.orientation.y,
                      odom.pose.pose.orientation.z,
                      odom.pose.pose.orientation.w)
        euler = tf.transformations.euler_from_quaternion(quaternion)

        msg = NavSts()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = 'map'

        llh = self.ned.ned2geodetic(
            [odom.pose.pose.position.x, odom.pose.pose.position.y, 0])

        msg.global_position.latitude = llh[0]
        msg.global_position.longitude = llh[1]
        msg.position.north = odom.pose.pose.position.x
        msg.position.east = odom.pose.pose.position.y
        msg.position.depth = 0.0
        msg.altitude = 0.0
        msg.body_velocity.x = odom.twist.twist.linear.x
        msg.body_velocity.y = odom.twist.twist.linear.y
        msg.body_velocity.z = odom.twist.twist.linear.z
        msg.orientation.roll = euler[0]
        msg.orientation.pitch = euler[1]
        msg.orientation.yaw = euler[2]
        msg.origin.latitude = self.origin_latitude
        msg.origin.longitude = self.origin_longitude

        self.xiroi_pose_pub.publish(msg)
        msg.header.seq = msg.header.seq + 1
def send_position_goal(stateRefPub, position):

    stateRef = NavSts()
    stateRef.header.seq = 0
    stateRef.header.stamp.secs = 0
    stateRef.header.stamp.nsecs = 0
    stateRef.global_position.latitude = 0.0
    stateRef.global_position.longitude = 0.0
    stateRef.origin.latitude = 0.0
    stateRef.origin.longitude = 0.0
    stateRef.position.north = position.north
    stateRef.position.east = position.east
    stateRef.position.depth = position.depth
    stateRef.altitude = 0.0
    stateRef.body_velocity.x = 0
    stateRef.body_velocity.y = 0
    stateRef.body_velocity.z = 0
    stateRef.gbody_velocity.x = 0
    stateRef.gbody_velocity.y = 0
    stateRef.gbody_velocity.z = 0
    stateRef.orientation.roll = 0
    stateRef.orientation.pitch = 0
    stateRef.orientation.yaw = 0
    stateRef.orientation_rate.roll = 0
    stateRef.orientation_rate.pitch = 0
    stateRef.orientation_rate.yaw = 0
    stateRef.position_variance.north = 0
    stateRef.position_variance.east = 0
    stateRef.position_variance.depth = 0
    stateRef.orientation_variance.roll = 0
    stateRef.orientation_variance.pitch = 0
    stateRef.orientation_variance.yaw = 0
    stateRef.status = 0
    
    send_state_ref(stateRefPub, stateRef)
Beispiel #4
0
 def __init__(self):
     QtCore.QObject.__init__(self)
     self._subscribers = []
     self._stateRef = NavSts()
     self._stateHat = NavSts()
     
     self.use_dp = True
     self.manager = HLManager()
def send_depth_goal(stateRefPub, position):

    try:
        depth_enable = rospy.ServiceProxy('DEPTH_enable', EnableControl)
        depth_enable(enable=True)

        velcon_enable = rospy.ServiceProxy('VelCon_enable', EnableControl)
        velcon_enable(enable=True)

        config_vel_controller = rospy.ServiceProxy(
            'ConfigureVelocityController', ConfigureVelocityController)
        config_vel_controller(ControllerName="DEPTH",
                              desired_mode=[0, 0, 2, 0, 0, 0])

        stateRef = NavSts()
        stateRef.header.seq = 0
        stateRef.header.stamp.secs = 0
        stateRef.header.stamp.nsecs = 0
        stateRef.global_position.latitude = 0.0
        stateRef.global_position.longitude = 0.0
        stateRef.origin.latitude = 0.0
        stateRef.origin.longitude = 0.0
        stateRef.position.north = position.north
        stateRef.position.east = position.east
        stateRef.position.depth = position.depth
        stateRef.altitude = 0.0
        stateRef.body_velocity.x = 0
        stateRef.body_velocity.y = 0
        stateRef.body_velocity.z = 0
        stateRef.gbody_velocity.x = 0
        stateRef.gbody_velocity.y = 0
        stateRef.gbody_velocity.z = 0
        stateRef.orientation.roll = 0
        stateRef.orientation.pitch = 0
        stateRef.orientation.yaw = 0
        stateRef.orientation_rate.roll = 0
        stateRef.orientation_rate.pitch = 0
        stateRef.orientation_rate.yaw = 0
        stateRef.position_variance.north = 0
        stateRef.position_variance.east = 0
        stateRef.position_variance.depth = 0
        stateRef.orientation_variance.roll = 0
        stateRef.orientation_variance.pitch = 0
        stateRef.orientation_variance.yaw = 0
        stateRef.status = 0
        stateRefPub.publish(stateRef)
        return 0

    except rospy.ServiceException, e:
        rospy.logerr("Failed to call change depth action" % e)
        return -1
def send_depth_goal(stateRefPub, position):

    try:
        depth_enable = rospy.ServiceProxy('DEPTH_enable', EnableControl)
        depth_enable(enable=True)

        velcon_enable = rospy.ServiceProxy('VelCon_enable', EnableControl)
        velcon_enable(enable=True)
        
        config_vel_controller = rospy.ServiceProxy('ConfigureVelocityController', ConfigureVelocityController)
        config_vel_controller(ControllerName="DEPTH", desired_mode=[0, 0, 2, 0, 0, 0])

        stateRef = NavSts()
        stateRef.header.seq = 0
        stateRef.header.stamp.secs = 0
        stateRef.header.stamp.nsecs = 0
        stateRef.global_position.latitude = 0.0
        stateRef.global_position.longitude = 0.0
        stateRef.origin.latitude = 0.0
        stateRef.origin.longitude = 0.0
        stateRef.position.north = position.north
        stateRef.position.east = position.east
        stateRef.position.depth = position.depth
        stateRef.altitude = 0.0
        stateRef.body_velocity.x = 0
        stateRef.body_velocity.y = 0
        stateRef.body_velocity.z = 0
        stateRef.gbody_velocity.x = 0
        stateRef.gbody_velocity.y = 0
        stateRef.gbody_velocity.z = 0
        stateRef.orientation.roll = 0
        stateRef.orientation.pitch = 0
        stateRef.orientation.yaw = 0
        stateRef.orientation_rate.roll = 0
        stateRef.orientation_rate.pitch = 0
        stateRef.orientation_rate.yaw = 0
        stateRef.position_variance.north = 0
        stateRef.position_variance.east = 0
        stateRef.position_variance.depth = 0
        stateRef.orientation_variance.roll = 0
        stateRef.orientation_variance.pitch = 0
        stateRef.orientation_variance.yaw = 0
        stateRef.status = 0
        stateRefPub.publish(stateRef)
        return 0

    except rospy.ServiceException, e:
        rospy.logerr("Failed to call change depth action" % e)
        return -1
Beispiel #7
0
    def __init__(self, name):
        """ Initialize the class """
        # Init class vars
        self.name = name
        self.navigation = NavSts()
        self.set_pose_depth = 2.0
        self.set_pose_axis = [[True, True, True, True, True, True]]
        self.desired_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

        # Get config parameters
        self.get_config()

        # Publisher
        self.pub_world_waypoint_req = rospy.Publisher(
            "/cola2_control/world_waypoint_req",
            WorldWaypointReq,
            queue_size=2)

        # Subscriber
        rospy.Subscriber("/cola2_navigation/nav_sts",
                         NavSts,
                         self.update_nav_sts,
                         queue_size=1)

        # Timer
        rospy.Timer(rospy.Duration(0.1), self.set_pose)

        # Show message
        rospy.loginfo("%s: initialized", self.name)
Beispiel #8
0
    def __init__(self):
        self.stateHat = rospy.Subscriber("stateHat", NavSts, self.onState)
        self.trackState = rospy.Subscriber("trackState", NavSts, self.onTrack)
        self.orientationSub = rospy.Subscriber("orientation", NavSts,
                                               self.onOrientation)
        self.gotoState = rospy.Subscriber("gotoState", NavSts, self.onGoTo)
        self.hasGoto = False
        self.pub = rospy.Publisher("stateRef", NavSts)

        self.track = NavSts()
        self.orientation = NavSts()
        self.goto = NavSts()

        self.radius = 4
        self.maxAngle = 15.0 / 180.0 * numpy.pi
        self.Kr = 10
Beispiel #9
0
 def surface_cb(self, msg): 
 
     self.send_depth_goal(0.5)
     d = NavSts()
     d.position.north = self.position.north
     d.position.east = self.position.east
     d.header.frame_id = rospy.get_namespace().replace('/', '')
     self.dockingPub.publish(d)
    def __init__(self):
        #rospy.Subscriber("desiredPosition", VehiclePose, self.onRef)
        rospy.Subscriber("stateHat", NavSts, self.onStateHat)
        rospy.Subscriber("trackedNav", NavSts, self.onTrackedNav)
        rospy.Subscriber("tauAch", BodyForceReq, self.onWindupFlag)

        self.stateHat = NavSts()

        self.out = rospy.Publisher("nuRef", BodyVelocityReq)

        w = numpy.array(
            rospy.get_param("dynamic_positioning/closed_loop_freq"))

        self.Kp = numpy.array([[2 * w[0], 0], [0, 2 * w[1]]],
                              dtype=numpy.float32)
        self.Ki = numpy.array([[w[0] * w[0], 0], [0, w[1] * w[1]]],
                              dtype=numpy.float32)

        self.Kt = 2 * self.Ki

        #self.Kp = numpy.reshape(
        #            numpy.array(rospy.get_param("dynamic_positioning/Kp"),
        #                        numpy.float32), [2,2]);
        #self.Ki = numpy.reshape(
        #            numpy.array(rospy.get_param("dynamic_positioning/Ki"),
        #                        numpy.float32), [2,2]);
        self.Ts = rospy.get_param("dynamic_positioning/Ts")

        self.mode = rospy.get_param("dynamic_positioning/mode")

        self.max = numpy.array(rospy.get_param("dynamic_positioning/max"),
                               dtype=numpy.float32)

        self.internalState = numpy.array([0, 0], numpy.float32)
        self.state = numpy.array([0, 0], numpy.float32)
        self.desired = numpy.array([0, 0], numpy.float32)
        self.ff = numpy.array([0, 0], numpy.float32)
        self.windup = numpy.array([0, 0], dtype=numpy.int8)
        self.lastW = numpy.array([0, 0], dtype=numpy.int8)
        self.lastI = numpy.array([0, 0], numpy.float32)

        self.R = numpy.identity(2, numpy.float32)
        self.uk_1 = 0
        self.lastTime = rospy.Time.now()
        '''Backward suff'''
        self.lastP = numpy.array([0, 0])
        self.lastFF = numpy.array([0, 0])

        model_name = rospy.get_param("model_name")
        b = rospy.get_param(model_name + "/dynamics/damping")
        Betainv = numpy.array([[1.0 / b[0], 0], [0, 1.0 / b[1]]],
                              dtype=numpy.float32)
        cp = numpy.cos(numpy.pi / 4)
        sp = numpy.sin(numpy.pi / 4)
        allocM = numpy.array([[cp, cp, -cp, -cp], [sp, -sp, sp, -sp]])

        self.Bstar = 0.9 * numpy.dot(Betainv, allocM)
        self.tmax = 13 / (2 * cp)
Beispiel #11
0
    def battery_level_cb(self, msg):

        self.batteryLevel = msg.data
        if self.batteryLevel < 45 and not self.alertPublished:
            alert = NavSts()
            alert.position.north = self.position.north
            alert.position.east = self.position.east
            alert.header.frame_id = rospy.get_namespace()
            self.chargeType = 'consumer'
            self.alertPublished = True
            self.batteryAlertPub.publish(alert)
            b = Bool()
            b.data = True
            self.sleepModePub.publish(b)
            self.send_depth_goal(0.1)
    def __init__(self, name):
        """ Initialize the class """
        # Init class vars
        self.name = name
        self.navigation = NavSts()
        self.set_zero_velocity_depth = 2.0
        self.set_zero_velocity_axis = [[
            False, False, False, False, False, False
        ]]
        self.current_enabled_axis = [False, False, False, False, False, False]
        self.lock = threading.Lock()

        # Get config parameters
        self.get_config()

        # Publisher
        self.pub_body_velocity_req = rospy.Publisher(
            "/cola2_control/body_velocity_req", BodyVelocityReq, queue_size=10)

        # Subscriber
        rospy.Subscriber("/cola2_navigation/nav_sts",
                         NavSts,
                         self.update_nav_sts,
                         queue_size=1)

        rospy.Subscriber("/cola2_control/world_waypoint_req",
                         WorldWaypointReq,
                         self.update_req,
                         queue_size=1)

        rospy.Subscriber("/cola2_control/body_velocity_req",
                         BodyVelocityReq,
                         self.update_req,
                         queue_size=1)

        rospy.Subscriber("/cola2_control/body_force_req",
                         BodyForceReq,
                         self.update_req,
                         queue_size=1)

        # Timer
        rospy.Timer(rospy.Duration(0.1), self.set_zero_velocity)

        # Show message
        rospy.loginfo("%s: initialized", self.name)
    def __init__(self, name):
        self.name = name
        self.nav = NavSts()
        self.odometry = Odometry()

        # Config
        self.aris_tilt = math.radians(15)
        self.aris_elevation_angle = math.radians(14)
        self.aris_fov = math.radians(30)
        self.aris_window_start = 1.5
        self.aris_window_length = 3.0
        self.aris_hz = 5
        self.naviagtion_init = False
        self.chain_detections = MarkerArray()
        self.broadcaster = tf.TransformBroadcaster()
        self.img_center = [0.0, 0.0]
        self.id = 0
        self.chain_links = [[0, 0, 5.75], [0.5, 0, 5.75], [1.0, 0.1, 5.75],
                            [1.5, 0.3, 5.75], [2.0, 0.5,
                                               5.75], [2.5, 0.5, 5.75],
                            [3.0, 0.5, 5.75], [3.3, 0.8,
                                               5.75], [3.5, 1.1, 5.75],
                            [3.5, 1.5, 5.75], [3.6, 2.0, 5.75]]

        # Create Publisher
        self.pub_marker = rospy.Publisher("/link_pose2", MarkerArray)
        self.pub_aris_footprint = rospy.Publisher('/aris_foot_print', Marker)
        self.pub_aris_img_pose = rospy.Publisher(
            '/cola2_perception/soundmetrics_aris3000/sonar_img_pose',
            PoseStamped)
        self.pub_aris_ifo = rospy.Publisher(
            '/cola2_perception/soundmetrics_aris3000/sonar_info', SonarInfo)

        # Create Subscriber
        rospy.Subscriber("/cola2_navigation/nav_sts",
                         NavSts,
                         self.update_nav_sts,
                         queue_size=1)
        rospy.Subscriber("/pose_ekf_slam/odometry",
                         Odometry,
                         self.update_odometry,
                         queue_size=1)
        # Enable Aris timer
        rospy.Timer(rospy.Duration(1.0 / self.aris_hz),
                    self.compute_link_detections)
    def __init__(self):
        '''Setup subscribers'''
        rospy.Subscriber("joy", Joy, self.onJoy)
        '''Setup publishers'''
        self.out = rospy.Publisher("TrackPoint", NavSts)
        self.point = NavSts()
        self.u_max = 0.5
        self.angle_max = 0.25
        self.Ts = 0.1
        '''Temporary addition for lat-lon outputs'''
        self.point.global_position.latitude = 44
        self.point.global_position.longitude = 15.305
        self.point.position_variance.north = 167.123
        self.point.position_variance.east = 155.623

        self.broadcaster = tf.TransformBroadcaster()
        self.base_frame = rospy.get_param("~base_frame", "base_link")
        self.underactuated = rospy.get_param("~underactuated", False)
Beispiel #15
0
    def __init__(self, name):
        self.name = name
        self.map = Map()
        self.nav = NavSts()
        self.stare_landmark_init = False
        self.offset_transformation = None
        self.keep_pose = False
        self.tolerance = 0.25
        self.push_init = False
        self.default_wrench = [45.0, 0.0, 0.0, 0.0, 0.0, 0.0]

        self.tfbr = tf.TransformBroadcaster()

        # Create publisher
        self.pub_world_waypoint_req = rospy.Publisher(
            "/cola2_control/world_waypoint_req",
            WorldWaypointReq,
            queue_size=2)

        # Create Subscriber
        rospy.Subscriber("/pose_ekf_slam/landmark_update/chain_position",
                         PoseWithCovarianceStamped,
                         self.update_chain_detector,
                         queue_size=1)

        rospy.Subscriber("/pose_ekf_slam/map",
                         Map,
                         self.update_map,
                         queue_size=1)

        rospy.Subscriber("/cola2_navigation/nav_sts",
                         NavSts,
                         self.update_nav,
                         queue_size=1)

        # Create Service
        self.enable_stare_srv = rospy.Service(
            '/cola2_control/enable_stare_chain', StareLandmark,
            self.stare_chain)

        self.disable_stare_srv = rospy.Service(
            '/cola2_control/disable_stare_chain', Empty, self.disable)

        self.last_detection = rospy.Time.now()
    def step(self):

        d = numpy.linalg.norm(self.lastPoint - self.nextPoint, 2)

        if d < self.radius:
            self.next = (self.next + 1) % len(self.points)
            self.nextPoint = numpy.array(self.points[self.next],
                                         dtype=numpy.float32)
            print "Change point to:", self.nextPoint

        diff = self.nextPoint - self.lastPoint
        yaw = math.atan2(diff[1], diff[0])
        self.lastPoint += self.u * self.Ts * numpy.array(
            [math.cos(yaw), math.sin(yaw)])

        pub = NavSts()
        pub.position.north = self.lastPoint[0]
        pub.position.east = self.lastPoint[1]
        self.out.publish(pub)
    def __init__(self, name):
        self.name = name
        self.link_detections = [0.0] * 2
        self.link_detections_index = 0
        self.total_link_wp = 0
        self.nav = NavSts()

        # Publishers
        self.pub_wwr = rospy.Publisher("/cola2_control/world_waypoint_req",
                                       WorldWaypointReq,
                                       queue_size=1)

        # Subscribers
        rospy.Subscriber("/link_pose2", MarkerArray,
                         self.update_link_detection)
        rospy.Subscriber("/udg_pandora/link_waypoints", MarkerArray,
                         self.update_link_wp)
        rospy.Subscriber('/cola2_navigation/nav_sts', NavSts,
                         self.nav_sts_update)
Beispiel #18
0
    def onState(self, data):
        ref = NavSts()
        dy = self.track.position.east - data.position.east
        dx = self.track.position.north - data.position.north
        ref.orientation.yaw = math.atan2(dy, dx)

        #Diver to goto point yaw
        dy2 = self.goto.position.east - self.track.position.east
        dx2 = self.goto.position.north - self.track.position.north
        dgyaw = 0
        if math.sqrt(dy2 * dy2 + dx2 * dx2) > 1:
            dgyaw = math.atan2(dy2, dx2)

        dyaw = 0
        if self.hasGoto:
            delta = (-self.track.orientation.yaw + dgyaw +
                     numpy.pi) % (2 * numpy.pi) - numpy.pi
            dyaw = self.maxAngle * math.tanh(self.Kr * delta)

        #print("Desired yaw diver: %f, desired yaw target: %f,  Dyaw: %f, delta: %f"
        #      % (self.track.orientation.yaw, dgyaw, dyaw, delta))

        ref.position.north = self.track.position.north + self.radius * math.cos(
            self.orientation.orientation.yaw + dyaw)
        ref.position.east = self.track.position.east + self.radius * math.sin(
            self.orientation.orientation.yaw + dyaw)
        ref.body_velocity.x = -self.track.body_velocity.x
        ref.body_velocity.y = -self.track.body_velocity.y

        pb = numpy.array([ref.position.north, ref.position.east])
        pg = numpy.array([self.goto.position.north, self.goto.position.east])
        if numpy.linalg.norm(pb - pg, 2) < self.radius / 2:
            ref.position.north = self.goto.position.north
            ref.position.east = self.goto.position.east

        ref.position.depth = self.track.position.depth
        ref.header.stamp = rospy.Time.now()

        self.pub.publish(ref)
Beispiel #19
0
    def __init__(self, name):
        """ Constructor """
        self.name = name

        # Initial time
        self.init_time = rospy.Time.now().to_sec()

        # Initial timeout
        self.timeout = 3600

        # Set up diagnostics
        self.diagnostic = DiagnosticHelper(self.name, "soft")

        # Publisher
        self.pub_total_time = rospy.Publisher("/cola2_safety/total_time",
                                              TotalTime,
                                              queue_size=2)

        # Create reset timeout service
        self.reset_timeout_srv = rospy.Service('/cola2_safety/reset_timeout',
                                               Empty, self.reset_timeout)

        # Timer to publish time since init (or last time reset)
        rospy.Timer(rospy.Duration(1.0), self.check_timeout)

        # Timer to reload timeout param from param server
        rospy.Timer(rospy.Duration(10.0), self.reload_timeout)

        # Check navigator --> This should not be in this node
        self.nav = NavSts()
        self.init_navigator_check = False
        self.last_navigator_callback = rospy.Time.now().to_sec()
        rospy.Subscriber("/cola2_navigation/nav_sts",
                         NavSts,
                         self.update_nav_sts,
                         queue_size=1)

        # Show message
        rospy.loginfo("%s: initialized", self.name)
    def __init__(self, name):
        """ Constructor """
        self.name = name
        self.last_nav = NavSts()
        self.landmark = [0.0, 0.0, 0.0]
        # Add landmark at x, y, z
        try:
            rospy.wait_for_service('/cola2_navigation/add_landmark', 20)
            self.add_landmark_client = rospy.ServiceProxy(
                '/cola2_navigation/add_landmark', AddLandmark)
        except rospy.exceptions.ROSException:
            rospy.logerr('%s, Error creating client to add landmark.',
                         self.name)
            rospy.signal_shutdown('Error creating add landmark client')
        req = AddLandmarkRequest()
        req.id.data = "docking"
        sigma = 0.000001
        req.landmark.pose.position.x = self.landmark[0]
        req.landmark.pose.position.y = self.landmark[1]
        req.landmark.pose.position.z = self.landmark[2]
        req.landmark.covariance[0] = sigma
        req.landmark.covariance[7] = sigma
        req.landmark.covariance[14] = sigma
        req.landmark.covariance[21] = sigma
        req.landmark.covariance[28] = sigma
        req.landmark.covariance[35] = sigma
        self.add_landmark_client(req)

        # Create publishers
        self.pub_range = rospy.Publisher('/cola2_navigation/range_update',
                                         RangeDetection,
                                         queue_size=2)

        # Subscriber
        rospy.Subscriber("/cola2_navigation/nav_sts",
                         NavSts,
                         self.update_nav,
                         queue_size=1)
class Nav(object):
    # Create the msg and subscriber as Class attributes. If they are in __init__ method and Nav class is inherited, they
    # will otherwise not be created/instanciated
    _nav = NavSts()

    def __init__(self):
        self.nav_sub = rospy.Subscriber("/nav/nav_sts", NavSts,
                                        self._navCallback)

    def _navCallback(self, msg):
        self._nav = msg

    def distance_to_target(self, target_location_ned):
        north = self._nav.global_position.latitude
        north = self._nav.global_position.longnitude

    def distance_between_two_global_positions(self, lon1, lat1, lon2, lat2):
        """
        Calculate the great circle distance between two points 
        on the earth (specified in decimal degrees)
        """
        # convert decimal degrees to radians
        lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
        # haversine formula
        dlon = lon2 - lon1
        dlat = lat2 - lat1
        a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2
        c = 2 * asin(sqrt(a))
        km = 6367 * c

        R = 6371  # radius of the earth in km
        x = (lon2 - lon1) * cos(0.5 * (lat2 + lat1))
        y = lat2 - lat1
        d = R * sqrt(x * x + y * y)
        # Return the value in metres
        #return (float(km) / 1000.0)
        return km, d
Beispiel #22
0
    def __init__(self):

        # position
        self.position = Point(0, 0, 0)
        rospy.Subscriber('position', NavSts, self.position_cb)

        # state reference publisher
        self.stateRefPub = rospy.Publisher('stateRef', NavSts, queue_size=1)

        self.action_rec_flag = 0  # 0 - waiting action, 1 - received action, 2 - executing action

        self.as_goal = aPadGoal()
        self.as_res = aPadResult()
        self.as_feed = aPadFeedback()

        # TODO create 4 docking stations -- 4 offset positions for every station + before docking, align the object to station
        self.position_offset = Point(-0.5, -0.5,
                                     0)  # for docking -- object offset

        self.pos_old = Point(self.position.x, self.position.y, self.position.z)

        self.pos_err = []
        self.perched_flag = 0  # flag for dummy action perching onto aMussel/aFish/other objects action
        self.perched_count = 0  # number of currently docked objects (default max 4)

        # initialize actions server structures
        self.action_server = actionlib.SimpleActionServer("action_server",
                                                          aPadAction,
                                                          auto_start=False)
        self.action_server.register_goal_callback(self.action_execute_cb)
        self.action_server.register_preempt_callback(self.action_preempt_cb)
        self.action_server.start()

        while not rospy.is_shutdown():
            # goal position for aMussel/aFish/object docked onto aPad (if perched!)
            #TODO multiple docking stations
            goalPosition = NED()
            goalPosition.north = self.position.x + self.position_offset.x
            goalPosition.east = self.position.y + self.position_offset.y
            #goalPosition.z = self.position.z + self.position_offset.z

            if self.perched_flag == 1:
                self.set_model_state(goalPosition)

            if self.action_server.is_active():

                ###################
                # received action #
                ###################

                if self.action_rec_flag == 1:

                    if self.as_goal.id == 0:
                        print 'Go to position action'  # 2d movement
                        self.pos_old = Point(self.position.x, self.position.y,
                                             0)
                        start = Vector3(self.position.x, self.position.y, 0)
                        end = Vector3(self.as_goal.pose.position.x,
                                      self.as_goal.pose.position.y, 0)

                        dl = sqrt(
                            pow(self.as_goal.pose.position.x -
                                self.position.x, 2) +
                            pow(self.as_goal.pose.position.y -
                                self.position.y, 2))

                        # if within 10cm of goal
                        if dl < 0.1:
                            self.action_rec_flag = 0  # waiting for new action
                            self.as_res.status = 0
                            self.pos_err = []
                            print 'finished executing go_to_position action'
                            print "###"
                            print self.position
                            print "###"
                            self.action_server.set_succeeded(self.as_res)
                        else:
                            try:
                                # send goal
                                fadp_enable = rospy.ServiceProxy(
                                    'FADP_enable', EnableControl)
                                fadp_enable(enable=True)

                                config_vel_controller = rospy.ServiceProxy(
                                    'ConfigureVelocityController',
                                    ConfigureVelocityController)
                                config_vel_controller(
                                    ControllerName="FADP",
                                    desired_mode=[2, 2, 0, 0, 0, 0])

                                velcon_enable = rospy.ServiceProxy(
                                    'VelCon_enable', EnableControl)
                                velcon_enable(enable=True)

                                stateRef = NavSts()
                                stateRef.header.seq = 0
                                stateRef.header.stamp.secs = 0
                                stateRef.header.stamp.nsecs = 0
                                stateRef.global_position.latitude = 0.0
                                stateRef.global_position.longitude = 0.0
                                stateRef.origin.latitude = 0.0
                                stateRef.origin.longitude = 0.0
                                stateRef.position.north = self.as_goal.pose.position.x
                                stateRef.position.east = self.as_goal.pose.position.y
                                stateRef.position.depth = 0
                                stateRef.altitude = 0.0
                                stateRef.body_velocity.x = 0
                                stateRef.body_velocity.y = 0
                                stateRef.body_velocity.z = 0
                                stateRef.gbody_velocity.x = 0
                                stateRef.gbody_velocity.y = 0
                                stateRef.gbody_velocity.z = 0
                                stateRef.orientation.roll = 0
                                stateRef.orientation.pitch = 0
                                stateRef.orientation.yaw = 0
                                stateRef.orientation_rate.roll = 0
                                stateRef.orientation_rate.pitch = 0
                                stateRef.orientation_rate.yaw = 0
                                stateRef.position_variance.north = 0
                                stateRef.position_variance.east = 0
                                stateRef.position_variance.depth = 0
                                stateRef.orientation_variance.roll = 0
                                stateRef.orientation_variance.pitch = 0
                                stateRef.orientation_variance.yaw = 0
                                stateRef.status = 0
                                self.stateRefPub.publish(stateRef)

                                self.action_rec_flag = 2  #executing trajectory

                            except rospy.ServiceException, e:
                                print "Service for activating controller failed: %s" % e
                                self.as_res.status = -1
                                self.action_server.set_aborted(self.as_res)
                                self.action_rec_flag = 0  # waiting for new actions

                    elif self.as_goal.id == 1:
                        print 'perch action'
                        self.action_rec_flag = 2  # start executing action

                    elif self.as_goal.id == 2:
                        print 'release action'
                        self.action_rec_flag = 2  # start executing action

                ####################
                # executing action #
                ####################

                elif self.action_rec_flag == 2:

                    self.as_res.id = self.as_goal.id
                    self.as_feed.id = self.as_goal.id

                    if self.as_goal.id == 0:
                        # Go to position action
                        dL = sqrt(
                            pow(self.as_goal.pose.position.x -
                                self.pos_old.x, 2) +
                            pow(self.as_goal.pose.position.y -
                                self.pos_old.y, 2))
                        dl = sqrt(
                            pow(self.as_goal.pose.position.x -
                                self.position.x, 2) +
                            pow(self.as_goal.pose.position.y -
                                self.position.y, 2))

                        if len(self.pos_err) < 20:
                            self.pos_err.append(dl)
                        else:
                            self.pos_err.pop(0)
                            self.pos_err.append(dl)

                        if (len(self.pos_err) == 20) and (
                                fabs(sum(self.pos_err) / len(self.pos_err)) <
                                0.5):  # mission is successfully finished
                            self.action_rec_flag = 0  # waiting for new action
                            self.as_res.status = 0
                            self.pos_err = []
                            print 'finished executing go_to_position action'
                            self.action_server.set_succeeded(self.as_res)
                        else:  # mission is still ongoing
                            self.as_feed.status = (
                                1 - dl / dL) * 100  # mission completeness
                            self.as_feed.pose.position = self.position
                            #print [fabs(sum(self.pos_err) / len(self.pos_err)), str(self.position), str(self.as_goal.pose.position)]
                            self.action_server.publish_feedback(self.as_feed)

                    elif self.as_goal.id == 1:
                        #if self.perched_count==4:
                        #return
                        print 'executing perch action'
                        self.perched_flag = 1
                        # static position publisher
                        #TODO make 4 static pose publishers, for 4 docking stations
                        self.staticPosPub = rospy.Publisher(
                            '/' + self.as_goal.object + '/position_static',
                            NavSts,
                            queue_size=1)
                        print 'finished executing perch action ' + self.as_goal.object
                        self.action_rec_flag = 0  # waiting for new action
                        self.as_res.status = 0
                        #self.perched_count+=1
                        self.action_server.set_succeeded(self.as_res)

                    elif self.as_goal.id == 2:
                        #if self.perched_count==4:
                        #return
                        print 'executing release action'
                        self.perched_flag = 0
                        msg = NavSts()
                        msg.position = NED(
                            self.position.x + self.position_offset.x,
                            self.position.y + self.position_offset.y, 0)
                        msg.status = 1  # status 1 -- the last static position, give control back to uvsim
                        #TODO make 4 static pose publishers, for 4 docking stations
                        self.staticPosPub.publish(
                            msg
                        )  # signal the end of static position to attached object
                        print 'finished executing release action'
                        self.action_rec_flag = 0  # waiting for new action
                        self.as_res.status = 0
                        #self.perched_count-=1
                        self.action_server.set_succeeded(self.as_res)
            else:
                pass

            rospy.sleep(rospy.Duration(0.05))
Beispiel #23
0
    def set_model_state(self, position):

        msg = NavSts()
        msg.position = position
        self.staticPosPub.publish(msg)
    def charging_procedure(self, msg):

        chargers = {}
        consumers = {}

        # gather data
        # 1. get all potential chargers
        self.helloPub.publish(String(rospy.get_namespace()))

        rospy.sleep(1)
        for charger in self.neighbors.keys():
            if charger == rospy.get_namespace():
                chargers[charger] = [
                    self.position.north, self.position.east, self.batteryLevel
                ]
                continue
            charge_info = rospy.ServiceProxy(charger + 'charge_info',
                                             GetChargeInfo)
            try:
                c = charge_info()
                chargers[charger] = [c.x, c.y, c.battery_level]
            except rospy.ServiceException as exc:
                print("Service did not process request: " + str(exc))

        tmp = chargers.keys()
        tmp.sort()

        for d in tmp:
            self.mdvrp.add_node('depot', d, chargers[d][0], chargers[d][1],
                                chargers[d][2])

        print self.mdvrp.depotLabels

        for consumer in self.chargeRequests:
            charge_info = rospy.ServiceProxy(consumer + 'charge_info',
                                             GetChargeInfo)
            try:
                c = charge_info()
                consumers[consumer] = [c.x, c.y, c.battery_level]
            except rospy.ServiceException as exc:
                print("Service did not process request: " + str(exc))

        tmp = consumers.keys()
        tmp.sort()

        for c in tmp:
            self.mdvrp.add_node('consumer', c, consumers[c][0],
                                consumers[c][1], (100 - consumers[c][2]) / 4)

        print self.mdvrp.consumerLabels

        self.cbmAlgorithm.optimize(self.bestSolutionPub, self.weightMatrixPub,
                                   self.mdvrp)

        rospy.sleep(1)

        solution = self.cbmAlgorithm.bestSolutionCoalition
        indices = solution.routes[self.mdvrp.depotLabels.index(
            rospy.get_namespace())]

        toVisit = []
        for i in indices[0]:
            label = self.mdvrp.nodeLabels[i]
            toVisit.append([label, consumers[label][0], consumers[label][1]])

        print[rospy.get_namespace(), toVisit]

        nextTarget = NavSts()
        for tgt in toVisit:
            t = tgt[0].strip("/amussel")
            nextTarget.header.frame_id = t
            nextTarget.position.north = tgt[1]
            nextTarget.position.east = tgt[2]
            #print nextTarget
            self.docking(nextTarget)
Beispiel #25
0
    def set_model_state(self, position):

        msg = NavSts()
        msg.position = position
        self.staticPosPub.publish(msg)
    def bacterial_chemotaxis_levy_walk(self, event):
        '''
        TODO -- description
        '''
        while self.position is None or not self.start:
            rospy.sleep(0.1)

        # uncomment for simulation time tracking
        #if '1' in rospy.get_namespace():
        #    print "$$$$$$$$$$$$$$$$$  " + str(rospy.get_time() - self.startTime)
        C = 0

        # get sensory signal amplitude
        amp = self.signalHistory[-2:]
        newAmplitude = amp[1]
        # if in tumbling process, use the amplitude value at the start of tumbling
        if self.tumble:
            oldAmplitude = self.tumbleValue
        else:
            oldAmplitude = amp[0]

        if newAmplitude < 0:  # no sensory data -- perform Levy walk
            # levy walk
            if self.levyTumble:
                self.levyA = 0
                # constant time period for tumbling activity
                self.levyTimeout = rospy.get_time() + 0.2
                #print "outside the area, tumbling"
            elif self.levyTimeout is None:
                t = self.levy_random()
                #print "## keeping direction for " + str(t)
                self.levyTimeout = rospy.get_time() + t
                self.levyA = 1
            elif rospy.get_time() >= self.levyTimeout:
                # toggle attractor variable
                self.levyA = 1 - self.levyA
                if self.levyA == 0:
                    # constant time period for tumbling activity
                    self.levyTimeout = rospy.get_time() + 0.2
                else:
                    # levy random variable
                    t = self.levy_random()
                    #print "## keeping direction for " + str(t)
                    self.levyTimeout = rospy.get_time() + t

            A = self.levyA
        else:  # bacterial chemotaxis
            # if signal strength has lowered, change the angle until the signal improves again
            if newAmplitude - oldAmplitude <= C:
                A = 0
                # if tumbling was already occurring, discard the new signal readings until improvement
                if not self.tumble:  # keep amplitude value at the start of tumbling process
                    self.tumbleValue = newAmplitude
                self.tumble = True
            else:
                A = 1
                self.tumble = False

        linVelocity = A
        if newAmplitude < 0:  # Levy walk
            angVelocity = choice([-1, 1
                                  ]) * (1 - A) * (60 + np.random.normal(0, 10))
        else:  # bacterial chemotaxis
            angVelocity = (1 - A) * (30 + np.random.normal(0, 10))
        angVelocityRad = radians(angVelocity)

        deltax = linVelocity * cos(angVelocityRad + radians(self.yaw))
        deltay = linVelocity * sin(angVelocityRad + radians(self.yaw))
        deltatheta = angVelocity

        # prepare state reference
        stateRef = NavSts()

        self.yaw += deltatheta
        self.yaw %= 360

        stateRef.position.north = self.position.north + deltax
        stateRef.position.east = self.position.east + deltay
        stateRef.position.depth = self.position.depth
        stateRef.orientation.yaw = self.yaw

        if stateRef.position.north < area[0][0] or stateRef.position.north > area[0][1] or \
            stateRef.position.east < area[1][0] or stateRef.position.east > area[1][1]:
            # random walk is at the edge of the allowed area -- force tumbling
            self.levyTumble = True
            return
        else:
            self.levyTumble = False

        # calc orientation (only yaw rotation is allowed)
        self.orientation.x = cos(radians(self.yaw))
        self.orientation.y = sin(radians(self.yaw))
        self.orientation.z = 0

        # initialize movement
        action_library.send_state_ref(self.stateRefPub, stateRef)
Beispiel #27
0
    def __init__(self):
        
        # position
        self.position = Point(0, 0, 0)
        rospy.Subscriber('position', NavSts, self.position_cb)

        # state reference publisher
        self.stateRefPub = rospy.Publisher('stateRef', NavSts, queue_size=1)
        
        self.action_rec_flag = 0  # 0 - waiting action, 1 - received action, 2 - executing action
        
        self.as_goal = aPadGoal()
        self.as_res = aPadResult()
        self.as_feed = aPadFeedback()

        # TODO create 4 docking stations -- 4 offset positions for every station + before docking, align the object to station
        self.position_offset = Point(-0.5, -0.5, 0)  # for docking -- object offset 

        self.pos_old = Point(self.position.x, self.position.y, self.position.z)
        
        self.pos_err = []
        self.perched_flag = 0  # flag for dummy action perching onto aMussel/aFish/other objects action
        self.perched_count = 0 # number of currently docked objects (default max 4)
        
        # initialize actions server structures
        self.action_server = actionlib.SimpleActionServer("action_server", aPadAction, auto_start=False)
        self.action_server.register_goal_callback(self.action_execute_cb)
        self.action_server.register_preempt_callback(self.action_preempt_cb)
        self.action_server.start()

        while not rospy.is_shutdown():
            # goal position for aMussel/aFish/object docked onto aPad (if perched!)
            #TODO multiple docking stations
            goalPosition = NED()
            goalPosition.north = self.position.x + self.position_offset.x
            goalPosition.east = self.position.y + self.position_offset.y
            #goalPosition.z = self.position.z + self.position_offset.z

            if self.perched_flag == 1:
                self.set_model_state(goalPosition)

            if self.action_server.is_active():

                ###################
                # received action #
                ###################

                if self.action_rec_flag == 1:
                
                    if self.as_goal.id == 0:
                        print 'Go to position action' # 2d movement
                        self.pos_old = Point(self.position.x, self.position.y, 0)
                        start = Vector3(self.position.x, self.position.y, 0)
                        end = Vector3(self.as_goal.pose.position.x, self.as_goal.pose.position.y, 0)

                        dl = sqrt(pow(self.as_goal.pose.position.x - self.position.x, 2) +
                                        pow(self.as_goal.pose.position.y - self.position.y, 2))

                        # if within 10cm of goal
                        if dl < 0.1:
                            self.action_rec_flag = 0  # waiting for new action
                            self.as_res.status = 0
                            self.pos_err = []
                            print 'finished executing go_to_position action'
                            print "###"
                            print self.position
                            print "###"
                            self.action_server.set_succeeded(self.as_res)
                        else:
                            try:
                                # send goal
                                fadp_enable = rospy.ServiceProxy('FADP_enable', EnableControl)
                                fadp_enable(enable=True)

                                config_vel_controller = rospy.ServiceProxy('ConfigureVelocityController', ConfigureVelocityController)
                                config_vel_controller(ControllerName="FADP", desired_mode=[2, 2, 0, 0, 0, 0])

                                velcon_enable = rospy.ServiceProxy('VelCon_enable', EnableControl)
                                velcon_enable(enable=True)
        
                                stateRef = NavSts()
                                stateRef.header.seq = 0
                                stateRef.header.stamp.secs = 0
                                stateRef.header.stamp.nsecs = 0
                                stateRef.global_position.latitude = 0.0
                                stateRef.global_position.longitude = 0.0
                                stateRef.origin.latitude = 0.0
                                stateRef.origin.longitude = 0.0
                                stateRef.position.north = self.as_goal.pose.position.x
                                stateRef.position.east = self.as_goal.pose.position.y
                                stateRef.position.depth = 0
                                stateRef.altitude = 0.0
                                stateRef.body_velocity.x = 0
                                stateRef.body_velocity.y = 0
                                stateRef.body_velocity.z = 0
                                stateRef.gbody_velocity.x = 0
                                stateRef.gbody_velocity.y = 0
                                stateRef.gbody_velocity.z = 0
                                stateRef.orientation.roll = 0
                                stateRef.orientation.pitch = 0
                                stateRef.orientation.yaw = 0
                                stateRef.orientation_rate.roll = 0
                                stateRef.orientation_rate.pitch = 0
                                stateRef.orientation_rate.yaw = 0
                                stateRef.position_variance.north = 0
                                stateRef.position_variance.east = 0
                                stateRef.position_variance.depth = 0
                                stateRef.orientation_variance.roll = 0
                                stateRef.orientation_variance.pitch = 0
                                stateRef.orientation_variance.yaw = 0
                                stateRef.status = 0
                                self.stateRefPub.publish(stateRef)
                                
                                self.action_rec_flag = 2  #executing trajectory

                            except rospy.ServiceException, e:
                                print "Service for activating controller failed: %s" % e
                                self.as_res.status = -1
                                self.action_server.set_aborted(self.as_res)
                                self.action_rec_flag = 0  # waiting for new actions

                    elif self.as_goal.id == 1:
                        print 'perch action'
                        self.action_rec_flag = 2  # start executing action

                    elif self.as_goal.id == 2:
                        print 'release action'
                        self.action_rec_flag = 2  # start executing action

                ####################
                # executing action #
                ####################

                elif self.action_rec_flag == 2:

                    self.as_res.id = self.as_goal.id
                    self.as_feed.id = self.as_goal.id

                    if self.as_goal.id == 0:
                        # Go to position action
                        dL = sqrt(pow(self.as_goal.pose.position.x - self.pos_old.x, 2) +
                                       pow(self.as_goal.pose.position.y - self.pos_old.y, 2))
                        dl = sqrt(pow(self.as_goal.pose.position.x - self.position.x, 2) +
                                       pow(self.as_goal.pose.position.y - self.position.y, 2))

                        if len(self.pos_err) < 20:
                            self.pos_err.append(dl)
                        else:
                            self.pos_err.pop(0)
                            self.pos_err.append(dl)

                        if (len(self.pos_err) == 20) and (fabs(sum(self.pos_err) / len(self.pos_err)) < 0.5):  # mission is successfully finished
                            self.action_rec_flag = 0  # waiting for new action
                            self.as_res.status = 0
                            self.pos_err = []
                            print 'finished executing go_to_position action'
                            self.action_server.set_succeeded(self.as_res)
                        else:  # mission is still ongoing
                            self.as_feed.status = (1 - dl / dL) * 100  # mission completeness
                            self.as_feed.pose.position = self.position
                            #print [fabs(sum(self.pos_err) / len(self.pos_err)), str(self.position), str(self.as_goal.pose.position)]
                            self.action_server.publish_feedback(self.as_feed)

                    elif self.as_goal.id == 1:
			#if self.perched_count==4:
				#return                        
			print 'executing perch action'
                        self.perched_flag = 1
                        # static position publisher
                        #TODO make 4 static pose publishers, for 4 docking stations
                        self.staticPosPub = rospy.Publisher('/' + self.as_goal.object + '/position_static', NavSts, queue_size=1)
                        print 'finished executing perch action ' + self.as_goal.object
                        self.action_rec_flag = 0  # waiting for new action
                        self.as_res.status = 0
                        #self.perched_count+=1
                        self.action_server.set_succeeded(self.as_res)

                    elif self.as_goal.id == 2:
			#if self.perched_count==4:
				#return 
                        print 'executing release action'
                        self.perched_flag = 0
                        msg = NavSts()
                        msg.position = NED(self.position.x + self.position_offset.x, self.position.y + self.position_offset.y, 0)
                        msg.status = 1 # status 1 -- the last static position, give control back to uvsim
                        #TODO make 4 static pose publishers, for 4 docking stations
                        self.staticPosPub.publish(msg) # signal the end of static position to attached object
                        print 'finished executing release action'
                        self.action_rec_flag = 0  # waiting for new action
                        self.as_res.status = 0
                        #self.perched_count-=1
                        self.action_server.set_succeeded(self.as_res)
            else:
                pass

            rospy.sleep(rospy.Duration(0.05))