def pr2_mover(object_list):
# TODO: Initialize variables
dict_list = []
TEST_SCENE_NUM = Int32()
OBJECT_NAME = String()
WHICH_ARM = String()
PICK_POSE = Pose()
PLACE_POSE = Pose()
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_list_param = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variables
TEST_SCENE_NUM.data = TEST_WORLD_NUM
left_dropbox = dropbox_list_param[0]['position']
right_dropbox = dropbox_list_param[1]['position']
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Loop through the pick list
for i in range(len(object_list_param)):
# TODO: Get the PointCloud for a given object and obtain it's centroid
labels = []
centroids = [] # to be list of tuples (x, y, z)
for object in object_list:
if object.label == object_list_param[i]['name']:
labels.append(object.label)
points_arr = ros_to_pcl(object.cloud).to_array()
centroids.append(np.mean(points_arr, axis=0)[:3])
else: continue
# labels.append('error')
# centroids.append(np.array([0, 0, 0]))
# recast centroid back to float
for centroid in centroids:
centroids_float = [np.asscalar(coordinate) for coordinate in centroid]
for label in labels:
# create requests
if label == object_list_param[i]['name']:
print labels
OBJECT_NAME.data = object_list_param[i]['name']
PICK_POSE.position.x = centroids_float[0]
PICK_POSE.position.y = centroids_float[1]
PICK_POSE.position.z = centroids_float[2]
# TODO: Assign the arm to be used for pick_place
OBJECT_GROUP = object_list_param[i]['group']
WHICH_ARM.data = 'right' if OBJECT_GROUP == 'green' else 'left'
# TODO: Create 'place_pose' for the object
DROPBOX_POSITION = left_dropbox if WHICH_ARM.data == 'left' else right_dropbox
PLACE_POSE.position.x = DROPBOX_POSITION[0]
PLACE_POSE.position.y = DROPBOX_POSITION[1]
PLACE_POSE.position.z = DROPBOX_POSITION[2]
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
# Populate various ROS messages
yaml_dict = make_yaml_dict(TEST_SCENE_NUM, WHICH_ARM, OBJECT_NAME, PICK_POSE, PLACE_POSE)
dict_list.append(yaml_dict)
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace)
# TODO: Insert your message variables to be sent as a service request
resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE)
print ("Response: ",resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
pass
else: continue
def callback_processing_thread(self, depth_image):
'''
Processing thread
'''
# Local copy
image = None
self.image_mutex.acquire()
if self.image is not None:
image = self.image.copy()
self.image_mutex.release()
if image is None:
return
# ======= Process depth image =======
keypoints = self.detect_keypoints(depth_image)
# Draw
if gconfig.DEVELOPMENT:
draw = image.copy()
blank = np.zeros((1, 1))
draw = cv.drawKeypoints(draw, keypoints, blank, (0, 255, 0),
cv.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
rects = []
crops = []
img_rgb = cv.cvtColor(image, cv.COLOR_BGR2RGB)
for keypoint in keypoints:
x = keypoint.pt[0]
y = keypoint.pt[1]
center = (int(x), int(y))
radius = int(keypoint.size / 2)
# Bounding box:
im_height, im_width, _ = img_rgb.shape
pad = int(0.4 * radius)
tl_x = max(0, center[0] - radius - pad)
tl_y = max(0, center[1] - radius - pad)
br_x = min(im_width - 1, tl_x + 2 * radius + pad)
br_y = min(im_height - 1, tl_y + 2 * radius + pad)
# cv.rectangle(draw, (tl_x, tl_y), (br_x, br_y), (200, 200, 200), 2)
rect = ((tl_x, tl_y), (br_x, br_y))
crop = img_rgb[tl_y:br_y, tl_x:br_x]
if crop.shape[0] > 0 and crop.shape[1] > 0:
crop = cv.resize(crop, (32, 32))
crop = crop.astype(np.float32) / 255.0
crops.append(crop)
rects.append(rect)
if len(crops) != 0:
preds = self.sign_model.predict(np.array(crops))
preds = np.argmax(preds, axis=1)
preds = preds.tolist()
for i in range(len(preds)):
if preds[i] == 0:
if gconfig.DEVELOPMENT:
cv.rectangle(draw, rects[i][0], rects[i][1],
(0, 0, 255), 3)
draw = cv.putText(draw, "LEFT", rects[i][0],
cv.FONT_HERSHEY_SIMPLEX, 0.5,
(0, 255, 0), 1, cv.LINE_AA)
self.trafficsign_pub.publish(Int32(0))
print("LEFT")
elif preds[i] == 1:
if gconfig.DEVELOPMENT:
cv.rectangle(draw, rects[i][0], rects[i][1],
(255, 0, 0), 3)
draw = cv.putText(draw, "RIGHT", rects[i][0],
cv.FONT_HERSHEY_SIMPLEX, 0.5,
(0, 255, 0), 1, cv.LINE_AA)
self.trafficsign_pub.publish(Int32(1))
print("RIGHT")
if gconfig.DEVELOPMENT:
self.sign_debug_stream_pub.publish(
self.cv_bridge.cv2_to_imgmsg(draw, "bgr8"))
#! /usr/bin/env python
import rospy
from std_msgs.msg import Int32
rospy.init_node('topic_publisher')
pub = rospy.Publisher('/counter', Int32, queue_size=1)
rate = rospy.Rate(2)
count = Int32()
count.data = 0
while not rospy.is_shutdown():
pub.publish(count)
count.data += 1
rate.sleep()
def image_cb(self, msg):
"""Identifies red lights in the incoming camera image and publishes the index
of the waypoint closest to the red light's stop line to /traffic_waypoint
Args: msg (Image): image from car-mounted camera
"""
if (self.waypoints_tree == None):
return
#''' #start TEST 2
if (self.is_traffic_light_ahead()
): # read image, run classifier, publish to waypoint updater
rospy.logwarn(
"tl_detector.image_cb: YES light within {0} waypoints".format(
WAYPOINT_DIFFERENCE))
#self.upcoming_traffic_light_pub.publish(Int32(1))
pass
else: #dont read image, and dont call classifier, return from here
#rospy.logwarn("tl_detector.image_cb: NO light within {0} waypoints".format(WAYPOINT_DIFFERENCE))
#rospy.logwarn("tl_detector.image_cb: NO light within 300 waypoints self.last_wp: {0}".format(self.last_wp))
self.upcoming_red_light_pub.publish(Int32(-1))
#self.upcoming_traffic_light_pub.publish(Int32(-1))
# publishing self.last_wp will make car stuck when TL are too close
# wait for some time before publishing -1 as waypoint updater may not pick up this signal
# it needs some time to start decelerating
return
#''' #end TEST 2
#''' #start TEST 1
#time_elapsed = timer() - self.last_img_process_time
##rospy.loginfo ("image_cb: time_elapsed {}".format(time_elapsed))
##Do not process the camera image unless 20 milliseconds have passed from last processing
#if (time_elapsed < CAMERA_IMG_PROCESS_INT):
# return;
#instead of above CAMERA_IMG_PROCESS_INT logic, use the flag from process_traffic lights which is more accurate
#that will reduce the time here but can mess up near the light, need to decelerate at that stage
#''' #end TEST 1
self.has_image = True
self.camera_image = msg
#rospy.logwarn("Got Image... ")
#rospy.logwarn("self.pose is not None: {0} ".format(self.pose is not None))
#rospy.logwarn("self.waypoints is not None: {0} ".format(self.waypoints is not None))
#rospy.logwarn("self.camera_image is not None: {0} ".format(self.camera_image is not None))
self.last_img_process_time = timer()
light_wp, state = self.process_traffic_lights(
) #whenever we get an image this cb is called
rospy.logwarn("tl_detector - light_wp: {0} ".format(light_wp))
rospy.logwarn("tl_detector - RED(state=0) state: {0}".format(state))
#rospy.logwarn("tl_detector - self.state: {0}".format(self.state))
#rospy.logwarn("tl_detector - self.state_count: {0}".format(self.state_count))
'''
Publish upcoming red lights at camera frequency.
Each predicted state has to occur `STATE_COUNT_THRESHOLD` number
of times till we start using it. Otherwise the previous stable state is
used.
'''
#stored prev state of light, for every image, determine if its state has changed and count
#can also update code if light is yellow
if self.state != state:
self.state_count = 0
self.state = state
elif self.state_count >= STATE_COUNT_THRESHOLD: #threshold = 3
#classifier may be noisy, make sure light is going to stay before taking action
self.last_state = self.state
light_wp = light_wp if state == TrafficLight.RED else -1
#for publishing location of light, only RED state is important
self.last_wp = light_wp
self.upcoming_red_light_pub.publish(Int32(light_wp))
#rospy.logwarn("tl_detector - publish RED light_wp: {0}".format(light_wp))
else:
self.upcoming_red_light_pub.publish(Int32(self.last_wp)) #
#rospy.logwarn("tl_detector - publish self.last_wp: {0}".format(self.last_wp))
self.state_count += 1
def __init__(self):
self.pos_pub = rospy.Publisher("goalPosPrev", Int32, queue_size=1)
self.msg = Int32()
def send_detected_objects_to_ros_msg(detected_objects_list):
labels = [] # the detected object labels
centroids = [] # the detectected object centroids, to be list of tuples (x, y, z)
dict_list = [] # a list to s
# ROS Message types
object_name = String() # the picked item
arm_name = String() # the arm the robot will use to pick
test_scene_num = Int32() # basically all the items on the table
pick_pose = Pose() # location of the item
place_pose = Pose() # location of the dropbox
# Get pick list an drop box locations
object_list_param = rospy.get_param('/object_list')
drop_box_param = rospy.get_param('/dropbox')
# Hard-coded scene number message
test_scene_num.data = 3
# Get the centroids and labels
for object in detected_objects_list:
labels.append(object.label)
points_arr = ros_to_pcl(object.cloud).to_array()
centroids.append(np.mean(points_arr, axis=0)[:3])
# Traverse the pick list and see if we've detected it
for i in range(0, len(object_list_param)):
# The object_list_param now holds the pick list and can be parsed to obtain object names and associated group.
object_name = object_list_param[i]['name']
object_group = object_list_param[i]['group']
# Check if this object was found with our perception analysis
if object_name in labels:
print("Found item %s at index: %d" % (object_name, labels.index(object_name)))
index = labels.index(object_name)
# Populate obect location
pick_pose.position.x = float(centroids[index][0])
pick_pose.position.y = float(centroids[index][1])
pick_pose.position.z = float(centroids[index][2])
# Get the bin and arm
if object_group == 'green':
arm_name = drop_box_param[1]['name']
destination = drop_box_param[1]['position']
elif object_group == 'red':
arm_name = drop_box_param[0]['name']
destination = drop_box_param[0]['position']
# Populate object destination
place_pose.position.x = float(destination[0])
place_pose.position.y = float(destination[1])
place_pose.position.z = float(destination[2])
# Save messages to dictionary
yaml_dict = make_yaml_dict(test_scene_num, str(arm_name), str(object_name), pick_pose, place_pose)
dict_list.append(yaml_dict)
else:
print("Did not find %s!" % (object_name))
# Write object list to file
send_to_yaml('output_3.yaml', dict_list)
import rospy
import time
import sys
from std_msgs.msg import Int32
# ===================Functions ===========================================
# ========================================================================
def join_request_handler(request_data):
rospy.loginfo("New UAV with ID: " + str(request_data) +
" has joined the network")
count += 1
node_topic_pub.Publish(count)
# ===================Initialization=======================================
# ========================================================================
rospy.init_node("node_counter")
rate = rospy.Rate(0.5)
node_topic_pub = rospy.Publisher("/swarm_node_count", Int32)
node_join_request_sub = ("/join _swarm", Int32, join_request_handler)
# ===================Keep Node Alive======================================
# ========================================================================
num_nodes = Int32()
num_nodes.data = input("Enter Number of Nodes To Test: ")
while not rospy.is_shutdown():
node_topic_pub.publish(num_nodes)
rate.sleep()
def pr2_mover(object_list):
# Initialize variables
dict_list = []
# labels = []
centroids = [] # to be list of tuples (x, y, z)
# Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
# Parse parameters into individual variables
dict_dropbox = {}
for p in dropbox_param:
dict_dropbox[p['name']] = p['position']
# TODO: Rotate PR2 in place to capture side tables for the collision map
# Loop through the pick list
for obj in object_list_param:
# Get the PointCloud for a given object and obtain it's centroid
object_name = String()
object_name.data = obj['name']
# Create 'place_pose' for the object
pick_pose = Pose()
pick_pose.position.x = 0
pick_pose.position.y = 0
pick_pose.position.z = 0
#set orientation to 0
pick_pose.orientation.x = 0
pick_pose.orientation.y = 0
pick_pose.orientation.z = 0
pick_pose.orientation.w = 0
#set place pose orientation to 0
place_pose = Pose()
place_pose.orientation.x = 0
place_pose.orientation.y = 0
place_pose.orientation.z = 0
place_pose.orientation.w = 0
for detected_object in object_list:
if detected_object.label == object_name.data:
#labels.append(detected_obj.label)
print "detected object == object_name.data"
points_arr = ros_to_pcl(detected_object.cloud).to_array()
pick_pose_np = np.mean(points_arr, axis=0)[:3]
centroids.append(np.mean(points_arr, axis=0)[:3])
pick_pose.position.x = np.asscalar(pick_pose_np[0])
pick_pose.position.y = np.asscalar(pick_pose_np[1])
pick_pose.position.z = np.asscalar(pick_pose_np[2])
break
# Assign the arm to be used for pick_place
arm_name = String()
if obj['group'] == 'red':
arm_name.data = 'left'
elif obj['group'] == 'green':
arm_name.data = 'right'
else:
print "ERROR: invalid group name. Must be red or green."
place_pose.position.x = dict_dropbox[arm_name.data][0]
place_pose.position.y = dict_dropbox[arm_name.data][1]
place_pose.position.z = dict_dropbox[arm_name.data][2]
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
test_scene_num = Int32()
test_scene_num.data = 1 #update with each world
dict_list.append(
make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose,
place_pose))
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine',
PickPlace)
# TODO: Insert your message variables to be sent as a service request
resp = pick_place_routine(test_scene_num, object_name, arm_name,
pick_pose, place_pose)
print("Response: ", resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
# TODO: Output your request parameters into output yaml file
yaml_filename = "output_" + str(test_scene_num.data) + ".yaml"
send_to_yaml(yaml_filename, dict_list)
def pr2_mover(object_list):
#Initialize variables
test_scene_num = Int32()
test_scene_num.data = 2 #scene number
dict_list = []
object_name_v = String()
pick_pose = Pose()
place_pose = Pose()
arm_name = String()
#Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
#Parse parameters into individual variables
for i in range(len(object_list_param)):
object_name = object_list_param[i]['name']
object_group = object_list_param[i]['group']
#loop through detected object and calculate their centroids
# TODO: Rotate PR2 in place to capture side tables for the collision map
#Loop through the pick list
for j, object in enumerate(object_list):
if object_name == object.label:
points_arr = ros_to_pcl(object.cloud).to_array()
#get the centroid of object detected and exist in the pick list
centroid = np.mean(points_arr, axis=0)[:3]
object_name_v.data = object_name
pick_pose.position.x = np.asscalar(centroid[0])
pick_pose.position.y = np.asscalar(centroid[1])
pick_pose.position.z = np.asscalar(centroid[2])
#Assign the arm to be used for pick_place
if object_group == dropbox_param[0]['group']:
arm_name.data = dropbox_param[0]['name']
place_pose.position.x = dropbox_param[0]['position'][0]
place_pose.position.y = dropbox_param[0]['position'][1]
place_pose.position.z = dropbox_param[0]['position'][2]
elif object_group == dropbox_param[1]['group']:
arm_name.data = dropbox_param[1]['name']
place_pose.position.x = dropbox_param[1]['position'][0]
place_pose.position.y = dropbox_param[1]['position'][1]
place_pose.position.z = dropbox_param[1]['position'][2]
#Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
yaml_dict = make_yaml_dict(test_scene_num, arm_name,
object_name_v, pick_pose,
place_pose)
dict_list.append(yaml_dict)
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy(
'pick_place_routine', PickPlace)
# TODO: Insert your message variables to be sent as a service request
resp = pick_place_routine(test_scene_num, arm_name,
object_name_v, pick_pose,
place_pose)
print("Response: ", resp.success)
except rospy.ServiceException, e:
print("Service call failed: %s" % e)
import rospy
from std_msgs.msg import Int32
rospy.init_node('counter_publisher')
pub = rospy.Publisher('/counter', Int32, queue_size=1)
rate = rospy.Rate(1)
var = Int32()
var.data = 0
while not rospy.is_shutdown():
pub.publish(var)
var.data += 1
rate.sleep()
def pr2_mover(detected_objects):
""" Connect to ROS service for pick and place routine.
Create request parameters as ROS messages.
Save request parameters as yaml files.
"""
# Initialize object list
objects = rospy.get_param('/object_list')
# Check consistency of detected objects list
if not len(detected_objects) == len(objects):
rospy.loginfo("List of detected objects does not match pick list.")
return
# Assign number of objects
num_objects = len(objects)
# Initialize test scene number
num_scene = rospy.get_param('/test_scene_num')
# Initialize message for test scene number
test_scene_num = Int32()
test_scene_num.data = num_scene
# TODO: Rotate PR2 in place to capture side tables for the collision map
# Initialize dropbox positions from ROS parameter
dropbox = rospy.get_param('/dropbox')
red_dropbox_position = dropbox[0]['position']
green_dropbox_position = dropbox[1]['position']
# Initialize counter for evaluating the accuracy of the prediction
hit_count = 0
# Create list of ground truth labels
true_labels = [element['name'] for element in objects]
# For each detected object, compare the predicted label with the
# ground truth from the pick list.
for detected_object in detected_objects:
# Initialize predicted label
predicted_label = detected_object.label
# compare prediction with ground truth
if predicted_label in true_labels:
# remove detected label from ground truth
true_labels.remove(predicted_label)
# count successful prediction
hit_count += 1
else:
# mark unsuccessful detection
detected_object.label = 'error'
# Log the accuracy
rospy.loginfo('Detected {} objects out of {}.'.format(hit_count, num_objects))
# Create list of detected objects sorted in the order of the pick list
sorted_objects = []
# Iterate over the pick list
for i in range(num_objects):
# Grab the label of the pick list item
pl_item_label = objects[i]['name']
# Find detected object corresponding to pick list item
for detected_object in detected_objects:
if detected_object.label == pl_item_label:
# Append detected object to sorted_objects list
sorted_objects.append(detected_object)
# Remove current object
detected_objects.remove(detected_object)
break
# Create lists for centroids and dropbox groups
centroids = []
dropbox_groups = []
for sorted_object in sorted_objects:
# Calculate the centroid
pts = ros_to_pcl(sorted_object.cloud).to_array()
centroid = np.mean(pts, axis=0)[:3]
# Append centroid as <numpy.float64> data type
centroids.append(centroid)
# Search for the matching dropbox group
# Assuming 1:1 correspondence between sorted objects and pick list
for pl_item in objects:
# Compare objects by their label
if pl_item['name'] == sorted_object.label:
# Matching object found, add the group to the list
dropbox_groups.append(pl_item['group'])
break
# Initialize list of request parameters for later output to yaml format
request_params = []
# Iterate over detected objects to generate ROS message for each object
for j in range(len(sorted_objects)):
# Create 'object_name' message with label as native string type
object_name = String()
object_name.data = str(sorted_objects[j].label)
# Initialize the dropbox group
object_group = dropbox_groups[j]
# Create 'arm_name' message
arm_name = String()
# Select right arm for green group and left arm for red group
arm_name.data = 'right' if object_group == 'green' else 'left'
# Convert <numpy.float64> data type to native float as expected by ROS
np_centroid = centroids[j]
scalar_centroid = [np.asscalar(element) for element in np_centroid]
# Create 'pick_pose' message with centroid as the position data
pick_pose = Pose()
pick_pose.position.x = scalar_centroid[0]
pick_pose.position.y = scalar_centroid[1]
pick_pose.position.z = scalar_centroid[2]
# Create 'place_pose' message with dropbox center as position data
place_pose = Pose()
dropbox_position = green_dropbox_position if object_group == 'green' else red_dropbox_position
place_pose.position.x = dropbox_position[0]
place_pose.position.y = dropbox_position[1]
place_pose.position.z = dropbox_position[2]
# Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
request_params.append(make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose))
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
# Call 'pick_place_routine' service
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace)
response = pick_place_routine(test_scene_num, object_name, arm_name, pick_pose, place_pose)
print("Response: {}".format(response.success))
except rospy.ServiceException, e:
print("Service call failed: {}".format(e))
def pcl_callback(pcl_msg):
# TODO: Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
# TODO: Voxel Grid Downsampling
vox = cloud.make_voxel_grid_filter()
# Choose voxel size.
LEAF_SIZE = 0.005
# Set the voxel size.
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
# Call the filter function to downsample cloud.
cloud_filtered = vox.filter()
# TODO: PassThrough Filter
passthrough_z = cloud_filtered.make_passthrough_filter()
# Assign axis and range.
filter_axis = 'z'
passthrough_z.set_filter_field_name(filter_axis)
axis_min = 0.6
axis_max = 1.1
passthrough_z.set_filter_limits(axis_min, axis_max)
# Use filter function to obtain new point cloud.
cloud_filtered = passthrough_z.filter()
# Filter in x direction also.
passthrough_x = cloud_filtered.make_passthrough_filter()
filter_axis = 'x'
passthrough_x.set_filter_field_name(filter_axis)
axis_min = 0.35
axis_max = 2.0
passthrough_x.set_filter_limits(axis_min, axis_max)
# Use filter function to obtain new point cloud.
cloud_filtered = passthrough_x.filter()
# Statistical outlier removal.
outlier_filter = cloud_filtered.make_statistical_outlier_filter()
# Set the number of neighboring points to analyze.
outlier_filter.set_mean_k(20)
# Threshold scale factor.
x = 0.25
# Any point with mean distance larger than mean distance + x*stddev will be an outlier.
outlier_filter.set_std_dev_mul_thresh(x)
cloud_filtered = outlier_filter.filter()
#test_pub.publish(pcl_to_ros(cloud_objects))
# TODO: RANSAC Plane Segmentation
seg = cloud_filtered.make_segmenter()
# Set the model you wish to fit.
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
# Max distance for a point to be considered to fit the model.
max_distance = 0.01
seg.set_distance_threshold(max_distance)
# Perform segmentation.
inliers, coefficients = seg.segment()
# TODO: Extract inliers and outliers
cloud_table = cloud_filtered.extract(inliers, negative=False)
cloud_objects = cloud_filtered.extract(inliers, negative=True)
# TODO: Euclidean Clustering
location_cloud = XYZRGB_to_XYZ(cloud_objects)
tree = location_cloud.make_kdtree()
#Create cluster extraction.
euclid_cluster = location_cloud.make_EuclideanClusterExtraction()
# Set tolerances.
euclid_cluster.set_ClusterTolerance(0.03)
euclid_cluster.set_MinClusterSize(10)
euclid_cluster.set_MaxClusterSize(2000)
# Search k-d tree for clusters.
euclid_cluster.set_SearchMethod(tree)
# Extract indices for found clusters.
cluster_indices = euclid_cluster.Extract()
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
cluster_color = get_color_list(len(cluster_indices))
#print(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
for i, index in enumerate(indices):
color_cluster_point_list.append([
location_cloud[index][0], location_cloud[index][1],
location_cloud[index][2],
rgb_to_float(cluster_color[j])
])
# Create new cloud with clusters a unique color.
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
# TODO: Convert PCL data to ROS messages
ros_cloud_table = pcl_to_ros(cloud_table)
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cloud_cluster = pcl_to_ros(cluster_cloud)
# TODO: Publish ROS messages
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
pcl_cluster_pub.publish(ros_cloud_cluster)
# Classify the clusters! (loop through each detected cluster one at a time)
detected_objects_labels = []
detected_objects = []
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster
pcl_cluster = cloud_objects.extract(pts_list)
# Compute the associated feature vector
ros_cluster = pcl_to_ros(pcl_cluster)
color_hist = compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
normal_hist = compute_normal_histograms(normals)
features = np.concatenate((color_hist, normal_hist))
# Make the prediction
prediction = clf.predict(scaler.transform(features.reshape(1, -1)))
label = encoder.inverse_transform(prediction)[0]
detected_objects_labels.append(label)
# Publish a label into RViz
label_pos = list(location_cloud[pts_list[0]])
label_pos[2] += 0.2
object_markers_pub.publish(make_label(label, label_pos, index))
# Add the detected object to the list of detected objects.
do = DetectedObject()
do.label = label
do.cloud = ros_cluster
detected_objects.append(do)
# Publish the list of detected objects
#rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels))
detected_objects_pub.publish(detected_objects)
# Generate YAML file containing labels and centriods for everything in pick_list.
labels = []
centriods = []
for item in pick_list:
for i in range(len(detected_objects)):
if detected_objects[i].label == item['name']:
print("Found", item['name'])
labels.append(item['name'])
points_array = ros_to_pcl(detected_objects[i].cloud).to_array()
cent = np.mean(points_array, axis=0)
centriods.append((np.asscalar(cent[0]), np.asscalar(cent[1]),
np.asscalar(cent[2])))
yaml_list = []
scene_num = Int32()
object_name = String()
arm = String()
pick_pose = Pose()
place_pose = Pose()
scene_num.data = 2
arm.data = 'none'
for i in range(len(labels)):
object_name.data = labels[i]
pick_pose.position.x = centriods[i][0]
pick_pose.position.y = centriods[i][1]
pick_pose.position.z = centriods[i][2]
yaml_dict = make_yaml_dict(scene_num, arm, object_name, pick_pose,
place_pose)
yaml_list.append(yaml_dict)
send_to_yaml('output_none.yaml', yaml_list)
import rosbag
from std_msgs.msg import Int32, String
bag = rosbag.Bag('test.bag', 'w')
try:
s = String()
s.data = 'foo'
i = Int32()
i.data = 42
bag.write('chatter', s)
bag.write('numbers', i)
finally:
bag.close()
def callback(msg):
doubled = Int32()
doubled.data = msg.data * 2
pub.publish(doubled)
def pr2_mover(object_list):
# TODO: Initialize variables
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param_list = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variables
db_dict = {
dropbox_param['group']: {
'position': dropbox_param['position'],
'name': dropbox_param['name']
}
for dropbox_param in dropbox_param_list
}
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Loop through the pick list
dict_list = []
for i in range(0, len(object_list_param)):
obj_param = object_list_param[i]
param_label = obj_param["name"]
for obj in object_list:
if obj.label == param_label:
matching_obj = obj
break
else:
continue
points_arr = ros_to_pcl(matching_obj.cloud).to_array()
centroid = np.mean(points_arr, axis=0)[:3]
dropbox = db_dict[obj_param['group']]
# TODO: Get the PointCloud for a given object and obtain it's centroid
# TODO: Create 'place_pose' for the object
pick_pos = Pose()
pick_pos.position.x, pick_pos.position.y, pick_pos.position.z = np.asscalar(centroid[0]), \
np.asscalar(centroid[1]), \
np.asscalar(centroid[2]) \
place_pos = Pose()
place_pos.position.x, place_pos.position.y, place_pos.position.z = dropbox[
'position']
test_scene_num = Int32()
test_scene_num.data = 2
object_name = String()
object_name.data = param_label
arm_name = String()
arm_name.data = dropbox['name']
# TODO: Assign the arm to be used for pick_place
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
#(test_scene_num, arm_name, object_name, pick_pose, place_pose)
dict_list.append(
make_yaml_dict(test_scene_num, arm_name, object_name, pick_pos,
place_pos))
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine',
PickPlace)
# TODO: Insert your message variables to be sent as a service request
resp = pick_place_routine(test_scene_num, object_name, arm_name,
pick_pos, place_pos)
print("Response: ", resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
#!/usr/bin/env python
import rospy
from std_msgs.msg import Int32
from rospy_fixed_len_bug.msg import FixedArrayBug
rospy.init_node("pub")
pub = rospy.Publisher("fixed_array", FixedArrayBug, queue_size=10)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
good_msg = FixedArrayBug()
good_msg.fixed_data = [Int32(i) for i in [0, 1, 2, 3]]
rospy.loginfo("Publishing: %s" % (str(good_msg)))
pub.publish(good_msg)
bad_msg = FixedArrayBug()
bad_msg.fixed_data = [Int32(i) for i in [2]]
rospy.loginfo("Publishing: %s" % (str(bad_msg)))
pub.publish(bad_msg)
rate.sleep()
def pr2_mover(object_list):
"""Cycle through each object provided in the object list, moving the
correct arm to pickup the object.
Function to load parameters and request PickPlace service.
:param object_list: List of detected objects
"""
# Initialize output list that'll store multiple object yaml dictionaries
output_list = []
# Load the parameters from the YAML files located in /pr2_robot/config/
object_list_param = rospy.get_param('/object_list')
dropbox_list_param = rospy.get_param('/dropbox')
# TODO: Get this PR2 rotation working
## Rotate PR2 in place to capture side tables for the collision map. The
## world_joint_controller controls the revolute joing world_joint between
## the robot's base_footprint and world coordinate frames
#pub_j1 = rospy.Publisher('/pr2/world_joint_controller/command',
# Float64, queue_size=10)
#pub_j1.publish(np.pi/2) # Rotate the PR2 counter-clockwise 90 deg
#pub_j1.publish(-np.pi) # Rotate the PR2 clockwise 180 deg
#pub_j1.publish(np.pi/2) # Rotate the PR2 back to neutral position
# Iterate through all objects that should be moved
for object_params in object_list_param:
object_name = object_params['name']
object_group = object_params['group']
# Check if the object to be moved was found in the perception analysis,
# populating the pick_pose message if it was
for object_i, object_val in enumerate(object_list):
if object_name != object_val.label:
# Skip this object b/c it doesn't match the object to be moved
continue
# Assign the scene number
ros_scene_num = Int32()
# TODO: Figure out what parameter holds the scene data
#ros_scene_num.data = rospy.get_param('/world_name')
test_num = 3
ros_scene_num.data = test_num
# Assign the object name
ros_object_name = String()
ros_object_name.data = object_name
# Assign the arm that'll be used to pickup the object
ros_arm_to_use = String()
if object_group == 'green':
# The green bin is on the robot's right
ros_arm_to_use.data = 'right'
else:
# The red bin is on the robot's left
ros_arm_to_use.data = 'left'
# Get the PointCloud for the object and obtain it's centroid
# (the average position of all points in the object cloud).
# Convert the cloud to an array, then calculate the average
# of the array.
points_array = ros_to_pcl(object_val.cloud).to_array()
centroid_numpy = np.mean(points_array, axis=0)[:3]
# Convert the numpy float64 to native python floats
centroid = [np.asscalar(x) for x in centroid_numpy]
# Assign the object pick pose
ros_pick_pose = Pose()
ros_pick_pose.position.x = centroid[0]
ros_pick_pose.position.y = centroid[1]
ros_pick_pose.position.z = centroid[2]
# Find the correct dropbox's position
box_pos = [0, 0, 0] # Set a default box position
for box_params in dropbox_list_param:
if box_params['group'] == object_group:
box_pos = box_params['position']
break
# TODO: Add a random offset to the dropbox's position
# Assign the dropbox pose
ros_place_pos = Pose()
ros_place_pos.position.x = box_pos[0]
ros_place_pos.position.y = box_pos[1]
ros_place_pos.position.z = box_pos[2]
# Add the object's yaml dict to the output_list
obj_yaml_dict = make_yaml_dict(ros_scene_num, ros_arm_to_use,
ros_object_name, ros_pick_pose,
ros_place_pos)
output_list.append(obj_yaml_dict)
print('processed %s' % ros_object_name.data)
## Wait for 'pick_place_routine' service to come up
#rospy.wait_for_service('pick_place_routine')
## Uses /srv/PickPlace.srv file
#pick_place_routine = rospy.ServiceProxy('pick_place_routine',
# PickPlace)
#try:
# # Insert the message variables to be sent as a service request
# resp = pick_place_routine(ros_scene_num, ros_object_name,
# ros_arm_to_use, ros_pick_pose,
# ros_place_pos)
# print("Response: ", resp.success)
#except rospy.ServiceException, e:
# print("Service call failed: %s" % e)
# Remove the object from object_list to indicate it was picked up
del object_list[object_i]
# Stop looking through the other identified objects
break
# Output your request parameters into an output yaml file
send_to_yaml('output_%i.yaml' % test_num, output_list)
def pr2_mover(object_list):
# TODO: Initialize variables
labels = []
centroids = [] # list of tuples (x,y,z)
yaml_dict_list = [] # dictionary to store yaml_dict
#list of msg variables sending to pr2_pick_place_server
test_scene_num = Int32()
object_name = String()
arm_name = String()
pick_pose = Pose()
place_pose = Pose()
#rosmsg info geometry_msgs/Pose
# geometry_msgs/Point position
# float64 x
# float64 y
# float64 z
# geometry_msgs/Quaternion orientation
# float64 x
# float64 y
# float64 z
# float64 w
#Based on definition of Pose() rosmsg defined above, initiatize the pick_pose
#and place_pose variables
pick_pose.position.x = 0
pick_pose.position.y = 0
pick_pose.position.z = 0
place_pose.position.x = 0
place_pose.position.y = 0
place_pose.position.z = 0
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variables
# This step is done through for loop later on
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Loop through the pick list
for obj in object_list_param:
# TODO: Get the PointCloud for a given object and obtain it's centroid
object_name.data = obj['name']
print("object_name.data:{}".format(object_name.data))
labels.append(object_name.data)
# Compare detected_objects (pass as 'object_list') and obj in object_list_param
# if match then calculate the centroid
for detected_obj in object_list:
if detected_obj.label == object_name.data: #if detected object match item in pick list
points_arr = ros_to_pcl(detected_obj.cloud).to_array()
centroid_np = np.mean(points_arr, axis=0)[:3]
print("centroid_np: {}".format(centroid_np))
# recast to native Python float type as expected by ROS message
pick_pose.position.x = np.asscalar(centroid_np[0])
pick_pose.position.y = np.asscalar(centroid_np[1])
pick_pose.position.z = np.asscalar(centroid_np[2])
break
# TODO: Assign the arm to be used for pick_place
# definte arm_name based on group
if (obj['group']) == "red":
arm_name.data = "left"
elif (obj['group']) == "green":
arm_name.data = "right"
else:
print("ERROR: group must be 'red' or 'green'!!!")
# TODO: Create 'place_pose' for the object based on group defined
# loop through dropbox_param and set the place_pose when matches with arm_name
# discovered
for db in dropbox_param:
if arm_name.data == db['name']:
place_pose.position.x = db['position'][0]
place_pose.position.y = db['position'][1]
place_pose.position.z = db['position'][2]
break
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
test_scene_num.data = 1
# Populate various ROS messages
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose)
yaml_dict_list.append(yaml_dict)
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace)
#TODO: Insert your message variables to be sent as a service request
#resp = pick_place_routine(test_scene_num, object_name, arm_name, pick_pose, place_pose)
#print ("Response: ",resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def pr2_mover(object_list):
# TODO: Initialize variables
WORLD_ID = 3
test_scene_number = Int32()
test_scene_number.data = WORLD_ID
object_name = String()
arm_name = String()
pick_pose = Pose()
place_pose = Pose()
my_dict_list = []
# TODO: Get/Read parameters
object_list_parm = rospy.get_param('/object_list')
dropbox_parm_list = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variables
object_parm_dict = {}
for idex in range(0, len(object_list_parm)):
object_parm_dict[object_list_parm[idex]
['name']] = object_list_parm[idex]
dropbox_parm_dict = {}
for idex in range(0, len(dropbox_parm_list)):
dropbox_parm_dict[dropbox_parm_list[idex]
['group']] = dropbox_parm_list[idex]
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Loop through the pick list
for object in object_list:
# TODO: Get the PointCloud for a given object and obtain it's centroid
points_arr = ros_to_pcl(object.cloud).to_array()
centroid = np.mean(points_arr, axis=0)[:3]
# Get configuration parameters for this kind of object
object_parm = object_parm_dict[object.label]
# Dropbox parameters
dropbox_parm = dropbox_parm_dict[object_parm['group']]
object_name.data = str(object.label)
# Create pick pose for this object
pick_pose.position.x = np.asscalar(centroid[0])
pick_pose.position.y = np.asscalar(centroid[1])
pick_pose.position.z = np.asscalar(centroid[2])
pick_pose.orientation.x = 0.0
pick_pose.orientation.y = 0.0
pick_pose.orientation.z = 0.0
pick_pose.orientation.w = 0.0
# TODO: Create 'place_pose' for the object
# use location of drop box plus an incremental offset - do not pile all objects at this same location
position = dropbox_parm['position'] + np.random.rand(3) / 10
place_pose.position.x = float(position[0])
place_pose.position.y = float(position[1])
place_pose.position.z = float(position[2])
place_pose.orientation.x = 0.0
place_pose.orientation.y = 0.0
place_pose.orientation.z = 0.0
place_pose.orientation.w = 0.0
# TODO: Assign the arm to be used for pick_place
arm_name.data = str(dropbox_parm['name'])
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
yaml_dict = make_yaml_dict(test_scene_number, arm_name, object_name,
pick_pose, place_pose)
my_dict_list.append(yaml_dict)
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine',
PickPlace)
# TODO: Insert your message variables to be sent as a service request
#resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE)
#print ("Response: ",resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def pr2_mover(object_list):
# TODO: Initialize variables
TEST_SCENE_NUM = Int32()
TEST_SCENE_NUM.data = WORLD
OBJECT_NAME = String()
WHICH_ARM = String()
PICK_POSE = Pose()
PLACE_POSE = Pose()
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variables
# labels = []
# centroids = [] # to be list of tuples (x, y, z)
# for object in object_list:
# labels.append(object.label)
# points_arr = ros_to_pcl(object.cloud).to_array()
# centroids.append(np.mean(points_arr, axis=0)[:3])
object_group_dict = {}
for obj in object_list_param:
object_group_dict[obj['name']] = obj['group']
# print(object_group_dict)
dropbox_dict = {}
color_pos_dict = {}
for box in dropbox_param:
dropbox_dict[box['name']] = box['position']
color_pos_dict[box['group']] = box['name']
# print(dropbox_dict)
# print(color_pos_dict)
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Loop through the pick list
dict_list = []
for i, obj in enumerate(object_list):
# TODO: Get the PointCloud for a given object and obtain it's centroid
OBJECT_NAME.data = str(obj.label)
points_arr = ros_to_pcl(obj.cloud).to_array()
PICK_POSE.position.x = np.asscalar(np.mean(points_arr, axis=0)[0])
PICK_POSE.position.y = np.asscalar(np.mean(points_arr, axis=0)[1])
PICK_POSE.position.z = np.asscalar(np.mean(points_arr, axis=0)[2])
# print(PICK_POSE.position.x, PICK_POSE.position.y, PICK_POSE.position.z)
PICK_POSE.orientation.x = 0
PICK_POSE.orientation.y = 0
PICK_POSE.orientation.z = 0
PICK_POSE.orientation.w = 0
# TODO: Create 'place_pose' for the object
# TODO: Assign the arm to be used for pick_place
WHICH_ARM.data = str(color_pos_dict[object_group_dict[obj.label]])
PLACE_POSE.position.x = dropbox_dict[WHICH_ARM.data][0]
PLACE_POSE.position.y = dropbox_dict[WHICH_ARM.data][1]
PLACE_POSE.position.z = dropbox_dict[WHICH_ARM.data][2]
PLACE_POSE.orientation.x = 0
PLACE_POSE.orientation.y = 0
PLACE_POSE.orientation.z = 0
PLACE_POSE.orientation.w = 0
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
dict_list.append(
make_yaml_dict(TEST_SCENE_NUM, WHICH_ARM, OBJECT_NAME, PICK_POSE,
PLACE_POSE))
# print(make_yaml_dict(TEST_SCENE_NUM, WHICH_ARM, OBJECT_NAME, PICK_POSE, PLACE_POSE))
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine',
PickPlace)
# TODO: Insert your message variables to be sent as a service request
resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM,
PICK_POSE, PLACE_POSE)
print("Response: ", resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
print "Message received: ", msg.data
# using 10s because single digits are used by other messages in the trianing interface.
if msg.data == 'task one':
signal_handler(Int32(10))
if msg.data == 'task two':
signal_handler(Int32(20))
if msg.data == 'task three':
signal_handler(Int32(30))
if __name__ == '__main__':
print "Running client mode..."
rospy.init_node('client_trial')
rospy.Subscriber('/exercise/mode', Int32, signal_handler, queue_size=2)
rospy.Subscriber('/recognizer/output', String, speech_handler, queue_size=1)
start_myo_pub = rospy.Publisher('/myo/launch', Int32, queue_size=1)
time.sleep(0.5)
start_myo_pub.publish(Int32(2))
## dynamically add publishers to MyoDemo2 class
MyoDemo2.pub_l = rospy.Publisher('/exercise/l/playback', Quaternion, queue_size=1)
MyoDemo2.pub_u = rospy.Publisher('/exercise/u/playback', Quaternion, queue_size=1)
print "Classifier launched. Listening to message..."
rospy.spin()
# Change this to change the speed of this program:
hertz = 100
first = True
# initialize ROS node
rospy.init_node('PC_Generator', anonymous=True)
# instantiate publisher
stepPublisher = rospy.Publisher("step", Int32, queue_size=0)
slicePublisher = rospy.Publisher("slice", PointCloudSliceMsg, queue_size=0)
# cloudPublisher = rospy.Publisher("pointCloud", PointCloud2, queue_size = 0)
# instantiate messages to publish
stepMsg = Int32()
cloudMsg = PointCloud2()
# Explicitly declare these global variables (probably not necessary)
currentAngle = None
currentScan = None
# Declare some ROS topic subscriber callback function
def scanCallback(msg):
global currentScan
#print "PCG: scanCallback called"
currentScan = msg
def angleCallback(msg):
def __init__(self):
self.stat_pub = rospy.Publisher("robotStatus", Int32, queue_size=1)
self.msg = Int32()
robobo_pantilt_srv = rospy.ServiceProxy('robot/robobo/movePanTilt',
MovePanTilt)
# Programa principal
ir_frontC = 0
ir_frontRR = 0
ir_frontR = 0
ir_frontLL = 0
ir_frontL = 0
ir_backC = 0
ir_backR = 0
ir_backL = 0
closeIRValue = 60
speed = 20
robobo_move_srv(Int8(speed), Int8(speed), Int32(1000), Int16(0))
while (ir_frontC < closeIRValue) and (ir_frontRR < closeIRValue) and (
ir_frontLL < closeIRValue):
robobo_move_srv(Int8(speed), Int8(speed), Int32(1000), Int16(0))
robobo_move_srv(Int8(0), Int8(0), Int32(1000), Int16(0)) # Stop motors
rospy.sleep(2)
robobo_pantilt_srv(Int16(0), Int8(0), Int16(1), Int16(50), Int8(15), Int16(1))
rospy.sleep(2)
robobo_move_srv(Int8(-speed), Int8(-speed), Int32(2000), Int16(0))
rospy.sleep(2)
class labyrinth_solver:
def __init__(self):
self.image_pub = rospy.Publisher("final_image", Image)
self.move_pub = rospy.Publisher("move_command", Int32)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image,
self.callback)
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, desired_encoding="bgr8")
except CvBridgeError, e:
print e
# crop out the labyrinth region (y by x)
cv_image = cv_image[22:240, 44:268]
# resize the image to 200x200 each region is 10x10
cv_image = cv2.resize(cv_image, (400, 400))
# transfer the image from RGB to HSV
hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
# Red Ball Segmentation
lower_red = np.array([0, 50, 180])
upper_red = np.array([50, 150, 250])
temp_ball = cv2.inRange(hsv_image, lower_red, upper_red)
# Erosion and Dilation processing
kernel = np.ones((3, 3), np.uint8)
temp_ball = cv2.dilate(temp_ball, kernel, iterations=2)
cv2.imshow("Red Ball", temp_ball)
# Calculate the contour
contours, hierarcy = cv2.findContours(temp_ball, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
# Select the biggest contout as the target
max_area = 0
for cnt in contours:
area = cv2.contourArea(cnt)
if area > max_area:
max_area = area
target = cnt
global position_history # calling global variable
# handling with target missing
if max_area >= 10:
(x, y), radius = cv2.minEnclosingCircle(target)
center = (int(x), int(y))
else:
center = position_history
# Compensate with some noise
radius = 10
if abs(center[0] - position_history[0]) + abs(
center[1] - position_history[1]) <= 4:
center = position_history
cv2.circle(cv_image, center, radius, (0, 255, 0), 2)
position_history = center
cv2.imshow("Ball tracking", cv_image)
# manipulate the center coordinate to be the nearest 10 while extract the position in 20 by 20
# FIRST check who is more close to 0
checkx = center[0] % 20 - 10
checky = center[1] % 20 - 15
if abs(checkx) <= abs(checky):
newx = center[0] - checkx
newy = center[1] * 0.955
elif abs(checkx) > abs(checky):
newx = center[0]
newy = 0.955 * (center[1] - checky)
newcenter = (newx, int(newy))
# read the reference map for animation
map_ref = cv2.imread('/home/sunyue/catkin_ws/src/tracking/map.png')
cv2.circle(map_ref, newcenter, radius, (0, 0, 255), -5)
# SECOND transfer the real location to the 20x20 grid
gridx = newcenter[0] / 20 + 1
gridy = newcenter[1] / 20 + 1
# A* for path planning
goal = [10, 2]
current = [gridx, gridy]
precheck = abs(current[0] - goal[0]) + abs(current[1] - goal[1])
if precheck == 0: check = 0
else: check = 100
path = np.array([current])
backup = np.array([[gridx, gridy, gridx, gridy]])
while check != 0:
# generate the potential candidate
north = [current[0], current[1] - 1]
south = [current[0], current[1] + 1]
east = [current[0] + 1, current[1]]
west = [current[0] - 1, current[1]]
#print current
# calculate the heuristic
n_heuristic = math.sqrt(
pow(north[0] - goal[0], 2) + pow(north[1] - goal[1], 2))
s_heuristic = math.sqrt(
pow(south[0] - goal[0], 2) + pow(south[1] - goal[1], 2))
e_heuristic = math.sqrt(
pow(east[0] - goal[0], 2) + pow(east[1] - goal[1], 2))
w_heuristic = math.sqrt(
pow(west[0] - goal[0], 2) + pow(west[1] - goal[1], 2))
# check the punishment of obstacle
if MAP[north[1] - 1, north[0] - 1] == 0: n_punish = 2000
else: n_punish = 0
if MAP[south[1] - 1, south[0] - 1] == 0: s_punish = 2000
else: s_punish = 0
if MAP[east[1] - 1, east[0] - 1] == 0: e_punish = 2000
else: e_punish = 0
if MAP[west[1] - 1, west[0] - 1] == 0: w_punish = 2000
else: w_punish = 0
#print n_punish, s_punish, e_punish, w_punish
# check last node never go back
num = path.shape[0] # get the path step number
if num != 1:
last_step = path[-2]
n_check = north - last_step
s_check = south - last_step
e_check = east - last_step
w_check = west - last_step
if (n_check[0] == 0 and n_check[1] == 0): n_punish = 2000
if (s_check[0] == 0 and s_check[1] == 0): s_punish = 2000
if (e_check[0] == 0 and e_check[1] == 0): e_punish = 2000
if (w_check[0] == 0 and w_check[1] == 0): w_punish = 2000
# sum the cost together
n_cost = int(n_heuristic + n_punish)
s_cost = int(s_heuristic + s_punish)
e_cost = int(e_heuristic + e_punish)
w_cost = int(w_heuristic + w_punish)
cost = [n_cost, s_cost, e_cost, w_cost]
# there will be some situations should be taken into consideration
index = np.argmin(cost) # where the smallest cost is located
mincost = cost[index]
# First only one direction cost is less than 1000, then just pick that
if mincost <= 1000: # there must be at least one solution
sumcheck = cost[0] + cost[1] + cost[2] + cost[3]
if sumcheck >= 6000: # only one next choice
if index == 0: next = north
elif index == 1: next = south
elif index == 2: next = east
elif index == 3: next = west
# update the path
path = np.append(path, [next], axis=0)
# update the check for next while
precheck = abs(next[0] - goal[0]) + abs(next[1] - goal[1])
if precheck == 0:
check = 0
# updat the current
current = next
elif (sumcheck >= 4000
and sumcheck < 6000): # two posible choices
if index == 0: next = north
elif index == 1: next = south
elif index == 2: next = east
elif index == 3: next = west
# update the path choose the one have the least cost
path = np.append(path, [next], axis=0)
# update the check for next while
precheck = abs(next[0] - goal[0]) + abs(next[1] - goal[1])
if precheck == 0:
check = 0
# save the branch to the back up [current, branch]
fakecost = cost
fakecost[
index] = 2000 # mannually fake the minimum cost choice
fakeindex = np.argmin(
fakecost) # where the smallest cost is located
if fakeindex == 0: branch = north
elif fakeindex == 1: branch = south
elif fakeindex == 2: branch = east
elif fakeindex == 3: branch = west
backup = np.append(
[[current[0], current[1], branch[0], branch[1]]],
backup,
axis=0)
# updat the current
current = next
elif (sumcheck >= 2000
and sumcheck < 4000): # three posible choices
if index == 0: next = north
elif index == 1: next = south
elif index == 2: next = east
elif index == 3: next = west
# update the path choose the one have the least cost
path = np.append(path, [next], axis=0)
# update the check for next while
precheck = abs(next[0] - goal[0]) + abs(next[1] - goal[1])
if precheck == 0:
check = 0
# save the branch to the back up [current, branch]
# second cost
secondcost = cost
secondcost[
index] = 2000 # mannually fake the minimum cost choice
secondindex = np.argmin(
secondcost) # where the smallest cost is located
if secondindex == 0: branch1 = north
elif secondindex == 1: branch1 = south
elif secondindex == 2: branch1 = east
elif secondindex == 3: branch1 = west
thirdcost = secondcost
thirdcost[
secondindex] = 2000 # mannually fake the minimum cost choice
thirdindex = np.argmin(
thirdcost) # where the smallest cost is located
if thirdindex == 0: branch2 = north
elif thirdindex == 1: branch2 = south
elif thirdindex == 2: branch2 = east
elif thirdindex == 3: branch2 = west
# update branch based on cost difference
backup = np.append(
[[current[0], current[1], branch2[0], branch2[1]]],
backup,
axis=0)
backup = np.append(
[[current[0], current[1], branch1[0], branch1[1]]],
backup,
axis=0)
# updat the current
current = next
elif mincost >= 2000: # there is no next choice we have go to backup branchs
# next step is the first ranking branch
next = [backup[0, 2], backup[0, 3]]
# cut the path back
current = [backup[0, 0], backup[0, 1]]
compare = abs(path - current)
summation = sum(np.transpose(compare))
index = np.argmin(summation)
# cut the path from 0 to current one
path = path[:index + 1]
# update the path with next step
path = np.append(path, [next], axis=0)
# delete the first backup
backup = backup[1:]
# update the check for next while
precheck = abs(next[0] - goal[0]) + abs(next[1] - goal[1])
if precheck == 0:
check = 0
# updat the current
current = next
# A* algorithm is ended here
# refine the step path to corner path
current = [path[0, 0], path[0, 1]]
corpath = np.array([current])
steps = path.shape[0]
i = 1
while i < steps:
onpath = [path[i, 0], path[i, 1]]
if (onpath[0] == current[0] or onpath[1] == current[1]):
i = i + 1
else:
corpath = np.append(corpath,
[[path[i - 1, 0], path[i - 1, 1]]],
axis=0)
i = i + 1
current = [path[i - 2, 0], path[i - 2, 1]]
corpath = np.append(corpath, [goal], axis=0)
# draw the path on the map animation
steps = corpath.shape[0]
i = 0
while i < steps - 1:
cv2.line(
map_ref, (20 * corpath[i, 0] - 10, 20 * corpath[i, 1] - 10),
(20 * corpath[i + 1, 0] - 10, 20 * corpath[i + 1, 1] - 10),
(255, 0, 0), 3)
i = i + 1
cv2.imshow("Map Image", map_ref)
# identify the motion of the youBot arm
current = [corpath[0, 0], corpath[0, 1]]
next = [corpath[1, 0], corpath[1, 1]]
# move direction left = 1 right = 2 forward = 3 backward = 4
if (current[0] > next[0] and current[1] == next[1]):
move = 1
print "Move to left", move
elif (current[0] < next[0] and current[1] == next[1]):
move = 2
print "Move to right", move
elif (current[0] == next[0] and current[1] > next[1]):
move = 3
print "Move to forward", move
elif (current[0] == next[0] and current[1] < next[1]):
move = 4
print "Move to backward", move
elif (current[0] == goal[0] and current[1] == goal[1]):
move = 5
else:
move = 0
self.move_pub.publish(Int32(move))
cv2.waitKey(1)
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(cv_image, encoding="bgr8"))
except CvBridgeError, e:
print e
def pr2_mover(object_list):
# TODO: Initialize variables
test_num = 1
ros_scene_num = Int32()
ros_object_name = String()
ros_arm_to_use = String()
ros_pick_pose = Pose()
ros_place_pos = Pose()
# Initialize output list that'll store multiple object yaml dictionaries
output_list = []
# Load the parameters from the YAML files located in /pr2_robot/config/
object_list_param = rospy.get_param('/object_list')
dropbox_list_param = rospy.get_param('/dropbox')
# Iterate through all objects that should be moved
for object_params in object_list_param:
object_name = object_params['name']
object_group = object_params['group']
# Check if the object
for object_i, object_val in enumerate(object_list):
if object_name != object_val.label:
# Skip this object it doesn't match the object to be moved
continue
ros_scene_num.data = test_num
ros_object_name.data = object_name
# Assign the arm that'll be used to pickup the object
if object_group == 'green':
ros_arm_to_use.data = 'right'
else:
ros_arm_to_use.data = 'left'
# Get the PointCloud for the object and obtain it's centroid
# (the average position of all points in the object cloud).
# Convert the cloud to an array, then calculate the average
# of the array.
points_array = ros_to_pcl(object_val.cloud).to_array()
centroid_numpy = np.mean(points_array, axis=0)[:3]
# Convert the numpy float64 to native python floats
centroid = [np.asscalar(x) for x in centroid_numpy]
#Create 'place_pose' for the object
ros_pick_pose.position.x = centroid[0]
ros_pick_pose.position.y = centroid[1]
ros_pick_pose.position.z = centroid[2]
# Find the correct dropbox's position
box_pos = [0, 0, 0]
for box_params in dropbox_list_param:
if box_params['group'] == object_group:
box_pos = box_params['position']
break
#Create 'place_pose' for the object
ros_place_pos.position.x = box_pos[0]
ros_place_pos.position.y = box_pos[1]
ros_place_pos.position.z = box_pos[2]
# Add the object's yaml dict to the output_list
obj_yaml_dict = make_yaml_dict(ros_scene_num, ros_arm_to_use,
ros_object_name, ros_pick_pose,
ros_place_pos)
output_list.append(obj_yaml_dict)
'''
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace)
# TODO: Insert your message variables to be sent as a service request
resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE)
print ("Response: ",resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
'''
print('Object name which successfully processed: %s ' %
ros_object_name.data)
# Remove the object from object_list when it was picked up
del object_list[object_i]
break
# Output your request parameters into output yaml file
send_to_yaml('output_%i.yaml' % test_num, output_list)
def pr2_mover(object_list):
# TODO: Initialize variables
test_scene_num = Int32()
object_name = String()
pick_pose = Pose()
place_pose = Pose()
arm_name = String()
yaml_dict_list = []
# update based on scene
test_scene_num.data = 3
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variables
# TODO: Rotate PR2 in place to capture side tables for the collision map
#pr2_mover_pub.publish(-1.57)
#rospy.sleep(15.0)
#pr2_mover_pub.publish(1.57)
#rospy.sleep(30.0)
#pr2_mover_pub.publish(0)
# Calculate object centoroids
labels = []
centroids = [] # to be list of tuples (x, y, z)
for object in object_list:
labels.append(object.label)
points_arr = ros_to_pcl(object.cloud).to_array()
centroids.append(np.mean(points_arr, axis=0)[:3])
# TODO: Loop through the pick list
for i in range(len(object_list_param)):
# TODO: Get the PointCloud for a given object and obtain it's centroid
object_name.data = object_list_param[i]['name']
object_group = object_list_param[i]['group']
try:
print labels, object_name.data
index = labels.index(object_name.data)
except ValueError:
print "Object not detected: %s" %object_name.data
continue
# TODO: Create 'place_pose' for the object
pick_pose.position.x = np.asscalar(centroids[index][0])
pick_pose.position.y = np.asscalar(centroids[index][1])
pick_pose.position.z = np.asscalar(centroids[index][2])
position = search_dicts('group', object_group, 'position', dropbox_param)
place_pose.position.x = position[0] * random.uniform(0.85, 1.15)
place_pose.position.y = position[1] * random.uniform(0.85, 1.15)
place_pose.position.z = position[2] * random.uniform(0.85, 1.15)
# TODO: Assign the arm to be used for pick_place
arm_name.data = search_dicts('group', object_group, 'name', dropbox_param)
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose)
yaml_dict_list.append(yaml_dict)
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
#'''
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace)
# TODO: Insert your message variables to be sent as a service request
resp = pick_place_routine(test_scene_num, object_name, arm_name, pick_pose, place_pose)
print ("Response: ",resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def pr2_mover(object_list):
# TODO: Initialize variables
labels = []
centroids = []
dict_list = []
test_scene_num.data = Int32()
object_name = String()
arm_name = String()
pick_pose = Pose()
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
# TODO: Loop through the pick list
for object in detected_objects:
# TODO: Get the PointCloud for a given object and obtain it's centroid
labels.append(object.label)
points_arr = ros_to_pcl(object.cloud).to_array()
centroids.append(np.mean(points_arr, axis=0)[:3])
# TODO: Parse parameters into individual variables i=index used to traverse the list??
for i in range(0, len(object_list_param)):
test_scene_num.data = 1
object_name.data = object_list_param[i]['name']
object_group.data = object_list_param[i]['group']
# TODO: Assign the arm to be used for pick_place
if object_group.data == 'green':
arm_name.data = 'right'
if object_group.data == 'red':
arm_name.data = 'left'
for k in range(len(labels)):
if labels[k] == object_name.data:
pick_pose.position.x = np.asscalar(centroids[i][0])
pick_pose.position.y = np.asscalar(centroids[i][1])
pick_pose.position.z = np.asscalar(centroids[i][2])
# TODO: Create 'place_pose' for the object
if arm_name.data == 'right':
place_pose.position.x = dropbox_param[1]['position'][0]
place_pose.position.y = dropbox_param[1]['position'][1]
place_pose.position.z = dropbox_param[1]['position'][2]
if arm_name.data == 'left':
place_pose.position.x = dropbox_param[0]['position'][0]
place_pose.position.y = dropbox_param[0]['position'][1]
place_pose.position.z = dropbox_param[0]['position'][2]
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
for i in range(0, len(object_list_param)):
# Populate various ROS messages
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose)
dict_list.append(yaml_dict)
send_to_yaml(yaml_filename, dict_list)
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace)
# TODO: Insert your message variables to be sent as a service request
resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE)
print ("Response: ",resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
#!/usr/bin/env python
import rospy
from std_msgs.msg import Int32
import time
rospy.init_node('fake_publisher')
pub = rospy.Publisher('/voice_command', Int32, queue_size=1)
start = time.time()
while True:
num = raw_input('please enter \n')
pub.publish(Int32(data=int(num)))
# if time.time() - start == 5.0:
# num = 1
# #num = raw_input('please enter \n')
# print "1"
# pub.publish(Int32(data=int(num)))
# elif time.time() - start == 10.0:
# num = 2
# #num = raw_input('please enter \n')
# pub.publish(Int32(data=int(num)))
# print "2"
# elif time.time() - start == 15.0:
# break
if not rospy.is_shutdown:
break
#rospy.spin()
def pr2_mover(object_list):
# here we bascially just do a bit data munging to make sure that we
# generate the message for the robot to pick up
# the gist is to read in the file where is specified the order how the things
# should be picked up
# and then lookup some other stuff from couple other places
# Initialize variables
labels = []
groups = []
centroids = []
# Get/Read parameters from param server
# did not figure out how to read the world we are in
object_list_param = rospy.get_param('/object_list')
dropbox = rospy.get_param('/dropbox')
# Parse parameters into individual variables
dropbox_lookup = dict(map(lambda i: [i['group'], i['position']], dropbox))
arm_lookup = {'red': 'left', 'green': 'right'}
# Loop through the pick list
# and build the lists of things. Lables, centroid and groups
for item in object_list_param:
# Get the PointCloud for a given object and obtain it's centroid
name = item['name']
print name
print map(lambda o: o.label, object_list)
objects = filter(lambda o: o.label == name, object_list)
if (not objects):
continue
object = objects[0]
labels.append(name)
groups.append(item['group'])
points_arr = ros_to_pcl(object.cloud).to_array()
centroids.append(np.mean(points_arr, axis=0)[:3])
# we need to retype the centroids to a different datatype that ROS can understand
centroids = map(lambda centroid: map(np.asscalar, centroid), centroids)
dict_list = []
for (centroid, label, group) in zip(centroids, labels, groups):
test_scene_num = Int32()
test_scene_num.data = world_name
object_name = String()
object_name.data = label
arm_name = String()
arm_name.data = arm_lookup[group]
pick_pose = Pose()
pick_pose.position.x = centroid[0]
pick_pose.position.y = centroid[1]
pick_pose.position.z = centroid[2]
place_pos_data = dropbox_lookup[group]
place_pose = Pose()
place_pose.position.x = place_pos_data[0]
place_pose.position.y = place_pos_data[1]
place_pose.position.z = place_pos_data[2]
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose)
dict_list.append(yaml_dict)
# Uncomment this if you want to see the robot doing work
# Unfortunately it does not work very often
# he just does not pick up things most of the time
# TODO: Create 'place_pose' for the object
# TODO: Assign the arm to be used for pick_place
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
# Wait for 'pick_place_routine' service to come up
# rospy.wait_for_service('pick_place_routine')
# try:
# pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace)
# # TODO: Insert your message variables to be sent as a service request
# resp = pick_place_routine(test_scene_num, object_name, arm_name, pick_pose, place_pose)
# print ("Response: ",resp.success)
# except rospy.ServiceException, e:
# print "Service call failed: %s"%e
# Output your request parameters into output yaml file
# once we collect all messages we need
send_to_yaml('yaml_filename_' + str(world_name) + '.yaml', dict_list)