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