def calculate_robot_position(landmarks, robot_location_prev, landmarks_prev):
#tuple of two yeti_snowplow.obstacle, geometries.Pose2D, tuple of two yeti_snowplow.obstacle
heading0 = -(landmarks[0].heading - landmarks_prev[0].heading) + robot_location_prev.theta
heading1 = -(landmarks[1].heading - landmarks_prev[1].heading) + robot_location_prev.theta
heading = (heading0 + heading1)/2
xa = landmarks[0].x
ya = landmarks[0].y
da = landmarks[0].distance
xb = landmarks[1].x
yb = landmarks[1].y
db = landmarks[1].distance
o = (2*yb - 2*ya)
p = (da*da - db*db - xa*xa + xb*xb - ya*ya + yb*yb)/o
q = -(2*xb - 2*xa)/o
s = p-ya
t = q*q + 1
u = 2*s*q - 2*xa
v = xa*xa + s*s - da*da
answer_one = geometries.Pose2D()
answer_one.x = (-u + math.sqrt(u*u - 4*t*v))/(2*t)
answer_one.y = q*answer_one.x + p
answer_one.theta = heading
answer_two = geometries.Pose2D()
answer_two.x = (-u - math.sqrt(u*u - 4*t*v))/(2*t)
answer_two.y = q*answer_two.x + p
answer_two.theta = heading
if(distance_pose2D(answer_one,robot_location_prev) < distance_pose2D(answer_two,robot_location_prev)):
return answer_one
else:
return answer_two
def __work():
global __pose_lock, __trade_lock
global __position, __versor, __landmarks
global __NAME, __OTHERS_NAMES, __possible_trade_partners
global __token
global __cmd_vel_pub, __cov_pub
global __initial_time
__pose_lock.acquire()
__trade_lock.acquire()
dot_p = cov.coverage_gradient_pos(__position, __versor, __landmarks)
dot_theta = cov.coverage_gradient_ver(__position, __versor, __landmarks)
coverage = cov.coverage_function(__position, __versor, __landmarks)
pose = gms.Pose2D(__position[0], __position[1],
cov.angle_from_versor(__versor))
__trade_lock.release()
__pose_lock.release()
if np.linalg.norm(dot_p) > 10.3:
dot_p *= 10.3 / np.linalg.norm(dot_p)
dot_theta = np.clip(dot_theta, -10.0, 10.0)
cmd_vel = gms.Pose2D(dot_p[0], dot_p[1], dot_theta)
__cmd_vel_pub.publish(cmd_vel)
__cov_pub.publish(coverage)
__trade_lock.acquire()
if np.linalg.norm(dot_p) < __TOLERANCE and np.linalg.norm(
dot_theta) < __TOLERANCE and __token:
if len(__possible_trade_partners) > 0:
lmks = [cov.landmark_from_pos(lmk) for lmk in __landmarks]
name = rdm.choice(__possible_trade_partners)
rsp = __trade_proxies[name](__NAME, pose, lmks)
if rsp.success:
__possible_trade_partners = list(__OTHERS_NAMES)
__landmarks = [
__landmarks[index]
for index in filter(lambda z: not z in rsp.to_remove,
range(len(__landmarks)))
]
__landmarks += [
cov.pos_from_landmark(lmk) for lmk in rsp.to_add
]
else:
__possible_trade_partners.remove(name)
token_accepted = False
possible_token_receivers = list(__OTHERS_NAMES)
while not token_accepted and len(possible_token_receivers) > 0:
rp.logwarn(__NAME + ': possible token receivers: ' +
str(possible_token_receivers))
name = rdm.choice(possible_token_receivers)
possible_token_receivers.remove(name)
rsp = __token_proxies[name]()
token_accepted = rsp.accepted
if token_accepted:
__token = False
elif len(__possible_trade_partners) == 0:
final_time = rp.get_time()
rp.logwarn('Finish! Elapsed time: ' +
str(final_time - __initial_time))
__token = False
__trade_lock.release()
def update_map(data):
global numScans
global map_ready
global obstacles
global obstacles_prev
points = filter_laser_scan(data, 0, 0)
scan_filtered = sensors.PointCloud()
for i in range(0, len(points)):
new_point = geometries.Point32()
new_point.x = points[i][0]
new_point.y = points[i][1]
scan_filtered.points.append(new_point)
scan_filtered_pub.publish(scan_filtered)
if not map_ready:
if numScans > 50:
map_ready = True
print "Map ready!"
else:
print "Number of scans: " + str(numScans)
obstacles = distance_cluster(points)
obstacle1 = geometries.Pose2D()
obstacle1.theta = 0
obstacle1.x = obstacles[0][0]
obstacle1.y = obstacles[0][1]
obstacle1_pub.publish(obstacle1)
obstacle2 = geometries.Pose2D()
obstacle2.theta = 0
obstacle2.x = obstacles[1][0]
obstacle2.y = obstacles[1][1]
obstacle2_pub.publish(obstacle2)
if not close_enough(obstacles_prev[0],
obstacles[0]) and not close_enough(
obstacles_prev[1], obstacles[1]):
numScans = 0
obstacles_prev = obstacles
if map_ready:
#print "localization using map..."
obstacle1 = geometries.Pose2D()
obstacle1.theta = 0
obstacle1.x = obstacles[0][0]
obstacle1.y = obstacles[0][1]
obstacle1_pub.publish(obstacle1)
obstacle2 = geometries.Pose2D()
obstacle2.theta = 0
obstacle2.x = obstacles[1][0]
obstacle2.y = obstacles[1][1]
obstacle2_pub.publish(obstacle2)
numScans = numScans + 1
def send_goal(self, model, nposes=1, poses2d=[]):
goal = lmsg.LocalizeGoal()
goal.object_name = model
goal.in_plane = model in self._in_plane
goal.nposes = nposes
goal.poses2d = poses2d
if goal.in_plane and not goal.poses2d:
goal.poses2d = [gmsg.Pose2D(x=0.0, y=0.0, theta=0.0)]
goal.sideways = False
goal.x_offset = 0.0
goal_z_offset = 0.0
if model in self._in_plane_settings:
in_plane_settings = self._in_plane_settings[model]
if 'sideways' in in_plane_settings:
goal.sideways = in_plane_settings['sideways']
if 'x_offset' in in_plane_settings:
goal.x_offset = in_plane_settings['x_offset']
if 'z_offset' in in_plane_settings:
goal.z_offset = in_plane_settings['z_offset']
self.set_settings(LocalizationClient._DefaultSettings)
if model in self._full_settings:
self.set_settings(self._full_settings[model])
self._poses = []
self._overlaps = []
self._localize.send_goal(goal, feedback_cb=self._feedback_cb)
def __pose_2d_from_pose_stamped(ps):
quat = ps.pose.orientation
quat_array = [quat.w, quat.x, quat.y, quat.z]
euler = t3de.quat2euler(quat_array)
pose_2d = gms.Pose2D(x=ps.pose.position.x,
y=ps.pose.position.y,
theta=euler[2])
return pose_2d
def to_pose2d(pose):
if isinstance(pose, geometry_msgs.PoseStamped):
p = pose.pose
elif isinstance(pose, geometry_msgs.Pose):
p = pose
else:
raise rospy.ROSException(
"Input parameter pose is not a valid geometry_msgs pose object")
return geometry_msgs.Pose2D(p.position.x, p.position.y, yaw(p))
def __work():
global __cmd_vel_lock
global __cmd_vel_pos, __cmd_vel_ang
global __pos, __ang
global __pub
__cmd_vel_lock.acquire()
__pos += __cmd_vel_pos * 1e-2
__ang += __cmd_vel_ang * 1e-2
__ang = np.arctan2(np.sin(__ang), np.cos(__ang))
pose_2d = gms.Pose2D(__pos[0], __pos[1], __ang)
pose_stamped = gms.PoseStamped(
pose=gms.Pose(position=gms.Point(x=__pos[0], y=__pos[1]),
orientation=gms.Quaternion(*quaternion_from_yaw(__ang))))
__cmd_vel_lock.release()
__pose_2d_pub.publish(pose_2d)
__pose_stamped_pub.publish(pose_stamped)
def __init__(self,
controller = fattcs.database['Default'],
yaw_controller = ycs.database['Default'],
#simulator = sms.simulators_dictionary['Default']
other_quads_names = ['Iris2', 'Iris3', 'Iris4'],
landmarks = [],
):
ms.Mission.__init__(self)
self.__controller = controller
self.__yaw_controller = yaw_controller
self.__converter = ipc.IrisPlusConverter()
self.__cmd_pub = rospy.Publisher('quad_cmd', qms.quad_cmd, queue_size=10)
self.__other_quads_names = other_quads_names
self.__landmarks = landmarks
self.__waypoint = gms.Pose2D(0.0, 0.0, 0.0)
self.__sim_state = qms.quad_state()
self.__sim_sub = rospy.Subscriber('quad_state', qms.quad_state, self.__simulator_callback)
def listener():
rospy.init_node('robot_position_test', anonymous=True)
# rospy.Subscriber("landmarks", yeti_snowplow.obstacle, calculate_robot_position)
# rospy.spin()
landmark1 = yeti_snowplow.obstacle()
landmark1.x = 3
landmark1.y = 4
landmark1.distance = 5
landmark1.heading = -31*0.017453293
landmark2 = yeti_snowplow.obstacle()
landmark2.x = 5
landmark2.y = 6
landmark2.distance = 7
landmark2.heading = -16*0.017453293
robot_location_prev = geometries.Pose2D()
landmark1_prev = yeti_snowplow.obstacle()
landmark1_prev.heading = -30*0.017453293
landmark2_prev = yeti_snowplow.obstacle()
landmark2_prev.heading = -15*0.017453293
robot_position_pub.publish(calculate_robot_position((landmark1,landmark2),robot_location_prev,(landmark1_prev,landmark2_prev)))
def send_goal(self, object_name, plane, poses2d):
params = self._params[object_name]
origin = copy.copy(params['origin']) # Must be copied
origin[3] = np.radians(origin[3])
origin[4] = np.radians(origin[4])
origin[5] = np.radians(origin[5])
goal = lmsg.LocalizeGoal()
rotation_range = params['rotation_range']
if rotation_range[0] < rotation_range[1] and \
rotation_range[2] > 0:
goal.poses2d = [ gmsg.Pose2D(poses2d[0].x,
poses2d[0].y,
np.radians(theta) + poses2d[0].theta)
for theta in np.arange(*rotation_range) ]
else:
goal.poses2d = poses2d
goal.object_name = object_name
goal.plane = plane
goal.origin = gmsg.Pose(gmsg.Point(*origin[0:3]),
gmsg.Quaternion(*tfs.quaternion_from_euler(
*origin[3:6])))
goal.refine_transform = params['refine_transform']
goal.check_borders = params['check_borders']
self._localize.send_goal(goal)
scan_filtered_pub = rospy.Publisher('scan_filtered',
sensors.PointCloud,
queue_size=10)
obstacle1_pub = rospy.Publisher('obstacle1', geometries.Pose2D, queue_size=10)
obstacle2_pub = rospy.Publisher('obstacle2', geometries.Pose2D, queue_size=10)
now = time.clock()
global numScans
numScans = 0
global obstacles
map_ready = False
obstacles = []
obstacles_prev = [[0, 0], [0, 0]]
robot_location_prev = geometries.Pose2D()
def distance_pose2D(one, two): #geometries.Pose2D, geometries.Pose2D
return distance(one.x, one.y, two.x, two.y)
def distance(x1, y1, x2, y2):
x = (x1 - x2)
y = (y1 - y2)
return math.sqrt(x * x + y * y)
def polar_to_cartesian(theta, rot):
x = math.cos(theta) * rot
y = math.sin(theta) * rot
import utilities.coverage as cov
import geometry_msgs.msg as gms
import quad_control.srv as qsv
import rospy as rp
import scipy.optimize as opt
import numpy as np
import threading as thd
__NAME = 'David'
__OTHERS_NAMES = ['Axel', 'Bo', 'Calle']
__XBB = [-5.0, 5.0]
__YBB = [-5.0, 5.0]
__TBB = [-np.pi, np.pi]
__TOLERANCE = 0.1
__waypoint = gms.Pose2D()
__landmarks = []
__pose = gms.Pose2D()
__possible_partners = list(__OTHERS_NAMES)
__pose_lock = thd.Lock()
__trade_lock = thd.Lock()
__mode = 'navigating'
def __get_pose_callback(msg):
global __pose
__pose_lock.acquire()
__pose = msg
__pose_lock.release()
def pose2D_from_pos_ver(pos, ver):
angle = angle_from_versor(ver)
return gms.Pose2D(pos[0], pos[1], angle)
def get_pose_2d(self):
return gms.Pose2D(self.__x, self.__y, self.__theta)
roslib.load_manifest('trigger_msgs')
import sys
import rospy
import dynamic_reconfigure.client
import time
import trigger_msgs.msg
import geometry_msgs.msg as gm
import time
import sys
dist = float(sys.argv[1])
print dist
rospy.init_node("move_back", anonymous=True)
move_base = rospy.Publisher('simple_move_base', gm.Pose2D)
r = rospy.Rate(10)
p2d = gm.Pose2D()
p2d.x = dist
p2d.y = .15
print 'move_back -.6'
r.sleep()
move_base.publish(p2d)
r.sleep()
r.sleep()
r.sleep()
r.sleep()
r.sleep()
#import threading as tr
#class ProjectorWakeUp(threading.Thread):
#
def numpy_to_pose2D(pose):
return geometry_msgs.Pose2D(*pose)
import std_msgs.msg as sms
import quad_control.msg as qms
import quad_control.srv as qsv
import threading as thd
import numpy as np
import rospy as rp
import transforms3d.euler as t3de
import trajectory_msgs.msg as tms
rp.init_node('coverage_router')
__NAME = rp.get_param('name')
__HEIGHT = 1.3
__lock = thd.Lock()
__coverage_cmd_vel = gms.Pose2D()
ip = cov.INITIAL_POSES[__NAME]
__pose_2d = gms.Pose2D(x=ip[0], y=ip[1], theta=ip[2])
def __twist_from_vel_2d(vel):
msg = gms.Twist()
msg.linear = gms.Vector3(x=vel.x, y=vel.y, z=0.0)
msg.angular = gms.Vector3(x=0.0, y=0.0, z=vel.theta)
return msg
def __multi_dof_joint_trajectory_from_vel_2d_pose_2d(vel_2d, pose_2d):
msg = tms.MultiDOFJointTrajectory()
pose = __transform_from_pose_2d(pose_2d)
def __work():
global __lock
global __agent
global __NAME, __OTHERS_NAMES, __possible_trade_partners
global __token
global __cmd_vel_pub, __cov_pub, __agent_pub
global __initial_time
__lock.acquire()
dot_p = __GAIN * __agent.position_coverage_gradient()
dot_theta = __GAIN * __agent.orientation_coverage_gradient()
pos = np.array(__agent.get_pose())[0:2]
#if __BOUNDARY_FUNCTION(pos)<0.0 and __BOUNDARY_FUNCTION_GRADIENT(pos).dot(dot_p)<0.0:
#dot_p = np.zeros(2)
#dot_theta = 0.0
#rp.logwarn(__NAME + ': Emergency! Out of boundary!: ' + str(pos))
#dot_p = cov.remove_parallel_component(dot_p, __BOUNDARY_FUNCTION_GRADIENT(pos))
#dot_p += __BOUNDARY_FUNCTION_GRADIENT(pos)/np.linalg.norm(__BOUNDARY_FUNCTION_GRADIENT(pos))*np.linalg.norm(dot_p)
#dot_p = cov.remove_parallel_component(dot_p, __BOUNDARY_FUNCTION_GRADIENT(pos))
#dot_p = __GAIN*__BOUNDARY_FUNCTION_GRADIENT(pos)
#elif __BOUNDARY_FUNCTION(pos)<=__SAFETY and __BOUNDARY_FUNCTION_GRADIENT(pos).dot(dot_p)<0.0:
#dot_p -= (__SAFETY-__BOUNDARY_FUNCTION(pos))*cov.projection(dot_p, __BOUNDARY_FUNCTION_GRADIENT(pos))
#dot_p = __GAIN*(dot_p*__BOUNDARY_FUNCTION(pos) + (__SAFETY-__BOUNDARY_FUNCTION(pos))*__BOUNDARY_FUNCTION_GRADIENT(pos))
#dot_p -= (__SAFETY-__BOUNDARY_FUNCTION(pos))/__SAFETY*cov.projection(dot_p, __BOUNDARY_FUNCTION_GRADIENT(pos))
#dot_p += __BOUNDARY_FUNCTION_GRADIENT(pos)/np.linalg.norm(__BOUNDARY_FUNCTION_GRADIENT(pos))*np.linalg.norm(dot_p)
vel = np.array([dot_p[0], dot_p[1], dot_theta])
if np.linalg.norm(vel) > __MAX_SPEED:
vel = vel / np.linalg.norm(vel) * __MAX_SPEED
#if np.linalg.norm(vel) < __TOLERANCE:
#vel = np.zeros(3)
__cov_pub.publish(__agent.coverage())
cmd_vel = gms.Pose2D(*vel)
__cmd_vel_pub.publish(cmd_vel)
__num_landmarks_pub.publish(len(__agent.get_landmarks()))
if np.linalg.norm(vel) < __TOLERANCE and __token:
if len(__possible_trade_partners) > 0:
partner = rdm.choice(__possible_trade_partners)
pose = __agent.get_pose_2d()
landmark_array = __agent.get_landmark_array()
assert len(landmark_array.data) == len(__agent.get_landmarks())
response = __trade_proxies[partner](__NAME, pose, landmark_array)
if response.success:
__possible_trade_partners = list(__OTHERS_NAMES)
indexes_to_remove = response.indexes_to_remove
landmarks_to_add = cov.landmarks_from_point_2d_array(
response.landmarks_to_add)
__agent.update_landmarks(indexes_to_remove, landmarks_to_add)
__landmarks_pub.publish(__agent.get_landmark_array())
#new_landmarks = [cov.Landmark.from_pose2d(lmk) for lmk in response.landmarks.data]
#__agent.set_landmarks(new_landmarks)
#assert len(__agent.get_landmarks()) == len(new_landmarks)
#array = __agent.get_landmark_array()
#assert len(array.data) == len(new_landmarks)
#__landmarks_pub.publish(array)
else:
__possible_trade_partners.remove(partner)
# new_landmarks = [cov.Landmark.from_pose2d(lmk) for lmk in response.landmarks.data]
# __agent.set_landmarks(new_landmarks)
token_accepted = False
possible_token_receivers = list(__OTHERS_NAMES)
while not token_accepted and len(possible_token_receivers) > 0:
rp.logwarn(__NAME + ': possible token receivers: ' +
str(possible_token_receivers))
name = rdm.choice(possible_token_receivers)
possible_token_receivers.remove(name)
rsp = __token_proxies[name]()
token_accepted = rsp.accepted
if token_accepted:
__token = False
elif len(__possible_trade_partners) == 0:
final_time = rp.get_time()
rp.logwarn(__NAME + ': Finish! Elapsed time: ' +
str(final_time - __initial_time))
#__coverage_done_pub.publish(sms.Empty())
#__token = False
elif np.linalg.norm(vel) > 2.0 * __TOLERANCE:
if len(__possible_trade_partners) < len(__OTHERS_NAMES):
rp.logwarn(__NAME + ': reset possible trade partners')
__possible_trade_partners = list(__OTHERS_NAMES)
__lock.release()
