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)