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)
