示例#1
0
 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 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
示例#4
0
 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
示例#5
0
 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
示例#6
0
文件: spin.py 项目: droter/corobots
 def __init__(self, nTurns, timePerTurn, angularV):
     self.nTurns = nTurns
     self.timePerTurn = timePerTurn
     self.angularV = angularV
     self.pose = Pose()
     self.traveled = 0
     self.observed = 0
示例#7
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
示例#8
0
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
示例#9
0
文件: spin.py 项目: droter/corobots
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
示例#10
0
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_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)
示例#12
0
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)
示例#13
0
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
示例#14
0
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)
示例#15
0
 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
示例#17
0
 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
示例#19
0
	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