def __init__(self): self.flag = False rospy.init_node('los_path_following') self.sub = rospy.Subscriber('/odometry/filtered', Odometry, self.callback, queue_size=1) # 20hz self.pub_thrust = rospy.Publisher('/manta/thruster_manager/input', Wrench, queue_size=1) # constructor object self.los = LOS() self.autopilot = AutopilotPID() self.reference_model = ReferenceModel(np.array((0, 0)), 0.05) # dynamic reconfigure self.config = {} self.srv_reconfigure = Server(AutopilotConfig, self.config_callback) """ action server guide https://github.com/strawlab/ros_common/blob/master/actionlib/src/actionlib/simple_action_server.py """ self.action_server = actionlib.SimpleActionServer(name='los_path', ActionSpec=LosPathFollowingAction, auto_start=False) self.action_server.register_goal_callback(self.goalCB) self.action_server.register_preempt_callback(self.preemptCB) self.action_server.start()
def __init__(self): # init node rospy.init_node('inspect_unknown_point') # init variables self.run_controller = False self.x = 0 self.y = 0 self.z = 0 self.roll = 0 self.pitch = 0 self.yaw = 0 self.centre_of_rot = Point() self.desired_radius = 10 self.desired_depth = -0.5 # pid regulators P_dist = 20 I_dist = 0 D_dist = 10 sat_dist = 5 self.PID_dist = PIDRegulator(P_dist, I_dist, D_dist, sat_dist) P_angle = 20 I_angle = 10 D_angle = 10 sat_angle = 5 self.PID_angle = PIDRegulator(P_angle, I_angle, D_angle, sat_angle) self.PID = AutopilotPID() # subscribers self.sub_pose = rospy.Subscriber('/odometry/filtered', Odometry, self.positionCallback, queue_size=1) # publishers self.pub_thrust = rospy.Publisher('/manta/thruster_manager/input', Wrench, queue_size=1) # action server setup self.action_server = actionlib.SimpleActionServer( name='inspect_point', ActionSpec=MoveAction, auto_start=False) self.action_server.register_goal_callback(self.goalCB) self.action_server.start()
def __init__(self): # init node rospy.init_node('pid_global_plan', anonymous=False) # init variables self.x = 0 self.y = 0 self.z = 0 self.roll = 0 self.pitch = 0 self.yaw = 0 self.reached_goal = True self.desired_depth = -0.5 self.path = [] self.current_goal = PoseStamped() self.sphere_of_acceptance = 0.2 self.vehicle_odom = Odometry() # pid regulators P_yaw = 25 I_yaw = 0 D_yaw = 10 sat_yaw = 15 self.PID_yaw = PIDRegulator(P_yaw, I_yaw, D_yaw, sat_yaw) # p, i, d, sat P_side = 20 I_side = 0 D_side = 10 sat_side = 5 self.PID_side = PIDRegulator(P_side, I_side, D_side, sat_side) self.PID = AutopilotPID() self._feedback = LosPathFollowingFeedback() self._result = LosPathFollowingResult() rospy.sleep(1) # subscribers self.sub_pose = rospy.Subscriber('/odometry/filtered', Odometry, self.positionCallback, queue_size=1) self.sub_pose = rospy.Subscriber( '/move_base_node/TrajectoryPlannerROS/global_plan', Path, self.globPlanCallback, queue_size=2) rospy.sleep(1) # publishers self.pub_thrust = rospy.Publisher('/manta/thruster_manager/input', Wrench, queue_size=1) self.marker_pub = rospy.Publisher('/pathplanning/closest_pt', Marker, queue_size=1) # action server rospy.sleep(1) self.move_base_client = actionlib.SimpleActionClient( 'move_base', MoveBaseAction) self.move_base_client.wait_for_server() self.action_server = actionlib.SimpleActionServer( name='pid_global_plan_server', ActionSpec=LosPathFollowingAction, auto_start=False) self.action_server.register_goal_callback(self.goalCB) self.action_server.register_preempt_callback(self.preemptCB) self.action_server.start() self.start_time = rospy.get_time() print("Rospy spin") print("Finished TaskManager")
class PidGlobalPlan(): def __init__(self): # init node rospy.init_node('pid_global_plan', anonymous=False) # init variables self.x = 0 self.y = 0 self.z = 0 self.roll = 0 self.pitch = 0 self.yaw = 0 self.reached_goal = True self.desired_depth = -0.5 self.path = [] self.current_goal = PoseStamped() self.sphere_of_acceptance = 0.2 self.vehicle_odom = Odometry() # pid regulators P_yaw = 25 I_yaw = 0 D_yaw = 10 sat_yaw = 15 self.PID_yaw = PIDRegulator(P_yaw, I_yaw, D_yaw, sat_yaw) # p, i, d, sat P_side = 20 I_side = 0 D_side = 10 sat_side = 5 self.PID_side = PIDRegulator(P_side, I_side, D_side, sat_side) self.PID = AutopilotPID() self._feedback = LosPathFollowingFeedback() self._result = LosPathFollowingResult() rospy.sleep(1) # subscribers self.sub_pose = rospy.Subscriber('/odometry/filtered', Odometry, self.positionCallback, queue_size=1) self.sub_pose = rospy.Subscriber( '/move_base_node/TrajectoryPlannerROS/global_plan', Path, self.globPlanCallback, queue_size=2) rospy.sleep(1) # publishers self.pub_thrust = rospy.Publisher('/manta/thruster_manager/input', Wrench, queue_size=1) self.marker_pub = rospy.Publisher('/pathplanning/closest_pt', Marker, queue_size=1) # action server rospy.sleep(1) self.move_base_client = actionlib.SimpleActionClient( 'move_base', MoveBaseAction) self.move_base_client.wait_for_server() self.action_server = actionlib.SimpleActionServer( name='pid_global_plan_server', ActionSpec=LosPathFollowingAction, auto_start=False) self.action_server.register_goal_callback(self.goalCB) self.action_server.register_preempt_callback(self.preemptCB) self.action_server.start() self.start_time = rospy.get_time() print("Rospy spin") print("Finished TaskManager") def goalCB(self): """ Get a new goal from the action server Update the goal and pass to the move_base action server """ self.reached_goal = False _goal = self.action_server.accept_new_goal() self.sphere_of_acceptance = _goal.sphereOfAcceptance self.desired_depth = _goal.desired_depth.z self.moveBaseClient(_goal) self.current_goal = _goal.next_waypoint PRINT_GOAL = True if PRINT_GOAL: print("sphere of accpetance: ", self.sphere_of_acceptance) print("desired depth: ", self.desired_depth) print("target x: ", _goal.next_waypoint.x) print("target y: ", _goal.next_waypoint.y) def moveBaseClient(self, _goal): """ Client for sending goal to move_base action server """ mb_goal = MoveBaseGoal() mb_goal.target_pose.header.frame_id = 'manta/odom' mb_goal.target_pose.header.stamp = rospy.Time.now() mb_goal.target_pose.pose.position.x = _goal.next_waypoint.x mb_goal.target_pose.pose.position.y = _goal.next_waypoint.y mb_goal.target_pose.pose.orientation.w = 1.0 self.move_base_client.send_goal(mb_goal) def globPlanCallback(self, msg): """ Update path from move_base """ self.path = msg.poses def withinSphereOfAcceptance(self): """ Check if manta is within the spehere of acceptance of the goal """ manta_pos = Point() manta_pos.x = self.x manta_pos.y = self.y distToGoal = self.distBetween2Positions(self.current_goal, manta_pos) return distToGoal < self.sphere_of_acceptance def distanceBetweenPoseAndSelf(self, pose): """ Distance between a pose and manta """ return np.sqrt((self.x - pose.position.x)**2 + abs(self.y - pose.position.y)**2) def distBetween2Poses(self, pose1, pose2): """ Distance between 2 poses """ return np.sqrt((pose1.position.x - pose2.position.x)**2 + abs(pose1.position.y - pose2.position.y)**2) def distBetween2Positions(self, pos1, pos2): """ Distance between 2 positions """ return np.sqrt((pos1.x - pos2.x)**2 + abs(pos1.y - pos2.y)**2) def shutdown(self): """ Stop manta """ rospy.loginfo("stopping the AUV...") rospy.sleep(10) def getCurvature(self, pose1, pose2, pose3): """ Calculate curvature of path """ side1 = self.distBetween2Poses(pose1, pose2) side2 = self.distBetween2Poses(pose2, pose3) side3 = self.distBetween2Poses(pose1, pose3) a = pose1.position b = pose2.position c = pose3.position area = (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x) curvature = 4 * area / (side1 * side2 * side3) return curvature def isValidRange(self, index, length_list): """ Check for valid index in a list """ return index >= 0 and index < length_list def statusActionGoal(self): """ Check if within sphere of acceptance and if succeded, report to action server """ # feedback self._feedback = LosPathFollowingFeedback() manta_pos = Point() manta_pos.x = self.x manta_pos.y = self.y self._feedback.distanceToGoal = self.distBetween2Positions( manta_pos, self.current_goal) self.action_server.publish_feedback(self._feedback) # succeeded if self.withinSphereOfAcceptance(): print("Within sphere of acceptance") self._result.terminalSector = True self.action_server.set_succeeded(self._result, text="goal completed") self.reached_goal = True def preemptCB(self): """ Cancel the action server """ if self.action_server.is_preempt_requested(): rospy.loginfo("Preempted requsted by global pid planner") self.action_server.set_preempted() def fixHeadingWrapping(self, err_yaw): """ Fix error in yaw such that manta rotates in the closest direction Example: Instead of rotating 3/2 pi clockwise, rotate -1/2 pi counter clockwise """ if err_yaw > math.pi: err_yaw = err_yaw - 2 * math.pi if err_yaw < -math.pi: err_yaw = err_yaw + 2 * math.pi return err_yaw def vectorMantaToPath(self, closest_pt): """ Get the vector from manta to the closest point on the global path from move_base """ vec_toward_path = np.array( [closest_pt.x - self.x, closest_pt.y - self.y]) rot_mat = np.array([[np.cos(self.yaw), -np.sin(self.yaw)], [np.sin(self.yaw), np.cos(self.yaw)]]) rot_mat = rot_mat.transpose() dir_manta_frame = np.dot(rot_mat, vec_toward_path) return dir_manta_frame def controller(self): """ Controller to follow the global plan Consists of 3 PID controllers: yaw PID, PID sideways eroor relative to path, and depth PID Functionality added so that manta turns in the right direction before following path """ index, closest_pt = self.findClosestPointIndex() yaw = self.getYawFrom2Pos(self.path[index].pose.position, self.path[index + 1].pose.position) # calculate curvature of path close to manta curvature = 0 if self.isValidRange(index - 1, len(self.path)) and self.isValidRange( index + 1, len(self.path)): curvature = self.getCurvature(self.path[index - 5].pose, self.path[index].pose, self.path[index + 5].pose) # yaw/angle error err_yaw = yaw - self.yaw err_yaw = self.fixHeadingWrapping(err_yaw) # get error sideways to path dir_manta_frame = self.vectorMantaToPath(closest_pt) err_side = dir_manta_frame[1] # regulate torque, extra torque if in a curved part of the path and facing correct direction torque_z = self.PID_yaw.regulate(err_yaw, rospy.get_time()) if (err_yaw < 0.1): curvature_gain = min(curvature / 10, 4) torque_z = torque_z + curvature_gain # regulate side force, sideways force only if facing the correct direction force_y = self.PID_side.regulate(err_side, rospy.get_time()) force_y = np.sign(force_y) * (max(abs(force_y) - 5 * abs(err_yaw), 0)) # fix to make forward speed slow down if off track force_x = 10 # max force forward force_x = max(force_x - 10 * abs(err_side) - 50 * abs(err_yaw), 0) # depth controller tau_depth_hold = self.PID.depthController(self.desired_depth, self.z, rospy.get_time()) # make wrench and publish wrench = Wrench() wrench.force.y = force_y wrench.force.x = force_x wrench.torque.z = torque_z wrench.force.z = tau_depth_hold self.pub_thrust.publish(wrench) # check status of goal self.statusActionGoal() # optional: print info for debugging print_info = False if print_info: print("contr") print("index: ", index, " closest_point:", closest_pt) print("Self yaw: ", self.yaw, " Point Yaw: ", yaw) print("Error yaw: ", err_yaw) print("Error side: ", err_side, " x_component: ", dir_manta_frame[0]) print("Force x: ", force_x, " Force y: ", force_y, "Torque z: ", testWrench.torque.z) print("Curvature: ", curvature) print("tau depth hold:", tau_depth_hold) print("current depth: ", self.z) def getYawFrom2Pos(self, pos1, pos2): """ Get the angle between two points """ return np.arctan2((pos2.y - pos1.y), (pos2.x - pos1.x)) def positionCallback(self, msg): """ Callback on odometry/filtered at 20 Hz? Runs controller """ self.vehicle_odom = msg self.time = msg.header.stamp.to_sec() global roll, pitch, yaw orientation_q = msg.pose.pose.orientation orientation_list = [ orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w ] (roll, pitch, yaw) = euler_from_quaternion(orientation_list) self.x = msg.pose.pose.position.x self.y = msg.pose.pose.position.y self.z = msg.pose.pose.position.z self.roll = roll self.pitch = pitch self.yaw = yaw if (len(self.path) > 2): if not self.reached_goal: self.controller() else: pass #print("Goal reached") def findClosestPointIndex(self): """ Find closest point in global plan """ distArray = [] for pose in self.path: distArray.append( abs(self.x - pose.pose.position.x)**2 + abs(self.y - pose.pose.position.y)**2) minIndex = np.argmin(distArray) closest_pt = self.path[minIndex].pose.position self.drawMarker(closest_pt) return minIndex, closest_pt def drawMarker(self, position): """ Draw closest point in RVIZ """ marker = Marker() marker.pose.position.x = position.x marker.pose.position.y = position.y marker.header.frame_id = "manta/odom" marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 self.marker_pub.publish(marker)
class InspectPoint: def __init__(self): # init node rospy.init_node('inspect_unknown_point') # init variables self.run_controller = False self.x = 0 self.y = 0 self.z = 0 self.roll = 0 self.pitch = 0 self.yaw = 0 self.centre_of_rot = Point() self.desired_radius = 10 self.desired_depth = -0.5 # subscribers self.sub_pose = rospy.Subscriber('/odometry/filtered', Odometry, self.positionCallback, queue_size=1) # publishers self.pub_thrust = rospy.Publisher('/manta/thruster_manager/input', Wrench, queue_size=1) # action server setup self.action_server = actionlib.SimpleActionServer(name='inspect_point_srv', ActionSpec=LosPathFollowingAction, auto_start=False) self.action_server.register_goal_callback(self.goalCB) self.action_server.start() # pid regulators P_dist = 20 I_dist = 0 D_dist = 10 sat_dist = 5 self.PID_dist = PIDRegulator(P_dist, I_dist, D_dist, sat_dist) P_angle = 20 I_angle = 10 D_angle = 10 sat_angle = 5 self.PID_angle = PIDRegulator(P_angle, I_angle, D_angle, sat_angle) self.PID = AutopilotPID() def fixHeadingWrapping(self, err_yaw): """ Fix error in yaw such that manta rotates in the closest direction Example: Instead of rotating 3/2 pi clockwise, rotate -1/2 pi counter clockwise """ if err_yaw > math.pi: err_yaw = err_yaw - 2*math.pi if err_yaw < -math.pi: err_yaw = err_yaw + 2*math.pi return err_yaw def getVectorFromMantaToCentre(self): """ Get vector from manta to the centre point of the object it is inspecting """ vec_to_mid = np.array([0,0]) vec_to_mid[0] = self.centre_of_rot.x - self.x vec_to_mid[1] = self.centre_of_rot.y - self.y return vec_to_mid def distanceToMid(self): """ Get distance to the centre point of the object it is inspecting """ return np.sqrt(abs(self.x-self.centre_of_rot.x)**2 + abs(self.y - self.centre_of_rot.y)**2) def getYawFrom2Pos(self, pos1, pos2): """ Get the angle between two points expressed in the global frame """ return np.arctan2((pos2.y - pos1.y),(pos2.x - pos1.x)) def controller(self): # distance control dist = self.distanceToMid() err_dist = dist - self.desired_radius pid_output = self.PID_dist.regulate(err_dist, rospy.get_time()) vec_to_mid_glob_frame = self.getVectorFromMantaToCentre() rot_mat = np.array([[np.cos(self.yaw), -np.sin(self.yaw)],[np.sin(self.yaw), np.cos(self.yaw)]]) rot_mat = rot_mat.transpose() vec_to_mid_manta_frame = np.dot(rot_mat,vec_to_mid_glob_frame) force_vec = vec_to_mid_manta_frame * pid_output force_x = force_vec[0] force_y = force_vec[1] # angle control manta_pos = Point() manta_pos.x = self.x manta_pos.y = self.y desired_angle = self.getYawFrom2Pos(manta_pos, self.centre_of_rot) err_yaw = desired_angle - self.yaw err_yaw = self.fixHeadingWrapping(err_yaw) torque_z = self.PID_angle.regulate(err_yaw, rospy.get_time()) # sideways force to rotate around point # dont go sideways unless facing towards point max_side_force = 5 sideway_force = max(0, max_side_force - 5*err_yaw) force_y = force_y + sideway_force # depth control tau_depth_hold = self.PID.depthController(self.desired_depth, self.z, rospy.get_time()) # make wrench and publish wrench = Wrench() wrench.force.x = force_x wrench.force.y = force_y wrench.torque.z = torque_z wrench.force.z = tau_depth_hold self.pub_thrust.publish(wrench) PRINT_INFO = False if PRINT_INFO: print("Force_x: ", force_x, " Force_y: ", force_y, " torque_z: ", torque_z) print("Error distance: ", err_dist, " Error yaw: ", err_yaw) def goalCB(self): """ Callback that gets called when the server """ print("got goal") self.run_controller = True goal = self.action_server.accept_new_goal() self.centre_of_rot.x = goal.next_waypoint.x self.centre_of_rot.y = goal.next_waypoint.y self.desired_radius = goal.sphereOfAcceptance print("cor x: ", self.centre_of_rot.x) print("cor y: ", self.centre_of_rot.y) print("des rad:", self.desired_radius) def distanceBetweenPoseAndSelf(self, pose): """ Distance from a pose to manta """ return np.sqrt(self.x-pose.position.x)**2 + abs(self.y - pose.position.y)**2 def positionCallback(self, msg): """ Callback on odometry/filtered Updates current pose and runs controller """ self.vehicle_odom = msg self.time = msg.header.stamp.to_sec() global roll, pitch, yaw orientation_q = msg.pose.pose.orientation orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w] (roll,pitch,yaw) = euler_from_quaternion(orientation_list) self.x = msg.pose.pose.position.x self.y = msg.pose.pose.position.y self.z = msg.pose.pose.position.z self.roll = roll self.pitch = pitch self.yaw = yaw # run controller if self.run_controller: self.controller()
class LosPathFollowing(object): # create messages that are used to send feedback/result _feedback = LosPathFollowingFeedback() _result = LosPathFollowingResult() def __init__(self): self.flag = False rospy.init_node('los_path_following') self.sub = rospy.Subscriber('/odometry/filtered', Odometry, self.callback, queue_size=1) # 20hz self.pub_thrust = rospy.Publisher('/manta/thruster_manager/input', Wrench, queue_size=1) # constructor object self.los = LOS() self.autopilot = AutopilotPID() self.reference_model = ReferenceModel(np.array((0, 0)), 0.05) # dynamic reconfigure self.config = {} self.srv_reconfigure = Server(AutopilotConfig, self.config_callback) """ action server guide https://github.com/strawlab/ros_common/blob/master/actionlib/src/actionlib/simple_action_server.py """ self.action_server = actionlib.SimpleActionServer(name='los_path', ActionSpec=LosPathFollowingAction, auto_start=False) self.action_server.register_goal_callback(self.goalCB) self.action_server.register_preempt_callback(self.preemptCB) self.action_server.start() def callback(self, msg): # update current position self.psi = self.los.quat2euler(msg) # update position and velocities self.los.updateState(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z, msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z, self.psi, msg.twist.twist.angular.z, msg.header.stamp.to_sec()) # update current goal self.psi_d = self.los.lookaheadBasedSteering() # reference model x_smooth = self.reference_model.discreteTustinMSD(np.array((2.0, self.psi_d))) print(x_smooth) # control force tau_d_heading = self.autopilot.headingController(self.psi_d, self.psi, self.los.t) # add speed controllers here thrust_msg = Wrench() thrust_msg.force.x = 1.0 thrust_msg.torque.z = tau_d_heading # 2.0*self.error_ENU if self.flag is True: # write to thrusters self.pub_thrust.publish(thrust_msg) # check if action goal succeeded self.statusActionGoal() def statusActionGoal(self): # feedback self._feedback.distanceToGoal = self.los.distance() self.action_server.publish_feedback(self._feedback) # succeeded if self.los.sphereOfAcceptance(): self._result.terminalSector = True self.action_server.set_succeeded(self._result) def preemptCB(self): # check that preempt has not been requested by the client if self.action_server.is_preempt_requested(): rospy.loginfo("Preempted requested by los path client") self.action_server.set_preempted() def goalCB(self): self.flag = True _goal = self.action_server.accept_new_goal() # set goal self.los.x_k = _goal.prev_waypoint.x self.los.y_k = _goal.prev_waypoint.y self.los.x_kp1 = _goal.next_waypoint.x self.los.y_kp1 = _goal.next_waypoint.y # forward speed self.speed = _goal.forward_speed.linear.x # sphere of acceptance self.los.R = _goal.sphereOfAcceptance def config_callback(self, config, level): """Handle updated configuration values.""" # Config has changed, reset PID controllers rospy.loginfo("""Reconfigure Request: {delta}, {p_rot}, {i_rot}, {d_rot}, {sat_rot} """.format(**config)) # update look-ahead distance self.los.delta = config['delta'] # self.pid_lin = PIDRegulator(config['pos_p'], config['pos_i'], config['pos_d'], config['pos_sat']) self.autopilot.updateGains(config['p_rot'], config['i_rot'], config['d_rot'], config['sat_rot']) # update config self.config = config return config
class LosPathFollowing(object): # create messages that are used to send feedback/result _feedback = LosPathFollowingFeedback() _result = LosPathFollowingResult() def __init__(self): self.flag = False rospy.init_node('los_path_following') self.sub = rospy.Subscriber('/odometry/filtered', Odometry, self.callback, queue_size=1) # 20hz self.pub_thrust = rospy.Publisher('/manta/thruster_manager/input', Wrench, queue_size=1) self.pub_desired = rospy.Publisher('/manta/los_desired', Odometry, queue_size=1) # constructor object self.los = LOS() self.PID = AutopilotPID() self.autopilot = AutopilotBackstepping() self.reference_model = ReferenceModel(np.array((0, 0)), self.los.h) # dynamic reconfigure self.config = {} self.srv_reconfigure = Server(AutopilotConfig, self.config_callback) """ action server guide https://github.com/strawlab/ros_common/blob/master/actionlib/src/actionlib/simple_action_server.py """ self.action_server = actionlib.SimpleActionServer(name='los_path', ActionSpec=LosPathFollowingAction, auto_start=False) self.action_server.register_goal_callback(self.goalCB) self.action_server.register_preempt_callback(self.preemptCB) self.action_server.start() def fixHeadingWrapping(self): e = self.psi - self.psi_ref if e < -math.pi: self.psi_ref = self.psi_ref - 2*math.pi if e > math.pi: self.psi_ref = self.psi_ref + 2*math.pi # reference model x_d = self.reference_model.discreteTustinMSD(np.array((self.los.speed, self.psi_ref))) psi_d = x_d[2] e = self.psi - psi_d if e > math.pi: psi_d = psi_d - 2*math.pi self.reference_model = ReferenceModel(np.array((self.los.u, self.los.psi)), self.los.h) x_d = self.reference_model.discreteTustinMSD(np.array((self.los.speed, psi_d))) if e < -math.pi: psi_d = psi_d + 2*math.pi self.reference_model = ReferenceModel(np.array((self.los.u, self.los.psi)), self.los.h) x_d = self.reference_model.discreteTustinMSD(np.array((self.los.speed, psi_d))) return x_d def callback(self, msg): # update current position self.psi = self.los.quat2euler(msg) # update position and velocities self.los.updateState(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z, msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z, self.psi, msg.twist.twist.angular.z, msg.header.stamp.to_sec()) # update current goal self.psi_ref = self.los.lookaheadBasedSteering() if self.flag is True: """ Wrapping would have been avoided by using quaternions instead of Euler angles if you don't care about wrapping, use this instead: x_d = self.reference_model.discreteTustinMSD(np.array((self.los.speed,psi_d))) """ x_d = self.fixHeadingWrapping() u_d = x_d[0] u_d_dot = x_d[1] psi_d = x_d[2] r_d = x_d[3] r_d_dot = x_d[4] los_msg = Odometry() los_msg.header.stamp = rospy.Time.now() quat_d = quaternion_from_euler(0, 0, psi_d) los_msg.pose.pose.position.z = msg.pose.pose.position.z los_msg.pose.pose.orientation.x = quat_d[0] los_msg.pose.pose.orientation.y = quat_d[1] los_msg.pose.pose.orientation.z = quat_d[2] los_msg.pose.pose.orientation.w = quat_d[3] los_msg.twist.twist.linear.x = u_d los_msg.twist.twist.angular.z = r_d self.pub_desired.publish(los_msg) # control force tau_d = self.autopilot.backstepping.controlLaw(self.los.u, self.los.u_dot, u_d, u_d_dot, self.los.v, self.psi, psi_d, self.los.r, r_d, r_d_dot) tau_depth_hold = self.PID.depthController(self.los.z_d, self.los.z, self.los.t) # add speed controllers here thrust_msg = Wrench() if tau_d[0] > 0.0: thrust_msg.force.x = tau_d[0] thrust_msg.force.y = tau_d[1] thrust_msg.force.z = tau_depth_hold thrust_msg.torque.z = tau_d[2] # 2.0*self.error_ENU # write to thrusters self.pub_thrust.publish(thrust_msg) # check if action goal succeeded self.statusActionGoal() def statusActionGoal(self): # feedback self._feedback.distanceToGoal = self.los.distance() self.action_server.publish_feedback(self._feedback) # succeeded if self.los.sphereOfAcceptance(): self._result.terminalSector = True self.action_server.set_succeeded(self._result, text="goal completed") self.flag = False def preemptCB(self): # check that preempt has not been requested by the client if self.action_server.is_preempt_requested(): rospy.loginfo("Preempted requested by los path client") self.action_server.set_preempted() self.flag = False def goalCB(self): self.flag = True _goal = self.action_server.accept_new_goal() # set goal self.los.x_k = self.los.x self.los.y_k = self.los.y self.los.x_kp1 = _goal.next_waypoint.x self.los.y_kp1 = _goal.next_waypoint.y # forward speed self.los.speed = _goal.forward_speed.linear.x # depth hold self.los.z_d = _goal.desired_depth.z # sphere of acceptance self.los.R = _goal.sphereOfAcceptance self.reference_model = ReferenceModel(np.array((self.los.u, self.los.psi)), self.los.h) def config_callback(self, config, level): """Handle updated configuration values.""" # Config has changed, reset PID controllers rospy.loginfo("""Reconfigure Request: {delta}, {p_rot}, {i_rot}, {d_rot}, {sat_rot} """.format(**config)) # update look-ahead distance self.los.delta = config['delta'] # self.pid_lin = PIDRegulator(config['pos_p'], config['pos_i'], config['pos_d'], config['pos_sat']) self.autopilot.updateGains(config['p_rot'], config['i_rot'], config['d_rot'], config['sat_rot']) # update config self.config = config return config