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