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