def set_pose(self, event):
""" Send zero velocity requests if the vehicle is below the
desired depth """
if self.navigation.position.depth > self.set_pose_depth:
wwr = WorldWaypointReq()
wwr.position.north = self.desired_pose[0]
wwr.position.east = self.desired_pose[1]
wwr.position.depth = self.desired_pose[2]
wwr.orientation.roll = self.desired_pose[3]
wwr.orientation.pitch = self.desired_pose[4]
wwr.orientation.yaw = self.desired_pose[5]
wwr.altitude_mode = False
wwr.goal.priority = GoalDescriptor.PRIORITY_SAFETY_LOW
wwr.header.stamp = rospy.Time.now()
for i in range(len(self.set_pose_axis)):
wwr.disable_axis.x = self.set_pose_axis[i][0]
wwr.disable_axis.y = self.set_pose_axis[i][1]
wwr.disable_axis.z = self.set_pose_axis[i][2]
wwr.disable_axis.roll = self.set_pose_axis[i][3]
wwr.disable_axis.pitch = self.set_pose_axis[i][4]
wwr.disable_axis.yaw = self.set_pose_axis[i][5]
# Set Zero Velocity
wwr.goal.requester = 'set_pose_' + str(i)
self.pub_world_waypoint_req.publish(wwr)
def test(self):
message = WorldWaypointReq()
message.header.stamp = rospy.get_rostime()
message.goal.requester = 'arnau'
message.goal.id = 1
message.goal.priority = 30
message.altitude_mode = False
message.position.north = 0.4
message.position.east = -0.19
message.position.depth = -0.27
message.altitude = 0.0
message.orientation.roll = 0.0
message.orientation.pitch = 0.0
message.orientation.yaw = 0.887
message.disable_axis.x = False
message.disable_axis.y = False
message.disable_axis.z = False
message.disable_axis.roll = True
message.disable_axis.pitch = True
message.disable_axis.yaw = False
message.position_tolerance.x= 0.5
message.position_tolerance.y= 0.5
message.position_tolerance.z= 0.5
message.orientation_tolerance.roll= 0.0
message.orientation_tolerance.pitch= 0.0
message.orientation_tolerance.yaw= 0.5
while not rospy.is_shutdown():
print 'publish'
message.header.stamp = rospy.get_rostime()
self.pub_auv.publish(message)
print 'Sleeping'
rospy.sleep(0.1)
def stare_chain_thread(self, index):
index = int(index)
rate = rospy.Rate(10)
while self.stare_landmark_init:
# Compute Transformation
angle = euler_from_quaternion([
self.map.landmark[index].pose.pose.orientation.x,
self.map.landmark[index].pose.pose.orientation.y,
self.map.landmark[index].pose.pose.orientation.z,
self.map.landmark[index].pose.pose.orientation.w
])
O = PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
t = PyKDL.Vector(self.map.landmark[index].pose.pose.position.x,
self.map.landmark[index].pose.pose.position.y,
self.map.landmark[index].pose.pose.position.z)
wTl = PyKDL.Frame(O, t)
wTo = wTl * self.offset_transformation
orientation = wTo.M.GetRPY()
position = wTo.p
wwr = WorldWaypointReq()
wwr.header.stamp = rospy.Time().now()
wwr.goal = GoalDescriptor(self.name, 1,
GoalDescriptor.PRIORITY_NORMAL)
wwr.altitude_mode = False
wwr.position.north = position[0]
wwr.position.east = position[1]
wwr.position.depth = position[2]
wwr.altitude = 5.0
wwr.orientation.roll = orientation[0]
wwr.orientation.pitch = orientation[1]
wwr.orientation.yaw = orientation[2]
wwr.disable_axis.x = False
wwr.disable_axis.y = False
wwr.disable_axis.z = True
wwr.disable_axis.roll = True
wwr.disable_axis.pitch = True
wwr.disable_axis.yaw = False
self.pub_world_waypoint_req.publish(wwr)
print 'publish:\n', wwr
if not self.keep_pose:
self.check_tolerance(wwr)
rate.sleep()
def iterate(self, wp_list):
wp = 0
found = False
while wp < len(wp_list) and not found:
print 'Go to waypoint ', wp
# Create msg for WP 'wp'
waypoint_req = WorldWaypointReq()
waypoint_req.goal.priority = GoalDescriptor.PRIORITY_NORMAL
waypoint_req.goal.requester = self.name + '_pose'
waypoint_req.disable_axis.x = False
waypoint_req.disable_axis.y = False
waypoint_req.disable_axis.z = False
waypoint_req.disable_axis.roll = True
waypoint_req.disable_axis.pitch = True
waypoint_req.disable_axis.yaw = False
waypoint_req.altitude_mode = False
waypoint_req.position.north = wp_list[wp][0]
waypoint_req.position.east = wp_list[wp][1]
waypoint_req.position.depth = wp_list[wp][2]
waypoint_req.orientation.yaw = wp_list[wp][3]
r = rospy.Rate(10)
while not check_tolerance(wp_list[wp], 1.0) and not found:
# Send waypoint req
waypoint_req.header.stamp = rospy.Time.now()
self.pub_wwr.publish(waypoint_req)
# Check if links detected
if self.link_detected():
if self.total_link_wp > 2:
found = True
else:
rospy.sleep(3.0)
r.sleep()
wp = wp + 1
if found:
print 'Chain Found!!!'
else:
print 'Chain not found!!! Waypoint list empty.'
def updateNavSts(self, nav_sts):
if self.is_enabled:
x = nav_sts.position.north
y = nav_sts.position.east
z = nav_sts.position.depth
if self.cluster_centers_sorted != None:
# set next point to go
x_des = self.cluster_centers_sorted[self.iter_wps, 0]
y_des = self.cluster_centers_sorted[self.iter_wps, 1]
z_des = self.cluster_centers_sorted[self.iter_wps, 2]
ex = x - x_des
ey = y - y_des
ez = z - z_des
# Criteria for reaching WP
error = np.sqrt(ex**2 + ey**2 + 0.0 * ez**2)
print "Error:", error
if error < self.error_threshold and self.iter_wps < len(
self.cluster_centers_sorted) - 1:
self.iter_wps = self.iter_wps + 1
marker = Marker()
marker.type = Marker.SPHERE
marker.pose.position.x = self.cluster_centers_sorted[
self.iter_wps, 0]
marker.pose.position.y = self.cluster_centers_sorted[
self.iter_wps, 1]
marker.pose.position.z = self.cluster_centers_sorted[
self.iter_wps, 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.15
marker.scale.y = 0.15
marker.scale.z = 0.15
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.id = self.iter_wps
# marker.lifetime = rospy.Duration(5.0)
self.pub_sonar_next_wp.publish(marker)
data = WorldWaypointReq()
data.header.stamp = rospy.Time.now()
data.header.frame_id = "/world"
data.goal.priority = 0
data.goal.id = self.iter_wps
data.altitude_mode = False
data.position.north = self.cluster_centers_sorted[
self.iter_wps, 0]
data.position.east = self.cluster_centers_sorted[self.iter_wps,
1]
data.position.depth = self.cluster_centers_sorted[
self.iter_wps, 2]
data.altitude = 0.0
data.orientation.roll = 0.0
data.orientation.pitch = 0.0
data.orientation.yaw = 0.0
data.disable_axis.x = False
data.disable_axis.y = False
data.disable_axis.z = False
data.disable_axis.roll = True
data.disable_axis.pitch = True
data.disable_axis.yaw = False
data.position_tolerance.x = 0.0
data.position_tolerance.y = 0.0
data.position_tolerance.z = 0.0
data.orientation_tolerance.roll = 0.0
data.orientation_tolerance.pitch = 0.0
data.orientation_tolerance.yaw = 0.0
self.pub_wwr.publish(data)