def rosmsg(self):
""":obj:`sensor_msgs.CamerInfo` : Returns ROS CamerInfo msg
"""
msg_header = Header()
msg_header.frame_id = self._frame
msg_roi = RegionOfInterest()
msg_roi.x_offset = 0
msg_roi.y_offset = 0
msg_roi.height = 0
msg_roi.width = 0
msg_roi.do_rectify = 0
msg = CameraInfo()
msg.header = msg_header
msg.height = self._height
msg.width = self._width
msg.distortion_model = 'plumb_bob'
msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.K = [
self._fx, 0.0, self._cx, 0.0, self._fy, self._cy, 0.0, 0.0, 1.0
]
msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
msg.P = [
self._fx, 0.0, self._cx, 0.0, 0.0, self._fx, self._cy, 0.0, 0.0,
0.0, 1.0, 0.0
]
msg.binning_x = 0
msg.binning_y = 0
msg.roi = msg_roi
return msg
def callback(image_msg):
global pub_Image
global pub_BoxesImage
global pub_Boxes
global face_cascade
global bridge
global boxes_msg
global faces
global local_position_pub
global goal_pose
# Convert ros image into a cv2 image
cv_img = bridge.imgmsg_to_cv2(image_msg, desired_encoding="passthrough")
cv_gray = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
# Find the faces and draw a rectangle around them
faces = face_cascade.detectMultiScale(cv_gray, 1.3, 5)
for (x, y, w, h) in faces:
cv_img = cv2.rectangle(cv_img, (x, y), (x + w, y + h), (255, 0, 0), 2)
# For each image, we also want to publish the croped image
crop_img = cv_img[y:y + h, x:x + w]
crop_msg = bridge.cv2_to_imgmsg(crop_img, encoding="bgr8")
# And the region of interest information
pub_BoxesImage.publish(crop_msg)
boxes_msg = RegionOfInterest()
boxes_msg.x_offset = x
boxes_msg.y_offset = y
boxes_msg.height = h
boxes_msg.width = w
boxes_msg.do_rectify = False
pub_Boxes.publish(boxes_msg)
# Publish the original image with the rectangles
mod_img = bridge.cv2_to_imgmsg(cv_img, encoding="bgr8")
pub_Image.publish(mod_img)
def camera_info(self):
msg_header = Header()
msg_header.frame_id = "f450/robot_camera_down"
msg_roi = RegionOfInterest()
msg_roi.x_offset = 0
msg_roi.y_offset = 0
msg_roi.height = 0
msg_roi.width = 0
msg_roi.do_rectify = 0
msg = CameraInfo()
msg.header = msg_header
msg.height = 480
msg.width = 640
msg.distortion_model = 'plumb_bob'
msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.K = [1.0, 0.0, 320.5, 0.0, 1.0, 240.5, 0.0, 0.0, 1.0]
msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
msg.P = [
1.0, 0.0, 320.5, -0.0, 0.0, 1.0, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0
]
msg.binning_x = 0
msg.binning_y = 0
msg.roi = msg_roi
return msg
def converts_to_ObjectInBoxe(image_size,
ymin,
xmin,
ymax,
xmax,
probability=(),
object_name_list=(),
use_normalized_coordinates=True,
do_rectify=False):
"""Adds a bounding box to an image.
Bounding box coordinates can be specified in either absolute (pixel) or
normalized coordinates by setting the use_normalized_coordinates argument.
Each string in display_str_list is displayed on a separate line above the
bounding box in black text on a rectangle filled with the input 'color'.
If the top of the bounding box extends to the edge of the image, the strings
are displayed below the bounding box.
Args:
image_size: image_size.
ymin: ymin of bounding box.
xmin: xmin of bounding box.
ymax: ymax of bounding box.
xmax: xmax of bounding box.
object_name_list: list of strings to display in box
(each to be shown on its own line).
use_normalized_coordinates: If True (default), treat coordinates
ymin, xmin, ymax, xmax as relative to the image. Otherwise treat
coordinates as absolute.
"""
im_height, im_width = image_size
if use_normalized_coordinates:
(left, right, top, bottom) = (xmin * im_width, xmax * im_width,
ymin * im_height, ymax * im_height)
else:
(left, right, top, bottom) = (xmin, xmax, ymin, ymax)
object = Object()
object.object_name = object_name_list[0]
object.probability = probability[0]
roi = RegionOfInterest()
roi.x_offset = int(left)
roi.y_offset = int(top)
roi.height = int(bottom - top)
roi.width = int(right - left)
roi.do_rectify = do_rectify
object_in_box = ObjectInBox()
object_in_box.object = object
object_in_box.roi = roi
return object_in_box
def execute_cb(self, goal):
rospy.loginfo("Goal Received!")
self.image_sub = rospy.Subscriber(self._sub_topic,
CompressedImage,
self.receiveImage,
queue_size=1,
buff_size=1000000000)
self.received_image = False
timeout = rospy.Time.now() + rospy.Duration(5, 0)
while not self.received_image and (rospy.Time.now() < timeout):
pass
self.image_sub.unregister()
if not self.received_image:
rospy.logerr(
"No image received from the subscribed topic : %s before timeout!!",
self._sub_topic)
self._as.set_aborted(text=str(
"No image received from the subscribed topic before timeout!!")
)
return
result = inference_server.msg.InferenceResult()
self.inference_output, num_detected, detected_classes, detected_scores, detected_boxes = object_detection.detect(
self.inference_input)
self.inference_image = CompressedImage()
self.inference_image.header.stamp = rospy.Time.now()
self.inference_image.format = "jpeg"
self.inference_image.data = np.array(
cv2.imencode('.jpg', self.inference_output)[1]).tostring()
self.image_pub.publish(self.inference_image)
result.image = self.inference_image
result.num_detections = num_detected
result.classes = detected_classes
result.scores = detected_scores
for i in range(len(detected_boxes)):
box = RegionOfInterest()
box.y_offset = detected_boxes[i][0]
box.height = (detected_boxes[i][2] - detected_boxes[i][0])
box.x_offset = detected_boxes[i][1]
box.width = (detected_boxes[i][3] - detected_boxes[i][1])
box.do_rectify = True
result.bounding_boxes.append(box)
try:
self._as.set_succeeded(result)
except Exception as e:
rospy.logerr(str(e))
self._as.set_aborted(text=str(e))
def camera_info_left(self):
msg_header = Header()
msg_header.frame_id = "uav_camera_down"
msg_roi = RegionOfInterest()
msg_roi.x_offset = 0
msg_roi.y_offset = 0
msg_roi.height = 0
msg_roi.width = 0
msg_roi.do_rectify = 0
msg = CameraInfo()
msg.header = msg_header
msg.height = 480
msg.width = 640
msg.distortion_model = 'plumb_bob'
msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.K = [1.0, 0.0, 320.5, 0.0, 1.0, 240.5, 0.0, 0.0, 1.0]
msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
msg.P = [1.0, 0.0, 320.5, -0.0, 0.0, 1.0, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0]
msg.binning_x = 0
msg.binning_y = 0
msg.roi = msg_roi
return msg
def camera_info_right(self):
msg_header = Header()
msg_header.frame_id = "uav_camera_down"
msg_roi = RegionOfInterest()
msg_roi.x_offset = 0
msg_roi.y_offset = 0
msg_roi.height = 0
msg_roi.width = 0
msg_roi.do_rectify = 0
msg = CameraInfo()
msg.header = msg_header
msg.height = 480
msg.width = 640
msg.distortion_model = 'plumb_bob'
msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.K = [1.0, 0.0, 320.5, 0.0, 1.0, 240.5, 0.0, 0.0, 1.0]
msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
msg.P = [1.0, 0.0, 320.5, -0.0, 0.0, 1.0, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0]
msg.binning_x = 0
msg.binning_y = 0
msg.roi = msg_roi
return msg
def depth_estm(li,ri)
stereo = cv2.StereoBM_create(numDisparities=16, blockSize=5)
def execute_cb(self, goal):
rospy.loginfo("Goal Received!")
start_time = rospy.Time.now()
result = inference_server.msg.InferenceResult()
if not goal.input_image.data:
self.inference_output, num_detected, detected_classes, detected_scores, detected_boxes = object_detection.detect(
self.inference_input)
else:
np_arr = np.fromstring(goal.input_image.data, np.uint8)
goal_inference_input = cv2.imdecode(np_arr,
cv2.CV_LOAD_IMAGE_COLOR)
self.inference_output, num_detected, detected_classes, detected_scores, detected_boxes = object_detection.detect(
goal_inference_input)
self.inference_image = CompressedImage()
self.inference_image.header.stamp = rospy.Time.now()
self.inference_image.format = "jpeg"
self.inference_image.data = np.array(
cv2.imencode('.jpg', self.inference_output)[1]).tostring()
self.image_pub.publish(self.inference_image)
result.image = self.inference_image
result.num_detections = num_detected
result.classes = detected_classes
result.scores = detected_scores
for i in range(len(detected_boxes)):
box = RegionOfInterest()
box.y_offset = detected_boxes[i][0]
box.height = (detected_boxes[i][2] - detected_boxes[i][0])
box.x_offset = detected_boxes[i][1]
box.width = (detected_boxes[i][3] - detected_boxes[i][1])
box.do_rectify = True
result.bounding_boxes.append(box)
total_inference_time = rospy.Time.now() - start_time
total_inference_time = total_inference_time.to_sec()
rospy.loginfo(
"The inference took {} seconds".format(total_inference_time))
try:
self._as.set_succeeded(result)
except Exception as e:
rospy.logerr(str(e))
self._as.set_aborted(text=str(e))
def compute_roi_from_indices(indices, width, height, padding = 30):
xvals = [ind - width*(math.floor(ind / width)) for ind in indices]
yvals = [math.floor(ind / width) for ind in indices]
roi = RegionOfInterest()
roi.x_offset = max(0, min(xvals) - padding)
roi.y_offset = max(0, min(yvals) - padding)
roi.height = max(yvals) - roi.y_offset + padding
if roi.height + roi.y_offset > height:
roi.height = height - roi.y_offset
roi.width = max(xvals) - roi.x_offset + padding
if roi.width + roi.x_offset > width:
roi.width = width - roi.x_offset
roi.do_rectify = 0
return roi
def callback(image_msg):
global pub_Image
global pub_BoxesImage
global pub_Boxes
global tensorflowNet
global bridge
global boxes_msg
global local_position_pub
global goal_pose
global current_pose
# Adjustable parameters
commandQuadFlag = False # set to true to actually steer quad
heatmapThresh = 0.5 # threshold heatmap by this before centroid calculation
nnFramesize = (64,48) # must match trained NN expectation
scaleY = 0.1 # horizontal image fractional offset to meters conversion
scaleZ = 0.1 # vertical image fractional offset to meters conversion
# Pull image from topic and convert ros image into a cv2 image
cv_img = bridge.imgmsg_to_cv2(image_msg, desired_encoding="passthrough")
cv_gray = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
# Resize to appropriate size for neural network input
nnFrame = cv2.resize(cv_gray,nnFramesize)
nnFrame = nnFrame[:,:,0] * float(1.0/255.0)
nnFrame = np.squeeze(nnFrame)
# Execute a forward pass of the neural network on the frame to get a
# heatmap of target likelihood
tensorflowNet.setInput(nnFrame)
heatmap = tensorflowNet.forward()
heatmap = np.squeeze(heatmap)
# Find the centroid
heatmap = heatmap*255.0 # scale appropriately
centroid = find_centerOfMass(heatmap, minThresh=heatmapThresh*255)
# Pull the actual pose
actual_pose = current_pose
# Convert the centroid to a setpoint position
centroid_frac = [(centroid[0] - nnFramesize[0])/nnFramesize[0], \
(centroid[1] - nnFramesize[1])/nnFramesize[1]]
setpoint_x = actual_pose.pose.position.x # Don't move in x just yet
setpoint_y = centroid_frac[0]*scaleY + actual_pose.pose.position.y
setpoint_z = centroid_frac[1]*scaleZ + actual_pose.pose.position.z
# Change setpoint position
if commandQuadFlag:
# DON'T ACTUALLY DO THIS UNTIL READY!
# Use the new setpoint
goal_pose.pose.position.x = setpoint_x
goal_pose.pose.position.y = setpoint_y
goal_pose.pose.position.z = setpoint_z
goal_pose.pose.orientation = actual_pose.pose.orientation
else:
# Don't change the setpoint position - just copy the pose
goal_pose.pose.position.x = actual_pose.pose.position.x
goal_pose.pose.position.y = actual_pose.pose.position.y
goal_pose.pose.position.z = actual_pose.pose.position.z
goal_pose.pose.orientation = actual_pose.pose.orientation
# Publish resized image as input to the NN
mod_img = bridge.cv2_to_imgmsg(nnFrame, encoding="bgr8") # this may not be right...
pub_Image.publish(mod_img)
# Draw a little rectangle at the quad centroid and publish. Do full size rectangle later
imagePlusBox = cv2.rectangle(nnFrame, (centroid[0]-1,centroid[1]-1), (centroid[0]+1,centroid[1]+1), (255,0,0), 2)
boxes_img = bridge.cv2_to_imgmsg(imagePlusBox, encoding="bgr8")
pub_BoxesImage.publish(boxes_img)
# Publish the centroid box coordinates themselves
boxes_msg = RegionOfInterest()
boxes_msg.x_offset = centroid[0]-1
boxes_msg.y_offset = centroid[1]-1
boxes_msg.height = 2
boxes_msg.width = 2
boxes_msg.do_rectify = False
pub_Boxes.publish(boxes_msg)
# Publish the actual setpoint position
# Make sure this is nice and smooth and what we want in stationary testing
# before actually using it as a flight control. Can even have everything but
# the actual command running without actually flying the quad.
if commandQuadFlag:
local_position_pub.publish(goal_pose)
def publish(self,
boxes,
scores,
classes,
num,
category_index,
pixels=[],
points=[],
masks=None,
fps=0):
msg = Fusion()
dists = []
angles = []
for i in range(boxes.shape[0]):
if category_index[classes[i]]['name'] == 'person':
if scores[i] > 0.4:
class_name = 'person'
ymin, xmin, ymax, xmax = tuple(boxes[i].tolist())
ymin = ymin * 600
xmin = xmin * 600
ymax = ymax * 600
xmax = xmax * 600
box = RegionOfInterest()
box.x_offset = np.abs(xmin + (xmax - xmin) / 2.0)
box.y_offset = np.abs(ymin + (ymax - ymin) / 2.0)
box.height = ymax - ymin
box.width = xmax - xmin
box.do_rectify = True
# FUSION PART - calcualte distance and angle
j = 0
valid_points = []
for pixel in pixels:
if pixel[0] > xmin and pixel[0] < xmax and pixel[
1] > ymin and pixel[1] < ymax:
valid_points.append([points[j][0], points[j][1]])
j = j + 1
### KNN CODE
if len(valid_points) >= 6:
dist, x, y = get_center_from_KMeans(valid_points)
if dist[0] < dist[1]:
ang = np.arctan2(y[0], -x[0]) * (180 / np.pi)
msg.distance.append(dist[0])
msg.angle.append(ang)
dists.append(dist[0])
angles.append(ang)
else:
ang = np.arctan2(y[1], -x[1]) * (180 / np.pi)
msg.distance.append(dist[1])
msg.angle.append(ang)
dists.append(dist[1])
angles.append(ang)
### N-points CODE(9)
#if len(valid_points) >= 9:
# dist, ang = get_center_from_mid_points(valid_points)
# msg.distance.append(dist)
# msg.angle.append(ang)
# dists.append(dist)
# angles.append(ang)
### DBSCAN CODE
#if len(valid_points) >= 6:
# dist, ang = get_center_from_DBSCAN(valid_points)
# msg.distance.append(dist)
# msg.angle.append(ang)
# dists.append(dist)
# angles.append(ang)
###
else:
dists.append('None')
angles.append('None')
# fill detection message with objects
obj = Object()
obj.box = box
obj.class_name = class_name
obj.score = int(100 * scores[i])
if masks is not None:
obj.mask = self._bridge.cv2_to_imgmsg(
masks[i], encoding="passthrough")
msg.objects.append(obj)
msg.fps = fps
self.FuPub.publish(msg)
return dists, angles
import rospy
from sensor_msgs.msg import CameraInfo
from sensor_msgs.msg import RegionOfInterest
from sensor_msgs.srv import SetCameraInfo
def set_camera_info_client(camerainfo):
rospy.wait_for_service('/thermal/set_camera_info')
try:
set_camera_info = rospy.ServiceProxy('/thermal/set_camera_info',
SetCameraInfo)
set_camera_info(camerainfo)
pass
except rospy.ServiceException, e:
print "Service call failed: %s" % e
if __name__ == "__main__":
camerainfo = CameraInfo()
camerainfo.height = 256
camerainfo.width = 320
RoiInfo = RegionOfInterest()
RoiInfo.x_offset = 20
RoiInfo.y_offset = 20
RoiInfo.height = 10
RoiInfo.width = 10
RoiInfo.do_rectify = True
camerainfo.roi = RoiInfo
set_camera_info_client(camerainfo)
def camera_info_back(self):
msg_header = Header()
msg_header.frame_id = "f450/robot_camera_down"
msg_roi = RegionOfInterest()
msg_roi.x_offset= 0
msg_roi.y_offset= 0
msg_roi.height = 0
msg_roi.width = 0
msg_roi.do_rectify=0
msg= CameraInfo()
msg.header = msg_header
msg.height = 480
msg.width = 640
msg.distortion_model = 'plumb_bob'
msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.K = [1.0, 0.0, 320.5, 0.0, 1.0, 240.5, 0.0, 0.0,1.0]
msg.R = [1.0, 0.0,0.0,0.0,0.866,0.5,0.0,-0.5,8.66]
msg.P = [1.0, 0.0, 320.5,0.0,0.0,1.0,240.5,0.0,0.0,0.0,1.0,0.0] #same as the front camera
msg.binning_x = 0
msg.binning_y = 0
msg.roi = msg_roi
return msg
def camera_info_front(self):
msg_header = Header()
msg_header.frame_id = "f450/robot_camera"
msg_roi = RegionOfInterest()
msg_roi.x_offset= 0
msg_roi.y_offset= 0
msg_roi.height = 0
msg_roi.width = 0
msg_roi.do_rectify=0
msg= CameraInfo()
msg.header = msg_header
msg.height = 480
msg.width = 640
msg.distortion_model = 'plumb_bob'
msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.K = [1.0, 0.0, 320.5, 0.0, 1.0, 240.5, 0.0, 0.0,1.0]
msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,0.0, 1.0]
msg.P = [1.0, 0.0, 320.5,0.0,0.0,1.0,240.5,0.0,0.0,0.0,1.0,0.0]
msg.binning_x = 0
msg.binning_y = 0
msg.roi = msg_roi
return msg
def convert_pixel_camera(self):
pass
def mocap_cb(self,msg):
self.curr_pose = msg
def state_cb(self,msg):
if msg.mode == 'OFFBOARD':
self.isReadyToFly = True
else:
print msg.mode
def update_state_cb(self,data):
self.mode= data.data
print self.mode
def tag_detections(self,msgs):
rate = rospy.Rate(30)
if len(msgs.detections)>0:
msg = msgs.detections[0].pose # The first element in the array is the pose
self.tag_pt= msg.pose.pose.position
self.pub.publish("FOUND UAV")
else:
if self.mode == "DESCENT":
self.pub.publish("MISSING UAV")
def get_descent(self, x, y,z):
des_vel = PositionTarget()
des_vel.header.frame_id = "world"
des_vel.header.stamp= rospy.Time,from_sec(time.time())
des_vel.coordinate_frame= 8
des_vel.type_mask = 3527
des_vel.velocity.x = x
des_vel.velocity.y = y
des_vel.velocity.z = z
return des_vel
def lawnmover(self, rect_x, rect_y, height, offset, offset_x):
if len(self.loc)== 0 :
takeoff = [self.curr_pose.pose.postion.x, self.curr_pose.pose.postion.y, height , 0 , 0 , 0 , 0]
move_to_offset = [self.curr_pose.pose.postion_x + offset, self.curr_posr.pose.postion_y - rect_y/2, height, 0 , 0 , 0 ,0 ]
self.loc.append(takeoff)
self.loc.append(move_to_offset)
left = True
while True:
if left:
x = self.loc[len(self.loc)-1][0]
y = self.loc[len(self.loc)-1][1] + rect_y/3
z = self.loc[len(self.loc)-1][2]
self.loc.append([x,y,z,0,0,0,0])
x = self.loc[len(self.loc)-1][0]
y = self.loc[len(self.loc)-1][1] + rect_y/3
z = self.loc[len(self.loc)-1][2]
self.loc.append([x,y,z,0,0,0,0])
x = self.loc[len(self.loc)-1][0]
y = self.loc[len(self.loc)-1][1] + rect_y/3
z = self.loc[len(self.loc)-1][2]
left = False
x = self.loc[len(self.loc)-1][0] + offset_x
y = self.loc[len(self.loc)-1][0]
z = self.loc[len(self.loc)-1][0]
self.loc.append([x,y,z,0,0,0,0])
if x > rect_x :
break
else:
x = self.loc[len(self.loc)-1][0]
y = self.loc[len(self.loc)-1][1]-rect_y/3
z = self.loc[len(self.loc)-1][2]
self.loc.append([x,y,z,0,0,0,0])
x = self.loc[len(self.loc)-1][0]
y = self.loc[len(self.loc)-1][1]-rect_y/3
z = self.loc[len(self.loc)-1][2]
self.loc.append([x,y,z,0,0,0,0])
x = self.loc[len(self.loc)-1][0]
y = self.loc[len(self.loc)-1][1]-rect_y/3
z = self.loc[len(self.loc)-1][2]
self.loc.append([x,y,z,0,0,0,0])
left = True
x = self.loc[len(self.loc)-1][0]+ offset_x
y = self.loc[len(self.loc)-1][1]
z = self.loc[len(self.loc)-1][2]
self.loc.append([x,y,z,0,0,0,0])
if x > rect_x:
break
rate = rospy.Rate(10)
self.des_pose = self.copy_pose(self.curr_pose) #Why do we need to copy curr_pose into des_pose
shape = len(self.loc)
pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size = 10)
print self.mode
while self.mode == "SURVEY" and not rospy.is_shutdown():
if self.waypointIndex == shape :
self.waypointIndex = 1 # resetting the waypoint index
if self.isReadyToFly:
des_x = self.loc[self.waypointIndex][0]
des_y = self.loc[self.waypointIndex][1]
des_z = self.loc[self.waypointIndex][2]
self.des_pose.pose.position.x = des_x
self.des_pose.pose.position.y = des_y
self.des_pose.pose.position.z = des_z
self.des_pose.pose.orientation.x = self.loc[self.waypointIndex][3]
self.des_pose.pose.orientation.y = self.loc[self.waypointIndex][4]
self.des_pose.pose.orientation.z = self.loc[self.waypointIndex][5]
self.des_pose.pose.orientation.w = self.loc[self.waypointIndex][6]
curr_x = self.curr_pose.pose.position.x
curr_y = self.curr_pose.pose.position.y
curr_z = self.curr_pose.pose.position.z
dist = math.sqrt((curr_x - des_x)*(curr_x - des_x) + (curr_y - des_y)*(curr_y - des_y ) + (curr_z - des_z)(curr_z - des_z))
if dist < self.distThreshold:
self.waypointIndex += 1
pose_pub.publish(self.des_pose)
rate.sleep()
def hover(self):
location = self.hover_loc
loc = [location,
location,
location,
location,
location,
location,
location,
location,
loaction]
rate = rospy.Rate(10)
rate.sleep()
sharp = len(loc)
pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size= 10)
des_pose = self.copy_pose(self.curr_pose)
waypoint_index = 0
sim_ctr = 1
print self.mode
while self.mode == "HOVER" and not rospy.is_shutdown():
if waypoint_index==shape:
waypoint_index = 0 # changing the way point index to 0
sim_ctr = sim_ctr + 1
print "HOVER STOP COUNTER:" + str(sim_ctr)
if self.isReadyToFly:
des_x = loc[waypoint_index][0]
des_y = loc[waypoint_index][1]
des_z = loc[waypoint_index][2]
des_pose.pose.position.x = des_x
des_pose.pose.position.y = des_y
des_pose.pose.position.z = des_z
des_pose.pose.orientation.x = loc[waypoint_index][3]
des_pose.pose.orientation.y = loc[waypoint_index][4]
des_pose.pose.orientation.z = loc[waypoint_index][5]
des_pose.pose.oirentation.w = loc[waypoint_index][6]
curr_x = self.curr_pose.pose.position.x
curr_y = self.curr_pose.pose.position.y
curr_z = self.curr_pose.pose.position.z
dist = math.sqrt((curr_x - des_x)*(curr_x - des_x) + (curr_y - des_y)(curr_y - des_y) + (curr_z - des_z)*(curr_z - des_z))
if dist<self.distThreshold :
waypoint_index += 1
if dist < self.distThreshold:
waypoint_index += 1
if sim_ctr == 50:
pass
pose_pub.publish(des_pose)
rate.sleep()
def scan(self, rect_y, offset_x):
move= ""
rate=rospy.Rate(10)
if self.waypointIndex %4 ==1:
move="back"
elif((self.waypointIndex + (self.waypointIndex % 4))/4)%2 == 0:
move = "right"
else:
move = "left"
print self.mode
loc = self.curr_pose.pose.position
print loc
print rect_y
print offset_x
while self.mode == "SCAN" and not rospy.is_shutdown():
if move == "left":
self.vel_pub.publish(self.get_decent(0,0.5,0.1))
if abs(self.curr_pose.pose.position.y - loc.y) > rect_y/3
self.pub.publish("SCAN COMPLETE")
elif move == "right":
self.vel_pub.publish(self.get_decent(0,-0.5,0.1))
if abs(self.curr_pose.pose.postion.y - loc.y) > rect_y/3
self.pub.publish("SCAN COMPLETE")
elif move == "back":
self.vel_pub.publish(self.get_decent(-0.3,0,1))
if abs(self.curr_pose.pose.position.x - loc.x) > offset_x:
self.pub.publish("SCAN COMPLETE")
else:
print "move error"
print abs(self.curr_pose.pose.position.y - loc.y)
print abs(self.curr_pose.pose.position.x - loc.x)
rate.sleep()
def descent(self):
rate = rospy.Rate(10) # 10 Hz
time_step = 1/10
print self.mode
self.x_change = 1
self.y_change = 1
self.x_prev_error = 0
self.y_prev_error = 0
self.x_sum_error=0
self.y_sum_error=0
self.x_exp_movement=0
self.y_exp_movement=0
self.x_track = 0
self.y_track = 0
self.curr_xerror=0
self.curr_yerror=0
while self.mode == "DESCENT" and self.curr_pose.pose.position.z > 0.2 and not rospy.is_shutdown():
err_x = self.curr_pose.pose.position.x - self.tag_pt.x
err_y = self.curr_pose.pose.position.y - self.tag_pt.y
self.x_change += err_x*self.KP + (((self.x_prev_error-self.curr_xerror)/time_step)* self.KD) + (self.x_sum_error * self.KI*time_step)
self.y_change += err_y*self.KP + (((self.y_prev_error-self.curr_yerror)/time_step)* self.KD) + (self.y_sum_error * self.KI*time_step)
self.x_change = max(min(0.4,self.x_change), -0.4)
self.y_change = max(min(0.4,self.y_change), -0.4)
if err_x > 0 and err_y < 0:
des = self.get_descent(-1*abs(self.x_change), 1*abs(self.y_change), -0.08)
elif err_x > 0 and err_y > 0:
des = self.get_descent(-1*abs(self.x_change), -1*abs(self.y_change), -0.08)
elif err_x < 0 and err_y > 0:
des = self.get_descent(1*abs(self.x_change), -1*abs(self.y_change), -0.08)
elif err_x < 0 and err_y < 0:
des = self.get_descent(1*abs(self.x_change), 1*abs(self.y_change), -0.08)
else:
des = self.get_descent(self.x_change, self.y_change, -0.08)
self.vel_pub.publish(des)
rate.sleep()
self.x_exp_movement = self.x_change
self.y_exp_movement = self.y_change
self.x_track = self.x_exp_movement + self.curr_pose.pose.position.x
self.y_track = self.y_exp_movement + self.curr_pose.pose.position.y
self.curr_xerror= self.x_track - self.tag_pt.x
self.curr_yerror= self.y_track - self.tag_pt.y
self.x_prev_error= err_x
self.y_prev_error= err_y
self.x_sum_error += err_x
self.y_sum_error += err_y
if self.curr_pose.pose.position.z < 0.2:
#Perform the necessary action to complete pickup instruct actuators
self.pub.publish("PICKUP COMPLETE")
def yolo_descent(self):
rate= rospy.Rate(10)
print self.mode
self.x_change = 1
self.y_change = 1
self.x_prev_error=0
self.y_prev_error=0
self.x_sum_error=0
self.y_sum_error=0
timeout = 120
yolo_KP = 0.08
yolo_KD = 0.004
yolo_KI = 0.0005
while self.mode == "DESCENT" and not rospy.is_shutdown():
err_x = 0 - self.target[0]
err_y = 0 - self.target[1]
self.x_change += err_x * yolo_KP + (self.x_prev_error * yolo_KD) + (self.x_sum_error * yolo_KI)
self.y_change += err_y * yolo_KP + (self.y_prev_error * yolo_KD) + (self.y_sum_error * yolo_KI)
self.x_change = max(min(0.4, self.x_change), -0.4)
self.y_change = max(min(0.4, self.y_change), -0.4)
if err_x > 0 and err_y < 0:
des = self.get_descent(-1 * self.x_change, -1 * self.y_change, -0.08)
elif err_x < 0 and err_y > 0:
des = self.get_descent(-1 * self.x_change, -1 * self.y_change, -0.08)
else:
des = self.get_descent(self.x_change, self.y_change, -0.08)
self.vel_pub.publish(des)
timeout -= 1
rate.sleep()
self.x_prev_error = err_x
self.y_prev_error = err_y
self.x_sum_error += err_x
self.y_sum_error += err_y
if timeout == 0 and self.curr_pose.pose.position.z > 0.7:
timeout = 120
print timeout
self.x_change = 0
self.y_change = 0
self.x_sum_error = 0
self.y_sum_error = 0
self.x_prev_error = 0
self.y_prev_error = 0
if self.curr_pose.pose.position.z < 0.2: # TODO
# self.mode = "HOVER" # PICK UP
# self.hover_loc = [self.curr_pose.pose.position.x] # TODO
self.pub.publish("PICKUP COMPLETE")
def rt_survey(self):
location = [self.saved_location.pose.position.x,
self.saved_location.pose.position.y,
self.saved_location.pose.position.z,
0, 0, 0, 0]
loc = [location,
location,
location,
location,
location]
rate = rospy.Rate(10)
rate.sleep()
shape = len(loc)
pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
des_pose = self.copy_pose(self.curr_pose)
waypoint_index = 0
sim_ctr = 1
print self.mode
while self.mode == "RT_SURVEY" and not rospy.is_shutdown():
if waypoint_index == shape:
waypoint_index = 0
sim_ctr += 1
if self.isReadyToFly:
des_x = loc[waypoint_index][0]
des_y = loc[waypoint_index][1]
des_z = loc[waypoint_index][2]
des_pose.pose.position.x = des_x
des_pose.pose.position.y = des_y
des_pose.pose.position.z = des_z
des_pose.pose.orientation.x = loc[waypoint_index][3]
des_pose.pose.orientation.y = loc[waypoint_index][4]
des_pose.pose.orientation.z = loc[waypoint_index][5]
des_pose.pose.orientation.w = loc[waypoint_index][6]
curr_x = self.curr_pose.pose.position.x
curr_y = self.curr_pose.pose.position.y
curr_z = self.curr_pose.pose.position.z
dist = math.sqrt((curr_x - des_x)*(curr_x - des_x) + (curr_y - des_y)*(curr_y - des_y) +
(curr_z - des_z)*(curr_z - des_z))
if dist < self.distThreshold:
waypoint_index += 1
if sim_ctr == 5:
self.pub.publish("RTS COMPLETE")
self.saved_location = None
pose_pub.publish(des_pose)
rate.sleep()
def controller(self):
while not rospy.is_shutdown():
if self.mode == "SURVEY":
self.lawnmover(200, 20, 7, 10, 3)
if self.mode == "HOVER":
self.hover()
if self.mode == "SCAN":
self.scan(20, 3) # pass x_offset, length of rectangle, to bound during small search
if self.mode == "TEST":
print self.mode
self.vel_pub.publish(self.get_descent(0, 0.1, 0))
if self.mode == "DESCENT":
self.descent()
if self.mode == "RT_SURVEY":
self.rt_survey()
def planner(self, msg):
if msg.data == "FOUND UAV" and self.mode == "SURVEY":
self.saved_location = self.curr_pose
self.mode = "SCAN"
if msg.data == "FOUND UAV" and self.mode == "SCAN":
self.detection_count += 1
print self.detection_count
if self.detection_count > 25:
self.hover_loc = [self.curr_pose.pose.position.x,
self.curr_pose.pose.position.y,
self.curr_pose.pose.position.z,
0, 0, 0, 0]
self.mode = "HOVER"
self.detection_count = 0
if msg.data == "FOUND UAV" and self.mode == "HOVER":
print self.detection_count
self.detection_count += 1
if self.detection_count > 40:
self.mode = "DESCENT"
self.detection_count = 0
if msg.data == "MISSING UAV" and self.mode == "DESCENT":
self.missing_count += 1
if self.missing_count > 80:
self.mode = "HOVER"
self.missing_count = 0
if msg.data == "FOUND UAV" and self.mode == "DESCENT":
self.missing_count = 0
if msg.data == "SCAN COMPLETE":
self.mode = "RT_SURVEY"
self.detection_count = 0
if msg.data == "HOVER COMPLETE":
if self.waypointIndex == 0: # TODO remove this, this keeps the drone in a loop of search
self.mode = "SURVEY"
else:
self.mode = "RT_SURVEY"
self.detection_count = 0
if msg.data == "RTS COMPLETE":
self.mode = "SURVEY"
if msg.data == "PICKUP COMPLETE":
# self.mode = "CONFIRM_PICKUP"
# go back and hover at takeoff location
self.hover_loc = [self.loc[0][0], self.loc[0][1], self.loc[0][2], 0, 0, 0, 0]
self.mode = "HOVER"
self.loc = []
self.waypointIndex = 0
def callback(image_msg):
global pub_Image
global pub_BoxesImage
global pub_Boxes
global face_cascade
global bridge
global boxes_msg
global faces
global local_position_pub
global goal_pose
# Convert ros image into a cv2 image
cv_img = bridge.imgmsg_to_cv2(image_msg, desired_encoding="passthrough")
cv_gray = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
# Find the faces and draw a rectangle around them
faces = face_cascade.detectMultiScale(cv_gray, 1.3, 5)
for (x, y, w, h) in faces:
cv_img = cv2.rectangle(cv_img, (x, y), (x + w, y + h), (255, 0, 0), 2)
# For each image, we also want to publish the croped image
crop_img = cv_img[y:y + h, x:x + w]
crop_msg = bridge.cv2_to_imgmsg(crop_img, encoding="bgr8")
# And the region of interest information
pub_BoxesImage.publish(crop_msg)
boxes_msg = RegionOfInterest()
boxes_msg.x_offset = x
boxes_msg.y_offset = y
boxes_msg.height = h
boxes_msg.width = w
boxes_msg.do_rectify = False
pub_Boxes.publish(boxes_msg)
# Publish the original image with the rectangles
mod_img = bridge.cv2_to_imgmsg(cv_img, encoding="bgr8")
pub_Image.publish(mod_img)
#Follow face
pose_msg = PoseStamped()
fig_x = image_msg.width / 2
fig_y = image_msg.height / 2
#kx = .001
#ky = .001
kx = .003
ky = .003
#xmax_m = 0.70
#xmin_m = -1.0
xmax_m = -3.5
xmin_m = -5.5
zmin_m = 0.5
zmax_m = 1.5
if len(faces) > 0:
face_x = boxes_msg.x_offset + (boxes_msg.width / 2)
face_y = boxes_msg.y_offset + (boxes_msg.height / 2)
x_error = fig_x - face_x
y_error = fig_y - face_y
pose_msg.pose.position.x = current_pose.pose.position.x + (x_error *
kx)
pose_msg.pose.position.z = current_pose.pose.position.z + (y_error *
ky)
else:
# No face to follow, remain stationary
pose_msg = goal_pose
if pose_msg.pose.position.x > xmax_m:
pose_msg.pose.position.x = xmax_m
if pose_msg.pose.position.x < xmin_m:
pose_msg.pose.position.x = xmin_m
if pose_msg.pose.position.z > zmax_m:
pose_msg.pose.position.z = zmax_m
if pose_msg.pose.position.z < zmin_m:
pose_msg.pose.position.z = zmin_m
#pose_msg.pose.position.x = goal_pose.pose.position.x
pose_msg.pose.position.y = goal_pose.pose.position.y
pose_msg.pose.orientation = goal_pose.pose.orientation
local_position_pub.publish(goal_pose)
# Update goal pose to be pose_msg
goal_pose = pose_msg
def __init__(self, node_name):
self.node_name = node_name
rospy.init_node(node_name)
rospy.loginfo("Starting node at " + str(node_name))
rospy.on_shutdown(self.cleanup)
# # Create the main display window
self.cv_window_name = self.node_name
# Create the cv_bridge object
self.bridge = CvBridge()
self.frame = None
self.rect = None
self.start = 0
self.roi_pub = rospy.Publisher('/roi', RegionOfInterest, queue_size=10)
self.image_sub = rospy.Subscriber(img_topic_name, Image, self.image_callback)
while not rospy.is_shutdown():
ROI = RegionOfInterest()
self.rect = None
#openpose-detection
if self.frame is not None:
self.start = time.time()
keypoints = openpose.forward(self.frame, False)
if len(keypoints) > 0:
b_parts = keypoints[0]
pts = []
for i in range(len(b_parts)):
#print 'x: {}, y:{}, confidence: {}'.format(b_parts[i][0], b_parts[i][1], b_parts[i][2])
if b_parts[i][2] > 0:
pts.append((b_parts[i][0], b_parts[i][1]))
points_matrix = np.array(pts).reshape((-1, 1, 2)).astype(np.int32)
# bounding-box
self.rect = cv2.boundingRect(points_matrix)
# Compute the time for this loop and estimate CPS as a running average
end = time.time()
duration = end - self.start
fps = int(1.0 / duration)
cv2.putText(self.frame, "FPS: " + str(fps), (10, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
(255, 255, 0))
if self.rect is not None:
ROI.x_offset = int(self.rect[0])
ROI.y_offset = int(self.rect[1])
ROI.width = int(self.rect[2])
ROI.height = int(self.rect[3])
ROI.do_rectify = True
cv2.rectangle(self.frame, (self.rect[0], self.rect[1]),
(self.rect[0] + self.rect[2], self.rect[1] + self.rect[3]), (0, 255, 0), 1)
self.roi_pub.publish(ROI)
# Update the image display
if self.frame is not None:
cv2.imshow(self.node_name, self.frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
return
