def main(): rospy.init_node('tf_test') rospy.loginfo("Node for testing actionlib server") #from_link = '/head_color_camera_l_link' #to_link = '/base_link' from_link = '/base_link' to_link = '/map' tfl = TransformListener() rospy.sleep(5) t = rospy.Time(0) mpose = PoseStamped() mpose.pose.position.x = 1 mpose.pose.position.y = 0 mpose.pose.position.z = 0 mpose.pose.orientation.x = 0 mpose.pose.orientation.y = 0 mpose.pose.orientation.z = 0 mpose.pose.orientation.w = 0 mpose.header.frame_id = from_link mpose.header.stamp = rospy.Time.now() rospy.sleep(5) mpose_transf = None rospy.loginfo('Waiting for transform for some time...') tfl.waitForTransform(to_link,from_link,t,rospy.Duration(5)) if tfl.canTransform(to_link,from_link,t): mpose_transf = tfl.transformPose(to_link,mpose) print mpose_transf else: rospy.logerr('Transformation is not possible!') sys.exit(0)
class Leader(): def __init__(self, goals): rospy.init_node('leader', anonymous=True) self.worldFrame = rospy.get_param("~worldFrame", "/world") self.frame = rospy.get_param("~frame") self.leaderFrame = rospy.get_param("~leaderFrame", "/leader") self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1) # self.leaderAdvertise=rospy.Publisher('leaderPosition',PoseStamped,queue_size=1) self.listener = TransformListener() rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback) self.goals = goals self.takeoffFlag = 0 self.goalIndex = 0 rospy.loginfo("demo start!!!!!!!") def cmdVelCallback(self, data): if data.linear.z != 0.0 and self.takeoffFlag == 0: self.takeoffFlag = 1 rospy.sleep(10) self.takeoffFlag = 2 def run(self): self.listener.waitForTransform(self.worldFrame, self.frame, rospy.Time(), rospy.Duration(5.0)) goal = PoseStamped() goal.header.seq = 0 goal.header.frame_id = self.worldFrame while not rospy.is_shutdown(): self.calc_goal(goal, self.goalIndex) self.pubGoal.publish(goal) # self.leaderAdvertise.publish(goal) t = self.listener.getLatestCommonTime(self.worldFrame, self.frame) if self.listener.canTransform(self.worldFrame, self.frame, t): position, quaternion = self.listener.lookupTransform( self.worldFrame, self.frame, t) rpy = tf.transformations.euler_from_quaternion(quaternion) if self.takeoffFlag == 1: self.goalIndex = 0 elif self.takeoffFlag == 2 and self.goalIndex < len( self.goals) - 1: rospy.sleep(self.goals[self.goalIndex][4] * 2) rospy.loginfo(self.goalIndex) self.goalIndex += 1 def calc_goal(self, goal, index): goal.header.seq += 1 goal.header.stamp = rospy.Time.now() goal.pose.position.x = self.goals[index][0] goal.pose.position.y = self.goals[index][1] goal.pose.position.z = self.goals[index][2] quaternion = tf.transformations.quaternion_from_euler( 0, 0, self.goals[index][3]) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3]
class Follower(): def __init__(self, leaderFrame, radius=0.5, phase=0, pointNum=2000): rospy.init_node('follower', anonymous=True) self.worldFrame = rospy.get_param("~worldFrame", "/world") self.frame = rospy.get_param("~frame") self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1) # rospy.Subscriber("/"+leaderName+'/leaderPosition',PoseStamped,self.followerSubCB) self.listener = TransformListener() self.goal = PoseStamped() self.leaderFrame = leaderFrame self.radius = radius self.phase = 0 self.pointNum = 2000 self.goalIndex = 0 rospy.loginfo("demo start!!!!!!!") def run(self): self.listener.waitForTransform(self.worldFrame, self.frame, rospy.Time(), rospy.Duration(5.0)) self.goal = PoseStamped() self.goal.header.seq = 0 self.goal.header.frame_id = self.worldFrame self.goal.pose.orientation.w = 1 while not rospy.is_shutdown(): self.pubGoal.publish(self.goal) # rospy.loginfo(self.goal) # rospy.loginfo(self.worldFrame) # rospy.loginfo(self.leaderFrame) # rospy.loginfo(self.frame) t = self.listener.getLatestCommonTime(self.worldFrame, self.leaderFrame) if self.listener.canTransform(self.worldFrame, self.leaderFrame, t): position, quaternion = self.listener.lookupTransform( self.worldFrame, self.leaderFrame, t) # rospy.loginfo(position) # rospy.loginfo(quaternion) self.followerGoalGenerate(position, quaternion) rospy.sleep(0.02) def followerGoalGenerate(self, position, quaternion): # rospy.loginfo("info received!") angle = self.goalIndex / self.pointNum * 2 * math.pi + self.phase self.goal.header.seq += 1 self.goal.header.stamp = rospy.Time.now() self.goal.pose.position.x = position[0] + self.radius * math.cos(angle) self.goal.pose.position.y = position[1] + self.radius * math.sin(angle) # self.goal.pose.position.z=position.z self.goal.pose.position.z = 0.8 self.goal.pose.orientation.w = quaternion[3] self.goal.pose.orientation.x = quaternion[0] self.goal.pose.orientation.y = quaternion[1] self.goal.pose.orientation.z = quaternion[2] self.goalIndex = self.goalIndex + 1
class Normal(): def __init__(self): rospy.init_node('follower', anonymous=True) self.worldFrame = rospy.get_param("~worldFrame", "/world") self.frame = rospy.get_param("~frame") self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1) # rospy.Subscriber("/"+leaderName+'/leaderPosition',PoseStamped,self.followerSubCB) self.listener = TransformListener() self.goal=PoseStamped() # 第一件事情,跟随这个目标,这个目标的格式应该是一个frame self.leaderFrame=rospy.get_param("~leaderFrame","") self.offsetX=rospy.get_param("~offsetX","0") self.offsetY=rospy.get_param("~offsetY","0") # self.offsetZ=rospy.get_param("~offsetZ","0") # 第二件事情,广播自身的位置吧 # self.pubAttitude=rospy.Publisher('pose',) # 第三件事情,侦听manager节点的状态信息 self.takeoffFlag = 0 self.goalIndex = 0 rospy.loginfo("demo start!!!!!!!") def run(self): self.listener.waitForTransform(self.worldFrame, self.frame, rospy.Time(), rospy.Duration(5.0)) self.goal = PoseStamped() self.goal.header.seq = 0 self.goal.header.frame_id = self.worldFrame self.goal.pose.orientation.w=1 while not rospy.is_shutdown(): self.pubGoal.publish(self.goal) # rospy.loginfo(self.goal) # rospy.loginfo(self.worldFrame) # rospy.loginfo(self.leaderFrame) # rospy.loginfo(self.frame) t = self.listener.getLatestCommonTime(self.worldFrame, self.leaderFrame) if self.listener.canTransform(self.worldFrame, self.leaderFrame, t): position, quaternion = self.listener.lookupTransform(self.worldFrame, self.leaderFrame, t) # rospy.loginfo(position) # rospy.loginfo(quaternion) self.followerGoalGenerate(position,quaternion) rospy.sleep(0.02) def followerGoalGenerate(self,position,quaternion): # rospy.loginfo("info received!") self.goal.header.seq += 1 self.goal.header.stamp = rospy.Time.now() self.goal.pose.position.x=position[0]+float(self.offsetX) self.goal.pose.position.y=position[1]+float(self.offsetY) # self.goal.pose.position.z=position.z self.goal.pose.position.z=0.8 self.goal.pose.orientation.w=quaternion[3] self.goal.pose.orientation.x=quaternion[0] self.goal.pose.orientation.y=quaternion[1] self.goal.pose.orientation.z=quaternion[2]
def main(): rospy.init_node('tf_test') rospy.loginfo("Node for testing actionlib server") #from_link = '/head_color_camera_l_link' #to_link = '/base_link' from_link = '/base_link' to_link = '/map' tfl = TransformListener() rospy.sleep(5) t = rospy.Time(0) mpose = PoseStamped() mpose.pose.position.x = 1 mpose.pose.position.y = 0 mpose.pose.position.z = 0 mpose.pose.orientation.x = 0 mpose.pose.orientation.y = 0 mpose.pose.orientation.z = 0 mpose.pose.orientation.w = 0 mpose.header.frame_id = from_link mpose.header.stamp = rospy.Time.now() rospy.sleep(5) mpose_transf = None rospy.loginfo('Waiting for transform for some time...') tfl.waitForTransform(to_link, from_link, t, rospy.Duration(5)) if tfl.canTransform(to_link, from_link, t): mpose_transf = tfl.transformPose(to_link, mpose) print mpose_transf else: rospy.logerr('Transformation is not possible!') sys.exit(0)
class Demo(): def __init__(self, goals): rospy.init_node('demo', anonymous=True) self.worldFrame = rospy.get_param("~worldFrame", "/world") self.frame = rospy.get_param("~frame") self.pubGoal = rospy.Publisher( 'goal', PoseStamped, queue_size=1) #publish to topic goal with msg type PoseStamped self.listener = TransformListener( ) #tflisterner is a method with functions relating to transforms self.goals = goals #Assign nX5 matrix containing target waypoints self.goalIndex = 0 # start with the first row of entries (first waypoint) def run(self): self.listener.waitForTransform( self.worldFrame, self.frame, rospy.Time(), rospy.Duration(5.0) ) #Find the transform from world frame to body frame, returns bool on if it can find a transform goal = PoseStamped() goal.header.seq = 0 goal.header.frame_id = self.worldFrame while not rospy.is_shutdown(): goal.header.seq += 1 goal.header.stamp = rospy.Time.now() goal.pose.position.x = self.goals[self.goalIndex][0] goal.pose.position.y = self.goals[self.goalIndex][1] goal.pose.position.z = self.goals[self.goalIndex][2] quaternion = tf.transformations.quaternion_from_euler( 0, 0, self.goals[self.goalIndex][3]) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] self.pubGoal.publish(goal) t = self.listener.getLatestCommonTime(self.worldFrame, self.frame) if self.listener.canTransform(self.worldFrame, self.frame, t): position, quaternion = self.listener.lookupTransform( self.worldFrame, self.frame, t) rpy = tf.transformations.euler_from_quaternion(quaternion) #If within position error bound, sleep and then move to next waypoint if math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.2 \ and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.2 \ and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.2 \ and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(10) \ and self.goalIndex < len(self.goals) - 1: rospy.sleep(self.goals[self.goalIndex][4]) self.goalIndex += 1
class Plotter: def __init__(self): self.fig = pp.figure() self.lin_ax = AxisFig(self.fig, 211) self.ang_ax = AxisFig(self.fig, 212, False) self.fig.tight_layout() self.canvas = FigureCanvas(self.fig) self.tl = TransformListener() self.des_sub = rospy.Subscriber('/desired_pose', Float32MultiArray, self.des_pose_cb) self.des_pose = [] def des_pose_cb(self, msg): self.des_pose = msg.data def loop(self): while not rospy.is_shutdown(): # update TF if self.tl.canTransform('base_link', 'tool0', rospy.Time(0)): tr = self.tl.lookupTransform('base_link', 'tool0', rospy.Time(0)) d = False # angle-axis from quaternion s2 = pl.sqrt(tr[1][0]**2 + tr[1][1]**2 + tr[1][2]**2) tu = [0, 0, 0] if s2 > 1e-6: t = pl.arctan2(s2, tr[1][3]) * 2 if t > pl.pi: t -= 2 * pl.pi tu = [t * tr[1][i] / s2 for i in range(3)] if len(self.des_pose): d = self.lin_ax.update(tr[0], self.des_pose[:3]) self.ang_ax.update(tu, self.des_pose[3:]) else: d = self.lin_ax.update(tr[0]) self.ang_ax.update(tu) if d: self.canvas.draw() rospy.sleep(0.1)
class Demo(): def __init__(self, goals): rospy.init_node('demo', anonymous=True) self.worldFrame = rospy.get_param("~worldFrame", "/world") self.frame = rospy.get_param("~frame") self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1) self.listener = TransformListener() self.goals = goals self.goalIndex = 0 def run(self): self.listener.waitForTransform(self.worldFrame, self.frame, rospy.Time(), rospy.Duration(5.0)) goal = PoseStamped() goal.header.seq = 0 goal.header.frame_id = self.worldFrame while not rospy.is_shutdown(): goal.header.seq += 1 goal.header.stamp = rospy.Time.now() goal.pose.position.x = self.goals[self.goalIndex][0] goal.pose.position.y = self.goals[self.goalIndex][1] goal.pose.position.z = self.goals[self.goalIndex][2] quaternion = tf.transformations.quaternion_from_euler( 0, 0, self.goals[self.goalIndex][3]) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] self.pubGoal.publish(goal) t = self.listener.getLatestCommonTime(self.worldFrame, self.frame) if self.listener.canTransform(self.worldFrame, self.frame, t): position, quaternion = self.listener.lookupTransform( self.worldFrame, self.frame, t) rpy = tf.transformations.euler_from_quaternion(quaternion) if math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.1 \ and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.1 \ and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.1 \ and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(8): if self.goalIndex < len(self.goals) - 1: rospy.sleep(self.goals[self.goalIndex][4]) self.goalIndex += 1 elif self.goalIndex == len(self.goals) - 1: rospy.sleep(self.goals[self.goalIndex][4]) self.goalIndex = 1
class Swarm(): def __init__(self): rospy.init_node('demo', anonymous=True) self.worldFrame = rospy.get_param("~worldFrame", "/world") self.frame = rospy.get_param("~frame") self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1) self.listener = TransformListener() self.goals = goals self.goalIndex = 1 self.index = 1 with open('test.csv','rb') as myfile: reader=csv.reader(myfile) lines = [line for line in reader] def run(self): self.listener.waitForTransform(self.worldFrame, self.frame, rospy.Time(), rospy.Duration(5.0)) goal = PoseStamped() goal.header.seq = 0 goal.header.frame_id = self.worldFrame while not rospy.is_shutdown(): goal.header.seq += 1 goal.header.stamp = rospy.Time.now() goal.pose.position.x = int(lines[self.goalIndex][3*index-1]) goal.pose.position.y = int(lines[self.goalIndex][3*index+0]) goal.pose.position.z = int(lines[self.goalIndex][3*index+1]) quaternion = tf.transformations.quaternion_from_euler(0, 0, 0) goal.pose.orientation.x = 0 goal.pose.orientation.y = 0 goal.pose.orientation.z = 0 goal.pose.orientation.w = 1 self.pubGoal.publish(goal) t = self.listener.getLatestCommonTime(self.worldFrame, self.frame) if self.listener.canTransform(self.worldFrame, self.frame, t): position, quaternion = self.listener.lookupTransform(self.worldFrame, self.frame, t) rpy = tf.transformations.euler_from_quaternion(quaternion) if math.fabs(position[0] - int(lines[self.goalIndex][3*index-1])) < 0.25 \ and math.fabs(position[1] - int(lines[self.goalIndex][3*index+0])) < 0.25 \ and math.fabs(position[2] - int(lines[self.goalIndex][3*index+1])) < 0.25 \ and self.goalIndex < len(lines): rospy.sleep(lines[self.goalIndex][1]) self.goalIndex += 1
class Demo: def __init__(self, goals): rospy.init_node("demo", anonymous=True) self.frame = rospy.get_param("~frame") self.pubGoal = rospy.Publisher("goal", PoseStamped, queue_size=1) self.listener = TransformListener() self.goals = goals self.goalIndex = 0 def run(self): self.listener.waitForTransform("/world", self.frame, rospy.Time(), rospy.Duration(5.0)) goal = PoseStamped() goal.header.seq = 0 goal.header.frame_id = "world" while not rospy.is_shutdown(): goal.header.seq += 1 goal.header.stamp = rospy.Time.now() goal.pose.position.x = self.goals[self.goalIndex][0] goal.pose.position.y = self.goals[self.goalIndex][1] goal.pose.position.z = self.goals[self.goalIndex][2] quaternion = tf.transformations.quaternion_from_euler(0, 0, self.goals[self.goalIndex][3]) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] self.pubGoal.publish(goal) t = self.listener.getLatestCommonTime("/world", self.frame) if self.listener.canTransform("/world", self.frame, t): position, quaternion = self.listener.lookupTransform("/world", self.frame, t) rpy = tf.transformations.euler_from_quaternion(quaternion) if ( math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.3 and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.3 and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.3 and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(10) and self.goalIndex < len(self.goals) - 1 ): rospy.sleep(self.goals[self.goalIndex][4]) self.goalIndex += 1
class ParticleFilter(object): """ Class to represent Particle Filter ROS Node Subscribes to /initialpose for initial pose estimate Publishes top particle estimate to /particlebest and all particles in cloud to /particlecloud """ def __init__(self): """ __init__ function to create main attributes, setup threshold values, setup rosnode subs and pubs """ rospy.init_node('pf') self.initialized = False self.num_particles = 150 self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.particle_cloud = [] self.lidar_points = [] self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) self.best_particle_pub = rospy.Publisher("particlebest", PoseStamped, queue_size=10) self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.best_guess = ( None, None) # (index of particle with highest weight, its weight) self.particles_to_replace = .075 self.n_effective = 0 # this is a measure of the particle diversity # pose_listener responds to selection of a new approximate robot # location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() # laser_subscriber listens for data from the lidar rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # create instances of two helper objects that are provided to you # as part of the project self.current_odom_xy_theta = [] self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() self.initialized = True def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( msg.pose.pose) print("xy_theta", xy_theta) self.initialize_particle_cloud( msg.header.stamp, xy_theta) # creates particle cloud at position passed in # by message print("INITIALIZING POSE") # Use the helper functions to fix the transform def initialize_particle_cloud(self, timestamp, xy_theta): """ Creates initial particle cloud based on robot pose estimate position """ self.particle_cloud = [] angle_variance = math.pi / 10 # POint the points in the general direction of the robot x_cur = xy_theta[0] y_cur = xy_theta[1] theta_cur = self.transform_helper.angle_normalize(xy_theta[2]) # print("theta_cur: ", theta_cur) for i in range(self.num_particles): # Generate values for and add a new particle!! x_rel = random.uniform(-.3, .3) y_rel = random.uniform(-.3, .3) new_theta = (random.uniform(theta_cur - angle_variance, theta_cur + angle_variance)) # TODO: Could use a tf transform to add x and y in the robot's coordinate system new_particle = Particle(x_cur + x_rel, y_cur + y_rel, new_theta) self.particle_cloud.append(new_particle) print("Done initializing particles") self.normalize_particles() # publish particles (so things like rviz can see them) self.publish_particles() print("normalized correctly") self.update_robot_pose(timestamp) print("updated robot pose") def normalize_particles(self): """ Normalizes particle weights to total but retains weightage """ total_weights = sum([particle.w for particle in self.particle_cloud]) # if your weights aren't normalized then normalize them if total_weights != 1.0: for i in self.particle_cloud: i.w = i.w / total_weights def update_robot_pose(self, timestamp): """ Update the estimate of the robot's pose in the map frame given the updated particles. There are two logical methods for this: (1): compute the mean pose based on all the high weight particles (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() print("Normalized particles in update robot pose") # create average pose for robot pose based on entire particle cloud average_x = 0 average_y = 0 average_theta = 0 # walk through all particles, calculate weighted average for x, y, z, in particle map. for p in self.particle_cloud: average_x += p.x * p.w average_y += p.y * p.w average_theta += p.theta * p.w # # create new particle representing weighted average values, pass in Pose to new robot pose self.robot_pose = Particle(average_x, average_y, average_theta).as_pose() print(timestamp) self.transform_helper.fix_map_to_odom_transform( self.robot_pose, timestamp) print("Done fixing map to odom") def publish_particles(self): """ Publish entire particle cloud as pose array for visualization in RVIZ Also publish the top / best particle based on its weight """ # Convert the particles from xy_theta to pose!! pose_particle_cloud = [] for p in self.particle_cloud: pose_particle_cloud.append(p.as_pose()) self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=pose_particle_cloud)) # doing shit based off best pose best_pose_quat = max(self.particle_cloud, key=attrgetter('w')).as_pose() #self.best_particle_pub.publish(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), pose=best_pose_quat) def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # TODO: FIX noise incorporation into movement. min_travel = 0.2 xy_spread = 0.02 / min_travel # More variance with driving forward theta_spread = .005 / min_travel random_vals_x = np.random.normal(0, abs(delta[0] * xy_spread), self.num_particles) random_vals_y = np.random.normal(0, abs(delta[1] * xy_spread), self.num_particles) random_vals_theta = np.random.normal(0, abs(delta[2] * theta_spread), self.num_particles) for p_num, p in enumerate(self.particle_cloud): # compute phi, or basically the angle from 0 that the particle # needs to be moving - phi equals OG diff angle - robot angle + OG partilce angle # ADD THE NOISE!! noisy_x = (delta[0] + random_vals_x[p_num]) noisy_y = (delta[1] + random_vals_y[p_num]) ang_of_dest = math.atan2(noisy_y, noisy_x) # calculate angle needed to turn in angle_to_dest ang_to_dest = self.transform_helper.angle_diff( self.current_odom_xy_theta[2], ang_of_dest) d = math.sqrt(noisy_x**2 + noisy_y**2) phi = p.theta + ang_to_dest p.x += math.cos(phi) * d p.y += math.sin(phi) * d p.theta += self.transform_helper.angle_normalize( delta[2] + random_vals_theta[p_num]) self.current_odom_xy_theta = new_odom_xy_theta def update_particles_with_laser(self, msg): """ calculate particle weights based off laser scan data passed into param """ # print("Updating particles with Laser") lidar_points = msg.ranges for p_deg, p in enumerate(self.particle_cloud): # do we need to compute particle pos in diff frame? p.occ_scan_mapped = [] # reset list for scan_distance in lidar_points: # handle edge case if scan_distance == 0.0: continue # calc a delta theta and use that to overlay scan data onto the particle headings pt_rad = deg2rad(p_deg) particle_pt_theta = self.transform_helper.angle_normalize( p.theta + pt_rad) particle_pt_x = p.x + math.cos( particle_pt_theta) * scan_distance particle_pt_y = p.y + math.sin( particle_pt_theta) * scan_distance # calculate distance from every single scan point in particle frame occ_value = self.occupancy_field.get_closest_obstacle_distance( particle_pt_x, particle_pt_y) # Think about cutting off max penalty if occ_value is too big p.occ_scan_mapped.append(occ_value) # assign weights based off newly assigned occ_scan_mapped # apply gaussian e**-d**2 to every weight, then cube to emphasize p.occ_scan_mapped = [(math.e / (d)**2) if (d)**2 != 0 else (math.e / (d + .01)**2) for d in p.occ_scan_mapped] p.occ_scan_mapped = [d**3 for d in p.occ_scan_mapped] p.w = sum(p.occ_scan_mapped) #print("Set weight to: ", p.w) p.occ_scan_mapped = [] self.normalize_particles() @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def resample_particles(self): """ Re initialize particles in self.particle_cloud based on preivous weightages. """ weights = [p.w for p in self.particle_cloud] # after calculating all particle weights, we want to calc the n_effective # self.n_effective = 0 self.n_effective = 1 / sum( [w**2 for w in weights]) # higher is more diversity, so less noise print("n_effective: ", self.n_effective) temp_particle_cloud = self.draw_random_sample( self.particle_cloud, weights, int((1 - self.particles_to_replace) * self.num_particles)) # temp_particle_cloud = self.draw_random_sample(self.particle_cloud, weights, self.num_particles) particle_cloud_to_transform = self.draw_random_sample( self.particle_cloud, weights, self.num_particles - int( (1 - self.particles_to_replace) * self.num_particles)) # NOISE POLLUTION - larger noise, smaller # particles # normal_std_xy = .25 normal_std_xy = 10 / self.n_effective # feedback loop? 8,3 normal_std_theta = 3 / self.n_effective # normal_std_theta = math.pi/21 random_vals_x = np.random.normal(0, normal_std_xy, len(particle_cloud_to_transform)) random_vals_y = np.random.normal(0, normal_std_xy, len(particle_cloud_to_transform)) random_vals_theta = np.random.normal(0, normal_std_theta, len(particle_cloud_to_transform)) for p_num, p in enumerate( particle_cloud_to_transform): # add in noise in x,y, theta p.x += random_vals_x[p_num] p.y += random_vals_y[p_num] p.theta += random_vals_theta[p_num] # reset the partilce cloud based on the newly transformed particles self.particle_cloud = temp_particle_cloud + particle_cloud_to_transform def scan_received(self, msg): """ Callback function for recieving laser scan - should pass data into global scan object """ """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, we hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative to the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) # grab from listener & store the the odometry pose in a more convenient format (x,y,theta) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) if not self.current_odom_xy_theta: self.current_odom_xy_theta = new_odom_xy_theta return # Now we've done all calcs, we exit the scan_recieved() method by either initializing a cloud if not (self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud # TODO: Where do we get the xy_theta needed for initialize_particle_cloud? self.initialize_particle_cloud(msg.header.stamp, self.current_odom_xy_theta) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose(msg.header.stamp) # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density # # publish particles (so things like rviz can see them) self.publish_particles() def run(self): """ main run loop for rosnode """ r = rospy.Rate(5) print("Nathan and Adi ROS Loop code is starting!!!") while not (rospy.is_shutdown()): # in the main loop all we do is continuously broadcast the latest # map to odom transform self.transform_helper.send_last_map_to_odom_transform() r.sleep()
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node( 'pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 300 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid # TODO: fill in the appropriate service call here. The resultant map should be assigned be passed # into the init method for OccupancyField # for now we have commented out the occupancy field initialization until you can successfully fetch the map #self.occupancy_field = OccupancyField(map) self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() # TODO: assign the lastest pose into self.robot_pose as a geometry_msgs.Pose object # just to get started we will fix the robot's pose to always be at the origin self.robot_pose = Pose() def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # TODO: modify particles using delta # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) def map_calc_range(self, x, y, theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized self.normalize_particles() # TODO: fill out the rest of the implementation def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ # TODO: implement this pass @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] # TODO create particles self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ pass # TODO: implement this def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) if not (self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density self.fix_map_to_odom_transform( msg ) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer TODO: if you want to learn a lot about tf, reimplement this... I can provide you with some hints as to what is going on here. """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose( translation, rotation), header=Header(stamp=msg.header.stamp, frame_id=self.base_frame)) self.tf_listener.waitForTransform(self.base_frame, self.odom_frame, msg.header.stamp, rospy.Duration(1.0)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not (hasattr(self, 'translation') and hasattr(self, 'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node( 'pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 300 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # TODO: define additional constants if needed #### DELETE BEFORE POSTING self.alpha1 = 0.2 self.alpha2 = 0.2 self.alpha3 = 0.2 self.alpha4 = 0.2 self.z_hit = 0.5 self.z_rand = 0.5 self.sigma_hit = 0.1 ##### DELETE BEFORE POSTING # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # request the map # Difficulty level 2 rospy.wait_for_service("static_map") static_map = rospy.ServiceProxy("static_map", GetMap) try: map = static_map().map except: print "error receiving map" self.occupancy_field = OccupancyField(map) self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ """ Difficulty level 2 """ # first make sure that the particle weights are normalized self.normalize_particles() use_mean = True if use_mean: mean_x = 0.0 mean_y = 0.0 mean_theta = 0.0 theta_list = [] weighted_orientation_vec = np.zeros((2, 1)) for p in self.particle_cloud: mean_x += p.x * p.w mean_y += p.y * p.w weighted_orientation_vec[0] += p.w * math.cos(p.theta) weighted_orientation_vec[1] += p.w * math.sin(p.theta) mean_theta = math.atan2(weighted_orientation_vec[1], weighted_orientation_vec[0]) self.robot_pose = Particle(x=mean_x, y=mean_y, theta=mean_theta).as_pose() else: weights = [] for p in self.particle_cloud: weights.append(p.w) best_particle = np.argmax(weights) self.robot_pose = self.particle_cloud[best_particle].as_pose() def update_particles_with_odom(self, msg): """ Implement a simple version of this (Level 1) or a more complex one (Level 2) """ new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta( self.odom_pose.pose) if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # Implement sample_motion_odometry (Prob Rob p 136) # Avoid computing a bearing from two poses that are extremely near each # other (happens on in-place rotation). delta_trans = math.sqrt(delta[0] * delta[0] + delta[1] * delta[1]) if delta_trans < 0.01: delta_rot1 = 0.0 else: delta_rot1 = ParticleFilter.angle_diff( math.atan2(delta[1], delta[0]), old_odom_xy_theta[2]) delta_rot2 = ParticleFilter.angle_diff(delta[2], delta_rot1) # We want to treat backward and forward motion symmetrically for the # noise model to be applied below. The standard model seems to assume # forward motion. delta_rot1_noise = min( math.fabs(ParticleFilter.angle_diff(delta_rot1, 0.0)), math.fabs(ParticleFilter.angle_diff(delta_rot1, math.pi))) delta_rot2_noise = min( math.fabs(ParticleFilter.angle_diff(delta_rot2, 0.0)), math.fabs(ParticleFilter.angle_diff(delta_rot2, math.pi))) for sample in self.particle_cloud: # Sample pose differences delta_rot1_hat = ParticleFilter.angle_diff( delta_rot1, gauss( 0, self.alpha1 * delta_rot1_noise * delta_rot1_noise + self.alpha2 * delta_trans * delta_trans)) delta_trans_hat = delta_trans - gauss( 0, self.alpha3 * delta_trans * delta_trans + self.alpha4 * delta_rot1_noise * delta_rot1_noise + self.alpha4 * delta_rot2_noise * delta_rot2_noise) delta_rot2_hat = ParticleFilter.angle_diff( delta_rot2, gauss( 0, self.alpha1 * delta_rot2_noise * delta_rot2_noise + self.alpha2 * delta_trans * delta_trans)) # Apply sampled update to particle pose sample.x += delta_trans_hat * math.cos(sample.theta + delta_rot1_hat) sample.y += delta_trans_hat * math.sin(sample.theta + delta_rot1_hat) sample.theta += delta_rot1_hat + delta_rot2_hat def map_calc_range(self, x, y, theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ pass def resample_particles(self): self.normalize_particles() values = np.empty(self.n_particles) probs = np.empty(self.n_particles) for i in range(len(self.particle_cloud)): values[i] = i probs[i] = self.particle_cloud[i].w new_particle_indices = ParticleFilter.weighted_values( values, probs, self.n_particles) new_particles = [] for i in new_particle_indices: idx = int(i) s_p = self.particle_cloud[idx] new_particles.append( Particle(x=s_p.x + gauss(0, .025), y=s_p.y + gauss(0, .025), theta=s_p.theta + gauss(0, .025))) self.particle_cloud = new_particles self.normalize_particles() # Difficulty level 1 def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ laser_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta( self.laser_pose.pose) for p in self.particle_cloud: adjusted_pose = (p.x + laser_xy_theta[0], p.y + laser_xy_theta[1], p.theta + laser_xy_theta[2]) # Pre-compute a couple of things z_hit_denom = 2 * self.sigma_hit**2 z_rand_mult = 1.0 / msg.range_max # This assumes quite a bit about the weights beforehand (TODO: could base this on p.w) new_prob = 1.0 # more agressive DEBUG, was 1.0 for i in range(0, len(msg.ranges), 6): pz = 1.0 obs_range = msg.ranges[i] obs_bearing = i * msg.angle_increment + msg.angle_min if math.isnan(obs_range): continue if obs_range >= msg.range_max: continue # compute the endpoint of the laser end_x = p.x + obs_range * math.cos(p.theta + obs_bearing) end_y = p.y + obs_range * math.sin(p.theta + obs_bearing) z = self.occupancy_field.get_closest_obstacle_distance( end_x, end_y) if math.isnan(z): z = self.laser_max_distance else: z = z[0] # not sure why this is happening pz += self.z_hit * math.exp(-(z * z) / z_hit_denom) / ( math.sqrt(2 * math.pi) * self.sigma_hit) pz += self.z_rand * z_rand_mult new_prob += pz**3 p.w = new_prob pass @staticmethod def angle_normalize(z): """ convenience function to map an angle to the range [-pi,pi] """ return math.atan2(math.sin(z), math.cos(z)) @staticmethod def angle_diff(a, b): """ Calculates the difference between angle a and angle b (both should be in radians) the difference is always based on the closest rotation from angle a to angle b examples: angle_diff(.1,.2) -> -.1 angle_diff(.1, 2*math.pi - .1) -> .2 angle_diff(.1, .2+2*math.pi) -> -.1 """ a = ParticleFilter.angle_normalize(a) b = ParticleFilter.angle_normalize(b) d1 = a - b d2 = 2 * math.pi - math.fabs(d1) if d1 > 0: d2 *= -1.0 if math.fabs(d1) < math.fabs(d2): return d1 else: return d2 @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements form the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = TransformHelpers.convert_pose_to_xy_and_theta( self.odom_pose.pose) self.particle_cloud = [] for i in range(self.n_particles): self.particle_cloud.append( Particle(x=xy_theta[0] + gauss(0, .25), y=xy_theta[1] + gauss(0, .25), theta=xy_theta[2] + gauss(0, .25))) self.normalize_particles() self.update_robot_pose() """ Level 1 """ def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ z = 0.0 for p in self.particle_cloud: z += p.w for i in range(len(self.particle_cloud)): self.particle_cloud[i].w /= z def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta( self.odom_pose.pose) if not (self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.resample_particles( ) # resample particles to focus on areas of high density self.update_robot_pose() # update robot's pose self.fix_map_to_odom_transform( msg ) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """ (translation, rotation) = TransformHelpers.convert_pose_inverse_transform( self.robot_pose) p = PoseStamped( pose=TransformHelpers.convert_translation_rotation_to_pose( translation, rotation), header=Header(stamp=msg.header.stamp, frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform( self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not (hasattr(self, 'translation') and hasattr(self, 'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) number_of_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.number_of_particles = 1000 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # Fetch map using OccupancyField rospy.wait_for_service('static_map') static_map = rospy.ServiceProxy('static_map', GetMap) self.occupancy_field = OccupancyField(static_map().map) self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() avg_x = 0 avg_y = 0 theta_x = 0 theta_y = 0 # Multiple x and y by particle weights to find new robot pose for particle in self.particle_cloud: avg_x += particle.x * particle.w avg_y += particle.y * particle.w theta_x += math.cos(particle.theta) * particle.w theta_y += math.sin(particle.theta) * particle.w # Calculate theta using arc tan of x and y components of all thetas multiplied by particle weights avg_theta = math.atan2(theta_y, theta_x) avg_particle = Particle(x=avg_x, y=avg_y, theta=avg_theta) # Update robot pose based on average particle self.robot_pose = avg_particle.as_pose() def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return temp = [] # Use trigonometry to update particles based on new odometry pose for particle in self.particle_cloud: psy = math.atan2(delta[1],delta[0])-old_odom_xy_theta[2] intermediate_theta = particle.theta + psy # Calculate radius based on change in x and y r = math.sqrt(delta[0]**2 + delta[1]**2) # Update x and y based on radius and new angle new_x = particle.x + r*math.cos(intermediate_theta) + np.random.randn()*0.1 new_y = particle.y + r*math.sin(intermediate_theta) + np.random.randn()*0.1 # Add change in angle to old angle new_theta = delta[2]+particle.theta + np.random.randn()*0.1 temp.append(Particle(new_x,new_y,new_theta)) self.particle_cloud = temp def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ probabilities = [] # create list of particle weights to pass into draw_random_sample for resampling for particle in self.particle_cloud: probabilities.append(particle.w) print particle.w print '\n' temp_particle_cloud = self.draw_random_sample(self.particle_cloud, probabilities, 100) self.particle_cloud = [] for particle in temp_particle_cloud: for i in range(10): self.particle_cloud.append(deepcopy(particle)) self.normalize_particles() def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ temp = 0 ranges = [] min_range = 5 for item in msg.ranges: # set ranges to 5 if the laser scan is 0 if item == 0: ranges.append(5) else: ranges.append(item) # do weighted averages for cleaner data for i in range(355): avg = sum(ranges[i:i+5]) / len(ranges[i:i+5]) if avg < min_range: min_range = avg min_theta = (i + 2.5)*math.pi / 180.0 # find the minimum range across 360 angles, this probably caused an issue r = min_range # Update particle x, y, theta based on min range, previous particles for particle in self.particle_cloud: x = particle.x+r*math.cos(particle.theta + min_theta) y = particle.y+r*math.sin(particle.theta + min_theta) temp = self.occupancy_field.get_closest_obstacle_distance(x,y) # Update particle weights using a sharp Gaussian particle.w = np.exp(-np.power(temp, 2.) / (2 * np.power(0.3, 2.))) @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] self.particle_cloud.append(Particle(xy_theta[0],xy_theta[1],xy_theta[2])) # Initialize particle cloud with a decent amount of noise for i in range (0,self.number_of_particles): self.particle_cloud.append(Particle(xy_theta[0]+np.random.randn()*.5,xy_theta[1]+np.random.randn()*.5,xy_theta[2]+np.random.randn()*.5)) self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ particle_sum = 0 # Sum up particle weights to divide by for normalization for particle in self.particle_cloud: particle_sum += particle.w # Make all particle weights add to 1 for particle in self.particle_cloud: particle.w /= particle_sum def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles() # resample particles to focus on areas of high density self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation), header=Header(stamp=msg.header.stamp,frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class MyAMCL: def __init__(self): self.initialized = False rospy.init_node('my_amcl') print "MY AMCL initialized" # todo make this static self.n_particles = 100 self.alpha1 = 0.2 self.alpha2 = 0.2 self.alpha3 = 0.2 self.alpha4 = 0.2 self.d_thresh = 0.2 self.a_thresh = math.pi/6 self.z_hit = 0.5 self.z_rand = 0.5 self.sigma_hit = 0.2 self.laser_max_distance = 2.0 self.laser_subscriber = rospy.Subscriber("scan", LaserScan, self.scan_received); self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_pub = rospy.Publisher("particlecloud", PoseArray) self.particle_cloud = [] self.last_transform_valid = False self.particle_cloud_initialized = False self.current_odom_xy_theta = [] # request the map rospy.wait_for_service("static_map") static_map = rospy.ServiceProxy("static_map", GetMap) try: self.map = static_map().map except: print "error receiving map" self.create_occupancy_field() self.initialized = True def create_occupancy_field(self): X = np.zeros((self.map.info.width*self.map.info.height,2)) total_occupied = 0 curr = 0 for i in range(self.map.info.width): for j in range(self.map.info.height): # occupancy grids are stored in row major order, if you go through this right, you might be able to use curr ind = i + j*self.map.info.width if self.map.data[ind] > 0: total_occupied += 1 X[curr,0] = float(i) X[curr,1] = float(j) curr += 1 O = np.zeros((total_occupied,2)) curr = 0 for i in range(self.map.info.width): for j in range(self.map.info.height): # occupancy grids are stored in row major order, if you go through this right, you might be able to use curr ind = i + j*self.map.info.width if self.map.data[ind] > 0: O[curr,0] = float(i) O[curr,1] = float(j) curr += 1 t = time.time() nbrs = NearestNeighbors(n_neighbors=1,algorithm="ball_tree").fit(O) distances, indices = nbrs.kneighbors(X) print time.time() -t closest_occ = {} curr = 0 for i in range(self.map.info.width): for j in range(self.map.info.height): ind = i + j*self.map.info.width closest_occ[ind] = distances[curr]*self.map.info.resolution curr += 1 # this is a bit adhoc, could probably integrate into an internal map structure self.closest_occ = closest_occ def update_robot_pose(self): # first make sure that the particle weights are normalized self.normalize_particles() use_mean = True if use_mean: mean_x = 0.0 mean_y = 0.0 mean_theta = 0.0 theta_list = [] weighted_orientation_vec = np.zeros((2,1)) for p in self.particle_cloud: mean_x += p.x*p.w mean_y += p.y*p.w weighted_orientation_vec[0] += p.w*math.cos(p.theta) weighted_orientation_vec[1] += p.w*math.sin(p.theta) mean_theta = math.atan2(weighted_orientation_vec[1],weighted_orientation_vec[0]) self.robot_pose = Particle(x=mean_x,y=mean_y,theta=mean_theta).as_pose() else: weights = [] for p in self.particle_cloud: weights.append(p.w) best_particle = np.argmax(weights) self.robot_pose = self.particle_cloud[best_particle].as_pose() def update_particles_with_odom(self, msg): new_odom_xy_theta = MyAMCL.convert_pose_to_xy_and_theta(self.odom_pose.pose) if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # Implement sample_motion_odometry (Prob Rob p 136) # Avoid computing a bearing from two poses that are extremely near each # other (happens on in-place rotation). delta_trans = math.sqrt(delta[0]*delta[0] + delta[1]*delta[1]) if delta_trans < 0.01: delta_rot1 = 0.0 else: delta_rot1 = MyAMCL.angle_diff(math.atan2(delta[1], delta[0]), old_odom_xy_theta[2]) delta_rot2 = MyAMCL.angle_diff(delta[2], delta_rot1) # We want to treat backward and forward motion symmetrically for the # noise model to be applied below. The standard model seems to assume # forward motion. delta_rot1_noise = min(math.fabs(MyAMCL.angle_diff(delta_rot1,0.0)), math.fabs(MyAMCL.angle_diff(delta_rot1, math.pi))); delta_rot2_noise = min(math.fabs(MyAMCL.angle_diff(delta_rot2,0.0)), math.fabs(MyAMCL.angle_diff(delta_rot2, math.pi))); for sample in self.particle_cloud: # Sample pose differences delta_rot1_hat = MyAMCL.angle_diff(delta_rot1, gauss(0, self.alpha1*delta_rot1_noise*delta_rot1_noise + self.alpha2*delta_trans*delta_trans)) delta_trans_hat = delta_trans - gauss(0, self.alpha3*delta_trans*delta_trans + self.alpha4*delta_rot1_noise*delta_rot1_noise + self.alpha4*delta_rot2_noise*delta_rot2_noise) delta_rot2_hat = MyAMCL.angle_diff(delta_rot2, gauss(0, self.alpha1*delta_rot2_noise*delta_rot2_noise + self.alpha2*delta_trans*delta_trans)) # Apply sampled update to particle pose sample.x += delta_trans_hat * math.cos(sample.theta + delta_rot1_hat) sample.y += delta_trans_hat * math.sin(sample.theta + delta_rot1_hat) sample.theta += delta_rot1_hat + delta_rot2_hat def get_map_index(self,x,y): x_coord = int((x - self.map.info.origin.position.x)/self.map.info.resolution) y_coord = int((y - self.map.info.origin.position.y)/self.map.info.resolution) # check if we are in bounds if x_coord > self.map.info.width or x_coord < 0: return float('nan') if y_coord > self.map.info.height or y_coord < 0: return float('nan') ind = x_coord + y_coord*self.map.info.width if ind >= self.map.info.width*self.map.info.height or ind < 0: return float('nan') return ind def map_calc_range(self,x,y,theta): ''' this is for a beam model... this is pretty damn slow...''' (x_curr,y_curr) = (x,y) ind = self.get_map_index(x_curr, y_curr) while not(math.isnan(ind)): if self.map.data[ind] > 0: return math.sqrt((x - x_curr)**2 + (y - y_curr)**2) x_curr += self.map.info.resolution*0.5*math.cos(theta) y_curr += self.map.info.resolution*0.5*math.sin(theta) ind = self.get_map_index(x_curr, y_curr) if math.isnan(ind): return float('nan') else: return self.map.info.range_max def resample_particles(self): self.normalize_particles() values = np.empty(self.n_particles) probs = np.empty(self.n_particles) for i in range(len(self.particle_cloud)): values[i] = i probs[i] = self.particle_cloud[i].w new_particle_indices = MyAMCL.weighted_values(values,probs,self.n_particles) new_particles = [] for i in new_particle_indices: idx = int(i) s_p = self.particle_cloud[idx] new_particles.append(Particle(x=s_p.x+gauss(0,.025),y=s_p.y+gauss(0,.025),theta=s_p.theta+gauss(0,.025))) self.particle_cloud = new_particles self.normalize_particles() def update_particles_with_laser(self, msg): laser_xy_theta = MyAMCL.convert_pose_to_xy_and_theta(self.laser_pose.pose) for p in self.particle_cloud: adjusted_pose = (p.x+laser_xy_theta[0], p.y+laser_xy_theta[1], p.theta+laser_xy_theta[2]) # Pre-compute a couple of things z_hit_denom = 2*self.sigma_hit**2 z_rand_mult = 1.0/msg.range_max # This assumes quite a bit about the weights beforehand (TODO: could base this on p.w) new_prob = 1.0 for i in range(5,len(msg.ranges),10): pz = 1.0 obs_range = msg.ranges[i] obs_bearing = i*msg.angle_increment+msg.angle_min if math.isnan(obs_range): continue if obs_range >= msg.range_max: continue # compute the endpoint of the laser end_x = p.x + obs_range*math.cos(p.theta+obs_bearing) end_y = p.y + obs_range*math.sin(p.theta+obs_bearing) ind = self.get_map_index(end_x,end_y) if math.isnan(ind): z = self.laser_max_distance else: z = self.closest_occ[ind] pz += self.z_hit * math.exp(-(z * z) / z_hit_denom) pz += self.z_rand * z_rand_mult new_prob += pz**3 p.w = new_prob @staticmethod def normalize(z): return math.atan2(math.sin(z), math.cos(z)) @staticmethod def angle_diff(a, b): a = MyAMCL.normalize(a) b = MyAMCL.normalize(b) d1 = a-b d2 = 2*math.pi - math.fabs(d1) if d1 > 0: d2 *= -1.0 if math.fabs(d1) < math.fabs(d2): return d1 else: return d2 @staticmethod def weighted_values(values, probabilities, size): bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod def convert_pose_to_xy_and_theta(pose): orientation_tuple = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) angles = euler_from_quaternion(orientation_tuple) return (pose.position.x, pose.position.y, angles[2]) def initialize_particle_cloud(self): self.particle_cloud_initialized = True (x,y,theta) = MyAMCL.convert_pose_to_xy_and_theta(self.odom_pose.pose) for i in range(self.n_particles): self.particle_cloud.append(Particle(x=x+gauss(0,.25),y=y+gauss(0,.25),theta=theta+gauss(0,.25))) self.normalize_particles() def normalize_particles(self): z = 0.0 for p in self.particle_cloud: z += p.w for i in range(len(self.particle_cloud)): self.particle_cloud[i].w /= z def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),frame_id="map"),poses=particles_conv)) def scan_received(self, msg): if not(self.initialized): return if not(self.tf_listener.canTransform("base_footprint",msg.header.frame_id,msg.header.stamp)): return if not(self.tf_listener.canTransform("base_footprint","odom",msg.header.stamp)): return p = PoseStamped(header=Header(stamp=rospy.Time(0),frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose("base_footprint",p) p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id="base_footprint"), pose=Pose()) #p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id="base_footprint"), pose=Pose(position=Point(x=0.0,y=0.0,z=0.0),orientation=Quaternion(x=0.0,y=0.0,z=0.0,w=0.0))) self.odom_pose = self.tf_listener.transformPose("odom", p) new_odom_xy_theta = MyAMCL.convert_pose_to_xy_and_theta(self.odom_pose.pose) if not(self.particle_cloud_initialized): self.initialize_particle_cloud() self.update_robot_pose() self.current_odom_xy_theta = new_odom_xy_theta self.fix_map_to_odom_transform(msg) else: delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) if math.fabs(delta[0]) > self.d_thresh or math.fabs(delta[1]) > self.d_thresh or math.fabs(delta[2]) > self.a_thresh: self.update_particles_with_odom(msg) self.update_robot_pose() self.update_particles_with_laser(msg) self.resample_particles() self.update_robot_pose() self.fix_map_to_odom_transform(msg) else: self.fix_map_to_odom_transform(msg, False) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg, recompute_odom_to_map=True): if recompute_odom_to_map: (translation, rotation) = MyAMCL.convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=MyAMCL.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=msg.header.stamp,frame_id="base_footprint")) self.odom_to_map = self.tf_listener.transformPose("odom", p) (translation, rotation) = MyAMCL.convert_pose_inverse_transform(self.odom_to_map.pose) self.tf_broadcaster.sendTransform(translation, rotation, msg.header.stamp+rospy.Duration(1.0), "odom", "map") @staticmethod def convert_translation_rotation_to_pose(translation,rotation): return Pose(position=Point(x=translation[0],y=translation[1],z=translation[2]), orientation=Quaternion(x=rotation[0],y=rotation[1],z=rotation[2],w=rotation[3])) @staticmethod def convert_pose_inverse_transform(pose): translation = np.zeros((4,1)) translation[0] = -pose.position.x translation[1] = -pose.position.y translation[2] = -pose.position.z translation[3] = 1.0 rotation = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) euler_angle = euler_from_quaternion(rotation) rotation = np.transpose(rotation_matrix(euler_angle[2], [0,0,1])) # the angle is a yaw transformed_translation = rotation.dot(translation) translation = (transformed_translation[0], transformed_translation[1], transformed_translation[2]) rotation = quaternion_from_matrix(rotation) return (translation, rotation)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "base_scan" # the topic where we will get laser scans from self.n_particles = 500 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model self.sigma = 0.08 # guess for how inaccurate lidar readings are in meters # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) self.marker_pub = rospy.Publisher("markers", MarkerArray, queue_size=10) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid self.map_server = rospy.ServiceProxy('static_map', GetMap) self.map = self.map_server().map # for now we have commented out the occupancy field initialization until you can successfully fetch the map self.occupancy_field = OccupancyField(self.map) self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. Computed by taking the weighted average of poses. """ # first make sure that the particle weights are normalized self.normalize_particles() x = 0 y = 0 theta = 0 angles = [] for particle in self.particle_cloud: x += particle.x * particle.w y += particle.y * particle.w v = [particle.w * math.cos(math.radians(particle.theta)), particle.w * math.sin(math.radians(particle.theta))] angles.append(v) theta = sum_vectors(angles) orientation_tuple = tf.transformations.quaternion_from_euler(0,0,theta) self.robot_pose = Pose(position=Point(x=x,y=y),orientation=Quaternion(x=orientation_tuple[0], y=orientation_tuple[1], z=orientation_tuple[2], w=orientation_tuple[3])) def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return for particle in self.particle_cloud: r1 = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2] d = math.sqrt((delta[0]**2) + (delta[1]**2)) particle.theta += r1 % 360 particle.x += d * math.cos(particle.theta) + normal(0,0.1) particle.y += d * math.sin(particle.theta) + normal(0,0.1) particle.theta += (delta[2] - r1 + normal(0,0.1)) % 360 # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized self.normalize_particles() newParticles = [] for i in range(len(self.particle_cloud)): # resample the same # of particles choice = random_sample() # all the particle weights sum to 1 csum = 0 # cumulative sum for particle in self.particle_cloud: csum += particle.w if csum >= choice: # if the random choice fell within the particle's weight newParticles.append(deepcopy(particle)) break self.particle_cloud = newParticles def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ for particle in self.particle_cloud: tot_prob = 0 for index, scan in enumerate(msg.ranges): x,y = self.transform_scan(particle,scan,index) # transform scan to view of the particle d = self.occupancy_field.get_closest_obstacle_distance(x,y) # calculate nearest distance to particle's scan (should be near 0 if it's on robot) tot_prob += math.exp((-d**2)/(2*self.sigma**2)) # add probability (0 to 1) to total probability tot_prob = tot_prob/len(msg.ranges) # normalize total probability back to 0-1 particle.w = tot_prob # assign particles weight def transform_scan(self, particle, distance, theta): """ Calculates the x and y of a scan from a given particle particle: Particle object distance: scan distance (from ranges) theta: scan angle (range index) """ return (particle.x + distance * math.cos(math.radians(particle.theta + theta)), particle.y + distance * math.sin(math.radians(particle.theta + theta))) @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) rad = 1 # meters self.particle_cloud = [] self.particle_cloud.append(Particle(xy_theta[0], xy_theta[1], xy_theta[2])) for i in range(self.n_particles-1): # initial facing of the particle theta = random.random() * 360 # compute params to generate x,y in a circle other_theta = random.random() * 360 radius = random.random() * rad # x => straight ahead x = radius * math.sin(other_theta) + xy_theta[0] y = radius * math.cos(other_theta) + xy_theta[1] particle = Particle(x,y,theta) self.particle_cloud.append(particle) self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ tot_weight = sum([particle.w for particle in self.particle_cloud]) or 1 for particle in self.particle_cloud: particle.w = particle.w/tot_weight; def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) marker_array = [] for index, particle in enumerate(self.particle_cloud): marker = Marker(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), pose=particle.as_pose(), type=0, scale=Vector3(x=particle.w*2,y=particle.w*1,z=particle.w*5), id=index, color=ColorRGBA(r=1,a=1)) marker_array.append(marker) self.marker_pub.publish(MarkerArray(markers=marker_array)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles() # resample particles to focus on areas of high density self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation), header=Header(stamp=msg.header.stamp,frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class ParticleFilter: def __init__(self): # once everything is setup initialized will be set to true self.initialized = False # initialize this particle filter node rospy.init_node('turtlebot3_particle_filter') # set the topic names and frame names self.base_frame = "base_footprint" self.map_topic = "map" self.odom_frame = "odom" self.scan_topic = "scan" # inialize our map self.map = OccupancyGrid() # create LikelihoodField object self.likelihood_field = LikelihoodField() # the number of particles used in the particle filter self.num_particles = 10000 # initialize the particle cloud array self.particle_cloud = [] # initialize the estimated robot pose self.robot_estimate = Pose() # set threshold values for linear and angular movement before we preform an update self.lin_mvmt_threshold = 0.2 self.ang_mvmt_threshold = (np.pi / 6) self.odom_pose_last_motion_update = None # Setup publishers and subscribers # publish the current particle cloud self.particles_pub = rospy.Publisher("particle_cloud", PoseArray, queue_size=10) # publish the estimated robot pose self.robot_estimate_pub = rospy.Publisher("estimated_robot_pose", PoseStamped, queue_size=10) # subscribe to the map server rospy.Subscriber(self.map_topic, OccupancyGrid, self.get_map) # subscribe to the lidar scan from the robot rospy.Subscriber(self.scan_topic, LaserScan, self.robot_scan_received) # enable listening for and broadcasting corodinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() # sleep to get map data before initializing particle cloud rospy.sleep(1) # intialize the particle cloud self.initialize_particle_cloud() self.initialized = True def get_map(self, data): self.map = data def initialize_particle_cloud(self): """ Initialize the particle cloud with random locations and orientations throughout the house """ # get map data and random indices that correspond to coordinates # with a light gray color (inside the house) map_data = self.map.data random_indices = draw_random_sample(self.num_particles, map_data, 0) # initialize variables to convert from a particle's position to # its index on the Occupancy Grid r = self.map.info.resolution x = self.map.info.origin.position.x y = self.map.info.origin.position.y w = self.map.info.width h = self.map.info.height for i in range(self.num_particles): # set pose data for particle p = Pose() p.position = Point() p.position.x = (random_indices[i] % w) * r + x p.position.y = (random_indices[i] // h) * r + y p.position.z = 0 p.orientation = Quaternion() q = quaternion_from_euler(0.0, 0.0, math.radians(360 * random_sample())) p.orientation.x = q[0] p.orientation.y = q[1] p.orientation.z = q[2] p.orientation.w = q[3] # initialize the new particle, where all will have the same weight (1.0) new_particle = Particle(p, 1.0) # append the particle to the particle cloud self.particle_cloud.append(new_particle) self.normalize_particles() self.publish_particle_cloud() def normalize_particles(self): """ Normalize the particle weights so they add up to 1 """ # initialize sum variable to normalize sum = 0 # add up all the particle weights into sum for part in self.particle_cloud: sum += part.w # reassign weights by dividing each particle's original weight by sum for part in self.particle_cloud: part.w = part.w / sum return def publish_particle_cloud(self): particle_cloud_pose_array = PoseArray() particle_cloud_pose_array.header = Header(stamp=rospy.Time.now(), frame_id=self.map_topic) particle_cloud_pose_array.poses for part in self.particle_cloud: particle_cloud_pose_array.poses.append(part.pose) self.particles_pub.publish(particle_cloud_pose_array) def publish_estimated_robot_pose(self): robot_pose_estimate_stamped = PoseStamped() robot_pose_estimate_stamped.pose = self.robot_estimate robot_pose_estimate_stamped.header = Header(stamp=rospy.Time.now(), frame_id=self.map_topic) self.robot_estimate_pub.publish(robot_pose_estimate_stamped) def resample_particles(self): """ Resample particles with probabilities proportionate to their weights """ # create an array of particle weights particle_weights = [] for part in self.particle_cloud: particle_weights.append(part.w) # sample particles with probabilities proportinate to their weights new_particle_cloud = choice(self.particle_cloud, self.num_particles, p = particle_weights) # deepcopy these newly sampled particles into particle_cloud for i in range(self.num_particles): self.particle_cloud[i] = deepcopy(new_particle_cloud[i]) return def robot_scan_received(self, data): # wait until initialization is complete if not(self.initialized): return # we need to be able to transfrom the laser frame to the base frame if not(self.tf_listener.canTransform(self.base_frame, data.header.frame_id, data.header.stamp)): return # wait for a little bit for the transform to become avaliable (in case the scan arrives # a little bit before the odom to base_footprint transform was updated) self.tf_listener.waitForTransform(self.base_frame, self.odom_frame, data.header.stamp, rospy.Duration(0.5)) if not(self.tf_listener.canTransform(self.base_frame, data.header.frame_id, data.header.stamp)): return # calculate the pose of the laser distance sensor p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=data.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # determine where the robot thinks it is based on its odometry p = PoseStamped( header=Header(stamp=data.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # we need to be able to compare the current odom pose to the prior odom pose # if there isn't a prior odom pose, set the odom_pose variable to the current pose if not self.odom_pose_last_motion_update: self.odom_pose_last_motion_update = self.odom_pose return if self.particle_cloud: # check to see if we've moved far enough to perform an update curr_x = self.odom_pose.pose.position.x old_x = self.odom_pose_last_motion_update.pose.position.x curr_y = self.odom_pose.pose.position.y old_y = self.odom_pose_last_motion_update.pose.position.y curr_yaw = get_yaw_from_pose(self.odom_pose.pose) old_yaw = get_yaw_from_pose(self.odom_pose_last_motion_update.pose) if (np.abs(curr_x - old_x) > self.lin_mvmt_threshold or np.abs(curr_y - old_y) > self.lin_mvmt_threshold or np.abs(curr_yaw - old_yaw) > self.ang_mvmt_threshold): # This is where the main logic of the particle filter is carried out self.update_particles_with_motion_model() self.update_particle_weights_with_measurement_model(data) self.normalize_particles() self.resample_particles() self.update_estimated_robot_pose() self.publish_particle_cloud() self.publish_estimated_robot_pose() self.odom_pose_last_motion_update = self.odom_pose def update_estimated_robot_pose(self): """ Based on the particles within the particle cloud, update the robot pose estimate """ # initialize sum variables to average px = py = pz = 0 ox = oy = oz = ow = 0 for part in self.particle_cloud: p = part.pose px += p.position.x py += p.position.y pz += p.position.z ox += p.orientation.x oy += p.orientation.y oz += p.orientation.z ow += p.orientation.w # update estimated robot pose by taking the average of all particle poses num = len(self.particle_cloud) self.robot_estimate.position.x = px/num self.robot_estimate.position.y = py/num self.robot_estimate.position.z = pz/num self.robot_estimate.orientation.x = ox/num self.robot_estimate.orientation.y = oy/num self.robot_estimate.orientation.z = oz/num self.robot_estimate.orientation.w = ow/num return def update_particle_weights_with_measurement_model(self, data): """ Update the particle weights using the likelihood field for range finders model """ # wait until initialization is complete if not(self.initialized): return # take into account 8 directions of data cardinal_directions_idxs = [0, 45, 90, 135, 180, 225, 270, 315] # compute the new probabilities for all particles based on model for part in self.particle_cloud: q = 1 for ang in cardinal_directions_idxs: ztk = data.ranges[ang] # if an detection is out of range, skip this angle if ztk > data.range_max: continue theta = get_yaw_from_pose(part.pose) x_ztk = part.pose.position.x + ztk * math.cos(theta + math.radians(ang)) y_ztk = part.pose.position.y + ztk * math.sin(theta + math.radians(ang)) dist = self.likelihood_field.get_closest_obstacle_distance(x_ztk, y_ztk) # if (x_ztk,y_ztk) is out of the map boundaries if math.isnan(dist): prob = 0.00001 else: prob = compute_prob_zero_centered_gaussian(dist, 0.1) # update total probability assuming z_hit=0.8, z_err=z_max=0.1 q = q * (0.8 * prob + 1) part.w = q return def update_particles_with_motion_model(self): """ Calculates how much the robot has moved using odometry and move all the particles correspondingly by the same amount with noise """ # calculate how the robot has moved curr_x = self.odom_pose.pose.position.x old_x = self.odom_pose_last_motion_update.pose.position.x dx = curr_x - old_x curr_y = self.odom_pose.pose.position.y old_y = self.odom_pose_last_motion_update.pose.position.y dy = curr_y - old_y curr_yaw = get_yaw_from_pose(self.odom_pose.pose) old_yaw = get_yaw_from_pose(self.odom_pose_last_motion_update.pose) dyaw = curr_yaw - old_yaw # move all the particles correspondingly with errors for part in self.particle_cloud: p = part.pose p.position.x += add_error(dx) p.position.y += add_error(dy) new_yaw = get_yaw_from_pose(p) + add_error(dyaw) q = quaternion_from_euler(0.0, 0.0, new_yaw) p.orientation.x = q[0] p.orientation.y = q[1] p.orientation.z = q[2] p.orientation.w = q[3] return
class Collector(): def __init__(self): rospy.init_node('collector', anonymous = False) self.lis=TransformListener(); self.data_out=SBC_Output(); rospy.Subscriber("/joint_states", JointState, self.j_callback) rospy.Subscriber("/finger1/ContactState", KCL_ContactStateStamped, self.f_callback1) rospy.Subscriber("/finger2/ContactState", KCL_ContactStateStamped, self.f_callback2) rospy.Subscriber("/pressure", PressureControl, self.p_callback) rospy.Subscriber("/prob_fail", Float64, self.prob_callback) self.publisher=rospy.Publisher("sbc_data", SBC_Output) self.point1=PointStamped() self.point2=PointStamped() self.rate=rospy.Rate(20); def getParams(self): self.data_out.D_Gain=rospy.get_param("/bhand_pid/d_gain") self.data_out.F_ref_pid=rospy.get_param("/bhand_pid/f_ref") self.data_out.I_Gain=rospy.get_param("/bhand_pid/i_gain") self.data_out.P_Gain=rospy.get_param("/bhand_pid/p_gain") self.data_out.freq=rospy.get_param("/pressure_reg/frequency") self.data_out.Beta=rospy.get_param("/bhand_sbc/beta") self.data_out.Delta=rospy.get_param("/bhand_sbc/delta") self.data_out.Eta=rospy.get_param("/bhand_sbc/eta") self.data_out.F_ref_sbc=rospy.get_param("/bhand_sbc/f_ref") def j_callback(self,data): self.joints=data; self.data_out.effort1=data.effort[1] self.data_out.effort2=data.effort[0] def f_callback1(self,data): self.data_out.Fn1=data.Fnormal; ft=np.array([data.tangential_force.x,data.tangential_force.y,data.tangential_force.z]) self.data_out.Ft1=np.sqrt(ft.dot(ft)); self.point1=PointStamped(); self.point1.header=data.header; self.point1.point=data.contact_position; def f_callback2(self,data): self.data_out.Fn2=data.Fnormal; ft=np.array([data.tangential_force.x,data.tangential_force.y,data.tangential_force.z]) self.data_out.Ft2=np.sqrt(ft.dot(ft)); self.point2=PointStamped(); self.point2.header=data.header; self.point2.point=data.contact_position; def p_callback(self,data): self.data_out.p_demand=data.p_demand; self.data_out.p_measure=data.p_measure; def prob_callback(self,data): self.data_out.Pfailure=data.data; def transform_it(self,data): if(data.header.frame_id): #data.header.stamp=rospy.Time.now(); if(self.lis.canTransform("base_link",data.header.frame_id,data.header.stamp) or True): #print(rospy.Time.now()) data.header.stamp=data.header.stamp-rospy.Duration(0.02) #point=self.lis.transformPoint("base_link", data) try: #self.lis.waitForTransform("base_link", data.header.frame_id, data.header.stamp, rospy.Duration(1)) # print(rospy.Time.now()) self.point=self.lis.transformPoint("base_link", data) return True except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn("TF problem 2") pass else: rospy.logwarn("Cannot Transform") else: print(data.header.frame_id) return False def get_distance(self,point1,point2): d=np.array([point1.x-point2.x, point1.y-point2.y, point1.z-point2.z]) return np.sqrt(d.dot(d)); def send_it(self): while not rospy.is_shutdown(): self.data_out.header.stamp=rospy.Time.now(); self.getParams() got_it=self.transform_it(self.point1); if(got_it): self.data_out.contact1=self.point.point got_it=self.transform_it(self.point2); if(got_it): self.data_out.contact2=self.point.point self.data_out.distance=self.get_distance(self.data_out.contact1,self.data_out.contact2)*100 self.publisher.publish(self.data_out); self.rate.sleep();
class ParticleFilter(object): """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 300 # the number of particles to use self.p_lost = .4 # The probability given to the robot being "lost" at any given time self.outliers_to_keep = int(self.n_particles * self.p_lost * 0.5) # The number of outliers to keep around self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # Make a ros service call to the /static_map service to get a nav_msgs/OccupancyGrid map. # Then use OccupancyField to make the map object robotMap = rospy.ServiceProxy('/static_map', GetMap)().map self.occupancy_field = OccupancyField(robotMap) print "OccupancyField initialized", self.occupancy_field self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) Our strategy is #2 to enable better tracking of unlikely particles in the future """ # first make sure that the particle weights are normalized self.normalize_particles() chosen_one = max(self.particle_cloud, key=lambda p: p.w) # TODO: assign the lastest pose into self.robot_pose as a geometry_msgs.Pose object # just to get started we will fix the robot's pose to always be at the origin self.robot_pose = chosen_one.as_pose() def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], angle_diff(new_odom_xy_theta[2], self.current_odom_xy_theta[2])) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return for i, particle in enumerate(self.particle_cloud): # TODO: Change odometry uncertainty to be ROS param # Calculate the angle difference between the old odometry position # and the old particle position. Then create a rotation matrix between # the two angles rotationmatrix = self.make_rotation_matrix(particle.theta - old_odom_xy_theta[2]) # rotate the motion vector, add the result to the particle rotated_delta = np.dot(rotationmatrix, delta[:2]) linear_randomness = np.random.normal(1, 0.2) angular_randomness = np.random.uniform(particle.turn_multiplier, 0.3) particle.x += rotated_delta[0] * linear_randomness particle.y += rotated_delta[1] * linear_randomness particle.theta += delta[2] * angular_randomness # Make sure the particle's angle doesn't wrap particle.theta = angle_diff(particle.theta, 0) def make_rotation_matrix(self, theta): """ make_rotation_matrix returns a rotation matrix given angle theta Args: theta (number): the angle of rotation in radians CCW Returns: ndarray: a two by two rotation matrix """ sinTheta = np.sin(theta) cosTheta = np.cos(theta) return np.array([[cosTheta, -sinTheta], [sinTheta, cosTheta]]) def map_calc_range(self, x, y, theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def lost_particles(self): """ lost_particles predicts which paricles are "lost" using unsupervised outlier detection. In this case, we choose to use Scikit Learn - OneClassSVM Args: Returns: inliers = particles that are not lost outlier = particles that are lost """ # First format training data x = [p.x for p in self.particle_cloud] y = [p.y for p in self.particle_cloud] X_train = np.array(zip(x, y)) # Next make unsupervised outlier detection model # We have chosen to use OneClassSVM # Lower nu to detect fewer outliers # Here, we use 1/2 of the lost probability : self.p_lost / 2.0 clf = OneClassSVM(nu=.3, kernel="rbf", gamma=0.1) clf.fit(X_train) # Predict inliers and outliers y_pred_train = clf.predict(X_train) # Create inlier and outlier particle lists inliers = [] outliers = [] # Iterate through particles and predictions to populate lists for p, pred in zip(self.particle_cloud, y_pred_train): if pred == 1: inliers.append(p) elif pred == -1: outliers.append(p) return inliers, outliers def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # TODO: Dynamically decide how many particles we need # make sure the distribution is normalized self.normalize_particles() # Calculate inlaying and exploring particle sets inliers, outliers = self.lost_particles() desired_outliers = int(self.n_particles * self.p_lost) desired_inliers = int(self.n_particles - desired_outliers) # Calculate the average turn_multiplier of the inliers mean_turn_multipler = np.mean([p.turn_multiplier for p in inliers]) print "Estimated turn multiplier:", mean_turn_multipler # Recalculate inliers probabilities = [p.w for p in self.particle_cloud] new_inliers = self.draw_random_sample(self.particle_cloud, probabilities, desired_inliers) # Recalculate outliers # This keeps some number of outlying particles around unchanged, and spreads the rest randomly around the map. if desired_outliers > min(len(outliers), self.outliers_to_keep): outliers.sort(key=lambda p: p.w, reverse=True) num_to_make = desired_outliers - min(len(outliers), self.outliers_to_keep) new_outliers = outliers[:self.outliers_to_keep] + \ [Particle().generate_uniformly_on_map(self.occupancy_field.map) for _ in xrange(num_to_make)] for p in new_outliers: p.turn_multiplier = mean_turn_multipler else: new_outliers = outliers[:desired_outliers] # Set all of the weights back to the same value. Concentration of particles now reflects weight. new_particles = new_inliers + new_outliers for p in new_particles: p.w = 1.0 p.turn_multiplier = np.random.normal(p.turn_multiplier, 0.1) self.normalize_particles() self.particle_cloud = new_particles @staticmethod def laser_uncertainty_model(distErr): """ Computes the probability of the laser returning a point distance distErr from the wall. Note that this uses an exponential distribution instead of anything reasonable for computational speed. Args: distErr (float): The distance between the point returned and the nearest wall on the map (in meters) Returns: probability (float): A probability, in the range 0...1 """ # TODO: make these into rosparams k = 0.1 # meters of half-life of distance probability for main distribution probMiss = 0.05 # Base probability that the laser scan is totally confused distErr = abs(distErr) return (1 / (1 + probMiss)) * (probMiss + 1 / (distErr / k + 1)) def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg Args: msg (LaserScan): incoming message """ # Transform to cartesian coordinates scan_points = PointCloud() scan_points.header = msg.header for i, range in enumerate(msg.ranges): if range == 0: continue # Calculate point in laser coordinate frame angle = msg.angle_min + i * msg.angle_increment x = range * np.cos(angle) y = range * np.sin(angle) scan_points.points.append(Point32(x=x, y=y)) # Transform into base_link coordinates scan_points = self.tf_listener.transformPointCloud('base_link', scan_points) # For each particle... for particle in self.particle_cloud: # Create a 3x3 matrix that transforms points from the origin to the particle rotmatrix = np.matrix([[np.cos(particle.theta), -np.sin(particle.theta), 0], [np.sin(particle.theta), np.cos(particle.theta), 0], [0, 0, 1]]) transmatrix = np.matrix([[1, 0, particle.x], [0, 1, particle.y], [0, 0, 1]]) mat33 = np.dot(transmatrix, rotmatrix) # Iterate through the points in the laser scan probabilities = [] for point in scan_points.points: # Move the point onto the particle xy = np.dot(mat33, np.array([point.x, point.y, 1])) # Figure out the probability of that point distToWall = self.occupancy_field.get_closest_obstacle_distance(xy.item(0), xy.item(1)) if np.isnan(distToWall): continue probabilities.append(self.laser_uncertainty_model(distToWall)) # Combine those into probability of this scan given hypothesized location # This is the bullshit thing Paul showed # TODO: exponent should be a rosparam totalProb = np.sum([p ** 3 for p in probabilities]) / len(probabilities) # Update the particle's probability with new info particle.w *= totalProb # Normalize particles self.normalize_particles() @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities Args: choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples Returns: samples (List): A list of n elements, deep-copied from choices """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta is None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] linear_variance = 0.5 # meters angular_variance = 4 xs = np.random.normal(xy_theta[0], linear_variance, size=self.n_particles) ys = np.random.normal(xy_theta[1], linear_variance, size=self.n_particles) thetas = np.random.vonmises(xy_theta[2], angular_variance, size=self.n_particles) self.particle_cloud = [Particle(x=xs[i], y=ys[i], theta=thetas[i]) for i in xrange(self.n_particles)] self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ total = sum([p.w for p in self.particle_cloud]) if total != 0: for p in self.particle_cloud: p.w /= total # Plan: divide each by the sum of all # TODO: implement this def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform(self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return startTime = time.clock() # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) if not (self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles() # resample particles to focus on areas of high density self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles print "Calculation time: {}ms".format((time.clock() - startTime) * 1000) # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer TODO: if you want to learn a lot about tf, reimplement this... I can provide you with some hints as to what is going on here. """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose(translation, rotation), header=Header(stamp=msg.header.stamp, frame_id=self.base_frame)) self.tf_listener.waitForTransform(self.base_frame, self.odom_frame, msg.header.stamp, rospy.Duration(1.0)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not (hasattr(self, 'translation') and hasattr(self, 'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class Controller(): Manual = 0 Automatic = 1 TakeOff = 2 def __init__(self): self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.listener = TransformListener() rospy.Subscriber("joy", Joy, self._joyChanged) rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged) self.cmd_vel_telop = Twist() #self.pidX = PID(20, 12, 0.0, -30, 30, "x") #self.pidY = PID(-20, -12, 0.0, -30, 30, "y") #self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z") #self.pidYaw = PID(50.0, 0.0, 0.0, -200.0, 200.0, "yaw") self.pidX = PID(20, 12.0, 0.2, -30, 30, "x") self.pidY = PID(-20, -12.0, -0.2, -30, 30, "y") #50000 800 self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 57000, "z") self.pidYaw = PID(50.0, 0.0, 0.0, -200.0, 200.0, "yaw") self.state = Controller.Manual self.targetZ = 1 self.targetX = 0.0 self.targetY = -1.0 self.des_angle = 90.0 self.lastZ = 0.0 self.power = 50000.0 self.pubVz = rospy.Publisher('vel_z', Float32, queue_size=1) self.lastJoy = Joy() def _cmdVelTelopChanged(self, data): self.cmd_vel_telop = data if self.state == Controller.Manual: self.pubNav.publish(data) def pidReset(self): self.pidX.reset() self.pidZ.reset() self.pidZ.reset() self.pidYaw.reset() def _joyChanged(self, data): if len(data.buttons) == len(self.lastJoy.buttons): delta = np.array(data.buttons) - np.array(self.lastJoy.buttons) print ("Buton ok") #Button 1 if delta[0] == 1 and self.state != Controller.Automatic: print("Automatic!") #thrust = self.cmd_vel_telop.linear.z #print(thrust) self.pidReset() self.pidZ.integral = self.power/self.pidZ.ki self.lastZ = 0.0 #self.targetZ = 1 self.state = Controller.Automatic #Button 2 if delta[1] == 1 and self.state != Controller.Manual: print("Manual!") self.pubNav.publish(self.cmd_vel_telop) self.state = Controller.Manual #Button 3 if delta[2] == 1: self.state = Controller.TakeOff print("TakeOff!") #Button 5 if delta[4] == 1: self.targetY = 0.0 self.des_angle = -90.0 #print(self.targetZ) #self.power += 100.0 print(self.power) #self.state = Controller.Automatic #Button 6 if delta[5] == 1: self.targetY = -1.0 self.des_angle = 90.0 #print(self.targetZ) #self.power -= 100.0 print(self.power) #self.state = Controller.Automatic self.lastJoy = data def run(self): thrust = 0 print("jello") while not rospy.is_shutdown(): if self.state == Controller.TakeOff: t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark3") if self.listener.canTransform("/mocap", "/Nano_Mark3", t): position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark3", t) print(position[0],position[1],position[2]) if position[2] > 0.2 or thrust > 54000: self.pidReset() #self.pidZ.integral = thrust / self.pidZ.ki #self.targetZ = 0.5 self.state = Controller.Automatic thrust = 0 else: thrust += 100 self.power = thrust msg = self.cmd_vel_telop msg.linear.z = thrust self.pubNav.publish(msg) if self.state == Controller.Automatic: # transform target world coordinates into local coordinates t = self.listener.getLatestCommonTime("/mocap","/Nano_Mark3") seconds = rospy.get_time() print(t) if self.listener.canTransform("/mocap","/Nano_Mark3", t): position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark3",t) #print(position[0],position[1],position[2]) euler = tf.transformations.euler_from_quaternion(quaternion) #print(euler[2]*(180/math.pi)) msg = self.cmd_vel_telop #print(self.power) if self.lastZ == 0.0: self.lastZ = position[2] last_time = seconds; else: dh = self.targetZ - position[2] v_z = (position[2]-self.lastZ)/((seconds-last_time)) #print(v_z) self.pubVz.publish(v_z) #por encima de goal if dh<0.0: self.power -=50 #if v_z>0.0: # self.power -=50 #else: #self.power +=50 #por debajo de goal if dh>0.0: if self.power > 57000.0: self.power = 57000.0 else: self.power += 50 #if v_z<0.0: # self.power +=50 # else: # self.power -=50 print(self.power) msg.linear.z = self.power #Descompostion of the x and y contributions following the Z-Rotation x_prim = self.pidX.update(0.0, self.targetX-position[0]) y_prim = self.pidY.update(0.0,self.targetY-position[1]) msg.linear.x = x_prim*math.cos(euler[2]) - y_prim*math.sin(euler[2]) msg.linear.y = x_prim*math.sin(euler[2]) + y_prim*math.cos(euler[2]) #---old stuff--- #msg.linear.x = self.pidX.update(0.0, 0.0-position[0]) #msg.linear.y = self.pidY.update(0.0,-1.0-position[1]) #msg.linear.z = self.pidZ.update(position[2],1.0) #z_prim = self.pidZ.update(0.0,self.targetZ-position[2]) #print(z_prim) #if z_prim < self.power: # msg.linear.z = self.power #else: # msg.linear.z = z_prim #msg.linear.z = z_prim #msg.linear.z = self.pidZ.update(0.0,1.0-position[2]) #self.pidZ.update(position[2], self.targetZ) #msg.angular.z = self.pidYaw.update(0.0,self.des_angle + euler[2]*(180/math.pi)) print(msg.linear.x,msg.linear.y,msg.linear.z,msg.angular.z) #print(euler[2]) #print(msg.angular.z) self.pubNav.publish(msg) rospy.sleep(0.01)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 300 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid # TODO: fill in the appropriate service call here. The resultant map should be assigned be passed # into the init method for OccupancyField gettingMap = rospy.ServiceProxy('static_map',GetMap) myMap = gettingMap().map # for now we have commented out the occupancy field initialization until you can successfully fetch the map self.occupancy_field = OccupancyField(myMap) self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() # TODO: assign the lastest pose into self.robot_pose as a geometry_msgs.Pose object # just to get started we will fix the robot's pose to always be at the origin self.robot_pose = Pose() best_particle = Particle(0,0,0,0) for particle in self.particle_cloud: if particle.w > best_particle.w: best_particle = particle self.robot_pose = best_particle.as_pose() #print "updated pose" + str(self.robot_pose) def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return for particle in self.particle_cloud: psi = math.atan2(delta[1],delta[0])- old_odom_xy_theta[2] r = math.sqrt((delta[0])**2 + (delta[1])**2) particle.x=particle.x+ r*math.cos(old_odom_xy_theta[2]+psi) particle.y = particle.y + r*math.sin(old_odom_xy_theta[2]+psi) particle.theta = old_odom_xy_theta[2] + delta[2] # TODO: modify particles using delta # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized self.normalize_particles() # TODO: fill out the rest of the implementation new_particles = [] probabilities = [] for particle in self.particle_cloud: probabilities.append(particle.w) new_particles = ParticleFilter.draw_random_sample(self.particle_cloud, probabilities, len(self.particle_cloud)) self.particle_cloud = new_particles print 'Particle cloud element 0' + '%s' %str(self.particle_cloud[0].x) + str(self.particle_cloud[0].y) print 'Particle cloud element 1' + '%s' %str(self.particle_cloud[1].x) + str(self.particle_cloud[1].y) def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ # TODO: implement this #print str(msg.range[0]) #We ctually need to go through all of the range so a for loop form 0 to 360 #We also need to delete any ranges that return 0 because that is a false reading and causes problems #print msg for part in self.particle_cloud: total_distace = 0 average_distance = 0 count = 0 for angle in range(359): distance_in_front = msg.ranges[angle] if distance_in_front == 0: pass else: count +=1 rad = angle/360 * 2 * math.pi part.x = part.x + distance_in_front*math.cos(part.theta + rad) part.y = part.y + distance_in_front*math.sin(part.theta + rad) distance = OccupancyField.get_closest_obstacle_distance(self.occupancy_field, part.x, part.y) total_distace += distance average_distance = total_distace/count part.w=(math.e**((-average_distance)**2)) #pass @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] #one particle at origin #self.particle_cloud.append(Particle(0,0,0)) # TODO create particles for i in range(self.n_particles): self.particle_cloud.append(Particle(randn(), randn(),randn())) #add robot location guess to each number self.normalize_particles() self.update_robot_pose() #print self.particle_cloud def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ pass # TODO: implement this #add up all weights of all particles, divide each particle by this number sumWeight = 0 for particle in self.particle_cloud: sumWeight += particle.w for particle in self.particle_cloud: particle.w /=sumWeight def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles() # resample particles to focus on areas of high density self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation), header=Header(stamp=msg.header.stamp,frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 500 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model self.sigma = 0.1 # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] rospy.wait_for_service("static_map") static_map = rospy.ServiceProxy("static_map", GetMap) try: map = static_map().map except: print("Could not receive map") # for now we have commented out the occupancy field initialization until you can successfully fetch the map self.occupancy_field = OccupancyField(map) self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() mean_x = 0 mean_y = 0 mean_theta = 0 mean_x_vector = 0 mean_y_vector = 0 for p in self.particle_cloud: mean_x += p.x*p.w mean_y += p.y*p.w mean_x_vector += math.cos(p.theta)*p.w mean_y_vector += math.sin(p.theta)*p.w mean_theta = math.atan2(mean_y_vector, mean_x_vector) self.robot_pose = Particle(x=mean_x,y=mean_y,theta=mean_theta).as_pose() def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = {'x': new_odom_xy_theta[0] - self.current_odom_xy_theta[0], 'y': new_odom_xy_theta[1] - self.current_odom_xy_theta[1], 'theta': new_odom_xy_theta[2] - self.current_odom_xy_theta[2]} delta['r'] = math.sqrt(delta['x']**2 + delta['y']**2) delta['rot'] = angle_diff(math.atan2(delta['y'],delta['x']), old_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return for p in self.particle_cloud: p.x += delta['r']*math.cos(delta['rot'] + p.theta) p.y += delta['r']*math.sin(delta['rot'] + p.theta) p.theta += delta['theta'] def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO(NOPE): nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized self.normalize_particles() indices = [i for i in range(len(self.particle_cloud))] probs = [p.w for p in self.particle_cloud] # print('b') # print(probs) new_indices = self.draw_random_sample(choices=indices, probabilities=probs, n=(self.n_particles)) new_particles = [] for i in new_indices: clean_index = int(i) old_particle = self.particle_cloud[clean_index] new_particles.append(Particle(x=old_particle.x+gauss(0,.05),y=old_particle.y+gauss(0,.05),theta=old_particle.theta+gauss(0,.05))) self.particle_cloud = new_particles self.normalize_particles() def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ for p in self.particle_cloud: weight_sum = 0 for i in range(360): n_o = p.nearest_obstacle(i, msg.ranges[i]) error = self.occupancy_field.get_closest_obstacle_distance(n_o[0], n_o[1]) weight_sum += math.exp(-error*error/(2*self.sigma**2)) p.w = weight_sum / 360 # print(p.w) self.normalize_particles() @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] for i in range(self.n_particles): self.particle_cloud.append(Particle(x=xy_theta[0]+gauss(0,0.25),y=xy_theta[1]+gauss(0,0.25),theta=xy_theta[2]+gauss(0,0.25))) self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ w = [deepcopy(p.w) for p in self.particle_cloud] z = sum(w) print(z) if z > 0: for i in range(len(self.particle_cloud)): self.particle_cloud[i].w = w[i] / z else: for i in range(len(self.particle_cloud)): self.particle_cloud[i].w = 1/len(self.particle_cloud) print(sum([p.w for p in self.particle_cloud])) def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles() # resample particles to focus on areas of high density self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation), header=Header(stamp=msg.header.stamp,frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class Controller(): Manual = 0 Automatic = 1 TakeOff = 2 Land = 3 def __init__(self): self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.listener = TransformListener() rospy.Subscriber("joy", Joy, self._joyChanged) rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged) self.cmd_vel_telop = Twist() #self.pidX = PID(20, 12, 0.0, -30, 30, "x") #self.pidY = PID(-20, -12, 0.0, -30, 30, "y") #self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z") #self.pidYaw = PID(50.0, 0.0, 0.0, -200.0, 200.0, "yaw") self.pidX = PID(20, 12, 0.0, -20, 20, "x") self.pidY = PID(-20, -12, 0.0, -20, 20, "y") #50000 800 self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z") self.pidYaw = PID(50.0, 0.0, 0.0, -100.0, 100.0, "yaw") self.state = Controller.Manual #Target Values self.pubtarX = rospy.Publisher('target_x', Float32, queue_size=1) self.pubtarY = rospy.Publisher('target_y', Float32, queue_size=1) self.pubtarZ = rospy.Publisher('target_z', Float32, queue_size=1) self.targetX = 0.0 self.targetY = 0.0 self.targetZ = 0.5 self.des_angle = 0.0 #self.power = 50000.0 #Actual Values self.pubrealX = rospy.Publisher('real_x', Float32, queue_size=1) self.pubrealY = rospy.Publisher('real_y', Float32, queue_size=1) self.pubrealZ = rospy.Publisher('real_z', Float32, queue_size=1) self.lastJoy = Joy() #Path view self.pubPath = rospy.Publisher('cf_Uni_path', MarkerArray, queue_size=100) self.path = MarkerArray() #self.p = [] #Square trajectory self.square_start = False self.square_pos = 0 #self.square =[[0.5,0.5,0.5,0.0], # [0.5,-0.5,0.5,90.0], # [-0.5,-0.5,0.5,180.0], # [-0.5,0.5,0.5,270.0]] #landing flag self.land_flag = False self.power = 0.0 def _cmdVelTelopChanged(self, data): self.cmd_vel_telop = data if self.state == Controller.Manual: self.pubNav.publish(data) def pidReset(self): self.pidX.reset() self.pidZ.reset() self.pidZ.reset() self.pidYaw.reset() def square_go(self): if self.square_start == False: self.square_pos = 0 self.targetX = square[self.square_pos][0] self.targetY = square[self.square_pos][1] self.targetZ = square[self.square_pos][2] self.des_angle = square[self.square_pos][3] self.square_pos = self.square_pos + 1 self.square_start = True else: self.targetX = square[self.square_pos][0] self.targetY = square[self.square_pos][1] self.targetZ = square[self.square_pos][2] self.des_angle = square[self.square_pos][3] self.square_pos = self.square_pos + 1 if self.square_pos == 4: self.square_pos = 0 def _joyChanged(self, data): if len(data.buttons) == len(self.lastJoy.buttons): delta = np.array(data.buttons) - np.array(self.lastJoy.buttons) print ("Buton ok") #Button 1 if delta[0] == 1 and self.state != Controller.Automatic: print("Automatic!") self.land_flag = False #thrust = self.cmd_vel_telop.linear.z #print(thrust) self.pidReset() self.pidZ.integral = 40.0 #self.targetZ = 1 self.state = Controller.Automatic #Button 2 if delta[1] == 1 and self.state != Controller.Manual: print("Manual!") self.land_flag = False self.pubNav.publish(self.cmd_vel_telop) self.state = Controller.Manual #Button 3 if delta[2] == 1: self.land_flag = False self.state = Controller.TakeOff print("TakeOff!") #Button 4 if delta[3] == 1: self.land_flag = True print("Landing!") self.square_start = False self.targetX = 0.0 self.targetY = 0.0 self.targetZ = 0.4 self.des_angle = 0.0 self.state = Controller.Automatic #Button 5 if delta[4] == 1: self.square_go() #self.targetX = square[0][0] #self.targetY = square[0][1] #self.targetZ = square[0][2] #self.des_angle = square[0][3] #print(self.targetZ) #self.power += 100.0 #print(self.power) self.state = Controller.Automatic #Button 6 if delta[5] == 1: self.square_start = False self.targetX = 0.0 self.targetY = 0.0 self.targetZ = 0.5 self.des_angle = 0.0 #print(self.targetZ) #self.power -= 100.0 #print(self.power) self.state = Controller.Automatic self.lastJoy = data def run(self): thrust = 0 print("jello") while not rospy.is_shutdown(): if self.state == Controller.TakeOff: t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark_Gon4") print(t,self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t)) if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t): position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4", t) print(position[0],position[1],position[2]) #if position[2] > 2.0 or thrust > 54000: if thrust > 55000: self.pidReset() self.pidZ.integral = thrust / self.pidZ.ki #self.targetZ = 0.5 self.state = Controller.Automatic thrust = 0 else: thrust += 500 #self.power = thrust msg = self.cmd_vel_telop msg.linear.z = thrust self.pubNav.publish(msg) if self.state == Controller.Land: t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark_Gon4") if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t): position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4", t) if position[2] > 0.05: msg_land = self.cmd_vel_telop self.power -=100 msg_land.linear.z = self.power self.pubNav.publish(msg_land) else: msg_land = self.cmd_vel_telop msg_land.linear.z = 0 self.pubNav.publish(msg_land) if self.state == Controller.Automatic: # transform target world coordinates into local coordinates t = self.listener.getLatestCommonTime("/mocap","/Nano_Mark_Gon4") print(t) if self.listener.canTransform("/mocap","/Nano_Mark_Gon4", t): position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4",t) #print(position[0],position[1],position[2]) euler = tf.transformations.euler_from_quaternion(quaternion) print(euler[2]*(180/math.pi)) msg = self.cmd_vel_telop #print(self.power) #Descompostion of the x and y contributions following the Z-Rotation x_prim = self.pidX.update(0.0, self.targetX-position[0]) y_prim = self.pidY.update(0.0,self.targetY-position[1]) msg.linear.x = x_prim*math.cos(euler[2]) - y_prim*math.sin(euler[2]) msg.linear.y = x_prim*math.sin(euler[2]) + y_prim*math.cos(euler[2]) #---old stuff--- #msg.linear.x = self.pidX.update(0.0, 0.0-position[0]) #msg.linear.y = self.pidY.update(0.0,-1.0-position[1]) #msg.linear.z = self.pidZ.update(position[2],1.0) #z_prim = self.pidZ.update(position[2],self.targetZ) #print(z_prim) #if z_prim < self.power: # msg.linear.z = self.power #else: # msg.linear.z = z_prim #msg.linear.z = self.power #print(self.power) msg.linear.z = self.pidZ.update(0.0,self.targetZ-position[2]) #self.pidZ.update(position[2], self.targetZ) msg.angular.z = self.pidYaw.update(0.0,self.des_angle*(math.pi/180) + euler[2])#*(180/math.pi)) #msg.angular.z = self.pidYaw.update(0.0,self.des_angle - euler[2])#*(180/math.pi)) print(msg.linear.x,msg.linear.y,msg.linear.z,msg.angular.z) #print(euler[2]) #print(msg.angular.z) self.pubNav.publish(msg) #Publish Real and Target position self.pubtarX.publish(self.targetX) self.pubtarY.publish(self.targetY) self.pubtarZ.publish(self.targetZ) self.pubrealX.publish(position[0]) self.pubrealY.publish(position[1]) self.pubrealZ.publish(position[2]) #change square point if abs(self.targetX-position[0])<0.08 and \ abs(self.targetY-position[1])<0.08 and \ abs(self.targetZ-position[2])<0.08 and \ self.square_start == True: self.square_go() #Landing if abs(self.targetX-position[0])<0.1 and \ abs(self.targetY-position[1])<0.1 and \ abs(self.targetZ-position[2])<0.1 and \ self.land_flag == True: self.state = Controller.Land self.power = msg.linear.z #Publish Path #point = Marker() #line = Marker() #point.header.frame_id = line.header.frame_id = 'mocap' #POINTS #point.action = point.ADD #point.pose.orientation.w = 1.0 #point.id = 0 #point.type = point.POINTS #point.scale.x = 0.01 #point.scale.y = 0.01 #point.color.g = 1.0 #point.color.a = 1.0 #LINE #line.action = line.ADD #line.pose.orientation.w = 1.0 #line.id = 1 #line.type = line.LINE_STRIP #line.scale.x = 0.01 #line.color.g = 1.0 #line.color.a = 1.0 #p = Point() #p.x = position[0] #p.y = position[1] #p.z = position[2] #point.points.append(p) # line.points.append(p) #self.path.markers.append(p) #id = 0 #for m in self.path.markers: # m.id = id # id += 1 #self.pubPath.publish(self.path) #self.pubPath.publish(point) #self.pubPath.publish(line) point = Marker() point.header.frame_id = 'mocap' point.type = point.SPHERE #points.header.stamp = rospy.Time.now() point.ns = 'cf_Uni_path' point.action = point.ADD #points.id = 0; point.scale.x = 0.005 point.scale.y = 0.005 point.scale.z = 0.005 point.color.a = 1.0 point.color.r = 1.0 point.color.g = 1.0 point.color.b = 0.0 point.pose.orientation.w = 1.0 point.pose.position.x = position[0] point.pose.position.y = position[1] point.pose.position.z = position[2] self.path.markers.append(point) id = 0 for m in self.path.markers: m.id = id id += 1 self.pubPath.publish(self.path) #point = Point() #point.x = position[0] #point.y = position[1] #point.z = position[2] #points.points.append(point) #self.p.append(pos2path) #self.path.header.stamp = rospy.Time.now() #self.path.header.frame_id = 'mocap' #self.path.poses = self.p #self.pubPath.publish(points) rospy.sleep(0.01)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid robot_pose: estimated position of the robot of type geometry_msgs/Pose """ # some constants! :) -emily and franz TAU = math.pi * 2.0 # to be used in update_particles_with_odom RADIAL_SIGMA = .03 # meters ORIENTATION_SIGMA = 0.03 * TAU def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 30 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # TODO: define additional constants if needed #set self.visualize_weights to True if you want to see a plot of xpos vs weights every time the particles are updated self.visualize_weights = True # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.rawcloud_pub = rospy.Publisher("rawcloud", PoseArray, queue_size=1) self.odomcloud_pub = rospy.Publisher("odomcloud", PoseArray, queue_size=1) self.lasercloud_pub = rospy.Publisher("lasercloud", PoseArray, queue_size=1) self.resamplecloud_pub = rospy.Publisher("resamplecloud", PoseArray, queue_size=1) self.finalcloud_pub = rospy.Publisher("finalcloud", PoseArray, queue_size=1) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid get_static_map = rospy.ServiceProxy('static_map', GetMap) self.occupancy_field = OccupancyField(get_static_map().map) self.robot_pose = Pose() self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (level 2) (2): compute the most likely pose (i.e. the mode of the distribution) (level 1) """ # first make sure that the particle weights are normalized self.normalize_particles() # compute mean pose by calculating the weighted average of each position and angle mean_x = 0 mean_y = 0 mean_theta = 0 for particle in self.particle_cloud: mean_x += particle.w * particle.x mean_y += particle.w * particle.y mean_theta += particle.w * particle.theta mean_particle = Particle(mean_x, mean_y, mean_theta) self.robot_pose = mean_particle.as_pose() def update_particles_with_odom(self, msg): """ Implement a simple version of this (Level 1) or a more complex one (Level 2) """ new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = ( new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return r1 = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2] delta_distance = np.linalg.norm([delta[0], delta[1]]) r2 = delta[2] - r1 for particle in self.particle_cloud: # randomly pick the deltas for radial distance, mean angle, and orientation angle delta_random_radius = np.random.normal(0, ParticleFilter.RADIAL_SIGMA) delta_random_mean_angle = random_sample() * ParticleFilter.TAU / 2.0 delta_random_orient_angle = np.random.normal(0, ParticleFilter.ORIENTATION_SIGMA) # calculate the deltas delta_random_x = delta_random_radius * math.cos(delta_random_mean_angle) delta_random_y = delta_random_radius * math.sin(delta_random_mean_angle) # update the mean (add deltas) particle.theta += r1 particle.x += math.cos(particle.theta) * delta_distance + delta_random_x particle.y += math.sin(particle.theta) * delta_distance + delta_random_y particle.theta += r2 + delta_random_orient_angle # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) def map_calc_range(self, x, y, theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights """ # make sure the distribution is normalized self.normalize_particles() probabilities = [particle.w for particle in self.particle_cloud] new_particle_cloud = [] for i in range(self.n_particles): random_particle = deepcopy(np.random.choice(self.particle_cloud, p=probabilities)) new_particle_cloud.append(random_particle) self.particle_cloud = new_particle_cloud def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ # compare the distance to the closest occupied location # of the hypothesis and laser scan measurement # give it a weight inversely proportional to the error valid_ranges = self.filter_laser(msg.ranges) for particle in self.particle_cloud: total_probability_density = 1 for angle in valid_ranges: radius = valid_ranges[angle] angle = (angle+.25*ParticleFilter.TAU) % ParticleFilter.TAU x = math.cos(angle+particle.theta) * radius + particle.x y = math.sin(angle+particle.theta) * radius + particle.y dist_to_nearest_neighbor = self.occupancy_field.get_closest_obstacle_distance(x, y) # calculate probability of nearest neighbor's distance probability_density = normal(dist_to_nearest_neighbor, .05) total_probability_density *= 1 + probability_density #the 1+ is hacky # TODO: make the total_probability_density function more legit particle.w = total_probability_density # rospy.loginfo(particle.w) def visualize_p_weights(self): """ Produces a plot of particle weights vs. x position """ # close any figures that are open plt.close('all') # initialize the things xpos = np.zeros(len(self.particle_cloud)) weights = np.zeros(len(self.particle_cloud)) x_i = 0 weights_i = 0 # grab the current values for p in self.particle_cloud: xpos[x_i] = p.x weights[weights_i] = p.w x_i += 1 weights_i += 1 # plotting current xpos and weights fig = plt.figure() plt.xlabel('xpos') plt.ylabel('weights') plt.title('xpos vs weights') plt.plot(xpos, weights, 'ro') plt.show(block=False) @staticmethod def angle_normalize(z): """ convenience function to map an angle to the range [-pi,pi] """ return math.atan2(math.sin(z), math.cos(z)) @staticmethod def angle_diff(a, b): """ Calculates the difference between angle a and angle b (both should be in radians) the difference is always based on the closest rotation from angle a to angle b examples: angle_diff(.1,.2) -> -.1 angle_diff(.1, 2*math.pi - .1) -> .2 angle_diff(.1, .2+2*math.pi) -> -.1 """ a = ParticleFilter.angle_normalize(a) b = ParticleFilter.angle_normalize(b) d1 = a - b d2 = 2 * math.pi - math.fabs(d1) if d1 > 0: d2 *= -1.0 if math.fabs(d1) < math.fabs(d2): return d1 else: return d2 @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements form the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self): """ Initialize the particle cloud. Arguments """ rospy.loginfo("initialize particle cloud") self.particle_cloud = [] map_info = self.occupancy_field.map.info for i in range(self.n_particles): x = random_sample()* map_info.width * map_info.resolution * 0.1 if random_sample() > 0.5: x = -x y = random_sample()* map_info.height * map_info.resolution * 0.1 if random_sample() > 0.5: y = -y theta = random_sample() * math.pi*2 self.particle_cloud.append(Particle(x, y, theta)) self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ sum = 0 for particle in self.particle_cloud: sum += particle.w for particle in self.particle_cloud: particle.w /= sum def publish_particles(self, pub): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not self.initialized: # wait for initialization to complete return if not (self.tf_listener.canTransform(self.base_frame, msg.header.frame_id, rospy.Time(0))): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node rospy.logwarn("can't transform to laser scan") return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, rospy.Time(0))): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node rospy.logwarn("can't transform to base frame") return # calculate pose of laser relative ot the robot base p = PoseStamped(header = Header(stamp = rospy.Time(0), frame_id = msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header = Header(stamp = rospy.Time(0), frame_id = self.base_frame)) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) if not self.particle_cloud: # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.publish_particles(self.rawcloud_pub) self.update_particles_with_odom(msg) # update based on odometry self.publish_particles(self.odomcloud_pub) self.update_particles_with_laser(msg) # update based on laser scan self.publish_particles(self.lasercloud_pub) self.resample_particles() # resample particles to focus on areas of high density self.update_robot_pose() # update robot's pose self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles if self.visualize_weights: self.visualize_p_weights() # publish particles (so things like rviz can see them) self.publish_particles(self.finalcloud_pub) def fix_map_to_odom_transform(self, msg): """ Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """ (translation, rotation) = TransformHelpers.convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation, rotation), header=Header(stamp=rospy.Time(0), frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not (hasattr(self, 'translation') and hasattr(self, 'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame) def filter_laser(self, ranges): """ Takes the message from a laser scan as an array and returns a dictionary of angle:distance pairs""" valid_ranges = {} for i in range(len(ranges)): if ranges[i] > 0.0 and ranges[i] < 3.5: valid_ranges[i] = ranges[i] return valid_ranges
class Controller(): ActionTakeOff = 0 ActionHover = 1 ActionLand = 2 ActionAnimation = 3 def __init__(self): self.lastNavdata = None self.lastState = State.Unknown rospy.on_shutdown(self.on_shutdown) rospy.Subscriber("ardrone/navdata", Navdata, self.on_navdata) self.pubTakeoff = rospy.Publisher('ardrone/takeoff', Empty, queue_size=1) self.pubLand = rospy.Publisher('ardrone/land', Empty, queue_size=1) self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.setFlightAnimation = rospy.ServiceProxy( 'ardrone/setflightanimation', FlightAnim) self.listener = TransformListener() self.action = Controller.ActionTakeOff self.pidX = PID(0.2, 0.12, 0.0, -0.3, 0.3, "x") self.pidY = PID(0.2, 0.12, 0.0, -0.3, 0.3, "y") self.pidZ = PID(1.0, 0, 0.0, -1.0, 1.0, "z") self.pidYaw = PID(0.5, 0, 0.0, -0.6, 0.6, "yaw") # X, Y, Z, Yaw self.goals = [ [0.0, 0.0, 2.0, math.radians(0)], "ANIM", [0.0, 0.0, 2.0, math.radians(0)], [1.0, 0.0, 1.5, math.radians(90)], [1.0, 1.0, 0.8, math.radians(180)], [-1.0, 1.0, 1.2, math.radians(0)], [1.0, 0.0, 0.8, math.radians(0)], ] self.goalIndex = 0 def on_navdata(self, data): self.lastNavdata = data if data.state != self.lastState: rospy.loginfo("State Changed: " + str(data.state)) self.lastState = data.state def on_shutdown(self): rospy.loginfo("Shutdown: try to land...") msg = Twist() for i in range(0, 1000): self.pubLand.publish() self.pubNav.publish(msg) rospy.sleep(1) def run(self): while not rospy.is_shutdown(): if self.action == Controller.ActionTakeOff: if self.lastState == State.Landed: self.pubTakeoff.publish() elif self.lastState == State.Hovering or self.lastState == State.Flying or self.lastState == State.Flying2: self.action = Controller.ActionHover elif self.action == Controller.ActionLand: msg = Twist() self.pubNav.publish(msg) self.pubLand.publish() elif self.action == Controller.ActionHover: rospy.loginfo('pid running') # transform target world coordinates into local coordinates targetWorld = PoseStamped() t = self.listener.getLatestCommonTime( "/vicon/ar_drone/ar_drone", "/world") if self.listener.canTransform("/vicon/ar_drone/ar_drone", "/world", t): targetWorld.header.stamp = t targetWorld.header.frame_id = "world" targetWorld.pose.position.x = self.goals[self.goalIndex][0] targetWorld.pose.position.y = self.goals[self.goalIndex][1] targetWorld.pose.position.z = self.goals[self.goalIndex][2] quaternion = tf.transformations.quaternion_from_euler( 0, 0, self.goals[self.goalIndex][3]) targetWorld.pose.orientation.x = quaternion[0] targetWorld.pose.orientation.y = quaternion[1] targetWorld.pose.orientation.z = quaternion[2] targetWorld.pose.orientation.w = quaternion[3] targetDrone = self.listener.transformPose( "/vicon/ar_drone/ar_drone", targetWorld) quaternion = (targetDrone.pose.orientation.x, targetDrone.pose.orientation.y, targetDrone.pose.orientation.z, targetDrone.pose.orientation.w) euler = tf.transformations.euler_from_quaternion( quaternion) # Run PID controller and send navigation message msg = Twist() #msg.linear.x = self.pidX.update(velX, targetVelX) msg.linear.x = self.pidX.update( 0.0, targetDrone.pose.position.x) msg.linear.y = self.pidY.update( 0.0, targetDrone.pose.position.y) msg.linear.z = self.pidZ.update( 0.0, targetDrone.pose.position.z) msg.angular.z = self.pidYaw.update(0.0, euler[2]) # disable hover mode msg.angular.x = 1 self.pubNav.publish(msg) if (math.fabs(targetDrone.pose.position.x) < 0.2 and math.fabs(targetDrone.pose.position.y) < 0.2 and math.fabs(targetDrone.pose.position.z) < 0.2 and math.fabs(euler[2]) < math.radians(20)): if self.goalIndex < len(self.goals) - 1: self.goalIndex += 1 if type(self.goals[self.goalIndex]) is str: msg = Twist() for i in range(0, 1000): self.pubNav.publish(msg) self.setFlightAnimation(8, 0) rospy.sleep(1.0) #for i in range(0, 1000): # self.pubNav.publish(msg) #rospy.sleep(1.5) self.goalIndex += 1 rospy.loginfo("Next Goal (X,Y,Z,Yaw): " + str(self.goals[self.goalIndex])) else: pass #self.action = Controller.ActionLand rospy.sleep(0.01)
class Controller(): ActionTakeOff = 0 ActionHover = 1 ActionLand = 2 ActionAnimation = 3 def __init__(self): self.lastNavdata = Navdata() self.lastImu = Imu() self.lastMag = Vector3Stamped() self.current_pose = PoseStamped() self.current_odom = Odometry() self.lastState = State.Unknown self.command = Twist() self.drone_msg = ARDroneData() self.cmd_freq = 1.0 / 200.0 self.drone_freq = 1.0 / 200.0 self.action = Controller.ActionTakeOff self.previousDebugTime = rospy.get_time() self.pose_error = [0, 0, 0, 0] self.pidX = PID(0.35, 0.15, 0.025, -1, 1, "x") self.pidY = PID(0.35, 0.15, 0.025, -1, 1, "y") self.pidZ = PID(1.5, 0.1, 0.5, -1.0, 1.0, "z") self.pidYaw = PID(0.75, 0.1, 0.2, -1.0, 1.0, "yaw") self.scale = 1.0 self.goal = [-1, 0, 0, height, 0] #set it to center to start self.goal_rate = [0, 0, 0, 0, 0] # Use the update the goal on time self.current_goal = Goal( ) # Use this to store current goal, reference time-dependent self.goal_done = False self.waypoints = None rospy.on_shutdown(self.on_shutdown) rospy.Subscriber("ardrone/navdata", Navdata, self.on_navdata) rospy.Subscriber("ardrone/imu", Imu, self.on_imu) rospy.Subscriber("ardrone/mag", Vector3Stamped, self.on_mag) rospy.Subscriber("arcontroller/goal", Goal, self.on_goal) rospy.Subscriber("arcontroller/waypoints", Waypoints, self.on_waypoints) rospy.Subscriber("qualisys/ARDrone/pose", PoseStamped, self.get_current_pose) rospy.Subscriber("qualisys/ARDrone/odom", Odometry, self.get_current_odom) self.pubTakeoff = rospy.Publisher('ardrone/takeoff', Empty, queue_size=1) self.pubLand = rospy.Publisher('ardrone/land', Empty, queue_size=1) self.pubCmd = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.pubDroneData = rospy.Publisher('droneData', ARDroneData, queue_size=1) self.pubGoal = rospy.Publisher('current_goal', Goal, queue_size=1) self.setFlightAnimation = rospy.ServiceProxy( 'ardrone/setflightanimation', FlightAnim) self.commandTimer = rospy.Timer(rospy.Duration(self.cmd_freq), self.sendCommand) #self.droneDataTimer = rospy.Timer(rospy.Duration(self.drone_freq),self.sendDroneData) #self.goalTimer = rospy.Timer(rospy.Duration(self.drone_freq),self.sendCurrentGoal) self.listener = TransformListener() def get_current_pose(self, data): self.current_pose = data def get_current_odom(self, data): self.current_odom = data def on_imu(self, data): self.lastImu = data def on_mag(self, data): self.lastMag = data def on_navdata(self, data): self.lastNavdata = data if data.state != self.lastState: rospy.loginfo("State Changed: " + str(data.state)) self.lastState = data.state def on_shutdown(self): rospy.loginfo("Shutdown: try to land...") self.command = Twist() for i in range(0, 1000): self.pubLand.publish() self.pubCmd.publish(self.command) rospy.sleep(1) def on_goal(self, data): rospy.loginfo('New goal.') self.goal = [data.t, data.x, data.y, data.z, data.yaw] self.goal_done = False def sendCommand(self, event=None): self.command.linear.x = self.scale * self.pidX.update( 0.0, self.pose_error[0]) self.command.linear.y = self.scale * self.pidY.update( 0.0, self.pose_error[1]) self.command.linear.z = self.pidZ.update(0.0, self.pose_error[2]) self.command.angular.z = self.pidYaw.update(0.0, self.pose_error[3]) self.drone_msg = ARDroneData() self.drone_msg.header.stamp = rospy.get_rostime() self.drone_msg.header.frame_id = 'drone_data' self.drone_msg.cmd = self.command self.drone_msg.goal.t = rospy.get_time() self.drone_msg.goal.x = self.goal[1] self.drone_msg.goal.y = self.goal[2] self.drone_msg.goal.z = self.goal[3] self.drone_msg.goal.yaw = self.goal[4] self.drone_msg.tm = self.lastNavdata.tm self.pubDroneData.publish(self.drone_msg) self.pubCmd.publish(self.command) def sendDroneData(self, event=None): self.drone_msg = ARDroneData() self.drone_msg.header.stamp = rospy.get_rostime() self.drone_msg.header.frame_id = 'drone_data' #self.drone_msg.navdata = self.lastNavdata #self.drone_msg.imu = self.lastImu #self.drone_msg.mag = self.lastMag #self.drone_msg.pose = self.current_pose #self.drone_msg.odom = self.current_odom self.drone_msg.cmd = self.command self.drone_msg.goal.t = rospy.get_time() self.drone_msg.goal.x = self.goal[1] self.drone_msg.goal.y = self.goal[2] self.drone_msg.goal.z = self.goal[3] self.drone_msg.goal.yaw = self.goal[4] self.drone_msg.tm = self.lastNavdata.tm self.pubDroneData.publish(self.drone_msg) def sendCurrentGoal(self, event=None): current_goal = Goal() current_goal.t = rospy.get_time() current_goal.x = self.goal[1] current_goal.y = self.goal[2] current_goal.z = self.goal[3] current_goal.yaw = self.goal[4] self.pubGoal.publish(current_goal) def on_waypoints(self, data): rospy.loginfo('New waypoints.') self.waypoints = [] for d in range(data.len): self.waypoints.append([ data.waypoints[d].t, data.waypoints[d].x, data.waypoints[d].y, data.waypoints[d].z, data.waypoints[d].yaw ]) rospy.loginfo(self.waypoints) def waypoint_follower(self, points): current_index = 0 next_index = current_index + 1 rospy.loginfo(points) time_wp = [goal[0] for goal in points] self.goal = points[current_index] #get the first point delta_time_wp = time_wp[1] - time_wp[0] self.current_goal.t = points[0][0] self.current_goal.x = points[0][1] self.current_goal.y = points[0][2] self.current_goal.z = points[0][3] self.current_goal.yaw = points[0][4] self.goal_rate = [(points[1][i] - points[0][i]) / delta_time_wp for i in range(5)] minX = .05 minY = .05 time0_wp = rospy.get_time() time_previous_goal = time0_wp while True: #for i in range(0,points.len()): #goal = points[i] # transform target world coordinates into local coordinates targetWorld = PoseStamped() t = self.listener.getLatestCommonTime("/ARDrone", "/mocap") if self.listener.canTransform("/ARDrone", "/mocap", t): # Get starting time time_current_goal = rospy.get_time() diff_time_goal = time_current_goal - time_previous_goal time_previous_goal = time_current_goal # Update the continuous goal using rate*t+current_goal current_goalX = self.goal_rate[1] * diff_time_goal + self.goal[ 1] current_goalY = self.goal_rate[2] * diff_time_goal + self.goal[ 2] current_goalZ = self.goal_rate[3] * diff_time_goal + self.goal[ 3] current_goalYaw = self.goal_rate[ 4] * diff_time_goal + self.goal[4] self.goal = [ time_current_goal - time0_wp, current_goalX, current_goalY, current_goalZ, current_goalYaw ] targetWorld.header.stamp = t targetWorld.header.frame_id = "mocap" targetWorld.pose.position.x = self.goal[1] targetWorld.pose.position.y = self.goal[2] targetWorld.pose.position.z = self.goal[3] quaternion = tf.transformations.quaternion_from_euler( 0, 0, self.goal[4]) targetWorld.pose.orientation.x = quaternion[0] targetWorld.pose.orientation.y = quaternion[1] targetWorld.pose.orientation.z = quaternion[2] targetWorld.pose.orientation.w = quaternion[3] targetDrone = self.listener.transformPose( "/ARDrone", targetWorld) quaternion = (targetDrone.pose.orientation.x, targetDrone.pose.orientation.y, targetDrone.pose.orientation.z, targetDrone.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) # Define the pose_error to publish the command in fixed rate self.pose_error = [ targetDrone.pose.position.x, targetDrone.pose.position.y, targetDrone.pose.position.z, euler[2] ] # Run PID controller and send navigation message # msg = Twist() # msg.linear.x = self.scale*self.pidX.update(0.0, targetDrone.pose.position.x) # msg.linear.y = self.scale*self.pidY.update(0.0, targetDrone.pose.position.y) # msg.linear.z = self.pidZ.update(0.0, targetDrone.pose.position.z) # msg.angular.z = self.pidYaw.update(0.0, euler[2]) # # disable hover mode # msg.angular.x = 0 # self.command = msg #self.pubCmd.publish(msg) # # WE define the drone data message and publish # drone_msg = ARDroneData() # drone_msg.header.stamp = rospy.get_rostime() # drone_msg.header.frame_id = 'drone_data' # drone_msg.navdata = self.lastNavdata # drone_msg.imu = self.lastImu # drone_msg.mag = self.lastMag # drone_msg.pose = self.current_pose # drone_msg.odom = self.current_odom # drone_msg.cmd = msg # self.pubDroneData.publish(drone_msg) error_xy = math.sqrt(targetDrone.pose.position.x**2 + targetDrone.pose.position.y**2) current_time_wp = rospy.get_time() if self.goal[ 0] < 0: #-1 implying that waypoints is not time-dependent # goal t, x, y, z, yaw #self.goal = points[current_index] if (error_xy < 0.2 and math.fabs(targetDrone.pose.position.z) < 0.2 and math.fabs(euler[2]) < math.radians(5)): if (current_index < len(points) - 1): current_index += 1 self.goal = points[current_index] else: return else: diff_time_wp = current_time_wp - time0_wp if diff_time_wp <= time_wp[-1]: # Check the index of current goal based on rospy time and time vector in waypoints current_index = next(x for x, val in enumerate(time_wp) if val >= diff_time_wp) if current_index >= next_index: # Meaning current goal is passed, update new goal if (current_index < len(points) - 1): next_index = current_index + 1 self.current_goal.t = diff_time_wp self.current_goal.x = points[current_index][1] self.current_goal.y = points[current_index][2] self.current_goal.z = points[current_index][3] self.current_goal.yaw = points[current_index][ 4] self.goal_rate = [ (points[next_index][i] - points[current_index][i]) / delta_time_wp for i in range(5) ] else: self.goal_rate = [1, 0, 0, 0, 0] #self.goal = points[current_index] #self.current else: self.goal = points[-1] return #time = rospy.get_time() diff_time_log = current_time_wp - self.previousDebugTime if diff_time_log > 0.5: #log_msg = "Current pos:" + [targetDrone.pose.position.x, targetDrone.pose.position.y,targetDrone.pose.position.z] rospy.loginfo('--------------------------------------') rospy.loginfo('Control:%.2f,%.2f,%.2f,%.2f | bat: %.2f', self.command.linear.x, self.command.linear.y, self.command.linear.z, self.command.angular.z, self.lastNavdata.batteryPercent) rospy.loginfo( 'Current position: [%.2f, %.2f, %.2f, %.2f, %.2f]', diff_time_wp, self.current_pose.pose.position.x, self.current_pose.pose.position.y, self.current_pose.pose.position.z, euler[2]) rospy.loginfo('Current goal:%.2f,%.2f,%.2f,%.2f,%.2f', self.goal[0], self.goal[1], self.goal[2], self.goal[3], self.goal[4]) rospy.loginfo('Error: %.2f,%.2f,%.2f', error_xy, math.fabs(targetDrone.pose.position.z), math.fabs(euler[2])) rospy.loginfo('--------------------------------------') self.previousDebugTime = current_time_wp # if (math.fabs(targetDrone.pose.position.x) < 0.1 # and math.fabs(targetDrone.pose.position.y) < 0.1 # and math.fabs(targetDrone.pose.position.z) < 0.2 # and math.fabs(euler[2]) < math.radians(10)): # if (index < len(points)-1): # index += 1 # self.goal = points[index] # else: # return # Check time duration and sleep # durationCmd = self.cmd_freq-(rospy.get_time() - startCmdTime) # if durationCmd>0: # Meaning the execution < cmd_freq # rospy.sleep(durationCmd) def go_to_goal(self, goal): #rospy.loginfo('Going to goal') #rospy.loginfo(goal) self.goal = goal # transform target world coordinates into local coordinates targetWorld = PoseStamped() t = self.listener.getLatestCommonTime("/ARDrone", "/mocap") if self.listener.canTransform("/ARDrone", "/mocap", t): # Get starting time startCmdTime = rospy.get_time() targetWorld.header.stamp = t targetWorld.header.frame_id = "mocap" targetWorld.pose.position.x = goal[1] targetWorld.pose.position.y = goal[2] targetWorld.pose.position.z = goal[3] quaternion = tf.transformations.quaternion_from_euler( 0, 0, goal[4]) targetWorld.pose.orientation.x = quaternion[0] targetWorld.pose.orientation.y = quaternion[1] targetWorld.pose.orientation.z = quaternion[2] targetWorld.pose.orientation.w = quaternion[3] targetDrone = self.listener.transformPose("/ARDrone", targetWorld) quaternion = (targetDrone.pose.orientation.x, targetDrone.pose.orientation.y, targetDrone.pose.orientation.z, targetDrone.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) # Define pose error to publish the command in fixed rate self.pose_error = [ targetDrone.pose.position.x, targetDrone.pose.position.y, targetDrone.pose.position.z, euler[2] ] error_xy = math.sqrt(targetDrone.pose.position.x**2 + targetDrone.pose.position.y**2) time = rospy.get_time() if time - self.previousDebugTime > 1: #log_msg = "Current pos:" + [targetDrone.pose.position.x, targetDrone.pose.position.y,targetDrone.pose.position.z] rospy.loginfo('--------------------------------------') rospy.loginfo('Control:%.2f,%.2f,%.2f,%.2f,bat:%.2f', self.command.linear.x, self.command.linear.y, self.command.linear.z, self.command.angular.z, self.lastNavdata.batteryPercent) rospy.loginfo('Current goal:%.2f,%.2f,%.2f,%.2f', goal[1], goal[2], goal[3], goal[4]) rospy.loginfo('Current pose:%.2f,%.2f,%.2f', self.current_pose.pose.position.x, self.current_pose.pose.position.y, self.current_pose.pose.position.z) rospy.loginfo('Error: %.2f,%.2f,%.2f', error_xy, math.fabs(targetDrone.pose.position.z), math.fabs(euler[2])) self.previousDebugTime = time if self.goal_done: rospy.loginfo("Goal done.") rospy.loginfo('-------------------------------------') if (error_xy < 0.2 and math.fabs(targetDrone.pose.position.z) < 0.2 and math.fabs(euler[2]) < math.radians(5)): self.goal_done = True else: self.goal_done = False def run(self): while not rospy.is_shutdown(): if self.action == Controller.ActionTakeOff: if self.lastState == State.Landed: pass #rospy.loginfo('Taking off.') #self.pubTakeoff.publish() elif self.lastState == State.Hovering or self.lastState == State.Flying or self.lastState == State.Flying2: self.action = Controller.ActionHover elif self.action == Controller.ActionLand: msg = Twist() self.pubCmd.publish(msg) self.pubLand.publish() elif self.action == Controller.ActionHover: if self.waypoints == None: #if self.goal_done == False: self.go_to_goal(self.goal) else: self.waypoint_follower(self.waypoints) self.waypoints = None rospy.sleep(0.01)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self, test = False): self.test = test if self.test: print "Running particle filter in test mode" else: print "Running particle filter" self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 20 # the number of particles to use self.d_thresh = 0.1 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model self.scan_count = 0 self.robot_pose = Pose(position=Point(x=.38, y=.6096,z=0), orientation=Quaternion(x=0, y=0, z=0, w=1)) # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray) self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.ang_spread = 10.0/180.0 * math.pi self.lin_spread = .1 self.current_odom_xy_theta = [] #Set up constants for Guassian Probability self.variance = .1 self.gauss_constant = math.sqrt(2*math.pi) self.expected_value = 0 # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid # We make a fake approximate map of an empty 2 by 2 square for testing to keep it simple. # If this worked better, we'd expand to a real OccupancyGrid, but we never got it to work better. map = OccupancyGrid() map.header.frame_id = '/odom' map.info.map_load_time = rospy.Time.now() map.info.resolution = .1 # The map resolution [m/cell] map.info.width = 288 map.info.height = 288 map.data = [[0 for _ in range(map.info.height)] for _ in range(map.info.width)] for row in range(map.info.height): map.data[0][row] = 1 map.data[map.info.width-1][row] = 1 for col in range(map.info.width): map.data[col][0] = 1 map.data[col][map.info.height -1] = 1 self.occupancy_field = OccupancyField(map) if self.test: print "Initialized" self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (level 2) (2): compute the most likely pose (i.e. the mode of the distribution) (level 1) """ if self.test: print "updating robot's pose" # first make sure that the particle weights are normalized self.normalize_particles() total_x = 0.0 total_y = 0.0 total_theta = 0.0 #calculates mean position of particle cloud according to particle weight #particles are normalized so sum of multiples will return mean for particle in self.particle_cloud: total_x += particle.x * particle.w total_y += particle.y * particle.w total_theta += particle.theta * particle.w total_theta = math.cos(total_theta/2) #set the robot pose to new position self.robot_pose = Pose(position=Point(x= +total_x, y=total_y,z=0), orientation=Quaternion(x=0, y=0, z=0, w=total_theta)) if self.test: print "updated pose:" print self.robot_pose def update_particles_with_odom(self, msg): """ Implement a simple version of this (Level 1) or a more complex one (Level 2) """ if self.test: print "Updating particles from odom" new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return #function moves the particle cloud according to the odometry data #particle noise is added using gaussian distribution #standard deviation of gaussian dist was experimentally measured x_sd = .001 y_sd = .001 theta_sd = .1 for particle in self.particle_cloud: particle.x += np.random.normal(particle.x + delta[0], x_sd) particle.y += np.random.normal(particle.y + delta[1], y_sd) particle.theta += np.random.normal(particle.theta + delta[2], theta_sd) def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights """ # make sure the distribution is normalized if self.test: print "Resampling Particles" #draw a random sample of particles from particle cloud #then normalize the remaining particles weights = [particle.w for particle in self.particle_cloud] self.particle_cloud = self.draw_random_sample( self.particle_cloud, weights, self.n_particles) self.normalize_particles() def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ if self.test: print "Updating particles from laser scan" for particle in self.particle_cloud: #for each particle, get closest object distance and theta closest_particle_object_distance, closest_particle_object_theta = self.occupancy_field.get_closest_obstacle_distance(particle.x, particle.y) closest_actual_object_distance = 1000 for i in range(1, 360): if msg.ranges[i] > 0.0 and msg.ranges[i] < closest_actual_object_distance: closest_actual_object_distance = msg.ranges[i] closest_actual_object_theta = (i/360.0)*2*math.pi #update the particle's weight and theta particle.w = self.gauss_particle_probability(closest_particle_object_distance-closest_actual_object_distance) particle.theta = closest_actual_object_theta - closest_particle_object_theta def gauss_particle_probability(self, difference): """ Takes the difference between the actual closest object and the closest object to the particle guess and, based on the variance, returns the weight """ return (1/(self.variance*self.gauss_constant))*math.exp(-.5*((difference - self.expected_value)/self.variance)**2) @staticmethod def angle_normalize(z): """ convenience function to map an angle to the range [-pi,pi] """ return math.atan2(math.sin(z), math.cos(z)) @staticmethod def angle_diff(a, b): """ Calculates the difference between angle a and angle b (both should be in radians) the difference is always based on the closest rotation from angle a to angle b examples: angle_diff(.1,.2) -> -.1 angle_diff(.1, 2*math.pi - .1) -> .2 angle_diff(.1, .2+2*math.pi) -> -.1 """ a = ParticleFilter.angle_normalize(a) b = ParticleFilter.angle_normalize(b) d1 = a-b d2 = 2*math.pi - math.fabs(d1) if d1 > 0: d2 *= -1.0 if math.fabs(d1) < math.fabs(d2): return d1 else: return d2 @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements form the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each index represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [deepcopy(choices[ind]) for ind in inds] return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ if self.test: print "Updating Initial Pose" xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if self.test: print "Initializing Cloud" if xy_theta == None: xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.robot_pose) print self.robot_pose print xy_theta # raw_input() self.particle_cloud = [] # TODO create particles #create a normal distribution of particles around starting position #then normalize and update pose accordingly x_vals = np.random.normal(xy_theta[0], self.lin_spread, self.n_particles) y_vals = np.random.normal(xy_theta[1], self.lin_spread, self.n_particles) t_vals = np.random.normal(xy_theta[2], self.ang_spread, self.n_particles) self.particle_cloud = [Particle(x_vals[i], y_vals[i], t_vals[i], 1) for i in xrange(self.n_particles)] self.normalize_particles() self.update_robot_pose() # TODO(mary): create particles def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ total_weight = sum([particle.w for particle in self.particle_cloud]) * 1.0 for particle in self.particle_cloud: particle.w /= total_weight def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),frame_id=self.map_frame),poses=particles_conv)) def scan_received(self, msg): self.scan_count +=1 """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0),frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) # if self.test or self.scan_count % 1 is 0: print "updated pose:" print self.robot_pose elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles() # resample particles to focus on areas of high density self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """ (translation, rotation) = TransformHelpers.convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=msg.header.stamp,frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class Controller(): ActionTakeOff = 0 ActionHover = 1 ActionLand = 2 ActionAnimation = 3 def __init__(self): self.lastNavdata = None self.lastState = State.Unknown rospy.on_shutdown(self.on_shutdown) rospy.Subscriber("ardrone/navdata", Navdata, self.on_navdata) rospy.Subscriber("arcontroller/goal", Goal , self.on_goal) rospy.Subscriber("arcontroller/waypoints", Waypoints, self.on_waypoints) self.pubTakeoff = rospy.Publisher('ardrone/takeoff', Empty, queue_size=1) self.pubLand = rospy.Publisher('ardrone/land', Empty, queue_size=1) self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.setFlightAnimation = rospy.ServiceProxy('ardrone/setflightanimation', FlightAnim) self.listener = TransformListener() self.action = Controller.ActionTakeOff self.pidX = PID(0.2, 0.12, 0.0, -0.3, 0.3, "x") self.pidY = PID(0.2, 0.12, 0.0, -0.3, 0.3, "y") self.pidZ = PID(1.0, 0, 0.0, -1.0, 1.0, "z") self.pidYaw = PID(0.5, 0, 0.0, -0.6, 0.6, "yaw") # X, Y, Z, Yaw #self.goals = [ # [0, 0, height, 0.941 ], # [1.789, 1.158, height, 0.941 ], # [-2.035, -1.539, height, 0.933 ] # ] #self.points_forward = [self.goals[1],self.goals[0],self.goals[2]] #[[0, 0, height, 0.941 ], #[-2.035, -1.539, height, 0.933 ]] #self.goalIndex = 0 self.goal = [0,0,height,0] #set it to center to start self.goal_done = False self.waypoints = None def on_navdata(self, data): self.lastNavdata = data if data.state != self.lastState: rospy.loginfo("State Changed: " + str(data.state)) self.lastState = data.state def on_shutdown(self): rospy.loginfo("Shutdown: try to land...") msg = Twist() for i in range(0, 1000): self.pubLand.publish() self.pubNav.publish(msg) rospy.sleep(1) def on_goal(self,data): rospy.loginfo('New goal.') self.goal = [data.x,data.y,data.z,data.yaw] self.goal_done = False def on_waypoints(self,data): rospy.loginfo('New waypoints.') self.waypoints = [] for d in data.waypoints: self.waypoints.append(d) def waypoint_follower(self, points): index = 0 self.goal = points[index] #get the first point minX = .05 minY = .05 while True:#for i in range(0,points.len()): #goal = points[i] # transform target world coordinates into local coordinates targetWorld = PoseStamped() t = self.listener.getLatestCommonTime("/vicon/ar_drone/ar_drone", "/world") if self.listener.canTransform("/vicon/ar_drone/ar_drone", "/world", t): targetWorld.header.stamp = t targetWorld.header.frame_id = "world" targetWorld.pose.position.x = self.goal[0] targetWorld.pose.position.y = self.goal[1] targetWorld.pose.position.z = self.goal[2] quaternion = tf.transformations.quaternion_from_euler(0, 0, self.goal[3]) targetWorld.pose.orientation.x = quaternion[0] targetWorld.pose.orientation.y = quaternion[1] targetWorld.pose.orientation.z = quaternion[2] targetWorld.pose.orientation.w = quaternion[3] targetDrone = self.listener.transformPose("/vicon/ar_drone/ar_drone", targetWorld) quaternion = ( targetDrone.pose.orientation.x, targetDrone.pose.orientation.y, targetDrone.pose.orientation.z, targetDrone.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) # Run PID controller and send navigation message msg = Twist() scale = 0.4 msg.linear.x = scale*self.pidX.update(0.0, targetDrone.pose.position.x) msg.linear.y = scale*self.pidY.update(0.0, targetDrone.pose.position.y) if (index != points.len()-1): if (math.fabs(msg.linear.x) < minX) : if (msg.linear.x < 0) : msg.linear.x = -minX elif (msg.linear.x > 0): msg.linear.x = minX if (math.fabs(msg.linear.y) < minY) : if (msg.linear.y < 0) : msg.linear.y = -minY elif (msg.linear.y > 0): msg.linear.y = minY msg.linear.z = self.pidZ.update(0.0, targetDrone.pose.position.z) msg.angular.z = self.pidYaw.update(0.0, euler[2]) # disable hover mode msg.angular.x = 1 self.pubNav.publish(msg) if (math.fabs(targetDrone.pose.position.x) < 0.2 and math.fabs(targetDrone.pose.position.y) < 0.2 and math.fabs(targetDrone.pose.position.z) < 0.2 and math.fabs(euler[2]) < math.radians(20)): if (index < points.len()-1): index += 1 self.goal = points[index] else: return def go_to_goal(self, goal): #rospy.loginfo('Going to goal') #rospy.loginfo(goal) self.goal = goal # transform target world coordinates into local coordinates targetWorld = PoseStamped() t = self.listener.getLatestCommonTime("/vicon/ar_drone/ar_drone", "/world") if self.listener.canTransform("/vicon/ar_drone/ar_drone", "/world", t): targetWorld.header.stamp = t targetWorld.header.frame_id = "world" targetWorld.pose.position.x = goal[0] targetWorld.pose.position.y = goal[1] targetWorld.pose.position.z = goal[2] quaternion = tf.transformations.quaternion_from_euler(0, 0, goal[3]) targetWorld.pose.orientation.x = quaternion[0] targetWorld.pose.orientation.y = quaternion[1] targetWorld.pose.orientation.z = quaternion[2] targetWorld.pose.orientation.w = quaternion[3] targetDrone = self.listener.transformPose("/vicon/ar_drone/ar_drone", targetWorld) quaternion = ( targetDrone.pose.orientation.x, targetDrone.pose.orientation.y, targetDrone.pose.orientation.z, targetDrone.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) # Run PID controller and send navigation message msg = Twist() scale = 0.4 msg.linear.x = scale*self.pidX.update(0.0, targetDrone.pose.position.x) msg.linear.y = scale*self.pidY.update(0.0, targetDrone.pose.position.y) msg.linear.z = self.pidZ.update(0.0, targetDrone.pose.position.z) msg.angular.z = self.pidYaw.update(0.0, euler[2]) # disable hover mode msg.angular.x = 1 self.pubNav.publish(msg) if (math.fabs(targetDrone.pose.position.x) < 0.2 and math.fabs(targetDrone.pose.position.y) < 0.2 and math.fabs(targetDrone.pose.position.z) < 0.2 and math.fabs(euler[2]) < math.radians(20)): self.goal_done = True rospy.loginfo("Goal done.") def run(self): while not rospy.is_shutdown(): if self.action == Controller.ActionTakeOff: if self.lastState == State.Landed: #rospy.loginfo('Taking off.') self.pubTakeoff.publish() elif self.lastState == State.Hovering or self.lastState == State.Flying or self.lastState == State.Flying2: self.action = Controller.ActionHover elif self.action == Controller.ActionLand: msg = Twist() self.pubNav.publish(msg) self.pubLand.publish() elif self.action == Controller.ActionHover: if self.waypoints == None: #if self.goal_done == False: self.go_to_goal(self.goal) else: self.waypoint_follower(self.waypoints) self.waypoints = None rospy.sleep(0.01)
class Controller(): Manual = 0 Automatic = 1 TakeOff = 2 Land = 3 def __init__(self): self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.listener = TransformListener() rospy.Subscriber("joy", Joy, self._joyChanged) rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged) self.cmd_vel_telop = Twist() #self.pidX = PID(20, 12, 0.0, -30, 30, "x") #self.pidY = PID(-20, -12, 0.0, -30, 30, "y") #self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z") #self.pidYaw = PID(50.0, 0.0, 0.0, -200.0, 200.0, "yaw") self.pidX = PID(20, 12, 0.0, -20, 20, "x") self.pidY = PID(-20, -12, 0.0, -20, 20, "y") #50000 800 self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z") self.pidYaw = PID(50.0, 0.0, 0.0, -100.0, 100.0, "yaw") self.state = Controller.Manual #Target Values self.pubtarX = rospy.Publisher('target_x', Float32, queue_size=1) self.pubtarY = rospy.Publisher('target_y', Float32, queue_size=1) self.pubtarZ = rospy.Publisher('target_z', Float32, queue_size=1) self.targetX = 0.0 self.targetY = 0.0 self.targetZ = 0.5 self.des_angle = 0.0 #self.power = 50000.0 #Actual Values self.pubrealX = rospy.Publisher('real_x', Float32, queue_size=1) self.pubrealY = rospy.Publisher('real_y', Float32, queue_size=1) self.pubrealZ = rospy.Publisher('real_z', Float32, queue_size=1) self.lastJoy = Joy() #Path view self.pubPath = rospy.Publisher('cf_Uni_path', MarkerArray, queue_size=100) self.path = MarkerArray() #self.p = [] #Square trajectory self.square_start = False self.square_pos = 0 #self.square =[[0.5,0.5,0.5,0.0], # [0.5,-0.5,0.5,90.0], # [-0.5,-0.5,0.5,180.0], # [-0.5,0.5,0.5,270.0]] #landing flag self.land_flag = False self.power = 0.0 def _cmdVelTelopChanged(self, data): self.cmd_vel_telop = data if self.state == Controller.Manual: self.pubNav.publish(data) def pidReset(self): self.pidX.reset() self.pidZ.reset() self.pidZ.reset() self.pidYaw.reset() def square_go(self): if self.square_start == False: self.square_pos = 0 self.targetX = square[self.square_pos][0] self.targetY = square[self.square_pos][1] self.targetZ = square[self.square_pos][2] self.des_angle = square[self.square_pos][3] self.square_pos = self.square_pos + 1 self.square_start = True else: self.targetX = square[self.square_pos][0] self.targetY = square[self.square_pos][1] self.targetZ = square[self.square_pos][2] self.des_angle = square[self.square_pos][3] self.square_pos = self.square_pos + 1 if self.square_pos == 4: self.square_pos = 0 def _joyChanged(self, data): if len(data.buttons) == len(self.lastJoy.buttons): delta = np.array(data.buttons) - np.array(self.lastJoy.buttons) print("Buton ok") #Button 1 if delta[0] == 1 and self.state != Controller.Automatic: print("Automatic!") self.land_flag = False #thrust = self.cmd_vel_telop.linear.z #print(thrust) self.pidReset() self.pidZ.integral = 40.0 #self.targetZ = 1 self.state = Controller.Automatic #Button 2 if delta[1] == 1 and self.state != Controller.Manual: print("Manual!") self.land_flag = False self.pubNav.publish(self.cmd_vel_telop) self.state = Controller.Manual #Button 3 if delta[2] == 1: self.land_flag = False self.state = Controller.TakeOff print("TakeOff!") #Button 4 if delta[3] == 1: self.land_flag = True print("Landing!") self.square_start = False self.targetX = 0.0 self.targetY = 0.0 self.targetZ = 0.4 self.des_angle = 0.0 self.state = Controller.Automatic #Button 5 if delta[4] == 1: self.square_go() #self.targetX = square[0][0] #self.targetY = square[0][1] #self.targetZ = square[0][2] #self.des_angle = square[0][3] #print(self.targetZ) #self.power += 100.0 #print(self.power) self.state = Controller.Automatic #Button 6 if delta[5] == 1: self.square_start = False self.targetX = 0.0 self.targetY = 0.0 self.targetZ = 0.5 self.des_angle = 0.0 #print(self.targetZ) #self.power -= 100.0 #print(self.power) self.state = Controller.Automatic self.lastJoy = data def run(self): thrust = 0 print("jello") while not rospy.is_shutdown(): if self.state == Controller.TakeOff: t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark_Gon4") print( t, self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t)) if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t): position, quaternion = self.listener.lookupTransform( "/mocap", "/Nano_Mark_Gon4", t) print(position[0], position[1], position[2]) #if position[2] > 2.0 or thrust > 54000: if thrust > 55000: self.pidReset() self.pidZ.integral = thrust / self.pidZ.ki #self.targetZ = 0.5 self.state = Controller.Automatic thrust = 0 else: thrust += 500 #self.power = thrust msg = self.cmd_vel_telop msg.linear.z = thrust self.pubNav.publish(msg) if self.state == Controller.Land: t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark_Gon4") if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t): position, quaternion = self.listener.lookupTransform( "/mocap", "/Nano_Mark_Gon4", t) if position[2] > 0.05: msg_land = self.cmd_vel_telop self.power -= 100 msg_land.linear.z = self.power self.pubNav.publish(msg_land) else: msg_land = self.cmd_vel_telop msg_land.linear.z = 0 self.pubNav.publish(msg_land) if self.state == Controller.Automatic: # transform target world coordinates into local coordinates t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark_Gon4") print(t) if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t): position, quaternion = self.listener.lookupTransform( "/mocap", "/Nano_Mark_Gon4", t) #print(position[0],position[1],position[2]) euler = tf.transformations.euler_from_quaternion( quaternion) print(euler[2] * (180 / math.pi)) msg = self.cmd_vel_telop #print(self.power) #Descompostion of the x and y contributions following the Z-Rotation x_prim = self.pidX.update(0.0, self.targetX - position[0]) y_prim = self.pidY.update(0.0, self.targetY - position[1]) msg.linear.x = x_prim * math.cos( euler[2]) - y_prim * math.sin(euler[2]) msg.linear.y = x_prim * math.sin( euler[2]) + y_prim * math.cos(euler[2]) #---old stuff--- #msg.linear.x = self.pidX.update(0.0, 0.0-position[0]) #msg.linear.y = self.pidY.update(0.0,-1.0-position[1]) #msg.linear.z = self.pidZ.update(position[2],1.0) #z_prim = self.pidZ.update(position[2],self.targetZ) #print(z_prim) #if z_prim < self.power: # msg.linear.z = self.power #else: # msg.linear.z = z_prim #msg.linear.z = self.power #print(self.power) msg.linear.z = self.pidZ.update( 0.0, self.targetZ - position[2] ) #self.pidZ.update(position[2], self.targetZ) msg.angular.z = self.pidYaw.update( 0.0, self.des_angle * (math.pi / 180) + euler[2]) #*(180/math.pi)) #msg.angular.z = self.pidYaw.update(0.0,self.des_angle - euler[2])#*(180/math.pi)) print(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z) #print(euler[2]) #print(msg.angular.z) self.pubNav.publish(msg) #Publish Real and Target position self.pubtarX.publish(self.targetX) self.pubtarY.publish(self.targetY) self.pubtarZ.publish(self.targetZ) self.pubrealX.publish(position[0]) self.pubrealY.publish(position[1]) self.pubrealZ.publish(position[2]) #change square point if abs(self.targetX-position[0])<0.08 and \ abs(self.targetY-position[1])<0.08 and \ abs(self.targetZ-position[2])<0.08 and \ self.square_start == True: self.square_go() #Landing if abs(self.targetX-position[0])<0.1 and \ abs(self.targetY-position[1])<0.1 and \ abs(self.targetZ-position[2])<0.1 and \ self.land_flag == True: self.state = Controller.Land self.power = msg.linear.z #Publish Path #point = Marker() #line = Marker() #point.header.frame_id = line.header.frame_id = 'mocap' #POINTS #point.action = point.ADD #point.pose.orientation.w = 1.0 #point.id = 0 #point.type = point.POINTS #point.scale.x = 0.01 #point.scale.y = 0.01 #point.color.g = 1.0 #point.color.a = 1.0 #LINE #line.action = line.ADD #line.pose.orientation.w = 1.0 #line.id = 1 #line.type = line.LINE_STRIP #line.scale.x = 0.01 #line.color.g = 1.0 #line.color.a = 1.0 #p = Point() #p.x = position[0] #p.y = position[1] #p.z = position[2] #point.points.append(p) # line.points.append(p) #self.path.markers.append(p) #id = 0 #for m in self.path.markers: # m.id = id # id += 1 #self.pubPath.publish(self.path) #self.pubPath.publish(point) #self.pubPath.publish(line) point = Marker() point.header.frame_id = 'mocap' point.type = point.SPHERE #points.header.stamp = rospy.Time.now() point.ns = 'cf_Uni_path' point.action = point.ADD #points.id = 0; point.scale.x = 0.005 point.scale.y = 0.005 point.scale.z = 0.005 point.color.a = 1.0 point.color.r = 1.0 point.color.g = 1.0 point.color.b = 0.0 point.pose.orientation.w = 1.0 point.pose.position.x = position[0] point.pose.position.y = position[1] point.pose.position.z = position[2] self.path.markers.append(point) id = 0 for m in self.path.markers: m.id = id id += 1 self.pubPath.publish(self.path) #point = Point() #point.x = position[0] #point.y = position[1] #point.z = position[2] #points.points.append(point) #self.p.append(pos2path) #self.path.header.stamp = rospy.Time.now() #self.path.header.frame_id = 'mocap' #self.path.poses = self.p #self.pubPath.publish(points) rospy.sleep(0.01)
class Multi_circle_1(): def __init__(self, goals): rospy.init_node('multi_circle_1', anonymous=True) self.worldFrame = rospy.get_param("~worldFrame", "/world") self.frame1 = rospy.get_param("~frame1") #self.frame2 = rospy.get_param("~frame2") self.radius = rospy.get_param("~radius") self.x = rospy.get_param("~x") self.y = rospy.get_param("~y") self.z = rospy.get_param("~z") self.freq = rospy.get_param("~freq") self.lap = rospy.get_param("~lap") self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1) self.listener = TransformListener() self.goals = goals self.goalIndex = 0 def run(self): self.listener.waitForTransform(self.worldFrame, self.frame1, rospy.Time(), rospy.Duration(5.0)) #rospy.loginfo("start running!") goal = PoseStamped() goal.header.seq = 0 goal.header.frame_id = self.worldFrame while not rospy.is_shutdown(): t1 = self.listener.getLatestCommonTime(self.worldFrame, self.frame1) if self.listener.canTransform(self.worldFrame, self.frame1, t1): position, quaternion = self.listener.lookupTransform( self.worldFrame, self.frame1, t1) rpy = tf.transformations.euler_from_quaternion(quaternion) goal.header.seq += 1 goal.header.stamp = rospy.Time.now() #self.x = position[0] #self.y = position[1] #self.z = position[2]+1 goal.pose.position.x = self.x goal.pose.position.y = self.y goal.pose.position.z = self.z quaternion = tf.transformations.quaternion_from_euler(0, 0, 0) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] self.pubGoal.publish(goal) break while not rospy.is_shutdown(): #rospy.loginfo("start running!") goal.header.seq += 1 goal.header.stamp = rospy.Time.now() self.pubGoal.publish(goal) t1 = self.listener.getLatestCommonTime(self.worldFrame, self.frame1) if self.listener.canTransform(self.worldFrame, self.frame1, t1): position, quaternion = self.listener.lookupTransform( self.worldFrame, self.frame1, t1) rpy = tf.transformations.euler_from_quaternion(quaternion) if math.fabs(position[0] - self.x) < 0.15 \ and math.fabs(position[1] - self.y) < 0.15 \ and math.fabs(position[2] - self.z) < 0.15 \ and math.fabs(rpy[2] - 0) < math.radians(10) : rospy.sleep(3) break t_start = rospy.Time.now().to_sec() #rospy.loginfo("t_start:%lf",t_start) t_now = t_start while not rospy.is_shutdown(): goal.header.seq += 1 goal.header.stamp = rospy.Time.now() goal.pose.position.x = self.x + self.radius * math.sin( (t_now - t_start) * 2 * math.pi * self.freq) goal.pose.position.y = self.y + self.radius - self.radius * math.cos( (t_now - t_start) * 2 * math.pi * self.freq) goal.pose.position.z = self.z quaternion = tf.transformations.quaternion_from_euler(0, 0, 0) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] self.pubGoal.publish(goal) t_now = rospy.Time.now().to_sec() rospy.loginfo("t_now-t_start:%lf", t_now - t_start)
class Controller: Idle = 0 Automatic = 1 TakingOff = 2 Landing = 3 def __init__(self, frame): self.frame = frame self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.listener = TransformListener() self.pidX = PID(35, 10, 0.0, -20, 20, "x") self.pidY = PID(-35, -10, -0.0, -20, 20, "y") self.pidZ = PID(4000, 3000.0, 2000.0, 10000, 60000, "z") self.pidYaw = PID(-50.0, 0.0, 0.0, -200.0, 200.0, "yaw") self.state = Controller.Idle self.goal = Pose() rospy.Subscriber("goal", Pose, self._poseChanged) rospy.Service("takeoff", std_srvs.srv.Empty, self._takeoff) rospy.Service("land", std_srvs.srv.Empty, self._land) def getTransform(self, source_frame, target_frame): now = rospy.Time.now() success = False if self.listener.canTransform(source_frame, target_frame, rospy.Time(0)): t = self.listener.getLatestCommonTime(source_frame, target_frame) if self.listener.canTransform(source_frame, target_frame, t): position, quaternion = self.listener.lookupTransform(source_frame, target_frame, t) success = True delta = (now - t).to_sec() * 1000 #ms if delta > 25: rospy.logwarn("Latency: %f ms.", delta) # self.listener.clear() # rospy.sleep(0.02) if success: return position, quaternion, t def pidReset(self): self.pidX.reset() self.pidZ.reset() self.pidZ.reset() self.pidYaw.reset() def _poseChanged(self, data): self.goal = data def _takeoff(self, req): rospy.loginfo("Takeoff requested!") self.state = Controller.TakingOff return std_srvs.srv.EmptyResponse() def _land(self, req): rospy.loginfo("Landing requested!") self.state = Controller.Landing return std_srvs.srv.EmptyResponse() def run(self): thrust = 0 while not rospy.is_shutdown(): now = rospy.Time.now() if self.state == Controller.TakingOff: r = self.getTransform("/world", self.frame) if r: position, quaternion, t = r if position[2] > 0.05 or thrust > 50000: self.pidReset() self.pidZ.integral = thrust / self.pidZ.ki self.targetZ = 0.5 self.state = Controller.Automatic thrust = 0 else: thrust += 100 msg = Twist() msg.linear.z = thrust self.pubNav.publish(msg) else: rospy.logerr("Could not transform from /world to %s.", self.frame) if self.state == Controller.Landing: self.goal.position.z = 0.05 r = self.getTransform("/world", self.frame) if r: position, quaternion, t = r if position[2] <= 0.1: self.state = Controller.Idle msg = Twist() self.pubNav.publish(msg) else: rospy.logerr("Could not transform from /world to %s.", self.frame) if self.state == Controller.Automatic or self.state == Controller.Landing: # transform target world coordinates into local coordinates r = self.getTransform("/world", self.frame) if r: position, quaternion, t = r targetWorld = PoseStamped() targetWorld.header.stamp = t targetWorld.header.frame_id = "world" targetWorld.pose = self.goal targetDrone = self.listener.transformPose(self.frame, targetWorld) quaternion = ( targetDrone.pose.orientation.x, targetDrone.pose.orientation.y, targetDrone.pose.orientation.z, targetDrone.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) msg = Twist() msg.linear.x = self.pidX.update(0.0, targetDrone.pose.position.x) msg.linear.y = self.pidY.update(0.0, targetDrone.pose.position.y) msg.linear.z = self.pidZ.update(0.0, targetDrone.pose.position.z) msg.angular.z = self.pidYaw.update(0.0, euler[2]) self.pubNav.publish(msg) else: rospy.logerr("Could not transform from /world to %s.", self.frame) if self.state == Controller.Idle: msg = Twist() self.pubNav.publish(msg) rospy.sleep(0.01)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ xy_theta = [] def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 250 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model self.model_noise_rate = 0.15 # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) print() # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid # TODO: fill in the appropriate service call here. The resultant map should be assigned be passed # into the init method for OccupancyField rospy.wait_for_service('static_map') grid = rospy.ServiceProxy('static_map',GetMap) my_map = grid().map # for now we have commented out the occupancy field initialization until you can successfully fetch the map self.field = OccupancyField(my_map) self.initialized = True def create_initial_particle_list(self,xy_theta): init_particle_list = [] n = self.n_particles for i in range(self.n_particles): w = 1.0/n x = gauss(xy_theta[0],0.5) y = gauss(xy_theta[1],0.5) theta = gauss(xy_theta[2],((math.pi)/2)) particle = Particle(x,y,theta,w) init_particle_list.append(particle) print("init_particle_list") return init_particle_list def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() # TODO: assign the lastest pose into self.robot_pose as a geometry_msgs.Pose object # just to get started we will fix the robot's pose to always be at the origin x = 0 y = 0 theta = 0 angles = [] for particle in self.particle_cloud: x += particle.x * particle.w y += particle.y * particle.w v = [particle.w * math.cos(math.radians(particle.theta)), particle.w * math.sin(math.radians(particle.theta))] angles.append(v) theta = sum_vectors(angles) orientation = tf.transformations.quaternion_from_euler(0,0,theta) self.robot_pose = Pose(position=Point(x=x,y=y),orientation=Quaternion(x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3])) def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ print('update_w_odom') new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta for particle in self.particle_cloud: parc = (math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2]) % 360 particle.x += (math.sqrt((delta[0]**2) + (delta[1]**2)))* math.cos(parc) particle.y += (math.sqrt((delta[0]**2) + (delta[1]**2))) * math.sin(parc) particle.theta += delta[2] else: self.current_odom_xy_theta = new_odom_xy_theta return #DONE # TODO: modify particles using delta # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized self.normalize_particles() values = np.empty(self.n_particles) probs = np.empty(self.n_particles) for i in range(len(self.particle_cloud)): values[i] = i probs[i] = self.particle_cloud[i].w new_random_particle = ParticleFilter.weighted_values(values,probs,self.n_particles) new_particles = [] for i in new_random_particle: idx = int(i) s_p = self.particle_cloud[idx] new_particles.append(Particle(x=s_p.x+gauss(0,.025),y=s_p.y+gauss(0,.05),theta=s_p.theta+gauss(0,.05))) self.particle_cloud = new_particles self.normalize_particles() def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ print('update_w_laser') readings = msg.ranges for particle in self.particle_cloud: for read in range(0,len(readings),3): self.field.get_particle_likelyhood(particle,readings[read],self.model_noise_rate,read) self.normalize_particles() self.resample_particles() @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)-1] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ print('draw_random_sample') values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) #self.particle_cloud = [] # TODO create particles self.particle_cloud = self.create_initial_particle_list(xy_theta) self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ w_sum = sum([p.w for p in self.particle_cloud]) for particle in self.particle_cloud: particle.w /= w_sum # TODO: implement this def publish_particles(self, msg): print('publish_particles') particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ print('scan_received') if not(self.initialized): # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles() # resample particles to focus on areas of high density self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation), header=Header(stamp=rospy.Time(0),frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return print('broadcast') self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.Time.now(), self.odom_frame, self.map_frame)
class Controller(): Manual = 0 Automatic = 1 TakeOff = 2 def __init__(self): self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.listener = TransformListener() rospy.Subscriber("joy", Joy, self._joyChanged) rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged) self.cmd_vel_telop = Twist() #self.pidX = PID(20, 12, 0.0, -30, 30, "x") #self.pidY = PID(-20, -12, 0.0, -30, 30, "y") #self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z") #self.pidYaw = PID(50.0, 0.0, 0.0, -200.0, 200.0, "yaw") self.pidX = PID(20, 12, 0.0, -20, 20, "x") self.pidY = PID(-20, -12, 0.0, -20, 20, "y") #50000 800 self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z") self.pidYaw = PID(50.0, 0.0, 0.0, -100.0, 100.0, "yaw") self.state = Controller.Manual #Target Values self.pubtarX = rospy.Publisher('target_x', Float32, queue_size=1) self.pubtarY = rospy.Publisher('target_y', Float32, queue_size=1) self.pubtarZ = rospy.Publisher('target_z', Float32, queue_size=1) self.targetZ = 0.5 self.targetX = 0.0 self.targetY = 0.0 self.des_angle = 0.0 #self.power = 50000.0 #Actual Values self.pubrealX = rospy.Publisher('real_x', Float32, queue_size=1) self.pubrealY = rospy.Publisher('real_y', Float32, queue_size=1) self.pubrealZ = rospy.Publisher('real_z', Float32, queue_size=1) self.lastJoy = Joy() #Path self.pubPath = rospy.Publisher('cf_Gon_path', Path, queue_size=1) self.path = Path() self.p = [] def _cmdVelTelopChanged(self, data): self.cmd_vel_telop = data if self.state == Controller.Manual: self.pubNav.publish(data) def pidReset(self): self.pidX.reset() self.pidZ.reset() self.pidZ.reset() self.pidYaw.reset() def _joyChanged(self, data): if len(data.buttons) == len(self.lastJoy.buttons): delta = np.array(data.buttons) - np.array(self.lastJoy.buttons) print ("Buton ok") #Button 1 if delta[0] == 1 and self.state != Controller.Automatic: print("Automatic!") #thrust = self.cmd_vel_telop.linear.z #print(thrust) self.pidReset() self.pidZ.integral = 40.0 #self.targetZ = 1 self.state = Controller.Automatic #Button 2 if delta[1] == 1 and self.state != Controller.Manual: print("Manual!") self.pubNav.publish(self.cmd_vel_telop) self.state = Controller.Manual #Button 3 if delta[2] == 1: self.state = Controller.TakeOff print("TakeOff!") #Button 5 if delta[4] == 1: #self.targetX = -1.0 #self.targetY = -1.0 self.targetZ = 1.0 self.des_angle = -90.0 #if self.des_angle > 179: # self.des_angle = -179.0 #print(self.targetZ) #self.power += 100.0 #print(self.power) self.state = Controller.Automatic #Button 6 if delta[5] == 1: #self.targetX = 1.0 #self.targetY = 1.0 self.targetZ = 0.5 self.des_angle = 90.0 #if self.des_angle < -179: # self.des_angle = 179.0 #print(self.targetZ) #self.power -= 100.0 #print(self.power) self.state = Controller.Automatic self.lastJoy = data def run(self): thrust = 0 print("jello") while not rospy.is_shutdown(): if self.state == Controller.TakeOff: t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark_Gon4") if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t): position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4", t) print(position[0],position[1],position[2]) #if position[2] > 0.1 or thrust > 50000: if thrust > 51000: self.pidReset() self.pidZ.integral = thrust / self.pidZ.ki #self.targetZ = 0.5 self.state = Controller.Automatic thrust = 0 else: thrust += 300 self.power = thrust msg = self.cmd_vel_telop msg.linear.z = thrust self.pubNav.publish(msg) if self.state == Controller.Automatic: # transform target world coordinates into local coordinates t = self.listener.getLatestCommonTime("/mocap","/Nano_Mark_Gon4") print(t) if self.listener.canTransform("/mocap","/Nano_Mark_Gon4", t): position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4",t) #print(position[0],position[1],position[2]) euler = tf.transformations.euler_from_quaternion(quaternion) print(euler[2]*(180/math.pi)) msg = self.cmd_vel_telop #print(self.power) #Descompostion of the x and y contributions following the Z-Rotation x_prim = self.pidX.update(0.0, self.targetX-position[0]) y_prim = self.pidY.update(0.0,self.targetY-position[1]) msg.linear.x = x_prim*math.cos(euler[2]) - y_prim*math.sin(euler[2]) msg.linear.y = x_prim*math.sin(euler[2]) + y_prim*math.cos(euler[2]) #---old stuff--- #msg.linear.x = self.pidX.update(0.0, 0.0-position[0]) #msg.linear.y = self.pidY.update(0.0,-1.0-position[1]) #msg.linear.z = self.pidZ.update(position[2],1.0) #z_prim = self.pidZ.update(position[2],self.targetZ) #print(z_prim) #if z_prim < self.power: # msg.linear.z = self.power #else: # msg.linear.z = z_prim #msg.linear.z = self.power #print(self.power) msg.linear.z = self.pidZ.update(0.0,self.targetZ-position[2]) #self.pidZ.update(position[2], self.targetZ) msg.angular.z = self.pidYaw.update(0.0,self.des_angle*(math.pi/180) + euler[2])#*(180/math.pi)) #msg.angular.z = self.pidYaw.update(0.0,self.des_angle - euler[2])#*(180/math.pi)) print(msg.linear.x,msg.linear.y,msg.linear.z,msg.angular.z) #print(euler[2]) #print(msg.angular.z) self.pubNav.publish(msg) #Publish Real and Target position self.pubtarX.publish(self.targetX) self.pubtarY.publish(self.targetY) self.pubtarZ.publish(self.targetZ) self.pubrealX.publish(position[0]) self.pubrealY.publish(position[1]) self.pubrealZ.publish(position[2]) print("que pasaaa") #Publish Path pos2path = PoseStamped() pos2path.pose.position.x = position[0] pos2path.pose.position.y = position[1] pos2path.pose.position.z = position[2] pos2path.pose.orientation.x = quaternion[0] pos2path.pose.orientation.y = quaternion[1] pos2path.pose.orientation.z = quaternion[2] pos2path.pose.orientation.w = quaternion[3] self.p.append(pos2path) print('holaaaaa') self.path.header.frame_id = 'mocap' self.path.poses = self.p self.pubPath.publish(self.path) rospy.sleep(0.01)
rate = rospy.Rate(100) q_target = None base_pub = rospy.Publisher('/cmd_vel', Twist) cmd_vel = Twist() error_x = 0 error_y = 0 error_rotation = 0 while not rospy.is_shutdown(): gripper_pose = PoseStamped() gripper_pose.header.frame_id = '/r_gripper_tool_frame' if (t.canTransform('/base_link', '/r_gripper_tool_frame', gripper_pose.header.stamp)): q = t.transformPose('/base_link', gripper_pose) if not q_target: q_target = q error_x = q_target.pose.position.x - q.pose.position.x error_y = q_target.pose.position.y - q.pose.position.y error_rotation = q_target.pose.orientation.z - q.pose.orientation.z print(error_rotation) cmd_vel.linear.x = -10.0 * error_x cmd_vel.angular.z = -10.0 * error_y + error_rotation * -5.0 cmd_vel.linear.y = error_rotation * 2.0 base_pub.publish(cmd_vel) rate.sleep()
class Controller(): Manual = 0 Automatic = 1 TakeOff = 2 def __init__(self): self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.listener = TransformListener() rospy.Subscriber("joy", Joy, self._joyChanged) rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged) self.cmd_vel_telop = Twist() self.pidX = PID(20, 12, 0.0, -30, 30, "x") self.pidY = PID(-20, -12, 0.0, -30, 30, "y") self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z") self.pidYaw = PID(-50.0, 0.0, 0.0, -200.0, 200.0, "yaw") self.state = Controller.Manual self.targetZ = 0.5 self.lastJoy = Joy() def _cmdVelTelopChanged(self, data): self.cmd_vel_telop = data if self.state == Controller.Manual: self.pubNav.publish(data) def pidReset(self): self.pidX.reset() self.pidZ.reset() self.pidZ.reset() self.pidYaw.reset() def _joyChanged(self, data): if len(data.buttons) == len(self.lastJoy.buttons): delta = np.array(data.buttons) - np.array(self.lastJoy.buttons) print ("Buton ok") #Button 1 if delta[0] == 1 and self.state != Controller.Automatic: print("Automatic!") thrust = self.cmd_vel_telop.linear.z print(thrust) self.pidReset() self.pidZ.integral = thrust / self.pidZ.ki self.targetZ = 0.5 self.state = Controller.Automatic #Button 2 if delta[1] == 1 and self.state != Controller.Manual: print("Manual!") self.pubNav.publish(self.cmd_vel_telop) self.state = Controller.Manual #Button 3 if delta[2] == 1: self.state = Controller.TakeOff print("TakeOff!") #Button 5 if delta[4] == 1: self.targetZ += 0.1 print(self.targetZ) #Button 6 if delta[5] == 1: self.targetZ -= 0.1 print(self.targetZ) self.lastJoy = data def run(self): thrust = 0 print("jello") while not rospy.is_shutdown(): if self.state == Controller.TakeOff: t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark") if self.listener.canTransform("/mocap", "/Nano_Mark", t): position, quaternion = self.listener.lookupTransform("/mocap", "/Nano_Mark", t) if position[2] > 0.1 or thrust > 50000: self.pidReset() self.pidZ.integral = thrust / self.pidZ.ki self.targetZ = 0.5 self.state = Controller.Automatic thrust = 0 else: thrust += 100 msg = self.cmd_vel_telop msg.linear.z = thrust self.pubNav.publish(msg) if self.state == Controller.Automatic: # transform target world coordinates into local coordinates t = self.listener.getLatestCommonTime("/Nano_Mark", "/mocap") print(t); if self.listener.canTransform("/Nano_Mark", "/mocap", t): targetWorld = PoseStamped() targetWorld.header.stamp = t targetWorld.header.frame_id = "mocap" targetWorld.pose.position.x = 0 targetWorld.pose.position.y = 0 targetWorld.pose.position.z = self.targetZ quaternion = tf.transformations.quaternion_from_euler(0, 0, 0) targetWorld.pose.orientation.x = quaternion[0] targetWorld.pose.orientation.y = quaternion[1] targetWorld.pose.orientation.z = quaternion[2] targetWorld.pose.orientation.w = quaternion[3] targetDrone = self.listener.transformPose("/Nano_Mark", targetWorld) quaternion = ( targetDrone.pose.orientation.x, targetDrone.pose.orientation.y, targetDrone.pose.orientation.z, targetDrone.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) #msg = self.cmd_vel_teleop msg.linear.x = self.pidX.update(0.0, targetDrone.pose.position.x) msg.linear.y = self.pidY.update(0.0, targetDrone.pose.position.y) msg.linear.z = self.pidZ.update(0.0, targetDrone.pose.position.z) #self.pidZ.update(position[2], self.targetZ) msg.angular.z = self.pidYaw.update(0.0, euler[2]) #print(euler[2]) #print(msg.angular.z) self.pubNav.publish(msg) rospy.sleep(0.01)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node( 'pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 600 # the number of particles to use self.particle_init_options = ParticleInitOptions.UNIFORM_DISTRIBUTION self.d_thresh = 0.1 # the amount of linear movement before performing an update self.a_thresh = math.pi / 12.0 # the amount of angular movement before performing an update self.num_lidar_points = 180 # int from 1 to 360 # Note: self.laser_max_distance is NOT implemented # TODO: Experiment with setting a max acceptable distance for lidar scans self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # laser_subscriber listens for data from the lidar rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received, queue_size=1) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # publish our hypotheses points self.hypothesis_pub = rospy.Publisher("hypotheses", MarkerArray, queue_size=10) # Publish our hypothesis points right before they get udpated through odom self.before_odom_hypothesis_pub = rospy.Publisher( "before_odom_hypotheses", MarkerArray, queue_size=10) # Publish where the hypothesis points will be once they're updated with the odometry self.future_hypothesis_pub = rospy.Publisher("future_hypotheses", MarkerArray, queue_size=10) # Publish the lidar scan that pf.py sees self.lidar_pub = rospy.Publisher("lidar_visualization", MarkerArray, queue_size=1) # Publish the lidar scan projected from the first hypothesis self.projected_lidar_pub = rospy.Publisher( "projected_lidar_visualization", MarkerArray, queue_size=1) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] # change use_projected_stable_scan to True to use point clouds instead of laser scans self.use_projected_stable_scan = False self.last_projected_stable_scan = None if self.use_projected_stable_scan: # subscriber to the odom point cloud rospy.Subscriber("projected_stable_scan", PointCloud, self.projected_scan_received) self.current_odom_xy_theta = [] self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() self.initialized = True def update_robot_pose(self, timestamp): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # assign the best particle's pose to self.robot_pose as a geometry_msgs.Pose object best_particle = self.particle_cloud[0] for particle in self.particle_cloud[1:]: if particle.w > best_particle.w: best_particle = particle self.robot_pose = best_particle.as_pose() self.transform_helper.fix_map_to_odom_transform( self.robot_pose, timestamp) def projected_scan_received(self, msg): self.last_projected_stable_scan = msg def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # Publish a visualization of all our particles before they get updated timestamp = rospy.Time.now() particle_color = (1.0, 0.0, 0.0) particle_markers = [ particle.as_marker(timestamp, count, "before_odom_hypotheses", particle_color) for count, particle in enumerate(self.particle_cloud) ] # Publish the visualization of all the particles in Rviz self.before_odom_hypothesis_pub.publish( MarkerArray(markers=particle_markers)) # delta xy_theta is relative to the odom frame, which is a global frame # Global Frame -> Robot Frame # Delta also works for relative to robot _> need to rotate it properly # Robot Frame - Rotate it so that it's projected from the particle in the particle frame # Need the difference between the particle theta and the robot theta # That's how much to rotate it by # diff_theta = self.current_odom_xy_theta[2] - # Particle Frame -> Global Frame for index, particle in enumerate(self.particle_cloud): diff_theta = self.current_odom_xy_theta[2] - (particle.theta - math.pi) partRotMtrx = np.array([[np.cos(diff_theta), -np.sin(diff_theta)], [np.sin(diff_theta), np.cos(diff_theta)]]) translationMtrx = np.array([[delta[0]], [delta[1]]]) partTranslationOp = partRotMtrx.dot(translationMtrx) # update particle position to move with delta self.particle_cloud[index].x -= partTranslationOp[0, 0] self.particle_cloud[index].y -= partTranslationOp[1, 0] self.particle_cloud[index].theta += delta[2] if len(self.particle_cloud) == 1: # For debugging purposes print("") print("Robot Theta: ", self.current_odom_xy_theta[2]) print("Particle Theta:", particle.theta) print("Diff Theta: ", diff_theta) print("Deltas before transformations:\nDelta x: ", delta[0], " | Delta y: ", delta[1], " | Delta theta: ", delta[2]) print("Deltas after transformations:\nDelta x: ", partTranslationOp[0, 0], " | Delta y: ", partTranslationOp[1, 0]) # Build up a list of all the just moved particles as Rviz Markers timestamp = rospy.Time.now() particle_color = (0.0, 0.0, 1.0) particle_markers = [ particle.as_marker(timestamp, count, "future_hypotheses", particle_color) for count, particle in enumerate(self.particle_cloud) ] # Publish the visualization of all the particles in Rviz self.future_hypothesis_pub.publish( MarkerArray(markers=particle_markers)) def map_calc_range(self, x, y, theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # cull particles # set looping variable values and initalize array to store significant points def returnFunc(part): return part.w self.particle_cloud.sort(key=returnFunc, reverse=True) numResamplingNodes = 500 resamplingNodes = self.particle_cloud[0:numResamplingNodes] # Calculate the number of particles to cluster around each resamplingNode cluster_size = math.ceil( (self.n_particles - numResamplingNodes) / numResamplingNodes) # Uniformly cluster the lowest weighted particles around the highest weighted particles (resamplingNodes) num_cluster = 0 cluster_radius = 0.25 cluster_theta_range = math.pi / 2.0 for resamplingNode in resamplingNodes: start_cluster_index = numResamplingNodes + num_cluster * cluster_size end_cluster_index = start_cluster_index + cluster_size if end_cluster_index > len(self.particle_cloud): end_cluster_index = len(self.particle_cloud) for particle_index in range(start_cluster_index, end_cluster_index): self.particle_cloud[particle_index].x = uniform( (resamplingNode.x - cluster_radius), (resamplingNode.x + cluster_radius)) self.particle_cloud[particle_index].y = uniform( (resamplingNode.y - cluster_radius), (resamplingNode.y + cluster_radius)) self.particle_cloud[particle_index].theta = uniform( (resamplingNode.w - cluster_theta_range), (resamplingNode.w + cluster_theta_range)) self.particle_cloud[particle_index].w = resamplingNode.w # self.particle_cloud[particle_index].w = uniform((resamplingNode.w - cluster_theta_range),(resamplingNode.w + cluster_theta_range)) num_cluster += 1 # TODO: Experiment with clustering points dependending on weight of the resamplingNode # #repopulate field # #loop through all the significant weighted particles (or nodes in the probability field) # nodeIndex = 0 # particleIndex = 0 # while nodeIndex < len(resamplingNodes): # #place points around nodes # placePointIndex = 0 # #loop through the number of points that need to be placed given the weight of the particle # while placePointIndex < self.n_particles * resamplingNodes[nodeIndex].w: # #place point in circular area around node # radiusRepopCircle = resamplingNodes[nodeIndex].w*10.0 # #create a point in the circular area # self.particle_cloud[particleIndex] = Particle(uniform((resamplingNodes[nodeIndex].x - radiusRepopCircle),(resamplingNodes[nodeIndex].x + radiusRepopCircle)),uniform((resamplingNodes[nodeIndex].y - radiusRepopCircle),(resamplingNodes[nodeIndex].y + radiusRepopCircle)),resamplingNodes[nodeIndex].theta) # #update iteration variables # particleIndex += 1 # placePointIndex += 1 # nodeIndex += 1 def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ # Note: This only updates the weights. This does not move the particles themselves # Only get the specified number of lidar points at regular slices downsampled_angle_range_list = [] downsampled_angles = np.linspace(0, 360, self.num_lidar_points, False) downsampled_angles_int = downsampled_angles.astype(int) for angle, range_ in enumerate(msg.ranges[0:360]): if angle in downsampled_angles_int: downsampled_angle_range_list.append((angle, range_)) # Filter out invalid ranges filtered_angle_range_list = [] for angle, range_ in downsampled_angle_range_list: if range_ != 0.0: filtered_angle_range_list.append((angle, range_)) # Transform ranges into numpy array of xs and ys relative_to_robot = np.zeros((len(filtered_angle_range_list), 2)) for index, (angle, range_) in enumerate(filtered_angle_range_list): relative_to_robot[index, 0] = range_ * np.cos(angle * np.pi / 180.0) # xs relative_to_robot[index, 1] = range_ * np.sin(angle * np.pi / 180.0) # ys # Build up an array of lidar markers for visualization lidar_markers = [] for index, xy_point in enumerate(relative_to_robot): lidar_markers.append( build_lidar_marker(msg.header.stamp, xy_point[0], xy_point[1], index, "base_link", "lidar_visualization", (1.0, 0.0, 0.0))) # Make sure to delete any old markers num_deletion_markers = 360 - len(lidar_markers) for _ in range(num_deletion_markers): marker_id = len(lidar_markers) lidar_markers.append( build_deletion_marker(msg.header.stamp, marker_id, "lidar_visualization")) # Publish lidar points for visualization self.lidar_pub.publish(MarkerArray(markers=lidar_markers)) # For every particle (hypothesis) we have for particle in self.particle_cloud: # Combine the xy positions of the scan with the xy w of the hypothesis # Rotation matrix could be helpful here (https://en.wikipedia.org/wiki/Rotation_matrix) # Build our rotation matrix R = np.array([[np.cos(particle.theta), -np.sin(particle.theta)], [np.sin(particle.theta), np.cos(particle.theta)]]) # Rotate the points according to particle orientation relative_to_particle = (R.dot(relative_to_robot.T)).T # relative_to_particle = relative_to_robot.dot(R) # Translate points to be relative to map origin relative_to_map = deepcopy(relative_to_particle) relative_to_map[:, 0:1] = relative_to_map[:, 0:1] + particle.x * np.ones( (relative_to_map. shape[0], 1)) relative_to_map[:, 1:2] = relative_to_map[:, 1:2] + particle.y * np.ones( (relative_to_map. shape[0], 1)) # Get the distances of each projected point to nearest obstacle distance_list = [] for xy_projected_point in relative_to_map: distance = self.occupancy_field.get_closest_obstacle_distance( xy_projected_point[0], xy_projected_point[1]) if not np.isfinite(distance): # Note: ac109 map has approximately a 10x10 bounding box # Hardcode 1m as the default distance in case the projected point is off the map distance = 1.0 distance_list.append(distance) # Calculate a weight for for this particle # Note: The further away a projected point is from an obstacle point, # the lower its weight should be weight = 1.0 / sum(distance_list) particle.w = weight # Normalize the weights self.normalize_particles() # Grab the first particle particle = self.particle_cloud[0] # Visualize the projected points around that particle projected_lidar_markers = [] for index, xy_point in enumerate(relative_to_map): projected_lidar_markers.append( build_lidar_marker(msg.header.stamp, xy_point[0], xy_point[1], index, "map", "projected_lidar_visualization")) # Make sure to delete any old markers num_deletion_markers = 360 - len(projected_lidar_markers) for _ in range(num_deletion_markers): marker_id = len(projected_lidar_markers) projected_lidar_markers.append( build_deletion_marker(msg.header.stamp, marker_id, "projected_lidar_visualization")) # Publish the projection visualization to rviz self.projected_lidar_pub.publish( MarkerArray(markers=projected_lidar_markers)) # Build up a list of all the particles as Rviz Markers timestamp = rospy.Time.now() particle_markers = [ particle.as_marker(timestamp, count) for count, particle in enumerate(n.particle_cloud) ] # Publish the visualization of all the particles in Rviz self.hypothesis_pub.publish(MarkerArray(markers=particle_markers)) @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( msg.pose.pose) self.initialize_particle_cloud(msg.header.stamp, xy_theta) def initialize_particle_cloud(self, timestamp, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is omitted, the odometry will be used """ # TODO: Check if moving the xy_theta stuff to where the robot initializes around a given set of points is helpful # if xy_theta is None: # xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(self.odom_pose.pose) # Check how the algorithm should initialize its particles # Distribute particles uniformly with parameters defining the number of particles and bounding box if self.particle_init_options == ParticleInitOptions.UNIFORM_DISTRIBUTION: #create an index to track the x cordinate of the particles being created #calculate the number of particles to place widthwize vs hightwize along the map based on the number of particles and the dimensions of the map num_particles_x = math.sqrt(self.n_particles) num_particles_y = num_particles_x index_x = -3 #iterate over the map to place points in a uniform grid while index_x < 4: index_y = -4 while index_y < 3: #create a particle at the location with a random orientation new_particle = Particle(index_x, index_y, uniform(0, 2 * math.pi)) #add the particle to the particle array self.particle_cloud.append(new_particle) #increment the index to place the next particle index_y += 7 / (num_particles_y) #increment index to place next column of particles index_x += 7 / num_particles_x # Distribute particles uniformly, but hard-coded (mainly for quick tests) elif self.particle_init_options == ParticleInitOptions.UNIFORM_DISTRIBUTION_HARDCODED: # Make a list of hypotheses that can update based on values xs = np.linspace(-3, 4, 21) ys = np.linspace(-4, 3, 21) for y in ys: for x in xs: for i in range(5): new_particle = Particle( x, y, np.random.uniform(0, 2 * math.pi)) self.particle_cloud.append(new_particle) # Create a single arbitrary particle (For debugging) elif self.particle_init_options == ParticleInitOptions.SINGLE_PARTICLE: new_particle = Particle(3.1, 0.0, -0.38802401685700466 + math.pi) self.particle_cloud.append(new_particle) # TODO: Set up robot pose on particle cloud initialization # self.update_robot_pose(timestamp) def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ #set variable inital values index = 0 weightSum = 0 # calulate the total particle weight while index < len(self.particle_cloud): weightSum += self.particle_cloud[index].w index += 1 index = 0 #normalize the weight for each particle by divifdng by the total weight while index < len(self.particle_cloud): self.particle_cloud[ index].w = self.particle_cloud[index].w / weightSum index += 1 pass def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, we hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative to the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) if not self.current_odom_xy_theta: self.current_odom_xy_theta = new_odom_xy_theta return if not (self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud(msg.header.stamp) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry if self.last_projected_stable_scan: last_projected_scan_timeshift = deepcopy( self.last_projected_stable_scan) last_projected_scan_timeshift.header.stamp = msg.header.stamp self.scan_in_base_link = self.tf_listener.transformPointCloud( "base_link", last_projected_scan_timeshift) self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose(msg.header.stamp) # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density # publish particles (so things like rviz can see them) self.publish_particles(msg)
class MyAMCL: def __init__(self): self.initialized = False rospy.init_node('my_amcl') print "MY AMCL initialized" # todo make this static self.n_particles = 100 self.alpha1 = 0.2 self.alpha2 = 0.2 self.alpha3 = 0.2 self.alpha4 = 0.2 self.d_thresh = 0.2 self.a_thresh = math.pi / 6 self.z_hit = 0.5 self.z_rand = 0.5 self.sigma_hit = 0.2 self.laser_max_distance = 2.0 self.laser_subscriber = rospy.Subscriber("scan", LaserScan, self.scan_received) self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_pub = rospy.Publisher("particlecloud", PoseArray) self.particle_cloud = [] self.last_transform_valid = False self.particle_cloud_initialized = False self.current_odom_xy_theta = [] # request the map rospy.wait_for_service("static_map") static_map = rospy.ServiceProxy("static_map", GetMap) try: self.map = static_map().map except: print "error receiving map" self.create_occupancy_field() self.initialized = True def create_occupancy_field(self): X = np.zeros((self.map.info.width * self.map.info.height, 2)) total_occupied = 0 curr = 0 for i in range(self.map.info.width): for j in range(self.map.info.height): # occupancy grids are stored in row major order, if you go through this right, you might be able to use curr ind = i + j * self.map.info.width if self.map.data[ind] > 0: total_occupied += 1 X[curr, 0] = float(i) X[curr, 1] = float(j) curr += 1 O = np.zeros((total_occupied, 2)) curr = 0 for i in range(self.map.info.width): for j in range(self.map.info.height): # occupancy grids are stored in row major order, if you go through this right, you might be able to use curr ind = i + j * self.map.info.width if self.map.data[ind] > 0: O[curr, 0] = float(i) O[curr, 1] = float(j) curr += 1 t = time.time() nbrs = NearestNeighbors(n_neighbors=1, algorithm="ball_tree").fit(O) distances, indices = nbrs.kneighbors(X) print time.time() - t closest_occ = {} curr = 0 for i in range(self.map.info.width): for j in range(self.map.info.height): ind = i + j * self.map.info.width closest_occ[ind] = distances[curr] * self.map.info.resolution curr += 1 # this is a bit adhoc, could probably integrate into an internal map structure self.closest_occ = closest_occ def update_robot_pose(self): # first make sure that the particle weights are normalized self.normalize_particles() use_mean = True if use_mean: mean_x = 0.0 mean_y = 0.0 mean_theta = 0.0 theta_list = [] weighted_orientation_vec = np.zeros((2, 1)) for p in self.particle_cloud: mean_x += p.x * p.w mean_y += p.y * p.w weighted_orientation_vec[0] += p.w * math.cos(p.theta) weighted_orientation_vec[1] += p.w * math.sin(p.theta) mean_theta = math.atan2(weighted_orientation_vec[1], weighted_orientation_vec[0]) self.robot_pose = Particle(x=mean_x, y=mean_y, theta=mean_theta).as_pose() else: weights = [] for p in self.particle_cloud: weights.append(p.w) best_particle = np.argmax(weights) self.robot_pose = self.particle_cloud[best_particle].as_pose() def update_particles_with_odom(self, msg): new_odom_xy_theta = MyAMCL.convert_pose_to_xy_and_theta( self.odom_pose.pose) if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # Implement sample_motion_odometry (Prob Rob p 136) # Avoid computing a bearing from two poses that are extremely near each # other (happens on in-place rotation). delta_trans = math.sqrt(delta[0] * delta[0] + delta[1] * delta[1]) if delta_trans < 0.01: delta_rot1 = 0.0 else: delta_rot1 = MyAMCL.angle_diff(math.atan2(delta[1], delta[0]), old_odom_xy_theta[2]) delta_rot2 = MyAMCL.angle_diff(delta[2], delta_rot1) # We want to treat backward and forward motion symmetrically for the # noise model to be applied below. The standard model seems to assume # forward motion. delta_rot1_noise = min( math.fabs(MyAMCL.angle_diff(delta_rot1, 0.0)), math.fabs(MyAMCL.angle_diff(delta_rot1, math.pi))) delta_rot2_noise = min( math.fabs(MyAMCL.angle_diff(delta_rot2, 0.0)), math.fabs(MyAMCL.angle_diff(delta_rot2, math.pi))) for sample in self.particle_cloud: # Sample pose differences delta_rot1_hat = MyAMCL.angle_diff( delta_rot1, gauss( 0, self.alpha1 * delta_rot1_noise * delta_rot1_noise + self.alpha2 * delta_trans * delta_trans)) delta_trans_hat = delta_trans - gauss( 0, self.alpha3 * delta_trans * delta_trans + self.alpha4 * delta_rot1_noise * delta_rot1_noise + self.alpha4 * delta_rot2_noise * delta_rot2_noise) delta_rot2_hat = MyAMCL.angle_diff( delta_rot2, gauss( 0, self.alpha1 * delta_rot2_noise * delta_rot2_noise + self.alpha2 * delta_trans * delta_trans)) # Apply sampled update to particle pose sample.x += delta_trans_hat * math.cos(sample.theta + delta_rot1_hat) sample.y += delta_trans_hat * math.sin(sample.theta + delta_rot1_hat) sample.theta += delta_rot1_hat + delta_rot2_hat def get_map_index(self, x, y): x_coord = int( (x - self.map.info.origin.position.x) / self.map.info.resolution) y_coord = int( (y - self.map.info.origin.position.y) / self.map.info.resolution) # check if we are in bounds if x_coord > self.map.info.width or x_coord < 0: return float('nan') if y_coord > self.map.info.height or y_coord < 0: return float('nan') ind = x_coord + y_coord * self.map.info.width if ind >= self.map.info.width * self.map.info.height or ind < 0: return float('nan') return ind def map_calc_range(self, x, y, theta): ''' this is for a beam model... this is pretty damn slow...''' (x_curr, y_curr) = (x, y) ind = self.get_map_index(x_curr, y_curr) while not (math.isnan(ind)): if self.map.data[ind] > 0: return math.sqrt((x - x_curr)**2 + (y - y_curr)**2) x_curr += self.map.info.resolution * 0.5 * math.cos(theta) y_curr += self.map.info.resolution * 0.5 * math.sin(theta) ind = self.get_map_index(x_curr, y_curr) if math.isnan(ind): return float('nan') else: return self.map.info.range_max def resample_particles(self): self.normalize_particles() values = np.empty(self.n_particles) probs = np.empty(self.n_particles) for i in range(len(self.particle_cloud)): values[i] = i probs[i] = self.particle_cloud[i].w new_particle_indices = MyAMCL.weighted_values(values, probs, self.n_particles) new_particles = [] for i in new_particle_indices: idx = int(i) s_p = self.particle_cloud[idx] new_particles.append( Particle(x=s_p.x + gauss(0, .025), y=s_p.y + gauss(0, .025), theta=s_p.theta + gauss(0, .025))) self.particle_cloud = new_particles self.normalize_particles() def update_particles_with_laser(self, msg): laser_xy_theta = MyAMCL.convert_pose_to_xy_and_theta( self.laser_pose.pose) for p in self.particle_cloud: adjusted_pose = (p.x + laser_xy_theta[0], p.y + laser_xy_theta[1], p.theta + laser_xy_theta[2]) # Pre-compute a couple of things z_hit_denom = 2 * self.sigma_hit**2 z_rand_mult = 1.0 / msg.range_max # This assumes quite a bit about the weights beforehand (TODO: could base this on p.w) new_prob = 1.0 for i in range(5, len(msg.ranges), 10): pz = 1.0 obs_range = msg.ranges[i] obs_bearing = i * msg.angle_increment + msg.angle_min if math.isnan(obs_range): continue if obs_range >= msg.range_max: continue # compute the endpoint of the laser end_x = p.x + obs_range * math.cos(p.theta + obs_bearing) end_y = p.y + obs_range * math.sin(p.theta + obs_bearing) ind = self.get_map_index(end_x, end_y) if math.isnan(ind): z = self.laser_max_distance else: z = self.closest_occ[ind] pz += self.z_hit * math.exp(-(z * z) / z_hit_denom) pz += self.z_rand * z_rand_mult new_prob += pz**3 p.w = new_prob @staticmethod def normalize(z): return math.atan2(math.sin(z), math.cos(z)) @staticmethod def angle_diff(a, b): a = MyAMCL.normalize(a) b = MyAMCL.normalize(b) d1 = a - b d2 = 2 * math.pi - math.fabs(d1) if d1 > 0: d2 *= -1.0 if math.fabs(d1) < math.fabs(d2): return d1 else: return d2 @staticmethod def weighted_values(values, probabilities, size): bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod def convert_pose_to_xy_and_theta(pose): orientation_tuple = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) angles = euler_from_quaternion(orientation_tuple) return (pose.position.x, pose.position.y, angles[2]) def initialize_particle_cloud(self): self.particle_cloud_initialized = True (x, y, theta) = MyAMCL.convert_pose_to_xy_and_theta(self.odom_pose.pose) for i in range(self.n_particles): self.particle_cloud.append( Particle(x=x + gauss(0, .25), y=y + gauss(0, .25), theta=theta + gauss(0, .25))) self.normalize_particles() def normalize_particles(self): z = 0.0 for p in self.particle_cloud: z += p.w for i in range(len(self.particle_cloud)): self.particle_cloud[i].w /= z def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id="map"), poses=particles_conv)) def scan_received(self, msg): if not (self.initialized): return if not (self.tf_listener.canTransform( "base_footprint", msg.header.frame_id, msg.header.stamp)): return if not (self.tf_listener.canTransform("base_footprint", "odom", msg.header.stamp)): return p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose("base_footprint", p) p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id="base_footprint"), pose=Pose()) #p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id="base_footprint"), pose=Pose(position=Point(x=0.0,y=0.0,z=0.0),orientation=Quaternion(x=0.0,y=0.0,z=0.0,w=0.0))) self.odom_pose = self.tf_listener.transformPose("odom", p) new_odom_xy_theta = MyAMCL.convert_pose_to_xy_and_theta( self.odom_pose.pose) if not (self.particle_cloud_initialized): self.initialize_particle_cloud() self.update_robot_pose() self.current_odom_xy_theta = new_odom_xy_theta self.fix_map_to_odom_transform(msg) else: delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) if math.fabs(delta[0]) > self.d_thresh or math.fabs( delta[1]) > self.d_thresh or math.fabs( delta[2]) > self.a_thresh: self.update_particles_with_odom(msg) self.update_robot_pose() self.update_particles_with_laser(msg) self.resample_particles() self.update_robot_pose() self.fix_map_to_odom_transform(msg) else: self.fix_map_to_odom_transform(msg, False) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg, recompute_odom_to_map=True): if recompute_odom_to_map: (translation, rotation) = MyAMCL.convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=MyAMCL.convert_translation_rotation_to_pose( translation, rotation), header=Header(stamp=msg.header.stamp, frame_id="base_footprint")) self.odom_to_map = self.tf_listener.transformPose("odom", p) (translation, rotation) = MyAMCL.convert_pose_inverse_transform( self.odom_to_map.pose) self.tf_broadcaster.sendTransform( translation, rotation, msg.header.stamp + rospy.Duration(1.0), "odom", "map") @staticmethod def convert_translation_rotation_to_pose(translation, rotation): return Pose(position=Point(x=translation[0], y=translation[1], z=translation[2]), orientation=Quaternion(x=rotation[0], y=rotation[1], z=rotation[2], w=rotation[3])) @staticmethod def convert_pose_inverse_transform(pose): translation = np.zeros((4, 1)) translation[0] = -pose.position.x translation[1] = -pose.position.y translation[2] = -pose.position.z translation[3] = 1.0 rotation = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) euler_angle = euler_from_quaternion(rotation) rotation = np.transpose(rotation_matrix( euler_angle[2], [0, 0, 1])) # the angle is a yaw transformed_translation = rotation.dot(translation) translation = (transformed_translation[0], transformed_translation[1], transformed_translation[2]) rotation = quaternion_from_matrix(rotation) return (translation, rotation)
class CF(): def __init__(self, i): self.worldFrame = rospy.get_param("~worldFrame", "/world") self.frame = 'crazyflie%d' % i self.zscale = 3 self.state = 0 self.position = [] self.orientation = [] self.pref = [] self.cmd_vel = [] self.Imu = [] self.goal = PoseStamped() self.goal.header.seq = 0 self.goal.header.stamp = rospy.Time.now() pub_name = '/crazyflie%d/goal' % i sub_name = '/crazyflie%d/CF_state' % i pub_cmd_diff = '/crazyflie%d/cmd_diff' % i sub_cmd_vel = '/crazyflie%d/cmd_vel' % i sub_imu_data = '/crazyflie%d/imu' % i self.pubGoal = rospy.Publisher(pub_name, PoseStamped, queue_size=1) self.pubCmd_diff = rospy.Publisher(pub_cmd_diff, Twist, queue_size=1) self.subCmd_vel = rospy.Subscriber(sub_cmd_vel, Twist, self.cmdCallback) self.subGoal = rospy.Subscriber(pub_name, PoseStamped, self.GoalCallback) self.subImu = rospy.Subscriber(sub_imu_data, Imu, self.ImuCallback) self.subState = rospy.Subscriber(sub_name, String, self.CfsCallback) self.listener = TransformListener() self.listener.waitForTransform(self.worldFrame, self.frame, rospy.Time(), rospy.Duration(5.0)) self.updatepos() self.send_cmd_diff() def CfsCallback(self, sdata): self.state = int(sdata.data) def ImuCallback(self, sdata): accraw = sdata.linear_acceleration imuraw = np.array([accraw.x, -accraw.y, -accraw.z]) self.updatepos() imuraw = cal_R(self.orientation[1], self.orientation[0], self.orientation[2]).dot(imuraw) self.Imu = np.array([imuraw[0], imuraw[1], 9.88 + imuraw[2]]) def GoalCallback(self, gdata): self.goal = gdata def cmdCallback(self, cdata): self.cmd_vel = cdata def hover_init(self, pnext, s): goal = PoseStamped() goal.header.seq = self.goal.header.seq + 1 goal.header.frame_id = self.worldFrame goal.header.stamp = rospy.Time.now() if self.state != 1: goal.pose.position.x = pnext[0] goal.pose.position.y = pnext[1] goal.pose.position.z = pnext[2] quaternion = tf.transformations.quaternion_from_euler(0, 0, 0) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] self.pubGoal.publish(goal) #print "Waiting for crazyflie %d to take off" %i else: t = self.listener.getLatestCommonTime(self.worldFrame, self.frame) if self.listener.canTransform(self.worldFrame, self.frame, t): position, quaternion = self.listener.lookupTransform( self.worldFrame, self.frame, t) rpy = tf.transformations.euler_from_quaternion(quaternion) dx = pnext[0] - position[0] dy = pnext[1] - position[1] dz = pnext[2] - position[2] dq = 0 - rpy[2] s = max(s, 0.5) goal.pose.position.x = position[0] + s * dx goal.pose.position.y = position[1] + s * dy goal.pose.position.z = position[2] + s * dz quaternion = tf.transformations.quaternion_from_euler( 0, 0, rpy[2] + s * dq) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] self.pubGoal.publish(goal) error = sqrt(dx**2 + dy**2 + dz**2) print 'Hovering error is %0.2f' % error if error < 0.1: return 1 return 0 def updatepos(self): t = self.listener.getLatestCommonTime(self.worldFrame, self.frame) if self.listener.canTransform(self.worldFrame, self.frame, t): self.position, quaternion = self.listener.lookupTransform( self.worldFrame, self.frame, t) rpy = tf.transformations.euler_from_quaternion(quaternion) self.orientation = rpy def goto(self, pnext): goal = PoseStamped() goal.header.stamp = rospy.Time.now() goal.header.seq = self.goal.header.seq + 1 goal.header.frame_id = self.worldFrame goal.pose.position.x = pnext[0] goal.pose.position.y = pnext[1] goal.pose.position.z = pnext[2] quaternion = tf.transformations.quaternion_from_euler(0, 0, 0) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] self.pubGoal.publish(goal) def gotoT(self, pnext, s): goal = PoseStamped() goal.header.stamp = rospy.Time.now() goal.header.seq = self.goal.header.seq + 1 goal.header.frame_id = self.worldFrame t = self.listener.getLatestCommonTime(self.worldFrame, self.frame) if self.listener.canTransform(self.worldFrame, self.frame, t): position, quaternion = self.listener.lookupTransform( self.worldFrame, self.frame, t) rpy = tf.transformations.euler_from_quaternion(quaternion) dx = pnext[0] - position[0] dy = pnext[1] - position[1] dz = pnext[2] - position[2] dq = 0 - rpy[2] goal.pose.position.x = position[0] + s * dx goal.pose.position.y = position[1] + s * dy goal.pose.position.z = position[2] + s * dz quaternion = tf.transformations.quaternion_from_euler( 0, 0, rpy[2] + s * dq) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] self.pubGoal.publish(goal) error = sqrt(dx**2 + dy**2 + dz**2) print 'error is %0.2f' % error if error < 0.1: return 1 else: return 0 def send_cmd_diff(self, roll=0, pitch=0, yawrate=0, thrust=49000): # note theoretical default thrust is 39201 equal to 35.89g lifting force # actual 49000 is 35.89 msg = Twist() msg.linear.x = -pitch #note vx is -pitch, see crazyflie_server.cpp line 165 msg.linear.y = roll #note vy is roll msg.linear.z = thrust msg.angular.z = yawrate self.pubCmd_diff.publish(msg)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 300 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # TODO: define additional constants if needed #### DELETE BEFORE POSTING self.alpha1 = 0.2 self.alpha2 = 0.2 self.alpha3 = 0.2 self.alpha4 = 0.2 self.z_hit = 0.5 self.z_rand = 0.5 self.sigma_hit = 0.1 ##### DELETE BEFORE POSTING # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # request the map # Difficulty level 2 rospy.wait_for_service("static_map") static_map = rospy.ServiceProxy("static_map", GetMap) try: map = static_map().map except: print "error receiving map" self.occupancy_field = OccupancyField(map) self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ """ Difficulty level 2 """ # first make sure that the particle weights are normalized self.normalize_particles() use_mean = True if use_mean: mean_x = 0.0 mean_y = 0.0 mean_theta = 0.0 theta_list = [] weighted_orientation_vec = np.zeros((2,1)) for p in self.particle_cloud: mean_x += p.x*p.w mean_y += p.y*p.w weighted_orientation_vec[0] += p.w*math.cos(p.theta) weighted_orientation_vec[1] += p.w*math.sin(p.theta) mean_theta = math.atan2(weighted_orientation_vec[1],weighted_orientation_vec[0]) self.robot_pose = Particle(x=mean_x,y=mean_y,theta=mean_theta).as_pose() else: weights = [] for p in self.particle_cloud: weights.append(p.w) best_particle = np.argmax(weights) self.robot_pose = self.particle_cloud[best_particle].as_pose() def update_particles_with_odom(self, msg): """ Implement a simple version of this (Level 1) or a more complex one (Level 2) """ new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # Implement sample_motion_odometry (Prob Rob p 136) # Avoid computing a bearing from two poses that are extremely near each # other (happens on in-place rotation). delta_trans = math.sqrt(delta[0]*delta[0] + delta[1]*delta[1]) if delta_trans < 0.01: delta_rot1 = 0.0 else: delta_rot1 = ParticleFilter.angle_diff(math.atan2(delta[1], delta[0]), old_odom_xy_theta[2]) delta_rot2 = ParticleFilter.angle_diff(delta[2], delta_rot1) # We want to treat backward and forward motion symmetrically for the # noise model to be applied below. The standard model seems to assume # forward motion. delta_rot1_noise = min(math.fabs(ParticleFilter.angle_diff(delta_rot1, 0.0)), math.fabs(ParticleFilter.angle_diff(delta_rot1, math.pi))); delta_rot2_noise = min(math.fabs(ParticleFilter.angle_diff(delta_rot2, 0.0)), math.fabs(ParticleFilter.angle_diff(delta_rot2, math.pi))); for sample in self.particle_cloud: # Sample pose differences delta_rot1_hat = ParticleFilter.angle_diff(delta_rot1, gauss(0, self.alpha1*delta_rot1_noise*delta_rot1_noise + self.alpha2*delta_trans*delta_trans)) delta_trans_hat = delta_trans - gauss(0, self.alpha3*delta_trans*delta_trans + self.alpha4*delta_rot1_noise*delta_rot1_noise + self.alpha4*delta_rot2_noise*delta_rot2_noise) delta_rot2_hat = ParticleFilter.angle_diff(delta_rot2, gauss(0, self.alpha1*delta_rot2_noise*delta_rot2_noise + self.alpha2*delta_trans*delta_trans)) # Apply sampled update to particle pose sample.x += delta_trans_hat * math.cos(sample.theta + delta_rot1_hat) sample.y += delta_trans_hat * math.sin(sample.theta + delta_rot1_hat) sample.theta += delta_rot1_hat + delta_rot2_hat def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ pass def resample_particles(self): self.normalize_particles() values = np.empty(self.n_particles) probs = np.empty(self.n_particles) for i in range(len(self.particle_cloud)): values[i] = i probs[i] = self.particle_cloud[i].w new_particle_indices = ParticleFilter.weighted_values(values,probs,self.n_particles) new_particles = [] for i in new_particle_indices: idx = int(i) s_p = self.particle_cloud[idx] new_particles.append(Particle(x=s_p.x+gauss(0,.025),y=s_p.y+gauss(0,.025),theta=s_p.theta+gauss(0,.025))) self.particle_cloud = new_particles self.normalize_particles() # Difficulty level 1 def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ laser_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.laser_pose.pose) for p in self.particle_cloud: adjusted_pose = (p.x+laser_xy_theta[0], p.y+laser_xy_theta[1], p.theta+laser_xy_theta[2]) # Pre-compute a couple of things z_hit_denom = 2*self.sigma_hit**2 z_rand_mult = 1.0/msg.range_max # This assumes quite a bit about the weights beforehand (TODO: could base this on p.w) new_prob = 1.0 # more agressive DEBUG, was 1.0 for i in range(0,len(msg.ranges),6): pz = 1.0 obs_range = msg.ranges[i] obs_bearing = i*msg.angle_increment+msg.angle_min if math.isnan(obs_range): continue if obs_range >= msg.range_max: continue # compute the endpoint of the laser end_x = p.x + obs_range*math.cos(p.theta+obs_bearing) end_y = p.y + obs_range*math.sin(p.theta+obs_bearing) z = self.occupancy_field.get_closest_obstacle_distance(end_x,end_y) if math.isnan(z): z = self.laser_max_distance else: z = z[0] # not sure why this is happening pz += self.z_hit * math.exp(-(z * z) / z_hit_denom) / (math.sqrt(2*math.pi)*self.sigma_hit) pz += self.z_rand * z_rand_mult new_prob += pz**3 p.w = new_prob pass @staticmethod def angle_normalize(z): """ convenience function to map an angle to the range [-pi,pi] """ return math.atan2(math.sin(z), math.cos(z)) @staticmethod def angle_diff(a, b): """ Calculates the difference between angle a and angle b (both should be in radians) the difference is always based on the closest rotation from angle a to angle b examples: angle_diff(.1,.2) -> -.1 angle_diff(.1, 2*math.pi - .1) -> .2 angle_diff(.1, .2+2*math.pi) -> -.1 """ a = ParticleFilter.angle_normalize(a) b = ParticleFilter.angle_normalize(b) d1 = a-b d2 = 2*math.pi - math.fabs(d1) if d1 > 0: d2 *= -1.0 if math.fabs(d1) < math.fabs(d2): return d1 else: return d2 @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements form the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] for i in range(self.n_particles): self.particle_cloud.append(Particle(x=xy_theta[0]+gauss(0,.25),y=xy_theta[1]+gauss(0,.25),theta=xy_theta[2]+gauss(0,.25))) self.normalize_particles() self.update_robot_pose() """ Level 1 """ def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ z = 0.0 for p in self.particle_cloud: z += p.w for i in range(len(self.particle_cloud)): self.particle_cloud[i].w /= z def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),frame_id=self.map_frame),poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0),frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.resample_particles() # resample particles to focus on areas of high density self.update_robot_pose() # update robot's pose self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """ (translation, rotation) = TransformHelpers.convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=msg.header.stamp,frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class Follow(): def __init__(self, goals): rospy.init_node('follow', anonymous=True) self.worldFrame = rospy.get_param("~worldFrame", "/world") self.frame = rospy.get_param("~frame") self.goal = rospy.get_param("~goal") self.x = rospy.get_param("~x") self.y = rospy.get_param("~y") self.z = rospy.get_param("~z") self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1) self.listener = TransformListener() self.goals = goals self.goalIndex = 0 def run(self): self.listener.waitForTransform(self.worldFrame, self.frame, rospy.Time(), rospy.Duration(5.0)) goal = PoseStamped() goal.header.seq = 0 goal.header.frame_id = self.worldFrame while not rospy.is_shutdown(): goal.header.seq += 1 goal.header.stamp = rospy.Time.now() goal.pose.position.x = self.x goal.pose.position.y = self.y goal.pose.position.z = self.z quaternion = tf.transformations.quaternion_from_euler(0, 0, 0) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] self.pubGoal.publish(goal) t = self.listener.getLatestCommonTime(self.worldFrame, self.frame) if self.listener.canTransform(self.worldFrame, self.frame, t): position, quaternion = self.listener.lookupTransform( self.worldFrame, self.frame, t) rpy = tf.transformations.euler_from_quaternion(quaternion) #rospy.loginfo(rpy) if math.fabs(position[0] - self.x) < 0.25 \ and math.fabs(position[1] - self.y) < 0.25 \ and math.fabs(position[2] - self.z) < 0.25 \ and math.fabs(rpy[2] - 0) < math.radians(10): rospy.sleep(3) self.goalIndex += 1 break while not rospy.is_shutdown(): goal.header.seq += 1 goal.header.stamp = rospy.Time.now() quaternion = tf.transformations.quaternion_from_euler(0, 0, 0) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] t = self.listener.getLatestCommonTime(self.worldFrame, self.goal) if self.listener.canTransform(self.worldFrame, self.goal, t): position, quaternion = self.listener.lookupTransform( self.worldFrame, self.goal, t) goal.pose.position.x = position[0] - 0.8 * math.sin(rpy[2]) goal.pose.position.y = position[1] + 0.8 * math.cos(rpy[2]) goal.pose.position.z = position[2] + 0.5 rpy = tf.transformations.euler_from_quaternion(quaternion) #rospy.loginfo(rpy) self.pubGoal.publish(goal)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node( 'pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 300 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) self.weight_pub = rospy.Publisher('visualization_marker', MarkerArray, queue_size=10) # laser_subscriber listens for data from the lidar rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() # Holds all particles self.particle_cloud = [] # Holds pre-normalized probabilities for each particle self.scan_probabilities = [] # change use_projected_stable_scan to True to use point clouds instead of laser scans self.use_projected_stable_scan = False self.last_projected_stable_scan = None if self.use_projected_stable_scan: # subscriber to the odom point cloud rospy.Subscriber("projected_stable_scan", PointCloud, self.projected_scan_received) self.current_odom_xy_theta = [] self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() self.initialized = True def update_robot_pose(self, timestamp): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() # Calculate the mean pose if self.particle_cloud: mean_x, mean_y, mean_theta = 0, 0, 0 for particle in self.particle_cloud: mean_x += particle.x mean_y += particle.y mean_theta += particle.theta mean_x /= len(self.particle_cloud) mean_y /= len(self.particle_cloud) mean_theta /= len(self.particle_cloud) self.robot_pose = Particle(mean_x, mean_y, mean_theta).as_pose() else: self.robot_pose = Pose() self.transform_helper.fix_map_to_odom_transform( self.robot_pose, timestamp) def projected_scan_received(self, msg): self.last_projected_stable_scan = msg def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # Modify particles using delta and inject noise. for particle in self.particle_cloud: # Step 1: turn particles in direction of translation # Compute the unit vector of the desired heading to move in heading_mag = math.sqrt(delta[0]**2 + delta[1]**2) heading_uv = np.array( [delta[0] / heading_mag, delta[1] / heading_mag]) # Compute the unit vector of the robot's current heading robot_uv = np.array([ np.cos(self.current_odom_xy_theta[2]), np.sin(self.current_odom_xy_theta[2]) ]) # Calculate the angle r_1 that is between the current heading and target heading r_1 = np.arccos(np.dot(robot_uv, heading_uv)) particle.theta += r_1 + np.random.normal(scale=.05) # Step 2: move particles forward distance of translation d = math.sqrt(delta[0]**2 + delta[1]**2) + np.random.normal(scale=.05) # Decompose the translation vector into x and y componenets particle.x += d * np.cos(particle.theta) particle.y += d * np.sin(particle.theta) # Step 3: turn particles to final angle r_2 = delta[2] - r_1 particle.theta += r_2 def map_calc_range(self, x, y, theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized self.normalize_particles() weights = [] for particle in self.particle_cloud: weights.append(particle.w) choices = self.draw_random_sample(self.particle_cloud, weights, self.n_particles) # Reset particle cloud self.particle_cloud = [] # Populate particle cloud with sampled choices for chosen_particle in choices: self.particle_cloud.append( Particle(chosen_particle.x, chosen_particle.y, chosen_particle.theta, chosen_particle.w)) def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ lidar_scan_angles = range(360) # Populates lidar_scan list with (theta, distance) for each lidar scan angle lidar_scan = [] for theta in lidar_scan_angles: distance = msg.ranges[theta] point = (theta, distance) lidar_scan.append(point) # Calculates the probability that each particle is the best estimate for the robot location self.scan_probabilities = [] for p in self.particle_cloud: particle_theta_prob = [] for point in lidar_scan: x_vector = p.x + point[1] * math.cos( math.radians(point[0]) + p.theta) y_vector = p.y + point[1] * math.sin( math.radians(point[0]) + p.theta) closest_object = self.occupancy_field.get_closest_obstacle_distance( x_vector, y_vector) # Calculate probabilities using a tuned function particle_theta_prob.append(1 / ((0.1 * closest_object)**2 + 1)) # Combine probability at every theta for every particle self.scan_probabilities.append( reduce(lambda a, b: a * b, particle_theta_prob)) @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( msg.pose.pose) self.initialize_particle_cloud(msg.header.stamp, xy_theta) def initialize_particle_cloud(self, timestamp, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is omitted, the odometry will be used """ if xy_theta is None: xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) # Create particles based on gaussian distribution centered around xy_theta self.particle_cloud = [] for g in range(self.n_particles): x = np.random.normal(xy_theta[0], scale=0.3) y = np.random.normal(xy_theta[1], scale=0.3) theta = np.random.normal(xy_theta[2], scale=0.1) self.particle_cloud.append(Particle(x, y, theta, 1)) self.normalize_particles() self.update_robot_pose(timestamp) def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ # Check if scan probabilities has been populated for each particle if len(self.scan_probabilities) == len(self.particle_cloud): sum_of_prob = sum(self.scan_probabilities) for i, particle in enumerate(self.particle_cloud): particle.w = self.scan_probabilities[i] / sum_of_prob def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def publish_weights(self, msg): # Visualize particle weights in rviz to get a better debug each particle weight_markers = MarkerArray() for i, particle in enumerate(self.particle_cloud): weight_markers.markers.append(particle.as_marker(i)) self.weight_pub.publish(weight_markers) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, we hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return # wait a little while to see if the transform becomes available. This fixes a race # condition where the scan would arrive a little bit before the odom to base_link transform # was updated. self.tf_listener.waitForTransform(self.base_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(0.5)) if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative to the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) if not self.current_odom_xy_theta: self.current_odom_xy_theta = new_odom_xy_theta return if not (self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud(msg.header.stamp) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry if self.last_projected_stable_scan: last_projected_scan_timeshift = deepcopy( self.last_projected_stable_scan) last_projected_scan_timeshift.header.stamp = msg.header.stamp self.scan_in_base_link = self.tf_listener.transformPointCloud( "base_link", last_projected_scan_timeshift) self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose(msg.header.stamp) # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density # publish particles (so things like rviz can see them) self.publish_particles(msg) self.publish_weights(msg)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 100 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model self.sigma = 0.08 # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) self.marker_pub = rospy.Publisher("markers", MarkerArray, queue_size=10) # laser_subscriber listens for data from the lidar # Dados do Laser: Mapa de verossimilhança/Occupancy field/Likehood map e Traçado de raios. # Traçado de raios: Leitura em ângulo que devolve distância, através do sensor. Dado o mapa, # extender a direção da distância pra achar um obstáculo. self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms #atualização de posição(odometria) self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] #Chamar o mapa a partir de funcao externa mapa = chama_mapa.obter_mapa() # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid # TODO: fill in the appropriate service call here. The resultant map should be assigned be passed # into the init method for OccupancyField # for now we have commented out the occupancy field initialization until you can successfully fetch the map self.occupancy_field = OccupancyField(mapa) self.initialized = True def update_robot_pose(self): print("Update Robot Pose") """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() # TODO: assign the lastest pose into self.robot_pose as a geometry_msgs.Pose object # just to get started we will fix the robot's pose to always be at the origin #Variaveis para soma do X,Y e angulo Theta da particula x_sum = 0 y_sum = 0 theta_sum = 0 #Loop de soma para as variaveis relativas a probabilidade da particula for p in self.particle_cloud: x_sum += p.x * p.w y_sum += p.y * p.w theta_sum += p.theta * p.w #Atributo robot_pose eh o pose da melhor particula possivel a partir das variaveis de soma self.robot_pose = Particle(x=x_sum, y=y_sum, theta=theta_sum).as_pose() def update_particles_with_odom(self,msg): print("Update Particles with Odom") """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta #R eh o raio feito a partir de um pitagoras com o X e Y da variacao Delta r = math.sqrt(delta[0]**2 + delta[1]**2) #Funcao para conseguir um valor entre -pi e pi e subtrair o antigo valor de theta. (Achei um pouco confusa, ) phi = math.atan2(delta[1], delta[0])-old_odom_xy_theta[2] for particle in self.particle_cloud: particle.x += r*math.cos(phi+particle.theta) particle.y += r*math.sin(phi+particle.theta) particle.theta += delta[2] # TODO: modify particles using delta # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ #Primeiro de tudo, normalizar particulas self.normalize_particles() #Criar array do numpy vazia do tamanho do numero de particulas. values = np.empty(self.n_particles) #Preencher essa lista com os indices das particulas for i in range(self.n_particles): values[i] = i #Criar uma lista para novas particulas new_particles = [] #Criar lista com os indices das particulas com mais probabilidade random_particles = ParticleFilter.weighted_values(values,[p.w for p in self.particle_cloud],self.n_particles) for i in random_particles: #Transformar o I em inteiro para corrigir bug de float int_i = int(i) #Pegar particula na possicao I na nuvem de particulas. p = self.particle_cloud[int_i] #Adicionar particulas somando um valor aleatorio da distribuicao gauss com media = 0 e desvio padrao = 0.025 new_particles.append(Particle(x=p.x+gauss(0,.025),y=p.y+gauss(0,.025),theta=p.theta+gauss(0,.025))) #Igualar nuvem de particulas a novo sample criado self.particle_cloud = new_particles #Normalizar mais uma vez as particulas. self.normalize_particles() def update_particles_with_laser(self, msg): print("Update Particles with laser") """ Updates the particle weights in response to the scan contained in the msg """ for p in self.particle_cloud: for i in range(360): #Distancia no angulo I distancia = msg.ranges[i] x = distancia * math.cos(i + p.theta) y = distancia * math.sin(i + p.theta) #Verificar se distancia minima eh diferente de nan erro_nan = self.occupancy_field.get_closest_obstacle_distance(x,y) if erro_nan is not float('nan'): # Adicionar valor para corrigir erro de nan (Retirado de outro codigo) p.w += math.exp(-erro_nan*erro_nan/(2*self.sigma**2)) #Normalizar particulas e criar um novo sample das mesmas self.normalize_particles() self.resample_particles() @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod #Nao estou usando esse metodo. Apenas o weighted_values def draw_random_sample(choices, probabilities, n): print("Draw Random Sample") """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): print("Update Initial Pose") """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] # TODO create particles # Criando particula print("Inicializacao da Cloud de Particulas") #Valor auxiliar para nao dar erro na hora de criacao das particulas. Irrelevante valor_aux = 0.3 for i in range (self.n_particles): self.particle_cloud.append(Particle(0, 0, 0, valor_aux)) # Randomizar particulas em volta do robo. for i in self.particle_cloud: i.x = xy_theta[0]+ random.uniform(-1,1) i.y = xy_theta[1]+ random.uniform(-1,1) i.theta = xy_theta[2]+ random.uniform(-45,45) #Normalizar as particulas e dar update na posicao do robo self.normalize_particles() self.update_robot_pose() print(xy_theta) def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ #Soma total das probabilidades das particulas w_sum = sum([p.w for p in self.particle_cloud]) #Dividir cada probabilidade de uma particula pela soma total for particle in self.particle_cloud: particle.w /= w_sum # TODO: implement this def publish_particles(self, msg): print("Publicar Particulas.") print(len(self.particle_cloud)) particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): print("Not Initialized") # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,rospy.Time(0))): print("Not 2") # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,rospy.Time(0))): print("Not 3") # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp = rospy.Time(0), frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) print("PASSOU") if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles() # resample particles to focus on areas of high density self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) # direcionar particulas quando um obstaculo é identificado. def fix_map_to_odom_transform(self, msg): print("Fix Map to Odom Transform") """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation), header=Header(stamp=rospy.Time(0),frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): print("Broadcast") """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class Generate_Point(): def __init__(self, point_num, delta, radius=1, deltaX=0, deltaY=0): rospy.init_node('demo', anonymous=True) self.worldFrame = rospy.get_param("~worldFrame", "/world") self.frame = rospy.get_param("~frame") self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1) self.listener = TransformListener() rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback) self.delta = delta self.point_num = point_num self.takeoffFlag = 0 self.radius = radius self.deltaX = deltaX self.deltaY = deltaY self.goalIndex = 0 rospy.loginfo("demo start!!!!!!!") def cmdVelCallback(self, data): if data.linear.z != 0.0 and self.takeoffFlag == 0: self.takeoffFlag = 1 rospy.sleep(10) self.takeoffFlag = 2 def run(self): self.listener.waitForTransform(self.worldFrame, self.frame, rospy.Time(), rospy.Duration(10.0)) goal = PoseStamped() goal.header.seq = 0 goal.header.frame_id = self.worldFrame while not rospy.is_shutdown(): self.circle_generate(goal, self.goalIndex) self.pubGoal.publish(goal) t = self.listener.getLatestCommonTime(self.worldFrame, self.frame) if self.listener.canTransform(self.worldFrame, self.frame, t): position, quaternion = self.listener.lookupTransform( self.worldFrame, self.frame, t) rpy = tf.transformations.euler_from_quaternion(quaternion) if self.takeoffFlag == 1: self.goalIndex = 0 elif self.takeoffFlag == 2 and self.goalIndex < self.point_num - 1: rospy.sleep(0.02) rospy.loginfo(self.goalIndex) self.goalIndex += 1 def circle_generate(self, goal, index): circle = 5 point_num = self.point_num delta = self.delta radius = self.radius offset_x = self.deltaX offset_y = self.deltaY angle = circle * index * 2 * math.pi / point_num + delta x = radius * math.cos(angle) + offset_x y = radius * math.sin(angle) + offset_y goal.header.seq += 1 goal.header.stamp = rospy.Time.now() goal.pose.position.x = x goal.pose.position.y = y goal.pose.position.z = 0.8 quaternion = tf.transformations.quaternion_from_euler(0, 0, 0) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3]
class ORKTabletop(object): """ Listens to the table and object messages from ORK. Provides ActionServer that assembles table and object into same message. NB - the table is an axis-aligned bounding box in the kinect's frame""" def __init__(self, name): self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.callback) self.pub = rospy.Publisher('/recognized_object_array_as_point_cloud', PointCloud2) self.pose_pub = rospy.Publisher('/recognized_object_array_as_pose_stamped', PoseStamped) # We listen for ORK's MarkerArray of tables on this topic self.table_topic = "/marker_tables" self.tl = TransformListener() # create messages that are used to publish feedback/result. # accessed by multiple threads self._result = TabletopResult() self.result_lock = threading.Lock() # used s.t. we don't return a _result message that hasn't been updated yet. self.has_data = False self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, TabletopAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() # TODO: Is this really the best structure for handling the callbacks? # Would it be possible to have separate callbacks for table and objects, each updating a most-recently-seen variable? # Or maybe use the message_filters.TimeSynchronizer class if corresponding table/object data has identical timestamps? def callback(self, data): rospy.loginfo("Objects %d", data.objects.__len__()) table_corners = [] # obtain table_offset and table_pose to = rospy.wait_for_message(self.table_topic, MarkerArray); # obtain Table corners ... rospy.loginfo("Tables hull size %d", to.markers.__len__()) if not to.markers: rospy.loginfo("No tables detected") return else: # NB - D says that ORK has this set to filter based on height. # IIRC, it's 0.6-0.8m above whatever frame is set as the floor point_array_size = 4 # for the 4 corners of the bounding box for i in range (0, point_array_size): p = Point() p.x = to.markers[0].points[i].x p.y = to.markers[0].points[i].y p.z = to.markers[0].points[i].z table_corners.append(p) # this is a table pose at the edge close to the robot, in the center of x axis table_pose = PoseStamped() table_pose.header = to.markers[0].header table_pose.pose = to.markers[0].pose # Determine table dimensions rospy.loginfo('calculating table pose bounding box in frame: %s' % table_pose.header.frame_id) min_x = sys.float_info.max min_y = sys.float_info.max max_x = -sys.float_info.max max_y = -sys.float_info.max for i in range (table_corners.__len__()): if table_corners[i].x > max_x: max_x = table_corners[i].x if table_corners[i].y > max_y: max_y = table_corners[i].y if table_corners[i].x < min_x: min_x = table_corners[i].x if table_corners[i].y < min_y: min_y = table_corners[i].y table_dim = Point() # TODO: if we don't (can't!) compute the height, should we at least give it non-zero depth? # (would also require shifting the observed centroid down by table_dim.z/2) table_dim.z = 0.0 table_dim.x = abs(max_x - min_x) table_dim.y = abs(max_y - min_y) print "Dimensions: ", table_dim.x, table_dim.y # Temporary frame used for transformations table_link = 'table_link' # centroid of a table in table_link frame centroid = PoseStamped() centroid.header.frame_id = table_link centroid.header.stamp = table_pose.header.stamp centroid.pose.position.x = (max_x + min_x)/2. centroid.pose.position.y = (max_y + min_y)/2. centroid.pose.position.z = 0.0 centroid.pose.orientation.x = 0.0 centroid.pose.orientation.y = 0.0 centroid.pose.orientation.z = 0.0 centroid.pose.orientation.w = 1.0 # generate transform from table_pose to our newly-defined table_link tt = TransformStamped() tt.header = table_pose.header tt.child_frame_id = table_link tt.transform.translation = table_pose.pose.position tt.transform.rotation = table_pose.pose.orientation self.tl.setTransform(tt) self.tl.waitForTransform(table_link, table_pose.header.frame_id, table_pose.header.stamp, rospy.Duration(3.0)) if self.tl.canTransform(table_pose.header.frame_id, table_link, table_pose.header.stamp): centroid_table_pose = self.tl.transformPose(table_pose.header.frame_id, centroid) self.pose_pub.publish(centroid_table_pose) else: rospy.logwarn("No transform between %s and %s possible",table_pose.header.frame_id, table_link) return # transform each object into desired frame; add to list of clusters cluster_list = [] for i in range (data.objects.__len__()): rospy.loginfo("Point clouds %s", data.objects[i].point_clouds.__len__()) pc = PointCloud2() pc = data.objects[i].point_clouds[0] arr = pointclouds.pointcloud2_to_array(pc, 1) arr_xyz = pointclouds.get_xyz_points(arr) arr_xyz_trans = [] for j in range (arr_xyz.__len__()): ps = PointStamped(); ps.header.frame_id = table_link ps.header.stamp = table_pose.header.stamp ps.point.x = arr_xyz[j][0] ps.point.y = arr_xyz[j][1] ps.point.z = arr_xyz[j][2] if self.tl.canTransform(table_pose.header.frame_id, table_link, table_pose.header.stamp): ps_in_kinect_frame = self.tl.transformPoint(table_pose.header.frame_id, ps) else: rospy.logwarn("No transform between %s and %s possible",table_pose.header.frame_id, table_link) return arr_xyz_trans.append([ps_in_kinect_frame.point.x, ps_in_kinect_frame.point.y, ps_in_kinect_frame.point.z]) pc_trans = PointCloud2() pc_trans = pointclouds.xyz_array_to_pointcloud2(np.asarray([arr_xyz_trans]), table_pose.header.stamp, table_pose.header.frame_id) self.pub.publish(pc_trans) cluster_list.append(pc_trans) rospy.loginfo("cluster size %d", cluster_list.__len__()) # finally - save all data in the object that'll be sent in response to actionserver requests with self.result_lock: self._result.objects = cluster_list self._result.table_dims = table_dim self._result.table_pose = centroid_table_pose self.has_data = True def execute_cb(self, goal): rospy.loginfo('Executing ORKTabletop action') # want to get the NEXT data coming in, rather than the current one. with self.result_lock: self.has_data = False rr = rospy.Rate(1.0) while not rospy.is_shutdown() and not self._as.is_preempt_requested(): with self.result_lock: if self.has_data: break rr.sleep() if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted() elif rospy.is_shutdown(): self._as.set_aborted() else: with self.result_lock: rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded(self._result)
class Figure8(): def __init__(self, goals): rospy.init_node('figure8', anonymous=True) self.worldFrame = rospy.get_param("~worldFrame", "/world") self.frame = rospy.get_param("~frame") self.radius = rospy.get_param("~radius") self.freq = rospy.get_param("~freq") self.lap = rospy.get_param("~lap") self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1) self.listener = TransformListener() self.goals = goals self.goalIndex = 0 def run(self): self.listener.waitForTransform(self.worldFrame, self.frame, rospy.Time(), rospy.Duration(5.0)) #rospy.loginfo("start running!") goal = PoseStamped() goal.header.seq = 0 goal.header.frame_id = self.worldFrame while not rospy.is_shutdown(): #rospy.loginfo("start running!") goal.header.seq += 1 goal.header.stamp = rospy.Time.now() goal.pose.position.x = self.goals[self.goalIndex][0] goal.pose.position.y = self.goals[self.goalIndex][1] goal.pose.position.z = self.goals[self.goalIndex][2] quaternion = tf.transformations.quaternion_from_euler(0, 0, self.goals[self.goalIndex][3]) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] self.pubGoal.publish(goal) t = self.listener.getLatestCommonTime(self.worldFrame, self.frame) if self.listener.canTransform(self.worldFrame, self.frame, t): position, quaternion = self.listener.lookupTransform(self.worldFrame, self.frame, t) rpy = tf.transformations.euler_from_quaternion(quaternion) if math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.15 \ and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.15 \ and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.15 \ and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(10) \ and self.goalIndex < len(self.goals) - 2: rospy.sleep(self.goals[self.goalIndex][4]) self.goalIndex += 1 rospy.loginfo("Index:%lf",self.goalIndex) if self.goalIndex == len(self.goals) - 2: break t_start= rospy.Time.now().to_sec() #rospy.loginfo("t_start:%lf",t_start) t_now=t_start while ((not rospy.is_shutdown()) and (t_now-t_start<self.lap/self.freq)): goal.header.seq += 1 goal.header.stamp = rospy.Time.now() goal.pose.position.x = self.goals[self.goalIndex-1][0]+self.radius*math.sin((t_now-t_start)*2*math.pi*2*self.freq) goal.pose.position.y = self.goals[self.goalIndex-1][1]+2*self.radius*math.sin((t_now-t_start)*2*math.pi*self.freq) goal.pose.position.z = self.goals[self.goalIndex-1][2] quaternion = tf.transformations.quaternion_from_euler(0, 0, self.goals[self.goalIndex-1][3]) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] self.pubGoal.publish(goal) t_now= rospy.Time.now().to_sec() rospy.loginfo("t_now-t_start:%lf",t_now-t_start) while not rospy.is_shutdown(): goal.header.seq += 1 goal.header.stamp = rospy.Time.now() goal.pose.position.x = self.goals[self.goalIndex][0] goal.pose.position.y = self.goals[self.goalIndex][1] goal.pose.position.z = self.goals[self.goalIndex][2] quaternion = tf.transformations.quaternion_from_euler(0, 0, self.goals[self.goalIndex][3]) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] self.pubGoal.publish(goal) #rospy.loginfo("Index:%lf",self.goalIndex) t = self.listener.getLatestCommonTime(self.worldFrame, self.frame) if self.listener.canTransform(self.worldFrame, self.frame, t): position, quaternion = self.listener.lookupTransform(self.worldFrame, self.frame, t) rpy = tf.transformations.euler_from_quaternion(quaternion) if math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.15 \ and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.15 \ and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.15 \ and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(10) \ and self.goalIndex < len(self.goals) - 1: rospy.sleep(self.goals[self.goalIndex][4]) self.goalIndex += 1
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter linear_mov: the amount of linear movement before triggering a filter update angular_mov: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('RMI_pf') self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 20 self.linear_mov = 0.1 self.angular_mov = math.pi / 10 self.laser_max_distance = 2.0 self.sigma = 0.05 # Descomentar essa linha caso /initialpose seja publicada # self.pose_listener = rospy.Subscriber("initialpose", # PoseWithCovarianceStamped, # self.update_initial_pose) self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) self.particle_pub = rospy.Publisher("particlecloud_rmi", PoseArray, queue_size=1) self.tf_listener = TransformListener() self.particle_cloud = [] self.current_odom_xy_theta = [] self.map_server = rospy.ServiceProxy('static_map', GetMap) self.map = self.map_server().map self.occupancy_field = OccupancyField(self.map) self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(1.0)) self.initialized = True def update_particles_with_odom(self, msg): new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # print 'new_odom_xy_theta', new_odom_xy_theta # Pega a posicao da odom (x,y,tehta) if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta # print 'delta', delta else: self.current_odom_xy_theta = new_odom_xy_theta return for particle in self.particle_cloud: d = math.sqrt((delta[0]**2) + (delta[1]**2)) # print 'particle_theta_1', particle.theta particle.x += d * (math.cos(particle.theta) + normal(0, 0.01)) particle.y += d * (math.sin(particle.theta) + normal(0, 0.01)) particle.theta = self.current_odom_xy_theta[2] #+ normal(0,0.05) # Systematic Resample def resample_particles(self): self.normalize_particles() # for particle in self.particle_cloud: # print 'TODAS PART', particle.w, particle.x, particle.y weights = [] for particle in self.particle_cloud: weights.append(particle.w) newParticles = [] N = len(weights) positions = (np.arange(N) + random.random()) / N cumulative_sum = np.cumsum(weights) i, j = 0, 0 while i < N: if positions[i] < cumulative_sum[j]: newParticles.append(deepcopy(self.particle_cloud[j])) i += 1 else: j += 1 self.particle_cloud = newParticles def update_particles_with_laser(self, msg): depths = [] for dist in msg.ranges: if not np.isnan(dist): depths.append(dist) fullDepthsArray = msg.ranges[:] # Verifica se ha objetos proximos ao robot if len(depths) == 0: self.closest = 0 self.position = 0 else: self.closest = min(depths) self.position = fullDepthsArray.index(self.closest) # print 'self.position, self.closest', self.position, self.closest, self.xy_theta_aux # print msg, '/scan' for index, particle in enumerate(self.particle_cloud): tot_prob = 0.0 for index, scan in enumerate(depths): x, y = self.transform_scan(particle, scan, index) # print 'x,y, scan', x, y, scan # usa o metodo get_closest_obstacle_distance para buscar o obstaculo mais proximo dentro do range x,y do grid map d = self.occupancy_field.get_closest_obstacle_distance(x, y) # quanto mais proximo de zero mais relevante tot_prob += math.exp((-d**2) / (2 * self.sigma**2)) tot_prob = tot_prob / len(depths) if math.isnan(tot_prob): particle.w = 1.0 else: particle.w = tot_prob # print 'LASER', particle.x, particle.y, particle.w def transform_scan(self, particle, distance, theta): return (particle.x + distance * math.cos(math.radians(particle.theta + theta)), particle.y + distance * math.sin(math.radians(particle.theta + theta))) def update_initial_pose(self, msg): xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) def initialize_particle_cloud(self, xy_theta=None): print 'Cria o set inicial de particulas' if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) x, y, theta = xy_theta # Altere este parametro para aumentar a circunferencia do filtro de particulas # Na VM ate 1 e suportado rad = 0.5 self.particle_cloud = [] self.particle_cloud.append( Particle(xy_theta[0], xy_theta[1], xy_theta[2])) # print 'particle_values_W', self.particle_cloud[0].w # print 'particle_values_X', self.particle_cloud[0].x # print 'particle_values_Y', self.particle_cloud[0].y # print 'particle_values_THETA', self.particle_cloud[0].theta for i in range(self.n_particles - 1): # initial facing of the particle theta = random.random() * 360 # compute params to generate x,y in a circle other_theta = random.random() * 360 radius = random.random() * rad # x => straight ahead x = radius * math.sin(other_theta) + xy_theta[0] y = radius * math.cos(other_theta) + xy_theta[1] particle = Particle(x, y, theta) self.particle_cloud.append(particle) self.normalize_particles() def normalize_particles(self): tot_weight = sum([particle.w for particle in self.particle_cloud]) or 1.0 for particle in self.particle_cloud: particle.w = particle.w / tot_weight def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose( )) # transforma a particula em POSE para ser entendida pelo ROS # print 'PARTII', [particles.x for particles in self.particle_cloud] # Publica as particulas no rviz (particloud_rmi) self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): # print msg """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # print 'msg.header.frame_id', msg.header.frame_id # calculate pose of laser relative ot the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry # listener.getLatestCommonTime("/base_link",object_pose_in.header.frame_id) # p = PoseStamped(header=Header(stamp=msg.header.stamp, p = PoseStamped( header=Header( stamp=self.tf_listener.getLatestCommonTime( self.base_frame, self.map_frame), # p = PoseStamped(header=Header(stamp=rospy.Time.now(), frame_id=self.base_frame), pose=Pose()) # p_aux = PoseStamped(header=Header(stamp=self.tf_listener.getLatestCommonTime("/base_link","/map"), p_aux = PoseStamped( header=Header( stamp=self.tf_listener.getLatestCommonTime( self.odom_frame, self.map_frame), # p_aux = PoseStamped(header=Header(stamp=rospy.Time.now(), frame_id=self.odom_frame), pose=Pose()) odom_aux = self.tf_listener.transformPose(self.map_frame, p_aux) odom_aux_xy_theta = convert_pose_to_xy_and_theta(odom_aux.pose) # print 'odom_aux_xy_theta', odom_aux_xy_theta self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # print 'self.odom_pose', self.odom_pose # (trans, root) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) # self.odom_pose = trans # print trans, root new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # new_odom_xy_theta = convert_pose_to_xy_and_theta(self.laser_pose.pose) xy_theta_aux = (new_odom_xy_theta[0] + odom_aux_xy_theta[0], new_odom_xy_theta[1] + odom_aux_xy_theta[1], new_odom_xy_theta[2]) self.xy_theta_aux = xy_theta_aux if not (self.particle_cloud): self.initialize_particle_cloud(xy_theta_aux) self.current_odom_xy_theta = new_odom_xy_theta elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.linear_mov or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.linear_mov or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.angular_mov): self.update_particles_with_odom(msg) self.update_particles_with_laser(msg) self.resample_particles() self.publish_particles(msg)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node( 'pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 500 # the number of particles to use self.d_thresh = 0.1 # the amount of linear movement before performing an update self.a_thresh = math.pi / 12 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # laser_subscriber listens for data from the lidar rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] # change use_projected_stable_scan to True to use point clouds instead of laser scans self.use_projected_stable_scan = False self.last_projected_stable_scan = None if self.use_projected_stable_scan: # subscriber to the odom point cloud rospy.Subscriber("projected_stable_scan", PointCloud, self.projected_scan_received) self.current_odom_xy_theta = [] # request the map from the map server rospy.wait_for_service('static_map') try: map_server = rospy.ServiceProxy('static_map', GetMap) map = map_server().map print map.info.resolution except: print "Service call failed!" # initializes the occupancyfield which contains the map self.occupancy_field = OccupancyField(map) print "initialized" self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() # for the pose, calculate the particle's mean location mean_particle = Particle(0, 0, 0, 0) mean_particle_theta_x = 0 mean_particle_theta_y = 0 for particle in self.particle_cloud: mean_particle.x += particle.x * particle.w mean_particle.y += particle.y * particle.w # angle is calculated using trig to account for angle runover distance_vector = np.sqrt( np.square(particle.y) + np.square(particle.x)) mean_particle_theta_x += distance_vector * np.cos( particle.theta) * particle.w mean_particle_theta_y += distance_vector * np.sin( particle.theta) * particle.w mean_particle.theta = np.arctan2(float(mean_particle_theta_y), float(mean_particle_theta_x)) self.robot_pose = mean_particle.as_pose() def projected_scan_received(self, msg): self.last_projected_stable_scan = msg def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return odom_noise = .3 # level of noise put into particles after update from odom to introduce variability # updates the particles based on r1, d, and r2. For more information on this, consult the website for particle in self.particle_cloud: # calculates r1, d, and r2 r1 = np.arctan2(float(delta[1]), float( delta[0])) - old_odom_xy_theta[2] d = np.sqrt(np.square(delta[0]) + np.square(delta[1])) r2 = delta[2] - r1 # updates the particles with the above variables, while also adding in some noise particle.theta = particle.theta + r1 * ( random_sample() * odom_noise + (1 - odom_noise / 2.0)) particle.x = particle.x + d * np.cos( particle.theta) * (random_sample() * odom_noise + (1 - odom_noise / 2.0)) particle.y = particle.y + d * np.sin( particle.theta) * (random_sample() * odom_noise + (1 - odom_noise / 2.0)) particle.theta = particle.theta + r2 * ( random_sample() * odom_noise + (1 - odom_noise / 2.0)) def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized self.normalize_particles() # creates choices and probabilities lists, which are the particles and their respective weights choices = [] probabilities = [] num_samples = len(self.particle_cloud) for particle in self.particle_cloud: choices.append(particle) probabilities.append(particle.w) # re-makes the particle cloud according to a random sample based on the probability distribution of the weights self.particle_cloud = self.draw_random_sample(choices, probabilities, num_samples) def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ # for each particle, find the total error based on 36 laser measurements taken from the Neato's actual position for particle in self.particle_cloud: error = [] for theta in range(0, 360, 10): rad = np.radians(theta) err = self.occupancy_field.get_closest_obstacle_distance( particle.x + msg.ranges[theta] * np.cos(particle.theta + rad), particle.y + msg.ranges[theta] * np.sin(particle.theta + rad)) if ( math.isnan(err) ): # if the get_closest_obstacle_distance method finds that a point is out of bounds, then the particle can't never be it particle.w = 0 break error.append( err**5 ) # each error is appended up to a power to make more likely particles have higher probability if ( sum(error) == 0 ): # if the particle is basically a perfect match, then we make the particle almost always enter the next iteration through resampling particle.w = 1.0 else: particle.w = 1.0 / sum( error ) # the errors are inverted such that large errors become small and small errors become large @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ # sets up an index list for the chosen particles, and makes bins for the probabilities values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize( random_sample(n), bins )] # chooses the new particles based on the probabilities of the old ones samples = [] for i in inds: samples.append( deepcopy(choices[int(i)]) ) # makes the new particle cloud based on the chosen particles return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ # levels of noise to introduce variability lin_noise = 1 ang_noise = math.pi / 2.0 # if doesn't exist, use odom if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # make a new particle cloud, and then create a bunch of particles at the initial location with some added noise self.particle_cloud = [] for x in range(self.n_particles): x = xy_theta[0] + (random_sample() * lin_noise - (lin_noise / 2.0)) y = xy_theta[1] + (random_sample() * lin_noise - (lin_noise / 2.0)) theta = xy_theta[2] + (random_sample() * ang_noise - (ang_noise / 2.0)) self.particle_cloud.append(Particle(x, y, theta)) # normalize particles because all weights were originall set to 1 on default self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ # takes the sum, and then divides all weights by the sum weights_sum = sum(particle.w for particle in self.particle_cloud) for particle in self.particle_cloud: particle.w /= weights_sum def publish_particles(self, msg): """Publishes the particles out for visualization and other purposes""" particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) if not (self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry if self.last_projected_stable_scan: last_projected_scan_timeshift = deepcopy( self.last_projected_stable_scan) last_projected_scan_timeshift.header.stamp = msg.header.stamp self.scan_in_base_link = self.tf_listener.transformPointCloud( "base_link", last_projected_scan_timeshift) self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density self.fix_map_to_odom_transform( msg ) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer""" (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose( translation, rotation), header=Header(stamp=msg.header.stamp, frame_id=self.base_frame)) self.tf_listener.waitForTransform(self.base_frame, self.odom_frame, msg.header.stamp, rospy.Duration(1.0)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not (hasattr(self, 'translation') and hasattr(self, 'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 300 # the number of particles to use self.model_noise_rate = 0.15 self.d_thresh = .2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # ????? # rospy.Subscriber('/simple_odom', Odometry, self.process_odom) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received, queue_size=10) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # [.0] * 3 # self.initial_particles = self.initial_list_builder() # self.particle_cloud = self.initialize_particle_cloud() print(self.particle_cloud) # self.current_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid # TODO: fill in the appropriate service call here. The resultant map should be assigned be passed # into the init method for OccupancyField # for now we have commented out the occupancy field initialization until you can successfully fetch the map mapa = obter_mapa() self.occupancy_field = OccupancyField(mapa) # self.update_particles_with_odom(msg) self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() # TODO: assign the lastest pose into self.robot_pose as a geometry_msgs.Pose object # just to get started we will fix the robot's pose to always be at the origin self.robot_pose = Pose() def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) print(new_odom_xy_theta) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) for p in self.particle_cloud: p.x += delta[0] p.y += delta[1] p.theta += delta[2] self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # ???? # TODO: modify particles using delta for p in self.particle_cloud: r = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2] d = math.sqrt((delta[0] ** 2) + (delta[1] ** 2)) p.theta += r % 360 p.x += d * math.cos(p.theta) + normal(0, .1) p.y += d * math.sin(p.theta) + normal(0, .1) p.theta += (delta[2] - r + normal(0, .1)) % 360 # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized # TODO: fill out the rest of the implementation self.particle_cloud = ParticleFilter.weighted_values(self.particle_cloud, [p.w for p in self.particle_cloud], len(self.particle_cloud)) for p in particle_cloud: p.w = 1 / len(self.particle_cloud) self.normalize_particles() def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ # TODO: implement this for r in msg.ranges: for p in self.particle_cloud: p.w = 1 self.occupancy_field.get_particle_likelyhood(p, r, self.model_noise_rate) self.normalize_particles() self.resample_particles() @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) print(size, bins) return values[np.digitize(random_sample(size), bins)] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # TODO create particles self.particle_cloud = self.initial_list_builder(xy_theta) self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ # TODO: implement this w_sum = 0 for p in self.particle_cloud: w_sum += p.w for p in self.particle_cloud: p.w /= w_sum def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): # wait for initialization to complete # print 1 return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,rospy.Time(0))): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node print 2 return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,rospy.Time(0))): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node print 3 return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) print(self.current_odom_xy_theta) # Essa list não está sendo "refeita" / preenchida print(new_odom_xy_theta) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) print(math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]), "hi") elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! ''' É AQUI!!!! ''' print('jorge') self.update_particles_with_odom(msg) # update based on odometry - FAZER self.update_particles_with_laser(msg) # update based on laser scan - FAZER self.update_robot_pose() # update robot's pose """ abaixo """ self.resample_particles() # resample particles to focus on areas of high density - FAZER self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation), header=Header(stamp=rospy.Time(0),frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.Time.now(), self.odom_frame, self.map_frame) def initial_list_builder(self, xy_theta): ''' Creates the initial particles list, using the super advanced methods provided to us by the one and only John Cena ''' initial_particles = [] for i in range(self.n_particles): p = Particle() p.x = gauss(xy_theta[0], 1) p.y = gauss(xy_theta[1], 1) p.theta = gauss(xy_theta[2], (math.pi / 2)) p.w = 1.0 / self.n_particles initial_particles.append(p) return initial_particles
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 300 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model self.robot_pose # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid # TODO: fill in the appropriate service call here. The resultant map should be assigned be passed # into the init method for OccupancyField self.occupancy_field = OccupancyField(map) self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (level 2) (2): compute the most likely pose (i.e. the mode of the distribution) (level 1) """ # TODO: assign the lastest pose into self.robot_pose as a geometry_msgs.Pose object # first make sure that the particle weights are normalized self.normalize_particles() def update_particles_with_odom(self, msg): """ Implement a simple version of this (Level 1) or a more complex one (Level 2) """ new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # TODO: modify particles using delta # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights """ # make sure the distribution is normalized self.normalize_particles() # TODO: fill out the rest of the implementation def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ # TODO: implement this pass @staticmethod def angle_normalize(z): """ convenience function to map an angle to the range [-pi,pi] """ return math.atan2(math.sin(z), math.cos(z)) @staticmethod def angle_diff(a, b): """ Calculates the difference between angle a and angle b (both should be in radians) the difference is always based on the closest rotation from angle a to angle b examples: angle_diff(.1,.2) -> -.1 angle_diff(.1, 2*math.pi - .1) -> .2 angle_diff(.1, .2+2*math.pi) -> -.1 """ a = ParticleFilter.angle_normalize(a) b = ParticleFilter.angle_normalize(b) d1 = a-b d2 = 2*math.pi - math.fabs(d1) if d1 > 0: d2 *= -1.0 if math.fabs(d1) < math.fabs(d2): return d1 else: return d2 @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements form the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] # TODO create particles self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ # TODO: implement this def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),frame_id=self.map_frame),poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0),frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.resample_particles() # resample particles to focus on areas of high density self.update_robot_pose() # update robot's pose self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """ (translation, rotation) = TransformHelpers.convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=msg.header.stamp,frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) start_particles: the number of particles first initalized end_particles: the number of particles which end in the filter middle_step: the step at which the number of particles has decayed about 50% d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): """ Define a new particle filter """ print("RUNNING") self.initialized = False # make sure we don't perform updates before everything is setup self.kidnap = False rospy.init_node( 'pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.start_particles = 1000 # the number of particles to use self.end_particles = 200 self.resample_count = 10 self.middle_step = 10 self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # publish weights for live graph node self.weight_pub = rospy.Publisher("/graph_data", Float64MultiArray, queue_size=10) # laser_subscriber listens for data from the lidar rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] # change use_projected_stable_scan to True to use point clouds instead of laser scans self.use_projected_stable_scan = False self.last_projected_stable_scan = None if self.use_projected_stable_scan: # subscriber to the odom point cloud rospy.Subscriber("projected_stable_scan", PointCloud, self.projected_scan_received) self.current_odom_xy_theta = [] self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() # publish the marker array # self.viz = rospy.Publisher('/particle_marker', Marker, queue_size=10) # self.marker = Marker() self.viz = rospy.Publisher('/particle_marker', MarkerArray, queue_size=10) self.markerArray = MarkerArray() self.initialized = True # assigns robot pose. used only a visual debugger, the real data comes from the bag file. def update_robot_pose(self, timestamp): #print("Guessing Robot Position") self.normalize_particles(self.particle_cloud) weights = [p.w for p in self.particle_cloud] index_best = weights.index(max(weights)) best_particle = self.particle_cloud[index_best] self.robot_pose = self.transform_helper.covert_xy_and_theta_to_pose( best_particle.x, best_particle.y, best_particle.theta) self.transform_helper.fix_map_to_odom_transform( self.robot_pose, timestamp) def projected_scan_received(self, msg): self.last_projected_stable_scan = msg # deadreckons particles with respect to robot motion. def update_particles_with_odom(self, msg): """ To apply the robot transformations to a particle, it can be broken down into a rotations, a linear movement, and another rotation (which could equal 0) """ #print("Deadreckoning") new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) delta_x = delta[0] delta_y = delta[1] delta_theta = delta[2] direction = math.atan2(delta_y, delta_x) theta_1 = self.transform_helper.angle_diff( direction, self.current_odom_xy_theta[2]) for p in self.particle_cloud: distance = math.sqrt((delta_x**2) + (delta_y**2)) + np.random.normal( 0, 0.001) dx = distance * np.cos(p.theta + theta_1) dy = distance * np.sin(p.theta + theta_1) p.x += dx p.y += dy p.theta += delta_theta + np.random.normal(0, 0.005) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # print("Resampling Particles") # make sure the distribution is normalized self.normalize_particles(self.particle_cloud) particle_cloud_length = len(self.particle_cloud) n_particles = ParticleFilter.sigmoid_function(self.resample_count, self.start_particles, self.end_particles, self.middle_step, 1) print("Number of Particles Reassigned: " + str(n_particles)) norm_weights = [p.w for p in self.particle_cloud] # print("Weights: "+ str(norm_weights)) top_percent = 0.20 ordered_indexes = np.argsort(norm_weights) ordered_particles = [ self.particle_cloud[index] for index in ordered_indexes ] best_particles = ordered_particles[int(particle_cloud_length * (1 - top_percent)):] new_particles = ParticleFilter.draw_random_sample( self.particle_cloud, norm_weights, n_particles - int(particle_cloud_length * top_percent)) dist = 0.001 # adding a square meter of noise around each ideal particle self.particle_cloud = [] self.particle_cloud += best_particles for p in new_particles: x_pos, y_pos, angle = p.x, p.y, p.theta x_particle = np.random.normal(x_pos, dist) y_particle = np.random.normal(y_pos, dist) theta_particle = np.random.normal(angle, 0.05) self.particle_cloud.append( Particle(x_particle, y_particle, theta_particle)) self.normalize_particles(self.particle_cloud) self.resample_count += 1 def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ #transform laser data from particle's perspective to map coords #print("Assigning Weights") scan = msg.ranges num_particles = len(self.particle_cloud) num_scans = 361 step = 2 angles = np.arange(num_scans) # will be scan indices (0-361) distances = np.array(scan) # will be scan values (scan) angles_rad = np.deg2rad(angles) for p in self.particle_cloud: sin_values = np.sin(angles_rad + p.theta) cos_values = np.cos(angles_rad + p.theta) d_angles_sin = np.multiply(distances, sin_values) d_angles_cos = np.multiply(distances, cos_values) d_angles_sin = d_angles_sin[0:361:step] d_angles_cos = d_angles_cos[0:361:step] total_beam_x = np.add(p.x, d_angles_cos) total_beam_y = np.add(p.y, d_angles_sin) particle_distances = self.occupancy_field.get_closest_obstacle_distance( total_beam_x, total_beam_y) cleaned_particle_distances = [ 2 * np.exp(-(dist**2)) for dist in particle_distances if (math.isnan(dist) != True) ] p_d_cubed = np.power(cleaned_particle_distances, 3) p.w = np.sum(p_d_cubed) self.normalize_particles(self.particle_cloud) @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ #print("Initial Pose Set") xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( msg.pose.pose) self.initialize_particle_cloud(msg.header.stamp, xy_theta) def initialize_particle_cloud(self, timestamp, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is omitted, the odometry will be used Also check to see if we are attempting the robot kidnapping problem or are given an initial 2D pose """ if self.kidnap: print("Kidnap Problem") x_bound, y_bound = self.occupancy_field.get_obstacle_bounding_box() x_particle = np.random.uniform(x_bound[0], x_bound[1], size=self.start_particles) y_particle = np.random.uniform(y_bound[0], y_bound[1], size=self.start_particles) theta_particle = np.deg2rad( np.random.randint(0, 360, size=self.start_particles)) else: print("Starting with Inital Position") if xy_theta is None: print("No Position Definied") xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) x, y, theta = xy_theta x_particle = np.random.normal(x, 0.25, size=self.start_particles) y_particle = np.random.normal(y, 0.25, size=self.start_particles) theta_particle = np.random.normal(theta, 0.001, size=self.start_particles) self.particle_cloud = [Particle(x_particle[i],\ y_particle[i],\ theta_particle[i]) \ for i in range(self.start_particles)] def normalize_particles(self, particle_list): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ #print("Normalize Particles") old_weights = [p.w for p in particle_list] new_weights = [] for p in particle_list: p.w = float(p.w) / sum(old_weights) new_weights.append(p.w) float_array = Float64MultiArray() float_array.data = new_weights self.weight_pub.publish(float_array) def publish_particles(self, msg): """ Publishes particle poses on the map. Uses Paul's default code at the moment, maybe later attempt to publish a visualization/MarkerArray """ particles_conv = [] for num, p in enumerate(self.particle_cloud): particles_conv.append(p.as_pose()) self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) # self.marker_update("map", self.particle_cloud, False) # self.viz.publish() def scan_received(self, msg): """ All control flow happens here! Special init case then goes into loop """ if not (self.initialized): # wait for initialization to complete return # wait a little while to see if the transform becomes available. This fixes a race # condition where the scan would arrive a little bit before the odom to base_link transform # was updated. # self.tf_listener.waitForTransform(self.base_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(0.5)) if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative to the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) if not self.current_odom_xy_theta: self.current_odom_xy_theta = new_odom_xy_theta return if not (self.particle_cloud): print("Particle Cloud Empty") # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud(msg.header.stamp) self.update_particles_with_laser(msg) self.normalize_particles(self.particle_cloud) self.update_robot_pose(msg.header.stamp) self.resample_particles() elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! print("UPDATING PARTICLES") self.update_particles_with_odom(msg) # update based on odometry if self.last_projected_stable_scan: last_projected_scan_timeshift = deepcopy( self.last_projected_stable_scan) last_projected_scan_timeshift.header.stamp = msg.header.stamp self.scan_in_base_link = self.tf_listener.transformPointCloud( "base_link", last_projected_scan_timeshift) self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose(msg.header.stamp) # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density # publish particles (so things like rviz can see them) self.publish_particles(msg) def marker_update(self, frame_id, p_cloud, delete): num = 0 if (delete): self.markerArray.markers = [] else: for p in p_cloud: marker = Marker() marker.header.frame_id = frame_id marker.header.stamp = rospy.Time.now() marker.ns = "my_namespace" marker.id = num marker.type = Marker.ARROW marker.action = Marker.ADD marker.pose = p.as_pose() marker.pose.position.z = 0.5 marker.scale.x = 1.0 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 # Don't forget to set the alpha! marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 num += 1 self.markerArray.markers.append(marker) @staticmethod def sigmoid_function(value, max_output, min_output, middle, inc=1): particle_difference = max_output - min_output exponent = inc * (value - (middle / 2)) return int(particle_difference / (1 + np.exp(exponent)) + min_output)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most caPose(ses) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): print "ParticleFilter initializing " self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('comp_robo_project2') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 200 # the number of paporticles to use self.d_thresh = 0.1 # the amount of linear movement before performing an update self.a_thresh = math.pi/12 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray) self.pose_pub = rospy.Publisher("predictedPose", PoseArray) self.scan_shift_pub = rospy.Publisher("scanShift", PoseArray) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() print "waiting for map server" rospy.wait_for_service('static_map') print "static_map service loaded" static_map = rospy.ServiceProxy('static_map', GetMap) worldMap = static_map() if worldMap: print "obtained map" # for now we have commented out the occupancy field initialization until you can successfully fetch the map self.occupancy_field = OccupancyField(worldMap.map) self.initialized = True print "ParticleFilter initialized" def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (level 2) (2): compute the most likely pose (i.e. the mode of the distribution) (level 1) """ highestWeight = 0 highestIndex = 0 weightsAndParticles = [] # array of tuples of the weight of each particle and the particle itself for i in range(len(self.particle_cloud)): weightsAndParticles.append((self.particle_cloud[i].w,self.particle_cloud[i])) # Order by weights sorted_by_first = sorted(weightsAndParticles, key=lambda tup: tup[0])[::-1] # Select the the top third of particles with the hightes weights (probablilities) topParticles = [i[1] for i in sorted_by_first][:int(self.n_particles*.3)] # Average the top wighted particles to be the guessed position self.robot_pose = self.averageHypos(topParticles) def averageHypos(self, hypoList): """ Averages the positions and angles of the input Particles hypoList must be a list of Particles returns Particle position info """ xList = [] yList = [] thetaList = [] if hypoList == [] or hypoList == None: print "hypoList is invalid" return Particle(x=0,y=0,theta=0,w=0).as_pose() # Sort particles' characteristics in appropriate lists for particle in hypoList: xList.append(particle.x) yList.append(particle.y) thetaList.append(particle.theta) # Average X and Y positions averageX = sum(xList)/len(xList) averageY = sum(yList)/len(yList) # Average angles by decomposing vectors, averaging components, and converting back to an angle unitXList = [] unitYList = [] for theta in thetaList: unitXList.append(math.cos(theta)) unitYList.append(math.sin(theta)) averageUnitX = sum(unitXList)/len(unitXList) averageUnitY = sum(unitYList)/len(unitYList) averageTheta = (math.atan2(averageUnitY,averageUnitX)+(2*math.pi))%(2*math.pi) return Particle(x=averageX,y=averageY,theta=averageTheta ,w=1.0).as_pose() def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # assumes map centered at 0,0 x_max_boundary = -self.occupancy_field.origin.position.x x_min_boundary = self.occupancy_field.origin.position.x y_max_boundary = -self.occupancy_field.origin.position.y y_min_boundary = self.occupancy_field.origin.position.y # Loops through particles to upsade with odom information for i in range(len(self.particle_cloud)): # Calculates amount of change for angle and X and Y position tempDelta = self.rotatePositionChange(old_odom_xy_theta, delta, self.particle_cloud[i]) self.particle_cloud[i].x += tempDelta[0] self.particle_cloud[i].y += tempDelta[1] self.particle_cloud[i].theta += tempDelta[2] # Accounts for angle wrapping if self.particle_cloud[i].theta > (2*math.pi) or self.particle_cloud[i].theta < 0: self.particle_cloud[i].theta = self.particle_cloud[i].theta%(2*math.pi) #check map boundaries. Any particles no longer within map boundaries are moved to boundary if self.particle_cloud[i].x > x_max_boundary: self.particle_cloud[i].x = x_max_boundary elif self.particle_cloud[i].x < x_min_boundary: self.particle_cloud[i].x = x_min_boundary if self.particle_cloud[i].y > y_max_boundary: self.particle_cloud[i].y = y_max_boundary elif self.particle_cloud[i].y < y_min_boundary: self.particle_cloud[i].y = y_min_boundary # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136). def rotatePositionChange(self,old_odom_xy_theta, delta, particle): """ Determines the change in X and Y for each hypothesis based on its angle """ angle = particle.theta - old_odom_xy_theta[2] newDeltaX = delta[0]*math.cos(angle) - delta[1]*math.sin(angle) newDeltaY = delta[0]*math.sin(angle) + delta[1]*math.cos(angle) return (newDeltaX,newDeltaY,delta[2]) def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ weights = [] choices = [] probabilities = [] # Sort particle cloud info into appropriate arrays for particle in self.particle_cloud: choices.append(particle) probabilities.append(particle.w) # Only resample 2/3 of the original number of particles from current pool numParticles = int(self.n_particles/3)*2 # Randomly draw particles from the current particle cloud biased towards points with higher weights temp_particle_cloud = self.draw_random_sample(choices, probabilities, numParticles) # Add uncertaintly/noise to all points for particle in self.particle_cloud: particle.x = particle.x + random.gauss(0, .1) particle.y = particle.y + random.gauss(0, .1) particle.theta = particle.theta + random.gauss(0, .4) # Randomly pick the remaining 1/3 of particles randomly from known unoccupied cells of map, then # combine with the 2/3 biasedly chosen earlier self.particle_cloud = temp_particle_cloud + self.generateRandomParticles(self.n_particles - numParticles) def generateRandomParticles(self, number): """ Generates random particles from the unoccupied portion of the map Returns array of random particles lengh of input number """ res = self.occupancy_field.map.info.resolution temp_particle_cloud = [] unoccupied_cells = self.occupancy_field.unoccupied_cells for i in range(number): random_pt_index = int(random.uniform(0,len(unoccupied_cells))) x = (unoccupied_cells[random_pt_index][0] ) * res + self.occupancy_field.origin.position.x y = (unoccupied_cells[random_pt_index][1] ) * res + self.occupancy_field.origin.position.y theta = random.uniform(0,2*math.pi) rand_particle = Particle(x = x, y = y, theta = theta) temp_particle_cloud.append(rand_particle) return temp_particle_cloud def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ # TODO: implement this scanList = [] pointList = [] # create list of valid scans for i in range(len(msg.ranges)): if msg.ranges[i] < 6 and msg.ranges[i] >.2: scanList.append((((float(i)/360.0)*2*math.pi),msg.ranges[i])) # iterate through all particles for particle in self.particle_cloud: angleDif = (particle.theta - self.robot_pose.orientation.z+2*math.pi)%(2*math.pi) #iterate through all valid scan points errorList = [] for datum in scanList: scanPosition = self.shiftScanToPoint(angleDif, datum, particle) #print scanPosition pointList.append(scanPosition) dist = self.occupancy_field.get_closest_obstacle_distance(scanPosition[0],scanPosition[1]) errorList.append(math.pow(dist, 3)) particle.w = 1/(sum(errorList)/len(errorList)) print "errorAverage: " + str(particle.w) #print "normError: " + str(self.particle_cloud) # for particle in self.particle_cloud: # weight = particle.w # particle.w = 1-weight #print "weight: " + str(particle.w) weightList = [] #self.normalize_particles() for i in range(len(self.particle_cloud)): weightList.append(self.particle_cloud[i].w) print weightList self.normalize_particles() #self.publish_shifted_scan(msg, pointList) def shiftScanToPoint(self,angleDif, datum, particle): #calculate real world position of datum laserAngle = (datum[0]+angleDif + 2*math.pi)%(2*math.pi) xDelta = datum[1]*math.cos(laserAngle) yDelta = datum[1]*math.sin(laserAngle) datumX = xDelta + particle.x datumY = yDelta + particle.y return (datumX,datumY) @staticmethod def angle_normalize(z): """ convenience function to map an angle to the range [-pi,pi] """ return math.atan2(math.sin(z), math.cos(z)) @staticmethod def angle_diff(a, b): """ Calculates the difference between angle a and angle b (both should be in radians) the difference is always based on the closest rotation from angle a to angle b examples: angle_diff(.1,.2) -> -.1 angle_diff(.1,2*math.pi-.1) -> .2 angle_diff(.1,.2+2*math.pi) -> -.1 """ a = ParticleFilter.angle_normalize(a) b = ParticleFilter.angle_normalize(b) d1 = a-b d2 = 2*math.pi - math.fabs(d1) if d1 > 0: d2 *= -1.0 if math.fabs(d1) < math.fabs(d2): return d1 else: return d2 @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements form the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ print "initializing particle cloud" self.particle_cloud = [] unoccupied_cells = self.occupancy_field.unoccupied_cells # When no guess given, initialize paricle cloud by random points in known unocupied portion of map if xy_theta == None: self.particle_cloud = generateRandomParticles(self, self.n_particles) else: print "guess given" for i in range(self.n_particles): x = random.gauss(xy_theta[0], 1) y = random.gauss(xy_theta[1], 1) theta = (random.gauss(xy_theta[2], 1.5)) rand_particle = Particle(x = x, y = y, theta = theta) self.particle_cloud.append(rand_particle) # Get map characteristics to generate points randomly in that realm. Assume self.particle_pub.publish() self.update_robot_pose() print "particle cloud initialized" def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0)""" numParticles = len(self.particle_cloud) weightArray = np.empty([numParticles, 1]) for i in range(numParticles): print weightArray[i] = self.particle_cloud[i].w print "weightArray: " + str(weightArray) normWeights = weightArray/np.sum(weightArray) print "normWeights: " + str(normWeights) for i in range(numParticles): self.particle_cloud[i].w = normWeights[i][0] def publish_predicted_pose(self, msg): # actually send the message so that we can view it in rviz self.pose_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),frame_id=self.map_frame),poses=[self.robot_pose])) def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),frame_id=self.map_frame),poses=particles_conv)) def publish_shifted_scan(self, msg, pointList): particles_conv = [] for point in pointList: particles_conv.append(Pose(position=Point(x=point[0],y=point[1],z=0), orientation=Quaternion(x=0, y=0, z=0, w=0))) # actually send the message so that we can view it in rviz self.scan_shift_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),frame_id=self.map_frame),poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ #print "scan received" if not(self.initialized): # wait for initialization to complete print "not initialized" return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0),frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) try: self.particle_cloud except: # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) if (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() self.publish_particles(msg) # update robot's pose self.resample_particles() # resample particles to focus on areas of high density self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) #self.publish_particles(msg) self.publish_predicted_pose(msg) def fix_map_to_odom_transform(self, msg): """ Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """ (translation, rotation) = TransformHelpers.convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=msg.header.stamp,frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)