diff --git a/.gitignore b/.gitignore index fc4b7ff..b45cbbe 100644 --- a/.gitignore +++ b/.gitignore @@ -7,7 +7,6 @@ __pycache__/ # C extensions *.so -*.cpp # Distribution / packaging .Python diff --git a/MANIFEST.in b/MANIFEST.in deleted file mode 100644 index 3ad8059..0000000 --- a/MANIFEST.in +++ /dev/null @@ -1,3 +0,0 @@ -recursive-include pysbpl/mprim * -recursive-include pysbpl/maps * - diff --git a/README.md b/README.md index 80cf60e..3c04365 100644 --- a/README.md +++ b/README.md @@ -1,29 +1,2 @@ # pysbpl -#### Forked from : https://github.com/poine/pysbpl -#### Edits Made By: Schmittle - -Python3 bindings and utilities for SBPL ( Search Based Planning Library, see https://github.com/sbpl/sbpl ). Just the bindings for ARASTAR and EnvironmentNAVXYTHETALAT. - - -## Install & Run -1. Install [SBPL](https://github.com/sbpl/sbpl) from source. -2. Clone this repo - ` git clone https://github.com/schmittlema/pysbpl.git` -3. Build. Feel free to remove user to make install system wide - - For Development: `pip install -e . --user` - - For Use: `pip install . --user` -4. Test by running: `python3 examples/run_sbpl_xytheta.py`. - -If everything worked a visualization should appear - -## To create new motion primitives -See `pysbpl/gen_mprim.py`. Run with: -`python3 pysbpl/gen_mprim.py` - -A test.mprim file should be created - - -## Troubleshooting - -**I properly installed SBPL but pip install is saying it doesnt exist:** This is likely a path linking issue. Add the path to `libsbpl.so` (usually `/usr/share/lib`) to `$LD_LIBRARY_PATH` - +Python bindings and utilities for SBPL ( Search Based Planning Library, see https://github.com/sbpl/sbpl ) diff --git a/example/run_sbpl_xytheta.py b/example/run_sbpl_xytheta.py deleted file mode 100755 index b1c83ae..0000000 --- a/example/run_sbpl_xytheta.py +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/env python -# An Example using pysbpl - -import os -import pysbpl -from pysbpl import map_util, plot, planner - -if __name__ == '__main__': - src_dir = os.path.dirname(pysbpl.__file__) - - # map centered at origin - params = { - 'map': map_util.Map(yaml_path=src_dir + '/maps/sandbox.yaml'), - 'perimeter':[[-0.05, -0.05], [0.05, -0.05], [0.05, 0.05], [-0.05, 0.05]], - 'start':[0.0, 0.0, 0.0], # not required - 'goal': [1.20, 3.16, 0.], # not required - 'goal_tol':[0.5, 0.5, 0.1], - 'vel':0.5, 'time_45_deg':10, - 'mprim_path': (src_dir + '/mprim/pushr.mprim').encode('utf-8'), - 'obs_thresh': 200, - 'inscribed_thresh': 200, - 'possibly_circumscribed_thresh': 200, - } - - # Basic API - sbpl_planner = planner.Planner(**params) - points, headings, actions = sbpl_planner.plan(params['start'], params['goal']) - - if points is not None: - g = plot.Window() - g.display_map(params['map']) - g.display_path(points) - g.show() diff --git a/params/config.rviz b/params/config.rviz new file mode 100644 index 0000000..d08641b --- /dev/null +++ b/params/config.rviz @@ -0,0 +1,229 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 934 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 0.699999988 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: CostMap + Topic: /costmap_node/costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.699999988 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Lines + Line Width: 0.0299999993 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /sbpl/path + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.100000001 + Class: rviz/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.300000012 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.150000006 + Head Radius: 0.0500000007 + Name: start + Shaft Length: 0.100000001 + Shaft Radius: 0.0250000004 + Shape: Arrow + Topic: /sbpl/start + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.100000001 + Class: rviz/PoseWithCovariance + Color: 255; 255; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.300000012 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.150000006 + Head Radius: 0.0500000007 + Name: goal + Shaft Length: 0.100000001 + Shaft Radius: 0.0250000004 + Shape: Arrow + Topic: /sbpl/goal + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.78549194 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.15972805 + Y: 0.555970073 + Z: -0.193922952 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 1.29979599 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.24859142 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1246 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000437fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004300000437000000df00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000437fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004300000437000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009840000003efc0100000002fb0000000800540069006d00650100000000000009840000022400fffffffb0000000800540069006d00650100000000000004500000000000000000000006ff0000043700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2436 + X: 58 + Y: 10 diff --git a/pysbpl/__init__.py b/pysbpl/__init__.py deleted file mode 100644 index c86f49c..0000000 --- a/pysbpl/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -from .map_util import Map -from .planner import Planner, Config -from .plot import Window diff --git a/pysbpl/gen_mprim.py b/pysbpl/gen_mprim.py deleted file mode 100755 index 4eb1d9d..0000000 --- a/pysbpl/gen_mprim.py +++ /dev/null @@ -1,328 +0,0 @@ -#!/usr/bin/env python - -''' - Generates a motion primitive file for SBPL - This script replaces the matlab utily provided with SBPL. - It still puzzles me why people keep using this prehistoric monstrosity... - - Schmittle: Modified heavily for readability, modularity, and connection with limit surface -''' - -import os, math, numpy as np, matplotlib.pyplot as plt - -from pysbpl.store_metadata import Store_metadata_mprims - -class MPrim: - ''' - Base motion primitive class - ''' - def __init__(self, base_prim_id, th_curr, th_res, grid_res, **kwargs): - ''' - Constructor for super class - **Parameters** - - `base_prim_id` (int): arbitrary ID - - `th_curr` (int): index of given theta - - `th_res` (double): theta resolution - - `grid_res` (double): resolution of lattice - **Returns** - - **None** - ''' - self.base_prim_id, self.th_curr = base_prim_id, th_curr - self.th = th_curr * th_res # current theta radians - self.cos_th, self.sin_th = math.cos(self.th), math.sin(self.th) - self.start_pt_c, self.start_pt = np.array([0, 0, th_curr]), np.array([0, 0, self.th]) - self.cost = kwargs.get('cost', 1) # mprim cost - self.interm_nb = kwargs.get('interm_nb', 10) # number of interim points - self.interm_dist = kwargs.get('interm_dist', grid_res) # distance between interim points - self.th_nb = 2*math.pi/th_res # number of discrete angles - self.grid_resolution = grid_res - self.th_res = th_res - - def to_string(self): - ''' - Converts mprim to string for .mprim file - **Parameters** - - **None** - **Returns** - - `txt` (str): mprim file string for given mprim - ''' - txt = 'primID: {}\n'.format(self.base_prim_id) - txt += 'startangle_c: {}\n'.format(self.th_curr) - txt += 'endpose_c: {} {} {}\n'.format(*self.end_pt_c) - txt += 'additionalactioncostmult: {}\n'.format(self.cost) - txt += 'intermediateposes: {}\n'.format(len(self.interm_pts)) - txt += ''.join(['{:.4f} {:.4f} {:.4f}\n'.format(*interm_pt) for interm_pt in self.interm_pts]) - return txt - - def round(self, real_pt): - ''' - [UPDATE]: grid_pt [-1] converted to integer - Converts real number to grid point - and returns real number as a multiple of resolution - **Parameters** - - `real_pt` (list): x, y, theta point - **Returns** - - `grid_pt` (list): x, y, theta point on grid - - real_pt_g` (list): x, y, theta point multiple of grid resolution - ''' - grid_pt = [int(np.round(real_pt[0]/self.grid_resolution)), int(np.round(real_pt[1]/self.grid_resolution)), int(int(round(real_pt[2]/self.th_res))%self.th_nb)] - real_pt_g = [grid_pt[0]*self.grid_resolution, grid_pt[1]*self.grid_resolution, grid_pt[2]] - return np.array(grid_pt), np.array(real_pt_g) - -class MPrim_arc(MPrim): - ''' - Arc motion primitive - ''' - def __init__(self, base_prim_id, th_curr, th_res, grid_res, **kwargs): - ''' - Constructor for arc class - **Parameters** - - `base_prim_id` (int): arbitrary ID - - `th_curr` (int): index of given theta - - `th_res` (double): theta resolution - - `grid_res` (double): resolution of lattice - **Returns** - - **None** - ''' - # super class constructor - MPrim.__init__(self, base_prim_id, th_curr, th_res, grid_res, **kwargs) - - dth_curr, R = kwargs['dth_curr'], kwargs['R'] # heading variation and arc radius - self.R = R #(Alrick)Storing the radius - - dth = dth_curr * self.th_res # heading variation radians - X1 = self.pt_on_circle(R if dth>0 else -R, dth) - R1 = np.array([[self.cos_th, -self.sin_th],[self.sin_th, self.cos_th]]) # rotation matrix - X2 = np.dot(R1, X1) # end point rotated to current angle (th) - self.end_pt = np.array([X2[0], X2[1], self.th+dth]) - self.end_pt_c, self.end_pt_grid = self.round(self.end_pt) # discretize - # print ("End point c: ", self.end_pt_c) - - # interim points - #(Alrick) Added interm_ths0 for angles - interm_ths0 = np.linspace(self.th, self.end_pt_c[-1] * self.th_res, num = self.interm_nb) - interm_ths1 = np.linspace(0, dth, num = self.interm_nb) - interm_pts1 = [self.pt_on_circle(R if dth>0 else -R, th) for th in interm_ths1] - self.interm_pts = np.zeros((self.interm_nb, 3)) - for i in range(self.interm_nb): - self.interm_pts[i,:2] = np.dot(R1, interm_pts1[i]) - # self.interm_pts[i,2] = self.th + interm_ths1[i] #(Alrick) Commented out - self.interm_pts[i,2] = interm_ths0[i] - # discretize last point to match end_pt_c, not sure if we need repeated points - self.interm_pts[-1,:2] = self.end_pt_c[:2] * grid_res - self.interm_pts[-1,-1] = self.end_pt_c[-1] * self.th_res - - def pt_on_circle(self, R, dtheta): - ''' - For a given dtheta give me the x, y of the point on the circle - See: https://math.stackexchange.com/questions/260096/find-the-coordinates-of-a-point-on-a-circle - & here: https://math.stackexchange.com/questions/1384994/rotate-a-point-on-a-circle-with-known-radius-and-position - for more details - **Parameters** - - `R` (double): arc radius - - `dtheta` (double): delta theta - **Returns** - - `[x, y]` (array): x,y point on circle - ''' - return [R*math.sin(dtheta), R*(1-math.cos(dtheta))] - -class MPrim_line(MPrim): - ''' - Straight line motion primitive - ''' - def __init__(self, base_prim_id, th_curr, th_res, grid_res, **kwargs): - ''' - Constructor for straight class - **Parameters** - - `base_prim_id` (int): arbitrary ID - - `th_curr` (int): index of given theta - - `th_res` (double): theta resolution - - `grid_res` (double): resolution of lattice - **Returns** - - **None** - ''' - # super class constructor - MPrim.__init__(self, base_prim_id, th_curr, th_res, grid_res, **kwargs) - desired_len = kwargs['len_c'] * self.grid_resolution # meters - actual_len = desired_len - th_err, max_th_err = float("inf"), 0.5*self.th_res - - i = 0 - # discretization can create a change in theta - # Try lengthening the line to better fit theta - # this is a bit of a hack - while abs(th_err) > max_th_err and i<5: - # calculate end point and error - self.end_pt = self.start_pt + [self.cos_th*actual_len, self.sin_th*actual_len, 0] - self.end_pt_c, self.end_pt_grid = self.round(self.end_pt) - th_err = self.th - math.atan2(self.end_pt_grid[1], self.end_pt_grid[0]) - if np.sign(desired_len) < 0: th_err += math.pi # account for driving backwards - if th_err > math.pi: th_err = 2*math.pi-th_err # make sure errors is in the right direction - actual_len += np.sign(desired_len)*0.5*self.grid_resolution - i+=1 - - # iterim points - dX = self.end_pt_grid - self.start_pt - self.interm_nb = np.linalg.norm(dX[:2]) / self.interm_dist + 2 - self.interm_pts = np.array([self.start_pt + i*dX for i in np.linspace(0, 1, int(self.interm_nb))]) - -class MPrimFactory: - ''' - Takes base prims array and applies to each angle - ''' - - def __init__(self, base_prims, grid_resolution=0.025, th_nb=16): - ''' - Constructor - **Parameters** - - `base_prims` (list): list of dicts describing base primitives - - `grid_resolution` (double): resolution of lattice - - `theta_nb` (int): heading discretization - **Returns** - - **None** - ''' - self.grid_resolution = grid_resolution - self.base_prims = base_prims - self.nb_mprim_per_angle = len(self.base_prims) - self.th_nb = th_nb - self.th_res = 2*math.pi/th_nb # heading resolution - self.metadata = Store_metadata_mprims() #Initializing mprim container - - def build(self): - ''' - Build motion primitives from base and store in self.mprims - **Parameters** - - **None** - **Returns** - - **None** - ''' - self.mprims = [] - for angle_c in range(0, self.th_nb): - for bpid, bp in enumerate(self.base_prims): - self.mprims.append(bp['kind'](bpid, angle_c, self.th_res, self.grid_resolution, **bp['params'])) - - def write(self, out_path): - ''' - Write out .mprim file to given out_path - **Parameters** - - `out_path` (str): path + filename for .mprim file - **Returns** - - **None** - ''' - with open(out_path, 'w') as f: - f.write('resolution_m: {:f}\n'.format(self.grid_resolution)) - f.write('numberofangles: {:d}\n'.format(self.th_nb)) - f.write('totalnumberofprimitives: {:d}\n'.format(self.th_nb*self.nb_mprim_per_angle)) - for idx, mprim in enumerate(self.mprims): - self.metadata.add_mprim(mprim) #(Alrick) Adding mprim to metadata - f.write(mprim.to_string()) - - self.metadata.save() #Save the metadata into a json file - - def plot(self, ngrid=50, show_angle=None, show_prim_id=None, plot_points=None, multi_plot=False): - ''' - Plot motion primitives on grid - **Parameters** - - `ngrid` (int): Size of grid for visualization x & y - - `show_angle` (list): which angles to visualize primitves. None=all - - `show_prim_id` (list): which primitve to show for a given angle. None=all - - `plot_points` (dict): which points to plot for a given primitive - **Returns** - - **None** - ''' - # Styling - plt.rc('grid', linestyle="-", color='black') - minc, maxc = -ngrid*self.grid_resolution, ngrid*self.grid_resolution - ax = plt.gca(label=str(show_angle)) - ax.set_xlim([minc,maxc]); ax.set_ylim([minc,maxc]) - minor_ticks = np.arange(minc, maxc, self.grid_resolution) - major_ticks = np.arange(minc, maxc, 4*self.grid_resolution) - ax.set_xticks(major_ticks); ax.set_yticks(major_ticks) - ax.set_xticks(minor_ticks, minor=True) - ax.set_yticks(minor_ticks, minor=True) - ax.set_xticklabels(np.round(major_ticks, 2), rotation='vertical') - ax.set_aspect('equal') - ax.set_ylabel('Y') - ax.set_xlabel('X') - ax.grid(which='minor', alpha=0.2) - ax.grid(which='major', alpha=0.5) - - if plot_points is None: - plot_points = {'start_end': True, 'start_end_disc': True, 'start_end_c':True, 'interim': True, 'interim_thetas': False} - - # Plotting - for p in self.mprims: - if show_angle is None or p.th_curr in show_angle: # for each angle - if show_prim_id is None or p.base_prim_id in show_prim_id: # for each base primitive - #TODO align marker colors to interim colors, otherwise confusing - # start & end points - if plot_points['start_end']: - plt.scatter([p.start_pt[0], p.end_pt[0]], [p.start_pt[1], p.end_pt[1]], marker=(5, 2)) # real - if plot_points['start_end_disc']: - plt.scatter([p.start_pt[0], p.end_pt_grid[0]], [p.start_pt[1], p.end_pt_grid[1]]) # discretized - - # discretized start/end check _c matches - if plot_points['start_end_c']: - plt.scatter([p.start_pt_c[0]*self.grid_resolution, p.end_pt_c[0]*self.grid_resolution], - [p.start_pt_c[1]*self.grid_resolution, p.end_pt_c[1]*self.grid_resolution], marker=(5, 2)) - - # intermediate points - if plot_points['interim']: - plt.plot(p.interm_pts[:,0], p.interm_pts[:,1], '.-') - - # plot interm thetas. causes a lot of graph noise - if plot_points['interim_thetas']: - a_len = 0.1 - for ip in p.interm_pts: - plt.arrow(ip[0], ip[1], a_len * math.cos(ip[2]), a_len * math.sin(ip[2]), - head_width=0.1*a_len, head_length=0.1*a_len, fc='k', ec='k') - if not multi_plot: - plt.show() - - def check_all_dirs(self, show_prim_id=None, th_nb=None): - ''' - Plot subset of motion primitives on grid. - Different plot for each angle - **Parameters** - - `show_prim_id` (list): which primitve to show for a given angle. None=all - - `th_nb` (list): which angles to show. None=all - **Returns** - - **None** - ''' - if th_nb is None: - th_nb = range(0,self.th_nb) # all - for a in th_nb: - self.plot(show_angle=[a], show_prim_id=show_prim_id, multi_plot=True) - plt.show() - -def gen_example_prims(): - base_prims = [ - {'kind':MPrim_line, 'params':{'len_c': 1, 'cost':2}}, # forward straight short - {'kind':MPrim_line, 'params':{'len_c': 8, 'cost':1}}, # forward straight long - #{'kind':MPrim_line, 'params':{'len_c':-1, 'cost':5}}, # backward straight short - {'kind':MPrim_arc, 'params':{'R':0.24, 'dth_curr': 1, 'cost':7}}, # forward sharp turning left - {'kind':MPrim_arc, 'params':{'R':0.24, 'dth_curr':-1, 'cost':7}}, # forward sharp turning right - {'kind':MPrim_arc, 'params':{'R':0.30, 'dth_curr': 1, 'cost':6}}, # forward wide turning left - {'kind':MPrim_arc, 'params':{'R':0.30, 'dth_curr':-1, 'cost':6}}, # forward wide turning right - {'kind':MPrim_arc, 'params':{'R':0.50, 'dth_curr': 1, 'cost':5}}, # forward wide turning left - {'kind':MPrim_arc, 'params':{'R':0.50, 'dth_curr':-1, 'cost':5}}, # forward wide turning right - {'kind':MPrim_arc, 'params':{'R':0.75, 'dth_curr': 1, 'cost':4}}, # forward wide turning left - {'kind':MPrim_arc, 'params':{'R':0.75, 'dth_curr':-1, 'cost':4}}, # forward wide turning right - {'kind':MPrim_arc, 'params':{'R':1.00, 'dth_curr': 1, 'cost':3}}, # forward wide turning left - {'kind':MPrim_arc, 'params':{'R':1.00, 'dth_curr':-1, 'cost':3}}, # forward wide turning right - {'kind':MPrim_arc, 'params':{'R':1.25, 'dth_curr': 1, 'cost':2}}, # forward wide turning left - {'kind':MPrim_arc, 'params':{'R':1.25, 'dth_curr':-1, 'cost':2}}, # forward wide turning right - {'kind':MPrim_arc, 'params':{'R':-0.15, 'dth_curr':-1, 'cost':6}}, # backward turning left - {'kind':MPrim_arc, 'params':{'R':-0.15, 'dth_curr': 1, 'cost':6}} # backward turning right - ] - return base_prims - -if __name__ == '__main__': - base_prims = gen_example_prims() - - f = MPrimFactory(base_prims, grid_resolution=0.005, th_nb=16) - f.build() - f.plot() # all on one plot - #f.check_all_dirs() # one by one - f.write('test.mprim') - print("Motion primitives created and saved!") diff --git a/pysbpl/map_util.py b/pysbpl/map_util.py deleted file mode 100644 index 58d7ab1..0000000 --- a/pysbpl/map_util.py +++ /dev/null @@ -1,23 +0,0 @@ -# Map class -import os -import cv2 -import yaml - -class Map: - def __init__(self, **kwargs): - if 'yaml_path' in kwargs: - self.load_yaml(kwargs['yaml_path']) - - def load_yaml(self, yaml_path): - print(' loading map from yaml {}'.format(yaml_path)) - with open(yaml_path, "r") as f: - _yaml = yaml.safe_load(f) - map_img_path = os.path.join(os.path.dirname(yaml_path), _yaml['image']) - self.img = cv2.imread(map_img_path, 0) - self.img[self.img==0] = 1 - self.img[self.img==255] = 0 - self.height, self.width = self.img.shape - self.resolution = _yaml['resolution'] - self.origin = _yaml['origin'] - self.occupied_thresh = _yaml['occupied_thresh'] - self.free_thresh = _yaml['free_thresh'] diff --git a/pysbpl/maps/sandbox.png b/pysbpl/maps/sandbox.png deleted file mode 100644 index 44ab6c6..0000000 Binary files a/pysbpl/maps/sandbox.png and /dev/null differ diff --git a/pysbpl/maps/sandbox.yaml b/pysbpl/maps/sandbox.yaml deleted file mode 100644 index cd648c1..0000000 --- a/pysbpl/maps/sandbox.yaml +++ /dev/null @@ -1,6 +0,0 @@ -image: sandbox.png -resolution: 0.1 -origin: [-50, -50, 0] -occupied_thresh: 0.1 -free_thresh: 0.09 -negate: 0 diff --git a/pysbpl/mprim/pushr.mprim b/pysbpl/mprim/pushr.mprim deleted file mode 100644 index 8ca4979..0000000 --- a/pysbpl/mprim/pushr.mprim +++ /dev/null @@ -1,723 +0,0 @@ -resolution_m: 0.100000 -numberofangles: 16 -totalnumberofprimitives: 48 -primID: 0 -startangle_c: 0 -endpose_c: 5 0 0 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0556 0.0000 0.0000 -0.1111 0.0000 0.0000 -0.1667 0.0000 0.0000 -0.2222 0.0000 0.0000 -0.2778 0.0000 0.0000 -0.3333 0.0000 0.0000 -0.3889 0.0000 0.0000 -0.4444 0.0000 0.0000 -0.5000 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 5 2 1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0559 0.0170 0.0223 -0.1119 0.0354 0.0445 -0.1677 0.0550 0.0668 -0.2235 0.0759 0.0890 -0.2791 0.0982 0.1113 -0.3346 0.1217 0.1335 -0.3900 0.1465 0.1558 -0.4451 0.1726 0.1781 -0.5000 0.2000 0.2003 -primID: 2 -startangle_c: 0 -endpose_c: 5 -2 -1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0559 -0.0170 -0.0223 -0.1119 -0.0354 -0.0445 -0.1677 -0.0550 -0.0668 -0.2235 -0.0759 -0.0890 -0.2791 -0.0982 -0.1113 -0.3346 -0.1217 -0.1335 -0.3900 -0.1465 -0.1558 -0.4451 -0.1726 -0.1781 -0.5000 -0.2000 -0.2003 -primID: 0 -startangle_c: 1 -endpose_c: 5 2 1 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0556 0.0222 0.3927 -0.1111 0.0444 0.3927 -0.1667 0.0667 0.3927 -0.2222 0.0889 0.3927 -0.2778 0.1111 0.3927 -0.3333 0.1333 0.3927 -0.3889 0.1556 0.3927 -0.4444 0.1778 0.3927 -0.5000 0.2000 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 5 1 0 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0531 0.0208 0.3509 -0.1071 0.0392 0.3071 -0.1618 0.0552 0.2632 -0.2172 0.0689 0.2193 -0.2731 0.0800 0.1755 -0.3294 0.0888 0.1316 -0.3861 0.0950 0.0877 -0.4430 0.0987 0.0439 -0.5000 0.1000 0.0000 -primID: 2 -startangle_c: 1 -endpose_c: 4 3 2 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0481 0.0267 0.4268 -0.0955 0.0551 0.4610 -0.1419 0.0853 0.4951 -0.1875 0.1171 0.5292 -0.2322 0.1505 0.5633 -0.2758 0.1856 0.5975 -0.3183 0.2222 0.6316 -0.3597 0.2604 0.6657 -0.4000 0.3000 0.6998 -primID: 0 -startangle_c: 2 -endpose_c: 4 4 2 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0444 0.0444 0.7854 -0.0889 0.0889 0.7854 -0.1333 0.1333 0.7854 -0.1778 0.1778 0.7854 -0.2222 0.2222 0.7854 -0.2667 0.2667 0.7854 -0.3111 0.3111 0.7854 -0.3556 0.3556 0.7854 -0.4000 0.4000 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 4 2 1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0411 0.0262 0.7589 -0.0831 0.0515 0.7324 -0.1260 0.0758 0.7059 -0.1697 0.0991 0.6795 -0.2142 0.1214 0.6530 -0.2595 0.1426 0.6265 -0.3056 0.1628 0.6000 -0.3525 0.1820 0.5735 -0.4000 0.2000 0.5470 -primID: 2 -startangle_c: 2 -endpose_c: 2 4 3 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0262 0.0411 0.8119 -0.0515 0.0831 0.8384 -0.0758 0.1260 0.8649 -0.0991 0.1697 0.8913 -0.1214 0.2142 0.9178 -0.1426 0.2595 0.9443 -0.1628 0.3056 0.9708 -0.1820 0.3525 0.9973 -0.2000 0.4000 1.0238 -primID: 0 -startangle_c: 3 -endpose_c: 2 5 3 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0222 0.0556 1.1781 -0.0444 0.1111 1.1781 -0.0667 0.1667 1.1781 -0.0889 0.2222 1.1781 -0.1111 0.2778 1.1781 -0.1333 0.3333 1.1781 -0.1556 0.3889 1.1781 -0.1778 0.4444 1.1781 -0.2000 0.5000 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 1 5 4 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0208 0.0531 1.2199 -0.0392 0.1071 1.2637 -0.0552 0.1618 1.3076 -0.0689 0.2172 1.3515 -0.0800 0.2731 1.3953 -0.0888 0.3294 1.4392 -0.0950 0.3861 1.4831 -0.0987 0.4430 1.5269 -0.1000 0.5000 1.5708 -primID: 2 -startangle_c: 3 -endpose_c: 3 4 2 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0267 0.0481 1.1440 -0.0551 0.0955 1.1098 -0.0853 0.1419 1.0757 -0.1171 0.1875 1.0416 -0.1505 0.2322 1.0075 -0.1856 0.2758 0.9733 -0.2222 0.3183 0.9392 -0.2604 0.3597 0.9051 -0.3000 0.4000 0.8710 -primID: 0 -startangle_c: 4 -endpose_c: 0 5 4 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0556 1.5708 -0.0000 0.1111 1.5708 -0.0000 0.1667 1.5708 -0.0000 0.2222 1.5708 -0.0000 0.2778 1.5708 -0.0000 0.3333 1.5708 -0.0000 0.3889 1.5708 -0.0000 0.4444 1.5708 -0.0000 0.5000 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: -2 5 5 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.5708 --0.0170 0.0559 1.5931 --0.0354 0.1119 1.6153 --0.0550 0.1677 1.6376 --0.0759 0.2235 1.6598 --0.0982 0.2791 1.6821 --0.1217 0.3346 1.7043 --0.1465 0.3900 1.7266 --0.1726 0.4451 1.7489 --0.2000 0.5000 1.7711 -primID: 2 -startangle_c: 4 -endpose_c: 2 5 3 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0170 0.0559 1.5485 -0.0354 0.1119 1.5263 -0.0550 0.1677 1.5040 -0.0759 0.2235 1.4818 -0.0982 0.2791 1.4595 -0.1217 0.3346 1.4373 -0.1465 0.3900 1.4150 -0.1726 0.4451 1.3927 -0.2000 0.5000 1.3705 -primID: 0 -startangle_c: 5 -endpose_c: -2 5 5 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0222 0.0556 1.9635 --0.0444 0.1111 1.9635 --0.0667 0.1667 1.9635 --0.0889 0.2222 1.9635 --0.1111 0.2778 1.9635 --0.1333 0.3333 1.9635 --0.1556 0.3889 1.9635 --0.1778 0.4444 1.9635 --0.2000 0.5000 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -1 5 4 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0208 0.0531 1.9217 --0.0392 0.1071 1.8779 --0.0552 0.1618 1.8340 --0.0689 0.2172 1.7901 --0.0800 0.2731 1.7463 --0.0888 0.3294 1.7024 --0.0950 0.3861 1.6585 --0.0987 0.4430 1.6147 --0.1000 0.5000 1.5708 -primID: 2 -startangle_c: 5 -endpose_c: -3 4 6 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0267 0.0481 1.9976 --0.0551 0.0955 2.0317 --0.0853 0.1419 2.0659 --0.1171 0.1875 2.1000 --0.1505 0.2322 2.1341 --0.1856 0.2758 2.1683 --0.2222 0.3183 2.2024 --0.2604 0.3597 2.2365 --0.3000 0.4000 2.2706 -primID: 0 -startangle_c: 6 -endpose_c: -4 4 6 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0444 0.0444 2.3562 --0.0889 0.0889 2.3562 --0.1333 0.1333 2.3562 --0.1778 0.1778 2.3562 --0.2222 0.2222 2.3562 --0.2667 0.2667 2.3562 --0.3111 0.3111 2.3562 --0.3556 0.3556 2.3562 --0.4000 0.4000 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -2 4 5 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0262 0.0411 2.3297 --0.0515 0.0831 2.3032 --0.0758 0.1260 2.2767 --0.0991 0.1697 2.2502 --0.1214 0.2142 2.2238 --0.1426 0.2595 2.1973 --0.1628 0.3056 2.1708 --0.1820 0.3525 2.1443 --0.2000 0.4000 2.1178 -primID: 2 -startangle_c: 6 -endpose_c: -4 2 7 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0411 0.0262 2.3827 --0.0831 0.0515 2.4092 --0.1260 0.0758 2.4357 --0.1697 0.0991 2.4621 --0.2142 0.1214 2.4886 --0.2595 0.1426 2.5151 --0.3056 0.1628 2.5416 --0.3525 0.1820 2.5681 --0.4000 0.2000 2.5946 -primID: 0 -startangle_c: 7 -endpose_c: -5 2 7 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0556 0.0222 2.7489 --0.1111 0.0444 2.7489 --0.1667 0.0667 2.7489 --0.2222 0.0889 2.7489 --0.2778 0.1111 2.7489 --0.3333 0.1333 2.7489 --0.3889 0.1556 2.7489 --0.4444 0.1778 2.7489 --0.5000 0.2000 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -5 1 8 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0531 0.0208 2.7907 --0.1071 0.0392 2.8345 --0.1618 0.0552 2.8784 --0.2172 0.0689 2.9223 --0.2731 0.0800 2.9661 --0.3294 0.0888 3.0100 --0.3861 0.0950 3.0539 --0.4430 0.0987 3.0977 --0.5000 0.1000 3.1416 -primID: 2 -startangle_c: 7 -endpose_c: -4 3 6 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0481 0.0267 2.7148 --0.0955 0.0551 2.6806 --0.1419 0.0853 2.6465 --0.1875 0.1171 2.6124 --0.2322 0.1505 2.5783 --0.2758 0.1856 2.5441 --0.3183 0.2222 2.5100 --0.3597 0.2604 2.4759 --0.4000 0.3000 2.4418 -primID: 0 -startangle_c: 8 -endpose_c: -5 0 8 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0556 0.0000 3.1416 --0.1111 0.0000 3.1416 --0.1667 0.0000 3.1416 --0.2222 0.0000 3.1416 --0.2778 0.0000 3.1416 --0.3333 0.0000 3.1416 --0.3889 0.0000 3.1416 --0.4444 0.0000 3.1416 --0.5000 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -5 -2 9 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0559 -0.0170 3.1639 --0.1119 -0.0354 3.1861 --0.1677 -0.0550 3.2084 --0.2235 -0.0759 3.2306 --0.2791 -0.0982 3.2529 --0.3346 -0.1217 3.2751 --0.3900 -0.1465 3.2974 --0.4451 -0.1726 3.3197 --0.5000 -0.2000 3.3419 -primID: 2 -startangle_c: 8 -endpose_c: -5 2 7 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0559 0.0170 3.1193 --0.1119 0.0354 3.0971 --0.1677 0.0550 3.0748 --0.2235 0.0759 3.0526 --0.2791 0.0982 3.0303 --0.3346 0.1217 3.0080 --0.3900 0.1465 2.9858 --0.4451 0.1726 2.9635 --0.5000 0.2000 2.9413 -primID: 0 -startangle_c: 9 -endpose_c: -5 -2 9 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0556 -0.0222 3.5343 --0.1111 -0.0444 3.5343 --0.1667 -0.0667 3.5343 --0.2222 -0.0889 3.5343 --0.2778 -0.1111 3.5343 --0.3333 -0.1333 3.5343 --0.3889 -0.1556 3.5343 --0.4444 -0.1778 3.5343 --0.5000 -0.2000 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -5 -1 8 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0531 -0.0208 3.4925 --0.1071 -0.0392 3.4487 --0.1618 -0.0552 3.4048 --0.2172 -0.0689 3.3609 --0.2731 -0.0800 3.3171 --0.3294 -0.0888 3.2732 --0.3861 -0.0950 3.2293 --0.4430 -0.0987 3.1855 --0.5000 -0.1000 3.1416 -primID: 2 -startangle_c: 9 -endpose_c: -4 -3 10 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0481 -0.0267 3.5684 --0.0955 -0.0551 3.6025 --0.1419 -0.0853 3.6367 --0.1875 -0.1171 3.6708 --0.2322 -0.1505 3.7049 --0.2758 -0.1856 3.7390 --0.3183 -0.2222 3.7732 --0.3597 -0.2604 3.8073 --0.4000 -0.3000 3.8414 -primID: 0 -startangle_c: 10 -endpose_c: -4 -4 10 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0444 -0.0444 3.9270 --0.0889 -0.0889 3.9270 --0.1333 -0.1333 3.9270 --0.1778 -0.1778 3.9270 --0.2222 -0.2222 3.9270 --0.2667 -0.2667 3.9270 --0.3111 -0.3111 3.9270 --0.3556 -0.3556 3.9270 --0.4000 -0.4000 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -4 -2 9 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0411 -0.0262 3.9005 --0.0831 -0.0515 3.8740 --0.1260 -0.0758 3.8475 --0.1697 -0.0991 3.8210 --0.2142 -0.1214 3.7946 --0.2595 -0.1426 3.7681 --0.3056 -0.1628 3.7416 --0.3525 -0.1820 3.7151 --0.4000 -0.2000 3.6886 -primID: 2 -startangle_c: 10 -endpose_c: -2 -4 11 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0262 -0.0411 3.9535 --0.0515 -0.0831 3.9800 --0.0758 -0.1260 4.0064 --0.0991 -0.1697 4.0329 --0.1214 -0.2142 4.0594 --0.1426 -0.2595 4.0859 --0.1628 -0.3056 4.1124 --0.1820 -0.3525 4.1389 --0.2000 -0.4000 4.1654 -primID: 0 -startangle_c: 11 -endpose_c: -2 -5 11 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0222 -0.0556 4.3197 --0.0444 -0.1111 4.3197 --0.0667 -0.1667 4.3197 --0.0889 -0.2222 4.3197 --0.1111 -0.2778 4.3197 --0.1333 -0.3333 4.3197 --0.1556 -0.3889 4.3197 --0.1778 -0.4444 4.3197 --0.2000 -0.5000 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -1 -5 12 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0208 -0.0531 4.3615 --0.0392 -0.1071 4.4053 --0.0552 -0.1618 4.4492 --0.0689 -0.2172 4.4931 --0.0800 -0.2731 4.5369 --0.0888 -0.3294 4.5808 --0.0950 -0.3861 4.6247 --0.0987 -0.4430 4.6685 --0.1000 -0.5000 4.7124 -primID: 2 -startangle_c: 11 -endpose_c: -3 -4 10 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0267 -0.0481 4.2856 --0.0551 -0.0955 4.2514 --0.0853 -0.1419 4.2173 --0.1171 -0.1875 4.1832 --0.1505 -0.2322 4.1491 --0.1856 -0.2758 4.1149 --0.2222 -0.3183 4.0808 --0.2604 -0.3597 4.0467 --0.3000 -0.4000 4.0126 -primID: 0 -startangle_c: 12 -endpose_c: 0 -5 12 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0556 4.7124 -0.0000 -0.1111 4.7124 -0.0000 -0.1667 4.7124 -0.0000 -0.2222 4.7124 -0.0000 -0.2778 4.7124 -0.0000 -0.3333 4.7124 -0.0000 -0.3889 4.7124 -0.0000 -0.4444 4.7124 -0.0000 -0.5000 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 2 -5 13 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0170 -0.0559 4.7346 -0.0354 -0.1119 4.7569 -0.0550 -0.1677 4.7792 -0.0759 -0.2235 4.8014 -0.0982 -0.2791 4.8237 -0.1217 -0.3346 4.8459 -0.1465 -0.3900 4.8682 -0.1726 -0.4451 4.8904 -0.2000 -0.5000 4.9127 -primID: 2 -startangle_c: 12 -endpose_c: -2 -5 11 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0170 -0.0559 4.6901 --0.0354 -0.1119 4.6679 --0.0550 -0.1677 4.6456 --0.0759 -0.2235 4.6234 --0.0982 -0.2791 4.6011 --0.1217 -0.3346 4.5788 --0.1465 -0.3900 4.5566 --0.1726 -0.4451 4.5343 --0.2000 -0.5000 4.5121 -primID: 0 -startangle_c: 13 -endpose_c: 2 -5 13 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0222 -0.0556 5.1051 -0.0444 -0.1111 5.1051 -0.0667 -0.1667 5.1051 -0.0889 -0.2222 5.1051 -0.1111 -0.2778 5.1051 -0.1333 -0.3333 5.1051 -0.1556 -0.3889 5.1051 -0.1778 -0.4444 5.1051 -0.2000 -0.5000 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 1 -5 12 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0208 -0.0531 5.0633 -0.0392 -0.1071 5.0195 -0.0552 -0.1618 4.9756 -0.0689 -0.2172 4.9317 -0.0800 -0.2731 4.8879 -0.0888 -0.3294 4.8440 -0.0950 -0.3861 4.8001 -0.0987 -0.4430 4.7563 -0.1000 -0.5000 4.7124 -primID: 2 -startangle_c: 13 -endpose_c: 3 -4 14 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0267 -0.0481 5.1392 -0.0551 -0.0955 5.1733 -0.0853 -0.1419 5.2075 -0.1171 -0.1875 5.2416 -0.1505 -0.2322 5.2757 -0.1856 -0.2758 5.3098 -0.2222 -0.3183 5.3440 -0.2604 -0.3597 5.3781 -0.3000 -0.4000 5.4122 -primID: 0 -startangle_c: 14 -endpose_c: 4 -4 14 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0444 -0.0444 5.4978 -0.0889 -0.0889 5.4978 -0.1333 -0.1333 5.4978 -0.1778 -0.1778 5.4978 -0.2222 -0.2222 5.4978 -0.2667 -0.2667 5.4978 -0.3111 -0.3111 5.4978 -0.3556 -0.3556 5.4978 -0.4000 -0.4000 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 2 -4 13 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0262 -0.0411 5.4713 -0.0515 -0.0831 5.4448 -0.0758 -0.1260 5.4183 -0.0991 -0.1697 5.3918 -0.1214 -0.2142 5.3654 -0.1426 -0.2595 5.3389 -0.1628 -0.3056 5.3124 -0.1820 -0.3525 5.2859 -0.2000 -0.4000 5.2594 -primID: 2 -startangle_c: 14 -endpose_c: 4 -2 15 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0411 -0.0262 5.5243 -0.0831 -0.0515 5.5508 -0.1260 -0.0758 5.5772 -0.1697 -0.0991 5.6037 -0.2142 -0.1214 5.6302 -0.2595 -0.1426 5.6567 -0.3056 -0.1628 5.6832 -0.3525 -0.1820 5.7097 -0.4000 -0.2000 5.7362 -primID: 0 -startangle_c: 15 -endpose_c: 5 -2 15 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0556 -0.0222 5.8905 -0.1111 -0.0444 5.8905 -0.1667 -0.0667 5.8905 -0.2222 -0.0889 5.8905 -0.2778 -0.1111 5.8905 -0.3333 -0.1333 5.8905 -0.3889 -0.1556 5.8905 -0.4444 -0.1778 5.8905 -0.5000 -0.2000 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 5 -1 0 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0531 -0.0208 5.9323 -0.1071 -0.0392 5.9761 -0.1618 -0.0552 6.0200 -0.2172 -0.0689 6.0639 -0.2731 -0.0800 6.1077 -0.3294 -0.0888 6.1516 -0.3861 -0.0950 6.1955 -0.4430 -0.0987 6.2393 -0.5000 -0.1000 6.2832 -primID: 2 -startangle_c: 15 -endpose_c: 4 -3 14 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0481 -0.0267 5.8564 -0.0955 -0.0551 5.8222 -0.1419 -0.0853 5.7881 -0.1875 -0.1171 5.7540 -0.2322 -0.1505 5.7199 -0.2758 -0.1856 5.6857 -0.3183 -0.2222 5.6516 -0.3597 -0.2604 5.6175 -0.4000 -0.3000 5.5834 diff --git a/pysbpl/planner.py b/pysbpl/planner.py deleted file mode 100755 index 6383baf..0000000 --- a/pysbpl/planner.py +++ /dev/null @@ -1,44 +0,0 @@ -import time -import numpy as np -from pysbpl import sbpl - -class Config: - def __init__(self, **kwargs): - self.env = sbpl.EnvironmentNAVXYTHETALAT() - self.map = kwargs['map'] - self.env.update_map(self.map) # loads map once b4 planning - - def initEnv(self, **kwargs): - self.start_id, self.goal_id = self.env.InitializeEnv(**kwargs) - -class Planner: - def __init__(self, **kwargs): - self.kwargs = kwargs - self.config = Config(**kwargs) - self.planner = sbpl.ARAPlanner(self.config.env) - - print('Planner Initialized') - - def plan(self, start, goal): - - print('Planning!') - print('Start: {}'.format(str(start))) - print('Goal: {} \n'.format(str(goal))) - - # Transform from origin - origin = np.array(self.config.map.origin) - self.kwargs['start'] = np.array(start) - origin - self.kwargs['goal'] = np.array(goal) - origin - - self.config.initEnv(**self.kwargs) - self.planner.initialize(self.config.start_id, self.config.goal_id) - return self.run() - - def run(self): - start_time = time.time() - points, headings, primitives = self.planner.run() - print('Planning Time: {}'.format(round(time.time() - start_time, 4))) - if points is None: - return None, None - points += self.config.map.origin[:2] - return points, headings, primitives diff --git a/pysbpl/plot.py b/pysbpl/plot.py deleted file mode 100755 index 6cc2b4f..0000000 --- a/pysbpl/plot.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python -import numpy as np -import matplotlib -import matplotlib.pyplot as plt - -class Window: - - def __init__(self): - self.map = None - - def display_map(self, map_file): - self.map = map_file - image = np.zeros((map_file.img.shape[0], map_file.img.shape[1],3)) - image[:,:,0] = np.invert(map_file.img) - image[:,:,1] = np.invert(map_file.img) - image[:,:,2] = np.invert(map_file.img) - image[image==254]=0 - plt.imshow(image) - ticks = matplotlib.ticker.FuncFormatter(lambda _x, pos: '{0:g}'.format(_x*map_file.resolution)) - plt.gca().xaxis.set_major_formatter(ticks) - plt.gca().set_xlabel('dimensions in meters') - plt.gca().yaxis.set_major_formatter(ticks) - plt.gca().set_ylabel('dimensions in meters') - plt.gca().invert_yaxis() - - def display_path(self, points): - map_points = self.world_to_map(points) - plt.plot(map_points[:,0], map_points[:,1], marker='.', markersize=1, color='#36CDC4') - - def world_to_map(self, points): - origin = self.map.origin[:2] - resolution = self.map.resolution - map_height = self.map.img.shape[0] - return [1, 1]*(points-origin)/resolution - - def show(self): - plt.show() diff --git a/pysbpl/store_metadata.py b/pysbpl/store_metadata.py deleted file mode 100755 index fe09744..0000000 --- a/pysbpl/store_metadata.py +++ /dev/null @@ -1,66 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Wed Mar 17 23:53:09 2021 - -@author: alrick -""" -#Script for saving files in steering angle and speed. -import math, json - -class Store_metadata_mprims: - """ - Class to store steering angle, speed, and travel time from the motion - primitives. - """ - - def __init__(self, wheel_base=0.225, save_path=None): - """ - Initliazing class params. - """ - self.save_path = "metadata.json" if not save_path else save_path - self.mprims_map = {} - self.wheel_base = wheel_base - - def add_mprim(self, base_prim): - """ - Processing the base_prim from MPrimFactory to output steering angle, - and travel time (path len / speed). Speed is fixed to be 0.5 m/s. - Input: base_prim: Class MPrim_arc (will later extend to MPrim_line) - - Formula to calculate the steering angle. - Link: https://github.com/prl-mushr/mushr/blob/master/mushr_description/kinematic_car_model.pdf - """ - - t = str(tuple([base_prim.th_curr] + list(base_prim.end_pt_c))) - - if (t not in self.mprims_map): - speed = 0.5 - - #Computing steering angle. - beta = math.asin(self.wheel_base / (2 * base_prim.R)) - st_angle = math.atan( 2 * math.tan(beta) ) - - #Compute path lenght, then convert to - #Have to check for angles greater than pi. - # d = math.sqrt(sum(x**2 for x in base_prim.end_pt_c[:2]))*base_prim.grid_resolution - # print ("Before Path len: ", d, base_prim.R, 2 * base_prim.R**2 - d**2, \ - # ( 2 * base_prim.R**2 - d**2) / (2 * base_prim.R**2), (base_prim.end_pt_c[-1] * base_prim.th_res)) - # path_len = math.acos( ( 2 * base_prim.R**2 - d**2) / (2 * base_prim.R**2) ) * base_prim.R - - #Path len (difference in start and end angle (in radians)) * radius - path_len = abs((base_prim.end_pt_c[-1] * base_prim.th_res - \ - base_prim.th) * base_prim.R * base_prim.grid_resolution) - - #Store mprims - self.mprims_map[t] = {"speed":speed, "steering_angle":st_angle, \ - "time":path_len/speed} - - def save(self): - """ - Save self.mprims_map in a json file at self.save_path. - """ - print ("\nSaving Metadata.") - with open(self.save_path, 'w') as file: - json.dump(self.mprims_map, file, sort_keys=False, indent=4) - \ No newline at end of file diff --git a/requirements.txt b/requirements.txt deleted file mode 100644 index 28d449c..0000000 --- a/requirements.txt +++ /dev/null @@ -1,22 +0,0 @@ -# Automatically generated by https://github.com/damnever/pigar. - -# pysbpl/setup.py: 2,7 -Cython == 0.29.21 - -# pysbpl/pysbpl/map_util.py: 4 -PyYAML == 5.3.1 - -# pysbpl/pysbpl/gen_mprim.py: 11 -# pysbpl/pysbpl/plot.py: 3,4 -matplotlib == 3.2.2 - -# pysbpl/pysbpl/gen_mprim.py: 11 -# pysbpl/pysbpl/planner.py: 2 -# pysbpl/pysbpl/plot.py: 2 -numpy == 1.19.2 - -# pysbpl/pysbpl/map_util.py: 3 -opencv_contrib_python == 3.4.0.12 - -# pysbpl/setup.py: 1 -setuptools == 20.7.0 diff --git a/src/Makefile b/src/Makefile new file mode 100644 index 0000000..ca6aa9c --- /dev/null +++ b/src/Makefile @@ -0,0 +1,9 @@ +pysbpl.so: sbpl.pyx setup_sbpl_ext.py + @echo "## Building sbpl python extension ##" + python setup_sbpl_ext.py build_ext --inplace + @echo "## all done##\n" + + +clean: + rm -rf *~ *.pyc sbpl.cpp pysbpl.so build + diff --git a/src/gem_mprim.py b/src/gem_mprim.py new file mode 100755 index 0000000..1875a2f --- /dev/null +++ b/src/gem_mprim.py @@ -0,0 +1,198 @@ +#!/usr/bin/env python + +''' + Generates a motion primitive file for SBPL + This script replaces the matlab utily provided with SBPL. + It still puzzles me why people keep using this prehistoric monstrosity... +''' + +import os, logging, math, numpy as np, scipy, matplotlib.image, matplotlib.pyplot as plt +import pdb + +LOG = logging.getLogger('gen_mprim') + +class MPrim: + def __init__(self, base_prim_id, th_c, **kwargs): + self.base_prim_id, self.th_c = base_prim_id, th_c + self.th = th_c * MPrimFactory.th_res + self.cth, self.sth = math.cos(self.th), math.sin(self.th) + self.start_pt_c, self.start_pt = np.array([0, 0, th_c]), np.array([0, 0, self.th]) + self.cost = kwargs.get('cost', 1) + self.interm_nb = kwargs.get('interm_nb', 10) + self.interm_dist = kwargs.get('interm_dist', MPrimFactory.grid_resolution) + + def to_string(self): + txt = 'primID: {}\n'.format(self.base_prim_id) + txt += 'startangle_c: {}\n'.format(self.th_c) + txt += 'endpose_c: {} {} {}\n'.format(*self.end_pt_c) + txt += 'additionalactioncostmult: {}\n'.format(self.cost) + txt += 'intermediateposes: {}\n'.format(len(self.interm_pts)) + txt += ''.join(['{:.4f} {:.4f} {:.4f}\n'.format(*interm_pt) for interm_pt in self.interm_pts]) + return txt + +def round_xy(X): + X_c = [int(np.round(X[0]/MPrimFactory.grid_resolution)), int(np.round(X[1]/MPrimFactory.grid_resolution)), int(round(X[2]/MPrimFactory.th_res))%MPrimFactory.th_nb] + X_r = [X_c[0]*MPrimFactory.grid_resolution, X_c[1]*MPrimFactory.grid_resolution, X[2]] + return np.array(X_c), np.array(X_r) + +def pt_on_0R_circle(R, th): return [R*math.sin(th), R*(1-math.cos(th))] + +class MPrim_arc(MPrim): + def __init__(self, base_prim_id, th_c, **kwargs): + MPrim.__init__(self, base_prim_id, th_c, **kwargs) + dth_c, R = kwargs['dth_c'], kwargs['R'] # heading variation and radius + dth = dth_c * MPrimFactory.th_res + X1 = pt_on_0R_circle(R if dth>0 else -R, dth) + R1 = np.array([[self.cth, -self.sth],[self.sth, self.cth]]) + X2 = np.dot(R1, X1) + self.end_pt = np.array([X2[0], X2[1], self.th+dth]) + self.end_pt_c, self.end_pt_grid = round_xy(self.end_pt) + interm_ths1 = np.linspace(0, dth, self.interm_nb) + interm_pts1 = [pt_on_0R_circle(R if dth>0 else -R, th) for th in interm_ths1] + self.interm_pts = np.zeros((self.interm_nb, 3)) + for i in range(self.interm_nb): + self.interm_pts[i,:2] = np.dot(R1, interm_pts1[i]) + self.interm_pts[i,2] = self.th + interm_ths1[i] + self.interm_pts[-1] = self.end_pt_grid # is that needed? CHECKME + #print self.start_pt_c[2], self.end_pt_c[2], self.interm_pts[:,2] + #print('a {:.3f} p {:02d}'.format(self.th, base_prim_id)) + +class MPrim_line(MPrim): + def __init__(self, base_prim_id, th_c, **kwargs): + MPrim.__init__(self, base_prim_id, th_c, **kwargs) + desired_len = kwargs['len_c'] * MPrimFactory.grid_resolution + actual_len = desired_len + th_err, max_th_err = float("inf"), 0.5*MPrimFactory.th_res + i = 0 + while abs(th_err) > max_th_err and i<5: + self.end_pt = self.start_pt + [self.cth*actual_len, self.sth*actual_len, 0] + self.end_pt_c, self.end_pt_grid = round_xy(self.end_pt) + th_err = self.th - math.atan2(self.end_pt_grid[1], self.end_pt_grid[0]) + if np.sign(desired_len) < 0: th_err += math.pi # account for driving backwards + if th_err > math.pi: th_err = 2*math.pi-th_err # make sure errors is in the right direction + l_err = np.linalg.norm(self.end_pt_grid[:2]) - desired_len + #print('l_err {:.3f} th_err {:.3f}'.format(l_err, th_err)) + actual_len += np.sign(desired_len)*0.5*MPrimFactory.grid_resolution + i+=1 + + dX = self.end_pt_grid - self.start_pt + n_interm = np.linalg.norm(dX) / self.interm_dist + 2# self.interm_nb + self.interm_nb = n_interm + self.interm_pts = np.array([self.start_pt + i*dX for i in np.linspace(0, 1, n_interm)]) + #print('a {:.3f} p {:02d} l_err {:.3f} th_err {:.3f}'.format(self.th, base_prim_id, l_err, th_err)) + + + +class MPrimFactory: + grid_resolution = 0.025 + th_nb = 16 # heading discretization + th_res = 2*math.pi/th_nb # heading resolution + + def __init__(self, base_prims, grid_resolution=0.025): + MPrimFactory.grid_resolution = grid_resolution + self.base_prims = base_prims + self.nb_mprim_per_angle = len(self.base_prims) + + def build(self, start_angle=0, end_angle=th_nb): + self.mprims = [] + for angle_c in range(start_angle, end_angle): + for bpid, bp in enumerate(self.base_prims): + self.mprims.append(bp['kind'](bpid, angle_c, **bp['params'])) + + def write(self, out_path): + LOG.info(' writing motion primitives to {}'.format(out_path)) + with open(out_path, 'w') as f: + f.write('resolution_m: {:f}\n'.format(MPrimFactory.grid_resolution)) + f.write('numberofangles: {:d}\n'.format(MPrimFactory.th_nb)) + f.write('totalnumberofprimitives: {:d}\n'.format(MPrimFactory.th_nb*self.nb_mprim_per_angle)) + for mprim in self.mprims: + f.write(mprim.to_string()) + + def plot(self, ngrid=50, show_angle=None, show_prim_id=None): + plt.rc('grid', linestyle="-", color='black') + minc, maxc = -ngrid*MPrimFactory.grid_resolution, ngrid*MPrimFactory.grid_resolution + ax = plt.gca() + ax.set_xlim([minc,maxc]); ax.set_ylim([minc,maxc]) + minor_ticks = np.arange(minc, maxc, MPrimFactory.grid_resolution) + major_ticks = np.arange(minc, maxc, 4*MPrimFactory.grid_resolution) + ax.set_xticks(major_ticks); ax.set_yticks(major_ticks) + ax.set_xticks(minor_ticks, minor=True) + ax.set_yticks(minor_ticks, minor=True) + plt.axes().set_aspect('equal') + ax.grid(which='minor', alpha=0.2) + ax.grid(which='major', alpha=0.5) + for p in self.mprims: + if show_angle is None or p.th_c in show_angle: + if show_prim_id is None or p.base_prim_id in show_prim_id: + plt.scatter([p.start_pt[0], p.end_pt[0]], [p.start_pt[1], p.end_pt[1]], marker=(5, 2)) + #plt.scatter([p.start_pt[0], p.end_pt_grid[0]], [p.start_pt[1], p.end_pt_grid[1]]) + plt.plot(p.interm_pts[:,0], p.interm_pts[:,1], '.-') + # check _c + plt.scatter([p.start_pt_c[0]*self.grid_resolution, p.end_pt_c[0]*self.grid_resolution], + [p.start_pt_c[1]*self.grid_resolution, p.end_pt_c[1]*self.grid_resolution], marker=(5, 2)) + # check interm thetas + a_len = 0.1 + for ip in p.interm_pts: + plt.arrow(ip[0], ip[1], a_len * math.cos(ip[2]), a_len * math.sin(ip[2]), + head_width=0.1*a_len, head_length=0.1*a_len, fc='k', ec='k') + + + +def check_all_dirs(f, show_prim_id): + for a in range(MPrimFactory.th_nb): + f.plot(show_angle=[a], show_prim_id=show_prim_id) + plt.show() + + +def gen_oscar_prims(): + base_prims = [ + {'kind':MPrim_line, 'params':{'len_c': 1, 'cost':2}}, # forward straight short + {'kind':MPrim_line, 'params':{'len_c': 8, 'cost':1}}, # forward straight long + #{'kind':MPrim_line, 'params':{'len_c':-1, 'cost':5}}, # backward straight short + {'kind':MPrim_arc, 'params':{'R':0.24, 'dth_c': 1, 'cost':7}}, # forward sharp turning left + {'kind':MPrim_arc, 'params':{'R':0.24, 'dth_c':-1, 'cost':7}}, # forward sharp turning right + {'kind':MPrim_arc, 'params':{'R':0.30, 'dth_c': 1, 'cost':6}}, # forward wide turning left + {'kind':MPrim_arc, 'params':{'R':0.30, 'dth_c':-1, 'cost':6}}, # forward wide turning right + {'kind':MPrim_arc, 'params':{'R':0.50, 'dth_c': 1, 'cost':5}}, # forward wide turning left + {'kind':MPrim_arc, 'params':{'R':0.50, 'dth_c':-1, 'cost':5}}, # forward wide turning right + {'kind':MPrim_arc, 'params':{'R':0.75, 'dth_c': 1, 'cost':4}}, # forward wide turning left + {'kind':MPrim_arc, 'params':{'R':0.75, 'dth_c':-1, 'cost':4}}, # forward wide turning right + {'kind':MPrim_arc, 'params':{'R':1.00, 'dth_c': 1, 'cost':3}}, # forward wide turning left + {'kind':MPrim_arc, 'params':{'R':1.00, 'dth_c':-1, 'cost':3}}, # forward wide turning right + {'kind':MPrim_arc, 'params':{'R':1.25, 'dth_c': 1, 'cost':2}}, # forward wide turning left + {'kind':MPrim_arc, 'params':{'R':1.25, 'dth_c':-1, 'cost':2}}, # forward wide turning right + #{'kind':MPrim_arc, 'params':{'R':-0.15, 'dth_c':-1, 'cost':6}}, # backward turning left + #{'kind':MPrim_arc, 'params':{'R':-0.15, 'dth_c': 1, 'cost':6}} # backward turning right + ] + f = MPrimFactory(base_prims, grid_resolution=0.005) + f.build() + f.write('/home/poine/work/oscar.git/oscar/oscar_navigation/params/sbpl/oscar_3.mprim') + +def gen_julie_prims(): + base_prims = [ + {'kind':MPrim_line, 'params':{'len_c': 1, 'cost':2}}, # forward straight short + {'kind':MPrim_line, 'params':{'len_c': 8, 'cost':1}}, # forward straight long + {'kind':MPrim_line, 'params':{'len_c':-1, 'cost':5}}, # backward straight short + {'kind':MPrim_arc, 'params':{'R':3., 'dth_c': 1, 'cost':3}}, # forward turning left + {'kind':MPrim_arc, 'params':{'R':3., 'dth_c':-1, 'cost':3}}, # forward turning right + {'kind':MPrim_arc, 'params':{'R':-3., 'dth_c':-1, 'cost':6}}, # backward turning left + {'kind':MPrim_arc, 'params':{'R':-3., 'dth_c': 1, 'cost':6}} # backward turning right + ] + f = MPrimFactory(base_prims, grid_resolution=0.025) + +if __name__ == '__main__': + logging.basicConfig(level=logging.INFO) + np.set_printoptions(precision=3, linewidth=300) + # julie + #f = MPrimFactory(grid_resolution=0.025) + # oscar + gen_oscar_prims() + #f = MPrimFactory(grid_resolution=0.005) + + #f.build()#start_angle=0, end_angle=1) + #f.write('/tmp/foo.mprim') + #check_all_dirs(f, show_prim_id=[3]) + #f.plot(show_angle=[15], show_prim_id=[5, 6]) + #plt.show() + + diff --git a/src/guidance b/src/guidance new file mode 120000 index 0000000..fe97e3f --- /dev/null +++ b/src/guidance @@ -0,0 +1 @@ +/home/poine/work/oscar.git/oscar/oscar_control/scripts/guidance \ No newline at end of file diff --git a/src/make_costmap.launch b/src/make_costmap.launch new file mode 100644 index 0000000..798c4c6 --- /dev/null +++ b/src/make_costmap.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/run_sbpl_xytheta.py b/src/run_sbpl_xytheta.py new file mode 100755 index 0000000..01e5579 --- /dev/null +++ b/src/run_sbpl_xytheta.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python + +import os, logging, yaml, matplotlib, matplotlib.image +LOG = logging.getLogger('sbpl_xytheta') +import pdb + +import pysbpl, sbpl_gui as gui, guidance + +class Config: + def __init__(self, **kwargs): + self.env = pysbpl.EnvironmentNAVXYTHETALAT() + self.start_id, self.goal_id = self.env.InitializeEnv(**kwargs) + self.map = kwargs['map'] + print self.start_id, self.goal_id + + +class Planner: + def __init__(self, config): + self.planner = pysbpl.ARAPlanner(config.env) + self.config = config + + def initialize(self, start_id, goal_id): + self.planner.initialize(start_id, goal_id) + + def run(self): + points, headings = self.planner.run() + points += self.config.map.origin[:2] + return points, headings + + +def test_on_ethz_track(): + print('test_on_ethz_track') + if 0: + params = { + 'map': guidance.Map('/home/poine/work/rosmip.git/rosmip/rosmip_worlds/maps/track_ethz_4_costmap.yaml'), + 'perimeter':[[-0.01, -0.04], [0.09, -0.04], [0.09, 0.04], [-0.01, 0.04]], + 'start':[0.165, 1.81, 1.57], + 'goal':[0.165, 1.80, 1.57], # one loop + #'goal':[0.185, 1.55, 1.57], # just on small line + 'goal_tol':[0.05, 0.05, 0.1], + 'vel':0.1, 'time_45_deg':10, + 'mprim_path':'/home/poine/work/oscar.git/oscar/oscar_navigation/params/sbpl/oscar_3.mprim', + } + if 0: + params = { + 'map': guidance.Map('/home/poine/work/rosmip.git/rosmip/rosmip_worlds/maps/track_ethz_dual_costmap_1.yaml'), + 'perimeter':[[-0.01, -0.04], [0.09, -0.04], [0.09, 0.04], [-0.01, 0.04]], + 'start':[2.4, 3.19, 0.], + 'goal':[2.4, 3.18, 0.], # one loop + 'goal_tol':[0.05, 0.05, 0.1], + 'vel':0.1, 'time_45_deg':10, + 'mprim_path':'/home/poine/work/oscar.git/oscar/oscar_navigation/params/sbpl/oscar_3.mprim', + } + if 0: + params = { + #'map': guidance.Map('/home/poine/work/rosmip.git/rosmip/rosmip_worlds/maps/track_oval_02_costmap.yaml'), + 'map': guidance.Map('/home/poine/work/rosmip.git/rosmip/rosmip_worlds/maps/track_slalom_01_costmap.yaml'), + 'perimeter':[[-0.01, -0.04], [0.09, -0.04], [0.09, 0.04], [-0.01, 0.04]], + 'start':[2.41, 0.25, 0.], + 'goal': [2.4, 0.25, 0.], # one loop + 'goal_tol':[0.05, 0.05, 0.1], + 'vel':0.1, 'time_45_deg':10, + 'mprim_path':'/home/poine/work/oscar.git/oscar/oscar_navigation/params/sbpl/oscar_3.mprim', + } + + params = { + 'map': guidance.Map('/home/poine/work/rosmip.git/rosmip/rosmip_worlds/maps/enac_bench/track_test2_costmap.yaml'), + 'perimeter':[[-0.01, -0.04], [0.09, -0.04], [0.09, 0.04], [-0.01, 0.04]], + 'start':[1.21, 0.16, 0.0], + 'goal': [1.20, 0.16, 0.], # one loop + 'goal_tol':[0.05, 0.05, 0.1], + 'vel':0.1, 'time_45_deg':10, + 'mprim_path':'/home/poine/work/oscar.git/oscar/oscar_navigation/params/sbpl/oscar_3.mprim', + } + + c = Config(**params) + p = Planner(c) + p.initialize(c.start_id, c.goal_id) + points, headings = p.run() + _path = guidance.path.Path(points=points, headings=headings) + _path.save('/tmp/foo.npz') + + g = gui.Window() + g.display_map(params['map']) + g.display_path(points) + g.show() + + +def test_on_julie(): + m = guidance.Map('/home/poine/work/simone/trunk/sbpl/test_sbpl/tmp/costmap.yaml') + + params = {'map':m, + 'perimeter':[[-0.01, -0.04], [0.09, -0.04], [0.09, 0.04], [-0.01, 0.04]], + 'start':[0.185, 1.5, 1.57], 'goal':[0.185, 1.49, 1.57], 'goal_tol':[0.01, 0.01, 0.1], + 'vel':0.1, 'time_45_deg':10, + 'mprim_path':'/home/poine/work/simone/trunk/julie/julie/julie_navigation/cfg/sbpl/julie_2.mprim'} + + c = Config(**params) + + p = Planner(c) + p.run() # segfault... fuck!!!! + + g = gui.Window() + g.display_map(m) + g.show() + + + +if __name__ == '__main__': + logging.basicConfig(level=logging.INFO) + #test_on_julie() + test_on_ethz_track() diff --git a/src/save_costmap.py b/src/save_costmap.py new file mode 100755 index 0000000..c032d0c --- /dev/null +++ b/src/save_costmap.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python + +import sys, os, rospy, nav_msgs.msg +import numpy as np, matplotlib, matplotlib.pyplot as plt, scipy.misc +import pdb + +def map_callback(msg): + print('got map {}x{}'.format(msg.info.width, msg.info.height)) + #img = np.flipud(np.array(msg.data).reshape((msg.info.width, msg.info.height))) # this was backwards.... + img = np.flipud(np.array(msg.data).reshape((msg.info.height, msg.info.width))) + #pdb.set_trace() + #plt.imshow(img) #Needs to be in row,col order + #plt.show() + #matplotlib.image.imsave(img_path, img) + scipy.misc.imsave(img_path, img) + yaml_path = os.path.splitext(img_path)[0]+'.yaml' + with open(yaml_path, 'w') as f: + f.write('''image: {} +resolution: {:.3f} +origin: [{}, {}, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 +'''.format(os.path.basename(img_path), msg.info.resolution, msg.info.origin.position.x, msg.info.origin.position.y)) + +if __name__ == '__main__': + topic = '/costmap_node/costmap/costmap' + img_path = sys.argv[1] if len(sys.argv)>1 else './tmp/costmap.png' + rospy.init_node('save_costmap') + rospy.Subscriber(topic, nav_msgs.msg.OccupancyGrid, map_callback, queue_size=1) + rospy.spin() diff --git a/pysbpl/sbpl.pyx b/src/sbpl.pyx similarity index 83% rename from pysbpl/sbpl.pyx rename to src/sbpl.pyx index 16c8c24..5248c16 100644 --- a/pysbpl/sbpl.pyx +++ b/src/sbpl.pyx @@ -49,22 +49,11 @@ cdef extern from "sbpl/discrete_space_information/environment_navxythetalat.h": void GetCoordFromState(int stateID, int& x, int& y, int& theta) void ConvertStateIDPathintoXYThetaPath(vector[int]* stateIDPath, vector[c_sbpl_xy_theta_pt_t]* xythetaPath) bool SetEnvParameter(const char* parameter, int value) - void GetActionsFromStateIDPath(vector[int]* sol , vector[EnvNAVXYTHETALATAction_t]* actions); - cdef struct EnvNAVXYTHETALATAction_t: - char starttheta - char dX - char dY - char endtheta - unsigned int cost - int motprimID - double turning_radius cdef class EnvironmentNAVXYTHETALAT: cdef c_EnvironmentNAVXYTHETALAT *thisptr # hold a C++ instance which we're wrapping - cdef unsigned char* mapdata def __cinit__(self): - self.mapdata = NULL self.thisptr = new c_EnvironmentNAVXYTHETALAT() if self.thisptr is NULL: raise MemoryError() @@ -79,22 +68,30 @@ cdef class EnvironmentNAVXYTHETALAT: self.thisptr.SetEnvParameter("cost_obsthresh", kwargs['obs_thresh']) self.thisptr.SetEnvParameter("cost_inscribed_thresh", kwargs['inscribed_thresh']) self.thisptr.SetEnvParameter("cost_possibly_circumscribed_thresh", kwargs['possibly_circumscribed_thresh']) - + # not sure how to get that without setting them again, or is it alsways 0 and 1 ? cdef double startx = kwargs['start'][0], starty = kwargs['start'][1], starttheta = kwargs['start'][2] cdef double goalx = kwargs['goal'][0], goaly = kwargs['goal'][1], goaltheta = kwargs['goal'][2] start_id = self.thisptr.SetStart(startx, starty, starttheta) goal_id = self.thisptr.SetGoal(goalx, goaly, goaltheta) - return start_id, goal_id + def initialize_from_file(self, filename): print "loading environment from {}".format(filename) self.thisptr.InitializeEnv(filename) def initialize_from_params(self, **kwargs): + _map = kwargs['map'] + cdef int width = _map.width, height = _map.height - cdef int width = kwargs['map'].width, height = kwargs['map'].height + cdef unsigned char* mapdata + mapdata = malloc(width*height*sizeof(unsigned char)) + for i, pixel_row in enumerate(_map.img[::-1]): # same as flipud + for j in range(len(pixel_row)): + mapdata[i*width + j] = int(pixel_row[j]*255) + + cdef double startx = kwargs['start'][0], starty = kwargs['start'][1], starttheta = kwargs['start'][2] cdef double goalx = kwargs['goal'][0], goaly = kwargs['goal'][1], goaltheta = kwargs['goal'][2] cdef goaltol_x = kwargs['goal_tol'][0], goaltol_y = kwargs['goal_tol'][1], goaltol_theta = kwargs['goal_tol'][2] @@ -107,24 +104,16 @@ cdef class EnvironmentNAVXYTHETALAT: cdef double nominalvel_mpersecs = kwargs['vel'], timetoturn45degsinplace_secs = kwargs['time_45_deg'] cdef unsigned char obsthresh = kwargs['obs_thresh'] cdef const char* c_mprim_path = kwargs['mprim_path'] - res = self.thisptr.InitializeEnv(width, height, self.mapdata, + res = self.thisptr.InitializeEnv(width, height, mapdata, startx, starty, starttheta, goalx, goaly, goaltheta, goaltol_x, goaltol_y, goaltol_theta, perimeter, cellsize_m, nominalvel_mpersecs, timetoturn45degsinplace_secs, obsthresh, c_mprim_path) - free(self.mapdata) + free(mapdata) if not res: return [] - - def update_map(self, new_map): - cdef int width = new_map.width, height = new_map.height - self.mapdata = malloc(width*height*sizeof(unsigned char)) - for i, pixel_row in enumerate(new_map.img[::-1]): # same as flipud - for j in range(len(pixel_row)): - self.mapdata[i*width + j] = int(pixel_row[j]*255) - # # SBPLPlanner # @@ -146,7 +135,6 @@ cdef extern from "sbpl/planners/araplanner.h": int replan(double allocated_time_secs, vector[int]* solution_stateIDs_V) cdef class ARAPlanner: - # TODO take planner params out of code cdef c_ARAPlanner *thisptr cdef c_EnvironmentNAVXYTHETALAT *env @@ -156,25 +144,22 @@ cdef class ARAPlanner: self.env = e.thisptr def initialize(self, start_id, goal_id): + print 'in planner initialize' if self.thisptr.set_start(start_id) == 0: print 'set_start failed' if self.thisptr.set_goal(goal_id) == 0: print 'set_goal failed' cdef double initialEpsilon = 3.0 self.thisptr.set_initialsolution_eps(initialEpsilon) - cdef bool bsearchuntilfirstsolution = True; + cdef bool bsearchuntilfirstsolution = False; self.thisptr.set_search_mode(bsearchuntilfirstsolution); def run(self): - cdef double allocated_time = 1. + print 'in planner run' + cdef double allocated_time = 30. cdef vector[int] sol - cdef vector[EnvNAVXYTHETALATAction_t] actions cdef int bRet = self.thisptr.replan(allocated_time, &sol) - if bRet != 1: - print("No solution!") - return None, None - else: - print("Solution Found!") + print 'bret ', bRet # discrete solution cdef vector[int].iterator it = sol.begin() @@ -194,8 +179,6 @@ cdef class ARAPlanner: xys.append([deref(it2).x, deref(it2).y]) thetas.append(deref(it2).theta) inc(it2) - print('Continuous solution has {} values'.format(len(xys))) + print('cont sol has {} values'.format(len(xys))) - # Get actions - self.env.GetActionsFromStateIDPath(&sol, &actions); - return np.array(xys), np.array(thetas), actions + return np.array(xys), np.array(thetas) diff --git a/src/sbpl_gui.py b/src/sbpl_gui.py new file mode 100755 index 0000000..afed817 --- /dev/null +++ b/src/sbpl_gui.py @@ -0,0 +1,393 @@ +#!/usr/bin/env python +import logging, sys, os, math, numpy as np, cv2, gi +gi.require_version('Gtk', '3.0') +from gi.repository import Gtk, GdkPixbuf, GLib, GObject +import matplotlib +from matplotlib.backends.backend_gtk3agg import FigureCanvasGTK3Agg as FigureCanvas +import matplotlib.pyplot as plt +import rospy, geometry_msgs.msg, nav_msgs.msg, tf, threading, subprocess +import pdb +import pysbpl, guidance + +# map_resolution = 0.005 +# #map_origin = [0., -0.3] +# map_origin = [0., 0.] +# map_w, map_h = 370, 500 + +# def world_to_map(prl): return [1, -1]*(prl-map_origin)/map_resolution+[0, map_h] + + +class Node: + def __init__(self, nav_goal_handler=None, **kwargs): + self.nav_goal_handler = nav_goal_handler + self.pub_path = rospy.Publisher('sbpl/path', nav_msgs.msg.Path, queue_size=1) + self.pub_start = rospy.Publisher('/sbpl/start', geometry_msgs.msg.PoseWithCovarianceStamped, queue_size=1) + self.pub_goal = rospy.Publisher('/sbpl/goal', geometry_msgs.msg.PoseWithCovarianceStamped, queue_size=1) + self.path = None + self.start = None + self.goal = None + self.nav_goal_sub = rospy.Subscriber('/move_base_simple/goal', geometry_msgs.msg.PoseStamped, self.on_nav_goal) + self._request_costmap = True + + def publish_path(self): + path_msg = nav_msgs.msg.Path() + path_msg.header.stamp = rospy.Time.now() + path_msg.header.frame_id="map" + for l, y in zip(self.path.points, self.path.headings): + pose = geometry_msgs.msg.PoseStamped() + pose.pose.position.x, pose.pose.position.y = l + o = pose.pose.orientation + o.x, o.y, o.z, o.w = tf.transformations.quaternion_from_euler(*[0, 0, y]) + path_msg.poses.append(pose) + self.pub_path.publish(path_msg) + + + def publish_markers(self): + msg = geometry_msgs.msg.PoseWithCovarianceStamped() + msg.header.frame_id = "map" + msg.header.stamp = rospy.Time.now() + msg.pose.pose.position.x = self.start[0] + msg.pose.pose.position.y = self.start[1] + msg.pose.pose.position.z = 0 + o = msg.pose.pose.orientation + o.x, o.y, o.z, o.w = tf.transformations.quaternion_from_euler(*[0, 0, self.start[2]]) + self.pub_start.publish(msg) + msg.pose.pose.position.x = self.goal[0] + msg.pose.pose.position.y = self.goal[1] + o.x, o.y, o.z, o.w = tf.transformations.quaternion_from_euler(*[0, 0, self.goal[2]]) + self.pub_goal.publish(msg) + + def set_path(self, _path): + self.path = _path + + def set_start(self, x, y, yaw): + self.start = (x, y, yaw) + + def set_goal(self, x, y, yaw): + self.goal = (x, y, yaw) + + def run(self): + rate = rospy.Rate(5.) + try: + while not rospy.is_shutdown(): + self.periodic() + rate.sleep() + except rospy.exceptions.ROSInterruptException: + pass + + def periodic(self): + if self.path is not None: self.publish_path() + if self.start is not None and self.goal is not None: + self.publish_markers() + if self._request_costmap: + self.fetch_costmap() + self._request_costmap = False + + def on_nav_goal(self, msg): + o = msg.pose.orientation; yaw = tf.transformations.euler_from_quaternion([o.x, o.y, o.z, o.w])[2] + GLib.idle_add(self.nav_goal_handler, msg.pose.position.x, msg.pose.position.y, yaw) + + + def fetch_costmap(self): + msg = rospy.wait_for_message('/costmap_node/costmap/costmap', nav_msgs.msg.OccupancyGrid) + #print ('got_costmap', msg) + # header: + # seq: 0 + # stamp: + # secs: 1511352708 + # nsecs: 336167472 + # frame_id: map + # info: + # map_load_time: + # secs: 0 + # nsecs: 0 + # resolution: 0.00499999988824 + # width: 500 + # height: 240 + # origin: + # position: + # x: 0.0 + # y: 0.0 + # z: 0.0 + # orientation: + # x: 0.0 + # y: 0.0 + # z: 0.0 + # w: 1.0 + # data: + o = msg.info.origin.position + kwargs = {'img': np.flipud(np.array(msg.data).reshape((msg.info.height, msg.info.width)))/255., + 'resolution': msg.info.resolution, + 'origin': [o.x, o.y, o.z] + } + self.costmap = guidance.Map(**kwargs) + print 'node got costmap' + + def request_costmap(self): self._request_costmap = True + + def register_nav_goal_handler(self, _handler): + self.nav_goal_handler = _handler + + + +class Model: + def __init__(self, **kwargs): + self.node = Node() + self.map_path = '/home/poine/work/rosmip.git/rosmip/rosmip_worlds/maps/enac_bench/track_test2.yaml' + self.params = { + 'map': guidance.Map(yaml_path='/home/poine/work/rosmip.git/rosmip/rosmip_worlds/maps/enac_bench/track_test2_costmap.yaml'), + 'perimeter':[[-0.01, -0.04], [0.09, -0.04], [0.09, 0.04], [-0.01, 0.04]], + 'start':[1.21, 0.18, 0.0], + 'goal': [1.20, 0.18, 0.], # one loop + 'goal_tol':[0.05, 0.05, 0.1], + 'vel':0.1, 'time_45_deg':10, + 'mprim_path':'/home/poine/work/oscar.git/oscar/oscar_navigation/params/sbpl/oscar_3.mprim', + 'obs_thresh': 100,# 254, #165 # 0.65*255 + 'inscribed_thresh': 90, + 'possibly_circumscribed_thresh': 80#165 + } + self.run_map_server() + self.run_costmap() + + def run_map_server(self): + cmd = 'rosrun map_server map_server {} __name:=map_server'.format(self.map_path) + self.map_server = subprocess.Popen(cmd, shell=True, close_fds=True) + + def run_costmap(self): + costmap_node_name = 'costmap_node' + if 0: + for pn, p in [('plugins', '[]'), + ('publish_frequency', '1')]: + subprocess.call( 'rosparam set /{}/{} {}'.format(costmap_node_name, pn, p), shell=True) + else: + with open('/tmp/tmp_costmap_cfg.yaml', 'w') as f: + f.write(''' +global_frame: /map +robot_base_frame: /base_footprint +transform_tolerance: 0.1 +footprint: [[-0.01, -0.04], [0.09, -0.04], [0.09, 0.04], [-0.01, 0.04]] +plugins: + - {name: static_map, type: "costmap_2d::StaticLayer"} + - {name: inflation, type: "costmap_2d::InflationLayer"} + +inflation: + enabled: true + cost_scaling_factor: 10.0 # exponential rate at which the obstacle cost drops off (default: 10) + inflation_radius: 0.06 #0.065 # max. distance from an obstacle at which costs are incurred for planning paths. + +static: + enabled: true + lethal_cost_threshold: 100 + map_topic: "/map" +''') + + subprocess.call( 'rosparam load -v /tmp/tmp_costmap_cfg.yaml costmap_node/costmap', shell=True) + + cmd = 'rosrun costmap_2d costmap_2d_node __name:={}'.format(costmap_node_name) + self.costmap_node = subprocess.Popen(cmd, shell=True, close_fds=True) + cmd = 'rosrun tf static_transform_publisher {} __name:=map_to_base_footprint_transform'.format('0 0 0 0 0 0 /base_footprint /map 100') + self.static_tf_node = subprocess.Popen(cmd, shell=True, close_fds=True) + + def quit(self): + print 'terminate map server' + self.map_server.terminate() + subprocess.call('rosnode kill /map_server', shell=True) + self.costmap_node.terminate() + subprocess.call('rosnode kill /costmap_node', shell=True) + self.static_tf_node.terminate() + subprocess.call('rosnode kill /map_to_base_footprint_transform', shell=True) + + def run_sbpl(self): + self.env = pysbpl.EnvironmentNAVXYTHETALAT() + self.params['map'] = self.node.costmap + #pdb.set_trace() + self.start_id, self.goal_id = self.env.InitializeEnv(**self.params) + self.planner = pysbpl.ARAPlanner(self.env) + self.planner.initialize(self.start_id, self.goal_id) + + points, headings = self.planner.run() + points += self.params['map'].origin[:2] + self.path = guidance.path.Path(points=points, headings=headings) + self.node.path = self.path + + def save_path(self, file_path): + self.path.save(file_path) + + def load_path(self, file_path): + self.path = guidance.path.Path(load=file_path) + self.node.path = self.path + p0 = [self.path.points[0,0], self.path.points[0,1],self.path.headings[0]] + self.node.set_start(*p0) + p1 = [self.path.points[-1, 0], self.path.points[-1, 1], self.path.headings[-1]] + self.node.set_goal(*p1) + + def register_nav_goal_handler(self, _handler): + self.node.register_nav_goal_handler(_handler) + + def set_goal(self, _p): + self.params['goal'] = _p + self.node.set_goal(*_p) + + def set_start(self, _p): + self.params['start'] = _p + self.node.set_start(*_p) + + + def get_goal(self): return self.node.goal + def get_start(self): return self.node.start + + +# class Window: + +# def __init__(self): +# pass + +# def display_map(self, m): +# plt.imshow(m.img) +# ticks = matplotlib.ticker.FuncFormatter(lambda _x, pos: '{0:g}'.format(_x*m.resolution)) +# plt.gca().xaxis.set_major_formatter(ticks) +# plt.gca().set_xlabel('dimensions in meters') +# plt.gca().yaxis.set_major_formatter(ticks) +# plt.gca().set_ylabel('dimensions in meters') + +# def display_path(self, points): +# map_points = world_to_map(points) +# plt.plot(map_points[:,0], map_points[:,1], marker='.', markersize=10, color='r') + + +# def show(self): +# plt.show() + + +class GUI: + def __init__(self): + self.b = Gtk.Builder() + gui_xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'sbpl_gui.xml') + self.b.add_from_file(gui_xml_path) + self.window = self.b.get_object("window") + self.window.set_title('SBPL') + self.start_entries = [self.b.get_object("entry_start_"+axis) for axis in ['x', 'y', 'yaw']] + self.goal_entries = [self.b.get_object("entry_goal_"+axis) for axis in ['x', 'y', 'yaw']] + self.last_dir = os.getcwd() + self.window.show_all() + + def display_start(self, _p): + for i in range(3): + self.start_entries[i].set_text('{}'.format(_p[i])) + + def display_goal(self, _p): + for i in range(3): + self.goal_entries[i].set_text('{}'.format(_p[i])) + + def request_path(self, action): + dialog = Gtk.FileChooserDialog("Please choose a file", self.window, action, + (Gtk.STOCK_CANCEL, Gtk.ResponseType.CANCEL, + Gtk.STOCK_OPEN, Gtk.ResponseType.OK)) + dialog.set_current_folder(self.last_dir) + ret = dialog.run() + file_path = dialog.get_filename() if ret == Gtk.ResponseType.OK else None + dialog.destroy() + if file_path is not None: + self.last_dir = os.path.dirname(file_path) + return file_path + + + + + +class App: + def __init__(self): + self.gui = GUI() + self.model = Model() + self.register_gui() + self.model.register_nav_goal_handler(self.on_nav_goal) + self.gui.display_start(self.model.params['start']) + self.gui.display_goal(self.model.params['goal']) + + def register_gui(self): + self.gui.window.connect("delete-event", self.quit) + self.gui.b.get_object("button_update_costmap").connect("clicked", self.on_update_costmap_clicked) + self.gui.b.get_object("button_run").connect("clicked", self.on_run_clicked) + self.gui.b.get_object("button_save_path").connect("clicked", self.on_save_path_clicked) + self.gui.b.get_object("button_load_path").connect("clicked", self.on_load_path_clicked) + for i in range(3): + self.gui.start_entries[i].connect("activate", self.on_start_callback) + for i in range(3): + self.gui.goal_entries[i].connect("activate", self.on_goal_callback) + + #self.gui.b.get_object("togglebutton_rec_start").connect("toggled", self.on_rec_start_toggled) + + def on_update_costmap_clicked(self, b): + self.model.node.request_costmap() + + def on_run_clicked(self, button): + self.model.run_sbpl() + + def on_save_path_clicked(self, button): + filename = self.gui.request_path(Gtk.FileChooserAction.SAVE) + if filename is not None: + self.model.save_path(filename) + + def on_load_path_clicked(self, button): + filename = self.gui.request_path(Gtk.FileChooserAction.OPEN) + if filename is not None: + self.model.load_path(filename) + self.gui.display_start(self.model.get_start()) + self.gui.display_goal(self.model.get_goal()) + + def on_start_callback(self, entry): + _p = list(self.model.params['start']) + for i in range(3): + try: + _p[i] = float(self.gui.start_entries[i].get_text()) + except ValueError: + pass + self.set_start(_p) + + def on_goal_callback(self, entry): + _p = list(self.model.params['goal']) + for i in range(3): + try: + _p[i] = float(self.gui.goal_entries[i].get_text()) + except ValueError: + pass + self.set_goal(_p) + + #def on_rec_start_toggled(self, button): + # print 'hello' + + + def on_nav_goal( self, x, y, yaw): + if self.gui.b.get_object("togglebutton_rec_start").get_active(): + self.set_start([x, y, yaw]) + if self.gui.b.get_object("togglebutton_rec_goal").get_active(): + self.set_goal([x, y, yaw]) + + def set_goal(self, _p): + self.model.set_goal(_p) + self.gui.display_goal(_p) + + + def set_start(self, _p): + self.model.set_start(_p) + self.gui.display_start(_p) + + def run(self): + self.ros_thread = threading.Thread(target=self.model.node.run) + self.ros_thread.start() + Gtk.main() + + def quit(self, a, b): + rospy.signal_shutdown("just because") + self.ros_thread.join() + print 'ros thread ended' + self.model.quit() + print 'all dead' + Gtk.main_quit() + + +if __name__ == '__main__': + rospy.init_node('sbpl_gui') + logging.basicConfig(level=logging.INFO) + np.set_printoptions(precision=3, linewidth=300) + App().run() diff --git a/src/sbpl_gui.xml b/src/sbpl_gui.xml new file mode 100644 index 0000000..02397c3 --- /dev/null +++ b/src/sbpl_gui.xml @@ -0,0 +1,308 @@ + + + + + + False + 800 + 600 + + + True + False + + + True + False + vertical + + + True + False + start + + + True + False + Planner + + + True + True + 0 + + + + + Update costmap + True + True + True + + + True + True + 1 + + + + + Run + True + True + True + + + True + True + 2 + + + + + Save path + True + True + True + 0.54000002145767212 + + + True + True + 3 + + + + + Load path + True + True + True + + + True + True + 4 + + + + + False + True + 0 + + + + + True + False + + + True + False + Start + + + 0 + 1 + + + + + True + True + + + 1 + 1 + + + + + True + True + + + 2 + 1 + + + + + True + True + + + 3 + 1 + + + + + True + False + Goal + + + 0 + 2 + + + + + True + True + + + 1 + 2 + + + + + True + True + + + 2 + 2 + + + + + True + True + + + 3 + 2 + + + + + True + False + X + + + 1 + 0 + + + + + True + False + Y + + + 2 + 0 + + + + + True + False + Yaw + + + 3 + 0 + + + + + True + False + ROS navgoal + + + 4 + 0 + + + + + Record + True + True + True + 0.49000000953674316 + + + 4 + 1 + + + + + Record + True + True + True + + + 4 + 2 + + + + + + + + False + True + 1 + + + + + True + True + 0 + + + + + True + False + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + False + True + 1 + + + + + + diff --git a/setup.py b/src/setup_sbpl_ext.py similarity index 51% rename from setup.py rename to src/setup_sbpl_ext.py index be67571..fb3a064 100755 --- a/setup.py +++ b/src/setup_sbpl_ext.py @@ -1,4 +1,6 @@ -import setuptools +from distutils.core import setup +from distutils.extension import Extension +from Cython.Build import cythonize from Cython.Distutils import build_ext ## @@ -17,9 +19,11 @@ def build_extensions(self): # extra_compile_args=["-std=c++11", "-Wno-cpp"] is for removing the deprecation warning # because defining NPY_NO_DEPRECATED_API to NPY_1_7_API_VERSION breaks compilation -ext = setuptools.Extension( - "pysbpl.sbpl", # name of extension - ["pysbpl/sbpl.pyx"], # filename of our Pyrex/Cython source + + +ext = Extension( + "pysbpl", # name of extension + ["sbpl.pyx"], # filename of our Pyrex/Cython source language="c++", # this causes Pyrex/Cython to create C++ source include_dirs=[], extra_compile_args=["-std=c++11", "-Wno-cpp"], @@ -28,26 +32,8 @@ def build_extensions(self): runtime_library_dirs=[], ) -with open("README.md", "r", encoding="utf-8") as fh: - long_description = fh.read() - -setuptools.setup( - name = 'pysbpl', +setup( + name = 'multicol', ext_modules = [ext], - cmdclass = {'build_ext': my_build_ext}, - version="0.0.1", - author="Edited by: Matt Schmittle", - author_email="schmttle@cs.washington.edu", - description="Python bindings for SBPL", - long_description=long_description, - long_description_content_type="text/markdown", - url="https://github.com/schmittlema/pysbpl", - packages=setuptools.find_packages(), - include_package_data=True, - classifiers=[ - "Programming Language :: Python :: 3", - "License :: OSI Approved :: BSD3 License", - "Operating System :: OS Independent", - ], - python_requires='>=3.6', + cmdclass = {'build_ext': my_build_ext} )