def __init__(self, world_map,occupancy_map, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
def __init__(self, pos_init, pos_goal, max_speed, max_omega):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface()
time.sleep(0.2)
self.max_speed = max_speed
self.max_omega = max_omega
self.prev_lin_vel = 0.0
self.prev_ang_vel = 0.0
self.pos_init = pos_init
self.pos_goal = pos_goal
#self.joy = xbox.Joystick() #Initialize joystick
self.state = [pos_init[0], pos_init[1], 0.0]
#Kalman initialization
self.kalman_filter = KalmanFilter(dim_x=3, dim_z=3)
self.kalman_filter.F = np.array([[1, 0, 0], [0, 1, 0],
[0, 0, 1]]) # State Transition Matrix
self.kalman_filter.H = np.array([[1, 0, 0], [0, 1, 0],
[0, 0, 1]]) # Measurement function
self.kalman_filter.P = np.array([[0, 0, 0], [0, 0, 0],
[0, 0, 0]]) # covariance matrix
self.kalman_filter.R = np.array([[0.035, 0, 0], [0, 0.035, 0],
[0, 0, 0.001]]) # state uncertainty
self.kalman_filter.Q = np.array([[0.005, 0, 0], [0, 0.005, 0],
[0, 0, 0.008]]) # process uncertainty
self.kalman_filter.x = np.array([self.state]).T
class RobotControl(object):
"""
Class used to interface with the rover. Gets sensor measurements through ROS subscribers,
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
#self.kalman_filter = KalmanFilter(world_map)
#self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def process_measurements(self):
"""
YOUR CODE HERE
This function is called at 60Hz
"""
meas = self.ros_interface.get_measurements()
imu_meas = self.ros_interface.get_imu()
self.ros_interface.command_velocity(0, 0)
return
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
self.pos_goal = pos_goal
# YOUR CODE AFTER THIS
#-------------------------------------------#
self.time = rospy.get_time()
self.controlOut = (0.0, 0.0, False)
self.count_noMeasurement = 0
#-------------------------------------------#
self.markers = world_map
self.idx_target_marker = 0
# Calculate the optimal path
# From pos_init to pos_goal
self.path_2D = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init,
pos_goal)
self.idx_path = 0
self.size_path = self.path_2D.shape[0]
print "path.shape[0]", self.size_path
# Generate the 3D path (include "theta")
self.path = np.zeros((self.size_path, 3))
theta = 0.0
for idx in range(self.size_path - 1):
delta = self.path_2D[(idx + 1), :] - self.path_2D[idx, :]
theta = atan2(delta[1], delta[0])
if theta > np.pi:
theta -= np.pi * 2
elif theta < -np.pi:
theta += np.pi * 2
self.path[idx, :] = np.concatenate(
(self.path_2D[idx, :], np.array([theta])), axis=1)
self.path[self.size_path - 1, 0:2] = self.path_2D[self.size_path - 1,
0:2]
self.path[self.size_path - 1, 2] = pos_goal[2] # theta
#
self.path[0, 2] = pos_init[2] # theta
print "3D path:"
print self.path
# Uncomment as completed
# Kalman filter
self.kalman_filter = KalmanFilter(world_map)
self.kalman_filter.mu_est = pos_init # 3*pos_init # For test
# Differential drive
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
self.wayPointFollowing = wayPointFollowing(max_speed, max_omega)
#
self.task_done = False
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# TODO for student: Comment this when running on the robot
#self.robot_sim = RobotSim(world_map, occupancy_map, pos_init, pos_goal,
# max_speed, max_omega, x_spacing, y_spacing)
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# speed control variables
self.v = 0.1 # allows persistent cmds through detection misses
self.omega = -0.1 # allows persistent cmds through detection misses
self.last_detect_time = rospy.get_time() #TODO on bot only
self.missed_vision_debounce = 1
self.start_time = 0
# generate the path assuming we know our start location, goal, and environment
self.path = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init,
pos_goal)
self.path_idx = 0
self.mission_complete = False
self.carrot_distance = 0.22
# Uncomment as completed
self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
class RobotControl(object):
"""
Class used to interface with the rover. Gets sensor measurements through ROS subscribers,
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
self.kalman_filter = KalmanFilter(world_map)
print("INITSTATE", self.kalman_filter.state)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
self.vel = 0
self.omega = 0
self.curInd = 0
self.path = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init,
pos_goal)
print(self.path)
self.curGoal = self.path[0]
self.done = False
def process_measurements(self):
"""
This function is called at 60Hz
"""
meas = self.ros_interface.get_measurements()
print("Mesurements", meas)
imu_meas = self.ros_interface.get_imu()
print(imu_meas)
updatedPosition = self.kalman_filter.step_filter(
self.vel, self.omega, imu_meas, meas)
print(np.linalg.norm(self.curGoal - updatedPosition[0:1]))
if ((np.abs(self.curGoal[0] - updatedPosition[0]) > 0.1)
or (np.abs(self.curGoal[1] - updatedPosition[1]) > 0.1)):
(v, omega, done) = self.diff_drive_controller.compute_vel(
updatedPosition, self.curGoal)
self.vel = v
self.omega = omega
print("commanded vel:", v, omega)
self.ros_interface.command_velocity(v, omega)
else:
print("updating")
self.curInd = self.curInd + 1
if self.curInd < len(self.path):
self.curGoal = self.path[self.curInd]
else:
self.done = True
updatedPosition.shape = (3, 1)
return
def __init__(self, world_map,occupancy_map, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
#self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
self.markers = world_map
self.vel = np.array([0., 0.])
self.imu_meas = np.array([])
self.meas = []
def __init__(self, pos_init, pos_goal, max_speed, max_omega, x_spacing,
y_spacing, t_cam_to_body, min_lin_x, max_lin_x,
rotate_fast_value, linear_k, k, min_linear_vel):
"""
Initialize the class
"""
self.min_lin_x = min_lin_x
self.max_lin_x = max_lin_x
self.rotate_fast_value = rotate_fast_value
self.linear_k = linear_k
self.k = k
self.min_linear_vel = min_linear_vel
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
def __init__(self, world_map,occupancy_map, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
self.markers = world_map
self.vel = np.array([0, 0])
self.imu_meas = np.array([])
self.meas = []
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
#self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
self.goals = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init, pos_goal)
# self.total_goals = self.goals.shape[0]
self.cur_goal = 2
self.end_goal = self.goals.shape[0] - 1
self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
class RobotControl(object):
"""
Class used to interface with the rover. Gets sensor measurements through ROS subscribers,
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
self.markers = world_map
self.vel = np.array([0., 0.])
self.imu_meas = np.array([])
self.meas = []
# self.max_speed = max_speed
# self.max_omega = max_omega
# self.goals = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init, pos_goal)
# self.total_goals = self.goals.shape[0]
# self.cur_goal = 2
# self.end_goal = self.goals.shape[0] - 1
# Uncomment as completed
#self.kalman_filter = KalmanFilter(world_map)
#self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def process_measurements(self):
"""
YOUR CODE HERE
This function is called at 60Hz
"""
meas = self.ros_interface.get_measurements()
imu_meas = self.ros_interface.get_imu()
self.meas = meas
self.imu_meas = imu_meas
# for computing motor gain
v = 0.3
w = 0.
self.ros_interface.command_velocity(v, w)
return
class RobotControl(object):
"""
Robot interface class. In charge of getting sensor measurements (ROS subscribers),
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def process_measurements(self):
"""
Basic code to run autonomously
"""
# Check samples from https://github.com/markwsilliman/turtlebot/blob/master/goforward_and_avoid_obstacle.py
sonar_meas = self.ros_interface.get_range()
imu_meas = self.ros_interface.get_imu()
if (sonar_meas == None):
return
elif (sonar_meas <= 30):
pose = np.array([0, 0, math.pi / 4]) # emulate
goal = np.array([0, 0])
else:
pose = np.array([0, 0, 0])
goal = np.array([1, 0])
vel = self.diff_drive_controller.compute_vel(pose, goal)
self.vel = vel[0:2]
self.ros_interface.command_velocity(vel[0], vel[1])
rospy.loginfo_throttle(5, "robot_control velocity:" + str(self.vel) +
" sonar:" +
str(sonar_meas)) #+" imu:"+str(imu_meas))
#rospy.loginfo("robot_control velocity:"+ str(self.vel)+" sonar:"+str(sonar_meas))#+" imu:"+str(imu_meas))
return
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
self.kalman_filter = KalmanFilter(world_map)
print("INITSTATE", self.kalman_filter.state)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
self.vel = 0
self.omega = 0
self.curInd = 0
self.path = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init,
pos_goal)
print(self.path)
self.curGoal = self.path[0]
self.done = False
class RobotControl(object):
"""
Class used to interface with the rover. Gets sensor measurements through ROS subscribers,
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, world_map,occupancy_map, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
#self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def process_measurements(self):
"""
YOUR CODE HERE
This function is called at 60Hz
"""
meas = self.ros_interface.get_measurements()
imu_meas = self.ros_interface.get_imu()
#print imu_meas
#self.ros_interface.command_velocity(0., -8.)
if meas and meas != None:
state=np.array([meas[0][0],meas[0][1],meas[0][2]])
vw=self.diff_drive_controller.compute_vel(state,np.array([[0,0]]))
print vw
while vw[2]:
self.ros_interface.command_velocity(0,0)
self.ros_interface.command_velocity(vw[0],vw[1])
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
self.kalman_filter = KalmanFilter(world_map)
self.prev_v = 0
self.est_pose = np.array([[0], [0], [0]])
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
self.prev_imu_meas = np.array([[0], [0], [0], [0], [0]])
def __init__(self, markers, occupancy_map, pos_init, pos_goal, max_speed,
min_speed, max_omega, x_spacing, y_spacing, t_cam_to_body,
mode):
"""
Initialize the class
"""
# plan a path around obstacles using dijkstra's algorithm
print('Planning path...')
path = findShortestPath(occupancy_map,
x_spacing,
y_spacing,
pos_init[0:2],
pos_goal[0:2],
dilate=2)
print('Done!')
self.path_manager = PathManager(path)
self.kalman_filter = KalmanFilter(markers, pos_init)
self.diff_drive_controller = DiffDriveController(
max_speed, min_speed, max_omega)
if 'HARDWARE' in mode:
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
elif 'SIMULATE' in mode:
self.robot_sim = RobotSim(markers, occupancy_map, pos_init,
pos_goal, max_speed, max_omega,
x_spacing, y_spacing,
self.path_manager.path, mode)
self.user_control = UserControl()
self.vel = 0 # save velocity to use for kalman filter
self.goal = self.path_manager.getNextWaypoint() # get first waypoint
# for logging postion data to csv file
self.stateSaved = []
self.tagsSaved = []
self.waypoints = []
class AmclAuxLocalization(object):
"""
Class used to interface with the rover. Gets sensor measurements through ROS subscribers,
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, camera_frame_id, base_frame_id, odom_frame_id, map_frame_id, ns_tag_detector, tag_pose_id, pos_init):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(camera_frame_id, base_frame_id, odom_frame_id, map_frame_id, ns_tag_detector)
self.time = rospy.get_time()
# YOUR CODE AFTER THIS
#-------------------------------------------#
self.markers = tag_pose_id
self.idx_target_marker = 0
#-------------------------------------------#
# Kalman filter
self.kalman_filter = KalmanFilter(tag_pose_id)
self.kalman_filter.mu_est = pos_init # 3*pos_init # For test
def process_measurements(self):
"""
YOUR CODE HERE
This function is called at 10Hz
"""
# tag_measurement = self.ros_interface.get_measurements()
# tag_measurement = self.ros_interface.get_measurements_tf()
tag_measurement = self.ros_interface.get_measurements_tf_directMethod()
# amcl_pose = self.ros_interface.get_amcl_pose()
print '-----------------'
print 'tag:'
print tag_measurement
#----------------------------------------#
# self.time = rospy.get_time()
# print "self.time",self.time
# Kalman filter
# self.kalman_filter.step_filter(self.controlOut[0], (-1)*imu_meas, tag_measurement, rospy.get_time())
self.kalman_filter.step_filter_by_amcl(self.ros_interface, tag_measurement, rospy.get_time())
"""
print "After--"
print "mu_est",self.kalman_filter.mu_est
print "angle_est =", (self.kalman_filter.mu_est[2,0]*180.0/np.pi), "deg"
"""
#
return
def __init__(self, world_map,occupancy_map, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
self.markers = world_map
self.vel = np.array([0.,0.])
self.imu_meas = np.array([])
self.meas = []
self.max_speed = max_speed
self.max_omega = max_omega
# self.goals = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init, pos_goal)
# self.total_goals = self.goals.shape[0]
# self.cur_goal = 2
# self.end_goal = self.goals.shape[0] - 1
# Uncomment as completed
#self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
class RobotControl(object):
"""
Robot interface
"""
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
self.time_init = -1.0
def process_measurements(self):
"""
This function is called at 60Hz
"""
# set init time, and start motor
#if 0 == 1:
if self.time_init == -1.0:
print "set init time"
self.time_init = rospy.get_time()
self.time_measurement = rospy.get_time()
# stop motor when 1 second has passed
diff_time = (self.time_measurement - self.time_init)
#print("init: {}, measurement: {}, diff: {}".format(self.time_init, self.time_measurement, diff_time))
if diff_time >= 1:
print "stop speed"
self.ros_interface.command_velocity(0, 0)
else:
print "set speed"
self.ros_interface.command_velocity(0.3, 0)
return
def __init__(self, camera_frame_id, base_frame_id, odom_frame_id, map_frame_id, ns_tag_detector, tag_pose_id, pos_init):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(camera_frame_id, base_frame_id, odom_frame_id, map_frame_id, ns_tag_detector)
self.time = rospy.get_time()
# YOUR CODE AFTER THIS
#-------------------------------------------#
self.markers = tag_pose_id
self.idx_target_marker = 0
#-------------------------------------------#
# Kalman filter
self.kalman_filter = KalmanFilter(tag_pose_id)
self.kalman_filter.mu_est = pos_init # 3*pos_init # For test
class RobotControl(object):
"""
Class used to interface with the rover. Gets sensor measurements through ROS subscribers,
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
#self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def process_measurements(self):
"""
YOUR CODE HERE
This function is called at 60Hz
"""
meas = self.ros_interface.get_measurements()
imu_meas = self.ros_interface.get_imu()
#print(meas)
done = False
#print("time = " + str(self.robot_sim.last_meas_time))
if meas is not None and meas != []:
print "meas = " + str(meas[0][0:3])
#print type(meas)
state = -np.array(meas[0][0:3])
goal = np.array([0, 0])
print "state = " + str(state)
#print type(state)
#print goal
#print type(goal)
v, omega, done = self.diff_drive_controller.compute_vel(
state, goal)
#print done
#self.robot_sim.command_velocity(v, omega)
self.ros_interface.command_velocity(v, omega)
else:
#print "No measurement"
#self.robot_sim.command_velocity(0, 0)
v = 0
omega = 0
self.ros_interface.command_velocity(v, omega)
print "v = " + str(v)
print "omega = " + str(omega)
return
class RobotControl(object):
"""
Robot interface class. In charge of getting sensor measurements (ROS subscribers),
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
def publish_action(self, speed, turn):
self.ros_interface.command_velocity(speed, turn)
def apply_avoidance_rules(self, distance_from_center, angle):
p_angle = util.proportional_rotation(angle)
action = "None"
if self.inside_too_close_box(distance_from_center):
# dangeorous distance, too close to turn, just try to go back
action = "Back"
rospy.loginfo("{}:{:.2f} {:.2f} dist:{:.2f}".format(action, angle, p_angle, distance_from_center))
self.react_to_inside_too_close_box(p_angle)
elif util.is_forward_left(angle):
# obstacle to left, go right
action = "go forward right"
self.publish_action(default_speed, p_angle)
elif util.is_forward_right(angle):
# obstacle to right, go left
action = "go forward left"
self.publish_action(default_speed, p_angle)
elif util.is_backwards_left(angle):
action = "obstacle backwards left"
elif util.is_backwards_right(angle):
action = "obstacle backwards right"
else:
action = "unknown"
#rospy.loginfo("{}:{:.2f} {:.2f} dist:{:.2f}".format(action, angle, p_angle, distance_from_center))
def react_to_inside_too_close_box(self,angle):
# go back and turn 180 degrees
self.publish_action(-default_speed*2, 0)
rate = rospy.Rate(avoid_check_rate)
rate.sleep()
if util.is_forward_right(angle):
sign = -1
else:
sign = 1
z = math.pi*1.6*sign
rospy.loginfo('Back angle:'+str(angle)+' new angle:'+str(z))
self.publish_action(0, z) # around 5 rad/s
rate.sleep()
def inside_too_close_box(self, distance_from_center):
forward_box_length, forward_box_width = self.forward_box()
return distance_from_center < forward_box_width
def get_closest_obstacle(self, scan):
# returns closest point to obstacle inside a vehicle's 'forward box'
# forward box is a rectangle ahead of the vehicle that if there is an obstacle
# inside it it's necessary to do an action in order to avoid it
forward_box_length, forward_box_width = self.forward_box()
is_inside = False
min_distance = max_distance
min_i = 0
min_angle = 0
for i, distance in enumerate(scan.ranges):
angle = scan.angle_min + i * scan.angle_increment
if util.is_within_range(angle) and util.is_valid_distance(distance):
#print i, distance, to_degrees(angle), angle
if distance < min_distance:
if point_is_inside_box(i, distance, scan, forward_box_length, forward_box_width, util.to_degrees(angle)):
is_inside = True
min_distance = distance
min_i = i
min_angle = angle
#angle = angle_from_scan(min_i, scan)
#angle_degrees = angle*180/math.pi
#print min_i, angle_degrees, min_distance
return is_inside, min_distance, min_angle
def forward_box(self):
# returns the Vehicle 'forward box'
# to do: in future versions forward box should depend on vehicle's speed
# more speed -> larger forward box
length = Behaviour.body_width*look_ahead_factor
width = clearance_safety_factor*Behaviour.body_width/2
return length, width
def check_action():
pass
def process_measurements(self):
"""
Basic code to run autonomously
"""
scan = self.ros_interface.get_scan()
if(scan == None):
return
action = "None"
angular_z = 0
linear_x = 0.3
is_inside, distance_from_center, angle = self.get_closest_obstacle(scan)
# only do something if obstacle is inside forward box
if is_inside:
#print 'point inside ', distance_from_center, angle
self.apply_avoidance_rules(distance_from_center, angle)
else:
self.publish_action(linear_x, angular_z)
return
class RobotControl(object):
"""
Class used to interface with the rover. Gets sensor measurements through ROS subscribers,
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# TODO for student: Comment this when running on the robot
#self.robot_sim = RobotSim(world_map, occupancy_map, pos_init, pos_goal,
# max_speed, max_omega, x_spacing, y_spacing)
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# speed control variables
self.v = 0.1 # allows persistent cmds through detection misses
self.omega = -0.1 # allows persistent cmds through detection misses
self.last_detect_time = rospy.get_time() #TODO on bot only
self.missed_vision_debounce = 1
self.start_time = 0
# generate the path assuming we know our start location, goal, and environment
self.path = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init,
pos_goal)
self.path_idx = 0
self.mission_complete = False
self.carrot_distance = 0.22
# Uncomment as completed
self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def process_measurements(self):
"""
YOUR CODE HERE
Main loop of the robot - where all measurements, control, and esimtaiton
are done. This function is called at 60Hz
"""
print(' ')
# TODO for student: Comment this when running on the robot
#meas = self.robot_sim.get_measurements()
#imu_meas = self.robot_sim.get_imu()
# TODO for student: Use this when transferring code to robot
meas = self.ros_interface.get_measurements()
imu_meas = self.ros_interface.get_imu()
# meas is the position of the robot with respect to the AprilTags
# print(meas)
# now that we have the measurements, update the predicted state
self.kalman_filter.step_filter(self.v, imu_meas, meas)
# print(self.kalman_filter.x_t)
# TODO remove on bot, shows predicted state on simulator
#self.robot_sim.set_est_state(self.kalman_filter.x_t)
# pull the next path point from the list
cur_goal = self.getCarrot()
# cur_goal = self.path[self.path_idx]
# TODO test to just go to a goal
# cur_goal[0] = 0.43
# cur_goal[1] = 2
# calculate the control commands need to reach next path point
#print('')
#print('current goal:')
#print(cur_goal)
#print('current state:')
#print(self.kalman_filter.x_t)
control_cmd = self.diff_drive_controller.compute_vel(
self.kalman_filter.x_t, cur_goal)
self.v = control_cmd[0]
self.omega = control_cmd[1]
#print('control command:')
# print(control_cmd)
if self.mission_complete:
self.v = 0
self.omega = 0
#print(control_cmd)
if control_cmd[2]:
if len(self.path) > (self.path_idx + 1):
self.path_idx = self.path_idx + 1
print('next goal')
else:
self.mission_complete = True
#TODO calibration test on bot only for linear velocity
'''
if self.start_time - 0 < 0.0001:
self.start_time = rospy.get_time()
if(rospy.get_time() - self.start_time > 4):
self.v = 0
self.omega = 0
else:
self.v = 0.15
self.omega = 0
'''
#TODO on bot only
self.ros_interface.command_velocity(self.v, self.omega)
#TODO for simulation
#self.robot_sim.command_velocity(self.v,self.omega)
return
def getCarrot(self):
'''
getCarrot - generates an artificial goal location along a path out
infront of the robot.
path - the set of points which make up the waypoints in the path
position - the current position of the robot
'''
path = self.path
idx = self.path_idx
pos = self.kalman_filter.x_t
# if the current line segment ends in the goal point, set that to the goal
if self.path_idx + 1 == len(self.path):
return self.path[(len(self.path) - 1)]
else:
# find the point on the current line closest to the robot
# calculate current line's slope and intercept
pt1 = self.path[self.path_idx]
pt2 = self.path[self.path_idx + 1]
x_diff = pt2[0] - pt1[0]
y_diff = pt2[1] - pt1[1]
vert = abs(x_diff) < 0.001
# using the current line's slope and intercept find the point on that
# line closest to the robots current point
# assumes all lines are either veritcal or horizontal
x_bot = pos[0][0]
y_bot = pos[1][0]
if vert:
x_norm = pt2[0]
y_norm = y_bot
else:
x_norm = x_bot
y_norm = pt2[1]
# if the normal point is past the end point of this segment inc path idx
# assumes all lines are either vertical or horizontal
inc = False
if vert:
if (y_diff > 0 and y_norm > pt2[1]) or (y_diff < 0
and y_norm < pt2[1]):
inc = True
else:
if (x_diff > 0 and x_norm > pt2[0]) or (x_diff < 0
and x_norm < pt2[0]):
inc = True
if (inc):
self.path_idx = self.path_idx + 1
print('increment path index')
# find a point L distance infront of the normal point on this line
# assumes all lines are either vertical or horizontal
if vert:
x_goal = pt2[0]
if y_diff > 0:
y_goal = y_bot + self.carrot_distance
else:
y_goal = y_bot - self.carrot_distance
else:
y_goal = pt2[1]
if x_diff > 0:
x_goal = x_bot + self.carrot_distance
else:
x_goal = x_bot - self.carrot_distance
goal = np.array([x_goal, y_goal])
#print(goal)
return goal
class RobotControl(object):
"""
Class used to interface with the rover. Gets sensor measurements through ROS subscribers,
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
self.kalman_filter = KalmanFilter(world_map)
self.prev_v = 0
self.est_pose = np.array([[0], [0], [0]])
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
self.prev_imu_meas = np.array([[0], [0], [0], [0], [0]])
def process_measurements(self, waypoint):
"""
waypoint is a 1D list [x,y] containing the next waypoint the rover needs to go
YOUR CODE HERE
Main loop of the robot - where all measurements, control, and esimtaiton
are done. This function is called at 60Hz
"""
#This gives the xy location and the orientation of the tag in the rover frame
#The orientation is zero when the rover faces directly at the tag
meas = self.ros_interface.get_measurements()
self.est_pose = self.kalman_filter.step_filter(self.prev_v,
self.prev_imu_meas,
meas)
state = [self.est_pose[0, 0], self.est_pose[1, 0], self.est_pose[2, 0]]
goal = [waypoint[0], waypoint[1]]
controls = self.diff_drive_controller.compute_vel(state, goal)
self.ros_interface.command_velocity(controls[0], controls[1])
self.prev_v = controls[0]
imu_meas = self.ros_interface.get_imu()
if imu_meas != None:
imu_meas[3, 0] = -imu_meas[
3, 0] #clockwise angular vel is positive from IMU
self.prev_imu_meas = imu_meas
return controls[2]
class RobotControl(object):
"""
Class used to interface with the rover. Gets sensor measurements through ROS subscribers,
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, world_map,occupancy_map, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
self.markers = world_map
self.vel = np.array([0.,0.])
self.imu_meas = np.array([])
self.meas = []
self.max_speed = max_speed
self.max_omega = max_omega
# self.goals = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init, pos_goal)
# self.total_goals = self.goals.shape[0]
# self.cur_goal = 2
# self.end_goal = self.goals.shape[0] - 1
# Uncomment as completed
#self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def process_measurements(self):
"""
YOUR CODE HERE
This function is called at 60Hz
"""
meas = self.ros_interface.get_measurements()
imu_meas = self.ros_interface.get_imu()
self.meas = meas
self.imu_meas = imu_meas
# unit test for following april tag
if (meas != None and meas):
cur_meas = meas[0]
# print(cur_meas)
tag_robot_pose = cur_meas[:3]
# print(self.tag_pos(cur_meas[3]))
tag_world_pose = np.float32(self.tag_pos(cur_meas[3]))
# print(tag_world_pose)
# print(tag_robot_pose)
state = self.robot_pos(tag_world_pose,tag_robot_pose)
goal = tag_world_pose
v,w,done = self.diff_drive_controller.compute_vel(state,goal)
if done:
v = 0.01
w = 0.
vel = (v,w)
self.vel = vel[:2]
self.done = done
else:
v = 0.
w = 0.
self.ros_interface.command_velocity(v,w)
return
def tag_pos(self, marker_id):
# print(marker_id)
# print(self.markers)
for i in range(len(self.markers)):
marker_i = np.copy(self.markers[i])
if int(float(marker_i[3])) == marker_id:
return marker_i[:3]
return None
def robot_pos(self, w_pos, r_pos):
H_W = np.array([[np.cos(w_pos[2]), -np.sin(w_pos[2]), w_pos[0]],
[np.sin(w_pos[2]), np.cos(w_pos[2]), w_pos[1]],
[0., 0., 1.]])
H_R = np.array([[np.cos(r_pos[2]), -np.sin(r_pos[2]), r_pos[0]],
[np.sin(r_pos[2]), np.cos(r_pos[2]), r_pos[1]],
[0., 0., 1.]])
# print(np.linalg.inv(H_R))
w_r = np.dot(H_W, np.linalg.inv(H_R))
return np.array([[w_r[0,2]],[w_r[1,2]],[np.arctan2(w_r[1,0],w_r[0,0])]])
class RobotControl(object):
"""
Robot interface class. In charge of getting sensor measurements (ROS subscribers),
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, pos_init, pos_goal, max_speed, max_omega, x_spacing,
y_spacing, t_cam_to_body, min_lin_x, max_lin_x,
rotate_fast_value, linear_k, k, min_linear_vel):
"""
Initialize the class
"""
self.min_lin_x = min_lin_x
self.max_lin_x = max_lin_x
self.rotate_fast_value = rotate_fast_value
self.linear_k = linear_k
self.k = k
self.min_linear_vel = min_linear_vel
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
def to_degrees(self, rad):
angle_degrees = rad * 180 / math.pi
return angle_degrees
def is_valid_distance(self, d):
"""
Discard Inf distances
"""
if (np.isnan(d) or np.isinf(d)):
return False
else:
return True
def is_within_range(self, i):
"""
Only consider -90 to 90 degrees
"""
#return -math.pi/4 < i < math.pi/4
return math.pi / 2 < abs(i) # LIDAR rotated backwards
def process_measurements(self):
"""
Basic code to run autonomously
"""
# Check samples from https://github.com/markwsilliman/turtlebot/blob/master/goforward_and_avoid_obstacle.py
scan = self.ros_interface.get_scan()
linear = 0
rotational = 0
rcount = 0
hack = False
msg = ""
if (scan == None):
return
rcount = 0
for i, real_dist in enumerate(scan.ranges):
angle = scan.angle_min + i * scan.angle_increment
denom = 1. + real_dist * real_dist
if self.is_within_range(angle) and self.is_valid_distance(
real_dist):
linear += math.sin(angle) / denom * 1.
rotational += math.cos(angle) / denom * 1.
rcount += 1
#print "%.5f"%self.to_degrees(angle)+"degrees x:%.5f"%real_dist+" x:%.2f"%linear+" z:%.2f"%rotational
try:
linear /= rcount
rotational /= rcount
except Exception as e:
pass
"""
if (linear > self.k):
linear = self.k
elif (linear < -self.k):
linear = -self.k
"""
linear_x = linear
angular_z = rotational
linear_x = linear # check this
print "linear_x:%.5f" % linear_x + " angular_z:%.1f" % self.to_degrees(
angular_z) + "(%.2f)" % angular_z + " " + msg
#self.ros_interface.command_velocity(linear_x, angular_z)
return
class RobotControl(object):
"""Class used to interface with the rover. Gets sensor measurements
through ROS subscribers, and transforms them into the 2D plane,
and publishes velocity commands.
"""
def __init__(self, markers, occupancy_map, pos_init, pos_goal, max_speed,
min_speed, max_omega, x_spacing, y_spacing, t_cam_to_body,
mode):
"""
Initialize the class
"""
# plan a path around obstacles using dijkstra's algorithm
print('Planning path...')
path = findShortestPath(occupancy_map,
x_spacing,
y_spacing,
pos_init[0:2],
pos_goal[0:2],
dilate=2)
print('Done!')
self.path_manager = PathManager(path)
self.kalman_filter = KalmanFilter(markers, pos_init)
self.diff_drive_controller = DiffDriveController(
max_speed, min_speed, max_omega)
if 'HARDWARE' in mode:
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
elif 'SIMULATE' in mode:
self.robot_sim = RobotSim(markers, occupancy_map, pos_init,
pos_goal, max_speed, max_omega,
x_spacing, y_spacing,
self.path_manager.path, mode)
self.user_control = UserControl()
self.vel = 0 # save velocity to use for kalman filter
self.goal = self.path_manager.getNextWaypoint() # get first waypoint
# for logging postion data to csv file
self.stateSaved = []
self.tagsSaved = []
self.waypoints = []
def process_measurements(self):
"""Main loop of the robot - where all measurements, control, and
estimation are done.
"""
v, omega, wayptReached = self.user_control.compute_vel()
self.robot_sim.command_velocity(v, omega)
return
#meas = None # for testing purposes
#imu_meas = None # for testing purpose
#pdb.set_trace()
if (imu_meas is None) and (meas is None):
pass
else:
state = self.kalman_filter.step_filter(self.vel, imu_meas, meas)
if 'SIMULATE' in mode:
self.robot_sim.set_est_state(state)
#print("X = {} cm, Y = {} cm, Theta = {} deg".format(100*state[0],100*state[1],state[2]*180/np.pi))
# save the estimated state and tag statuses for offline animation
self.stateSaved.append(state)
self.waypoints.append(self.path_manager.getActiveWaypointsPos())
if meas is None:
self.tagsSaved.append(None)
else:
meas = np.array(meas)
tagIDs = [int(i) for i in meas[:, 3]]
self.tagsSaved.append(tagIDs)
v, omega, wayptReached = self.diff_drive_controller.compute_vel(
state, self.goal.pos)
self.vel = v
if wayptReached:
self.goal = self.path_manager.getNextWaypoint()
self.diff_drive_controller.done = False # reset diff controller status
if self.goal is None:
print('Goal has been reached!')
np.savez('savedState.npz',
stateSaved=self.stateSaved,
tagsSaved=self.tagsSaved,
waypoints=self.waypoints)
print('Position data saved!')
if 'HARDWARE' in mode:
self.ros_interface.command_velocity(0, 0)
elif 'SIMULATE' in mode:
self.robot_sim.command_velocity(0, 0)
self.robot_sim.done = True
return
else:
if 'HARDWARE' in mode:
self.ros_interface.command_velocity(self.vel, omega)
elif 'SIMULATE' in mode:
self.robot_sim.command_velocity(self.vel, omega)
return
def myhook():
print "shutdown time!"
class RobotControl(object):
"""
Class used to interface with the rover. Gets sensor measurements through ROS subscribers,
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, world_map,occupancy_map, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
self.markers = world_map
self.vel = np.array([0, 0])
self.imu_meas = np.array([])
self.meas = []
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
#self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
self.goals = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init, pos_goal)
# self.total_goals = self.goals.shape[0]
self.cur_goal = 2
self.end_goal = self.goals.shape[0] - 1
self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def process_measurements(self):
"""
YOUR CODE HERE
This function is called at 60Hz
"""
meas = self.ros_interface.get_measurements()
self.meas = meas;
print 'tag output'
print meas
# imu_meas: measurment comig from the imu
imu_meas = self.ros_interface.get_imu()
self.imu_meas = imu_meas
print 'imu measurement'
print imu_meas
pose = self.kalman_filter.step_filter(self.vel, self.imu_meas, np.asarray(self.meas))
# Code to follow AprilTags
'''
if(meas != None and meas):
cur_meas = meas[0]
tag_robot_pose = cur_meas[0:3]
tag_world_pose = self.tag_pos(cur_meas[3])
state = self.robot_pos(tag_world_pose, tag_robot_pose)
goal = tag_world_pose
vel = self.diff_drive_controller.compute_vel(state, goal)
self.vel = vel[0:2];
print vel
if(not vel[2]):
self.ros_interface.command_velocity(vel[0], vel[1])
else:
vel = (0.01, 0.1)
self.vel = vel
self.ros_interface.command_velocity(vel[0], vel[1])
'''
# Code to move autonomously
goal = self.goals[self.cur_goal]
print 'pose'
print pose
print 'goal'
print goal
vel = self.diff_drive_controller.compute_vel(pose, goal)
self.vel = vel[0:2];
print 'speed'
print vel
if(not vel[2]):
self.ros_interface.command_velocity(vel[0], vel[1])
else:
vel = (0, 0)
if self.cur_goal < self.end_goal:
self.cur_goal = self.cur_goal + 1
self.ros_interface.command_velocity(vel[0], vel[1])
self.vel = vel
return
def tag_pos(self, marker_id):
for i in range(len(self.markers)):
marker_i = np.copy(self.markers[i])
if marker_i[3] == marker_id:
return marker_i[0:3]
return None
def robot_pos(self, w_pos, r_pos):
H_W = np.array([[math.cos(w_pos[2]), -math.sin(w_pos[2]), w_pos[0]],
[math.sin(w_pos[2]), math.cos(w_pos[2]), w_pos[1]],
[0, 0, 1]])
H_R = np.array([[math.cos(r_pos[2]), -math.sin(r_pos[2]), r_pos[0]],
[math.sin(r_pos[2]), math.cos(r_pos[2]), r_pos[1]],
[0, 0, 1]])
w_r = H_W.dot(inv(H_R))
robot_pose = np.array([[w_r[0,2]], [w_r[1,2]], [math.atan2(w_r[1,0], w_r[0, 0])]])
return robot_pose
class RobotControl(object):
"""
Class used to interface with the rover. Gets sensor measurements through ROS subscribers,
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
self.pos_goal = pos_goal
# YOUR CODE AFTER THIS
#-------------------------------------------#
self.time = rospy.get_time()
self.controlOut = (0.0, 0.0, False)
self.count_noMeasurement = 0
#-------------------------------------------#
self.markers = world_map
self.idx_target_marker = 0
# Calculate the optimal path
# From pos_init to pos_goal
self.path_2D = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init,
pos_goal)
self.idx_path = 0
self.size_path = self.path_2D.shape[0]
print "path.shape[0]", self.size_path
# Generate the 3D path (include "theta")
self.path = np.zeros((self.size_path, 3))
theta = 0.0
for idx in range(self.size_path - 1):
delta = self.path_2D[(idx + 1), :] - self.path_2D[idx, :]
theta = atan2(delta[1], delta[0])
if theta > np.pi:
theta -= np.pi * 2
elif theta < -np.pi:
theta += np.pi * 2
self.path[idx, :] = np.concatenate(
(self.path_2D[idx, :], np.array([theta])), axis=1)
self.path[self.size_path - 1, 0:2] = self.path_2D[self.size_path - 1,
0:2]
self.path[self.size_path - 1, 2] = pos_goal[2] # theta
#
self.path[0, 2] = pos_init[2] # theta
print "3D path:"
print self.path
# Uncomment as completed
# Kalman filter
self.kalman_filter = KalmanFilter(world_map)
self.kalman_filter.mu_est = pos_init # 3*pos_init # For test
# Differential drive
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
self.wayPointFollowing = wayPointFollowing(max_speed, max_omega)
#
self.task_done = False
def process_measurements(self):
"""
YOUR CODE HERE
This function is called at 60Hz
"""
meas = self.ros_interface.get_measurements()
imu_meas = self.ros_interface.get_imu()
# print 'meas',meas
print 'imu_meas', imu_meas
"""
# Control the robot to track the tag
if (meas is None) or (meas == []):
# stop
# self.ros_interface.command_velocity(0.0,0.0)
#
if self.count_noMeasurement > 30:
# Actually the ros_interface will stop the motor itself if we don't keep sending new commands
self.ros_interface.command_velocity(0.0,0.0)
else:
# Keep the old command
self.ros_interface.command_velocity(self.controlOut[0],self.controlOut[1])
self.count_noMeasurement += 1
else:
print 'meas',meas
self.count_noMeasurement = 0
# Thew differential drive controller that lead the robot to the tag
self.controlOut = self.diff_drive_controller.compute_vel((-1)*np.array(meas[0][0:3]),np.array([0.0,0.0,0.0]))
self.ros_interface.command_velocity(self.controlOut[0],self.controlOut[1])
(v,omega,done) = (self.controlOut[0], self.controlOut[1],False )
"""
"""
if (rospy.get_time() - self.time) < 1.0:
self.ros_interface.command_velocity(0.0, 3.0)
# Linear velocity = 0.3 m/s, Angular velocity = 0.5 rad/s
else:
self.ros_interface.command_velocity(0.0,0.0)
"""
"""
(v,omega,done) = (0.0, 0.0, False)
self.ros_interface.command_velocity(v, omega)
"""
"""
# Directly move to the goal position
if self.controlOut[2]: # done
self.ros_interface.command_velocity(0.0, 0.0)
else:
self.controlOut = self.wayPointFollowing.compute_vel(self.kalman_filter.mu_est, self.pos_goal )
# self.controlOut = self.diff_drive_controller.compute_vel(self.kalman_filter.mu_est, self.pos_goal )
if self.controlOut[2]: # done
self.ros_interface.command_velocity(0.0, 0.0)
else:
self.ros_interface.command_velocity(self.controlOut[0], self.controlOut[1])
"""
"""
#----------------------------------------#
# Switch the targets (way-point of the optimal path) and do the position control
# if self.controlOut[2] and self.idx_path == self.size_path-1: # all done
if self.idx_path == self.size_path-1 and self.controlOut[0] < 0.05 and self.controlOut[1] < 0.1 : # all done
self.ros_interface.command_velocity(0.0, 0.0)
else:
self.controlOut = self.diff_drive_controller.compute_vel(self.kalman_filter.mu_est, self.path[self.idx_path,:] )
self.ros_interface.command_velocity(self.controlOut[0], self.controlOut[1])
if self.controlOut[2] and self.idx_path < self.size_path-1: # way-point done
self.idx_path += 1
#----------------------------------------#
"""
#----------------------------------------#
print "self.idx_path", self.idx_path
# Switch the targets (way-point of the optimal path) and do the position control
# way-point following (no reducing speeed when reach a way-point)
# if self.controlOut[2] and self.idx_path == self.size_path-1: # all done
if self.task_done: # all done
self.ros_interface.command_velocity(0.0, 0.0)
elif self.idx_path == self.size_path - 1: # The last one
# Change the threshold
self.diff_drive_controller.threshold = 0.02 # 3 cm
# Using diff_drive_controller to reach the goal position with right direction
self.controlOut = self.diff_drive_controller.compute_vel(
self.kalman_filter.mu_est, self.path[self.idx_path, :])
# if self.controlOut[0] < 0.05 and self.controlOut[1] < 0.1 : # all done
if self.controlOut[2]: # all done
self.ros_interface.command_velocity(0.0, 0.0)
self.task_done = True # all done
else:
self.ros_interface.command_velocity(self.controlOut[0],
self.controlOut[1])
else:
# The way-points, using wayPointFollowing to trace the trajectory without pausing
if self.idx_path == self.size_path - 2: # The last 2nd one
self.controlOut = self.diff_drive_controller.compute_vel(
self.kalman_filter.mu_est, self.path[self.idx_path, :])
else:
self.controlOut = self.wayPointFollowing.compute_vel(
self.kalman_filter.mu_est, self.path[self.idx_path, :])
# self.controlOut = self.wayPointFollowing.compute_vel(self.kalman_filter.mu_est, self.path[self.idx_path,:] )
self.ros_interface.command_velocity(self.controlOut[0],
self.controlOut[1])
if self.controlOut[
2] and self.idx_path < self.size_path - 1: # way-point done
self.idx_path += 1
#----------------------------------------#
# self.time = rospy.get_time()
# print "self.time",self.time
# Kalman filter
self.kalman_filter.step_filter(self.controlOut[0], (-1) * imu_meas,
meas, rospy.get_time())
print "mu_est", self.kalman_filter.mu_est
return
class RobotControl(object):
"""
Class used to interface with the rover. Gets sensor measurements through ROS subscribers,
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
#self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def process_measurements(self):
"""
YOUR CODE HERE
This function is called at 60Hz
"""
meas = self.ros_interface.get_measurements(
) #type list (matriz de 1x3 meas[0][2], posicion del tag [[x, y, z, id]])
imu_meas = self.ros_interface.get_imu()
if meas != None:
# convert meas into a np array
measu = meas[0]
# IMPORTANTE USAR ***MEASU*** Y NO MEAS!!!
#print measurements, python 2 btw...
print("*******")
print("Tag: "),
print(measu[3])
measu3f = np.round(measu, 3)
print("Measurements: "),
print(measu3f[:3])
#print state
state = np.array([0.0, 0.0, 0.0])
print("State: "),
print(state)
goal = np.array([measu[0], -measu[1], measu[2]])
vw = self.diff_drive_controller.compute_vel(state, goal)
#print comanded velocity
vw3f = np.round(vw, 3)
print("Computed command vel: "),
print(vw3f[:2])
if vw[2] == False:
self.ros_interface.command_velocity(vw[0], vw[1])
if vw[2] == True:
print("*****The robot arrived its destination*****")
else:
#self.ros_interface.command_velocity(0, 0)
return
class RobotControl(object):
"""
Robot interface class. In charge of getting sensor measurements (ROS subscribers),
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
def to_degrees(self, rad):
angle_degrees = rad * 180 / math.pi
return angle_degrees
def is_valid_distance(self, d):
"""
Discard Inf distances
"""
if (np.isnan(d) or np.isinf(d)):
return False
else:
return True
def is_within_range(self, i):
"""
Only consider -90 to 90 degrees
"""
#return -math.pi/4 < i < math.pi/4
return math.pi / 2 < abs(i) # LIDAR rotated backwards
def process_measurements(self):
"""
Basic code to run autonomously
"""
# Check samples from https://github.com/Arkapravo/laser_obstacle_avoidance_pr2/blob/master/src/obstacle_avoidance_pr2.cpp
scan = self.ros_interface.get_scan()
angular_z = 0
linear_x = 0
k = 0.4 # 0.3
min_linear = 0.09
rotate_fast_value = 1. # 5.8 # rad/s
linear_k = 0.15
go_forward = 0.5 # default value
action = "None"
if (scan == None):
return
#print "i:%.5f"%angle+"(%.5f"%self.to_degrees(angle)+"degrees) x:%.5f"%real_dist+" x:%.2f"%linear+" z:%.2f"%rotational
min_range = scan.ranges[0]
min_range_angle = 0
min_angle = 0
for j in range(0, 360): #increment by one degree
real_dist = scan.ranges[j]
angle = scan.angle_min + j * scan.angle_increment
if self.is_within_range(angle) and self.is_valid_distance(
real_dist):
if (real_dist < min_range):
min_range = real_dist
min_angle = angle
min_range_angle = j / 2
#print "minimum range is [%.3f]"%min_range+" at an angle of [%.3f]"%min_range_angle
if (
min_range <= 0.5
): # min_range<=0.5 gave box pushing like behaviour, min_range<=1.2 gave obstacle avoidance
if (min_range_angle < 90):
angular_z = rotate_fast_value
linear_x = 0
action = "left "
else:
angular_z = -rotate_fast_value
linear_x = 0
action = "right"
else:
angular_z = 0
linear_x = go_forward
action = "straight"
print "action:" + action + " min distance[%.2f]" % min_range + " angle [%.2f]" % min_range_angle + " (%.1f)" % angle + " min angle %.1f" % min_angle
self.ros_interface.command_velocity(linear_x, angular_z)
return
class RobotControl(object):
"""
Class used to interface with the rover. Gets sensor measurements through ROS subscribers,
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, pos_init, pos_goal, max_speed, max_omega):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface()
time.sleep(0.2)
self.max_speed = max_speed
self.max_omega = max_omega
self.prev_lin_vel = 0.0
self.prev_ang_vel = 0.0
self.pos_init = pos_init
self.pos_goal = pos_goal
#self.joy = xbox.Joystick() #Initialize joystick
self.state = [pos_init[0], pos_init[1], 0.0]
#Kalman initialization
self.kalman_filter = KalmanFilter(dim_x=3, dim_z=3)
self.kalman_filter.F = np.array([[1, 0, 0], [0, 1, 0],
[0, 0, 1]]) # State Transition Matrix
self.kalman_filter.H = np.array([[1, 0, 0], [0, 1, 0],
[0, 0, 1]]) # Measurement function
self.kalman_filter.P = np.array([[0, 0, 0], [0, 0, 0],
[0, 0, 0]]) # covariance matrix
self.kalman_filter.R = np.array([[0.035, 0, 0], [0, 0.035, 0],
[0, 0, 0.001]]) # state uncertainty
self.kalman_filter.Q = np.array([[0.005, 0, 0], [0, 0.005, 0],
[0, 0, 0.008]]) # process uncertainty
self.kalman_filter.x = np.array([self.state]).T
#Uncomment as completed
#self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def KalmanUpdate(self):
self.kalman_filter.predict() #For the P matrix prediction
self.kalman_filter.x = np.array([self.state]).T
self.kalman_filter.update(np.array(
[MeasState[2:5]]).T) #MeasState[2:5] are the sensor data
updated_state = [
self.kalman_filter.x[0][0], self.kalman_filter.x[1][0],
self.kalman_filter.x[2][0]
]
return updated_state
def control(self):
global ang_vel_rep_left, ang_vel_rep_right, period, baseAngle, baseMagValue, baseAccValue, MeasState
global loop_ctr, AccList
distances = self.ros_interface.get_distances()
imu_meas = self.ros_interface.get_imu()
#imu measurements
MagValue = imu_meas[3:6]
AccValue = [
imu_meas[0] - baseAccValue[0], imu_meas[1] - baseAccValue[1]
]
#Skip acceleration noise
if abs(AccValue[0]) < 0.15:
AccValue[0] = 0.0
if abs(AccValue[1]) < 0.15:
AccValue[1] = 0.0
Angle = -(math.atan2(MagValue[1], MagValue[0]) - baseAngle)
#print([MagValue[0], MagValue[1]])
#State estimation from measurements
MeasState[0] = (self.prev_lin_vel + AccValue[0] * period) * math.cos(
self.state[2]) #x #Velocity
MeasState[1] = (self.prev_lin_vel + AccValue[0] * period) * math.sin(
self.state[2]) #y
MeasState[2] = self.state[0] + MeasState[
0] * period #Position mhpws 8elei self.state anti gia measstate?
MeasState[3] = self.state[1] + MeasState[1] * period
MeasState[4] = Angle
#Predict next state
self.state[0] = self.state[0] + self.prev_lin_vel * period * math.cos(
self.state[2]) # x
self.state[1] = self.state[1] + self.prev_lin_vel * period * math.sin(
self.state[2]) # y
self.state[2] = self.state[2] + self.prev_ang_vel * period # theta
#self.state[2] = Angle
#Updated state, sensor fusion
self.state = self.KalmanUpdate()
#self.state[2] = (self.KalmanUpdate())[2]
#Angle between -pi and pi
if self.state[2] > math.pi:
self.state[2] -= 2 * math.pi
elif self.state[2] < -math.pi:
self.state[2] += 2 * math.pi
print(self.state[0], self.state[1], self.state[2] * 180 / math.pi)
#Repultion fields from sensors
ang_vel_rep_left = max(0.0,
(self.max_omega / 2 - 1.0 * distances[0]) * 3.0)
ang_vel_rep_right = -max(
0.0, (self.max_omega / 2 - 1.0 * distances[2]) * 3.0)
angle_from_goal = math.atan2(self.pos_goal[1] - self.state[1],
self.pos_goal[0] - self.state[0])
#Attraction field
if abs(angle_from_goal - self.state[2]) < 0.1:
ang_vel_attr = 0
else:
ang_vel_attr = 2.3 * (angle_from_goal - self.state[2])
ang_vel_attr = np.sign(ang_vel_attr) * min(
0.7, abs(ang_vel_attr)) #angle_vel between -0.7 and 0.7
#print(ang_vel_attr)
lin_vel = min(self.max_speed, 5 * (distances[1] - 0.16))
#Linear velocity not below 0.1 m/s
if lin_vel > 0:
lin_vel = max(lin_vel, 0.1)
else:
lin_vel = min(lin_vel, -0.1)
lin_vel = max(lin_vel, -self.max_speed)
ang_vel = ang_vel_attr + ang_vel_rep_right + ang_vel_rep_left
self.ros_interface.command_velocity(lin_vel, ang_vel)
self.prev_lin_vel = lin_vel
self.prev_ang_vel = ang_vel
return
def PerformSquare(self):
global baseAngle, MeasState, square_angles, square_index, timer
imu_meas = self.ros_interface.get_imu()
#imu measurements
MagValue = imu_meas[3:5]
Angle = -(math.atan2(MagValue[1], MagValue[0]) - baseAngle)
self.state[2] = Angle
if self.state[2] > math.pi: #angle between -pi and pi
self.state[2] -= 2 * math.pi
elif self.state[2] < -math.pi:
self.state[2] += 2 * math.pi
if (square_index == 2) and (self.state[2] < 0):
self.state[2] += 2 * math.pi
print(self.state[2] * 180 / math.pi)
if timer > 3:
timer = -1
square_index += 1
angle_from_goal = square_angles[square_index]
#Attraction field
if abs(angle_from_goal - self.state[2]) < 0.06:
ang_vel_attr = 0
else:
ang_vel_attr = 2.5 * (angle_from_goal - self.state[2])
ang_vel_attr = np.sign(ang_vel_attr) * min(
1.5, abs(ang_vel_attr)) #angle_vel between -1.2 and 1.2
self.ros_interface.command_velocity(0.2, ang_vel_attr)
return
