From ebed186312660a21ce99088a1a7cdd09aa5f03cf Mon Sep 17 00:00:00 2001 From: Aryamaan Dhomne Date: Sun, 23 Apr 2023 15:22:24 -0400 Subject: [PATCH 01/15] add local waypoint --- .../.vscode/c_cpp_properties.json | 20 ++ .../amp_local_waypoint/__init__.py | 0 .../amp_local_waypoint/follow_waypoint.py | 184 ++++++++++++++++++ src/amp_local_waypoint/package.xml | 24 +++ .../resource/amp_local_waypoint | 0 src/amp_local_waypoint/setup.cfg | 4 + src/amp_local_waypoint/setup.py | 25 +++ src/amp_local_waypoint/test/test_copyright.py | 23 +++ src/amp_local_waypoint/test/test_flake8.py | 25 +++ src/amp_local_waypoint/test/test_pep257.py | 23 +++ 10 files changed, 328 insertions(+) create mode 100644 src/amp_local_waypoint/.vscode/c_cpp_properties.json create mode 100644 src/amp_local_waypoint/amp_local_waypoint/__init__.py create mode 100644 src/amp_local_waypoint/amp_local_waypoint/follow_waypoint.py create mode 100644 src/amp_local_waypoint/package.xml create mode 100644 src/amp_local_waypoint/resource/amp_local_waypoint create mode 100644 src/amp_local_waypoint/setup.cfg create mode 100644 src/amp_local_waypoint/setup.py create mode 100644 src/amp_local_waypoint/test/test_copyright.py create mode 100644 src/amp_local_waypoint/test/test_flake8.py create mode 100644 src/amp_local_waypoint/test/test_pep257.py diff --git a/src/amp_local_waypoint/.vscode/c_cpp_properties.json b/src/amp_local_waypoint/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..499dede --- /dev/null +++ b/src/amp_local_waypoint/.vscode/c_cpp_properties.json @@ -0,0 +1,20 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/opt/ros/foxy/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-arm64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/src/amp_local_waypoint/amp_local_waypoint/__init__.py b/src/amp_local_waypoint/amp_local_waypoint/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/amp_local_waypoint/amp_local_waypoint/follow_waypoint.py b/src/amp_local_waypoint/amp_local_waypoint/follow_waypoint.py new file mode 100644 index 0000000..82ac599 --- /dev/null +++ b/src/amp_local_waypoint/amp_local_waypoint/follow_waypoint.py @@ -0,0 +1,184 @@ +from math import sqrt +from pickletools import uint8 + +from cv2 import rotate +import rclpy +import numpy as np +from rclpy.node import Node +from nav2_msgs.msg import Costmap +from nav_msgs.msg import Odometry +import cv2 +from geometry_msgs.msg import Quaternion, Point, PoseStamped, PoseWithCovarianceStamped +import scipy.ndimage +from transforms3d.euler import quat2euler, euler2quat + +LETHAL_OBSTACLE = 254 +NO_INFO = 255 +INFLATION_BUFFER = 253 +MEDIUM_COST = 128 +FREE_SPACE = 0 + +COST_IMAGE_MAPPINGS = { + LETHAL_OBSTACLE: 255, + NO_INFO: 0, + INFLATION_BUFFER: 128, + MEDIUM_COST: 192, + FREE_SPACE: 0 +} + + +class CostMapSubscriber(Node): + + initialKartYaw = 0 + currentKartYaw = 0 + currentKartQuat = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + + goal_x = 0 + goal_y = 0 + goal_z = 0 + + def __init__(self): + super().__init__('CostMap_Subscriber') + self.create_subscription(Costmap, '/local_costmap/costmap_raw', + self.costmap_callback, 10) + self.create_subscription(Odometry, '/odom', self.odom_callback, 10) + self.create_subscription(PoseWithCovarianceStamped, '/initial', + self.inital_pose_callback, 10) + self.goal_pose_publisher = self.create_publisher( + PoseStamped, '/goal_pose', 10) + self.goal_timer = self.create_timer(3, self.goal_pose_callback) + + def costmap_callback(self, msg): + costmap = msg.data + costmapWidth = msg.metadata.size_y + costmapHeight = msg.metadata.size_x + costmapResolution = msg.metadata.resolution + xOffset = msg.metadata.origin.position.x + yOffset = msg.metadata.origin.position.y + costmapArray = np.zeros((costmapHeight, costmapWidth), dtype=np.uint8) + costmapIndex = 0 + for i in range(costmapHeight): + for j in range(costmapWidth): + costmapArray[i][j] = COST_IMAGE_MAPPINGS.get( + costmap[costmapIndex], costmap[costmapIndex]) + costmapIndex += 1 + threshCostmap = cv2.threshold(costmapArray, 100, 255, + cv2.THRESH_BINARY_INV)[1] + threshCostmap = cv2.copyMakeBorder(threshCostmap, 1, 1, 1, 1, + cv2.BORDER_CONSTANT, (0)) + # mask1 = np.zeros((costmapWidth, costmapHeight), dtype='uint8') + # mask1[:, 0:int(costmapHeight/2)] = 255 + # mask1 = scipy.ndimage.rotate(mask1, angle=np.rad2deg(self.currentKartYaw - self.initialKartYaw), reshape=False, mode='mirror') + # mask1 = np.tile(np.linspace(0, 1.0, costmapWidth, dtype='float32'), (costmapHeight, 1)) + # mask1 = scipy.ndimage.rotate(mask1, angle=np.rad2deg(self.currentKartYaw - self.initialKartYaw), reshape=False, mode='nearest') + # mask1 = cv2.copyMakeBorder(mask1, 1, 1, 1, 1, cv2.BORDER_CONSTANT, (0)) + # maskedThreshCostmap = np.where(threshCostmap==255, mask1, threshCostmap) + # maskedThreshCostmap = cv2.copyMakeBorder(maskedThreshCostmap, 1, 1, 1, 1, cv2.BORDER_CONSTANT, (0)) + # distImg = cv2.distanceTransform(maskedThreshCostmap, cv2.DIST_L2, 5) + mask = np.zeros((costmapHeight, costmapWidth)) + mask_x_values = np.linspace(-1, 1, costmapWidth * 2) + mask_y_values = (-4 / 5) * ((mask_x_values)**2) + mask_x_indices = np.floor( + ((mask_x_values + 1) / 2) * (costmapWidth - 1)).astype(int) + mask_y_indices = np.floor( + ((mask_y_values + 1) / 2) * (costmapHeight - 1)).astype(int) + mask[mask_y_indices, mask_x_indices] = 1 + for value in zip(mask_y_indices, mask_x_indices): + mask[0:value[0], value[1]] = 1 + mask = scipy.ndimage.rotate( + mask, + angle=np.rad2deg(-1 * + (self.currentKartYaw - self.initialKartYaw)) - 90, + reshape=False, + mode='nearest') + mask = cv2.copyMakeBorder(mask, 1, 1, 1, 1, cv2.BORDER_CONSTANT, (0)) + maskedThershCostmap = (threshCostmap * mask).astype('uint8') + distImg = cv2.distanceTransform(maskedThershCostmap, cv2.DIST_L2, 5) + print(distImg.dtype) + distImgNormalized = distImg.copy() + cv2.normalize(distImg, distImgNormalized, 0, 1.0, cv2.NORM_MINMAX) + distImgDisplay = distImgNormalized.copy() + cv2.normalize(distImgNormalized, distImgDisplay, 0, 1.0, + cv2.NORM_MINMAX) + max_loc = cv2.minMaxLoc(distImgNormalized)[3] + # max_loc_costmap_origin = (max_loc[0]-(costmapWidth/2), max_loc[1]-(costmapHeight/2)) + max_loc_costmap_origin = max_loc + max_loc_costmap_origin_scaled = tuple( + cord * costmapResolution for cord in max_loc_costmap_origin) + max_loc_costmap_origin_offset = (max_loc_costmap_origin_scaled[0] + + xOffset, + max_loc_costmap_origin_scaled[1] + + yOffset) + result = costmapArray.copy() + result = cv2.cvtColor(result, cv2.COLOR_GRAY2BGR) + centx = max_loc[0] + centy = max_loc[1] + # result[centy, centx] = [0, 128, 0] + result = cv2.circle(result, (centx, centy), + 5, (0, 100, 0), + thickness=cv2.FILLED) + self.get_logger().info(f"{[centx, centy]}") + self.get_logger().info(f"{max_loc_costmap_origin_scaled}") + self.get_logger().info(f"{max_loc_costmap_origin_offset}") + self.get_logger().info(f"{msg.metadata.origin}") + distImgDisplay = cv2.flip(distImgDisplay, 1) + threshCostmapDisplay = cv2.flip(threshCostmap, 1) + result = cv2.flip(result, 1) + + self.goal_x = max_loc_costmap_origin_offset[0] + self.goal_y = max_loc_costmap_origin_offset[1] + + # finalSize = (costmapArray.shape[0]*2, costmapArray.shape[1]*2) + # distImgDisplay = cv2.resize(distImgDisplay, finalSize, interpolation=cv2.INTER_NEAREST) + # threshCostmapDisplay = cv2.resize(threshCostmapDisplay, finalSize, interpolation=cv2.INTER_NEAREST) + # result = cv2.resize(result, finalSize, interpolation=cv2.INTER_NEAREST) + cv2.imshow('costmap', result) + cv2.imshow('costmapThresh', threshCostmap) + cv2.imshow('costampDist', distImgDisplay) + cv2.waitKey(1) + + # TODO: restrict to paths that only go "forward" relative to the robot + # NOTE: Rviz rotation flipped, + # NOTE: Resolution is the scale of the points (meters/cell) + # NOTE: FMM planner, scipy + # NOTE: add offsets after scaled up to convert to relative pose + + def odom_callback(self, msg): + orientationQuat = [ + msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, msg.pose.pose.orientation.z + ] + orientationEulers = quat2euler(orientationQuat) + self.currentKartYaw = orientationEulers[2] + # self.get_logger().info(f"yaw: {self.currentKartYaw}") + + def inital_pose_callback(self, msg): + initial_pose_quat = [ + msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, msg.pose.pose.orientation.z + ] + initial_pose_euler = quat2euler(initial_pose_quat) + self.initialKartYaw = initial_pose_euler[2] + # self.get_logger().info(f"yaw: {self.initialKartYasw}") + + def goal_pose_callback(self): + msg = PoseStamped() + msg.pose.position.x = float(self.goal_x) + msg.pose.position.y = float(self.goal_y) + msg.pose.orientation = self.currentKartQuat + self.goal_pose_publisher.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + + subscriber = CostMapSubscriber() + + rclpy.spin(subscriber) + + subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/amp_local_waypoint/package.xml b/src/amp_local_waypoint/package.xml new file mode 100644 index 0000000..a1d8d04 --- /dev/null +++ b/src/amp_local_waypoint/package.xml @@ -0,0 +1,24 @@ + + + + amp_local_waypoint + 0.0.0 + TODO: Package description + arya + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + rclpy + nav2_msgs + libopencv-dev + python3-opencv + python3-scipy + python-transforms3d-pip + + + ament_python + + diff --git a/src/amp_local_waypoint/resource/amp_local_waypoint b/src/amp_local_waypoint/resource/amp_local_waypoint new file mode 100644 index 0000000..e69de29 diff --git a/src/amp_local_waypoint/setup.cfg b/src/amp_local_waypoint/setup.cfg new file mode 100644 index 0000000..8970d4a --- /dev/null +++ b/src/amp_local_waypoint/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/amp_local_waypoint +[install] +install_scripts=$base/lib/amp_local_waypoint diff --git a/src/amp_local_waypoint/setup.py b/src/amp_local_waypoint/setup.py new file mode 100644 index 0000000..381148e --- /dev/null +++ b/src/amp_local_waypoint/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup + +package_name = 'amp_local_waypoint' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='arya', + maintainer_email='arya@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': + ['listener = amp_local_waypoint.follow_waypoint:main'], + }, +) diff --git a/src/amp_local_waypoint/test/test_copyright.py b/src/amp_local_waypoint/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/src/amp_local_waypoint/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/amp_local_waypoint/test/test_flake8.py b/src/amp_local_waypoint/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/amp_local_waypoint/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/amp_local_waypoint/test/test_pep257.py b/src/amp_local_waypoint/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/amp_local_waypoint/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From 8647c0f332532db8a7ab0b43a3a0b9c08f70e808 Mon Sep 17 00:00:00 2001 From: Aryamaan Date: Thu, 27 Apr 2023 18:46:56 -0400 Subject: [PATCH 02/15] rename --- .../amp_local_goal}/__init__.py | 0 .../amp_local_goal}/follow_waypoint.py | 40 +++++++------------ .../package.xml | 2 +- .../resource/amp_local_waypoint | 0 src/amp_local_goal/setup.cfg | 4 ++ .../setup.py | 4 +- .../test/test_copyright.py | 0 .../test/test_flake8.py | 0 .../test/test_pep257.py | 0 .../.vscode/c_cpp_properties.json | 20 ---------- src/amp_local_waypoint/setup.cfg | 4 -- 11 files changed, 21 insertions(+), 53 deletions(-) rename src/{amp_local_waypoint/amp_local_waypoint => amp_local_goal/amp_local_goal}/__init__.py (100%) rename src/{amp_local_waypoint/amp_local_waypoint => amp_local_goal/amp_local_goal}/follow_waypoint.py (76%) rename src/{amp_local_waypoint => amp_local_goal}/package.xml (96%) rename src/{amp_local_waypoint => amp_local_goal}/resource/amp_local_waypoint (100%) create mode 100644 src/amp_local_goal/setup.cfg rename src/{amp_local_waypoint => amp_local_goal}/setup.py (85%) rename src/{amp_local_waypoint => amp_local_goal}/test/test_copyright.py (100%) rename src/{amp_local_waypoint => amp_local_goal}/test/test_flake8.py (100%) rename src/{amp_local_waypoint => amp_local_goal}/test/test_pep257.py (100%) delete mode 100644 src/amp_local_waypoint/.vscode/c_cpp_properties.json delete mode 100644 src/amp_local_waypoint/setup.cfg diff --git a/src/amp_local_waypoint/amp_local_waypoint/__init__.py b/src/amp_local_goal/amp_local_goal/__init__.py similarity index 100% rename from src/amp_local_waypoint/amp_local_waypoint/__init__.py rename to src/amp_local_goal/amp_local_goal/__init__.py diff --git a/src/amp_local_waypoint/amp_local_waypoint/follow_waypoint.py b/src/amp_local_goal/amp_local_goal/follow_waypoint.py similarity index 76% rename from src/amp_local_waypoint/amp_local_waypoint/follow_waypoint.py rename to src/amp_local_goal/amp_local_goal/follow_waypoint.py index 82ac599..500ef00 100644 --- a/src/amp_local_waypoint/amp_local_waypoint/follow_waypoint.py +++ b/src/amp_local_goal/amp_local_goal/follow_waypoint.py @@ -1,23 +1,21 @@ -from math import sqrt -from pickletools import uint8 - -from cv2 import rotate import rclpy import numpy as np from rclpy.node import Node from nav2_msgs.msg import Costmap from nav_msgs.msg import Odometry import cv2 -from geometry_msgs.msg import Quaternion, Point, PoseStamped, PoseWithCovarianceStamped +from geometry_msgs.msg import Quaternion, PoseStamped, PoseWithCovarianceStamped import scipy.ndimage -from transforms3d.euler import quat2euler, euler2quat +from transforms3d.euler import quat2euler +# Costmap values (hardcoded in ROS) LETHAL_OBSTACLE = 254 NO_INFO = 255 INFLATION_BUFFER = 253 MEDIUM_COST = 128 FREE_SPACE = 0 +# Remapping costmap values to custom brightness values in costmap image COST_IMAGE_MAPPINGS = { LETHAL_OBSTACLE: 255, NO_INFO: 0, @@ -39,16 +37,21 @@ class CostMapSubscriber(Node): def __init__(self): super().__init__('CostMap_Subscriber') + # read costmap values, calculate next goal self.create_subscription(Costmap, '/local_costmap/costmap_raw', self.costmap_callback, 10) + # update currentKartYaw self.create_subscription(Odometry, '/odom', self.odom_callback, 10) + # set intialKartYaw self.create_subscription(PoseWithCovarianceStamped, '/initial', self.inital_pose_callback, 10) + # Publish new goal pose for nav2 every 3 seconds self.goal_pose_publisher = self.create_publisher( PoseStamped, '/goal_pose', 10) self.goal_timer = self.create_timer(3, self.goal_pose_callback) def costmap_callback(self, msg): + # Initialize costmap costmap = msg.data costmapWidth = msg.metadata.size_y costmapHeight = msg.metadata.size_x @@ -62,19 +65,14 @@ def costmap_callback(self, msg): costmapArray[i][j] = COST_IMAGE_MAPPINGS.get( costmap[costmapIndex], costmap[costmapIndex]) costmapIndex += 1 + threshCostmap = cv2.threshold(costmapArray, 100, 255, cv2.THRESH_BINARY_INV)[1] threshCostmap = cv2.copyMakeBorder(threshCostmap, 1, 1, 1, 1, cv2.BORDER_CONSTANT, (0)) - # mask1 = np.zeros((costmapWidth, costmapHeight), dtype='uint8') - # mask1[:, 0:int(costmapHeight/2)] = 255 - # mask1 = scipy.ndimage.rotate(mask1, angle=np.rad2deg(self.currentKartYaw - self.initialKartYaw), reshape=False, mode='mirror') - # mask1 = np.tile(np.linspace(0, 1.0, costmapWidth, dtype='float32'), (costmapHeight, 1)) - # mask1 = scipy.ndimage.rotate(mask1, angle=np.rad2deg(self.currentKartYaw - self.initialKartYaw), reshape=False, mode='nearest') - # mask1 = cv2.copyMakeBorder(mask1, 1, 1, 1, 1, cv2.BORDER_CONSTANT, (0)) - # maskedThreshCostmap = np.where(threshCostmap==255, mask1, threshCostmap) - # maskedThreshCostmap = cv2.copyMakeBorder(maskedThreshCostmap, 1, 1, 1, 1, cv2.BORDER_CONSTANT, (0)) - # distImg = cv2.distanceTransform(maskedThreshCostmap, cv2.DIST_L2, 5) + + + mask = np.zeros((costmapHeight, costmapWidth)) mask_x_values = np.linspace(-1, 1, costmapWidth * 2) mask_y_values = (-4 / 5) * ((mask_x_values)**2) @@ -101,7 +99,6 @@ def costmap_callback(self, msg): cv2.normalize(distImgNormalized, distImgDisplay, 0, 1.0, cv2.NORM_MINMAX) max_loc = cv2.minMaxLoc(distImgNormalized)[3] - # max_loc_costmap_origin = (max_loc[0]-(costmapWidth/2), max_loc[1]-(costmapHeight/2)) max_loc_costmap_origin = max_loc max_loc_costmap_origin_scaled = tuple( cord * costmapResolution for cord in max_loc_costmap_origin) @@ -113,7 +110,6 @@ def costmap_callback(self, msg): result = cv2.cvtColor(result, cv2.COLOR_GRAY2BGR) centx = max_loc[0] centy = max_loc[1] - # result[centy, centx] = [0, 128, 0] result = cv2.circle(result, (centx, centy), 5, (0, 100, 0), thickness=cv2.FILLED) @@ -128,20 +124,14 @@ def costmap_callback(self, msg): self.goal_x = max_loc_costmap_origin_offset[0] self.goal_y = max_loc_costmap_origin_offset[1] - # finalSize = (costmapArray.shape[0]*2, costmapArray.shape[1]*2) - # distImgDisplay = cv2.resize(distImgDisplay, finalSize, interpolation=cv2.INTER_NEAREST) - # threshCostmapDisplay = cv2.resize(threshCostmapDisplay, finalSize, interpolation=cv2.INTER_NEAREST) - # result = cv2.resize(result, finalSize, interpolation=cv2.INTER_NEAREST) cv2.imshow('costmap', result) cv2.imshow('costmapThresh', threshCostmap) cv2.imshow('costampDist', distImgDisplay) cv2.waitKey(1) - # TODO: restrict to paths that only go "forward" relative to the robot # NOTE: Rviz rotation flipped, # NOTE: Resolution is the scale of the points (meters/cell) - # NOTE: FMM planner, scipy - # NOTE: add offsets after scaled up to convert to relative pose + # TODO: FMM planner, scipy def odom_callback(self, msg): orientationQuat = [ @@ -150,7 +140,6 @@ def odom_callback(self, msg): ] orientationEulers = quat2euler(orientationQuat) self.currentKartYaw = orientationEulers[2] - # self.get_logger().info(f"yaw: {self.currentKartYaw}") def inital_pose_callback(self, msg): initial_pose_quat = [ @@ -159,7 +148,6 @@ def inital_pose_callback(self, msg): ] initial_pose_euler = quat2euler(initial_pose_quat) self.initialKartYaw = initial_pose_euler[2] - # self.get_logger().info(f"yaw: {self.initialKartYasw}") def goal_pose_callback(self): msg = PoseStamped() diff --git a/src/amp_local_waypoint/package.xml b/src/amp_local_goal/package.xml similarity index 96% rename from src/amp_local_waypoint/package.xml rename to src/amp_local_goal/package.xml index a1d8d04..d88093f 100644 --- a/src/amp_local_waypoint/package.xml +++ b/src/amp_local_goal/package.xml @@ -1,7 +1,7 @@ - amp_local_waypoint + amp_local_goal 0.0.0 TODO: Package description arya diff --git a/src/amp_local_waypoint/resource/amp_local_waypoint b/src/amp_local_goal/resource/amp_local_waypoint similarity index 100% rename from src/amp_local_waypoint/resource/amp_local_waypoint rename to src/amp_local_goal/resource/amp_local_waypoint diff --git a/src/amp_local_goal/setup.cfg b/src/amp_local_goal/setup.cfg new file mode 100644 index 0000000..1def341 --- /dev/null +++ b/src/amp_local_goal/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/amp_local_goal +[install] +install_scripts=$base/lib/amp_local_goal diff --git a/src/amp_local_waypoint/setup.py b/src/amp_local_goal/setup.py similarity index 85% rename from src/amp_local_waypoint/setup.py rename to src/amp_local_goal/setup.py index 381148e..a379c67 100644 --- a/src/amp_local_waypoint/setup.py +++ b/src/amp_local_goal/setup.py @@ -1,6 +1,6 @@ from setuptools import setup -package_name = 'amp_local_waypoint' +package_name = 'amp_local_goal' setup( name=package_name, @@ -20,6 +20,6 @@ tests_require=['pytest'], entry_points={ 'console_scripts': - ['listener = amp_local_waypoint.follow_waypoint:main'], + ['listener = amp_local_goal.follow_waypoint:main'], }, ) diff --git a/src/amp_local_waypoint/test/test_copyright.py b/src/amp_local_goal/test/test_copyright.py similarity index 100% rename from src/amp_local_waypoint/test/test_copyright.py rename to src/amp_local_goal/test/test_copyright.py diff --git a/src/amp_local_waypoint/test/test_flake8.py b/src/amp_local_goal/test/test_flake8.py similarity index 100% rename from src/amp_local_waypoint/test/test_flake8.py rename to src/amp_local_goal/test/test_flake8.py diff --git a/src/amp_local_waypoint/test/test_pep257.py b/src/amp_local_goal/test/test_pep257.py similarity index 100% rename from src/amp_local_waypoint/test/test_pep257.py rename to src/amp_local_goal/test/test_pep257.py diff --git a/src/amp_local_waypoint/.vscode/c_cpp_properties.json b/src/amp_local_waypoint/.vscode/c_cpp_properties.json deleted file mode 100644 index 499dede..0000000 --- a/src/amp_local_waypoint/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,20 +0,0 @@ -{ - "configurations": [ - { - "browse": { - "databaseFilename": "${default}", - "limitSymbolsToIncludedHeaders": false - }, - "includePath": [ - "/opt/ros/foxy/include/**", - "/usr/include/**" - ], - "name": "ROS", - "intelliSenseMode": "gcc-arm64", - "compilerPath": "/usr/bin/gcc", - "cStandard": "gnu11", - "cppStandard": "c++14" - } - ], - "version": 4 -} \ No newline at end of file diff --git a/src/amp_local_waypoint/setup.cfg b/src/amp_local_waypoint/setup.cfg deleted file mode 100644 index 8970d4a..0000000 --- a/src/amp_local_waypoint/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/amp_local_waypoint -[install] -install_scripts=$base/lib/amp_local_waypoint From 26c2aa20199ea05d7e6e1712ba534bcafcc07c0f Mon Sep 17 00:00:00 2001 From: Aryamaan Dhomne Date: Thu, 27 Apr 2023 18:49:03 -0400 Subject: [PATCH 03/15] fix build --- .../resource/{amp_local_waypoint => amp_local_goal} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/amp_local_goal/resource/{amp_local_waypoint => amp_local_goal} (100%) diff --git a/src/amp_local_goal/resource/amp_local_waypoint b/src/amp_local_goal/resource/amp_local_goal similarity index 100% rename from src/amp_local_goal/resource/amp_local_waypoint rename to src/amp_local_goal/resource/amp_local_goal From 134294f0dbe8514442e70d4d3cfc95fe6603a63d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 27 Apr 2023 22:52:54 +0000 Subject: [PATCH 04/15] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/amp_local_goal/amp_local_goal/follow_waypoint.py | 2 -- src/amp_local_goal/setup.py | 3 +-- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/amp_local_goal/amp_local_goal/follow_waypoint.py b/src/amp_local_goal/amp_local_goal/follow_waypoint.py index 500ef00..1b7db49 100644 --- a/src/amp_local_goal/amp_local_goal/follow_waypoint.py +++ b/src/amp_local_goal/amp_local_goal/follow_waypoint.py @@ -70,8 +70,6 @@ def costmap_callback(self, msg): cv2.THRESH_BINARY_INV)[1] threshCostmap = cv2.copyMakeBorder(threshCostmap, 1, 1, 1, 1, cv2.BORDER_CONSTANT, (0)) - - mask = np.zeros((costmapHeight, costmapWidth)) mask_x_values = np.linspace(-1, 1, costmapWidth * 2) diff --git a/src/amp_local_goal/setup.py b/src/amp_local_goal/setup.py index a379c67..e97dbd1 100644 --- a/src/amp_local_goal/setup.py +++ b/src/amp_local_goal/setup.py @@ -19,7 +19,6 @@ license='TODO: License declaration', tests_require=['pytest'], entry_points={ - 'console_scripts': - ['listener = amp_local_goal.follow_waypoint:main'], + 'console_scripts': ['listener = amp_local_goal.follow_waypoint:main'], }, ) From bc049b231a80f99ee7e32c1a8f9b7be017b350e2 Mon Sep 17 00:00:00 2001 From: Aryamaan Dhomne Date: Wed, 3 May 2023 09:55:38 -0400 Subject: [PATCH 05/15] fix test, update maintainer info --- src/amp_local_goal/package.xml | 8 +++----- src/amp_local_goal/setup.py | 8 ++++---- src/amp_local_goal/test/test_copyright.py | 23 --------------------- src/amp_local_goal/test/test_flake8.py | 25 ----------------------- 4 files changed, 7 insertions(+), 57 deletions(-) delete mode 100644 src/amp_local_goal/test/test_copyright.py delete mode 100644 src/amp_local_goal/test/test_flake8.py diff --git a/src/amp_local_goal/package.xml b/src/amp_local_goal/package.xml index d88093f..644ca98 100644 --- a/src/amp_local_goal/package.xml +++ b/src/amp_local_goal/package.xml @@ -3,12 +3,10 @@ amp_local_goal 0.0.0 - TODO: Package description - arya - TODO: License declaration + Calculates local waypoint for the kart and sends it to goal_pose topic + adhomne + Apache-2.0 - ament_copyright - ament_flake8 ament_pep257 python3-pytest rclpy diff --git a/src/amp_local_goal/setup.py b/src/amp_local_goal/setup.py index e97dbd1..09a627d 100644 --- a/src/amp_local_goal/setup.py +++ b/src/amp_local_goal/setup.py @@ -13,10 +13,10 @@ ], install_requires=['setuptools'], zip_safe=True, - maintainer='arya', - maintainer_email='arya@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', + maintainer='adhomne', + maintainer_email='adhomne@purdue.edu', + description='Calculates local waypoint for the kart and sends it to goal_pose topic', + license='Apache-2.0', tests_require=['pytest'], entry_points={ 'console_scripts': ['listener = amp_local_goal.follow_waypoint:main'], diff --git a/src/amp_local_goal/test/test_copyright.py b/src/amp_local_goal/test/test_copyright.py deleted file mode 100644 index cc8ff03..0000000 --- a/src/amp_local_goal/test/test_copyright.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/src/amp_local_goal/test/test_flake8.py b/src/amp_local_goal/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/src/amp_local_goal/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) From 6a3aed55b314c6b744438a675f4dbfea3f1863aa Mon Sep 17 00:00:00 2001 From: Aryamaan Dhomne Date: Wed, 3 May 2023 09:56:37 -0400 Subject: [PATCH 06/15] fix formatting for pre-commit --- src/amp_local_goal/package.xml | 3 ++- src/amp_local_goal/setup.py | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/amp_local_goal/package.xml b/src/amp_local_goal/package.xml index 644ca98..000c04c 100644 --- a/src/amp_local_goal/package.xml +++ b/src/amp_local_goal/package.xml @@ -3,7 +3,8 @@ amp_local_goal 0.0.0 - Calculates local waypoint for the kart and sends it to goal_pose topic + Calculates local waypoint for the kart and sends it to goal_pose topic adhomne Apache-2.0 diff --git a/src/amp_local_goal/setup.py b/src/amp_local_goal/setup.py index 09a627d..5daa81b 100644 --- a/src/amp_local_goal/setup.py +++ b/src/amp_local_goal/setup.py @@ -15,7 +15,8 @@ zip_safe=True, maintainer='adhomne', maintainer_email='adhomne@purdue.edu', - description='Calculates local waypoint for the kart and sends it to goal_pose topic', + description= + 'Calculates local waypoint for the kart and sends it to goal_pose topic', license='Apache-2.0', tests_require=['pytest'], entry_points={ From 872ab02284e5a9ef3f6e1a9888971cde490da989 Mon Sep 17 00:00:00 2001 From: Aryamaan Dhomne Date: Wed, 3 May 2023 15:31:15 -0400 Subject: [PATCH 07/15] Add params & documentation Add params for goal update frequency, parabola coefficient, and showing costmaps. Seperate and comment code. --- .../amp_local_goal/follow_waypoint.py | 68 +++++++++++++------ .../params/local_goal_params.yaml | 6 ++ 2 files changed, 53 insertions(+), 21 deletions(-) create mode 100644 src/amp_local_goal/params/local_goal_params.yaml diff --git a/src/amp_local_goal/amp_local_goal/follow_waypoint.py b/src/amp_local_goal/amp_local_goal/follow_waypoint.py index 1b7db49..b65f5e3 100644 --- a/src/amp_local_goal/amp_local_goal/follow_waypoint.py +++ b/src/amp_local_goal/amp_local_goal/follow_waypoint.py @@ -30,6 +30,9 @@ class CostMapSubscriber(Node): initialKartYaw = 0 currentKartYaw = 0 currentKartQuat = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + parabola_numer = 0 + parabola_denom = 0 + show_costmaps = False goal_x = 0 goal_y = 0 @@ -37,6 +40,12 @@ class CostMapSubscriber(Node): def __init__(self): super().__init__('CostMap_Subscriber') + + goal_update_freq = self.get_parameter("/goal_update_freq") + self.parabola_numer = self.get_parameter("/parabola_numer") + self.parabola_denom = self.get_parameter("/parabola_denom") + self.show_costmaps = self.get_parameter("/show_costmaps") + # read costmap values, calculate next goal self.create_subscription(Costmap, '/local_costmap/costmap_raw', self.costmap_callback, 10) @@ -45,10 +54,11 @@ def __init__(self): # set intialKartYaw self.create_subscription(PoseWithCovarianceStamped, '/initial', self.inital_pose_callback, 10) - # Publish new goal pose for nav2 every 3 seconds + # Publish new goal pose for nav2 with timer self.goal_pose_publisher = self.create_publisher( PoseStamped, '/goal_pose', 10) - self.goal_timer = self.create_timer(3, self.goal_pose_callback) + self.goal_timer = self.create_timer(goal_update_freq, + self.goal_pose_callback) def costmap_callback(self, msg): # Initialize costmap @@ -66,14 +76,18 @@ def costmap_callback(self, msg): costmap[costmapIndex], costmap[costmapIndex]) costmapIndex += 1 + # threshold for costmap values that meet our minimum (atm, just under inflation buffer) threshCostmap = cv2.threshold(costmapArray, 100, 255, cv2.THRESH_BINARY_INV)[1] + # add border around image to disincentivize picking values that are far from the kart threshCostmap = cv2.copyMakeBorder(threshCostmap, 1, 1, 1, 1, cv2.BORDER_CONSTANT, (0)) + # apply parabola mask to image mask = np.zeros((costmapHeight, costmapWidth)) mask_x_values = np.linspace(-1, 1, costmapWidth * 2) - mask_y_values = (-4 / 5) * ((mask_x_values)**2) + mask_y_values = (self.parabola_numer / self.parabola_denom) * ( + (mask_x_values)**2) mask_x_indices = np.floor( ((mask_x_values + 1) / 2) * (costmapWidth - 1)).astype(int) mask_y_indices = np.floor( @@ -89,43 +103,55 @@ def costmap_callback(self, msg): mode='nearest') mask = cv2.copyMakeBorder(mask, 1, 1, 1, 1, cv2.BORDER_CONSTANT, (0)) maskedThershCostmap = (threshCostmap * mask).astype('uint8') + + # Compute Distance Transform distImg = cv2.distanceTransform(maskedThershCostmap, cv2.DIST_L2, 5) - print(distImg.dtype) distImgNormalized = distImg.copy() cv2.normalize(distImg, distImgNormalized, 0, 1.0, cv2.NORM_MINMAX) - distImgDisplay = distImgNormalized.copy() - cv2.normalize(distImgNormalized, distImgDisplay, 0, 1.0, - cv2.NORM_MINMAX) + + # Find point furthest away from all obstacles max_loc = cv2.minMaxLoc(distImgNormalized)[3] + + # scale points to real world distance max_loc_costmap_origin = max_loc max_loc_costmap_origin_scaled = tuple( cord * costmapResolution for cord in max_loc_costmap_origin) + + # move points to current offset of local map from global origin max_loc_costmap_origin_offset = (max_loc_costmap_origin_scaled[0] + xOffset, max_loc_costmap_origin_scaled[1] + yOffset) - result = costmapArray.copy() - result = cv2.cvtColor(result, cv2.COLOR_GRAY2BGR) - centx = max_loc[0] - centy = max_loc[1] - result = cv2.circle(result, (centx, centy), - 5, (0, 100, 0), - thickness=cv2.FILLED) + + # log values self.get_logger().info(f"{[centx, centy]}") self.get_logger().info(f"{max_loc_costmap_origin_scaled}") self.get_logger().info(f"{max_loc_costmap_origin_offset}") self.get_logger().info(f"{msg.metadata.origin}") - distImgDisplay = cv2.flip(distImgDisplay, 1) - threshCostmapDisplay = cv2.flip(threshCostmap, 1) - result = cv2.flip(result, 1) self.goal_x = max_loc_costmap_origin_offset[0] self.goal_y = max_loc_costmap_origin_offset[1] - cv2.imshow('costmap', result) - cv2.imshow('costmapThresh', threshCostmap) - cv2.imshow('costampDist', distImgDisplay) - cv2.waitKey(1) + # display costmaps in GUI + if self.show_costmaps: + # add green point for current goal + result = costmapArray.copy() + result = cv2.cvtColor(result, cv2.COLOR_GRAY2BGR) + centx = max_loc[0] + centy = max_loc[1] + result = cv2.circle(result, (centx, centy), + 5, (0, 100, 0), + thickness=cv2.FILLED) + + # HACK: flip images so they are oriented correctly + distImgDisplay = cv2.flip(distImgNormalized, 1) + threshCostmapDisplay = cv2.flip(threshCostmap, 1) + result = cv2.flip(result, 1) + + cv2.imshow('costmap', result) + cv2.imshow('costmapThresh', threshCostmapDisplay) + cv2.imshow('costampDist', distImgDisplay) + cv2.waitKey(1) # NOTE: Rviz rotation flipped, # NOTE: Resolution is the scale of the points (meters/cell) diff --git a/src/amp_local_goal/params/local_goal_params.yaml b/src/amp_local_goal/params/local_goal_params.yaml new file mode 100644 index 0000000..45c9b85 --- /dev/null +++ b/src/amp_local_goal/params/local_goal_params.yaml @@ -0,0 +1,6 @@ +amp_local_goal: + ros__parameters: + goal_update_freq: 3 + parabola_numer: -4 + parabola_denom: 5 + show_costmaps: true From 89a1125fe9c872a41764ba3f205d7e9b310a3a4c Mon Sep 17 00:00:00 2001 From: Aryamaan Date: Wed, 3 May 2023 18:01:36 -0400 Subject: [PATCH 08/15] Change node name to follow ROS convention Co-authored-by: Xuyang Chen --- src/amp_local_goal/amp_local_goal/follow_waypoint.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/amp_local_goal/amp_local_goal/follow_waypoint.py b/src/amp_local_goal/amp_local_goal/follow_waypoint.py index b65f5e3..569a01e 100644 --- a/src/amp_local_goal/amp_local_goal/follow_waypoint.py +++ b/src/amp_local_goal/amp_local_goal/follow_waypoint.py @@ -39,7 +39,7 @@ class CostMapSubscriber(Node): goal_z = 0 def __init__(self): - super().__init__('CostMap_Subscriber') + super().__init__('costmap_subscriber') goal_update_freq = self.get_parameter("/goal_update_freq") self.parabola_numer = self.get_parameter("/parabola_numer") From 73e49567fa4ceb1483c6f36ee202101b5968172a Mon Sep 17 00:00:00 2001 From: Aryamaan Date: Thu, 4 May 2023 17:42:31 -0500 Subject: [PATCH 09/15] Remove extra newlines Co-authored-by: Alan Chung Ma --- src/amp_local_goal/amp_local_goal/follow_waypoint.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/amp_local_goal/amp_local_goal/follow_waypoint.py b/src/amp_local_goal/amp_local_goal/follow_waypoint.py index 569a01e..94ec0bc 100644 --- a/src/amp_local_goal/amp_local_goal/follow_waypoint.py +++ b/src/amp_local_goal/amp_local_goal/follow_waypoint.py @@ -183,11 +183,8 @@ def goal_pose_callback(self): def main(args=None): rclpy.init(args=args) - subscriber = CostMapSubscriber() - rclpy.spin(subscriber) - subscriber.destroy_node() rclpy.shutdown() From 1ff7da0d73445aca254652ae993a4e66d6a0a18a Mon Sep 17 00:00:00 2001 From: Aryamaan Dhomne Date: Tue, 9 May 2023 15:13:57 +0530 Subject: [PATCH 10/15] Update local goal, add README, remove pytests Add costmap threshold param for local goal, timetsamp for goal pose, docstrings, check for initial_kart_yaw in costmap_callback, update variable names to be consistent. --- src/amp_local_goal/README.md | 7 ++ .../amp_local_goal/follow_waypoint.py | 67 +++++++++++-------- src/amp_local_goal/package.xml | 3 +- .../params/local_goal_params.yaml | 1 + src/amp_local_goal/setup.py | 2 +- 5 files changed, 49 insertions(+), 31 deletions(-) create mode 100644 src/amp_local_goal/README.md diff --git a/src/amp_local_goal/README.md b/src/amp_local_goal/README.md new file mode 100644 index 0000000..abc7ba7 --- /dev/null +++ b/src/amp_local_goal/README.md @@ -0,0 +1,7 @@ +# Local Goal + +Executable: + +## Files + +## Parameters (and default values) diff --git a/src/amp_local_goal/amp_local_goal/follow_waypoint.py b/src/amp_local_goal/amp_local_goal/follow_waypoint.py index 94ec0bc..71a8c49 100644 --- a/src/amp_local_goal/amp_local_goal/follow_waypoint.py +++ b/src/amp_local_goal/amp_local_goal/follow_waypoint.py @@ -27,24 +27,31 @@ class CostMapSubscriber(Node): - initialKartYaw = 0 - currentKartYaw = 0 - currentKartQuat = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + initial_kart_yaw = None + current_kart_yaw = 0 + current_kart_quat = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) parabola_numer = 0 parabola_denom = 0 show_costmaps = False + costmap_threshold = 0 + costmap_timestamp = None + goal_x = 0 goal_y = 0 goal_z = 0 def __init__(self): + """ + Gets params and sets up subscribes/publishes to the required nodes. + """ super().__init__('costmap_subscriber') goal_update_freq = self.get_parameter("/goal_update_freq") self.parabola_numer = self.get_parameter("/parabola_numer") self.parabola_denom = self.get_parameter("/parabola_denom") self.show_costmaps = self.get_parameter("/show_costmaps") + self.costmap_threshold = self.get_parameter("/costmap_threshold") # read costmap values, calculate next goal self.create_subscription(Costmap, '/local_costmap/costmap_raw', @@ -61,6 +68,9 @@ def __init__(self): self.goal_pose_callback) def costmap_callback(self, msg): + if self.initial_kart_yaw == None: + self.get_logger.warn("initial_kart_yaw is None! Not running costmap callback.") + return # Initialize costmap costmap = msg.data costmapWidth = msg.metadata.size_y @@ -77,7 +87,7 @@ def costmap_callback(self, msg): costmapIndex += 1 # threshold for costmap values that meet our minimum (atm, just under inflation buffer) - threshCostmap = cv2.threshold(costmapArray, 100, 255, + threshCostmap = cv2.threshold(costmapArray, self.costmap_threshold, 255, cv2.THRESH_BINARY_INV)[1] # add border around image to disincentivize picking values that are far from the kart threshCostmap = cv2.copyMakeBorder(threshCostmap, 1, 1, 1, 1, @@ -98,7 +108,7 @@ def costmap_callback(self, msg): mask = scipy.ndimage.rotate( mask, angle=np.rad2deg(-1 * - (self.currentKartYaw - self.initialKartYaw)) - 90, + (self.current_kart_yaw - self.initial_kart_yaw)) - 90, reshape=False, mode='nearest') mask = cv2.copyMakeBorder(mask, 1, 1, 1, 1, cv2.BORDER_CONSTANT, (0)) @@ -111,29 +121,19 @@ def costmap_callback(self, msg): # Find point furthest away from all obstacles max_loc = cv2.minMaxLoc(distImgNormalized)[3] + max_loc_global_coords = (((max_loc[0] * costmapResolution) + xOffset), ((max_loc[1] * costmapResolution) + yOffset)) - # scale points to real world distance - max_loc_costmap_origin = max_loc - max_loc_costmap_origin_scaled = tuple( - cord * costmapResolution for cord in max_loc_costmap_origin) + self.get_logger().info(f"Global goal coords: {max_loc_global_coords}") - # move points to current offset of local map from global origin - max_loc_costmap_origin_offset = (max_loc_costmap_origin_scaled[0] + - xOffset, - max_loc_costmap_origin_scaled[1] + - yOffset) - - # log values - self.get_logger().info(f"{[centx, centy]}") - self.get_logger().info(f"{max_loc_costmap_origin_scaled}") - self.get_logger().info(f"{max_loc_costmap_origin_offset}") - self.get_logger().info(f"{msg.metadata.origin}") - - self.goal_x = max_loc_costmap_origin_offset[0] - self.goal_y = max_loc_costmap_origin_offset[1] + # update gloabl vars for publisher + self.goal_x = max_loc_global_coords[0] + self.goal_y = max_loc_global_coords[1] + self.costmap_timestamp = msg.header.stamp # display costmaps in GUI if self.show_costmaps: + self.get_logger().info(f"Plotted goal pixel coords: {[centx, centy]}") + # add green point for current goal result = costmapArray.copy() result = cv2.cvtColor(result, cv2.COLOR_GRAY2BGR) @@ -143,7 +143,7 @@ def costmap_callback(self, msg): 5, (0, 100, 0), thickness=cv2.FILLED) - # HACK: flip images so they are oriented correctly + # HACK: flip images so they are oriented correctly as they are shown in Rviz distImgDisplay = cv2.flip(distImgNormalized, 1) threshCostmapDisplay = cv2.flip(threshCostmap, 1) result = cv2.flip(result, 1) @@ -155,29 +155,40 @@ def costmap_callback(self, msg): # NOTE: Rviz rotation flipped, # NOTE: Resolution is the scale of the points (meters/cell) - # TODO: FMM planner, scipy def odom_callback(self, msg): + """Update the odometry of the kart, to be used for goal pose. + + msg - stamped_pose msg + """ orientationQuat = [ msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z ] orientationEulers = quat2euler(orientationQuat) - self.currentKartYaw = orientationEulers[2] + self.current_kart_yaw = orientationEulers[2] def inital_pose_callback(self, msg): + """Set the initial pose of the kart, once nav2 is initialized. + To be used for updating the rotation of the mask. + + msg - stamped_pose msg + """ initial_pose_quat = [ msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z ] initial_pose_euler = quat2euler(initial_pose_quat) - self.initialKartYaw = initial_pose_euler[2] + self.initial_kart_yaw = initial_pose_euler[2] def goal_pose_callback(self): + """Publish the goal pose to the goal_pose topic based on global vars. + """ msg = PoseStamped() msg.pose.position.x = float(self.goal_x) msg.pose.position.y = float(self.goal_y) - msg.pose.orientation = self.currentKartQuat + msg.pose.orientation = self.current_kart_quat + msg.header.stamp = self.costmap_timestamp self.goal_pose_publisher.publish(msg) diff --git a/src/amp_local_goal/package.xml b/src/amp_local_goal/package.xml index 000c04c..bdd9de2 100644 --- a/src/amp_local_goal/package.xml +++ b/src/amp_local_goal/package.xml @@ -5,11 +5,10 @@ 0.0.0 Calculates local waypoint for the kart and sends it to goal_pose topic - adhomne + Aryamaan Dhomne Apache-2.0 ament_pep257 - python3-pytest rclpy nav2_msgs libopencv-dev diff --git a/src/amp_local_goal/params/local_goal_params.yaml b/src/amp_local_goal/params/local_goal_params.yaml index 45c9b85..8ab98c4 100644 --- a/src/amp_local_goal/params/local_goal_params.yaml +++ b/src/amp_local_goal/params/local_goal_params.yaml @@ -4,3 +4,4 @@ amp_local_goal: parabola_numer: -4 parabola_denom: 5 show_costmaps: true + costmap_threshold: 100 diff --git a/src/amp_local_goal/setup.py b/src/amp_local_goal/setup.py index 5daa81b..bca8222 100644 --- a/src/amp_local_goal/setup.py +++ b/src/amp_local_goal/setup.py @@ -18,7 +18,7 @@ description= 'Calculates local waypoint for the kart and sends it to goal_pose topic', license='Apache-2.0', - tests_require=['pytest'], + tests_require=[], entry_points={ 'console_scripts': ['listener = amp_local_goal.follow_waypoint:main'], }, From ee2aebd721e9759d01ba4ee9507c86493210d83a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 9 May 2023 09:46:52 +0000 Subject: [PATCH 11/15] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/amp_local_goal/README.md | 2 +- .../amp_local_goal/follow_waypoint.py | 23 +++++++++++-------- 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/src/amp_local_goal/README.md b/src/amp_local_goal/README.md index abc7ba7..1e494c9 100644 --- a/src/amp_local_goal/README.md +++ b/src/amp_local_goal/README.md @@ -1,6 +1,6 @@ # Local Goal -Executable: +Executable: ## Files diff --git a/src/amp_local_goal/amp_local_goal/follow_waypoint.py b/src/amp_local_goal/amp_local_goal/follow_waypoint.py index 71a8c49..57b458d 100644 --- a/src/amp_local_goal/amp_local_goal/follow_waypoint.py +++ b/src/amp_local_goal/amp_local_goal/follow_waypoint.py @@ -35,7 +35,6 @@ class CostMapSubscriber(Node): show_costmaps = False costmap_threshold = 0 costmap_timestamp = None - goal_x = 0 goal_y = 0 @@ -43,7 +42,7 @@ class CostMapSubscriber(Node): def __init__(self): """ - Gets params and sets up subscribes/publishes to the required nodes. + Gets params and sets up subscribes/publishes to the required nodes. """ super().__init__('costmap_subscriber') @@ -69,7 +68,8 @@ def __init__(self): def costmap_callback(self, msg): if self.initial_kart_yaw == None: - self.get_logger.warn("initial_kart_yaw is None! Not running costmap callback.") + self.get_logger.warn( + "initial_kart_yaw is None! Not running costmap callback.") return # Initialize costmap costmap = msg.data @@ -87,8 +87,8 @@ def costmap_callback(self, msg): costmapIndex += 1 # threshold for costmap values that meet our minimum (atm, just under inflation buffer) - threshCostmap = cv2.threshold(costmapArray, self.costmap_threshold, 255, - cv2.THRESH_BINARY_INV)[1] + threshCostmap = cv2.threshold(costmapArray, self.costmap_threshold, + 255, cv2.THRESH_BINARY_INV)[1] # add border around image to disincentivize picking values that are far from the kart threshCostmap = cv2.copyMakeBorder(threshCostmap, 1, 1, 1, 1, cv2.BORDER_CONSTANT, (0)) @@ -108,7 +108,8 @@ def costmap_callback(self, msg): mask = scipy.ndimage.rotate( mask, angle=np.rad2deg(-1 * - (self.current_kart_yaw - self.initial_kart_yaw)) - 90, + (self.current_kart_yaw - self.initial_kart_yaw)) - + 90, reshape=False, mode='nearest') mask = cv2.copyMakeBorder(mask, 1, 1, 1, 1, cv2.BORDER_CONSTANT, (0)) @@ -121,18 +122,20 @@ def costmap_callback(self, msg): # Find point furthest away from all obstacles max_loc = cv2.minMaxLoc(distImgNormalized)[3] - max_loc_global_coords = (((max_loc[0] * costmapResolution) + xOffset), ((max_loc[1] * costmapResolution) + yOffset)) + max_loc_global_coords = (((max_loc[0] * costmapResolution) + xOffset), + ((max_loc[1] * costmapResolution) + yOffset)) self.get_logger().info(f"Global goal coords: {max_loc_global_coords}") - # update gloabl vars for publisher + # update global vars for publisher self.goal_x = max_loc_global_coords[0] self.goal_y = max_loc_global_coords[1] self.costmap_timestamp = msg.header.stamp # display costmaps in GUI if self.show_costmaps: - self.get_logger().info(f"Plotted goal pixel coords: {[centx, centy]}") + self.get_logger().info( + f"Plotted goal pixel coords: {[centx, centy]}") # add green point for current goal result = costmapArray.copy() @@ -158,7 +161,7 @@ def costmap_callback(self, msg): def odom_callback(self, msg): """Update the odometry of the kart, to be used for goal pose. - + msg - stamped_pose msg """ orientationQuat = [ From f7bb783199217446bd0d575395ae4956023a9a59 Mon Sep 17 00:00:00 2001 From: Aryamaan Dhomne Date: Wed, 10 May 2023 09:58:59 +0530 Subject: [PATCH 12/15] Update readme --- src/amp_local_goal/README.md | 24 +++++++++++++++++++ .../amp_local_goal/follow_waypoint.py | 1 + 2 files changed, 25 insertions(+) diff --git a/src/amp_local_goal/README.md b/src/amp_local_goal/README.md index 1e494c9..91498ba 100644 --- a/src/amp_local_goal/README.md +++ b/src/amp_local_goal/README.md @@ -4,4 +4,28 @@ Executable: ## Files +. +├── amp_local_goal +│   ├── follow_waypoint.py +│   ├── \_\_init**.py +│   └── \_\_pycache** +│   └── \_\_init**.cpython-38.pyc +├── files.txt +├── package.xml +├── params +│   └── local_goal_params.yaml +├── README.md +├── resource +│   └── amp_local_goal +├── setup.cfg +├── setup.py +└── test +├── \_\_pycache** +│   ├── test_copyright.cpython-38-PYTEST.pyc +│   ├── test_flake8.cpython-38-PYTEST.pyc +│   └── test_pep257.cpython-38-PYTEST.pyc +└── test_pep257.py + ## Parameters (and default values) + +## Topics diff --git a/src/amp_local_goal/amp_local_goal/follow_waypoint.py b/src/amp_local_goal/amp_local_goal/follow_waypoint.py index 57b458d..657b679 100644 --- a/src/amp_local_goal/amp_local_goal/follow_waypoint.py +++ b/src/amp_local_goal/amp_local_goal/follow_waypoint.py @@ -94,6 +94,7 @@ def costmap_callback(self, msg): cv2.BORDER_CONSTANT, (0)) # apply parabola mask to image + # TODO: Update this to use np.meshgrid instead of linspace mask = np.zeros((costmapHeight, costmapWidth)) mask_x_values = np.linspace(-1, 1, costmapWidth * 2) mask_y_values = (self.parabola_numer / self.parabola_denom) * ( From 754cf8248c473cb47953a2382e37b09c45073e39 Mon Sep 17 00:00:00 2001 From: reschivon Date: Fri, 12 May 2023 03:22:44 -0400 Subject: [PATCH 13/15] amp_local_goal: add documentation and README for integration add documentation and README --- src/amp_local_goal/README.md | 48 +++++++++---------- .../params/local_goal_params.yaml | 10 ++-- 2 files changed, 29 insertions(+), 29 deletions(-) diff --git a/src/amp_local_goal/README.md b/src/amp_local_goal/README.md index 91498ba..3ee8bf9 100644 --- a/src/amp_local_goal/README.md +++ b/src/amp_local_goal/README.md @@ -1,31 +1,31 @@ # Local Goal -Executable: +Greedy goal setting algorithm based on maximizing open space in a `CostMap`. + +We don't know ahead of time what the track looks like, so the kart has to compute its next waypoints in real time. Since this stack currently +does not perform accurate global mapping, goal selection is limited to local obstacle data. An extra difficulty is that the track is not fully enclosed by +obstacles but weakly defined by cones (which have large gaps in them). So a free space finding algo, which is not easily affected by gaps, is our solution. ## Files -. -├── amp_local_goal -│   ├── follow_waypoint.py -│   ├── \_\_init**.py -│   └── \_\_pycache** -│   └── \_\_init**.cpython-38.pyc -├── files.txt -├── package.xml -├── params -│   └── local_goal_params.yaml -├── README.md -├── resource -│   └── amp_local_goal -├── setup.cfg -├── setup.py -└── test -├── \_\_pycache** -│   ├── test_copyright.cpython-38-PYTEST.pyc -│   ├── test_flake8.cpython-38-PYTEST.pyc -│   └── test_pep257.cpython-38-PYTEST.pyc -└── test_pep257.py - -## Parameters (and default values) +`amp_local_goal/follow_waypoint.py`: _Node and algorithm implementation_ +`params/local_goal_params.yaml`: _Parameters for the algo with descriptions_ + +## Parameters +See `local_goal_params.yaml` ## Topics + +When a costmap is recieved, the most recent value of `odom` is used without further synchronization. + +Subscribed + +- `/local_costmap/costmap_raw`: nav2_msgs/CostMap: Costmap used for goal selection, including metadata on position and scaling + +- `/odom`: nav2_msgs/Odometry: Used to find the kart's current yaw + +- `/initial`: nav2_msgs/PoseWithCovarianceStamped: Used to find the initial yaw, must be available before algo can start + +Published + +- `/goal_pose`: \ No newline at end of file diff --git a/src/amp_local_goal/params/local_goal_params.yaml b/src/amp_local_goal/params/local_goal_params.yaml index 8ab98c4..caba8d4 100644 --- a/src/amp_local_goal/params/local_goal_params.yaml +++ b/src/amp_local_goal/params/local_goal_params.yaml @@ -1,7 +1,7 @@ amp_local_goal: ros__parameters: - goal_update_freq: 3 - parabola_numer: -4 - parabola_denom: 5 - show_costmaps: true - costmap_threshold: 100 + goal_update_freq: 3 # Frequency to publish goal pose regardless of costmap availability + parabola_numerator: -4 # Goal selection is fixed to a prabolic region in order to bias it to the robot's "front" + parabola_denominator: 5 + show_costmaps: false # Debugging views + costmap_threshold: 100 # Threshold above which costmap voxels are treated as obstacles From 6d140b1f4cc0ce1139150c1cb487d439bfb3ceea Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 12 May 2023 07:23:47 +0000 Subject: [PATCH 14/15] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/amp_local_goal/README.md | 5 +++-- src/amp_local_goal/params/local_goal_params.yaml | 8 ++++---- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/amp_local_goal/README.md b/src/amp_local_goal/README.md index 3ee8bf9..0f954c0 100644 --- a/src/amp_local_goal/README.md +++ b/src/amp_local_goal/README.md @@ -12,11 +12,12 @@ obstacles but weakly defined by cones (which have large gaps in them). So a free `params/local_goal_params.yaml`: _Parameters for the algo with descriptions_ ## Parameters + See `local_goal_params.yaml` ## Topics -When a costmap is recieved, the most recent value of `odom` is used without further synchronization. +When a costmap is received, the most recent value of `odom` is used without further synchronization. Subscribed @@ -28,4 +29,4 @@ Subscribed Published -- `/goal_pose`: \ No newline at end of file +- `/goal_pose`: diff --git a/src/amp_local_goal/params/local_goal_params.yaml b/src/amp_local_goal/params/local_goal_params.yaml index caba8d4..41a9e9c 100644 --- a/src/amp_local_goal/params/local_goal_params.yaml +++ b/src/amp_local_goal/params/local_goal_params.yaml @@ -1,7 +1,7 @@ amp_local_goal: ros__parameters: - goal_update_freq: 3 # Frequency to publish goal pose regardless of costmap availability - parabola_numerator: -4 # Goal selection is fixed to a prabolic region in order to bias it to the robot's "front" + goal_update_freq: 3 # Frequency to publish goal pose regardless of costmap availability + parabola_numerator: -4 # Goal selection is fixed to a prabolic region in order to bias it to the robot's "front" parabola_denominator: 5 - show_costmaps: false # Debugging views - costmap_threshold: 100 # Threshold above which costmap voxels are treated as obstacles + show_costmaps: false # Debugging views + costmap_threshold: 100 # Threshold above which costmap voxels are treated as obstacles From 1862b2e1755d61127847b0e47482ef896ea7a18b Mon Sep 17 00:00:00 2001 From: reschivon Date: Fri, 12 May 2023 03:27:01 -0400 Subject: [PATCH 15/15] amp_lg: change node name to find_local_goal I think I screwed up the suggested changes in the pr so it didnt get changed --- src/amp_local_goal/amp_local_goal/follow_waypoint.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/amp_local_goal/amp_local_goal/follow_waypoint.py b/src/amp_local_goal/amp_local_goal/follow_waypoint.py index 657b679..811f881 100644 --- a/src/amp_local_goal/amp_local_goal/follow_waypoint.py +++ b/src/amp_local_goal/amp_local_goal/follow_waypoint.py @@ -44,7 +44,7 @@ def __init__(self): """ Gets params and sets up subscribes/publishes to the required nodes. """ - super().__init__('costmap_subscriber') + super().__init__('find_local_goal') goal_update_freq = self.get_parameter("/goal_update_freq") self.parabola_numer = self.get_parameter("/parabola_numer")