def get_pose(self): """Converts the current EKF state into a Pose object.""" pose = Pose() pose.header.frame_id = "world" pose.x, pose.y, pose.theta = self.state_tuple() pose.cov = tuple(self.covariance.flat) return pose
def odom_to_pose(odom): pose = Pose() pose.x = odom.pose.pose.position.x pose.y = odom.pose.pose.position.y, pose.theta = quaternion_to_angle(odom.pose.pose.orientation.z, odom.pose.pose.orientation.w) return pose
def __init__(self): rospy.init_node('obstacle_avoidance', log_level = rospy.DEBUG , anonymous = True) self.publishers = {} self.failedQueue = [] self.cachedObstacles = [] self.arrivedWaypoints = [] self.waypointQueue = [] rospy.wait_for_service('get_landmarks') self.publishers['debug'] = rospy.Publisher('debug', String) get_landmarks_srv = rospy.ServiceProxy('get_landmarks', GetLandmarks) landmarks = get_landmarks_srv().all_wps self.landmarkTypes = {} for landmark in landmarks: self.landmarkTypes[str(landmark.x) + ',' + str(landmark.y)] = landmark.ltype self.publishers['rawnav'] = rospy.Publisher('ch_rawnav', Goal) self.publishers['obstacle'] = rospy.Publisher('ch_obstacle', Goal) self.publishers['absGoal'] = rospy.Publisher('ch_absgoal', Goal) self.publishers['netForce'] = rospy.Publisher('ch_netforce', Goal) self.publishers['perpForce'] = rospy.Publisher('ch_perpforce', Goal) self.publishers['repForce'] = rospy.Publisher('ch_repforce', Goal) self.publishers['velCmd'] = rospy.Publisher('ch_velcmd', Goal) self.publishers['recovery'] = rospy.Publisher('ch_recovery', Goal) self.publishers['commandVelocity'] = rospy.Publisher('cmd_vel', Twist) self.publishers['waypointsReached'] = rospy.Publisher('waypoints_reached', Point) self.publishers['waypointsFailed'] = rospy.Publisher('waypoints_failed', Point) self.publishers['goalsNavigation'] = rospy.Publisher('goals_nav', Point) rospy.Subscriber('scan', LaserScan, self.scanCallback) rospy.Subscriber('pose', Pose, self.poseCallback) rospy.Subscriber('waypoints', Point, self.waypointCallback) rospy.Subscriber('ch_qrcodecount', Goal, self.stopRecoveryCallback) rospy.Subscriber('goals', Point, self.goalCallback) rospy.Subscriber('goals_nav', Point, self.goalCallback) self.distanceTraveled = 0.0 self.timeDeployed = -1 self.goal = Point(0, 0, 0) self.pose = Pose() self.pose.x = 0 self.pose.y = 0 self.pose.theta = 0 self.previousCommand = Polar(0, 0) self.previousRobotPose = Pose() self.previousRobotPose.x = self.pose.x self.previousRobotPose.y = self.pose.y self.timeSinceLastWaypoint = 0 self.timeLastScanned = 0 self.timeLastMoved = 0 self.angleMin = 0 self.angleMax = 0 self.previousWaypointQueueLength = 0 self.recovering = False self.i = 0
def __init__(self, nTurns, timePerTurn, angularV): self.nTurns = nTurns self.timePerTurn = timePerTurn self.angularV = angularV self.pose = Pose() self.traveled = 0 self.observed = 0
def calc_lmpose(robot_pose, dist, angle, camera_angle): """ Calculate the pose of a landmark, given robot's pose and its camera readings. :param robot_pose: Robot pose. :param dist: Range reading given by robot camera. :param angle: Angle reading given by robot camera. :param camera_angle: The angle between camera's orientation and robot's orientation. -pi / 2 or pi / 2. :return: Pose of a landmark. """ lm_pose = Pose() lm_pose.header.frame_id = "landmark" robot_lm_angle = robot_pose.theta + camera_angle + angle lm_pose.x = math.cos(robot_lm_angle) * dist lm_pose.y = math.sin(robot_lm_angle) * dist lm_pose.theta = 0 lm_pose.theta = 0 return lm_pose
def odom_to_pose(odom): """Utility function to convert an Odometry message into a Pose message.""" pose = Pose() pose.header = odom.header pose.x = odom.pose.pose.position.x pose.y = odom.pose.pose.position.y qz = odom.pose.pose.orientation.z qw = odom.pose.pose.orientation.w pose.theta = atan2(2 * qw * qz, 1 - 2 * qz * qz) pose.cov = reduce_covariance(odom.pose.covariance) return pose
def odom_to_pose(odom): pose = Pose() pose.header = odom.header pose.x = odom.pose.pose.position.x pose.y = odom.pose.pose.position.y qz = odom.pose.pose.orientation.z qw = odom.pose.pose.orientation.w pose.theta = atan2(2*qw*qz, 1-2*qz*qz) return pose
def odom_callback(odom): global LAST_ODOM_MEASUREMENT global CURRENT_POSE reported_pose = odom_to_pose(odom) delta_x = reported_pose.x - LAST_ODOM_MEASUREMENT.x delta_y = reported_pose.y - LAST_ODOM_MEASUREMENT.y delta_theta = reported_pose.theta - LAST_ODOM_MEASUREMENT.theta reported_linear_distance = euclidean_distance(0, 0, delta_x, delta_y) calibrated_linear_distance = LINEAR_CALIBRATION * reported_linear_distance calibrated_theta = ANGULAR_CALIBRATION * delta_theta \ + (LIN_ANGULAR_CALIBRATION * reported_linear_distance) angle_average = 0.5 * (CURRENT_POSE.theta + calibrated_theta) calibrated_x = calibrated_linear_distance * math.cos(angle_average) calibrated_y = calibrated_linear_distance * math.sin(angle_average) calibrated_pose = Pose(CURRENT_POSE.x + calibrated_x, CURRENT_POSE.y + calibrated_y, CURRENT_POSE.theta + calibrated_theta) LAST_ODOM_MEASUREMENT = reported_pose CURRENT_POSE = calibrated_pose CALIBRATION_PUBLISHER.publish(CURRENT_POSE)
def laser_callback(scan): rospy.loginfo("laser callback, pose (%.4g, %.4g, %.4g)", pose.x, pose.y, pose.theta) global posepub global lockedOn # upon startup we won't have a place to bootstrap from, so ignore if pose.x < 0.1 and pose.y < 0.1: return currentPose = pose samplePoints = get_sample_points(pose) #samplePoints = [(pose.x, pose.y, pose.theta)] sampleProbability = [] #rospy.loginfo("%s", samplePoints) sample = pose maxprob = 0 for sample in samplePoints: newLaser = get_expected_scan(sample) (currentProbability, goodscans) = get_sample_probability(scan.ranges, newLaser) if currentProbability > maxprob: maxprob = currentProbability sampleProbability.append(currentProbability) #rospy.loginfo("Prob: %g",currentProbability) sum_x = 0.0 sum_y = 0.0 sum_theta = 0.0 count = 0.0 for i in range(0, len(samplePoints)): sample = samplePoints[i] sum_x += sampleProbability[i] * sample[0] sum_y += sampleProbability[i] * sample[1] sum_theta += sampleProbability[i] * sample[2] count = count + sampleProbability[i] rospy.loginfo("Prob: %6.3g from %d scan points", count, goodscans) mean_pose = [sum_x / count, sum_y / count, sum_theta / count] rospy.loginfo("Mean pose: (%f, %f, %f)", mean_pose[0], mean_pose[1], mean_pose[2]) # if we don't like any of the samples, don't say anything. # this is probably wrong, but may help when lost or when # there are too many obstacles about. if maxprob < (probThresh**goodscans) or goodscans <= 10: rospy.loginfo("Skipping laser estimate, thresh = %6.3g", probThresh**goodscans) lockedOn = False # we'll spit out a bogus pose to let the GUI know we're still alive posemsg = Pose() posemsg.x = -1000 posemsg.y = -1000 posemsg.theta = 0 posemsg.cov = [0, 0, 0, 0, 0, 0, 0, 0, 0] posepub.publish(posemsg) return lockedOn = True sum_cx = 0.0 sum_cy = 0.0 sum_ctheta = 0.0 sum_xy = 0.0 sum_xt = 0.0 sum_yt = 0.0 #cCount = len(samplePoints) - 1 #cCount = 0 sum_wt = 0.0 sum_wt2 = 0.0 for i in range(0, len(samplePoints)): sample = samplePoints[i] xx = (sample[0] - mean_pose[0]) yy = (sample[1] - mean_pose[1]) tt = (sample[2] - mean_pose[2]) wt = sampleProbability[i] / count #rospy.loginfo("%s at P = %5.3g",sample,wt) sum_cx += xx * xx * wt sum_cy += yy * yy * wt sum_ctheta += tt * tt * wt sum_xy += xx * yy * wt sum_xt += xx * tt * wt sum_yt += yy * tt * wt sum_wt += wt sum_wt2 += wt * wt #cCount = sum_wt/(sum_wt*sum_wt - sum_wt2) cCount = 1 #sum_wt # if we are near a discontinuity in the underlying distribution, # the covariance will be bogus (not really Gaussian). So for now # we just don't report, but could consider sending mean pose # with a default covariance matrix. if sum_cx / cCount < 1e-6 or sum_cy / cCount < 1e-6 or sum_ctheta / cCount < 1e-6: rospy.loginfo("Modifying laser estimate, diag(cov) = %.3g, %.3g, %.3g",\ sum_cx/cCount, sum_cy/cCount, sum_ctheta/cCount) # Use a sloppy independent covariance here, I guess? covariance = [max(sum_cx/cCount, 1e-6), 0, 0, \ 0, max(sum_cy/cCount, 1e-6), 0, \ 0, 0, max(sum_ctheta/cCount, 1e-6)] return else: covariance = [sum_cx/cCount, sum_xy/cCount, sum_xt/cCount, \ sum_xy/cCount, sum_cy/cCount, sum_yt/cCount, \ sum_xt/cCount, sum_yt/cCount, sum_ctheta/cCount] #rospy.loginfo("Covariance: %s", coveriance_pose) posemsg = Pose() # convert from Kinect pose back to robot center pose posemsg.x = (mean_pose[0] - kinectOffset * math.cos(mean_pose[2])) - pose.x posemsg.y = (mean_pose[1] - kinectOffset * math.sin(mean_pose[2])) - pose.y posemsg.theta = mean_pose[2] - pose.theta posemsg.cov = covariance posepub.publish(posemsg)
from sensor_msgs.msg import LaserScan from corobot_common.srv import GetCoMap from corobot_common.msg import Pose from corobot_common.srv import GetPixelOccupancyResponse #from corobot_map import map import math from bresenhem import bresenhem # Get static map once rospy.wait_for_service('get_map') map_srv = rospy.ServiceProxy('get_map', GetCoMap) my_map = map_srv().map # Initializing pose pose = Pose() # PROXIMITY WIDTH AND LENGTH proxWidth = 1000 # in X direction proxHeight = 1000 # in y direction # Which laser scans do we consider? laser_samples_inc = 10 # KINECT LASER SCAN RANGES AND INCREMENT startTheta = -0.513185441494 endTheta = 0.49990695715 thetaIncrement = 0.00158295687288 * laser_samples_inc probThresh = 0.42 # per good scan sample
def laser_callback(scan): rospy.loginfo("laser callback, pose (%.4g, %.4g, %.4g)",pose.x,pose.y,pose.theta) global posepub global lockedOn # upon startup we won't have a place to bootstrap from, so ignore if pose.x < 0.1 and pose.y < 0.1: return currentPose = pose samplePoints = get_sample_points(pose) #samplePoints = [(pose.x, pose.y, pose.theta)] sampleProbability = [] #rospy.loginfo("%s", samplePoints) sample = pose maxprob = 0 for sample in samplePoints: newLaser = get_expected_scan(sample) (currentProbability, goodscans) = get_sample_probability(scan.ranges, newLaser) if currentProbability > maxprob: maxprob = currentProbability sampleProbability.append(currentProbability) #rospy.loginfo("Prob: %g",currentProbability) sum_x = 0.0 sum_y = 0.0 sum_theta = 0.0 count = 0.0 for i in range(0, len(samplePoints)): sample = samplePoints[i] sum_x += sampleProbability[i] * sample[0] sum_y += sampleProbability[i] * sample[1] sum_theta += sampleProbability[i] * sample[2] count = count + sampleProbability[i] rospy.loginfo("Prob: %6.3g from %d scan points", count, goodscans) mean_pose = [sum_x/count, sum_y/count, sum_theta/count] rospy.loginfo("Mean pose: (%f, %f, %f)",mean_pose[0],mean_pose[1],mean_pose[2]) # if we don't like any of the samples, don't say anything. # this is probably wrong, but may help when lost or when # there are too many obstacles about. if maxprob < (probThresh**goodscans) or goodscans <= 10: rospy.loginfo("Skipping laser estimate, thresh = %6.3g",probThresh**goodscans) lockedOn = False # we'll spit out a bogus pose to let the GUI know we're still alive posemsg = Pose() posemsg.x = -1000 posemsg.y = -1000 posemsg.theta = 0 posemsg.cov = [0, 0, 0, 0, 0, 0, 0, 0, 0] posepub.publish(posemsg) return lockedOn = True sum_cx = 0.0 sum_cy = 0.0 sum_ctheta = 0.0 sum_xy = 0.0 sum_xt = 0.0 sum_yt = 0.0 #cCount = len(samplePoints) - 1 #cCount = 0 sum_wt = 0.0 sum_wt2 = 0.0 for i in range(0, len(samplePoints)): sample = samplePoints[i] xx = (sample[0] - mean_pose[0]) yy = (sample[1] - mean_pose[1]) tt = (sample[2] - mean_pose[2]) wt = sampleProbability[i]/count #rospy.loginfo("%s at P = %5.3g",sample,wt) sum_cx += xx * xx * wt sum_cy += yy * yy * wt sum_ctheta += tt * tt * wt sum_xy += xx * yy * wt sum_xt += xx * tt * wt sum_yt += yy * tt * wt sum_wt += wt sum_wt2 += wt*wt #cCount = sum_wt/(sum_wt*sum_wt - sum_wt2) cCount = 1#sum_wt # if we are near a discontinuity in the underlying distribution, # the covariance will be bogus (not really Gaussian). So for now # we just don't report, but could consider sending mean pose # with a default covariance matrix. if sum_cx/cCount < 1e-6 or sum_cy/cCount < 1e-6 or sum_ctheta/cCount < 1e-6: rospy.loginfo("Modifying laser estimate, diag(cov) = %.3g, %.3g, %.3g",\ sum_cx/cCount, sum_cy/cCount, sum_ctheta/cCount) # Use a sloppy independent covariance here, I guess? covariance = [max(sum_cx/cCount, 1e-6), 0, 0, \ 0, max(sum_cy/cCount, 1e-6), 0, \ 0, 0, max(sum_ctheta/cCount, 1e-6)] else: covariance = [sum_cx/cCount, sum_xy/cCount, sum_xt/cCount, \ sum_xy/cCount, sum_cy/cCount, sum_yt/cCount, \ sum_xt/cCount, sum_yt/cCount, sum_ctheta/cCount] #rospy.loginfo("Covariance: %s", coveriance_pose) posemsg = Pose() # convert from Kinect pose back to robot center pose posemsg.x = (mean_pose[0] - kinectOffset*math.cos(mean_pose[2])) - pose.x posemsg.y = (mean_pose[1] - kinectOffset*math.sin(mean_pose[2])) - pose.y posemsg.theta = mean_pose[2] - pose.theta posemsg.cov = covariance posepub.publish(posemsg)
def get_pose(self): pose = Pose() pose.header.frame_id = "world" pose.x, pose.y, pose.theta = self.state_tuple() pose.cov = tuple(self.covariance.flat) return pose
#!/usr/bin/env python import roslib roslib.load_manifest("corobot_calibration_node") import rospy from nav_msgs.msg import Odometry from corobot_common.msg import Pose import math LAST_ODOM_MEASUREMENT = None CURRENT_POSE = Pose() CALIBRATION_PUBLISHER = None LOG_HERTZ = 10 LINEAR_CALIBRATION = 1.0 ANGULAR_CALIBRATION = 1.0 LIN_ANGULAR_CALIBRATION = 1.0 def odom_callback(odom): global LAST_ODOM_MEASUREMENT global CURRENT_POSE reported_pose = odom_to_pose(odom) delta_x = reported_pose.x - LAST_ODOM_MEASUREMENT.x delta_y = reported_pose.y - LAST_ODOM_MEASUREMENT.y delta_theta = reported_pose.theta - LAST_ODOM_MEASUREMENT.theta reported_linear_distance = euclidean_distance(0, 0, delta_x, delta_y) calibrated_linear_distance = LINEAR_CALIBRATION * reported_linear_distance
def convertRobotCoorToGlobalCoor(self, polarPoint): sp = Pose() sp.x = self.pose.x + polarPoint.d * cos(self.pose.theta + polarPoint.a) sp.y = self.pose.y + polarPoint.d * sin(self.pose.theta + polarPoint.a) sp.theta = 0 return sp
#!/usr/bin/env python import roslib roslib.load_manifest("corobot_odometry_logging") import rospy from nav_msgs.msg import Odometry from geometry_msgs.msg import Twist from corobot_common.msg import Pose FEATURES = { 'odom': None, 'cmd_vel': None, 'qr_pose': Pose(), 'barcode_loc': Pose(), } LOG_HERTZ = 10 def odom_callback(odom): global FEATURES FEATURES['odom'] = odom def cmd_vel_callback(vel): global FEATURES FEATURES['cmd_vel'] = vel def qrcode_pose_callback(qr_pose): global FEATURES