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)
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)
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
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)
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
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)
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)
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)
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)
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
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))
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)
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)
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))