From d57b6a085b9cdbfca30c66dac9a384b88879027d Mon Sep 17 00:00:00 2001 From: Matt Schmittle Date: Fri, 19 Mar 2021 07:35:32 -0400 Subject: [PATCH] Revert "Alpysbpl" --- .gitignore | 1 - MANIFEST.in | 3 - README.md | 29 +- example/run_sbpl_xytheta.py | 33 -- params/config.rviz | 229 ++++++++++ pysbpl/__init__.py | 3 - pysbpl/gen_mprim.py | 328 -------------- pysbpl/map_util.py | 23 - pysbpl/maps/sandbox.png | Bin 5551 -> 0 bytes pysbpl/maps/sandbox.yaml | 6 - pysbpl/mprim/pushr.mprim | 723 ------------------------------ pysbpl/planner.py | 44 -- pysbpl/plot.py | 37 -- pysbpl/store_metadata.py | 66 --- requirements.txt | 22 - src/Makefile | 9 + src/gem_mprim.py | 198 ++++++++ src/guidance | 1 + src/make_costmap.launch | 26 ++ src/run_sbpl_xytheta.py | 112 +++++ src/save_costmap.py | 31 ++ {pysbpl => src}/sbpl.pyx | 57 +-- src/sbpl_gui.py | 393 ++++++++++++++++ src/sbpl_gui.xml | 308 +++++++++++++ setup.py => src/setup_sbpl_ext.py | 36 +- 25 files changed, 1339 insertions(+), 1379 deletions(-) delete mode 100644 MANIFEST.in delete mode 100755 example/run_sbpl_xytheta.py create mode 100644 params/config.rviz delete mode 100644 pysbpl/__init__.py delete mode 100755 pysbpl/gen_mprim.py delete mode 100644 pysbpl/map_util.py delete mode 100644 pysbpl/maps/sandbox.png delete mode 100644 pysbpl/maps/sandbox.yaml delete mode 100644 pysbpl/mprim/pushr.mprim delete mode 100755 pysbpl/planner.py delete mode 100755 pysbpl/plot.py delete mode 100755 pysbpl/store_metadata.py delete mode 100644 requirements.txt create mode 100644 src/Makefile create mode 100755 src/gem_mprim.py create mode 120000 src/guidance create mode 100644 src/make_costmap.launch create mode 100755 src/run_sbpl_xytheta.py create mode 100755 src/save_costmap.py rename {pysbpl => src}/sbpl.pyx (83%) create mode 100755 src/sbpl_gui.py create mode 100644 src/sbpl_gui.xml rename setup.py => src/setup_sbpl_ext.py (51%) 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 44ab6c6b99c4421ca00f52330f3003d375c88ace..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 5551 zcmeAS@N?(olHy`uVBq!ia0y~yV15C@96$kvj1%1!3=E=Wo-U3d6^w5WGV(Goa2(!n zcYlV$iHd7%o?n}mR5=FiRahA}Aw=I}mH0h{l|dE{c_(SL3Xd{J!(lWLjAn+>l3=t% z9IXyUtHsea!D!oPv|%{fDjsbWkG6_OTg9WT;?Y*|XsdX%RXo}%9&HsPTE)misVY~a Y7aKkli+WNJ1saL0 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} )