Skip to content

Commit

Permalink
#70 added jupyter notebook for playing around
Browse files Browse the repository at this point in the history
  • Loading branch information
Patrick Schmelter committed Jan 28, 2020
1 parent 02b6de1 commit 82a08d1
Showing 1 changed file with 191 additions and 0 deletions.
191 changes: 191 additions & 0 deletions ros_ws/src/autonomous/wallfollowing2/playground/wallfollowing.ipynb
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,191 @@
{
"nbformat": 4,
"nbformat_minor": 2,
"metadata": {
"language_info": {
"name": "python",
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"version": "3.7.3-final"
},
"orig_nbformat": 2,
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"npconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": 3,
"kernelspec": {
"name": "python37364bitd5f9cec6caaf46a082f83008ecbb7c86",
"display_name": "Python 3.7.3 64-bit"
}
},
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
"# some imports\n",
"import math\n",
"import numpy as np\n",
"from math import atan2\n",
"from collections import namedtuple\n",
"from circle_fit import hyper_fit"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"# We need to define our circle\n",
"\n",
"class Circle():\n",
" def __init__(self, center, radius):\n",
" self.center = center\n",
" self.radius = radius\n",
"\n",
" def create_array(self, start_angle, end_angle, sample_count=50):\n",
" points = np.zeros((sample_count, 2))\n",
" angles = np.linspace(start_angle, end_angle, sample_count)\n",
" points[:, 0] = self.center.x + np.sin(angles) * self.radius\n",
" points[:, 1] = self.center.y + np.cos(angles) * self.radius\n",
" return points\n",
"\n",
" def get_angle(self, point):\n",
" return atan2(point.x - self.center.x, point.y - self.center.y)\n",
"\n",
" def get_closest_point(self, point):\n",
" x = point.x - self.center.x\n",
" y = point.y - self.center.y\n",
" distance = (x**2 + y**2) ** 0.5\n",
" return Point(\n",
" self.center.x + x * self.radius / distance,\n",
" self.center.y + y * self.radius / distance\n",
" )\n",
"\n",
" @staticmethod\n",
" def fit(points):\n",
" center_x, center_y, radius, _ = hyper_fit(points)\n",
" return Circle(Point(center_x, center_y), radius)"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"def get_scan_as_cartesian(laser_scan):\n",
" ranges = np.array(laser_scan.ranges)\n",
"\n",
" angles = np.linspace(\n",
" laser_scan.angle_min,\n",
" laser_scan.angle_max,\n",
" ranges.shape[0])\n",
"\n",
" laser_range = laser_scan.angle_max - laser_scan.angle_min\n",
" usable_range = math.radians(parameters.usable_laser_range) # TODO parameters\n",
" if usable_range < laser_range:\n",
" skip_left = int((-laser_scan.angle_min - usable_range / 2) / laser_range * ranges.shape[0])\n",
" skip_right = int((laser_scan.angle_max - usable_range / 2) / laser_range * ranges.shape[0])\n",
" angles = angles[skip_left:-1 - skip_right]\n",
" ranges = ranges[skip_left:-1 - skip_right]\n",
"\n",
" inf_mask = np.isinf(ranges)\n",
" if inf_mask.any():\n",
" ranges = ranges[~inf_mask]\n",
" angles = angles[~inf_mask]\n",
"\n",
" points = np.zeros((ranges.shape[0], 2))\n",
" points[:, 0] = -np.sin(angles) * ranges\n",
" points[:, 1] = np.cos(angles) * ranges\n",
"\n",
" return points"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"def find_left_right_border(points, margin_relative=0.1):\n",
" margin = int(points.shape[0] * margin_relative)\n",
"\n",
" relative = points[margin + 1:-margin, :] - points[margin:-margin - 1, :]\n",
" distances = np.linalg.norm(relative, axis=1)\n",
"\n",
" return margin + np.argmax(distances) + 1"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [],
"source": [
"def handle_scan(laser_scan, delta_time):\n",
" points = get_scan_as_cartesian(laser_scan)\n",
"\n",
" if points.shape[0] == 0:\n",
" print(\"Skipping current laser scan message since it contains no finite values.\")\n",
" return\n",
"\n",
" split = find_left_right_border(points)\n",
"\n",
" right_wall = points[:split:4, :]\n",
" left_wall = points[split::4, :]\n",
"\n",
" left_circle = Circle.fit(left_wall)\n",
" right_circle = Circle.fit(right_wall)\n",
"\n",
" barrier_start = int(points.shape[0] * (0.5 - parameters.barrier_size_realtive)) # TODO parameters\n",
" barrier_end = int(points.shape[0] * (0.5 + parameters.barrier_size_realtive)) # TODO parameters\n",
" barrier = np.max(points[barrier_start: barrier_end, 1])\n",
"\n",
" follow_walls(left_circle, right_circle, barrier, delta_time)\n",
"\n",
" show_circle_in_rviz(left_circle, left_wall, 0)\n",
" show_circle_in_rviz(right_circle, right_wall, 1)"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [
{
"ename": "NameError",
"evalue": "name 'scan_message' is not defined",
"output_type": "error",
"traceback": [
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[0;31mNameError\u001b[0m Traceback (most recent call last)",
"\u001b[0;32m<ipython-input-6-287091ad213b>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m 3\u001b[0m \u001b[0;31m# what does delta_time do?\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 4\u001b[0m \u001b[0mdelta_time\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;36m0\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 5\u001b[0;31m \u001b[0mhandle_scan\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mscan_message\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mdelta_time\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m",
"\u001b[0;31mNameError\u001b[0m: name 'scan_message' is not defined"
]
}
],
"source": [
"# lastly import sample scan data and start the wallfollowing\n",
"# TODO import sample scan data\n",
"# what does delta_time do?\n",
"delta_time = 0\n",
"handle_scan(scan_message, delta_time)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
]
}

0 comments on commit 82a08d1

Please sign in to comment.