def send_depth_goal(self, depth):
pos_old = NED(self.position.north, self.position.east,
self.position.depth)
goal = NED(pos_old.north, pos_old.east, depth)
if action_library.send_depth_goal(self.stateRefPub, goal) == -1:
return
pos_err = []
while not rospy.is_shutdown():
dL = abs(goal.depth - pos_old.depth)
dl = abs(goal.depth - self.position.depth)
if len(pos_err) < 10:
pos_err.append(dl)
else:
pos_err.pop(0)
pos_err.append(dl)
if (len(pos_err)
== 10) and (fabs(sum(pos_err) / len(pos_err)) <
0.05): # mission is successfully finished
return
else: #mission is still ongoing
rospy.sleep(0.1)
def send_depth_goal(self, depth):
pos_old = NED(self.position.north, self.position.east,
self.position.depth)
goal = NED(pos_old.north, pos_old.east, depth)
# send the goal to depth controller
if action_library.send_depth_goal(self.stateRefPub, goal) == -1:
return False
# init error structure
pos_err = []
# wait until goal is reached
while not rospy.is_shutdown():
# distance from goal depth
dl = abs(goal.depth - self.position.depth)
# remembers the last 10 errors --> to change this, just replace 10 with desired number
# (bigger err structure equals more precision, but longer time to acheive the goal)
if len(pos_err) < 10:
pos_err.append(dl)
else:
pos_err.pop(0)
pos_err.append(dl)
# goal precision in m --> for better precision replace the number with the smaller one, e.g. 0.001 for 1 mm precision
# for worse precision, insert greater one, e.g. 1 for 1 m precision
if (len(pos_err)
== 10) and (fabs(sum(pos_err) / len(pos_err)) <
0.05): # mission is successfully finished
return True
else: # mission is still ongoing, sleep for 0.1 s
rospy.sleep(rospy.Duration(0.1))
def battery_cb(self, msg):
self.batteryCall = NED(msg.north, msg.east, msg.depth)
#self.send_position_goal = self.batteryCall
pos_old = NED(self.position.north, self.position.east,
self.position.depth)
goal = NED(msg.north, msg.east, pos_old.depth)
action_library.send_position_goal(self.stateRefPub, goal)
def position_cb(self, msg):
if not self.start:
return
self.position = NED(msg.position.north, msg.position.east,
msg.position.depth)
def __init__(self):
self.start = False
self.current = NED(0, 0, 0)
self.position = None
self.neighbors = {}
rospy.Subscriber('/scenario_start', Bool, self.start_cb)
self.startPub = rospy.Publisher('start_sim', Bool, queue_size=1)
self.stateRefPub = rospy.Publisher('stateRef', NavSts, queue_size=1)
rospy.Subscriber('position', NavSts, self.position_cb)
self.batteryLevel = None
rospy.Subscriber('battery_level', Float64, self.battery_level_cb)
self.batteryAlertPub = rospy.Publisher('/battery_alert',
NavSts,
queue_size=100)
self.sleepModePub = rospy.Publisher('power_sleep_mode',
Bool,
queue_size=1)
self.alertPublished = False
rospy.Service('charge_info', GetChargeInfo, self.charge_info_srv)
self.chargeType = None
rospy.spin()
def init_noise_sim(self):
"""
Initializes noise simulation object. Depending on noise_mode parameter,
it generates an instance of respective class. If the noise_mode is set to
"none", the object will not be created.
"""
noiseMode = rospy.get_param('~noise_mode')
if noiseMode == "static":
try:
from noise_sim_static import NoiseSimStatic
except ImportError:
rospy.logerr("ERROR: Can't import module NoiseSimstatic")
return -1
sourcePosition = NED()
sourcePosition.north = rospy.get_param('~noise_source_north')
sourcePosition.east = rospy.get_param('~noise_source_east')
sourcePosition.depth = rospy.get_param('~noise_source_depth')
self.noiseSim = NoiseSimStatic(sourcePosition)
'''
You can insert new noise modes here (just unindent for one tab to be in the
same level as other elif keywords).
***
Template:
elif noiseMode == "mode":
try:
from noise_sim_mode import NoiseSimMode
except ImportError:
rospy.logerr("ERROR: Can't import module NoiseSimMode")
return -1
self.noiseSim = NoiseSimMode()
***
IMPORTANT: implement the desired behavior in noise_sim_mode.py (for reference,
see already implemented modes)
'''
elif noiseMode == "none":
return 0
else:
rospy.logerr("Noise mode {0} is not implemented!".format(noiseMode))
def __init__(self):
# - - -
# class variables
# start flag, all callback functions will wait until the controller gets the scenario_start message
self.start = False
# ping structures
self.pingCount = 0
self.pingAvgHeading = NED(0, 0, 0)
self.pingSum = NED(0, 0, 0)
# current heading
self.current = NED(0, 0, 0)
# position
self.position = None
# battery call
self.batteryCall = None
# - - -
# topic subscribers
rospy.Subscriber('/scenario_start', Bool, self.start_cb)
rospy.Subscriber('/battery_alert', NED, self.battery_cb)
rospy.Subscriber('position', NavSts, self.position_cb)
rospy.Subscriber('ping_sensor', NED, self.ping_sensor_cb)
rospy.Subscriber('current_sensor', TwistStamped,
self.current_sensor_cb)
# - - -
# topic publishers
self.startPub = rospy.Publisher('start_sim', Bool, queue_size=1)
self.stateRefPub = rospy.Publisher('stateRef', NavSts, queue_size=1)
# - - -
# timers
self.timerOne = rospy.Timer(rospy.Duration(1), self.timer_one_cb)
# - - -
# hand over control to ROS
rospy.spin()
def __init__(self, sourcePos):
super(NoiseSimStatic, self).__init__()
self.sourcePosition = NED()
self.sourcePosition.north = sourcePos.north
self.sourcePosition.east = sourcePos.east
self.sourcePosition.depth = sourcePos.depth
self.orientation = Point(0, 0, 0)
rospy.Subscriber("stateRef", NavSts, self.state_ref_cb)
def _do_hover_callback(self, req):
goal = req.goal.values
rospy.loginfo(
"DoHover: received goal [{0}, {1}, {2}, {4}, {5}]".format(
goal[0], goal[1], goal[2], goal[3], goal[4], goal[5]))
action_start_time = rospy.get_time()
timeoutReached = False
# take the received goal and push it to the motor controls semantics
# get a publisher
pub = rospy.Publisher("/pilot/position_req", PilotRequest)
goalNED = NED()
goalNED.north, goalNED.east, goalNED.depth = goal[0], goal[1], goal[2]
goalRPY = RPY()
goalRPY.roll, goalRPY.pitch, goalRPY.yaw = goal[3], goal[4], goal[5]
# repeatedly call world waypoint req to move the robot to the goal
# while the robot is not at teh desired location
while not (rospy.is_shutdown()
or self._nav != None and utils.epsilonEqualsNED(
self._nav.position, goalNED, 0.5, depth_e=0.6)
and utils.epsilonEqualsY(self._nav.orientation, goalRPY, .2)
# we compare on just pitch and yaw for 5 axis robot
):
# publish the goal repeatedly
pilotMsg = PilotRequest()
pilotMsg.position = list(goal)
pub.publish(pilotMsg)
# rospy.loginfo("Sent Goal!!")
rospy.sleep(0.5)
if timeoutReached:
return DoHoverResponse(False)
print("Sleeping for a while ...")
rospy.sleep(4)
# pub and subscriber will be removed at the end of executes context
rospy.loginfo("DoHover: complete")
return DoHoverResponse(True)
def loop(self):
if self._nav is not None:
velocity, heading = calculate_body_velocity(
self._nav.body_velocity.x, self._nav.body_velocity.y,
self._nav.orientation.yaw)
print("Body Velocity: {0}".format(velocity))
print("Velocity Heading: {0}".format(heading))
pose = VehiclePose(position=NED(north=self._nav.position.north,
east=self._nav.position.east,
depth=self._nav.position.depth),
orientation=RPY(roll=0, pitch=0, yaw=heading))
# publish an arrow marker to according to above calculation
self.rviz_points_array(pose)
def start_cb(self, msg):
battery = action_library.Battery()
print battery.get_level()
print "################"
return
self.startTime = rospy.get_time()
# enable and configure controllers
self.velcon_enable = rospy.ServiceProxy('VelCon_enable', EnableControl)
self.velcon_enable(enable=True)
self.fadp_enable = rospy.ServiceProxy('FADP_enable', EnableControl)
self.fadp_enable(enable=True)
self.config_vel_controller = rospy.ServiceProxy(
'ConfigureVelocityController', ConfigureVelocityController)
self.config_vel_controller(ControllerName="FADP",
desired_mode=[2, 2, 2, 0, 0, 0])
# submerge
while self.position is None:
rospy.sleep(0.1)
action_library.send_position_goal(
self.stateRefPub, NED(self.position.north, self.position.east, 5))
depthOld = self.position.depth
posErr = []
while True:
dL = abs(self.position.depth - depthOld)
dl = abs(5 - self.position.depth)
if len(posErr) < 20:
posErr.append(dl)
else:
posErr.pop(0)
posErr.append(dl)
if (len(posErr) == 20) and (fabs(sum(posErr) / len(posErr)) < 0.1
): # mission is successfully finished
break
self.start = True
self.startPub.publish(msg)
def __init__(self):
self.start = False
self.current = NED(0, 0, 0)
self.position = None
rospy.Subscriber('/scenario_start', Bool, self.start_cb)
self.startPub = rospy.Publisher('start_sim', Bool, queue_size=1)
self.stateRefPub = rospy.Publisher('stateRef', NavSts, queue_size=1)
rospy.Subscriber('position', NavSts, self.position_cb)
rospy.Subscriber("ping_sensor", NED, self.ping_sensor_cb)
rospy.Subscriber('goto_surface', Bool, self.surface_cb)
self.dockingPub = rospy.Publisher('/apad1/docked', NavSts, queue_size=1)
rospy.spin()
def __init__(self):
self.start = False
self.position = NED()
self.oldCurrent = None
if rospy.has_param('~current_publish_rate'):
self.currPubRate = rospy.get_param('~current_publish_rate')
else:
self.currPubRate = 0.2
self.currPubTimeout = rospy.get_time() + self.currPubRate
rospy.Subscriber("start_sim", Bool, self.start_cb)
rospy.Subscriber("position", NavSts, self.position_cb)
self.currSensorPub = rospy.Publisher("current_sensor",
TwistStamped,
queue_size=1)
def __init__(self):
self.start = False
self.position = NED()
self.oldTemp = None
if rospy.has_param('~temp_publish_rate'):
self.tempPubRate = rospy.get_param('~temp_publish_rate')
else:
self.tempPubRate = 0.2
self.tempPubTimeout = rospy.get_time() + self.tempPubRate
rospy.Subscriber("start_sim", Bool, self.start_cb)
rospy.Subscriber("position", NavSts, self.position_cb)
self.tempSensorPub = rospy.Publisher("temperature_sensor",
Float64,
queue_size=1)
def __init__(self):
self.start = False
self.current = NED(0, 0, 0)
self.position = None
rospy.Subscriber('/scenario_start', Bool, self.start_cb)
self.startPub = rospy.Publisher('start_sim', Bool, queue_size=1)
self.stateRefPub = rospy.Publisher('stateRef', NavSts, queue_size=1)
rospy.Subscriber('position', NavSts, self.position_cb)
rospy.Subscriber("ping_sensor", NED, self.ping_sensor_cb)
rospy.Subscriber("current_sensor", TwistStamped,
self.current_sensor_cb)
self.cl = actionClient(rospy.get_namespace())
rospy.spin()
def __init__(self):
self.start = False
self.current = NED(0, 0, 0)
self.position = None
self.neighbors = {}
rospy.Subscriber('/hello', String, self.hello_cb)
self.helloPub = rospy.Publisher('/hello', String, queue_size=100)
rospy.Subscriber('/scenario_start', Bool, self.start_cb)
self.startPub = rospy.Publisher('start_sim', Bool, queue_size=1)
self.stateRefPub = rospy.Publisher('stateRef', NavSts, queue_size=1)
rospy.Subscriber('position', NavSts, self.position_cb)
self.batteryLevel = None
#rospy.Subscriber('battery_level', Int32, self.battery_level_cb)
self.batteryLevel = 80
rospy.Service('charge_info', GetChargeInfo, self.charge_info_srv)
self.chargeType = 'charger'
self.chargeRequests = []
rospy.Subscriber('/battery_alert', NavSts, self.charge_cb)
self.cl = actionClient(rospy.get_namespace())
self.mdvrp = mdvrp.mdvrp()
self.cbmAlgorithm = cbm_algorithm.cbm_algorithm("cbm_parameters.xml")
self.bestSolutionPub = rospy.Publisher("/bestSolution",
BestSolution,
queue_size=20)
self.weightMatrixPub = rospy.Publisher("/weightMatrix",
WeightMatrix,
queue_size=20)
rospy.Subscriber('/optimize', Bool, self.charging_procedure)
rospy.spin()
def __init__(self):
self.current = NED(0, 0, 0)
self.position = None
self.start = False
# position service
rospy.Service('get_position', GetPosition, self.get_position_srv)
# get sensory reading service
rospy.Service('get_sensory_reading', GetSensoryReading,
self.get_sensory_reading_srv)
self.stateRefPub = rospy.Publisher('stateRef', NavSts, queue_size=1)
self.startPub = rospy.Publisher('start_sim', Bool, queue_size=1)
rospy.Subscriber('/scenario_start', Bool, self.start_cb)
rospy.Subscriber('current_sensor', TwistStamped,
self.current_sensor_cb)
rospy.Subscriber('position', NavSts, self.position_cb)
rospy.spin()
def __init__(self):
self.start = False
self.position = NED()
self.orientation = None #Point() the orientation vector
self.oldNoise = None
self.intensity = rospy.get_param('~noise_intensity')
if rospy.has_param('~noise_publish_rate'):
self.noisePubRate = rospy.get_param('~noise_publish_rate')
else:
self.noisePubRate = 0.1
self.noisePubTimeout = rospy.get_time() + self.noisePubRate
rospy.Subscriber("start_sim", Bool, self.start_cb)
rospy.Subscriber("position", NavSts, self.position_cb)
rospy.Subscriber("/noise_intensity", Float64, self.noise_intensity_cb)
self.noiseSensorPub = rospy.Publisher("noise_sensor",
Float64,
queue_size=1)
north_range_mussel = [-100, 100]
east_range_mussel = [-100, 100]
# generate random positions
posID_pad = []
posID_fish = []
posID_mussel = []
while len(apad_positions) < apad_number:
north = uniform(north_range_pad[0], north_range_pad[1])
east = uniform(east_range_pad[0], east_range_pad[1])
tmp = '%.2f%.2f' % (north, east)
if tmp not in posID_pad:
posID_pad.append(tmp)
apad_positions.append(NED(north, east, 0))
while len(afish_positions) < afish_number:
north = uniform(north_range_fish[0], north_range_fish[1])
east = uniform(east_range_fish[0], east_range_fish[1])
tmp = '%.2f%.2f' % (north, east)
if tmp not in posID_pad:
posID_pad.append(tmp)
afish_positions.append(NED(north, east, 0))
while len(amussel_positions) < amussel_number:
north = uniform(north_range_mussel[0], north_range_mussel[1])
east = uniform(east_range_mussel[0], east_range_mussel[1])
tmp = '%.2f%.2f' % (north, east)
if tmp not in posID_mussel:
posID_mussel.append(tmp)
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))
positions_pad = []
posID_pad = []
positions_fish = []
posID_fish = []
positions_mussel = []
posID_mussel = []
while len(positions_pad) < n_pad:
north = uniform(north_range_pad[0], north_range_pad[1])
east = uniform(east_range_pad[0], east_range_pad[1])
tmp = '%.2f%.2f' % (north, east)
if tmp not in posID_pad:
posID_pad.append(tmp)
positions_pad.append(NED(north, east, 0))
while len(positions_fish) < n_fish:
north = uniform(north_range_fish[0], north_range_fish[1])
east = uniform(east_range_fish[0], east_range_fish[1])
tmp = '%.2f%.2f' % (north, east)
if tmp not in posID_pad:
posID_pad.append(tmp)
positions_fish.append(NED(north, east, 0))
while len(positions_mussel) < n_mussel:
north = uniform(north_range_mussel[0], north_range_mussel[1])
east = uniform(east_range_mussel[0], east_range_mussel[1])
tmp = '%.2f%.2f' % (north, east)
if tmp not in posID_mussel:
posID_mussel.append(tmp)
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 position_cb(self, msg):
# read current position
self.position = NED(msg.position.north, msg.position.east,
msg.position.depth)
def reset_ping_structures(self):
self.pingCount = 0
self.pingAvgHeading = NED(0, 0, 0)
self.pingSum = NED(0, 0, 0)
def position_cb(self, msg):
self.position = NED(msg.position.north, msg.position.east, msg.position.depth)
if self.position is None or self.start is False:
return
def position_cb(self, msg):
self.position = NED(msg.position.north, msg.position.east,
msg.position.depth)
