def look_around_movement(self, factor=1):
self.look_around = True
current_orientation = tf.transformations.euler_from_quaternion([
self.odometry.pose.pose.orientation.x,
self.odometry.pose.pose.orientation.y,
self.odometry.pose.pose.orientation.z,
self.odometry.pose.pose.orientation.w
])
waypoint_req = WorldWaypointReq()
waypoint_req.goal.priority = GoalDescriptor.PRIORITY_NORMAL
waypoint_req.goal.requester = self.name + '_pose'
waypoint_req.disable_axis.x = not self.odometry_updated
waypoint_req.disable_axis.y = not self.odometry_updated
waypoint_req.disable_axis.z = True
waypoint_req.disable_axis.roll = True
waypoint_req.disable_axis.pitch = True
waypoint_req.disable_axis.yaw = False
waypoint_req.position.north = float(self.odometry.pose.pose.position.x)
waypoint_req.position.east = float(self.odometry.pose.pose.position.y)
waypoint_req.orientation.yaw = cola2_lib.normalizeAngle(
current_orientation[2] + (factor * self.yaw_offset))
for i in range(int(100 * factor)):
waypoint_req.header.stamp = rospy.Time.now()
self.pub_waypoint_req.publish(waypoint_req)
rospy.sleep(0.1)
print 'look around.'
waypoint_req.orientation.yaw = cola2_lib.normalizeAngle(
current_orientation[2] - (factor * self.yaw_offset))
for i in range(int(150 * factor)):
waypoint_req.header.stamp = rospy.Time.now()
self.pub_waypoint_req.publish(waypoint_req)
rospy.sleep(0.1)
print 'look around..'
waypoint_req.orientation.yaw = current_orientation[2]
for i in range(int(50 * factor)):
waypoint_req.header.stamp = rospy.Time.now()
self.pub_waypoint_req.publish(waypoint_req)
rospy.sleep(0.1)
print 'look around...'
self.look_around = False
def unNormalizeAngle(self, current_angle, new_angle):
"""
This function unNormalize the Angle obtaining a continuous values
avoiding the discontinuity, jumps from 3.14 to -3.14
@param current_angle: contain the current angle not normalized
@type current_angle: double
@param new_angle: contain the new angle normalized
@type new_angle: double
"""
# rospy.loginfo('Current angle ' + str(current_angle) +
# ' New angle ' + str(new_angle))
if abs(current_angle) > np.pi:
#We are over one lap over
norm_curr = cola2_lib.normalizeAngle(current_angle)
if abs(new_angle - norm_curr) > np.pi:
# rospy.loginfo('Overflow 2')
if new_angle < 0.0:
inc0 = -1.0 * (-np.pi - new_angle)
inc1 = -1.0 * (np.pi - norm_curr)
else:
inc0 = -1.0 * (np.pi - new_angle)
inc1 = (-np.pi - norm_curr)
return current_angle + inc0 + inc1
else:
# rospy.loginfo('Actual plus diff')
return current_angle + (new_angle - norm_curr)
else:
if abs(new_angle - current_angle) > np.pi:
# rospy.loginfo('Over Flow')
if new_angle < 0.0:
inc0 = -1.0 * (-np.pi - new_angle)
inc1 = -1.0 * (np.pi - current_angle)
else:
inc0 = -1.0 * (np.pi - new_angle)
inc1 = (-np.pi - current_angle)
return current_angle + inc0 + inc1
else:
# rospy.loginfo('Tal qual')
return new_angle
def __compute_yaw_rate__(self, y_offset, x_offset):
# Publish Body Velocity Request
body_velocity_req = BodyVelocityReq()
distance = math.sqrt(y_offset**2 + x_offset**2)
#print 'distance compute yaw: ', distance
if distance > 1.5:
# twist set-point
body_velocity_req.twist.angular.z = y_offset / 4.0
#print '>>>>>>>>>>>>>>>>>>>>>>>bigger than 1.5 m'
else:
body_velocity_req.twist.angular.z = cola2_lib.normalizeAngle(
self.chain_orientation - self.current_yaw) / 10.0
#print '>>>>>>>>>>>>>>>>>>>>>>>smaller than 1.5 m'
#print 'Chain orientation:', self.chain_orientation, ' Current yaw: ', self.current_yaw
if body_velocity_req.twist.angular.z > 0.15:
body_velocity_req.twist.angular.z = 0.15
elif body_velocity_req.twist.angular.z < -0.15:
body_velocity_req.twist.angular.z = -0.15
# header & goal
body_velocity_req.header.stamp = rospy.Time().now()
body_velocity_req.goal.priority = GoalDescriptor.PRIORITY_NORMAL
body_velocity_req.goal.requester = self.name + '_velocity'
# Check if DoF is disable
body_velocity_req.disable_axis.x = True
body_velocity_req.disable_axis.y = True
body_velocity_req.disable_axis.z = True
body_velocity_req.disable_axis.roll = True
body_velocity_req.disable_axis.pitch = True
body_velocity_req.disable_axis.yaw = False
return body_velocity_req
def pub_imu_callback(self, event):
""" This method is a callback of a timer. This publishes imu and pressure
sensor data """ # TODO: euler rate is not angular velocity!
# First publish pressure sensor
msg = PressureSensor()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'pressure_sensor'
msg.temperature = 25.0
msg.pressure = self.odom.pose.pose.position.z * self.water_density * 9.81 / 100000.0
self.pub_pressure.publish(msg)
# Imu
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = 'adis_imu'
# Add some noise
roll = self.orientation[0] + np.random.normal(
0.0, self.imu_orientation_covariance[0])
pitch = self.orientation[1] + np.random.normal(
0.0, self.imu_orientation_covariance[1])
yaw = self.orientation[2] + np.random.normal(
0.0, self.imu_orientation_covariance[2])
# Rotate as necessary
vehicle_rpy = PyKDL.Rotation.RPY(roll, pitch, yaw)
imu_orientation = self.imu_tf.M * vehicle_rpy
# Get RPY
angle = imu_orientation.GetRPY()
# Derive to obtain rates
if not self.imu_init:
# Initialize heading buffer in order to apply a
# savitzky_golay derivation
if len(self.heading_buffer) == 0:
self.heading_buffer.append(angle[2])
inc = cola2_lib.normalizeAngle(angle[2] - self.heading_buffer[-1])
self.heading_buffer.append(self.heading_buffer[-1] + inc)
if len(self.heading_buffer) == len(self.savitzky_golay_coeffs):
self.imu_init = True
# Euler derivation to roll and pitch, so:
self.last_imu_orientation = angle
self.last_imu_update = imu.header.stamp
else:
period = (imu.header.stamp - self.last_imu_update).to_sec()
if period < 0.001:
period = 0.001 # Minimum allowed period
# For yaw rate we apply a savitzky_golay derivation
inc = cola2_lib.normalizeAngle(angle[2] - self.heading_buffer[-1])
self.heading_buffer.append(self.heading_buffer[-1] + inc)
self.heading_buffer.pop(0)
imu.angular_velocity.z = np.convolve(self.heading_buffer,
self.savitzky_golay_coeffs,
mode='valid') / period
# TODO: Roll rate and Pitch rate should be also
# savitzky_golay derivations?
imu.angular_velocity.x = cola2_lib.normalizeAngle(
angle[0] - self.last_imu_orientation[0]) / period
imu.angular_velocity.y = cola2_lib.normalizeAngle(
angle[1] - self.last_imu_orientation[1]) / period
self.last_imu_orientation = angle
self.last_imu_update = imu.header.stamp
imu.angular_velocity_covariance = [
0.0001, 0., 0., 0., 0.0001, 0., 0., 0., 0.0002
]
# Euler to Quaternion
angle = tf.transformations.quaternion_from_euler(
angle[0], angle[1], angle[2])
imu.orientation.x = angle[0]
imu.orientation.y = angle[1]
imu.orientation.z = angle[2]
imu.orientation.w = angle[3]
imu.orientation_covariance[0] = self.imu_orientation_covariance[0]
imu.orientation_covariance[4] = self.imu_orientation_covariance[1]
imu.orientation_covariance[8] = self.imu_orientation_covariance[2]
#rospy.loginfo("%s: publishing imu", self.name)
self.pub_imu.publish(imu)
# Publish TF
imu_tf = tf.TransformBroadcaster()
o = tf.transformations.quaternion_from_euler(
math.radians(self.imu_tf_array[3]),
math.radians(self.imu_tf_array[4]),
math.radians(self.imu_tf_array[5]), 'sxyz')
imu_tf.sendTransform(
(self.imu_tf_array[0], self.imu_tf_array[1], self.imu_tf_array[2]),
o, imu.header.stamp, 'adis_imu_from_sim_nav_sensors', 'hug')
def store_pose_same_orgin(self, element, frame, ori, file):
"""
This function convert the first pose and orientation in the world frame,
to the frame passed as second argument. Includes new extra orientation
to simplify the learning process Finally it store it at the file
passed as third parameter.
@param pose: Position and Orientation of the element in world
@type pose: Pose
@param frame: Position and Orientation of the frame
@type frame: Pose
@param file: Position and Orientation of the element
@type ori: Orientation in yaw of the valve or the panel
@param ori: Double
@param file: Position and Orientation of the element
@type file: File
@return ret: Return the new position wroted in the file
@tyep ret: Pose
"""
element_frame = tf.transformations.quaternion_matrix([
element.orientation.x, element.orientation.y,
element.orientation.z, element.orientation.w
])
element_frame[0, 3] = element.position.x
element_frame[1, 3] = element.position.y
element_frame[2, 3] = element.position.z
trans_matrix = tf.transformations.quaternion_matrix([
frame.orientation.x, frame.orientation.y, frame.orientation.z,
frame.orientation.w
])
trans_matrix[0, 3] = frame.position.x
trans_matrix[1, 3] = frame.position.y
trans_matrix[2, 3] = frame.position.z
#invert Matrix
inv_mat = np.zeros([4, 4])
inv_mat[3, 3] = 1.0
inv_mat[0:3, 0:3] = np.transpose(trans_matrix[0:3, 0:3])
inv_mat[0:3, 3] = np.dot((-1 * inv_mat[0:3, 0:3]), trans_matrix[0:3,
3])
new_frame = np.dot(inv_mat, element_frame)
yaw_element = tf.transformations.euler_from_matrix(element_frame)[2]
difference = cola2_lib.normalizeAngle(yaw_element - ori)
new_pose = Pose()
quaternion = tf.transformations.quaternion_from_matrix(new_frame)
new_pose.orientation.x = quaternion[0]
new_pose.orientation.y = quaternion[1]
new_pose.orientation.z = quaternion[2]
new_pose.orientation.w = quaternion[3]
new_pose.position.x = new_frame[0, 3]
new_pose.position.y = new_frame[1, 3]
new_pose.position.z = new_frame[2, 3]
euler = tf.transformations.euler_from_matrix(new_frame)
line = (repr(rospy.get_time()) + " " + repr(new_frame[0, 3]) + " " +
repr(new_frame[1, 3]) + " " + repr(new_frame[2, 3]) + " " +
repr(difference) + " " + repr(euler[0]) + " " +
repr(euler[1]) + " " + repr(euler[2]) + "\n")
file.write(line)
return new_pose
def cluster_meanshift(self, X):
cluster_centers_filtered = np.array([])
# Compute clustering with MeanShift
# The following bandwidth can be automatically detected using
bandwidth = estimate_bandwidth(X[:, 0:2], quantile=0.2)
#print "bandwidth: ", bandwidth
ms = MeanShift(bandwidth=0.2, bin_seeding=False,
cluster_all=False) #, min_bin_freq=10
ms.fit(X[:, 0:2])
labels = ms.labels_
#ipdb.set_trace()
cluster_centers = ms.cluster_centers_
labels_unique = np.unique(labels)
#remove -1 label
labels_unique = [x for x in labels_unique if x >= 0]
n_clusters_ = len(labels_unique)
print("number of estimated clusters : %d" % n_clusters_)
self.lock.acquire()
self.markerArray = MarkerArray()
#fig = pl.figure(1)
#pl.ion()
#pl.clf()
cluster_centers = np.append(cluster_centers,
np.zeros([len(cluster_centers), 1]), 1)
colors = cycle('bgrcmybgrcmybgrcmybgrcmy')
for k, col in zip(range(n_clusters_), colors):
my_members = labels == k
try:
#mean of all detection z's
cluster_centers[k, 2] = sum(X[my_members, 2]) / len(
X[my_members, 2])
cluster_center = cluster_centers[k]
except:
print "k: ", k, "len de cluster centers: ", len(
cluster_centers)
#pl.plot(X[my_members, 0], X[my_members, 1], col + '.')
#pl.plot(cluster_center[0], cluster_center[1], 'o', markerfacecolor=col,
# markeredgecolor='k', markersize=14)
#ipdb.set_trace()
print "len my member: ", sum(my_members)
if len(cluster_centers_filtered) != 0:
cluster_centers_filtered = np.vstack(
(cluster_centers_filtered, cluster_centers[k]))
else:
cluster_centers_filtered = cluster_centers[k]
self.sort_cluster_centers([
cluster_centers_filtered,
])
points_cluster_centers = []
for k in range(len(self.cluster_centers_sorted)):
#if np.sum(my_members) > self.min_num_det_x_cluster:
marker = Marker()
marker.type = Marker.SPHERE
marker.pose.position.x = self.cluster_centers_sorted[k, 0]
marker.pose.position.y = self.cluster_centers_sorted[k, 1]
marker.pose.position.z = self.cluster_centers_sorted[k, 2]
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 0.0
marker.header.frame_id = '/world'
marker.header.stamp = rospy.Time.now()
marker.scale.x = 0.1 + k * 0.05
marker.scale.y = 0.1 + k * 0.05
marker.scale.z = 0.1 + k * 0.05
marker.color.r = 1.0
marker.color.g = 0.0 + k * 0.07
marker.color.b = 0.0
marker.color.a = 1.0
marker.id = k
self.markerArray.markers.append(marker)
# Gather all points of clusters from current to last detected to compute the chain main axis line
if k >= self.iter_wps:
points_cluster_centers.append([
self.cluster_centers_sorted[k, 0],
self.cluster_centers_sorted[k, 1],
self.cluster_centers_sorted[k, 2]
])
print 'Waypoints per calcular la ratlla:', k
# Fit line to cluster points
vx, vy, vz, cx, cy, cz = cv2.fitLine(
np.array(np.float32(points_cluster_centers)), cv2.cv.CV_DIST_HUBER,
0, 0.01, 0.01)
# Publish a line marker to visualize main chain axis
marker = Marker()
marker.type = Marker.LINE_STRIP
marker.action = Marker.ADD
marker.header.frame_id = '/world'
marker.header.stamp = rospy.Time.now()
marker.scale.x = 0.1
marker.color.b = 1.0
marker.color.a = 1.0
marker.id = 10000
p1 = Point()
p1.x = cx
p1.y = cy
p1.z = cz
p2 = Point()
p2.x = vx * 4 + cx
p2.y = vy * 4 + cy
p2.z = vz * 4 + cz
marker.points.append(p1)
marker.points.append(p2)
self.markerArray.markers.append(marker)
# Update orientation of main chain axis line
self.orientation_line = math.atan2((p2.y - p1.y), (p2.x - p1.x))
if self.direction:
self.orientation_line = cola2_lib.normalizeAngle(
self.orientation_line + math.pi)
print 'Line orientation: ', math.degrees(self.orientation_line)
self.pub_chain_orientation.publish(Float32(self.orientation_line))
#my_members = labels == -1
#pl.plot(X[my_members, 0], X[my_members, 1], 'k' + '.')
#pl.title('Estimated number of clusters: %d' % n_clusters_)
#pl.draw(i)
self.lock.release()
def generateNewPose(self, current_pose, current_vel):
"""
Generates the new position in the current state
"""
t = -math.log(self.s) / self.alpha
#self.tf
# if self.backward :
# t = self.tf + math.log(self.s)/self.alpha
# else :
# t = -math.log(self.s)/self.alpha
# for each atractor or state obtain the weigh
#rospy.loginfo('Time :' + str(t) )
h = np.zeros(self.states)
for i in xrange(self.states):
h[i] = self.gaussPDF(t, self.Mu_t[i], self.Sigma_t[i])
# normalize the value
if t > self.Mu_t[self.states - 1] + (self.Sigma_t[self.states - 1] *
1.2):
print 'The time used in the demonstration is exhausted'
#self.enabled = False
self.s = 1.0
return [[], []]
else:
h = h / np.sum(h)
#init to vectors
currTar = np.zeros(self.dof)
currWp = np.zeros(shape=(self.dof, self.dof))
#For each actuator, State, Acumulate the position using weigh
#CurrTar = The center of the GMM * weight of the state
#CurrWp = Sigma of the GMM * weight of the State
for i in xrange(self.dof):
currTar = currTar + self.Mu_x[:, i] * h[i]
currWp = currWp + self.Wp[i, :, :] * h[i]
#rospy.loginfo('Current Tar '+ str(currTar))
#rospy.loginfo('Current Wp '+ str(currWp))
#Compute acceleration
#currAcc = currWp * (currTar-currPos) - ( m.kV * currVel);
#Current pose has to bee a np array
#Work Around to aboid the diference size of the algorithm
#print 'Size pose ' + str(len(current_pose)) + ' Size dof ' + str(self.dof)
if len(current_pose) != self.dof:
selected_pose = current_pose[self.dofs == 1]
selected_vel = current_vel[self.dofs == 1]
else:
selected_pose = current_pose
selected_vel = current_vel
diff = currTar - selected_pose
diff[3] = cola2_lib.normalizeAngle(diff[3])
#rospy.loginfo('Kv ' + str(self.kV.tolist()))
desAcc = np.dot(currWp, diff) - np.dot(self.kV, selected_vel)
# action is a scalar value to evaluate the safety
#rospy.loginfo('Des Acc' + str(self.desAcc))
desVel = selected_vel + desAcc * self.interval_time
#NOT needed
desPos = selected_pose + desVel * self.interval_time
self.s = self.s - self.alpha * self.s * self.interval_time * self.action #*1.5
return [desPos, desVel]
def map_ack_data_callback(self, data):
""" This is the main callback. Data is recieved, processed and sent
to pose and velocity controllers """
# Compute desired positions and velocities
desired = [0 for x in range(12)]
for i in range(6):
if (data.axes[i] < 0):
desired[i] = abs(
data.axes[i]) * self.min_pos[i] + self.base_pose[i]
else:
desired[i] = data.axes[i] * self.max_pos[i] + self.base_pose[i]
if i > 2:
# Normalize angles
desired[i] = cola2_lib.normalizeAngle(desired[i])
for i in range(6, 12):
if (data.axes[i] < 0):
desired[i] = abs(data.axes[i]) * self.min_vel[i - 6]
else:
desired[i] = data.axes[i] * self.max_vel[i - 6]
# Check if pose controller is enabled
for b in range(6):
if data.buttons[b] == 1:
self.pose_controlled_axis[b] = True
if self.actualize_base_pose:
self.base_pose[b] = self.last_pose[b]
rospy.loginfo("%s: axis %s now is pose", self.name, str(b))
# Check if velocity controller is enabled
for b in range(6, 12):
if data.buttons[b] == 1:
self.pose_controlled_axis[b - 6] = False
rospy.loginfo("%s: axis %s now is velocity", self.name,
str(b - 6))
if self.nav_init:
# Positions
world_waypoint_req = WorldWaypointReq()
world_waypoint_req.goal.priority = GoalDescriptor.PRIORITY_TELEOPERATION
world_waypoint_req.goal.requester = self.name + '_pose'
world_waypoint_req.position.north = desired[0]
world_waypoint_req.position.east = desired[1]
world_waypoint_req.position.depth = desired[2]
world_waypoint_req.orientation.roll = desired[3]
world_waypoint_req.orientation.pitch = desired[4]
world_waypoint_req.orientation.yaw = desired[5]
world_waypoint_req.disable_axis.x = not self.pose_controlled_axis[0]
world_waypoint_req.disable_axis.y = not self.pose_controlled_axis[1]
world_waypoint_req.disable_axis.z = not self.pose_controlled_axis[2]
world_waypoint_req.disable_axis.roll = not self.pose_controlled_axis[
3]
world_waypoint_req.disable_axis.pitch = not self.pose_controlled_axis[
4]
world_waypoint_req.disable_axis.yaw = not self.pose_controlled_axis[
5]
world_waypoint_req.header.stamp = rospy.Time().now()
# if not world_waypoint_req.disable_axis.pitch:
# rospy.logfatal("%s: PITCH IS NOT DISABLED!", self.name)
# world_waypoint_req.disable_axis.pitch = True
if (world_waypoint_req.disable_axis.x
and world_waypoint_req.disable_axis.y
and world_waypoint_req.disable_axis.z
and world_waypoint_req.disable_axis.roll
and world_waypoint_req.disable_axis.pitch
and world_waypoint_req.disable_axis.yaw):
world_waypoint_req.goal.priority = GoalDescriptor.PRIORITY_TELEOPERATION_LOW
self.pub_world_waypoint_req.publish(world_waypoint_req)
# Velocities
body_velocity_req = BodyVelocityReq()
body_velocity_req.goal.priority = GoalDescriptor.PRIORITY_TELEOPERATION
body_velocity_req.goal.requester = self.name + '_vel'
body_velocity_req.twist.linear.x = desired[6]
body_velocity_req.twist.linear.y = desired[7]
body_velocity_req.twist.linear.z = desired[8]
body_velocity_req.twist.angular.x = desired[9]
body_velocity_req.twist.angular.y = desired[10]
body_velocity_req.twist.angular.z = desired[11]
body_velocity_req.disable_axis.x = self.pose_controlled_axis[0]
body_velocity_req.disable_axis.y = self.pose_controlled_axis[1]
body_velocity_req.disable_axis.z = self.pose_controlled_axis[2]
body_velocity_req.disable_axis.roll = self.pose_controlled_axis[3]
body_velocity_req.disable_axis.pitch = self.pose_controlled_axis[4]
body_velocity_req.disable_axis.yaw = self.pose_controlled_axis[5]
# Check if DoF is disable
if abs(body_velocity_req.twist.linear.x) < 0.025:
body_velocity_req.disable_axis.x = True
if abs(body_velocity_req.twist.linear.y) < 0.025:
body_velocity_req.disable_axis.y = True
if abs(body_velocity_req.twist.linear.z) < 0.025:
body_velocity_req.disable_axis.z = True
if abs(body_velocity_req.twist.angular.x) < 0.01:
body_velocity_req.disable_axis.roll = True
if abs(body_velocity_req.twist.angular.y) < 0.01:
body_velocity_req.disable_axis.pitch = True
if abs(body_velocity_req.twist.angular.z) < 0.01:
body_velocity_req.disable_axis.yaw = True
# If all DoF are disabled set priority to LOW
if (body_velocity_req.disable_axis.x
and body_velocity_req.disable_axis.y
and body_velocity_req.disable_axis.z
and body_velocity_req.disable_axis.roll
and body_velocity_req.disable_axis.pitch
and body_velocity_req.disable_axis.yaw):
body_velocity_req.goal.priority = GoalDescriptor.PRIORITY_TELEOPERATION_LOW
# Publish message
body_velocity_req.header.stamp = rospy.Time().now()
self.pub_body_velocity_req.publish(body_velocity_req)