def odomCB(self, msg): ''' Store deltas between consecutive odometry messages in the coordinate space of the car. Odometry data is accumulated via dead reckoning, so it is very inaccurate on its own. ''' position = np.array([ msg.pose.pose.position.x, msg.pose.pose.position.y]) orientation = Utils.quaternion_to_angle(msg.pose.pose.orientation) pose = np.array([position[0], position[1], orientation]) if isinstance(self.last_pose, np.ndarray): # changes in x,y,theta in local coordinate system of the car rot = Utils.rotation_matrix(-self.last_pose[2]) delta = np.array([position - self.last_pose[0:2]]).transpose() local_delta = (rot*delta).transpose() self.odometry_data = np.array([local_delta[0,0], local_delta[0,1], orientation - self.last_pose[2]]) self.last_pose = pose self.last_stamp = msg.header.stamp self.odom_initialized = True else: print "...Received first Odometry message" self.last_pose = pose # this topic is slower than lidar, so update every time we receive a message self.update()
def initialize_particles_pose(self, pose): ''' Initialize particles in the general region of the provided pose. ''' print "SETTING POSE" print pose self.state_lock.acquire() self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES) self.particles[:,0] = pose.position.x + np.random.normal(loc=0.0,scale=0.5,size=self.MAX_PARTICLES) self.particles[:,1] = pose.position.y + np.random.normal(loc=0.0,scale=0.5,size=self.MAX_PARTICLES) self.particles[:,2] = Utils.quaternion_to_angle(pose.orientation) + np.random.normal(loc=0.0,scale=0.4,size=self.MAX_PARTICLES) self.state_lock.release()
def clicked_goal_cb(self, msg): self.goal = np.array([msg.pose.position.x, msg.pose.position.y, Utils.quaternion_to_angle(msg.pose.orientation)]) print("Current Pose: ", self.last_pose) print("SETTING Goal: ", self.goal)
def mppi_cb(self, msg): #print("callback") if self.last_pose is None: self.last_pose = torch.Tensor([ msg.pose.position.x, msg.pose.position.y, Utils.quaternion_to_angle(msg.pose.orientation) ]).type(self.dtype) # Default: initial goal to be where the car is when MPPI node is initialized # If we are testing, don't update the goal (we modify it in the test instead) if not self.testing: self.goal_tensor = self.last_pose.clone() self.lasttime = msg.header.stamp.to_sec() return theta = Utils.quaternion_to_angle(msg.pose.orientation) curr_pose = torch.Tensor( [msg.pose.position.x, msg.pose.position.y, theta]).type(self.dtype) difference_from_goal = np.sqrt((curr_pose[0] - self.goal_tensor[0])**2 + (curr_pose[1] - self.goal_tensor[1])**2) if difference_from_goal < 0.5: self.MIN_VEL = -0.45 #TODO make sure these are right self.MAX_VEL = 0.45 # else: # self.MIN_VEL = -0.75 #TODO make sure these are right # self.MAX_VEL = 0.75 # don't do any goal checking for testing purposes pose_dot = curr_pose - self.last_pose # get state self.last_pose = curr_pose timenow = msg.header.stamp.to_sec() dt = timenow - self.lasttime self.lasttime = timenow nn_input = torch.Tensor([ pose_dot[0], pose_dot[1], pose_dot[2], np.sin(theta), np.cos(theta), 0.0, 0.0, dt ]).type(self.dtype) self.pose_pub.publish(Utils.particle_to_posestamped(curr_pose, 'map')) poses = self.mppi(curr_pose, nn_input) run_ctrl = None if not self.cont_ctrl: run_ctrl = self.get_control().cpu().numpy() self.recent_controls[self.control_i] = run_ctrl self.control_i = (self.control_i + 1) % self.recent_controls.shape[0] pub_control = self.recent_controls.mean(0) self.speed = pub_control[0] self.steering_angle = pub_control[1] if self.viz: self.visualize(poses) # For testing: send control into model and pretend this is the real location if self.testing: test_nn_input = nn_input.clone() test_nn_input[5:7] = run_ctrl test_pose_dot = self.model( Variable(test_nn_input, requires_grad=False)) test_pose_dot = test_pose_dot.data test_pose = curr_pose + test_pose_dot test_pose[2] = Utils.clamp_angle(test_pose[2]) return test_pose
#if hasattr(msg.pose, 'pose'): """ if initialize: x = msg.pose.pose.position.x y = msg.pose.pose.position.y theta = Utils.quaternion_to_angle(msg.pose.pose.orientation) particles = np.array([[x,y,theta]], dtype=np.float64) oLR = OpenLoopRollout(particles, state_lock) initialize = False else: """ oLR.apply_motion_model(msg) elif topic == '/initialpose': x = msg.pose.pose.position.x y = msg.pose.pose.position.y theta = Utils.quaternion_to_angle(msg.pose.pose.orientation) particles = np.array([[x,y,theta]], dtype=np.float64) oLR = OpenLoopRollout(particles, state_lock) #initialize = False elif 'map' in topic: #print(msg) print('') elif topic == '/pf/ta/viz/inferred_pose' and oLR is not None: x = msg.pose.position.x y = msg.pose.position.y oLR.add_true_pose(np.array([[x, y]])) #elif (topic == "/vesc/sensors/core" or topic == "/vesc/sensors/servo_position_command") and oLR is not None: # oLR.apply_kin_model(msg, topic) #elif 'offset' in topic: # print(topic)
def odomCB(self, msg): self.trajectory.addPoint( msg.pose.pose.position.x, msg.pose.pose.position.y, quaternion_to_angle(msg.pose.pose.orientation)) self.trajectory.publish_viz()
def mppi_cb(self, msg): #print("callback") if self.last_pose is None: self.last_pose = torch.Tensor([ msg.pose.position.x, msg.pose.position.y, Utils.quaternion_to_angle(msg.pose.orientation) ]).type(self.dtype) self.lasttime = msg.header.stamp.to_sec() if self.goal_tensor is None: # Default: initial goal to be where the car is when MPPI node is initialized self.goal_tensor = torch.Tensor([ msg.pose.position.x, msg.pose.position.y, Utils.quaternion_to_angle(msg.pose.orientation) ]).type(self.dtype) return theta = Utils.quaternion_to_angle(msg.pose.orientation) curr_pose = torch.Tensor( [msg.pose.position.x, msg.pose.position.y, theta]).type(self.dtype) # don't do any goal checking for testing purposes if not self.testing: if self.at_goal: print 'Already at goal' return # TODO: if close to the goal, there's nothing to do XY_THRESHOLD = 0.35 THETA_THRESHOLD = 0.5 # about 10 degrees difference_from_goal = (curr_pose - self.goal_tensor).abs() xy_distance_to_goal = difference_from_goal[:2].norm() theta_distance_to_goal = difference_from_goal[2] % (2 * np.pi) if xy_distance_to_goal < XY_THRESHOLD and theta_distance_to_goal < THETA_THRESHOLD: print 'Goal achieved' self.at_goal = True self.speed = 0 self.steering_angle = 0 return pose_dot = curr_pose - self.last_pose # get state self.last_pose = curr_pose timenow = msg.header.stamp.to_sec() dt = timenow - self.lasttime self.lasttime = timenow nn_input = torch.Tensor([ pose_dot[0], pose_dot[1], pose_dot[2], np.sin(theta), np.cos(theta), 0.0, 0.0, dt ]).type(self.dtype) self.pose_pub.publish(Utils.particle_to_posestamped(curr_pose, 'map')) poses = self.mppi(curr_pose, nn_input) run_ctrl = None if not self.cont_ctrl: run_ctrl = self.get_control() self.speed = run_ctrl[0] self.steering_angle = run_ctrl[1] if self.viz: self.visualize(poses) # For testing: send control into model and pretend this is the real location if self.testing: # TODO kinematic model can't handle just (3,)? self.model.particles = curr_pose.view(1, 3) self.model.controls = torch.Tensor( [self.speed, self.steering_angle]).type(self.dtype).view(1, 2) self.model.ackerman_equations() return self.model.particles.view(3)
def updateControl(self, message): if self.pathSet: # Part 0 - Init position and orientation self.pose = np.array( [message.pose.position.x, message.pose.position.y]) self.orientation = Utils.quaternion_to_angle( message.pose.orientation) # # Part 1 - Find closest node # distToNodesSq = self.distSq(self.path, self.pose) # # Check if at node # if len(np.argwhere(distToNodesSq == 0)) > 0: # closestNode = self.pose # closestNodeIndex = np.argwhere(distToNodesSq==0).flatten()[0] # # else, find closest node # else: # t = np.clip( np.sum( (self.pose-self.path[:-1])*self.pathSegs, axis=1 ), 0, 0.995) # projection = self.path[:-1] + (t[:, None]*self.pathSegs) # distToSegments = self.dist(projection, self.pose) # closestNodeIndex = np.argmin(distToSegments)+1 # closestNode = self.path[closestNodeIndex] # # Too close to goal, go straight to it # distToGoalSq = self.distSq(self.pose, self.goalLoc) # if distToGoalSq < self.Lfw2: # print 'close to goal' # # If within radius, stop # if distToGoalSq < 2000: # self.speed = 0 # # Go straight to goal # self.driveTowardNode(self.goalLoc) # # Check line segments starting from closestNode # # Find intersection between circle around car and forward path from closestPoint # else: # pointTracker = closestNodeIndex # goal = None # while goal is None: # if pointTracker > len(self.path)-2: # break # p1 = self.path[pointTracker] # v = self.path[pointTracker+1] - self.path[pointTracker] # a = np.dot(v, v) # b = 2 * np.dot(v, (p1-self.pose)) # c = np.dot(p1, p1) + np.dot(self.pose, self.pose)\ # - 2 * np.dot(p1, self.pose) - self.Lfw2 # disc = b**2 - 4*a*c # if disc < 0: # goal = None # else: # sqrt_disc = math.sqrt(disc) # t1 = (-b + sqrt_disc) / float((2*a)) # t2 = (-b - sqrt_disc) / float((2*a)) # if not (0 <= t1 <= 1 or 0 <= t2 <= 1): # goal = None # else: # if not (0 <= t1 <= 1) and (0 <= t2 <= 1): # t = t2 # elif (0 <= t1 <= 1) and not (0 <= t2 <= 1): # t = t1 # else: # t = max(t1, t2) # goal = p1+(t*v) # pointTracker = pointTracker + 1 # # If no goal found # if goal is None: # print 'too far away' # self.driveTowardNode(closestNode) # else: # Pure pursuit # self.purePursuit(goal) # if self.goalReachedFlag == False: # currPointLoc = self.path[self.pointCounter] # currDist = self.dist(currPointLoc, self.pose) # print currPointLoc-self.pose # if currDist > 10: # self.driveTowardNode(currPointLoc) # else: # if self.pointCounter != (len(self.path)-1): # self.pointCounter = self.pointCounter + 1 # print "new node acquired" # else: # print "goal reached" # self.goalReachedFlag = True # else: # self.speed = 0 # self.steering_angle = 0 currPointLoc = self.path[self.pointCounter] currDist = self.dist(currPointLoc, self.pose) print "Distance: ", currDist if currDist > 10: self.driveTowardNode(currPointLoc) else: self.purePursuit(currPointLoc) if currDist < 0.2: if self.pointCounter != (len(self.path) - 1): self.pointCounter = self.pointCounter + 1 print "new node acquired" else: print "goal reached" self.pathSet = False # Publish message msg = AckermannDriveStamped() msg.drive.steering_angle = self.steering_angle msg.drive.speed = self.speed self.pub.publish(msg)
def set_current_pose(self,msg): x = msg.pose.pose.position.x y = msg.pose.pose.position.y robot_angle = Utils.quaternion_to_angle(msg.pose.pose.orientation) self.current_pose = np.array([x,y,robot_angle])
def __init__(self, T, K, sigma=[0.5, 0.1], _lambda=0.5, testing=False): self.MIN_VEL = -0.8 #TODO make sure these are right self.MAX_VEL = 0.8 self.MIN_DEL = -0.34 self.MAX_DEL = 0.34 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)) self.CAR_LENGTH = 0.33 self.testing = testing self.last_pose = None # MPPI params self.T = T # Length of rollout horizon self.K = K # Number of sample rollouts self._lambda = _lambda self.goal_tensor = None self.lasttime = 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 self.dtype = torch.cuda.FloatTensor self.model = KinematicMotionModel() print("Created Kinematic Motion Model") print("Torch Datatype:", self.dtype) # initialize these once self.ctrl = torch.zeros((T,2)).cuda() self.sigma = torch.Tensor(sigma).type(self.dtype) self.inv_sigma = 1.0 / self.sigma #(2,) self.sigma = self.sigma.repeat(self.T, self.K, 1) # (T,K,2) self.noise = torch.Tensor(self.T, self.K, 2).type(self.dtype) # (T,K,2) self.poses = torch.Tensor(self.K, self.T, 3).type(self.dtype) # (K,T,3) self.running_cost = torch.zeros(self.K).type(self.dtype) # (K,) # control outputs self.msgid = 0 # visualization paramters self.num_viz_paths = 40 if self.K < self.num_viz_paths: self.num_viz_paths = self.K # 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.central_path_pub = rospy.Publisher("/mppi/path_center", Path, queue_size = 1) # 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_info = map_msg.info # Save info about map for later use # rotation self.map_angle = -Utils.quaternion_to_angle(self.map_info.origin.orientation) self.map_c, self.map_s = np.cos(self.map_angle), np.sin(self.map_angle) print("Map Information:\n",self.map_info) # 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.logical_not(self.permissible_region) # 0 is permissible, 1 is not self.permissible_region = torch.from_numpy(self.permissible_region.astype(np.int)).type(torch.cuda.ByteTensor) # since we are using it as a tensor self.pose_pub = rospy.Publisher("/mppi/pose", PoseStamped, queue_size=10) 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)
def clicked_goal_cb(self, msg): self.goal_tensor = torch.Tensor([msg.pose.position.x, msg.pose.position.y, Utils.quaternion_to_angle(msg.pose.orientation)]).type(self.dtype) print("Current Pose: ", self.last_pose) print("SETTING Goal: ", self.goal_tensor)
def main(): rospy.init_node('line_follower', anonymous=True) # Initialize the node """ Load these parameters from launch file We provide suggested starting values of params, but you should tune them to get the best performance for your system Look at constructor of LineFollower class for description of each var 'Default' values are ones that probably don't need to be changed (but you could for fun) 'Starting' values are ones you should consider tuning for your system """ # YOUR CODE HERE plan_topic = rospy.get_param( '~plan_topic') # Default val: '/planner_node/car_plan' pose_topic = rospy.get_param( '~pose_topic') # Default val: '/sim_car_pose/pose' if True: # if on robot? else in rviz pose_topic = "/pf/viz/inferred_pose" plan_lookahead = rospy.get_param('~plan_lookahead') # Starting val: 5 translation_weight = rospy.get_param( '~translation_weight') # Starting val: 1.0 rotation_weight = rospy.get_param('~rotation_weight') # Starting val: 0.0 kp = rospy.get_param('~kp') # Startinig val: 1.0 ki = rospy.get_param('~ki') # Starting val: 0.0 kd = rospy.get_param('~kd') # Starting val: 0.0 error_buff_length = rospy.get_param( '~error_buff_length') # Starting val: 10 speed = 2.0 # rospy.get_param('~speed') # Default val: 1.0 loadFinalPlan = True # set this to True to use preexisting plan instead of creating a new one in RVIZ if not loadFinalPlan: # make new plan in RVIZ by setting initial and goal poses raw_input("Press Enter to when plan available..." ) # Waits for ENTER key press # Use rospy.wait_for_message to get the plan msg # Convert the plan msg to a list of 3-element numpy arrays # Each array is of the form [x,y,theta] # Create a LineFollower object raw_plan = rospy.wait_for_message(plan_topic, PoseArray) # raw_plan is a PoseArray which has an array of geometry_msgs/Pose called poses plan_array = [] for pose in raw_plan.poses: plan_array.append( np.array([ pose.position.x, pose.position.y, utils.quaternion_to_angle(pose.orientation) ])) else: # use preexisting plan from plan_creator.launch and plan_cleanup.launch # # # print "Len of plan array: %d" % len(plan_array) # # # # print plan_array plan_relative_path = "/saved_plans/plan_12_9_2018" # load plan_array # load raw_plan msg (PoseArray) loaded_vars = pickle.load( open(CURRENT_PKG_PATH + plan_relative_path, "r")) plan_array = loaded_vars[0] raw_plan = loaded_vars[1] # visualize loaded plan PA_pub = rospy.Publisher("/LoadedPlan", PoseArray, queue_size=1) for i in range(0, 5): rospy.sleep(0.5) PA_pub.publish(raw_plan) # # print "LoadedPlan Published" try: if raw_plan: pass except rospy.ROSException: exit(1) lf = LineFollower(plan_array, pose_topic, plan_lookahead, translation_weight, rotation_weight, kp, ki, kd, error_buff_length, speed) # Create a Line follower rospy.spin() # Prevents node from shutting down
def pose_cb(self, msg): # print "inside line_follower ,pose_cb" time.sleep(0) # # print "Callback received current pose. " cur_pose = np.array([ msg.pose.position.x, msg.pose.position.y, utils.quaternion_to_angle(msg.pose.orientation) ]) # print "Current pose: ", cur_pose # # # # print "plan[:,[0,1]]", type(np.array(self.plan)), np.array(self.plan)[:,[0,1]] # # find closest point and delete all points before it in the plan # # only done once at the start of following the plan # if self.found_closest_point == False: # min_path_distance = np.Infinity # to find closest path point and delete all points before it # for count, position in enumerate(np.array(self.plan)[:, [0, 1]]): # distance = np.sqrt(np.square(cur_pose[0] - position[0]) + np.square(cur_pose[1] - position[1])) # if distance < min_path_distance: # self.found_closest_point = True # min_path_distance = distance # if count > 0: # self.plan.pop(0) success, error = self.compute_error(cur_pose) self.error = error # print "Success, Error: ", success, error if not success: # We have reached our goal self.pose_sub = None # Kill the subscriber self.speed = 0.0 # Set speed to zero so car stops if False: # show error plot? # plot the error here title_string = "Error plot with kp=%.2f, kd=%.2f, ki=%.2f t_w=%.2f r_w=%.2f" % \ (self.kp, self.kd, self.ki, self.translation_weight, self.rotation_weight) fig = plt.figure() ax = fig.add_subplot(111) # ax.plot(self.total_error_list) plt.title(title_string) plt.text(0.5, 0.85, 'Total error = %.2f' % np.trapz(abs(np.array(self.total_error_list))), horizontalalignment='center', verticalalignment='center', transform=ax.transAxes) plt.xlabel('Iterations') plt.ylabel('Error') plt.show() np.savetxt("/home/joe/Desktop/Error_1.csv", np.array(self.total_error_list), delimiter=",") return 0 f = None # if computer vision angle is published then use that angle pid_angle = self.compute_steering_angle(error) # if self.angle_from_computer_vision is not None and self.angle_from_computer_vision > -98.0 and self.error < 2: if False: delta = self.angle_from_computer_vision print "CV ANGLE chosen: ", delta else: # if computer vision angle is not published then use pid controller angle delta = pid_angle print "PID ANGLE chosen: ", delta try: self.f.write("CV ANGLE: " + str(delta) + "\tPID ANGLE" + str(pid_angle)) except (IOError, AttributeError): pass if delta < self.min_delta: self.min_delta = delta if delta > self.max_delta: self.max_delta = delta print 'min=%f and max=%f' % (self.min_delta, self.max_delta) # if True: # not using laser_wanderer_robot.launch # Setup the control message ads = AckermannDriveStamped() ads.header.frame_id = '/map' ads.header.stamp = rospy.Time.now() ads.drive.steering_angle = delta ads.drive.speed = 2.0 self.cmd_pub.publish(ads) # Send the control message to laser_wanderer_robot.launch else: float_msg = Float32() float_msg.data = delta self.float_pub.publish(float_msg)
def normalize_angle(angle): #return angle return Utils.quaternion_to_angle(Utils.angle_to_quaternion(angle))
def set_inferred_pose(self, msg): if not self.start_flag: self.start = (msg.pose.position.x, msg.pose.position.y) self.start_orientation = Utils.quaternion_to_angle(msg.pose.orientation) self.start_flag = True
def load_raw_datas(): return np.load("/home/nvidia/our_catkin_ws/src/lab3/src/raw_datas.npy") bag = rosbag.Bag(argv[1]) tandt = bag.get_type_and_topic_info() t1='/vesc/sensors/core' t2='/vesc/sensors/servo_position_command' t3='/pf/ta/viz/inferred_pose' topics = [t1,t2,t3] min_datas = tandt[1][t3][1] # number of t3 messages is less than t1, t2 raw_datas = np.zeros((min_datas,DATA_SIZE)) last_servo, last_vel = 0.0, 0.0 n_servo, n_vel = 0, 0 idx=0 # The following for-loop cycles through the bag file and averages all control # inputs until an inferred_pose from the particle filter is recieved. We then # save that data into a buffer for later processing. # You should experiment with additional data streams to see if your model # performance improves. for topic, msg, t in bag.read_messages(topics=topics): if topic == t1: last_vel += (msg.state.speed - SPEED_TO_ERPM_OFFSET) / SPEED_TO_ERPM_GAIN n_vel += 1 elif topic == t2: last_servo += (msg.data - STEERING_TO_SERVO_OFFSET) / STEERING_TO_SERVO_GAIN n_servo += 1 elif topic == t3 and n_vel > 0 and n_servo > 0: timenow = msg.header.stamp last_t = timenow.to_sec() last_vel /= n_vel last_servo /= n_servo orientation = Utils.quaternion_to_angle(msg.pose.orientation) data = np.array([msg.pose.position.x, msg.pose.position.y, orientation, last_vel, last_servo, last_t]) raw_datas[idx,:] = data last_vel = 0.0 last_servo = 0.0 n_vel = 0 n_servo = 0 idx = idx+1 if idx % 1000==0: print('.') bag.close() # Pre-process the data to remove outliers, filter for smoothness, and calculate # values not directly measured by sensors # Note: # Neural networks and other machine learning methods would prefer terms to be # equally weighted, or in approximately the same range of values. Here, we can # keep the range of values to be between -1 and 1, but any data manipulation we # do here from raw values to our model input we will also need to do in our # MPPI code. # We have collected: # raw_datas = [ x, y, theta, v, delta, time] # We want to have: # x_datas[i, :] = [x_dot, y_dot, theta_dot, sin(theta), cos(theta), v, delta, dt] # y_datas[i-1,:] = [x_dot, y_dot, theta_dot ] raw_datas = raw_datas[:idx, :] # Clip to only data found from bag file raw_datas = raw_datas[ np.abs(raw_datas[:,3]) < 0.75 ] # discard bad controls raw_datas = raw_datas[ np.abs(raw_datas[:,4]) < 0.36 ] # discard bad controls np.save("raw_datas.npy", raw_datas) return raw_datas
if __name__ == '__main__': model = 'odom' bag_path = '/home/nvidia/catkin_ws/src/lab1/bags/circle.bag' positions = [] actual_pos = [] num_iters = 1000 if model == 'odom': init_top = '/initialpose' odom_top = '/vesc/odom' infr_top = '/pf/ta/viz/inferred_pose' bag = rosbag.Bag(bag_path) # get initial pose message from bag init_msg = list(bag.read_messages(topics=init_top))[0][1] x,y = init_msg.pose.pose.position.x, init_msg.pose.pose.position.y yaw = utils.quaternion_to_angle(init_msg.pose.pose.orientation) particles = np.array((x,y,yaw)).reshape((1,3)) # TODO: remove noise odom_mm = MotionModel.OdometryMotionModel(particles, noise=False) positions.append(tuple(odom_mm.particles[0][:2])) for i, tup in enumerate(bag.read_messages(topics=odom_top)): print odom_mm.particles topic, msg, t = tup # apply movement msg odom_mm.motion_cb(msg) positions.append(tuple(odom_mm.particles[0][:2])) if i == num_iters: break for i, tup in enumerate(bag.read_messages(topics=infr_top)): topic, msg, t = tup actual_pos.append((msg.pose.position.x, msg.pose.position.y))
def poseCB(self, msg): pose = np.array([msg.pose.pose.position.x, msg.pose.pose.position.y, utils.quaternion_to_angle(msg.pose.pose.orientation)]) if(self.init == True and self.stop_charging == False): self.pure_pursuit(pose)
def pose_cb(self, msg): print "" time.sleep(0) print "Callback received current pose. " cur_pose = np.array([ msg.pose.position.x, msg.pose.position.y, utils.quaternion_to_angle(msg.pose.orientation) ]) print "Current pose: ", cur_pose # print "plan[:,[0,1]]", type(np.array(self.plan)), np.array(self.plan)[:,[0,1]] # find closest point and delete all points before it in the plan # only done once at the start of following the plan if self.found_closest_point == False: min_path_distance = np.Infinity # to find closest path point and delete all points before it for count, position in enumerate(np.array(self.plan)[:, [0, 1]]): distance = np.sqrt( np.square(cur_pose[0] - position[0]) + np.square(cur_pose[1] - position[1])) if distance < min_path_distance: self.found_closest_point = True min_path_distance = distance if count > 0: self.plan.pop(0) success, error = self.compute_error(cur_pose) print "Success, Error: ", success, error if not success: # We have reached our goal self.pose_sub = None # Kill the subscriber self.speed = 0.0 # Set speed to zero so car stops # plot the error here title_string = "Error plot with kp=%.2f, kd=%.2f, ki=%.2f t_w=%.2f r_w=%.2f" % \ (self.kp, self.kd, self.ki, self.translation_weight, self.rotation_weight) fig = plt.figure() ax = fig.add_subplot(111) # ax.plot(self.total_error_list) plt.title(title_string) plt.text(0.5, 0.85, 'Total error = %.2f' % np.trapz(abs(np.array(self.total_error_list))), horizontalalignment='center', verticalalignment='center', transform=ax.transAxes) plt.xlabel('Iterations') plt.ylabel('Error') plt.show() np.savetxt("/home/joe/Desktop/Error_1.csv", np.array(self.total_error_list), delimiter=",") return 0 delta = self.compute_steering_angle(error) print "delta is %f" % delta # Setup the control message ads = AckermannDriveStamped() ads.header.frame_id = '/map' ads.header.stamp = rospy.Time.now() ads.drive.steering_angle = delta ads.drive.speed = self.speed # Send the control message self.cmd_pub.publish(ads)
def main(): rospy.init_node('line_follower', anonymous=True) # Initialize the node """ Load these parameters from launch file We provide suggested starting values of params, but you should tune them to get the best performance for your system Look at constructor of LineFollower class for description of each var 'Default' values are ones that probably don't need to be changed (but you could for fun) 'Starting' values are ones you should consider tuning for your system """ # YOUR CODE HERE plan_topic = rospy.get_param( '~plan_topic') # Default val: '/planner_node/car_plan' pose_topic = rospy.get_param( '~pose_topic') # Default val: '/sim_car_pose/pose' plan_lookahead = rospy.get_param('~plan_lookahead') # Starting val: 5 translation_weight = rospy.get_param( '~translation_weight') # Starting val: 1.0 rotation_weight = rospy.get_param('~rotation_weight') # Starting val: 0.0 kp = rospy.get_param('~kp') # Startinig val: 1.0 ki = rospy.get_param('~ki') # Starting val: 0.0 kd = rospy.get_param('~kd') # Starting val: 0.0 error_buff_length = rospy.get_param( '~error_buff_length') # Starting val: 10 speed = rospy.get_param('~speed') # Default val: 1.0 raw_input( "Press Enter to when plan available...") # Waits for ENTER key press # Use rospy.wait_for_message to get the plan msg # Convert the plan msg to a list of 3-element numpy arrays # Each array is of the form [x,y,theta] # Create a LineFollower object raw_plan = rospy.wait_for_message(plan_topic, PoseArray) # raw_plan is a PoseArray which has an array of geometry_msgs/Pose called poses plan_array = [] for pose in raw_plan.poses: plan_array.append( np.array([ pose.position.x, pose.position.y, utils.quaternion_to_angle(pose.orientation) ])) print "Len of plan array: %d" % len(plan_array) # print plan_array try: if raw_plan: pass except rospy.ROSException: exit(1) lf = LineFollower(plan_array, pose_topic, plan_lookahead, translation_weight, rotation_weight, kp, ki, kd, error_buff_length, speed) # Create a Line follower rospy.spin() # Prevents node from shutting down
def pose_to_config(msg): return np.array([msg.position.x, msg.position.y, utils.quaternion_to_angle(msg.orientation)], np.float)
def main(): sys.stderr = DevNull() #ignore error messages rospy.init_node('line_follower', anonymous=True) # Initialize the node # Load these parameters from launch file # We provide suggested starting values of params, but you should # tune them to get the best performance for your system # Look at constructor of LineFollower class for description of each var # 'Default' values are ones that probably don't need to be changed (but you could for fun) # 'Starting' values are ones you should consider tuning for your system # YOUR CODE HERE plan_topic = rospy.get_param( '~plan_topic', '/planner_node/car_plan') # Default val: '/planner_node/car_plan' pose_topic = rospy.get_param( '~pose_topic', 'pf/viz/inferred_pose' ) # pf/viz/inferred_pose from particle filter, '/car/car_pose' from map ground truth plan_lookahead = rospy.get_param('plan_lookahead', 5) # Starting val: 5 translation_weight = rospy.get_param('~translation_weight', 1.0) # Starting val: 1.0 rotation_weight = rospy.get_param('~rotation_weight', 0.0) # Starting val: 0.0 kp = rospy.get_param('~kp', 1.0) # Startinig val: 1.0 ki = rospy.get_param('~ki', 0.0) # Starting val: 0.0 kd = rospy.get_param('~kd', 0.0) # Starting val: 0.0 error_buff_length = rospy.get_param('~error_buff_length', 10) # Starting val: 10 speed = rospy.get_param('~speed', 1.0) # Default val: 1.0 # print('set the car initialpose to the start waypoint') raw_input("Press Enter to set initialpose...") # Waits for ENTER key press initialpose_pub = rospy.Publisher(initialpose_topic, PoseWithCovarianceStamped, queue_size=10) pcs = PoseWithCovarianceStamped() pcs.header.frame_id = "map" pcs.header.stamp = rospy.Time() pcs.pose.pose.position.x = 49.9 pcs.pose.pose.position.y = 11.938 pcs.pose.pose.position.z = 0.0 pcs.pose.pose.orientation.x = 0.0 pcs.pose.pose.orientation.y = 0.0 pcs.pose.pose.orientation.z = -0.105 pcs.pose.pose.orientation.w = 0.995 rospy.sleep(1.0) initialpose_pub.publish(pcs) # Use rospy.wait_for_message to get the plan msg # Convert the plan msg to a list of 3-element numpy arrays # Each array is of the form [x,y,theta] # Create a LineFollower object # YOUR CODE HERE # rostopic info /planner_node/car_plan Type: geometry_msgs/PoseArray print( 'waiting for the plan to be published from PlannerNode, plan_topic: ', plan_topic) converted_plan = [] for msg in rospy.wait_for_message('/planner_node/car_plan', PoseArray).poses: converted_plan.append([ msg.position.x, msg.position.y, utils.quaternion_to_angle(msg.orientation) ]) print('plan is received, car starts to drive along the plan...') # Create a LineFollower object LineFollower(converted_plan, pose_topic, plan_lookahead, translation_weight, rotation_weight, kp, ki, kd, error_buff_length, speed) rospy.spin() # Prevents node from shutting down
def path_plan(self): plan_array_poses = [] #array for poses from plan array pose_array = PoseArray() good_waypoints = [ [49.99999888, 12.79999971, 0.542342069743, 0.840157770533], [51.50999884, 11.19999971, 0.945164118288, -0.326595758546], [47.3542327881, 10.4503288269, 0.987244066665, 0.159214172846], [39.5492019653, 13.1202926636, 0.98515892037, 0.171644695857], [37.59999916, 16.7999998, 0.967361322004, 0.247087236826], [33.38514328, 15.2581377029, -0.800676142281, 0.599097417105], [28.69999936, 12.89999976, 0.896279989118, 0.443488648228], [24.99999944, 15.19999979, -0.972833980528, 0.231503879732], [21.3340473175, 16.02470054, 0.999556369522, 0.0297836221581], [17.68499976, 14.08699963, -0.973198562116, 0.211794571634], [10.79999976, 7.69999963, -0.475298562116, 0.879824571634] ] a = len(good_waypoints) #print "size:", a #print good_waypoints[1][0] package1 = 'racecar' launch1 = 'map_server.launch' package2 = 'planning_utils' launch2 = 'planner_node.launch' #p1 = subprocess.Popen("roslaunch racecar map_server.launch", shell=True) #p2 = subprocess.Popen("roslaunch planning_utils planner_node.launch", shell = True) rosp = rospkg.RosPack() racecar_pkg = rosp.get_path("racecar") uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch_file_map = racecar_pkg + "/launch/teleop.launch" launch_map = roslaunch.parent.ROSLaunchParent(uuid, [launch_file_map]) launch_map.start() rospy.sleep(20) '''' #uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch_file_plannerNode = os.path.join(rospkg.RosPack().get_path(package2), 'launch', launch2) launch_plannerNode = roslaunch.parent.ROSLaunchParent(uuid, [launch_file_plannerNode]) launch_plannerNode.start() rospy.sleep(15) ''' if self.create_new_path: for i in range(0, a - 1): if i is 0: print "Waiting for initial pose.." initPose = rospy.wait_for_message( INITIALPOSE_TOPIC, PoseWithCovarianceStamped) else: initPose = self.createInitPose(good_waypoints[i]) print "pub initial pose :", initPose.pose.pose.position.x, initPose.pose.pose.position.y self.initial_pose_pub.publish(initPose) goalPose = self.createGoalPose(good_waypoints[i + 1]) print "pub goal pose :", goalPose.pose.position.x, initPose.pose.pose.position.y self.goal_pose_pub.publish(goalPose) current_plan = rospy.wait_for_message( '/planner_node/car_plan', PoseArray) # wait for planner node to calculate plan k = len(current_plan.poses) # Convert the plan msg to a list of 3-element numpy arrays # Each array is of the form [x,y,theta] csv_pa = [] for i in range(k): pose_array.header.stamp = rospy.Time.now( ) # set header timestamp value pose_array.header.frame_id = "map" # set header frame id value tempPose = Pose() #tempPose.header.stamp = rospy.Time.now() #tempPose.header.frame_id = 'map' tempPose.position.x = current_plan.poses[i].position.x tempPose.position.y = current_plan.poses[i].position.y tempPose.position.z = current_plan.poses[i].position.z tempPose.orientation = current_plan.poses[i].orientation pose_array.poses.append(tempPose) csv_pose = np.array([ tempPose.position.x, tempPose.position.y, utils.quaternion_to_angle(tempPose.orientation) ], np.float) csv_pa.append(csv_pose) #np.savetxt("/home/pri/ee545/ros_ws/src/final/waypoints/paths/path_1.csv", a, delimiter=",") #instead of publishing to a topic, write to csv once. #myFile = open('/home/pri/ee545/ros_ws/src/final/waypoints/paths/path_1.csv', 'w') #writer = csv.writer(myFile) #writer.writerows(csv_pa) #myFile.close() #tempPose = self.createInitPose(good_waypoints[0]) #self.initial_pose_pub.publish(tempPose) os.system("rosnode kill planner_node") rospy.sleep(10) raw_input("Press Enter to publish the plan ...") print "Pubplishing pose array to topic", FINAL_PLAN_TOPIC n = 0 while not rospy.is_shutdown(): #print "Pubplishing pose array to topic", FINAL_PLAN_TOPIC self.pose_array_pub.publish(pose_array) rospy.sleep(5)
def viz_sub_cb(self, msg): # Create the PoseArray to publish. Will contain N poses, where the n-th pose # represents the last pose in the n-th trajectory # print "inside viz_sub_cb" cur_pose = np.array([ msg.pose.position.x, msg.pose.position.y, utils.quaternion_to_angle(msg.pose.orientation) ]) # current car pose if self.show_all_poses: # all poses in each rollout pose_range = range(0, self.rollouts.shape[1]) else: # only final pose in each rollout pose_range = range(self.rollouts.shape[1] - 2, self.rollouts.shape[1]) # Read http://docs.ros.org/jade/api/geometry_msgs/html/msg/PoseArray.html PA = PoseArray() # create a PoseArray() msg PA.header.stamp = rospy.Time.now() # set header timestamp value PA.header.frame_id = "map" # set header frame id value PA.poses = [] for i in range(0, self.rollouts.shape[0]): # for each [7] rollouts for j in pose_range: # for pose in range(0, 300) to show all, or range(299,300) to show only final pose P = Pose() pose = self.rollouts[ i, j, :] # self.rollouts[i, 299, :] will be an array of [x, y, theta] for the final pose (last j index) for rollout i # This is in car frame, so assumes straight is x axis # P.position.x = pose[0] # P.position.y = pose[1] # P.position.z = 0 # P.orientation = utils.angle_to_quaternion(pose[2]) # Transform pose from car frame to map frame # Method 1: Map Frame to Robot Frame # Rotation must be done before translation with this method, but it's ok to do both in one step # First find translation matrix [2, 1] from map origin to robot position # Second find rotation matrix [2, 2] from map x axis to robot x axis # Third rotate the rollout position about the origin of the robot axis [a.k.a. robot position] with the same angle as from map x axis to robot x axis # Fourth translate the rollout position with the same translation matrix as from the map origin to the robot position translation_map_to_robot = np.array([[cur_pose[0]], [cur_pose[1]] ]).reshape([2, 1]) rotation_matrix_map_to_robot = utils.rotation_matrix( cur_pose[2]) rollout_position = np.array([[pose[0]], [pose[1]]]).reshape([2, 1]) map_position = rotation_matrix_map_to_robot * rollout_position + translation_map_to_robot P.position.x = map_position[0] P.position.y = map_position[1] P.position.z = 0 P.orientation = utils.angle_to_quaternion( pose[2] + cur_pose[2] ) # car's yaw angle + rollout pose's angle from car PA.poses.append(P) # print "Publishing Rollout Vizualization" self.viz_pub.publish(PA)
def mppi_cb(self, msg): # new_lambda = mp._lambda * 0.99 # This wasn't in skeleton code: Decay Lambda # mp.update_lambda(new_lambda) # This wasn't in skeleton code: Decay Lambda # dprint('New Lambda: ', mp._lambda) # This wasn't in skeleton code: Decay Lambda if self.last_pose is None: self.last_pose = self.dtype([ msg.pose.position.x, msg.pose.position.y, Utils.quaternion_to_angle(msg.pose.orientation) ]) # Default: initial goal to be where the car is when MPPI node is # initialized # miyu - commented out for lab 4 #self.goal = self.last_pose self.lasttime = msg.header.stamp.to_sec() return theta = Utils.quaternion_to_angle(msg.pose.orientation) curr_pose = self.dtype( [msg.pose.position.x, msg.pose.position.y, theta]) #t_temp = time.time() #valid = self.out_of_bounds(curr_pose) #print('valid time: ', time.time()-t_temp) #if not valid: # return # print("Got mppi_cb: ", msg, curr_pose) pose_dot = curr_pose.sub(self.last_pose) # get state pose_dot[2] = wrap_pi_pi_number( pose_dot[2] ) # This was not in skeleton code: Clamp Theta between -pi and pi self.last_pose = curr_pose timenow = msg.header.stamp.to_sec() dt = timenow - self.lasttime #dt = 0.1 # from dt to 0.1 self.lasttime = timenow self.neural_net_input.zero_() self.neural_net_input = self.dtype([ pose_dot[0], pose_dot[1], pose_dot[2], np.sin(theta), np.cos(theta), 0.0, 0.0, dt ]) run_ctrl, poses = self.mppi(curr_pose) #, nn_input) #run_ctrl = run_ctrl.view(CONTROL_SIZE) self.U[:, :, 0:self.T - 1] = self.U[:, :, 1:self.T] self.U[:, :, self.T - 1] = 0.0 self.U[:, :, 0:self.T - 1] = self.U[:, :, 1:self.T] self.U[:, :, self.T - 1] = 0.0 consider_roi = plan[self.currentPlanWaypointIndex][2] should_advance = False if not consider_roi: self.send_controls(run_ctrl[0], run_ctrl[1]) print("We're using MPPI", self.currentPlanWaypointIndex, self.roi.tape_seen, self.theta_weight) else: roi_tape_seen = self.roi.tape_seen if roi_tape_seen: control = self.roi.PID.calc_control(self.roi.error) self.roi.PID.drive(control) print("We're using ROI", self.currentPlanWaypointIndex, self.roi.tape_seen, self.theta_weight) if self.roi.tape_at_bottom: should_advance = True else: self.send_controls(run_ctrl[0], run_ctrl[1]) print("We're using MPPI", self.currentPlanWaypointIndex, self.roi.tape_seen, self.theta_weight) ########################## FOR final project diff_x = self.goal[0] - curr_pose[0] diff_y = self.goal[1] - curr_pose[1] diff = (diff_x)**2 + (diff_y**2) diff = math.sqrt(diff) tol = 0.1 if consider_roi else 0.7 if (should_advance or diff < tol) and self.currentPlanWaypointIndex < len(plan) - 1: print("Reached: ", self.currentPlanWaypointIndex, " Curr Pose: ", curr_pose, " Goal pose: ", self.goal) self.advance_to_next_goal() print("Next: ", self.currentPlanWaypointIndex, " Curr Pose: ", curr_pose, " Goal pose: ", self.goal) # self.U.zero_() ##################### if poses is not None: self.visualize(poses) self.visualizePlan()
def main(): rospy.init_node('plan_creator', anonymous=True) # Initialize the node # launch nodes from python # https://answers.ros.org/question/263862/if-it-possible-to-launch-a-launch-file-from-python/ uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) map_server_launch = \ roslaunch.parent.ROSLaunchParent(uuid, [RACECAR_PKG_PATH + "/launch/includes/common/map_server.launch"]) map_server_launch.start() # launch nodes from python # https://answers.ros.org/question/263862/if-it-possible-to-launch-a-launch-file-from-python/ uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) planner_node_launch = roslaunch.parent.ROSLaunchParent( uuid, [PLANNER_PKG_PATH + "/launch/planner_node.launch"]) planner_node_launch.start() # start rviz by opening a new terminal in a new tab and running command "rviz" subprocess.call('gnome-terminal --tab --execute rviz', shell=True) individual_plan_parts = [] # [x, y, theta (given in deg and coverted to rad) path_points = [ [2500, 640, -11.3099 * np.pi / 180.0], # INITIAL Point # orange path [2600, 660, 20.0 * np.pi / 180.0], # REQUIRED Point 1 # yellow path [2615, 590, 125.0 * np.pi / 180.0], # [2410, 485, 160 * np.pi / 180.0], # end of yellow path # green path [1965, 400, -150 * np.pi / 180.0], # end of green path # blue path [1880, 440, -150 * np.pi / 180.0], # REQUIRED Point 2 # purple path [1660, 490, -150 * np.pi / 180.0], # end of purple path [1550, 600, -171 * np.pi / 180.0], # end of orange path # orange path [1520, 600, -175 * np.pi / 180.0], # end of yellow [1435, 545, 160 * np.pi / 180.0], # REQUIRED Point 3, end of green [1350, 440, 160 * np.pi / 180.0], # REQUIRED Point 4, end of cyan [1050, 450, -160.0 * np.pi / 180.0], # end of purple [650.0, 650.0, -120 * np.pi / 180.0], # end of violet [540, 835, -120 * np.pi / 180.0] # end of cyan ] # REQUIRED Point 5 (END) for i in range(0, len(path_points) - 1): rospy.sleep(15) # wait for planner_node to load or respawnn # raw_input("\n\nPRESS ENTER WHEN READY TO PLAN\n\n") initial_pose = path_points[i] goal_pose = path_points[i + 1] individual_plan_parts.append(get_plan(initial_pose, goal_pose, i)) if i != len(path_points) - 2: os.system('rosnode kill planner_node') # if planner_node doesn't autorespawn then use this: # rospy.sleep(1) # # launch nodes from python # # https://answers.ros.org/question/263862/if-it-possible-to-launch-a-launch-file-from-python/ # uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) # roslaunch.configure_logging(uuid) # planner_node_launch = roslaunch.parent.ROSLaunchParent(uuid, [PLANNER_PKG_PATH + # "/launch/planner_node.launch"]) # planner_node_launch.start() plan_array = [] # Read http://docs.ros.org/jade/api/geometry_msgs/html/msg/PoseArray.html PA = PoseArray() # create a PoseArray() msg PA.header.stamp = rospy.Time.now() # set header timestamp value PA.header.frame_id = "map" # set header frame id value PA.poses = [] for i, raw_plan in enumerate(individual_plan_parts): print "\nraw_plan ", i, " published of type: ", type(raw_plan) for pose in raw_plan.poses: plan_array.append( np.array([ pose.position.x, pose.position.y, utils.quaternion_to_angle(pose.orientation) ])) P = Pose() P.position.x = pose.position.x P.position.y = pose.position.y P.position.z = 0 P.orientation = pose.orientation PA.poses.append(P) PA_pub = rospy.Publisher(PLAN_POSE_ARRAY_TOPIC, PoseArray, queue_size=1) for i in range(0, 5): rospy.sleep(0.5) PA_pub.publish(PA) os.system('rosnode kill planner_node') # save plan_array to file # save plan PoseArray msg to file file_temp = open('/home/tim/car_ws/src/final/saved_plans/plan_12_9_2018', 'w') pickle.dump([plan_array, PA], file_temp) file_temp.close() print "\n\n DONE \n\n"
def __init__(self, T, K, sigma=[0.5, 0.1], _lambda=0.5, testing=False): self.MIN_VEL = -0.75 #TODO make sure these are right self.MAX_VEL = 0.75 self.MIN_DEL = -0.34 self.MAX_DEL = 0.34 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)) self.CAR_LENGTH = 0.33 self.XY_THRESHOLD = float(rospy.get_param("/threshold/goal/dist", 0.4)) self.THETA_THRESHOLD = float( rospy.get_param("/threshold/goal/theta", np.pi)) # config self.viz = True # visualize rollouts self.cont_ctrl = False # publish path continously self.testing = testing self.last_pose = None self.at_goal = True self.speed = 0 self.steering_angle = 0 self.prev_ctrl = None # MPPI params self.T = T # Length of rollout horizon self.K = K # Number of sample rollouts self._lambda = float(_lambda) self.goal_tensor = None self.lasttime = 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 model_name = rospy.get_param("~nn_model", "/media/JetsonSSD/model3.torch") self.model = torch.load(model_name) self.model.cuda() # tell torch to run the network on the GPU self.model.eval() # Model ideally runs faster in eval mode self.dtype = torch.cuda.FloatTensor print("Loading:", model_name) print("Model:\n", self.model) print("Torch Datatype:", self.dtype) # initialize these once self.ctrl = torch.zeros((T, 2)).cuda() self.sigma = torch.Tensor(sigma).type(self.dtype) self.inv_sigma = 1.0 / self.sigma #(2,) self.sigma = self.sigma.expand(self.T, self.K, 2) # (T,K,2) self.noise = torch.Tensor(self.T, self.K, 2).type(self.dtype) # (T,K,2) self.poses = torch.Tensor(self.K, self.T, 3).type(self.dtype) # (K,T,3) self.running_cost = torch.zeros(self.K).type(self.dtype) # (K,) self.pose_cost = torch.Tensor(self.K).type(self.dtype) #(K,) self.bad_ks = torch.Tensor(self.K).type(self.dtype) #(K,) self.recent_controls = np.zeros((3, 2)) self.control_i = 0 # control outputs self.msgid = 0 # visualization paramters self.num_viz_paths = 20 if self.K < self.num_viz_paths: self.num_viz_paths = self.K # 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.central_path_pub = rospy.Publisher("/mppi/path_center", Path, queue_size=1) # 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_info = map_msg.info # Save info about map for later use self.map_angle = -Utils.quaternion_to_angle( self.map_info.origin.orientation) self.map_c, self.map_s = np.cos(self.map_angle), np.sin(self.map_angle) print("Map Information:\n", self.map_info) # Create numpy array representing map for later use self.map_height = map_msg.info.height self.map_width = map_msg.info.width PERMISSIBLE_REGION_FILE = '/home/nvidia/catkin_ws/src/lab4/maps/permissible_region' if os.path.isfile(PERMISSIBLE_REGION_FILE + '.npy'): self.permissible_region = np.load(PERMISSIBLE_REGION_FILE + '.npy')[::-1, :] else: 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.logical_not( self.permissible_region) # 0 is permissible, 1 is not KERNEL_SIZE = 31 # 15 cm = 7 pixels = kernel size 15x15 kernel = np.ones((KERNEL_SIZE, KERNEL_SIZE)) kernel /= kernel.sum() self.permissible_region = signal.convolve2d( self.permissible_region, kernel, mode='same') > 0 # boolean 2d array np.save(PERMISSIBLE_REGION_FILE, self.permissible_region) self.permissible_region = torch.from_numpy( self.permissible_region.astype(np.int)).type( torch.cuda.ByteTensor) # since we are using it as a tensor self.pose_pub = rospy.Publisher("/mppi/pose", PoseStamped, queue_size=10) print("Making callbacks") self.goal_sub = rospy.Subscriber("/pp/path_goal", PoseStamped, self.clicked_goal_cb, queue_size=1) self.goal_sub = rospy.Subscriber("/pp/max_vel", Float64, self.max_vel_cb, queue_size=1) self.goal_sub_clicked = 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) self.joy_sub = rospy.Subscriber('/vesc/joy', Joy, self.joy_cb)
# q3 plot noise TODO #plt.scatter([ex], [ey], color=next(colors)) # q2 open loop plt.scatter([ex], [ey], color='r') #plt.annotate(str(i), (ex, ey)) if PRINT: print(ex, ey) if i == NUM_ITERS: break # get init pose init_msg = expected_msgs[0] init_x, init_y = (init_msg[1].pose.position.x, init_msg[1].pose.position.y) init_theta = utils.quaternion_to_angle(init_msg[1].pose.orientation) init_time = init_msg[2] # plot init pose xs = [init_x] * NUM_PARTICLES ys = [init_y] * NUM_PARTICLES plt.scatter(xs, ys, color='k', alpha=0.5) if PRINT: print 'OURS ' + MODEL print 't0' print xs print ys # populate all particles to have the initial pose particles = np.zeros((NUM_PARTICLES, 3)) particles[:, 0] = init_x particles[:, 1] = init_y
def odomCB(self, msg): self.odom_angle = quaternion_to_angle(msg.pose.pose.orientation)
def cb_heading(data): global heading heading = utils.quaternion_to_angle(data.pose.orientation)