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 check_map_ack(self, event):
""" This is a callback for a timer. It publishes ack safety message
and pose and velocity safety messages if map_ack is lost """
if self.map_ack_init:
self.diagnostic.add(
"last_ack", str(rospy.Time.now().to_sec() - self.last_map_ack))
if self.map_ack_alive:
self.map_ack_alive = False
self.diagnostic.setLevel(DiagnosticStatus.OK)
else:
rospy.loginfo("%s: we have lost map_ack!", self.name)
self.diagnostic.setLevel(DiagnosticStatus.WARN,
'Communication with map_ack lost!')
body_velocity_req = BodyVelocityReq()
body_velocity_req.goal.priority = GoalDescriptor.PRIORITY_TELEOPERATION_LOW
body_velocity_req.goal.requester = self.name + '_vel'
body_velocity_req.twist.linear.x = 0.0
body_velocity_req.twist.linear.y = 0.0
body_velocity_req.twist.linear.z = 0.0
body_velocity_req.twist.angular.x = 0.0
body_velocity_req.twist.angular.y = 0.0
body_velocity_req.twist.angular.z = 0.0
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 = True
body_velocity_req.header.stamp = rospy.Time().now()
self.pub_body_velocity_req.publish(body_velocity_req)
world_waypoint_req = WorldWaypointReq()
world_waypoint_req.goal.priority = GoalDescriptor.PRIORITY_TELEOPERATION_LOW
world_waypoint_req.goal.requester = self.name + '_pose'
world_waypoint_req.disable_axis.x = True
world_waypoint_req.disable_axis.y = True
world_waypoint_req.disable_axis.z = True
world_waypoint_req.disable_axis.roll = True
world_waypoint_req.disable_axis.pitch = True
world_waypoint_req.disable_axis.yaw = True
world_waypoint_req.header.stamp = rospy.Time().now()
self.pub_world_waypoint_req.publish(world_waypoint_req)
else:
rospy.loginfo("%s: waiting for map ack...", self.name)
# Send ack message
msg = String()
msg.data = str(self.seq) + ' ok'
self.pub_check_joystick.publish(msg)
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 = 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 = 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 facing(self, angle, depth, max_angle_error):
"""Face specific orientation while reaching desired Z."""
ww_req = WorldWaypointReq()
ww_req.goal.requester = self.name + "_pose"
ww_req.goal.priority = GoalDescriptor.PRIORITY_NORMAL
ww_req.position.depth = depth
ww_req.orientation.yaw = cola2_lib.wrapAngle(angle)
ww_req.disable_axis.x = True
ww_req.disable_axis.y = True
ww_req.disable_axis.z = False
ww_req.disable_axis.roll = True
ww_req.disable_axis.pitch = True
ww_req.disable_axis.yaw = False
r = rospy.Rate(10)
while abs(self.vehicle_angle -
angle) > max_angle_error and not self.disable_trajectory:
ww_req.header.stamp = rospy.Time.now()
self.pub_world_waypoint_req.publish(ww_req)
r.sleep()
def __init__(self, name):
self.name = name
# Default parameters
self.window_length = 3.0
self.window_start = 1.0
self.waypoint_req = WorldWaypointReq()
self.body_velocity_req = BodyVelocityReq()
self.odometry = Odometry()
self.sonar_img_pose = PoseStamped()
self.lock = threading.RLock()
self.last_waypoint = WorldWaypointReq()
self.last_waypoint_update = rospy.Time.now().to_sec()
self.look_around = False
self.yaw_offset = 0.45 # 0.35 => 25 degrees
self.do_turn_around = False
self.lock = threading.RLock()
self.listener = tf.TransformListener()
self.broadcaster = tf.TransformBroadcaster()
self.odometry_updated = False
self.big_turn_around = False
# self.get_config()
self.pub_yaw_rate = rospy.Publisher('/cola2_control/body_velocity_req',
BodyVelocityReq)
self.pub_waypoint_req = rospy.Publisher(
'/cola2_control/world_waypoint_req', WorldWaypointReq)
self.pub_marker = rospy.Publisher('/udg_pandora/orientation_link',
Marker)
# Create Subscriber Updates (z)
rospy.Subscriber('/udg_pandora/link_waypoints',
MarkerArray,
self.sonar_waypoint_update,
queue_size=1)
rospy.Subscriber('/cola2_perception/soundmetrics_aris3000/sonar_info',
SonarInfo,
self.sonar_info_update,
queue_size=1)
rospy.Subscriber('/udg_pandora/world_waypoint_req',
WorldWaypointReq,
self.world_waypoint_req_update,
queue_size=1)
rospy.Subscriber('/pose_ekf_slam/odometry',
Odometry,
self.odometry_update,
queue_size=1)
rospy.Subscriber(
'/cola2_perception/soundmetrics_aris3000/sonar_img_pose',
PoseStamped,
self.sonar_img_pose_update,
queue_size=1)
rospy.Timer(rospy.Duration(0.05), self.publish_control)
rospy.Timer(rospy.Duration(0.5), self.update_sonar_img_tf)
wps = []
with open(
"/home/gordon/ros_workspace/auv_learning/behaviours/data/lawnmower.txt"
) as f:
wps_raw = f.read().splitlines()
wps = [point.split(",") for point in wps_raw]
# Wait for Nav
while not rospy.is_shutdown() and _stats == None:
rospy.sleep(0.5)
print "Waiting for nav"
#wps = [[0, 0], [5, 5]]
idx = 1
for wp in wps:
wp_msg = WorldWaypointReq()
print wp[0]
wp_msg.position.north = float(wp[0])
wp_msg.position.east = float(wp[1])
wp_msg.position.depth = 0
wp_msg.goal.id = idx
# Wait until the waypoint has been reached
if _stats != None and wp_msg != None:
while not (rospy.is_shutdown() and utils.epsilonEqualsNED(
_stats.position, wp_msg.position, 0.5, depth_e=0.6)):
check = utils.epsilonEqualsNED(_stats.position,
wp_msg.position,
0.5,
depth_e=0.6)
print(
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)
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)
