def _find_control(self, tag, mask, details, header, factor, other_one=None):
""" Find a control """
corners = [None, None]
indexes = mask.nonzero()
points = []
indexes2 = []
for i in range(len(indexes[1])):
pnt = details[indexes[0][i], indexes[1][i]]
if pnt[2] > DISH_MAXIMUM:
print 'rejecting point, too distant ', pnt[2]
mask[indexes[0][i], indexes[1][i]] = 0
continue
if other_one:
dist = np.sqrt((other_one[0].point.y - pnt[0])**2 + \
(other_one[0].point.z - pnt[1])**2 +\
(other_one[0].point.x - pnt[2])**2)
if dist > 1.0:
print 'rejecting point, too far away from other one', dist
mask[indexes[0][i], indexes[1][i]] = 0
continue
points.append(details[indexes[0][i], indexes[1][i]])
indexes2.append((indexes[0][i], indexes[1][i]))
points = np.array(points)
if len(points) < 3:
log("Error: only {} {} points found.".format(len(points), tag))
raise ZarjConfused('Error: only {} {} points found.'.format(
len(points), tag))
mean = np.mean(points, axis=0)[2]
stdv = np.std(points, axis=0)[2]
for i, pnt in enumerate(points):
if abs(pnt[2] - mean) < factor * stdv:
_add_bounding_point(pnt, corners)
else:
mask[indexes[0][i], indexes[1][i]] = 0
point = PointStamped()
point.header = header
point.point.x = corners[0][0]
point.point.y = corners[0][1]
point.point.z = corners[0][2]
cor1 = self.fc.zarj.transform.tf_buffer.transform(point, 'pelvis')
point = PointStamped()
point.header = header
point.point.x = corners[1][0]
point.point.y = corners[1][1]
point.point.z = corners[1][2]
cor2 = self.fc.zarj.transform.tf_buffer.transform(point, 'pelvis')
log("{}, {} {}".format(tag, cor1, cor2))
return (cor1, cor2)
python类PointStamped()的实例源码
def _find_feature(self, tag, low, high):
""" Find a feature on the door"""
# we only want the center of the image
cloud = self.fc.zarj.eyes.get_stereo_cloud()
image, details = self.fc.zarj.eyes.get_cloud_image_with_details(cloud)
shape = image.shape
image = image[0:shape[0], shape[1]/3:2*shape[1]/3]
details = details[0:shape[0], shape[1]/3:2*shape[1]/3]
colors = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
image_dump = os.environ.get("ZARJ_IMAGE_DUMP")
mask = cv2.inRange(colors, low, high)
if image_dump is not None:
image_idx = rospy.get_time()
cv2.imwrite(image_dump+'/door_{}.png'.format(image_idx), image)
cv2.imwrite(image_dump+'/door_{}_{}.png'.format(tag, image_idx),
mask)
indexes = mask.nonzero()
points = []
for i in range(len(indexes[1])):
pnt = details[indexes[0][i], indexes[1][i]]
if pnt[2] > 2.0:
print 'Discard non {} point, too far away'.format(tag)
continue
points.append(details[indexes[0][i], indexes[1][i]])
points = np.array(points)
if len(points) < 10:
log("Failed to find {} in the door".format(tag))
return None
avg = np.mean(points, axis=0)
point = PointStamped()
point.header = cloud.header
point.point.x = avg[0]
point.point.y = avg[1]
point.point.z = avg[2]
final = self.fc.zarj.transform.tf_buffer.transform(point, 'pelvis')
log('{} located at about {}'.format(tag, final.point))
return final.point.y
def __init__(self):
self.rate = rospy.get_param("~rate", 20.0)
self.period = 1.0 / self.rate
# angular mode maps angular z directly to steering angle
# (adjusted appropriately)
# non-angular mode is somewhat suspect, but it turns
# a linear y into a command to turn just so that the
# achieved linear x and y match the desired, though
# the vehicle has to turn to do so.
self.angular_mode = rospy.get_param("~angular_mode", True)
self.tf_buffer = tf2_ros.Buffer()
self.tf = tf2_ros.TransformListener(self.tf_buffer)
self.steer_link = rospy.get_param("~steer_link", "lead_steer")
self.steer_joint = rospy.get_param("~steer_joint", "lead_steer_joint")
# +/- this angle
self.min_steer_angle = rospy.get_param("~min_steer_angle", -0.7)
self.max_steer_angle = rospy.get_param("~max_steer_angle", 0.7)
self.wheel_joint = rospy.get_param("~wheel_joint", "wheel_lead_axle")
self.wheel_radius = rospy.get_param("~wheel_radius", 0.15)
# the spin center is always on the fixed axle y axis of the fixed axle,
# it is assume zero rotation on the steer_joint puts the steering
# at zero rotation with respect to fixed axle x axis (or xz plane)
self.fixed_axle_link = rospy.get_param("~fixed_axle_link", "back_axle")
self.point_pub = rospy.Publisher("cmd_vel_spin_center", PointStamped, queue_size=1)
self.steer_pub = rospy.Publisher("steer_joint_states", JointState, queue_size=1)
# TODO(lucasw) is there a way to get TwistStamped out of standard
# move_base publishers?
self.joint_state = JointState()
self.joint_state.name.append(self.steer_joint)
self.joint_state.position.append(0.0)
self.joint_state.velocity.append(0.0)
self.joint_state.name.append(self.wheel_joint)
self.joint_state.position.append(0.0)
self.joint_state.velocity.append(0.0)
self.cmd_vel = Twist()
rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=2)
self.timer = rospy.Timer(rospy.Duration(self.period), self.update)
