def __init__(self, gsl, sil, vil):
self.LOG.info('init')
self._sil = sil
self.tc = ThrottleControl(gsl)
self.sc = SteeringControl(gsl)
self.tlc = TopLevelControl(gsl, sil, self.tc, self.sc)
self.pp = PathPlanner(gsl, sil, vil, self.tc, self.sc)
def main():
global thetas
configureLeftHand()
planner = PathPlanner("right_arm")
grip = Gripper('right')
grip.calibrate()
raw_input("gripper calibrated")
grip.open()
table_size = np.array([3, 1, 10])
table_pose = PoseStamped()
table_pose.header.frame_id = "base"
table_pose.pose.position.x = .9
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = -5 - .112
#put cup
cup_pos = start_pos_xy
end_pos = goal_pos_xy
putCupObstacle(planner, cup_pos, "cup1")
putCupObstacle(planner, end_pos, "cup2")
planner.add_box_obstacle(table_size, "table", table_pose)
#move gripper to behind cup
position = cup_pos + np.array([-0.1, 0, -0.02])
orientation = np.array([0, np.cos(np.pi / 4), 0, np.cos(np.pi / 4)])
executePositions(planner, [position], [orientation])
raw_input("moved behind cup, press enter")
#move to cup and remove cup1 as obstacle since picking up
removeCup(planner, "cup1")
position = cup_pos + np.array([0, 0, -0.05])
executePositions(planner, [position], [orientation])
raw_input("moved to cup, press to close")
grip.close()
rospy.sleep(1)
raw_input("gripped")
moveAndPour(planner, end_pos)
raw_input("poured")
executePositions(planner, [position + np.array([0, 0, 0.02])],
[orientation])
grip.open()
removeCup(planner, "cup2")
def on_start(self, goal, frame):
self.goal = goal
filtered_image = self.obstacleDetector.applyFilter(frame)
planner = PathPlanner()
self.path, expanded, found_goal = planner.planPath(
filtered_image, self.marbleStateManager.get_position(), goal)
print("Got a path of length: " + str(len(self.path)))
def __init__(self):
self.path_planner = PathPlanner()
# will need a map:
map_maker = MapMaker()
map_maker.set_map()
map_maker.calibrate_map()
lowres_map = map_maker.get_lowres_map()
self.path_planner.set_map(lowres_map)
def __init__(self):
if not self.load_parameters(): sys.exit(1)
self._planner = PathPlanner('{}_arm'.format(self._arm))
rospy.on_shutdown(self.shutdown)
if not self.register_callbacks(): sys.exit(1)
def setUp(self):
self.path_planner = PathPlanner()
# will need a map:
map_maker = MapMaker()
map_maker.set_map()
map_maker.calibrate_map()
lowres_map = map_maker.get_lowres_map()
self.path_planner.set_map(lowres_map)
self.path_planner.set_start_node()
self.path_planner.set_end_node()
def mover_setup(self):
limb = Limb("right")
# Right arm planner
self.planner = PathPlanner("right_arm")
# Left arm planner
self.planner_left = PathPlanner("left_arm")
place_holder = {'images': [], 'camera_infos': []}
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
count = 0
print(limb.endpoint_pose())
calibration_points = []
if ROBOT == "sawyer":
Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3])
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
else:
Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4])
Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
# Controller for right arm
self.c = Controller(Kp, Ki, Kd, Kw, Limb('right'))
self.orien_const = OrientationConstraint()
self.orien_const.link_name = "right_gripper";
self.orien_const.header.frame_id = "base";
self.orien_const.orientation.y = -1.0;
self.orien_const.absolute_x_axis_tolerance = 0.1;
self.orien_const.absolute_y_axis_tolerance = 0.1;
self.orien_const.absolute_z_axis_tolerance = 0.1;
self.orien_const.weight = 1.0;
box = PoseStamped()
box.header.frame_id = "base"
box.pose.position.x = self.box.pose.position.x
box.pose.position.y = self.box.pose.position.y
# box.pose.position.z = self.box.pose.position.z
box.pose.position.z = self.conveyor_z
# box.pose.position.x = 0.5
# box.pose.position.y = 0.0
# box.pose.position.z = 0.0
box.pose.orientation.x = 0.0
box.pose.orientation.y = 0.0
box.pose.orientation.z = 0.0
box.pose.orientation.w = 1.0
self.planner.add_box_obstacle((100, 100, 0.00001), "box", box)
# Controller for left arm
self.c_left = Controller(Kp, Ki, Kd, Kw, Limb('left'))
def __init__(self, sub_topic, pub_topic):
self.planner = None
self.limb = None
self.executed = False
self.planner = PathPlanner("right_arm")
self.limb = Limb("right")
# self.orien_const = OrientationConstraint()
# self.orien_const.link_name = "right_gripper"
# self.orien_const.header.frame_id = "base"
# self.orien_const.orientation.z = 0
# self.orien_const.orientation.y = 1
# self.orien_const.orientation.w = 0
# self.orien_const.absolute_x_axis_tolerance = 0.1
# self.orien_const.absolute_y_axis_tolerance = 0.1
# self.orien_const.absolute_z_axis_tolerance = 0.1
# self.orien_const.weight = 1.0
size = [1, 2, 2]
obstacle_pose = PoseStamped()
obstacle_pose.header.frame_id = "base"
obstacle_pose.pose.position.x = -1
obstacle_pose.pose.position.y = 0
obstacle_pose.pose.position.z = 0
obstacle_pose.pose.orientation.x = 0
obstacle_pose.pose.orientation.y = 0
obstacle_pose.pose.orientation.z = 0
obstacle_pose.pose.orientation.w = 1
self.planner.add_box_obstacle(size, "wall", obstacle_pose)
size = [.75, 1, 1]
obstacle_pose = PoseStamped()
obstacle_pose.header.frame_id = "base"
obstacle_pose.pose.position.x = 1
obstacle_pose.pose.position.y = 0
obstacle_pose.pose.position.z = -.4
obstacle_pose.pose.orientation.x = 0
obstacle_pose.pose.orientation.y = 0
obstacle_pose.pose.orientation.z = 0
obstacle_pose.pose.orientation.w = 1
self.planner.add_box_obstacle(size, "table", obstacle_pose)
self.pub = rospy.Publisher(pub_topic, Bool)
self.sub = rospy.Subscriber(sub_topic, PoseStamped, self.callback)
def __init__(self, map_name='campus'):
self.map = MapLoader.load(path='resources/campus.json')
self.pt = PositionTracker(osmap=self.map)
self.pf = PositionFetcher(self.pt)
self.planner = PathPlanner(self.pt, on_wp_reached=self.remove_waypoint)
self.vis = Visualizer(map_name=map_name,
pt=self.pt,
add_wp=self.add_waypoint,
osmap=self.map)
self.waypoints_lock = Lock()
self.waypoints_changed = True
self.waypoints: Optional[List[Waypoint]] = None
self.set_waypoints([])
def __init__(self, dt, target_speed, vehicle, param_dict):
super(RandomBehaviorPlanner, self).__init__(vehicle, dt, param_dict)
self.dt = dt
self.target_speed = target_speed
self.path_planner = PathPlanner(self.vehicle, {'dt': dt,
'target_speed': target_speed})
self.p_l = param_dict['p_l']
self.p_r = param_dict['p_r']
self.switcher_step = 0 # cycles through 0, 1, ..., (Tds - 1) each timestep
self.chg_hazard_l = False # there is a hazard on the left
self.hazard_c = False # there is a hazard ahead
self.chg_hazard_r = False # there is a hazard on the right
def execute_rendezvous(
self, master_coords, drone_coords
): # calculates rendezvous path and sets nav goal for robots at the provided poses
if self.executing_rendezvous or not self.map_initialized:
return False
# init and run the path planner
path_planner = PathPlanner(self.occupancy_grid, self.map_width,
self.map_height, self.map_resolution,
self.map_origin)
path_planner.set_start(master_coords)
path_planner.set_goal(drone_coords)
rendezvous_path = path_planner.a_star()
# abort if no path found
if len(rendezvous_path) == 0:
return False
# parse the result and start nav to goal for both robots
grid_goal = rendezvous_path[int(math.floor(len(rendezvous_path) / 2))]
world_goal = path_planner.grid_to_world(grid_goal)
print 'executing rendezvous at ' + str(world_goal)
self.master_node.start_navigation(
world_goal[0], world_goal[1], 0,
(master_coords[0], master_coords[1], 0))
self.drone_node.start_navigation(world_goal[0], world_goal[1], 0,
(drone_coords[0], drone_coords[1], 0))
self.executing_rendezvous = True
return True
def configureLeftHand():
planner = PathPlanner("left_arm")
position = np.array([0.516, 0.485, 0.362])
orientation = [0.647, 0.655, -0.281, 0.271]
executePositions(planner, [position], [orientation])
def calc_cspace(self, mapdata):
"""
Calculates the C-Space, i.e., makes the obstacles in the map thicker.
:return [OccupancyGrid] The C-Space.
"""
rospy.loginfo("Calculating C-Space")
# Go through each cell in the occupancy grid
# Inflate the obstacles where necessary
padding = 2
newmap = deepcopy(mapdata)
# for each cell in mapdata
# if (cell is walkable) and (cell has an obstacle within padding)
# add padding in newmap at current cell
# input (x,y,pad)
# sx = min(max(0, x-pad), width)
# sy = min(max(0, y-pad), height)
# ex = max(min(width, x+pad), 0)
# ey = max(min(height, y+pad), 0)
# for each xx in range(sx,ex)
# for each yy in range(sy,ey)
# if cell(xx,yx) has obstacle
# return True
# return False
new_data = []
for item in mapdata.data:
new_data.append(item)
for h in range(mapdata.info.height - 1):
for w in range(mapdata.info.width - 1):
neighbors = ExpandMap.neighbors_of_8(mapdata, w, h)
if len(neighbors) < 8:
new_data[ExpandMap.grid_to_index(mapdata, w, h)] = 100
#Create a GridCells message and publish it
expanded_cells = GridCells()
expanded_cells.header.frame_id = "map"
expanded_cells.cell_width = mapdata.info.resolution
expanded_cells.cell_height = mapdata.info.resolution
expanded_cells.cells = []
for h in range(mapdata.info.height):
for w in range(mapdata.info.width):
index = ExpandMap.grid_to_index(mapdata, w, h)
if new_data[index] == 100:
msg_Point = Point()
world_point = PathPlanner.grid_to_world(mapdata, w, h)
msg_Point.x = world_point.x
msg_Point.y = world_point.y
msg_Point.z = 0
expanded_cells.cells.append(msg_Point)
self.expanded_cells.publish(expanded_cells)
## Return the C-space
#print("END")
#print(mapdata)
newmap.data = new_data
return newmap
def __init__(self):
rospy.init_node('waypoint_updater')
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
# TODO: Add a subscriber for /traffic_waypoint and /obstacle_waypoint below
self.final_waypoints_pub = rospy.Publisher('final_waypoints', Lane, queue_size=1)
# TODO: Add other member variables you need below
self.current_pose = None
self.base_waypoints = None
self.planner = PathPlanner(LOOKAHEAD_WPS)
def main():
get_top_left_AR_tag()
# print("Here are the marker rotations: " + str(marker_rotations))
# print("Here are the marker translations: " + str(marker_translations))
global top_left_x
global top_left_y
global top_left_z
assert len(marker_translations) == 1
R = marker_rotations[0]
t = marker_translations[0]
top_left_x, top_left_y, top_left_z = t
sawyer = Robot(PathPlanner("right_arm"), None, "right_gripper", 0.095)
# Adding environment obstacles for the robot to avoid during path planning
# add_block_obstacle(sawyer, position=[0.67452, -0.4455, 0.916557], name='right_wall', size=[5, 0, 5], orient=[1, 0, 0, 0])
add_block_obstacle(sawyer,
position=[0.460625, 0.5444, 0.916557],
name='left_wall',
size=[5, 0, 5],
orient=[1, 0, 0, 0])
# add_block_obstacle(sawyer, position=[, , TABLE_HEIGHT - sawyer.gripper_length], name='table', size=[3.5, 3.5, 0], orient=[0, 1, 0, 0])
# add_block_obstacle(planner, [0,0,0], 'ceiling', [10, 5, 1])
add_block_obstacle(
sawyer,
position=[0.81, -0.26, -0.1 + (0.5 * BLOCK_TOTAL_HEIGHT)],
name='baseplate',
size=[
5 * BLOCK_SIDE_LENGTH, 6 * BLOCK_SIDE_LENGTH,
0.5 * BLOCK_TOTAL_HEIGHT
])
add_block_obstacle(sawyer,
position=[0.81, -0.26, -0.1 - (BLOCK_TOTAL_HEIGHT / 2)],
name='table',
size=[5, 5, BLOCK_TOTAL_HEIGHT / 2])
#TODO: Finish adding as many environment constraints as possible without compromising on path planning.
dest_x, dest_y = block_coord_to_sawyer((4, 5))
print('Top left:', top_left_x, top_left_y)
start_x3 = top_left_x - 0.08 - (BLOCK_SIDE_LENGTH / 2.0)
start_y3 = top_left_y + 0.0335
print(start_x3, start_y3)
start_x3, start_y3 = start_num_to_sawyer(3)
print(start_x3, start_y3)
print()
for i in range(8):
print(start_num_to_sawyer(i))
while 1 < 2:
pick_and_place_block(sawyer, start_x3, start_y3, -0.0320774, dest_x,
dest_y, top_left_z + (0.5 * BLOCK_SQUARE_HEIGHT),
'block_test')
def grasp(key,
x,
y,
z,
x_threshhold=0.02,
y_threshhold=0.02,
hoverDist=0.020,
zoffset=.92,
orien_const=[],
or_x=0.0,
or_y=-1.0,
or_z=0.0,
or_w=0.0):
#while not rospy.is_shutdown():
try:
left_arm_planner = PathPlanner("left_arm")
right_arm_planner = PathPlanner("right_arm")
goal = PoseStamped()
goal.header.frame_id = "base"
#x, y, and z position
goal.pose.position.x = x + x_threshhold
goal.pose.position.y = y + y_threshhold
goal.pose.position.z = z - zoffset + hoverDist
#Orientation as a quaternion
goal.pose.orientation.x = or_x
goal.pose.orientation.y = or_y
goal.pose.orientation.z = or_z
goal.pose.orientation.w = or_w
if key == 'left_arm':
plan = left_arm_planner.plan_to_pose(goal, orien_const)
openLeftGripper()
left_arm_planner.execute_plan(plan)
closeLeftGripper()
else:
plan = right_arm_planner.plan_to_pose(goal, orien_const)
openRightGripper()
right_arm_planner.execute_plan(plan)
closeRightGripper()
except Exception as e:
print e
traceback.print_exc()
def play_chords(note_pos):
planner = PathPlanner("left_arm")
new_note_pos = []
num_waypoints_per_note = 4
# add waypoints above the note to ensure the note is struck properly
for note in note_pos:
waypoint1 = Vector3()
waypoint1.x = note.x
waypoint1.y = note.y
waypoint1.z = note.z + waypoint_offset
waypoint2 = Vector3()
waypoint2.x = note.x
waypoint2.y = note.y
waypoint2.z = note.z + waypoint_offset / 2.0
new_note_pos += [waypoint1, waypoint2, note, waypoint1]
for i, pos in enumerate(new_note_pos):
print(i, pos)
# Loop until that position is reached
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = pos.x
goal_1.pose.position.y = pos.y
goal_1.pose.position.z = pos.z
# #Orientation as a quaternion, facing straight down
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
plan = planner.plan_to_pose(goal_1, list())
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def main():
# rospy.spin()
left = baxter_interface.Limb('left')
lj = left.joint_names()
left_gripper = robot_gripper.Gripper('left')
right = baxter_interface.Limb('right')
rj = right.joint_names()
right_gripper = robot_gripper.Gripper('right')
print(right_gripper.force())
left.set_joint_position_speed(0.8)
right.set_joint_position_speed(0.8)
print("Initializing planner")
planner_right = PathPlanner("right_arm")
planner_left = PathPlanner("left_arm")
print("Calling function")
# move_at_angle(left, right, lj, rj, planner_left, planner_right, 0.4)
return_to_neutral(left, right, lj, rj, planner_left, planner_right)
def __init__(self):
print("Starting init")
self.left_arm = baxter_interface.Limb('left')
print("Left arm obtrained")
self.lj = self.left_arm.joint_names()
self.left_gripper = robot_gripper.Gripper('left')
self.left_joint_neutral = self.left_arm.joint_angles()
self.right_arm = baxter_interface.Limb('right')
self.rj = self.right_arm.joint_names()
self.right_gripper = robot_gripper.Gripper('right')
self.right_joint_neutral = self.right_arm.joint_angles()
self.planner_left = PathPlanner('left_arm')
self.planner_right = PathPlanner('right_arm')
self.orien_const_left_vert = OrientationConstraint()
self.orien_const_left_vert.link_name = "left_gripper"
self.orien_const_left_vert.header.frame_id = "base"
self.orien_const_left_vert.orientation.y = -1.0
self.orien_const_left_vert.absolute_x_axis_tolerance = 0.1
self.orien_const_left_vert.absolute_y_axis_tolerance = 0.1
self.orien_const_left_vert.absolute_z_axis_tolerance = 0.1
self.orien_const_left_vert.weight = 1.0
self.orien_const_right_vert = copy.deepcopy(self.orien_const_left_vert)
self.orien_const_right_vert.link_name = "right_gripper"
self.previous_deviation = (None, None)
self.setting_to_neutral = False
self.tilt_after_neutral = True
self.pub = rospy.Publisher("/board_controller_log",
std_msgs.msg.String,
queue_size=10)
rospy.Subscriber("/physics_inference", std_msgs.msg.String,
self.perform_tilt)
rospy.Subscriber("/control/neutral_set", std_msgs.msg.Bool,
self.neutral_listener)
rospy.Subscriber("control/tilt_set", std_msgs.msg.Bool,
self.tilt_listener)
def __init__(self):
rospy.init_node('waypoint_updater')
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
# TODO: Add a subscriber for /traffic_waypoint and /obstacle_waypoint below
rospy.Subscriber('/traffic_waypoint', Int32, self.traffic_cb)
rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_lights_cb)
self.final_waypoints_pub = rospy.Publisher('final_waypoints', Lane, queue_size=1)
self.speed_limit = rospy.get_param('/waypoint_loader/velocity') * 1000 / 3600.
# TODO: Add other member variables you need below
self.light_wp = None
self.lights = None
self.current_pose = None
self.base_waypoints = None
self.planner = PathPlanner(LOOKAHEAD_WPS)
self.planner.set_speed_limit(self.speed_limit)
def __init__(self, dt, target_speed, vehicle, param_dict, CAV_agents_dict):
super(CAVBehaviorPlanner, self).__init__(vehicle, dt, param_dict)
self.dt = dt
self.target_speed = target_speed
self.path_planner = PathPlanner(self.vehicle, {'dt': dt,
'target_speed': target_speed})
self.F = param_dict['F']
self.w = param_dict['w']
self.theta_CL = param_dict['theta_CL']
self.theta_CR = param_dict['theta_CR']
self.eps = param_dict['eps']
self.chg_hazard_l = False
self.hazard_c = False
self.chg_hazard_r = False
self.switcher_step = 0 # cycles through 0, 1, ..., (Tds - 1) each timestep
self.Qv_l = 0.9 * self.target_speed
self.Qv_c = 0.9 * self.target_speed
self.Qv_r = 0.9 * self.target_speed
self.change_buf = Queue(maxsize=self.F)
while not self.change_buf.full():
self.change_buf.put(False)
self.Qf = 0
self.rCL = 0
self.rCR = 0
self.neighbor_left = []
self.neighbor_current = []
self.neighbor_right = []
self.behavior = Behavior.KEEP_LANE
self.closeneighbor_left = []
self.closeneighbor_current = []
self.closeneighbor_right = []
self.CAV_agents_dict = CAV_agents_dict
class PlanningAndControlLayer():
LOG = logging.getLogger('PlanningAndControlLayer')
def __init__(self, gsl, sil, vil):
self.LOG.info('init')
self._sil = sil
self.tc = ThrottleControl(gsl)
self.sc = SteeringControl(gsl)
self.tlc = TopLevelControl(gsl, sil, self.tc, self.sc)
self.pp = PathPlanner(gsl, sil, vil, self.tc, self.sc)
def on(self):
self.LOG.info('on')
self.tc.on()
self.sc.on()
self.tlc.on()
self.pp.on()
def tick(self):
if (self._sil.gps()._rc_enable == 1):
self.tlc.tick()
else:
self.pp.tick()
def generate_paths(self, cost_map):
problem_valid = False
# Choose a goal position
num_alternatives = len(self.possible_goals)
goal_position = self.possible_goals[random.randint(0, num_alternatives-1)]
while not problem_valid:
# Trying to generate a new problem
start_position = (random.randint(0, self.height - 1), random.randint(0, self.width - 1))
# If the start happen to be within an obstacle, we discard them and
# try new samples
if cost_map.is_occupied(start_position[0], start_position[1]):
continue
if start_position == goal_position:
continue
try:
path_planner = PathPlanner(cost_map)
dijkstra_path, cost = path_planner.dijkstra(start_position, goal_position)
greedy_path, cost = path_planner.greedy(start_position, goal_position)
a_star_path, cost = path_planner.a_star(start_position, goal_position)
problem_valid = True
except AttributeError:
# In case there is no valid path
continue
# print(start_position, goal_position)
# plt.matshow(cost_map.grid)
# plt.plot(start_position[1], start_position[0], 'g*', markersize=8)
# plt.plot(goal_position[1], goal_position[0], 'rx', markersize=8)
# title = str(start_position) + ", " + str(goal_position)
# plt.title(title)
# plt.show()
return [dijkstra_path, greedy_path, a_star_path]
class PathPlannerTest(unittest.TestCase):
def setUp(self):
self.path_planner = PathPlanner()
# will need a map:
map_maker = MapMaker()
map_maker.set_map()
map_maker.calibrate_map()
lowres_map = map_maker.get_lowres_map()
self.path_planner.set_map(lowres_map)
self.path_planner.set_start_node()
self.path_planner.set_end_node()
'''
Function:
Tests:
Calls:
Notes:
'''
def testCalculate_angular_movement(self):
# Test different movements for different start and end angles in pi/4 incerements
# if you start at 0 and have increasing targets, these are the correct movements. You can shift back one each time you increase the starting angle
correct_movements = [
0, -pi / 4, -pi / 2, -pi * (3 / 4), -pi, pi * (3 / 4), pi / 2,
pi / 4
]
for i in range(0, 8):
# Test targets from 0 to 2pi in pi/4 increments
target = pi * i / 4
print("TARGET PI * {}/4".format(i))
for j in range(0, 8):
# Test starting place from 0 to 2pi for each target
current = pi * (j / 4)
movement = self.path_planner.calculate_angular_movement(
current, target)
self.assertEqual(correct_movements[j - i], movement)
def testPos_from_coordinates(self):
coord_zero = [0, 0]
predicted_pos = self.path_planner.pos_from_coordinates(coord_zero)
pos_zero = [0, 0]
self.assertEqual(predicted_pos, pos_zero)
def calc_cspace(self, mapdata, padding):
"""
Calculates the C-Space, i.e., makes the obstacles in the map thicker.
Publishes the list of cells that were added to the original map.
:param mapdata [OccupancyGrid] The map data.
:param padding [int] The number of cells around the obstacles.
:return [[int8]] The C-Space as a list of values from 0 to 100.
"""
rospy.loginfo("Calculating C-Space")
## Goes through each cell in the occupancy grid
## Inflates the obstacles where necessary based on padding argument
new_map = []
total_range = mapdata.info.width * mapdata.info.height
for p in range(0,padding):
copy = list(mapdata.data)
for i in range(0, total_range):
if mapdata.data[i] == 100:
#coordinates are found via indexing through occupancy grid.
#Index to grid is used to convert the index to useable grid coordinates
coordinates = PathPlanner.index_to_grid(mapdata, i)
pos_x = coordinates[0]
pos_y = coordinates[1]
neighbors = PathPlanner.neighbors_of_8(mapdata, pos_x, pos_y)
#iterates through neighbors list and changes all available surrounding neighbors to obstacles
for n in neighbors:
index_value = PathPlanner.grid_to_index(mapdata, n[0], n[1])
copy[index_value] = 100
world_pos = PathPlanner.grid_to_world(mapdata, n[0], n[1])
cell_point = Point()
cell_point.x = world_pos[0]
cell_point.y = world_pos[1]
cell_point.z = 0
new_map.append(cell_point)
mapdata.data = copy
return mapdata.data
def __init__(self, topic='cmd_vel_mux/input/navi', scan='scan', pos='slam_out_pose', obstacle_detect=False):
rospy.init_node('turtlebot_move', anonymous=True)
self.pub = rospy.Publisher(topic, Twist, queue_size=1)
self.subPos = rospy.Subscriber(pos, PoseStamped, self._callbackPos)
self.obstacleDetector = ObstacleDetector()
self.twist = Twist()
self.dest = [1024,1024]
self.path = PathPlanner()
self.pos = None
self._max_speed = 0.5 # m/s
self._max_omega = 0.5 # rad/s
self._range = (-1, 1)
class RoboSys:
def __init__(self):
#self.listener = KinectSubscriber()
self.map_maker = MapMaker()
#self.map_maker.set_map()
#self.map_data = self.map_maker.get_lowres_map()
self.path_planner = PathPlanner()
#self.marker_detector = MarkerDetector()
#self.rgb_data = []
def signal_handler(self, signal, frame):
global run
run = False
print("Shutting down")
cv2.destroyAllWindows()
def run(self):
#rospy.init_node('KinectSubscriber', anonymous=True)
#signal.signal(signal.SIGINT, self.signal_handler)
#while(run):
#self.map_data = self.listener.get_map_data()
#self.rgb_data = self.listener.get_rgb_data()
#self.marker_detector.detect_markers(self.rgb_data)
# cv2.imshow("RGB Image Window", detected)
# cv2.waitKey(3)
self.map_maker.set_map(
) # setting to default map in map_maker, can pass other map live
lowres_map = self.map_maker.get_lowres_map()
self.path_planner.set_map(lowres_map)
self.path_planner.set_start_node() # setting to default
self.path_planner.set_end_node() # setting to default
self.path_planner.run()
class PlanPath:
def __init__(self, marbleStateManager):
self.marbleStateManager = marbleStateManager
self.obstacle_detector = SimpleBlobDetector.createObstacleDetector()
self.planner = PathPlanner()
def tick(self):
filtered_image = self.obstacle_detector.applyFilter(
WhiteBoard.RAW_FRAME)
WhiteBoard.PATH, expanded, found_goal = self.planner.planPath(
filtered_image, self.marbleStateManager.get_position(), GOAL)
print("Got a path of length: " + str(len(WhiteBoard.PATH)) +
", found goal: " + str(found_goal))
mark_path(WhiteBoard.PATH, WhiteBoard.FRAME_TO_SHOW)
show_and_wait(WhiteBoard.FRAME_TO_SHOW)
if (found_goal):
return SUCCESS
else:
return FAILURE
:return:
"""
rotation_angle = robot_pose.heading
robot_position = robot_pose.position
goal_point_position_in_global_frame = self._find_position_on_path(
self.goal_point)
rotation_matrix = np.array([[
np.cos(rotation_angle), -np.sin(rotation_angle), robot_position[0]
], [np.sin(rotation_angle),
np.cos(rotation_angle), robot_position[1]], [0, 0, 1]])
coordinate_in_robot_frame = np.dot(
np.linalg.inv(rotation_matrix),
np.append(goal_point_position_in_global_frame, 1.0))
return coordinate_in_robot_frame[:2]
if __name__ == '__main__':
waypoint_list = [[0, 0], [1, 1], [2, 2], [3, 3]]
waypoints, goal = PathPlanner.create_waypoints(waypoint_list)
controller = PurePursuit(waypoints, 10, 5, 5)
print(('Test find position based on s coordinate, position is {}'.format(
controller._find_position_on_path(np.math.sqrt(3)))))
print(
('Test binary search insertion place function: insertion place is {}'.
format(Util.find_insert_place(list(range(10)), 10))))
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
plannerR = PathPlanner("right_arm")
plannerL = PathPlanner("left_arm")
right_gripper = robot_gripper.Gripper('right')
left_gripper = robot_gripper.Gripper('left')
# Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
# Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students
# Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
# Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
# right_arm = Limb("right")
# controller = Controller(Kp, Kd, Ki, Kw, right_arm)
##
## Add the obstacle to the planning scene here
##
obstacle1 = PoseStamped()
obstacle1.header.frame_id = "base"
#x, y, and z position
obstacle1.pose.position.x = 0.4
obstacle1.pose.position.y = 0
obstacle1.pose.position.z = -0.29
#Orientation as a quaternion
obstacle1.pose.orientation.x = 0.0
obstacle1.pose.orientation.y = 0.0
obstacle1.pose.orientation.z = 0.0
obstacle1.pose.orientation.w = 1
plannerR.add_box_obstacle(np.array([1.2, 1.2, .005]), "tableBox",
obstacle1)
# obstacle2 = PoseStamped()
# obstacle2.header.frame_id = "wall"
# #x, y, and z position
# obstacle2.pose.position.x = 0.466
# obstacle2.pose.position.y = -0.670
# obstacle2.pose.position.z = -0.005
# #Orientation as a quaternion
# obstacle2.pose.orientation.x = 0.694
# obstacle2.pose.orientation.y = -0.669
# obstacle2.pose.orientation.z = 0.251
# obstacle2.pose.orientation.w = -0.092
# planner.add_box_obstacle(np.array([0.01, 2, 2]), "wall", obstacle2)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
# orien_const = OrientationConstraint()
# orien_const.link_name = "right_gripper";
# orien_const.header.frame_id = "base";
# orien_const.orientation.y = -1.0;
# orien_const.absolute_x_axis_tolerance = 0.1;
# orien_const.absolute_y_axis_tolerance = 0.1;
# orien_const.absolute_z_axis_tolerance = 0.1;
# orien_const.weight = 1.0;
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.440
goal_3.pose.position.y = -0.012
goal_3.pose.position.z = 0.549
#Orientation as a quaternion
goal_3.pose.orientation.x = -0.314
goal_3.pose.orientation.y = -0.389
goal_3.pose.orientation.z = 0.432
goal_3.pose.orientation.w = 0.749
#plan = plannerR.plan_to_pose(goal_3, list())
raw_input(
"Press <Enter> to move the right arm to goal pose 3: ")
if not plannerR.execute_plan(
plannerR.plan_to_pose(goal_3, list())):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.448
goal_2.pose.position.y = -0.047
goal_2.pose.position.z = -0.245
#Orientation as a quaternion
goal_2.pose.orientation.x = 0
goal_2.pose.orientation.y = 1
goal_2.pose.orientation.z = 0
goal_2.pose.orientation.w = 0
#plan = plannerR.plan_to_pose(goal_2, list())
raw_input(
"Press <Enter> to move the right arm to goal pose 2: ")
if not plannerR.execute_plan(
plannerR.plan_to_pose(goal_2, list())):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.448
goal_1.pose.position.y = -0.047
goal_1.pose.position.z = -0.245
#Orientation as a quaternion
goal_1.pose.orientation.x = 1
goal_1.pose.orientation.y = 0
goal_1.pose.orientation.z = 0
goal_1.pose.orientation.w = 0
# Might have to edit this . . .
#plan = plannerR.plan_to_pose(goal_1, list()) # put orien_const here
raw_input(
"Press <Enter> to move the right arm to goal pose 1: ")
if not plannerR.execute_plan(
plannerR.plan_to_pose(goal_1, list())):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
# K values for Part 5
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3
]) # Borrowed from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Borrowed from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
# Initialize the controller for Part 5
# controller = Controller( . . . )
#-----------------------------------------------------#
## Add any obstacles to the planning scene here
#-----------------------------------------------------#
size = np.array([0.4, 1.2, 0.1])
pose = PoseStamped()
pose.header.frame_id = "base"
pose.pose.position.x = 0.5
pose.pose.position.y = 0.0
pose.pose.position.z = 0.0
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.z = 1.0
# planner.add_box_obstacle(size, "box", pose)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
# orien_const = OrientationConstraint()
# orien_const.link_name = "right_gripper";
# orien_const.header.frame_id = "base";
# orien_const.orientation.y = -1.0;
# orien_const.absolute_x_axis_tolerance = 0.1;
# orien_const.absolute_y_axis_tolerance = 0.1;
# orien_const.absolute_z_axis_tolerance = 0.1;
# orien_const.weight = 1.0;
def move_to_goal(x,
y,
z,
orien_const=[],
or_x=0.0,
or_y=-1.0,
or_z=0.0,
or_w=0.0):
while not rospy.is_shutdown():
try:
goal = PoseStamped()
goal.header.frame_id = "base"
#x, y, and z position
goal.pose.position.x = x
goal.pose.position.y = y
goal.pose.position.z = z
#Orientation as a quaternion
goal.pose.orientation.x = or_x
goal.pose.orientation.y = or_y
goal.pose.orientation.z = or_z
goal.pose.orientation.w = or_w
plan = planner.plan_to_pose(goal, orien_const)
# raw_input("Press <Enter> to move the right arm to goal pose: ")
rospy.sleep(1)
# Might have to edit this for part 5
if not planner.execute_plan(plan):
raise Exception("Execution failed")
else:
break
except Exception as e:
print e
traceback.print_exc()
# else:
# break
while not rospy.is_shutdown():
right_gripper = robot_gripper.Gripper('right')
right_gripper.set_moving_force(80.0)
right_gripper.set_holding_force(80.0)
# right_gripper.calibrate()
# Set your goal positions here
print("starting")
# move_to_goal(0.85, -0.3001, 0.1)
# rospy.sleep(1.)
move_to_goal(0.85, -0.2995, 0.1)
print("opening")
right_gripper.open()
rospy.sleep(1.)
print("executing")
move_to_goal(0.85, -0.2995, -0.030) #-0.41
print("closings")
# right_gripper.close()
print("MISSED: ", right_gripper.missed())
print("FORCEEE: ", right_gripper.force())
print("Done")
# move_to_goal(0.4225 + 0.1, -0.1265, 0.7725 - 0.92)
# right_gripper.close()
# move_to_goal(0.4225 + 0.1 + 0.05, -0.1265, 0.7725 - 0.92)
# right_gripper.open()
break
def main():
"""docstring for main"""
pp = PathPlanner(True)
pp.meters_per_cell = 0.4
pp.field_frame_id = "odom_combined"
pp.goal_timeout = 15.0
pp.pick_furthest = False
pp.file_name = os.path.join(os.getcwd(), "survey.csv")
pp.field_shape_publish_rate = 1.0
pp_thread = threading.Thread(target=pp.Init())
pp_thread.start()
time.sleep(1.0)
data = MoveBaseGoal()
data.target_pose.pose.position.x = 1
data.target_pose.pose.position.y = 1
data.target_pose.pose.position.x *= pp.meters_per_cell
data.target_pose.pose.position.x += pp.offset[0]
data.target_pose.pose.position.y *= pp.meters_per_cell
data.target_pose.pose.position.y = pp.offset[1] - data.target_pose.pose.position.y
pp.odomCallback(data)
curr_dest = pp.current_destination
while curr_dest != None:
if rospy.is_shutdown():
return
if curr_dest == -1:
continue
data = curr_dest
time.sleep(0.1)
data.target_pose.pose.position.x *= pp.meters_per_cell
data.target_pose.pose.position.x += pp.offset[0]
data.target_pose.pose.position.y *= pp.meters_per_cell
data.target_pose.pose.position.y = pp.offset[1] - data.target_pose.pose.position.y
pp.odomCallback(data)
curr_dest = pp.current_destination
class TurtleTeleOp(object):
def __init__(self, topic='cmd_vel_mux/input/navi', scan='scan', pos='slam_out_pose', obstacle_detect=False):
rospy.init_node('turtlebot_move', anonymous=True)
self.pub = rospy.Publisher(topic, Twist, queue_size=1)
self.subPos = rospy.Subscriber(pos, PoseStamped, self._callbackPos)
self.obstacleDetector = ObstacleDetector()
self.twist = Twist()
self.dest = [1024,1024]
self.path = PathPlanner()
self.pos = None
self._max_speed = 0.5 # m/s
self._max_omega = 0.5 # rad/s
self._range = (-1, 1)
def set_move(self, speed, omega):
self.twist.linear.x, self.twist.angular.z = speed, omega
def _callbackPos(self, data):
self.pos = data
print math.sqrt(math.pow((self.pos.pose.position.y/0.025)+1024-self.dest[1],2)+pow((self.pos.pose.position.x/0.025)+1024-self.dest[0],2))
def move(self, x, y):
# receive range in [-1, 1]
speed = self._clamp(y, *self._range) * self._max_speed
omega = self._clamp(x, *self._range) * self._max_omega
self.set_move(speed, omega)
self.publish()
def _callbackScan(self, data):
print("scan")
def moveToWaypoint(self, position, waid):
print("destination",position)
print("Current", [int(self.pos.pose.position.x/0.025+1024),int(self.pos.pose.position.y/0.025+1024)])
route = self.path.createPath(self.pos, position)
print "path created"
print route
i=0
for dest in route:
self.dest = dest;
i = i+1
if i<10: continue
print("destination",dest)
print("Current", [int(self.pos.pose.position.x/0.025+1024),int(self.pos.pose.position.y/0.025+1024)])
i=0
pos = self.pos.pose.position
yaw = self._getYaw()
curPos = [((pos.x/0.025)+1024),((pos.y/0.025)+1024)]
hip=0
ac = dest[0]-curPos[0]
oc = dest[1]-curPos[1]
yaw = round(yaw,2)
angle = round(math.atan2(oc, ac),2)
if angle>yaw or 5>abs(angle-self._getYaw())>3.14 :
while(abs(angle-self._getYaw())>0.06):
self.move(2*abs(angle-self._getYaw()), 0)
else:
while(abs(angle-self._getYaw())>0.06):
self.move(-2*abs(angle-self._getYaw()), 0)
while(20>math.sqrt(math.pow((self.pos.pose.position.y/0.025)+1024-dest[1],2)+pow((self.pos.pose.position.x/0.025)+1024-dest[0],2))>3):
self.move(0, 0.3)
#if self.obstacleDetector.is_obstacle() == True :
# return False;
print "done"
return True;
def stop(self):
self.publish(Twist())
def publish(self):
self.pub.publish(self.twist)
def _clamp(self, n, minn, maxn):
return max(min(maxn, n), minn)
def _getYaw(self):
(roll, pitch, yaw) = euler_from_quaternion([self.pos.pose.orientation.x,self.pos.pose.orientation.y, self.pos.pose.orientation.z, self.pos.pose.orientation.w])
return yaw