def neighbors(self, state): max_angle = utils.max_angle(self.min_turn_radius, state.radius) self.thetas = np.linspace(-max_angle, max_angle, num=self.branch_factor) self.euclidean_neighbors[:, 0] = state.radius * np.cos( self.thetas + state.angle) + state.center[0] self.euclidean_neighbors[:, 1] = state.radius * np.sin( self.thetas + state.angle) + state.center[1] # we use this several times, so compute this icky math here to avoid unnecessary computation euc = self.euclidean_neighbors.copy() utils.world_to_map(euc, self.map.map_info) euc = np.round(euc).astype(int) radii = self.map.get_distances(euc, coord_convert=False, check_bounds=True) mask = np.logical_and( np.invert(self.map.get_explored(euc, coord_convert=False)), self.map.get_permissible(euc, coord_convert=False)) neighbors = map( lambda c, r, t: Circle( center=c.copy( ), radius=r, angle=state.angle + t, deflection=t), self.euclidean_neighbors[mask, :], radii[mask], self.thetas[mask]) return neighbors
def out_of_bounds_poses(self, poses): t0 = time.time() grid_poses = poses.clone() dprint('World Poses: ', grid_poses) # t1 = time.time() #print('time for clone:', t1 - t0) Utils.world_to_map(grid_poses, self.map_info) # t2 = time.time() # print('time for util:', t2 - t1) dprint('Grid Poses: ', grid_poses) grid_poses.round_() # It's important to round, not floor # t3 = time.time() # print('time for round:', t3 - t2) #grid_poses = grid_poses[:, :2] # Gets rid of theta grid_poses = grid_poses.type(torch.cuda.LongTensor) #t4 = time.time() #print('time for type:', t4 - t3) #map_indices = grid_poses[:, 0] + grid_poses[:, 1] occupancyValues = self.permissible_region_torch[grid_poses[:, 1], grid_poses[:, 0]] #t5 = time.time() #print('time for occupancy:', t5 - t4) return occupancyValues
def startCB(self, msg): #rospy.loginfo('recieved start msg') self.get_omap() self.start = np.array([(msg.pose.pose.position.x, msg.pose.pose.position.y, 0)]) Utils.world_to_map(self.start, self.map_info) self.start = np.array((self.start[0][0], 1300 - self.start[0][1], self.start[0][2])).astype(int)
def final_cost(self, pose, state): # TODO make a better final_cost (running tally of bound check) np_pose = pose.cpu().numpy() Utils.world_to_map(np_pose, self.map_info) np_pose = np.round(np_pose).astype(int) bounds_check = 10000 * self.permissible_region[np_pose[:, 0], np_pose[:, 1]] return torch.from_numpy(bounds_check).type(self.dtype)
def init_pose_cb(self, msg): # Get the pose of the car w.r.t the map in meters rx_trans = np.array( [msg.pose.pose.position.x, msg.pose.pose.position.y], dtype=np.float) rx_rot = utils.quaternion_to_angle(msg.pose.pose.orientation) # Get the pose of the car w.r.t the map in pixels if self.map_info is not None: map_rx_pose = utils.world_to_map( (rx_trans[0], rx_trans[1], rx_rot), self.map_info) # Update the pose of the car if either bounds checking is not enabled, # or bounds checking is enabled but the car is in-bounds if (self.permissible_region is None or self.permissible_region[int(map_rx_pose[1] + 0.5), int(map_rx_pose[0] + 0.5)] == 1): self.cur_odom_to_base_lock.acquire() self.cur_map_to_odom_lock.acquire() # Compute where the car is w.r.t the odometry frame offset_in_map = rx_trans - self.cur_map_to_odom_trans self.cur_odom_to_base_trans = np.zeros(2, dtype=np.float) self.cur_odom_to_base_trans[0] = offset_in_map[0] * np.cos( -self.cur_map_to_odom_rot) - offset_in_map[1] * np.sin( -self.cur_map_to_odom_rot) self.cur_odom_to_base_trans[1] = offset_in_map[0] * np.sin( -self.cur_map_to_odom_rot) + offset_in_map[1] * np.cos( -self.cur_map_to_odom_rot) self.cur_odom_to_base_rot = rx_rot - self.cur_map_to_odom_rot self.cur_map_to_odom_lock.release() self.cur_odom_to_base_lock.release()
def out_of_bounds(self, pose): grid_poses = pose.clone().view(1, 3) dprint('World Poses: ', grid_poses) Utils.world_to_map(grid_poses, self.map_info) dprint('Grid Poses: ', grid_poses) grid_poses.round_() grid_x = int(round(grid_poses[0][0])) grid_y = int(round(grid_poses[0][1])) occupancyVal = self.permissible_region[grid_y, grid_x] if occupancyVal == 1: #occupancyVal = self.map_data[grid_x + grid_y * self.map_info.width)] # if occupancyVal == -1 or occupancyVal == 100: print('Pose is invalid!!!!') return 0 else: #print('Valid pose: ', pose) print('Pose is valid!!!') return 1
def _ray(x: float, y: float, angle: float, gridmap: OccupancyGrid) -> List[Tuple[int, int]]: start = x + np.cos(angle) * SENSOR.MIN_RANGE, y + np.sin( angle) * SENSOR.MIN_RANGE end = x + np.cos(angle) * SENSOR.MAX_RANGE, y + np.sin( angle) * SENSOR.MAX_RANGE start, end = world_to_map([start, end], gridmap) ray = bresenham_line(start, end) return ray
def fuse_laser_scan(self, laser_scan: LaserScan): """Fuse the laser scan data into the probabilistic occupancy grid map.""" assert isinstance(laser_scan, LaserScan) # laser scan points in map coordinates points_r = self._read_laser_data(laser_scan) points_w = robot_to_world(points_r, self.odometry) points = world_to_map(points_w, self.map) # extend the map if needed extended = self._extend_map(points) if extended: points = world_to_map(points_w, self.map) # robot position in map coordinates robot = world_to_map([self.position], self.map)[0] # raytrace the points and update the map accordingly free, occupied = self._raytrace(robot, points) self._update_map(free, occupied)
def goalCB(self, msg): #rospy.loginfo('recieved goal msg') self.goal = np.array([(msg.pose.position.x, msg.pose.position.y, 0)]) Utils.world_to_map(self.goal, self.map_info) self.goal = np.array((self.goal[0][0], 1300 - self.goal[0][1], self.goal[0][2])).astype(int) #rospy.loginfo(self.goal) if self.environment[self.goal[1]][self.goal[0]] == 0: #rospy.loginfo(self.goal) if self.start != None and self.goal != None: #rospy.loginfo('getting path') path = np.array( self.rrt(self.environment, self.start, self.goal)).astype(float) #rospy.loginfo(path) Utils.map_to_world(path, self.map_info) path = path.astype(int) #rospy.loginfo('visualizing') self.visualize(path, self.start, self.goal) else: self.goal = None
def neighbors(self, state): ''' This function determines the set of points along the perimeter of the given circle state that are accessible according to ackermann geometry and steering angle limitations. Then it samples the permissible theta space to generate neighbors. TODO: computation of neighboring thetas is not accurate - it should actually be the direction tangent to the path arc at the intersection with the circle radius. For now this works, and is a strict underestimate of the reachable thetas ''' max_angle = utils.max_angle(self.min_turn_radius, state.radius) percentage = np.power(2.0 * self.branch_factor / np.pi, 0.9) actual_branch_factor = 2 * int(max_angle * percentage / 2.0) + 1 thetas = np.linspace(-max_angle, max_angle, num=actual_branch_factor) euclidean_neighbors = np.zeros((actual_branch_factor, 2)) euclidean_neighbors[:, 0] = state.radius * np.cos( thetas + state.angle) + state.center[0] euclidean_neighbors[:, 1] = state.radius * np.sin( thetas + state.angle) + state.center[1] # perform coordinate space conversion here and then index into the exploration and permissible # buffers to prune states which are not permissible or already explored euc = euclidean_neighbors.copy() utils.world_to_map(euc, self.map.map_info) euc = np.round(euc).astype(int) radii = self.map.get_distances( euc, coord_convert=False, check_bounds=True) - 0.05 radii = np.clip(radii, 0.0, self.max_circle_radius) mask = np.logical_and( np.invert(self.map.get_explored(euc, coord_convert=False)), self.map.get_permissible(euc, coord_convert=False)) # generate a neighbor state for each of the permissible neighbors = map( lambda c, r, t: Circle( center=c.copy(), radius=r, angle=state.angle + t, deflection=t ), euclidean_neighbors[mask, :], radii[mask], thetas[mask]) return neighbors
def running_cost(self, pose, goal, ctrl, noise): np_pose = pose.cpu().numpy() # Kx3 Utils.world_to_map(np_pose, self.map_info) np_pose = np.round(np_pose).astype(int) np_pose[:, 0] = np.clip(np_pose[:, 0], 0, self.map_height - 1) np_pose[:, 1] = np.clip(np_pose[:, 1], 0, self.map_width - 1) bounds_check = 100 * self.permissible_region[ np_pose[:, 0], np_pose[:, 1]] # TODO check this bounds checks bounds_check = torch.from_numpy(bounds_check).type(self.dtype) #delta = pose[:,0:2] - torch.from_numpy(goal[0:2]).type(self.dtype) delta = pose - torch.from_numpy(goal).type(self.dtype) pose_cost = 2.5 * torch.sum(delta**2, 1) #pose_cost = 2.5*torch.sum((pose[:,0:2]-torch.from_numpy(goal[0:2]).type(self.dtype))**2, 1) ctrl_cost = torch.abs( self._lambda * ((noise * (1 / self.sigma)).mm(ctrl.expand(1, 2).transpose(0, 1)))) #print("costs:",torch.mean(pose_cost), torch.mean(ctrl_cost), torch.mean(bounds_check)) return pose_cost + torch.squeeze(ctrl_cost) + bounds_check
def timer_cb(self, event): now = rospy.Time.now() # Publish a tf between map and odom if one does not already exist # Otherwise, get the most recent tf between map and odom self.cur_map_to_odom_lock.acquire() try: tmp_trans, tmp_rot = self.transformer.lookupTransform( "/odom", "/map", rospy.Time(0)) self.cur_map_to_odom_trans[0] = tmp_trans[0] self.cur_map_to_odom_trans[1] = tmp_trans[1] self.cur_map_to_odom_rot = ( tf.transformations.euler_from_quaternion(tmp_rot))[2] if tmp_trans[2] == -0.0001: self.br.sendTransform( ( self.cur_map_to_odom_trans[0], self.cur_map_to_odom_trans[1], 0.0001, ), tf.transformations.quaternion_from_euler( 0, 0, self.cur_map_to_odom_rot), now, "/odom", "/map", ) except Exception: self.br.sendTransform( (self.cur_map_to_odom_trans[0], self.cur_map_to_odom_trans[1], 0.0001), tf.transformations.quaternion_from_euler( 0, 0, self.cur_map_to_odom_rot), now, "/odom", "/map", ) self.cur_map_to_odom_lock.release() # Get the time since the last update if self.last_stamp is None: self.last_stamp = now dt = (now - self.last_stamp).to_sec() # Add noise to the speed self.last_speed_lock.acquire() v = self.last_speed + np.random.normal( loc=self.SPEED_OFFSET * self.last_speed, scale=self.SPEED_NOISE, size=1) self.last_speed_lock.release() # Add noise to the steering angle self.last_steering_angle_lock.acquire() delta = self.last_steering_angle + np.random.normal( loc=self.STEERING_ANGLE_OFFSET * self.last_steering_angle, scale=self.STEERING_ANGLE_NOISE, size=1, ) self.last_steering_angle_lock.release() self.cur_odom_to_base_lock.acquire() # Apply kinematic car model to the previous pose new_pose = np.array( [ self.cur_odom_to_base_trans[0], self.cur_odom_to_base_trans[1], self.cur_odom_to_base_rot, ], dtype=np.float, ) if np.abs(delta) < 1e-2: # Changes in x, y, and theta dtheta = 0 dx = v * np.cos(self.cur_odom_to_base_rot) * dt dy = v * np.sin(self.cur_odom_to_base_rot) * dt # New joint values joint_left_throttle = v * dt / self.CAR_WHEEL_RADIUS joint_right_throttle = v * dt / self.CAR_WHEEL_RADIUS joint_left_steer = 0.0 joint_right_steer = 0.0 else: # Changes in x, y, and theta tan_delta = np.tan(delta) dtheta = ((v / self.CAR_LENGTH) * tan_delta) * dt dx = (self.CAR_LENGTH / tan_delta) * (np.sin(self.cur_odom_to_base_rot + dtheta) - np.sin(self.cur_odom_to_base_rot)) dy = (self.CAR_LENGTH / tan_delta) * ( -1 * np.cos(self.cur_odom_to_base_rot + dtheta) + np.cos(self.cur_odom_to_base_rot)) # New joint values # Applt kinematic car model to compute wheel deltas h_val = (self.CAR_LENGTH / tan_delta) - (self.CAR_WIDTH / 2.0) joint_outer_throttle = (((self.CAR_WIDTH + h_val) / (0.5 * self.CAR_WIDTH + h_val)) * v * dt / self.CAR_WHEEL_RADIUS) joint_inner_throttle = (((h_val) / (0.5 * self.CAR_WIDTH + h_val)) * v * dt / self.CAR_WHEEL_RADIUS) joint_outer_steer = np.arctan2(self.CAR_LENGTH, self.CAR_WIDTH + h_val) joint_inner_steer = np.arctan2(self.CAR_LENGTH, h_val) # Assign joint values according to whether we are turning left or right if (delta) > 0.0: joint_left_throttle = joint_inner_throttle joint_right_throttle = joint_outer_throttle joint_left_steer = joint_inner_steer joint_right_steer = joint_outer_steer else: joint_left_throttle = joint_outer_throttle joint_right_throttle = joint_inner_throttle joint_left_steer = joint_outer_steer - np.pi joint_right_steer = joint_inner_steer - np.pi # Apply kinematic model updates and noise to the new pose new_pose[0] += (dx + np.random.normal( loc=self.FORWARD_OFFSET, scale=self.FORWARD_FIX_NOISE, size=1) + np.random.normal( loc=0.0, scale=np.abs(v) * self.FORWARD_SCALE_NOISE, size=1)) new_pose[1] += ( dy + np.random.normal( loc=self.SIDE_OFFSET, scale=self.SIDE_FIX_NOISE, size=1) + np.random.normal( loc=0.0, scale=np.abs(v) * self.SIDE_SCALE_NOISE, size=1)) new_pose[2] += dtheta + np.random.normal( loc=self.THETA_OFFSET, scale=self.THETA_FIX_NOISE, size=1) new_pose[2] = self.clip_angle(new_pose[2]) # Compute the new pose w.r.t the map in meters new_map_pose = np.zeros(3, dtype=np.float) new_map_pose[0] = self.cur_map_to_odom_trans[0] + ( new_pose[0] * np.cos(self.cur_map_to_odom_rot) - new_pose[1] * np.sin(self.cur_map_to_odom_rot)) new_map_pose[1] = self.cur_map_to_odom_trans[1] + ( new_pose[0] * np.sin(self.cur_map_to_odom_rot) + new_pose[1] * np.cos(self.cur_map_to_odom_rot)) new_map_pose[2] = self.cur_map_to_odom_rot + new_pose[2] # Get the new pose w.r.t the map in pixels if self.map_info is not None: new_map_pose = utils.world_to_map(new_map_pose, self.map_info) # Update the pose of the car if either bounds checking is not enabled, # or bounds checking is enabled but the car is in-bounds new_map_pose_x = int(new_map_pose[0] + 0.5) new_map_pose_y = int(new_map_pose[1] + 0.5) if self.permissible_region is None or ( new_map_pose_x >= 0 and new_map_pose_x < self.permissible_region.shape[1] and new_map_pose_y >= 0 and new_map_pose_y < self.permissible_region.shape[0] and self.permissible_region[new_map_pose_y, new_map_pose_x] == 1): # Update pose of base_footprint w.r.t odom self.cur_odom_to_base_trans[0] = new_pose[0] self.cur_odom_to_base_trans[1] = new_pose[1] self.cur_odom_to_base_rot = new_pose[2] # Update joint values self.joint_msg.position[0] += joint_left_throttle self.joint_msg.position[1] += joint_right_throttle self.joint_msg.position[2] += joint_left_throttle self.joint_msg.position[3] += joint_right_throttle self.joint_msg.position[4] = joint_left_steer self.joint_msg.position[5] = joint_right_steer # Clip all joint angles for i in range(len(self.joint_msg.position)): self.joint_msg.position[i] = self.clip_angle( self.joint_msg.position[i]) # Publish the tf from odom to base_footprint self.br.sendTransform( (self.cur_odom_to_base_trans[0], self.cur_odom_to_base_trans[1], 0.0), tf.transformations.quaternion_from_euler( 0, 0, self.cur_odom_to_base_rot), now, "base_footprint", "odom", ) # Publish the joint states self.joint_msg.header.stamp = now self.cur_joints_pub.publish(self.joint_msg) self.last_stamp = now self.cur_odom_to_base_lock.release() # Publish current state as a PoseStamped topic cur_pose = PoseStamped() cur_pose.header.frame_id = "map" cur_pose.header.stamp = now cur_pose.pose.position.x = (self.cur_odom_to_base_trans[0] + self.cur_map_to_odom_trans[0]) cur_pose.pose.position.y = (self.cur_odom_to_base_trans[1] + self.cur_map_to_odom_trans[1]) cur_pose.pose.position.z = 0.0 rot = self.cur_odom_to_base_rot + self.cur_map_to_odom_rot cur_pose.pose.orientation = utils.angle_to_quaternion(rot) self.state_pub.publish(cur_pose)
def is_pose_reachable(self, pose: Pose) -> bool: """Test if a given pose can be reached (lies within the map and is far enough from obstacles).""" x, y = world_to_map([pose_to_point(pose)], self.gridmap)[0] return self.gridmap.is_valid_coordinate(x, y) and not self.maze[y][x]
def plan_path( self, start: Pose, goal: Pose, radius: Optional[float] = None, simplify: bool = True, robust: bool = True, ) -> Tuple[Path, float]: """Plan a path from start to goal over a given map using A* algorithm. Parameters ---------- start : Pose Start position in world coordinates. goal : Pose Goal position in world coordinates. radius : float, optional The final point in the found path is within the neighborhood of a given radius from the goal. Default `2 * robot_size`. simplify : bool, optional If True (default), simplifies the found path by reducing the amount of waypoints to the minimal necessary so that the path still doesn't collide with obstacles. robust : bool, optional If True (default), steers A* heuristic to avoid paths that are close to obstacles. Returns ------- path : path The found path. length : float Path length (sum of distances between consecutive points). """ if radius is None: radius = ASTAR_TOLERANCE_FACTOR * self.robot_size radius /= self.gridmap.resolution # transform start and goal to the useful format start, goal = pose_to_point(start), pose_to_point(goal) # transform start and goal from world coordinates to map coordinates start, goal = world_to_map([start, goal], self.gridmap) # steer the A* heuristic for robust planning def steered_heuristic(a: Tuple[int, int], b: Tuple[int, int]) -> float: return euclidean_distance( a, b) + OBSTACLE_PENALTY_FACTOR / self.dist_to_obstacles[a[1], a[0]] self.astar.heuristic = steered_heuristic if robust else euclidean_distance # find a path using A* search path = self.astar.search(self.maze, start, goal, neighborhood="N8", radius=radius) # no path found if len(path) < 2: return Path(), np.inf # simplify the found path if simplify: path = self._simplify_path(path, self.maze) # transform the path back to the world coordinates path = map_to_world(path, self.gridmap) # create the Path message path = points_to_path(path) # compute the path length length = path_length(path) return path, length
def __init__(self, T, K, sigma=0.5, _lambda=0.5, roi = None): if IS_ON_ROBOT: self.SPEED_TO_ERPM_OFFSET = float(rospy.get_param("/vesc/speed_to_erpm_offset", 0.0)) self.SPEED_TO_ERPM_GAIN = float(rospy.get_param("/vesc/speed_to_erpm_gain", 4614.0)) self.STEERING_TO_SERVO_OFFSET = float(rospy.get_param("/vesc/steering_angle_to_servo_offset", 0.5304)) self.STEERING_TO_SERVO_GAIN = float(rospy.get_param("/vesc/steering_angle_to_servo_gain", -1.2135)) else: self.SPEED_TO_ERPM_OFFSET = 0.0 self.SPEED_TO_ERPM_GAIN = 4614.0 self.STEERING_TO_SERVO_OFFSET = 0.5304 self.STEERING_TO_SERVO_GAIN = -1.2135 self.CAR_LENGTH = 0.33 self.theta_weight = 0.1 self.bounds_check_weight = 1.0 self.last_pose = None # MPPI params self.T = T # Length of rollout horizon self.K = K # Number of sample rollouts self.sigma = sigma self._lambda = _lambda self.roi = roi self.was_roi_tape_seen = False self.goal = None # Lets keep track of the goal pose (world frame) over time self.lasttime = None self.last_control = None # PyTorch / GPU data configuration # TODO # you should pre-allocate GPU memory when you can, and re-use it when # possible for arrays storing your controls or calculated MPPI costs, etc if CUDA: self.model = torch.load(MODEL_FILENAME).eval() else: self.model = torch.load(MODEL_FILENAME, map_location=lambda storage, loc: storage).eval() # Maps CPU storage and serialized location back to CPU storage if CUDA: self.model.cuda() # tell torch to run the network on the GPU self.dtype = torch.cuda.FloatTensor else: self.dtype = torch.FloatTensor self.Sigma = self.dtype(sigma_data) # Covariance Matrix shape: (CONTROL_SIZE, CONTROL_SIZE) self.SigmaInv = torch.inverse(self.Sigma) self.U = self.dtype(CONTROL_SIZE, self.K, self.T).zero_() self.Epsilon = self.dtype(CONTROL_SIZE, self.K, self.T).zero_() self.Trajectory_cost = self.dtype(1, self.K).zero_() self.trajectories = self.dtype(3, self.K, self.T).zero_() self.noisyU = self.dtype(CONTROL_SIZE, self.K, self.T).zero_() self.neural_net_input = self.dtype(8).zero_() self.neural_net_input_torch = self.dtype(self.K, 8).zero_() self.bounds_check = self.dtype(self.K).zero_() self.pose_cost = self.dtype(self.K).zero_() self.omega = self.dtype(1, self.K).zero_() self.x_tminus1 = self.dtype(self.K, 3).zero_() self.x_t = self.dtype(self.K, 3).zero_() self.initial_distance = None self.intermediate = self.dtype(1,self.K).zero_() #self.pre_delta_control = self.dtype(CONTROL_SIZE, self.T).zero_() #self.delta_control = self.dtype(CONTROL_SIZE, self.K, self.T).zero_() #self.omega_expand = self.dtype(CONTROL_SIZE, self.K, self.T).zero_() #self.trajectoryMinusMin = self.dtype(1, self.K).zero_() print("Loading:", MODEL_FILENAME) print("Model:\n",self.model) print("Torch Datatype:", self.dtype) self.U = torch.cuda.FloatTensor(CONTROL_SIZE, self.K, self.T).zero_() # wtf? #self.Sigma = torch.cuda.FloatTensor(CONTROL_SIZE,CONTROL_SIZE).zero_() self.Epsilon = torch.cuda.FloatTensor(CONTROL_SIZE, self.K, self.T).zero_() self.Trajectory_cost = torch.cuda.FloatTensor(self.K, 1).zero_() # control outputs self.msgid = 0 # Store last control input self.last_control = None # visualization parameters self.num_viz_paths = 1#40 if self.K < self.num_viz_paths: self.num_viz_paths = self.K if IS_ON_ROBOT: # We will publish control messages and a way to visualize a subset of our # rollouts, much like the particle filter self.ctrl_pub = rospy.Publisher(rospy.get_param("~ctrl_topic", "/vesc/high_level/ackermann_cmd_mux/input/nav_0"), AckermannDriveStamped, queue_size=2) self.path_pub = rospy.Publisher("/mppi/paths", Path, queue_size = self.num_viz_paths) self.plan_pub = rospy.Publisher("/mppi/plan_path", Path, queue_size = self.num_viz_paths) # Use the 'static_map' service (launched by MapServer.launch) to get the map map_service_name = rospy.get_param("~static_map", "static_map") print("Getting map from service: ", map_service_name) rospy.wait_for_service(map_service_name) map_msg = rospy.ServiceProxy(map_service_name, GetMap)().map # The map, will get passed to init of sensor model self.map_data = torch.cuda.LongTensor(map_msg.data) self.map_info = map_msg.info # Save info about map for later use print("Map Information:\n",self.map_info) ############## FOR FINAL DEMO plan.py self.currentPlanWaypointIndex = -1 desiredWaypointIndexToExecute = 8 for i in range(desiredWaypointIndexToExecute + 1): self.advance_to_next_goal() ################## # Create numpy array representing map for later use self.map_height = map_msg.info.height self.map_width = map_msg.info.width array_255 = np.array(map_msg.data).reshape((map_msg.info.height, map_msg.info.width)) self.permissible_region = np.zeros_like(array_255, dtype=bool) self.permissible_region[array_255==0] = 1 # Numpy array of dimension (map_msg.info.height, map_msg.info.width), # With values 0: not permissible, 1: permissible #self.permissible_region = np.negative(self.permissible_region) # 0 is permissible, 1 is not planx = (2600,1880,1435,1250,540) plany = (660,440,545,460,835) i = 0 particalx = 1000 particaly = 1000 if particalx == planx[i] and particaly == plany[i]: i += 1 current_waypoint = planx[i],plany[i] grid_poses = planx[i],plany[i], 0 print grid_poses Utils.world_to_map(grid_poses, self.map_info) print current_waypoint particalx = 2600 particaly = 660 if particalx == planx[i] and particaly == plany[i]: i += 1 current_waypoint = planx[i],plany[i] print current_waypoint pause() # csvfile = open(CSV_FILE, 'r') # # reader = csv.reader(csvfile, delimiter=',') # # len_csv = sum(1 for row in reader) # # badpoints = np.zeros((len_csv - 1, 2), dtype=np.int32) # firstLine = True # # index = 0 # reader = csv.reader(csvfile, delimiter=',') # badpoints = None # for row in reader: # if firstLine: # firstLine = False # continue # if badpoints is None: # print('C') # badpoints = np.array([[int(row[0]), self.map_height - int(row[1])]], dtype=np.int32) # else: # badpoints = np.append(badpoints, np.array([[int(row[0]), self.map_height - int(row[1])]], dtype=np.int32), axis=0) # badpoints = None with open(CSV_FILE, 'r') as csvfile: reader = csv.reader(csvfile, delimiter=',') firstLine = True for row in reader: if firstLine: firstLine = False continue if badpoints is None: print('C') badpoints = np.array([[int(row[0]), self.map_height - int(row[1])]], dtype=np.int32) else: badpoints = np.append(badpoints, np.array([[int(row[0]), int(row[1])]], dtype=np.int32), axis=0) print('Badpoints: ', badpoints) redBufferSpace = 10 for i in range(0, redBufferSpace+1): plus0_indices = np.minimum(badpoints[:, 0] + i, self.permissible_region.shape[0] - 1) minus0_indices = np.maximum(badpoints[:, 0] - i, 0) for j in range(0, redBufferSpace+1): minus1_indices = np.maximum(badpoints[:,1]+j, 0) plus1_indices = np.minimum(badpoints[:,1] - j, self.permissible_region.shape[1] -1) self.permissible_region[minus0_indices, plus1_indices] = 1 self.permissible_region[plus0_indices, minus1_indices] = 1 self.permissible_region[plus0_indices, plus1_indices] = 1 self.permissible_region[minus0_indices, minus1_indices] = 1 print('Sum in permissible region before: ', np.sum(self.permissible_region)) indices = np.argwhere(array_255 == 100) bufferSize = 16 # Tune this for i in range(0, bufferSize + 1): # Create buffer between car and walls print('i: ', i) plus0_indices = np.minimum(indices[:, 0] + i, self.permissible_region.shape[0] - 1) minus0_indices = np.maximum(indices[:, 0] - i, 0) for j in range(0, bufferSize+1): minus1_indices = np.maximum(indices[:,1]-j, 0) plus1_indices = np.minimum(indices[:,1] - j, self.permissible_region.shape[1] -1) self.permissible_region[minus0_indices, plus1_indices] = 1 self.permissible_region[plus0_indices, minus1_indices] = 1 self.permissible_region[plus0_indices, plus1_indices] = 1 self.permissible_region[minus0_indices, minus1_indices] = 1 print('Sum in permissible region after: ', np.sum(self.permissible_region)) self.permissible_region_torch = torch.from_numpy( self.permissible_region.astype(float) ).type(self.dtype) print("Making callbacks") self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.clicked_goal_cb, queue_size=1) self.pose_sub = rospy.Subscriber("/pf/ta/viz/inferred_pose", PoseStamped, self.mppi_cb, queue_size=1)
'''start_pose = np.array([[1490, 570, 1.85]]) waypoints = np.array([[600, 835, 4.0 ], [600, 700, 5.0 ], [2500, 640, 6.0]])''' start_pose1 = np.array([32.7, 11.3, 4]) waypoints1 = np.array([30.52, 11.05, 2]) plan = [] map_img, map_info = utils.get_map(MAP_TOPIC) '''Below 8 lines were used to find the intermediate waypoints to create an optimal path''' map_to_world(start_pose, map_info) map_to_world(waypoints, map_info) start_pose1 = utils.world_to_map(start_pose1, map_info) waypoints1 = utils.world_to_map(waypoints1, map_info) print('start_pose1:') print(start_pose1) print('waypoints1:') print(waypoints1) pp = PathPlanner(plan, waypoints) # offline plan location offline_path = '/home/car-user/racecar_ws/src/final/offline_paths/final_refined.npy' # check if offline plan already exists. if yes, do not continue if os.path.isfile(offline_path):