def __init__(self, search_target, search_area):
State.__init__(self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=[
'px_input', 'fx_input', 'search_input',
'search_confidence_input'
])
# initialize controller
self.CameraPID = CameraPID()
self.pub_thrust = rospy.Publisher('/manta/thruster_manager/input',
Wrench,
queue_size=1)
# thrust message
self.thrust_msg = Wrench()
# my current pose
self.vehicle_odom = Odometry()
self.sub_pose = rospy.Subscriber('/odometry/filtered',
Odometry,
self.positionCallback,
queue_size=1)
# set init_flag
self.init_flag = True
# straight-line path segment
self.search_target = search_target
self.search_y = search_area.y
self.search_x = search_area.x
def __init__(self, target):
State.__init__(self,
outcomes=['found', 'unseen', 'passed', 'missed'],
output_keys=[
'px_output', 'fx_output', 'search_output',
'search_confidence_output'
])
self.target = target
self.search_timeout = 30.0
self.sampling_time = 0.2
self.timer = 0.0
self.task_status = 'missed'
self.CameraPID = CameraPID()
self.sub_object = rospy.Subscriber('/pole_midpoint',
CameraObjectInfo,
self.objectDetectionCallback,
queue_size=1)
self.sub_pose = rospy.Subscriber('/odometry/filtered',
Odometry,
self.positionCallback,
queue_size=1)
print(self.sub_pose)
self.pub_thrust = rospy.Publisher('/manta/thruster_manager/input',
Wrench,
queue_size=1)
self.thrust_msg = Wrench()
class TrackTarget(State):
def __init__(self, search_target, search_area):
State.__init__(self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=[
'px_input', 'fx_input', 'search_input',
'search_confidence_input'
])
# initialize controller
self.CameraPID = CameraPID()
self.pub_thrust = rospy.Publisher('/manta/thruster_manager/input',
Wrench,
queue_size=1)
# thrust message
self.thrust_msg = Wrench()
# my current pose
self.vehicle_odom = Odometry()
self.sub_pose = rospy.Subscriber('/odometry/filtered',
Odometry,
self.positionCallback,
queue_size=1)
# set init_flag
self.init_flag = True
# straight-line path segment
self.search_target = search_target
self.search_y = search_area.y
self.search_x = search_area.x
def positionCallback(self, msg):
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.psi = yaw
def pathPlanning(self):
# straight-line path segment
y_delta = self.search_y - self.vehicle_odom.pose.pose.position.y
x_delta = self.search_x - self.vehicle_odom.pose.pose.position.x
# angle
self.search_direction = np.arctan2(y_delta, x_delta)
def pathTracking(self):
pass
def nav_result_cb(self, userdata, status, result):
if status == GoalStatus.PREEMPTED:
rospy.loginfo("Waypoint preempted")
if status == GoalStatus.SUCCEEDED:
rospy.loginfo("Waypoint succeeded")
def alignWithTarget(self, object_fx, object_px, search_input,
object_confidence):
tau_heave = self.CameraPID.depthController(
-0.5, self.vehicle_odom.pose.pose.position.z, self.time)
self.thrust_msg.force.z = tau_heave
target_center_screen = object_fx * (0.60)
if search_input == 'found' and object_confidence >= 1.0:
# fix bounding boxes, their center values are calculated wrong and are not in center
tau_sway = self.CameraPID.swayController(target_center_screen,
object_px, self.time)
tau_surge = self.CameraPID.speedController(
0.1, self.vehicle_odom.twist.twist.linear.x, self.time)
# you're on the right path, keep this heading
tau_heading = self.CameraPID.headingController(
self.psi, self.psi, self.time)
#publish
self.thrust_msg.torque.z = tau_heading
self.thrust_msg.force.x = tau_surge
self.thrust_msg.force.y = tau_sway
else:
tau_surge = self.CameraPID.speedController(
0.3, self.vehicle_odom.twist.twist.linear.x, self.time)
tau_heading = self.CameraPID.headingController(
self.search_direction, self.psi, self.time)
tau_sway = 0.0
#publish
self.thrust_msg.torque.z = tau_heading
self.thrust_msg.force.x = tau_surge
self.thrust_msg.force.y = tau_sway
self.pub_thrust.publish(self.thrust_msg)
def execute(self, userdata):
sleep(0.2)
# track the target
nav_goal = LosPathFollowingGoal()
# initialize the direction you want to be looking
if self.init_flag is True:
self.pathPlanning()
self.init_flag = False
self.alignWithTarget(userdata.fx_input, userdata.px_input,
userdata.search_input,
userdata.search_confidence_input)
self.pathTracking()
return 'succeeded'