def hough_it(self, n_ball, iteration):
# create gray scale image of balls
gray_image = cv.CreateImage((self.width, self.height), 8, 1)
cv.CvtColor(self.cv_image, gray_image, cv.CV_BGR2GRAY)
# create gray scale array of balls
gray_array = self.cv2array(gray_image)
# find Hough circles
circles = cv2.HoughCircles(gray_array, cv.CV_HOUGH_GRADIENT, 1, 40, param1=50, \
param2=self.hough_accumulator, minRadius=self.hough_min_radius, \
maxRadius=self.hough_max_radius)
# Check for at least one ball found
if circles is None:
# display no balls found message on head display
self.splash_screen("no balls", "found")
# no point in continuing so exit with error message
sys.exit("ERROR - hough_it - No golf balls found")
circles = numpy.uint16(numpy.around(circles))
ball_data = {}
n_balls = 0
circle_array = numpy.asarray(self.cv_image)
# check if golf ball is in ball tray
for i in circles[0, :]:
# convert to baxter coordinates
ball = self.pixel_to_baxter((i[0], i[1]), self.tray_distance)
if self.is_near_ball_tray(ball):
# draw the outer circle in red
cv2.circle(circle_array, (i[0], i[1]), i[2], (0, 0, 255), 2)
# draw the center of the circle in red
cv2.circle(circle_array, (i[0], i[1]), 2, (0, 0, 255), 3)
elif i[1] > 800:
# draw the outer circle in red
cv2.circle(circle_array, (i[0], i[1]), i[2], (0, 0, 255), 2)
# draw the center of the circle in red
cv2.circle(circle_array, (i[0], i[1]), 2, (0, 0, 255), 3)
else:
# draw the outer circle in green
cv2.circle(circle_array, (i[0], i[1]), i[2], (0, 255, 0), 2)
# draw the center of the circle in green
cv2.circle(circle_array, (i[0], i[1]), 2, (0, 255, 0), 3)
ball_data[n_balls] = (i[0], i[1], i[2])
n_balls += 1
circle_image = cv.fromarray(circle_array)
cv.ShowImage("Hough Circle", circle_image)
# 3ms wait
cv.WaitKey(3)
# display image on head monitor
font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 1)
position = (30, 60)
s = "Searching for golf balls"
cv.PutText(circle_image, s, position, font, self.white)
msg = cv_bridge.CvBridge().cv_to_imgmsg(circle_image, encoding="bgr8")
self.pub.publish(msg)
if self.save_images:
# save image of Hough circles on raw image
file_name = self.image_dir \
+ "hough_circle_" + str(n_ball) + "_" + str(iteration) + ".jpg"
cv.SaveImage(file_name, circle_image)
# Check for at least one ball found
if n_balls == 0: # no balls found
# display no balls found message on head display
self.splash_screen("no balls", "found")
# less than 12 balls found, no point in continuing, exit with error message
sys.exit("ERROR - hough_it - No golf balls found")
# select next ball and find it's position
next_ball = self.find_next_golf_ball(ball_data, iteration)
# find best gripper angle to avoid touching neighbouring ball
angle = self.find_gripper_angle(next_ball, ball_data)
# return next golf ball position and pickup angle
return next_ball, angle
def _cb(self, img_msg, pda_msg):
br = cv_bridge.CvBridge()
img = br.imgmsg_to_cv2(img_msg, desired_encoding="bgr8")
curGray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
people_poses = pda_msg.poses
wrist_pose = None
elbow_pose = None
neck_pose = None
has_waving_person = False
print len(people_poses)
if self.preGray is not None:
for person_pose in people_poses:
for limb_prefix in ['R', 'L']:
try:
wrist_index = person_pose.limb_names.index(
limb_prefix + "Wrist")
elbow_index = person_pose.limb_names.index(
limb_prefix + "Elbow")
neck_index = person_pose.limb_names.index("Neck")
except ValueError:
continue
if not np.all(
np.array(person_pose.confidences)[[
wrist_index, elbow_index, neck_index
]] > self.arms_score_threshold):
continue
wrist_pose = person_pose.poses[wrist_index]
elbow_pose = person_pose.poses[elbow_index]
neck_pose = person_pose.poses[neck_index]
if (elbow_pose.position.y > wrist_pose.position.y):
wrist_elbow_dist = int(
((wrist_pose.position.x - elbow_pose.position.x)**2
+ (wrist_pose.position.y - elbow_pose.position.y)
**2)**0.5)
y_tl = int(
max(0, elbow_pose.position.y -
wrist_elbow_dist * 1.5))
x_tl = int(
max(0,
elbow_pose.position.x - wrist_elbow_dist / 2))
width = int(
min(480 - x_tl, 640 - y_tl,
wrist_elbow_dist * 1.5))
height = width
preRoi = self.preGray[x_tl:x_tl + width,
y_tl:y_tl + height]
curRoi = curGray[x_tl:x_tl + width, y_tl:y_tl + height]
flow = cv2.calcOpticalFlowFarneback(
preRoi, curRoi, 0.5, 3, 15, 3, 5, 1.2, 0)
# print preRoi.shape
has_waving_person = True
cv2.rectangle(img, (x_tl, y_tl),
(x_tl + width, y_tl + height),
(0, 0, 255), 10)
break
if (has_waving_person == True):
print "found out"
break
self.preGray = curGray
debug_img_msg = br.cv2_to_imgmsg(img, encoding="bgr8")
debug_img_msg.header = img_msg.header
self.img_pub_.publish(debug_img_msg)
def __init__(self):
# Initialize main node for publishing
rospy.init_node('MainAction')
rospy.loginfo('... Initializing MainAction node')
# Enable baxter if it isn't already enabled
baxter_interface.RobotEnable(CHECK_VERSION).state().enabled
baxter_interface.RobotEnable(CHECK_VERSION).enable()
# Get create main publisher for activating the Joint movements
self.pub = rospy.Publisher('/robot/limb/left/joint_command',
JointCommand,
queue_size=10)
# Create Class to control Baxter's Joints
self.command = JointCommand()
self.rate = rospy.Rate(70)
# Variable to show every print statement for testing
SHOW_RESULTS = False
# Define screen pixels for Baxter's image (face LCD screen)
h = 600
w = 1024
# Load image for the Baxter's screen
img1 = cv2.imread("Tejada.png")
# Resize the image to get to the maximum possible screen size
img1 = cv2.resize(img1, (w, h), interpolation=cv2.INTER_CUBIC)
# Create publisher for the Image that will be loaded to Baxter
pub = rospy.Publisher('/robot/xdisplay',
Image,
latch=True,
queue_size=1)
# Define message that will be loaded to the Image-publisher
msg1 = cv_bridge.CvBridge().cv2_to_imgmsg(img1, encoding="bgr8")
# Publish the desired message that contains the image
pub.publish(msg1)
# Get main vector for the trajectory
T1 = trajectory_A.TrajectoryA(5)
print("\n... Processing trajectory 1 ...\n")
vector_x = T1.vector_x
vector_y = T1.vector_y
vector_z = T1.vector_z
vector_angle_x = T1.vector_angle_x
vector_angle_y = T1.vector_angle_y
vector_angle_z = T1.vector_angle_z
print("\n... Done processing trajectory 1 ...\n")
# TM_test = transformation.Transformation(vector_angle_x[0], vector_angle_y[0],
# vector_angle_z[0], [vector_x[0], vector_y[0], vector_z[0]])
# print(TM_test.TM)
# THETAS = inverse_kinematics.inverse_kinematics_baxter(TM_test.TM, "left")
# print(THETAS)
# Get each vector of theta_i for all points
self.THETA_1 = []
self.THETA_2 = []
self.THETA_4 = []
self.THETA_5 = []
self.THETA_6 = []
self.THETA_7 = []
# Generate main trajectories finding each angle of the joints with IK
for i in range(len(vector_x)):
# Get each transformation matrix for all cartesian points given
TM_i = transformation.Transformation(
vector_angle_x[i], vector_angle_y[i], vector_angle_z[i],
[vector_x[i], vector_y[i], vector_z[i]])
# Get each THETA_i for each point with the inverse-kinematics
THETAS_i = inverse_kinematics.inverse_kinematics_baxter(
TM_i.TM, "left")
# Create all vectors of THETA_i for all necessary points
self.THETA_1.append(float(THETAS_i[0]))
self.THETA_2.append(float(THETAS_i[1]))
self.THETA_4.append(float(THETAS_i[2]))
self.THETA_5.append(float(THETAS_i[3]))
self.THETA_6.append(float(THETAS_i[4]))
self.THETA_7.append(float(THETAS_i[5]))
if SHOW_RESULTS == True:
print("\n THETA_1: \n", np.rad2deg(self.THETA_1))
print("\n max(THETA_1) =", max(np.rad2deg(self.THETA_1)))
print("\n min(THETA_1) =", min(np.rad2deg(self.THETA_1)))
print("\n THETA_2: \n", np.rad2deg(self.THETA_2))
print("\n max(THETA_2) =", max(np.rad2deg(self.THETA_2)))
print("\n min(THETA_2) =", min(np.rad2deg(self.THETA_2)))
print("\n THETA_4: \n", np.rad2deg(self.THETA_4))
print("\n max(THETA_4) =", max(np.rad2deg(self.THETA_4)))
print("\n min(THETA_4) =", min(np.rad2deg(self.THETA_4)))
print("\n THETA_5: \n", np.rad2deg(self.THETA_5))
print("\n max(THETA_5) =", max(np.rad2deg(self.THETA_5)))
print("\n min(THETA_5) =", min(np.rad2deg(self.THETA_5)))
print("\n THETA_6: \n", np.rad2deg(self.THETA_6))
print("\n max(THETA_6) =", max(np.rad2deg(self.THETA_6)))
print("\n min(THETA_6) =", min(np.rad2deg(self.THETA_6)))
print("\n THETA_7: \n", np.rad2deg(self.THETA_7))
print("\n max(THETA_7) =", max(np.rad2deg(self.THETA_7)))
print("\n min(THETA_7) =", min(np.rad2deg(self.THETA_7)))
def main(num_of_backsteps=1,dropout=1):
# define the list of topics that you want to extract
ros_topics = [
# the duckiebot name can change from one bag file to the other, so define
# the topics WITHOUT the duckiebot name in the beginning
"/camera_node/image/compressed",
"/lane_controller_node/car_cmd"
]
# define the bags_directory in order to extract the data
bags_directory = os.path.join(os.getcwd(), "bag_files")
# define data_directory
data_directory = 'data'
if not os.path.exists(data_directory):
os.makedirs(data_directory)
# define train and test directories inside the data directory
test_dir = os.path.join(data_directory, "test")
train_dir = os.path.join(data_directory, "train")
if not os.path.exists(test_dir):
os.mkdir(test_dir)
if not os.path.exists(test_dir):
os.mkdir(test_dir)
if not os.path.exists(train_dir):
os.mkdir(train_dir)
cvbridge = cv_bridge.CvBridge()
# create a dataframe to store the data for all bag files
# df_all = pd.DataFrame()
first_time = True
for file in os.listdir(bags_directory):
if not file.endswith(".bag"):
continue
# extract bag_ID to include it in the data for potential future use (Useful in case of weird data distributions
# or final results, since you will be able to associate the data with the bag files)
bag_ID = file.partition(".bag")[0]
# extract the duckiebot name to complete the definition of the nodes
duckiebot_name = file.partition("_")[2].partition(".bag")[0]
# complete the topics names with the duckiebot name in the beginning
ros_topics_temp = copy(ros_topics)
for num, topic in enumerate(ros_topics_temp):
ros_topics_temp[num] = "/" + duckiebot_name + topic
# define absolute path of the bag_file
abs_path = os.path.abspath(os.path.join(bags_directory, file))
print("Extract data for {} file.".format(file))
try:
msgs = extract_messages(abs_path, ros_topics_temp)
except rosbag.bag.ROSBagException:
print("Failed to open {}".format(abs_path))
continue
######## This following part is implementation specific ########
# The composition of the ros messages is different (e.g. different names in the messages) and also different
# tools are used to handle the different extracted data (e.g. cvbridge for images). As a result, the following
# part of the script can be used as a basis to extract the data, but IT HAS TO BE MODIFIED based on your topics.
# easy way to find the structure of your ros messages : print dir(msgs[name_of_topic])
# extract the images and car_cmds messages
ext_images = msgs["/" + duckiebot_name + "/camera_node/image/compressed"].messages
ext_car_cmds = msgs["/" + duckiebot_name + "/lane_controller_node/car_cmd"].messages
# create dataframe with the images and the images' timestamps
for num, img in enumerate(ext_images):
# get the rgb image
img = cvbridge.compressed_imgmsg_to_cv2(img.message)
img = image_preprocessing(img) # -> each image is of dimensions (1, 48x96x3=13824)
# hack to get the timestamp of each image in <float 'secs.nsecs'> format instead of <int 'rospy.rostime.Time'>
temp_timestamp = ext_images[num].timestamp
img_timestamp = temp_timestamp.secs + temp_timestamp.nsecs *10 ** -len(str(temp_timestamp.nsecs))
temp_df = pd.DataFrame({
'img': [img],
'img_timestamp': [img_timestamp]
})
if num == 0:
df_imgs = temp_df.copy()
else:
df_imgs = df_imgs.append(temp_df, ignore_index=True)
# create dataframe with the car_cmds and the car_cmds' timestamps
for num, cmd in enumerate(ext_car_cmds):
# read wheel commands messages
cmd_msg = cmd.message
# hack to get the timestamp of each image in <float 'secs.nsecs'> format instead of <int 'rospy.rostime.Time'>
temp_timestamp = ext_car_cmds[num].timestamp
vel_timestamp = temp_timestamp.secs + temp_timestamp.nsecs * 10 ** -len(str(temp_timestamp.nsecs))
temp_df = pd.DataFrame({
'vel_timestamp': [vel_timestamp],
'vel_omega': [cmd_msg.omega],
'vel_v': [cmd_msg.v]
})
if num == 0:
df_cmds = temp_df.copy()
else:
df_cmds = df_cmds.append(temp_df, ignore_index=True)
# synchronize data
print("Starting synchronization of data for {} file.".format(file))
temp_synch_data, temp_synch_imgs = synchronize_data(df_imgs, df_cmds, bag_ID)
# print temp_synch_data.shape, temp_synch_imgs.shape
# backstepping data preparation
if num_of_backsteps > 1:
temp_synch_imgs,temp_synch_data = backstepping_prep(temp_synch_imgs,temp_synch_data,num_of_backsteps,dropout)
if first_time:
synch_data = copy(temp_synch_data)
synch_imgs = copy(temp_synch_imgs)
first_time = False
else:
synch_data = np.vstack((synch_data, temp_synch_data))
synch_imgs = np.vstack((synch_imgs, temp_synch_imgs))
print("\nShape of total data: {} , shape of total images: {}\n".format(synch_data.shape, synch_imgs.shape))
print("Synchronization of all data is finished.\n")
# define size of train dataset
train_size = int(0.9 * synch_data.shape[0])
# create train dataframe
df_data_train = pd.DataFrame({
'img_timestamp': synch_data[:train_size, 0],
'vel_timestamp': synch_data[:train_size, 1],
'vel_v': synch_data[:train_size, 2],
'vel_omega': synch_data[:train_size, 3],
'bag_ID': synch_data[:train_size, 4],
})
# create train dataframe for images in order to save them in the same .h5 file with the rest train data
df_img_train = pd.DataFrame(data=synch_imgs[:train_size,:],index=range(train_size))
# create test dataframe
df_data_test = pd.DataFrame({
'img_timestamp': synch_data[train_size:, 0],
'vel_timestamp': synch_data[train_size:, 1],
'vel_v': synch_data[train_size:, 2],
'vel_omega': synch_data[train_size:, 3],
'bag_ID': synch_data[train_size:, 4],
})
# create test dataframe for images in order to save them in the same .h5 file with the rest test data
df_img_test = pd.DataFrame(data=synch_imgs[train_size:,:],index=range(train_size,synch_data.shape[0]))
#pd.DataFrame({
#'img': synch_imgs[train_size:,:]
#},index=range(train_size,synch_data.shape[0]))
# save train and test datasets to .h5 files
# ATTENTION 1 !!
# If the datasets become too large, you could face memory errors on laptops.
# If you face memory errors while saving the following files, split the data to multiple .h5 files.
# ATTENTION 2 !!
# The .h5 files are tricky and require special attention. In these files you save compressed objects and you can
# have more than one objects saved in the same file. If for example we have two different dataframes df1 and df2,
# then df1.to_hdf('file.h5', key='df1') and df2.to_hdf('file.h5', key='df2') will result to both df1, df2 to be
# saved in 'file.h5' file but with different key for each dataframe. However, if we save the same dataframe to the
# same .h5 file with the same key, then in this file you will have the same information twice as different objects
# and thus the size of the .h5 file will be double for no reason and without any warning. As a result, here since
# the key does not change, we will check if the .h5 file exists before saving the new data, and if it exists we will
# first remove the previous file ad then save the new data.
# define the names of the train and test .h5 files
train_set_name = os.path.join(train_dir, 'train_set.h5')
test_set_name = os.path.join(test_dir, 'test_set.h5')
# check if these two files exist in the data directory and if yes remove them before saving the new files
if os.path.isfile(train_set_name):
os.remove(train_set_name)
if os.path.isfile(test_set_name):
os.remove(test_set_name)
# df_all_train.to_hdf(train_set_name, 'table')
df_data_train.to_hdf(train_set_name, key='data')
df_img_train.to_hdf(train_set_name, key='images')
df_data_train.info()
df_img_train.info()
# df_all_test.to_hdf(test_set_name, 'table')
df_data_test.to_hdf(test_set_name, key='data')
df_img_test.to_hdf(test_set_name, key='images')
print("\nThe total {} data were split into {} training and {} test datasets and saved in {} "
"directory.".format(synch_data.shape[0], df_data_train.shape[0], df_data_test.shape[0], data_directory))
def __init__(self):
self.bridge = cv_bridge.CvBridge()
self.image_sub = rospy.Subscriber('/camera/color/image_raw', Image,
self.image_cb)
self.pixel_pub = rospy.Publisher('obj_points', pixels, queue_size=1)
self.pixels = pixels()
def publishHazmap(self):
hazRaw = self.polPack['hazmap']
#Invert to match ROS convention of white=traversable
self.hazPub.publish(cv_bridge.CvBridge().cv2_to_imgmsg(
cv2.bitwise_not(hazRaw), encoding='mono8'))
def camera_callback(self, data, camera_name):
# Convert image from a ROS image message to a CV image
try:
self.cv_image = cv_bridge.CvBridge().imgmsg_to_cv(data, "bgr8")
except cv_bridge.CvBridgeError, e:
print e
def display_image():
img = cv2.imread(FACE_IMAGE_PATH)
msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
pub = rospy.Publisher('/robot/xdisplay', Image, queue_size=100, latch=True)
pub.publish(msg)
#!/usr/bin/env python
import cv2
import rospy
import rospkg
import cv_bridge
from sensor_msgs.msg import Image
if __name__ == '__main__':
rospy.init_node("display_robotman")
rospack = rospkg.RosPack()
image_path = rospack.get_path('robotman_gazebo') + '/images/'
img = cv2.imread(image_path + 'screen.png')
msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
pub = rospy.Publisher('/screen/body/image',
Image,
latch=True,
queue_size=1)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
pub.publish(msg)
rate.sleep()
def shape_cam_callback(self, msg):
bridge = cv_bridge.CvBridge()
image = bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
self.hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
def __init__(self):
self.bridge = cv_bridge.CvBridge()
cv2.namedWindow("window", 1)
self.image_sub = rospy.Subscriber('camera/rgb/image_raw', Image,
self.image_callback)
def __init__(self):
self.bridge = cv_bridge.CvBridge()
self.image_sub = rospy.Subscriber('/ur5/usbcam/image_raw', Image,
self.image_callback)
def _callback(self, img_msg, mask_msg):
bridge = cv_bridge.CvBridge()
bgr_img = bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8')
mask_img = bridge.imgmsg_to_cv2(mask_msg, desired_encoding='mono8')
if mask_img.size < 1:
logwarn_throttle(10, 'Too small sized image')
return
logwarn_throttle(10, '[FCNMaskForLabelNames] >> Start Processing <<')
if mask_img.ndim == 3 and mask_img.shape[2] == 1:
mask_img = mask_img.reshape(mask_img.shape[:2])
if mask_img.shape != bgr_img.shape[:2]:
jsk_logwarn('Size of mask and color image is different.'
'Resizing.. mask {0} to {1}'.format(
mask_img.shape, bgr_img.shape[:2]))
mask_img = resize(mask_img, bgr_img.shape[:2],
preserve_range=True).astype(np.uint8)
blob = bgr_img - self.mean_bgr
blob = blob.transpose((2, 0, 1))
x_data = np.array([blob], dtype=np.float32)
if self.gpu != -1:
x_data = cuda.to_gpu(x_data, device=self.gpu)
x = Variable(x_data, volatile=True)
self.model(x)
pred_datum = cuda.to_cpu(self.model.score.data[0])
candidate_labels = [
self.target_names.index(name) for name in self.tote_contents
]
label_pred_in_candidates = pred_datum[candidate_labels].argmax(axis=0)
label_pred = np.zeros_like(label_pred_in_candidates)
for idx, label_val in enumerate(candidate_labels):
label_pred[label_pred_in_candidates == idx] = label_val
label_pred[mask_img == 0] = 0 # set bg_label
label_viz = label2rgb(label_pred, bgr_img, bg_label=0)
label_viz = (label_viz * 255).astype(np.uint8)
debug_msg = bridge.cv2_to_imgmsg(label_viz, encoding='rgb8')
debug_msg.header = img_msg.header
self.pub_debug.publish(debug_msg)
output_mask = np.ones(mask_img.shape, dtype=np.uint8)
output_mask *= 255
for label_val, label_name in enumerate(self.target_names):
if label_name in self.label_names:
assert label_name == 'kleenex_paper_towels'
assert label_val == 21
label_mask = ((label_pred == label_val) * 255).astype(np.uint8)
contours, hierachy = cv2.findContours(label_mask,
cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
cv2.drawContours(output_mask, contours, -1, 255, -1)
# output_mask[label_pred == label_val] = False
# output_mask = output_mask.astype(np.uint8)
# output_mask[output_mask == 1] = 255
output_mask[mask_img == 0] = 0
output_mask_msg = bridge.cv2_to_imgmsg(output_mask, encoding='mono8')
output_mask_msg.header = img_msg.header
self.pub.publish(output_mask_msg)
logwarn_throttle(10, '[FCNMaskForLabelNames] >> Finshed processing <<')
def pick_and_place(self):
n_ball = 0
while True and n_ball < 12: # assume no more than 12 golf balls
n_ball += 1
iteration = 0
angle = 0.0
# use Hough circles to find balls and select one ball
next_ball, angle = self.hough_it(n_ball, iteration)
error = 2 * self.ball_tolerance
print
print "Ball number ", n_ball
print "==============="
# iterate to find next golf ball
# if hunting to and fro accept error in position
while error > self.ball_tolerance and iteration < 10:
iteration += 1
next_ball, angle, error = self.golf_ball_iterate(
n_ball, iteration, next_ball)
font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 1)
position = (30, 60)
s = "Picking up golf ball"
cv.PutText(self.cv_image, s, position, font, self.white)
msg = cv_bridge.CvBridge().cv_to_imgmsg(self.cv_image,
encoding="bgr8")
self.pub.publish(msg)
print "DROPPING BALL ANGLE =", angle * (math.pi / 180)
if angle != self.yaw: # if neighbouring ball
pose = (
self.pose[0], # rotate gripper to avoid hitting neighbour
self.pose[1],
self.pose[2],
self.pose[3],
self.pose[4],
angle)
self.baxter_ik_move(self.limb, pose)
cv.PutText(self.cv_image, s, position, font, self.white)
msg = cv_bridge.CvBridge().cv_to_imgmsg(self.cv_image,
encoding="bgr8")
self.pub.publish(msg)
# slow down to reduce scattering of neighbouring golf balls
self.limb_interface.set_joint_position_speed(0.1)
# move down to pick up ball
pose = (self.pose[0] + self.cam_x_offset,
self.pose[1] + self.cam_y_offset,
self.pose[2] + (0.112 - self.distance), self.pose[3],
self.pose[4], angle)
self.baxter_ik_move(self.limb, pose)
self.print_arm_pose()
# close the gripper
self.gripper.close()
s = "Moving to ball tray"
cv.PutText(self.cv_image, s, position, font, self.white)
msg = cv_bridge.CvBridge().cv_to_imgmsg(self.cv_image,
encoding="bgr8")
self.pub.publish(msg)
pose = (self.pose[0], self.pose[1], self.pose[2] + 0.198,
self.pose[3], self.pose[4], self.yaw)
self.baxter_ik_move(self.limb, pose)
# speed up again
self.limb_interface.set_joint_position_speed(0.5)
# display current image on head display
cv.PutText(self.cv_image, s, position, font, self.white)
msg = cv_bridge.CvBridge().cv_to_imgmsg(self.cv_image,
encoding="bgr8")
self.pub.publish(msg)
# move dowm
pose = (self.ball_tray_place[n_ball - 1][0],
self.ball_tray_place[n_ball - 1][1], self.pose[2] - 0.19,
self.pose[3], self.pose[4], self.pose[5])
self.baxter_ik_move(self.limb, pose)
# display current image on head display
s = "Placing golf ball in ball tray"
cv.PutText(self.cv_image, s, position, font, self.white)
msg = cv_bridge.CvBridge().cv_to_imgmsg(self.cv_image,
encoding="bgr8")
self.pub.publish(msg)
# open the gripper
self.gripper.open()
# prepare to look for next ball
pose = (self.golf_ball_x, self.golf_ball_y, self.golf_ball_z,
-1.0 * math.pi, 0.0 * math.pi, 0.0 * math.pi)
self.baxter_ik_move(self.limb, pose)
# display all balls found on head display
self.splash_screen("all balls", "found")
print "All balls found"
def __init__(self, cam_name, cbk=None, img_fmt="passthrough"):
self.cam_name, self.img_cbk = cam_name, cbk
self.bridge = cv_bridge.CvBridge()
self.img, self.sub, self.img_fmt = None, None, img_fmt
def sendImage(self, img):
msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
#print(msg)
self.pub.publish(msg)
def publishDEM(self):
self.demPub.publish(cv_bridge.CvBridge().cv2_to_imgmsg(
self.polPack['src'], encoding='64FC1'))
print 'DEM published'
def callback(self, imgmsg):
bridge = cv_bridge.CvBridge()
rgb = bridge.imgmsg_to_cv2(imgmsg, desired_encoding="rgb8")
masks, labels, confs = self._model.predict(
[rgb.astype(np.float32).transpose(2, 0, 1)]
)
masks = masks[0]
labels = labels[0]
confs = confs[0]
class_ids = labels + 1
del labels
if self._blacklist:
keep = ~np.isin(class_ids, self._blacklist)
masks = masks[keep]
class_ids = class_ids[keep]
confs = confs[keep]
if len(class_ids) > 0:
keep = masks.sum(axis=(1, 2)) > 0
class_ids = class_ids[keep]
masks = masks[keep]
confs = confs[keep]
if len(class_ids) > 0:
uniq, counts = np.unique(class_ids, return_counts=True)
keep = []
for cls_id, count in zip(uniq, counts):
if count == 1:
index = np.argwhere(class_ids == cls_id)[0, 0]
else:
index = np.argmax(confs[class_ids == cls_id])
keep.append(index)
class_ids = class_ids[keep]
masks = masks[keep]
confs = confs[keep]
if len(class_ids) > 0:
sort = np.argsort(confs)
class_ids = class_ids[sort]
masks = masks[sort]
confs = confs[sort]
instance_ids = np.arange(0, len(masks))
label_ins = np.full(rgb.shape[:2], -1, dtype=np.int32)
for ins_id, mask in zip(instance_ids, masks):
label_ins[mask] = ins_id
ins_msg = bridge.cv2_to_imgmsg(label_ins)
ins_msg.header = imgmsg.header
self._pub_ins.publish(ins_msg)
instance_ids_active = np.unique(label_ins)
keep = np.isin(instance_ids, instance_ids_active)
instance_ids = instance_ids[keep]
class_ids = class_ids[keep]
confs = confs[keep]
cls_msg = ObjectClassArray(header=imgmsg.header)
for ins_id, cls_id, conf in zip(instance_ids, class_ids, confs):
cls_msg.classes.append(
ObjectClass(
instance_id=ins_id, class_id=cls_id, confidence=conf,
)
)
self._pub_cls.publish(cls_msg)
def on_predict(self, goal):
rospy.loginfo('nbv_3d_cnn_predict: action start.')
if (self.model_type == ''):
model_file_prefix = ''
output_channels = 1
elif (self.model_type == 'flat'):
model_file_prefix = 'flat'
output_channels = 1
elif (self.model_type == 'circular'):
model_file_prefix = 'circular'
output_channels = 1
elif (self.model_type == 'quat'):
model_file_prefix = 'scoreangle'
output_channels = 2
elif (self.model_type == 'autocomplete'):
model_file_prefix = 'autocomplete'
output_channels = 1
else:
rospy.logfatal('Unknown model type: ' + self.model_type)
exit(1)
image_width = goal.empty.width
image_height = goal.empty.height
if (goal.frontier.width != image_width
or goal.frontier.height != image_height):
rospy.error(
'nbv_3d_cnn_predict: empty image has size %d %d, but frontier image has size %d %d, aborted.'
% (image_width, image_height, goal.frontier.width,
goal.frontier.height))
self.action_server.set_aborted()
return
bridge = cv_bridge.CvBridge()
empty_image = bridge.imgmsg_to_cv2(goal.empty,
desired_encoding='mono8')
frontier_image = bridge.imgmsg_to_cv2(goal.frontier,
desired_encoding='mono8')
np_empty_image = empty_image
np_frontier_image = frontier_image
input_image = [np_empty_image, np_frontier_image]
input_image = np.transpose(input_image, [1, 2, 0])
input_image = input_image.astype(
'float32') / 255.0 # convert to zeros and ones
input_shape = [image_height, image_width]
if (self.model == None or self.last_input_shape != input_shape):
load_input_shape = [image_height, image_width, 2]
rospy.loginfo(
'nbv_3d_cnn_predict: last input shape does not match, reloading model (type "%s").'
% self.model_type)
#model = nbv_2d_model.get_2d_model(self.sensor_range_voxels, load_input_shape, 2)
if (self.model_type == ''):
model = nbv_2d_model.get_2d_model(self.sensor_range_voxels,
load_input_shape,
self.sub_image_expand_pow)
elif (self.model_type == 'flat'):
model = nbv_2d_model.get_flat_2d_model(
self.sensor_range_voxels, load_input_shape,
self.sub_image_expand_pow)
elif (self.model_type == 'quat'):
model = nbv_2d_model.get_quat_2d_model(
self.sensor_range_voxels, load_input_shape)
elif (self.model_type == 'autocomplete'):
model = nbv_2d_model.get_autocomplete_2d_model(
self.sensor_range_voxels, load_input_shape)
elif (self.model_type == 'circular'):
model = nbv_2d_model.get_circular_2d_model(
self.sensor_range_voxels, load_input_shape,
self.sub_image_expand_pow)
model.summary()
model.load_weights(self.checkpoint_file)
self.model = model
self.last_input_shape = input_shape
else:
model = self.model
rospy.loginfo('nbv_3d_cnn_predict: predicting.')
prediction = model.predict(np.array([
input_image,
]))
rospy.loginfo('nbv_3d_cnn_predict: sending result.')
prediction = prediction[0]
if (output_channels == 1):
prediction = np.transpose(prediction, [2, 0, 1])
prediction = prediction[0]
elif (output_channels == 2):
prediction = np.transpose(prediction, [2, 0, 1])
prediction = np.asarray([
prediction[0], prediction[1],
np.full((image_height * self.sub_image_expand,
image_width * self.sub_image_expand), 0.0)
])
prediction = np.transpose(prediction, [1, 2, 0])
pass
if (output_channels == 1):
encoding = "32FC1"
else:
encoding = "32FC3"
pass
prediction = prediction.astype('float32')
result = nbv_3d_cnn_msgs.PredictResult()
result.scores = bridge.cv2_to_imgmsg(prediction, encoding=encoding)
self.action_server.set_succeeded(result)
rospy.loginfo('nbv_3d_cnn_predict: action succeeded.')
pass
def __init__(self):
self.bridge = cv_bridge.CvBridge()
self.image_sub = rospy.Subscriber('/camera/rgb/image_raw', Image,
self.image_callback)
self.cmd_vel_pub = rospy.Publisher('direction', String, queue_size=1)
try:
import PIL.Image
except ImportError:
print(
'Missing library(Pillow)! you can install it using `pip3 install Pillow`'
)
try:
import numpy
except ImportError:
print(
'Missing library(numpy)! you can install it using `pip3 install numpy`'
)
from sensor_msgs.msg import Image
rospy.init_node('issue_cv_bridge_node')
bridge = cv_bridge.CvBridge()
empty_image = PIL.Image.new('RGB', (10, 10)) # create new empty 10*10 image
ros_image = bridge.cv2_to_imgmsg(
numpy.asarray(empty_image),
encoding='rgb8') # convert PIL image to ROS image
rate = rospy.Rate(1)
image_publisher = rospy.Publisher('~image', Image, queue_size=1)
while not rospy.is_shutdown():
image_publisher.publish(ros_image)
rate.sleep()
def publicador_a_pantalla(self,texto):
position = (300, 300)
fondo_negro = np.full((600,1024, 3), 0, dtype = np.uint8)
cv2.putText(fondo_negro, texto, position, cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,255), 4)
msg = cv_bridge.CvBridge().cv2_to_imgmsg(fondo_negro)
self.pub.publish(msg)
def process_image(image, depth, net, gaze):
global old_class_ids, old_conf, old_indices, old_boxes
width = image.shape[1]
height = image.shape[0]
# create input blob
blob = cv2.dnn.blobFromImage(image,
scale, (416, 416), (0, 0, 0),
True,
crop=False)
# set input blob for the network
net.setInput(blob)
# run inference through the network
# and gather predictions from output layers
outs = net.forward(get_output_layers(net))
# initialization
class_ids = []
confidences = []
boxes = []
# for each detection from each output layer
# get the confidence, class id, bounding box params
# and ignore weak detections (confidence < 0.5)
for out in outs:
for detection in out:
scores = detection[5:]
class_id = np.argmax(scores)
confidence = scores[class_id]
if confidence > 0.5:
center_x = int(detection[0] * width)
center_y = int(detection[1] * height)
w = int(detection[2] * width)
h = int(detection[3] * height)
x = center_x - w / 2
y = center_y - h / 2
class_ids.append(class_id)
confidences.append(float(confidence))
boxes.append([x, y, w, h])
# apply non-max suppression
indices = cv2.dnn.NMSBoxes(boxes, confidences, conf_threshold,
nms_threshold)
# go through the detections remaining
# after nms and draw bounding box
# ann = cv2.cvtColor(depth, cv2.COLOR_BGRA2BGR)
for i in old_indices:
i = i[0]
box = old_boxes[i]
x = max(0, box[0])
y = max(0, box[1])
w = max(0, box[2])
h = max(0, box[3])
if str(classes[old_class_ids[i]]) == "person":
pub.publish("add not(saw(" + str(classes[old_class_ids[i]]) + "," +
str(int(100 * old_conf[i])) + "," +
str(int(x + w / 2)) + "," + str(int(y)) + "," +
str(int(w)) + "," + str(int(h)) + ")).\n")
old_indices = indices
old_boxes = boxes
old_conf = confidences
old_class_ids = class_ids
for i in indices:
i = i[0]
box = boxes[i]
x = max(0, box[0])
y = max(0, box[1])
w = max(0, box[2])
h = max(0, box[3])
"""
# Trying to do something with depth data, but it seems like gibberish
# There's an issue open on the pyfreenect2 GitHub
print(depth.shape)
d = depth[int((y + h / 2) * 424 / 1080)][int((x + w / 2) * 512 / 1920)]
print("test")
print(d)
sum = [0, 0, 0, 0]
for j in range(int((x + w / 2) * 512 / 1920), int((x + w / 2) * 512 / 1920) + 10):
for k in range(int((y + h / 2) * 424 / 1080), int((y + h / 2) * 424 / 1080) + 10):
sum = np.add(depth[k][j], sum)
sum = sum / 100
print(sum)
print(image.shape)
print(image[int((y + h / 2))][int((x + w / 2))])
distance = d[3] / 255
"""
# Gaze Detection
# if str(classes[class_ids[i]]) == "person":
# Run gaze detection only on the bounding box region of people
# Somewhat redundant, as the gaze detection does face recognition as well
# Seems necessary though, as it won't detect multiple people, and it needs the face data to determine direction
# gaze.refresh(image[y:y + h, x:x + w])
# Place green crosses on eyes
# image[y:y + h, x:x + w] = gaze.annotated_frame()
# This is weird
# Even if pupils_located is a success, the ratio methods subtract 10 from a denominator somewhere
# So sometimes there's a divide by 0 error
# This try catch is just to stop the program from dying, but sometimes it still does
# It's dying when the video feeds freeze and go black and white
# if gaze.pupils_located:
# try:
# cv2.putText(image, "pupil x: " + str(round(gaze.horizontal_ratio(), 3)) + " y: " + str(
# round(gaze.vertical_ratio(), 3)), (int(x + w - 200), y - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
# COLORS[class_ids[i]], 2)
# except ZeroDivisionError:
# pass
# ALMA Communication
# saw(label, confidence (50-100), center_X, center_Y, width, height, time)
# Note that time gets added by ALMA because we use obs (see ALMA documentation)
# Still no depth
# pub.publish(
# "obs saw(" + str(classes[class_ids[i]]) + "," + str(int(100*confidences[i])) + "," + str(int(x + w/2)) + "," + str(
# int(y)) + "," + str(int(w)) + "," + str(int(h)) + ").\n")
if str(classes[class_ids[i]]) == "person":
pub.publish("add saw(" + str(classes[class_ids[i]]) + "," +
str(int(100 * confidences[i])) + "," +
str(int(x + w / 2)) + "," + str(int(y)) + "," +
str(int(w)) + "," + str(int(h)) + ").\n")
#sys.stderr.write("Sent: obs saw(" + str(classes[class_ids[i]]) + "," + str(int(100*confidences[i])) + "," + str(int(x + w/2)) + "," + str( int(y)) + "," + str(int(w)) + "," + str(int(h)) + ").\n")
# Draws bounding box on color image
# Math with passing parameters probably not optimal, but just some addition so not too time inefficient
draw_bounding_box(image, class_ids[i], confidences[i], int(x), int(y),
int(x + w), int(y + h))
# Draws small point at the center of every box on the depth image
# These points don't seem to match up; depth sensor and color sensor aren't the same aspect ratio or FOV
# Some kinect drivers have the ability to flatten it out or get real space coordinates
# draw_bounding_box(ann, class_ids[i], confidences[i], int((x + w / 2) * 512 / 1920),
# int((y + h / 2) * 424 / 1080), 5 + int((x + w / 2) * 512 / 1920),
# 5 + int((y + h / 2) * 424 / 1080))
# display color and depth output on screen
out_image_name = "color" # + str(index)
cv2.imshow(out_image_name, image)
# cv2.imshow("depth", ann)
#Send color output to Baxter screen
msg = cv_bridge.CvBridge().cv2_to_imgmsg(cv2.resize(image, (1024, 600)),
encoding="bgr8")
videoPub.publish(msg)
# SPDX-License-Identifier: CC0-1.0
from __future__ import absolute_import, division, print_function
import json
import os
import cv2
import cv_bridge
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
import mpld3
imgmsg_to_cv2 = cv_bridge.CvBridge().imgmsg_to_cv2
import marv
from marv_nodes.types_capnp import File
from marv_detail.types_capnp import Section, Widget
from marv_robotics.bag import get_message_type, raw_messages
TOPIC = '/wide_stereo/left/image_rect_throttle'
@marv.node(File)
@marv.input('cam', marv.select(raw_messages, TOPIC))
def image(cam):
"""Extract first image of input stream to jpg file.
Args:
def __init__(self):
self.bridge = cv_bridge.CvBridge()
self.image_sub = rospy.Subscriber('camera/rgb/image_raw', Image,
self.image_callback)
self.twist = Twist()
def _step(self, action):
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except (rospy.ServiceException) as e:
print ("/gazebo/unpause_physics service call failed")
if action == 0: #FORWARD
vel_cmd = Twist()
vel_cmd.linear.x = 0.1
vel_cmd.angular.z = 0.0
self.vel_pub.publish(vel_cmd)
elif action == 1: #LEFT
vel_cmd = Twist()
vel_cmd.linear.x = 0.05
vel_cmd.angular.z = 0.1
self.vel_pub.publish(vel_cmd)
elif action == 2: #RIGHT
vel_cmd = Twist()
vel_cmd.linear.x = 0.05
vel_cmd.angular.z = -0.1
self.vel_pub.publish(vel_cmd)
data = None
img_data=None
image=None
while data is None:
try:
data = rospy.wait_for_message('/scan', LaserScan, timeout=5)
img_data = rospy.wait_for_message('/camera/rgb/image_raw', Image, timeout=5)
#h = image_data.height
#w = image_data.width
#turn kinect image to cv2 image
image = cv_bridge.CvBridge().imgmsg_to_cv2(img_data,desired_encoding='mono8')
#turn grayscale image to binary image(inorder to have more contrast)
(thresh, im_bw) = cv2.threshold(image, 128, 255, cv2.THRESH_BINARY | cv2.THRESH_OTSU)
cv2.imshow("window",im_bw)
cv2.waitKey(3)
except:
pass
print("NOTHING&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&")
rospy.wait_for_service('/gazebo/pause_physics')
try:
#resp_pause = pause.call()
self.pause()
except (rospy.ServiceException) as e:
print ("/gazebo/pause_physics service call failed")
state,done = self.discretize_observation(data,5)
if not done:
if action == 0:
reward = 5
else:
reward = 1
else:
reward = -20
return state, reward, done, im_bw
def rosbag_to_sample(rosbag_fname):
print('Parsing {0}'.format(rosbag_fname))
bebop_topics = params['bebop']['topics']
### read all messages
bag = rosbag.Bag(rosbag_fname, 'r')
msg_dict = collections.defaultdict(list)
start_time = None
for topic, msg, t in bag.read_messages():
if start_time is None:
start_time = t.to_sec()
else:
assert(start_time <= t.to_sec())
msg_dict[topic].append((t.to_sec() - start_time, msg))
bag.close()
# get_measured_vel(rosbag_fname, msg_dict) # necessary hack
if len(msg_dict[bebop_topics['start_rollout']]) == 0:
print('\tNo start_rollout, returning None')
return None
### sort by time
for k, v in msg_dict.items():
msg_dict[k] = sorted(v, key=lambda x: x[0])
### prune start and end
is_coll = (bebop_topics['collision'] in msg_dict.keys())
start_time = msg_dict[bebop_topics['start_rollout']][0][0]
end_time = msg_dict[bebop_topics['collision']][0][0] if is_coll else np.inf
for topic, time_msg in msg_dict.items():
msg_dict[topic] = [(t, msg) for t, msg in time_msg if start_time <= t <= end_time]
### necessary info for Samples
dt = params['dt']
T = params['prediction']['dagger']['T']
### space by dt
image_time_msgs = spaced_messages(msg_dict[bebop_topics['image']], dt)[-T:]
cmd_vel_time_msgs = aligned_messages(image_time_msgs, msg_dict[bebop_topics['cmd_vel']], dt)[-T:]
# measured_vel_time_msgs = aligned_messages(image_time_msgs, msg_dict[bebop_topics['measured_vel']], dt)[-T:]
### create sample
sample = Sample(T=len(image_time_msgs), meta_data=params)
cvb = cv_bridge.CvBridge()
for t, (image_time_msg, cmd_vel_time_msg) in enumerate(zip(image_time_msgs, cmd_vel_time_msgs)):
image_msg = image_time_msg[1]
cmd_vel_msg = cmd_vel_time_msg[1]
### image
im = AgentBebop2d.process_image(image_msg, cvb)
sample.set_O(im.ravel(), t=t, sub_obs='camera')
### collision
sample.set_O([0], t=t, sub_obs='collision')
### linearvel
sample.set_X([cmd_vel_msg.linear.x, cmd_vel_msg.linear.y], t=t)
sample.set_U([cmd_vel_msg.linear.x, cmd_vel_msg.linear.y], t=t)
sample.set_O([int(is_coll)], t=-1, sub_obs='collision')
# sample.set_O([int(np.random.random() > 0.5)], t=-1, sub_obs='collision') # tmp
assert(sample.isfinite())
return sample
def __init__(self, cam, img_topic="/trr_vision/start_finish/image_debug"):
rospy.loginfo(' publishing image on ({})'.format(img_topic))
self.image_pub = rospy.Publisher(img_topic,
sensor_msgs.msg.Image,
queue_size=1)
self.bridge = cv_bridge.CvBridge()
def __init__(self, name, chess_size, dim, approximate=0):
super().__init__(name)
self.board = ChessboardInfo()
self.board.n_cols = chess_size[0]
self.board.n_rows = chess_size[1]
self.board.dim = dim
# make sure n_cols is not smaller than n_rows, otherwise error computation will be off
if self.board.n_cols < self.board.n_rows:
self.board.n_cols, self.board.n_rows = self.board.n_rows, self.board.n_cols
image_topic = "monocular/image_rect"
camera_topic = "monocular/camera_info"
tosync_mono = [
(image_topic, sensor_msgs.msg.Image),
(camera_topic, sensor_msgs.msg.CameraInfo),
]
if approximate <= 0:
sync = message_filters.TimeSynchronizer
else:
sync = functools.partial(ApproximateTimeSynchronizer,
slop=approximate)
tsm = sync([
message_filters.Subscriber(self, type, topic)
for (topic, type) in tosync_mono
], 10)
tsm.registerCallback(self.queue_monocular)
left_topic = "stereo/left/image_rect"
left_camera_topic = "stereo/left/camera_info"
right_topic = "stereo/right/image_rect"
right_camera_topic = "stereo/right/camera_info"
tosync_stereo = [(left_topic, sensor_msgs.msg.Image),
(left_camera_topic, sensor_msgs.msg.CameraInfo),
(right_topic, sensor_msgs.msg.Image),
(right_camera_topic, sensor_msgs.msg.CameraInfo)]
tss = sync([
message_filters.Subscriber(self, type, topic)
for (topic, type) in tosync_stereo
], 10)
tss.registerCallback(self.queue_stereo)
self.br = cv_bridge.CvBridge()
self.q_mono = Queue()
self.q_stereo = Queue()
mth = ConsumerThread(self.q_mono, self.handle_monocular)
mth.setDaemon(True)
mth.start()
sth = ConsumerThread(self.q_stereo, self.handle_stereo)
sth.setDaemon(True)
sth.start()
self.mc = MonoCalibrator([self.board])
self.sc = StereoCalibrator([self.board])
def __init__(self):
super().__init__("getCamDataNode")
self.bridge = cv_bridge.CvBridge()
self.image_sub = self.create_subscription(Image, "/camera/image_raw", self.handle_camera_data, 10)
