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 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)