From 8691b6759ab73f220af2732adc4e217153cc5a5a Mon Sep 17 00:00:00 2001 From: RyanAspen Date: Tue, 19 Sep 2023 19:44:19 -0400 Subject: [PATCH 01/76] Test --- starter_project/autonomy/msg/StarterProjectTag.msg | 4 ++++ starter_project/autonomy/src/perception.cpp | 2 ++ 2 files changed, 6 insertions(+) create mode 100644 starter_project/autonomy/msg/StarterProjectTag.msg diff --git a/starter_project/autonomy/msg/StarterProjectTag.msg b/starter_project/autonomy/msg/StarterProjectTag.msg new file mode 100644 index 000000000..fb3b6f843 --- /dev/null +++ b/starter_project/autonomy/msg/StarterProjectTag.msg @@ -0,0 +1,4 @@ +int32 tagId +float32 xTagCenterPixel +float32 yTagCenterPixel +float32 closenessMetric \ No newline at end of file diff --git a/starter_project/autonomy/src/perception.cpp b/starter_project/autonomy/src/perception.cpp index 986b4a6a4..5df36999d 100644 --- a/starter_project/autonomy/src/perception.cpp +++ b/starter_project/autonomy/src/perception.cpp @@ -51,6 +51,8 @@ namespace mrover { // hint: you have mTagDictionary, mTagCorners, mTagIds, and mTagDetectorParams member variables already defined! // hint: write and use the "getCenterFromTagCorners" and "getClosenessMetricFromTagCorners" functions + cv::aruco::detectMarkers(image, mTagDictionary, mTagCorners, mTagIds, mTagDetectorParams); + tags.clear(); // Clear old tags in output vector // TODO: implement me! From 46a2479b7f84c34ebfac23d73dfb04556fc91310 Mon Sep 17 00:00:00 2001 From: RyanAspen Date: Thu, 21 Sep 2023 19:54:17 -0400 Subject: [PATCH 02/76] First commit --- src/perception/costmap/cost_map.cpp | 32 ++++++++++++ src/perception/costmap/cost_map.hpp | 43 ++++++++++++++++ .../costmap/cost_map.processing.cpp | 49 +++++++++++++++++++ src/perception/costmap/pch.hpp | 5 ++ 4 files changed, 129 insertions(+) create mode 100644 src/perception/costmap/cost_map.cpp create mode 100644 src/perception/costmap/cost_map.hpp create mode 100644 src/perception/costmap/cost_map.processing.cpp create mode 100644 src/perception/costmap/pch.hpp diff --git a/src/perception/costmap/cost_map.cpp b/src/perception/costmap/cost_map.cpp new file mode 100644 index 000000000..ed0565dc0 --- /dev/null +++ b/src/perception/costmap/cost_map.cpp @@ -0,0 +1,32 @@ +#include "cost_map.hpp" + +namespace mrover { + + void CostMapNodelet::onInit() { + mNh = getMTNodeHandle(); + mPnh = getMTPrivateNodeHandle(); + mNh.param("publish_cost_maps", publishCostMaps, true); + mNh.param("verbose", verbose, false); + mNh.param("element_size", element_size, 1); + mNh.param("map_width", map_width, 1); + mNh.param("map_height", map_height, 1); + mNh.param("cutoff", cutoff, 1); + + + //mCostMapPub = mCmt.advertise("cost_maps", 1); + + mPcSub = mNh.subscribe("camera/left/points", 1, &CostMapNodelet::pointCloudCallback, this); + + } +} + +/* +TODO: +- Nodelet takes in pointcloud +PARALLELIZE +- Get cost of point +- Transform point to nav basis +- Stitch to nav grid +END + +*/ \ No newline at end of file diff --git a/src/perception/costmap/cost_map.hpp b/src/perception/costmap/cost_map.hpp new file mode 100644 index 000000000..04d618665 --- /dev/null +++ b/src/perception/costmap/cost_map.hpp @@ -0,0 +1,43 @@ +#include "pch.hpp" + +namespace mrover { + + struct CostMapNode { + cv::Point2f position{}; + double cost = 0; + }; + /* + (2,3) -> 100 + (3,4) -> 0 + (1,1) -> 100 + + -1 -1 -1 -1 -1 + -1 100 -1 -1 -1 + -1 -1 -1 100 -1 + -1 -1 -1 -1 0 + -1 -1 -1 -1 -1 + + */ + + class CostMapNodelet : public nodelet::Nodelet { + private: + ros::NodeHandle mNh, mPnh, mCmt; + ros::Publisher mCostMapPub; + ros::Subscriber mPcSub; + std::vector costMapPointList; + void onInit() override; + + bool publishCostMaps = false; + bool verbose = false; + float element_size = 1; + int map_width = 1; + int map_height = 1; + float cutoff = 1; + + public: + CostMapNodelet() = default; + ~CostMapNodelet() override = default; + void pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg); + + }; +} \ No newline at end of file diff --git a/src/perception/costmap/cost_map.processing.cpp b/src/perception/costmap/cost_map.processing.cpp new file mode 100644 index 000000000..d2f968814 --- /dev/null +++ b/src/perception/costmap/cost_map.processing.cpp @@ -0,0 +1,49 @@ +#include "cost_map.hpp" + +#include "../point.hpp" +#include +#include + +namespace mrover { + + /** + * Calculate the costs of each point in pointcloud + * and stitch to occupancy grid. + * + * @param msg Point cloud message + */ + void CostMapNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg){ + assert(msg); + assert(msg->height > 0); + assert(msg->width > 0); + + auto* pointPtr = reinterpret_cast(msg->data.data()); + // Question: Determine number of points in pointcloud in msg + int size = 0; //TODO + std::for_each(std::execution::par_unseq, pointPtr, pointPtr + size, [&cutoff](Point& point) + { + float curv = point.curvature; + int cost = 0; + if (curv >= cutoff) + { + cost = 100; + } + // Question: How do we get the transformation matrix to get to the correct basis? + + + + }); + + } +} + +/* +TODO: +- Nodelet takes in pointcloud +PARALLELIZE +- Get cost of point +- Transform point to nav basis +- Stitch to nav grid +END + +*/ \ No newline at end of file diff --git a/src/perception/costmap/pch.hpp b/src/perception/costmap/pch.hpp new file mode 100644 index 000000000..c7d149d7a --- /dev/null +++ b/src/perception/costmap/pch.hpp @@ -0,0 +1,5 @@ +#pragma once + +#include +#include +#include \ No newline at end of file From ca4cb072a1ccfaf70c59e3e824e40f6db513e95a Mon Sep 17 00:00:00 2001 From: RyanAspen Date: Tue, 31 Oct 2023 20:21:01 -0400 Subject: [PATCH 03/76] Initial Cost_map commit --- src/perception/cost_map/cost_map.cpp | 32 ++++++++++++ src/perception/cost_map/cost_map.hpp | 43 ++++++++++++++++ .../cost_map/cost_map.processing.cpp | 49 +++++++++++++++++++ src/perception/cost_map/pch.hpp | 5 ++ 4 files changed, 129 insertions(+) create mode 100644 src/perception/cost_map/cost_map.cpp create mode 100644 src/perception/cost_map/cost_map.hpp create mode 100644 src/perception/cost_map/cost_map.processing.cpp create mode 100644 src/perception/cost_map/pch.hpp diff --git a/src/perception/cost_map/cost_map.cpp b/src/perception/cost_map/cost_map.cpp new file mode 100644 index 000000000..ed0565dc0 --- /dev/null +++ b/src/perception/cost_map/cost_map.cpp @@ -0,0 +1,32 @@ +#include "cost_map.hpp" + +namespace mrover { + + void CostMapNodelet::onInit() { + mNh = getMTNodeHandle(); + mPnh = getMTPrivateNodeHandle(); + mNh.param("publish_cost_maps", publishCostMaps, true); + mNh.param("verbose", verbose, false); + mNh.param("element_size", element_size, 1); + mNh.param("map_width", map_width, 1); + mNh.param("map_height", map_height, 1); + mNh.param("cutoff", cutoff, 1); + + + //mCostMapPub = mCmt.advertise("cost_maps", 1); + + mPcSub = mNh.subscribe("camera/left/points", 1, &CostMapNodelet::pointCloudCallback, this); + + } +} + +/* +TODO: +- Nodelet takes in pointcloud +PARALLELIZE +- Get cost of point +- Transform point to nav basis +- Stitch to nav grid +END + +*/ \ No newline at end of file diff --git a/src/perception/cost_map/cost_map.hpp b/src/perception/cost_map/cost_map.hpp new file mode 100644 index 000000000..04d618665 --- /dev/null +++ b/src/perception/cost_map/cost_map.hpp @@ -0,0 +1,43 @@ +#include "pch.hpp" + +namespace mrover { + + struct CostMapNode { + cv::Point2f position{}; + double cost = 0; + }; + /* + (2,3) -> 100 + (3,4) -> 0 + (1,1) -> 100 + + -1 -1 -1 -1 -1 + -1 100 -1 -1 -1 + -1 -1 -1 100 -1 + -1 -1 -1 -1 0 + -1 -1 -1 -1 -1 + + */ + + class CostMapNodelet : public nodelet::Nodelet { + private: + ros::NodeHandle mNh, mPnh, mCmt; + ros::Publisher mCostMapPub; + ros::Subscriber mPcSub; + std::vector costMapPointList; + void onInit() override; + + bool publishCostMaps = false; + bool verbose = false; + float element_size = 1; + int map_width = 1; + int map_height = 1; + float cutoff = 1; + + public: + CostMapNodelet() = default; + ~CostMapNodelet() override = default; + void pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg); + + }; +} \ No newline at end of file diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp new file mode 100644 index 000000000..d2f968814 --- /dev/null +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -0,0 +1,49 @@ +#include "cost_map.hpp" + +#include "../point.hpp" +#include +#include + +namespace mrover { + + /** + * Calculate the costs of each point in pointcloud + * and stitch to occupancy grid. + * + * @param msg Point cloud message + */ + void CostMapNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg){ + assert(msg); + assert(msg->height > 0); + assert(msg->width > 0); + + auto* pointPtr = reinterpret_cast(msg->data.data()); + // Question: Determine number of points in pointcloud in msg + int size = 0; //TODO + std::for_each(std::execution::par_unseq, pointPtr, pointPtr + size, [&cutoff](Point& point) + { + float curv = point.curvature; + int cost = 0; + if (curv >= cutoff) + { + cost = 100; + } + // Question: How do we get the transformation matrix to get to the correct basis? + + + + }); + + } +} + +/* +TODO: +- Nodelet takes in pointcloud +PARALLELIZE +- Get cost of point +- Transform point to nav basis +- Stitch to nav grid +END + +*/ \ No newline at end of file diff --git a/src/perception/cost_map/pch.hpp b/src/perception/cost_map/pch.hpp new file mode 100644 index 000000000..c7d149d7a --- /dev/null +++ b/src/perception/cost_map/pch.hpp @@ -0,0 +1,5 @@ +#pragma once + +#include +#include +#include \ No newline at end of file From f79037bd325f5a68a9d6ae64c9928015d90f116e Mon Sep 17 00:00:00 2001 From: RyanAspen Date: Sun, 5 Nov 2023 11:07:54 -0500 Subject: [PATCH 04/76] Removed costmap folder copy --- src/perception/costmap/cost_map.cpp | 32 ------------ src/perception/costmap/cost_map.hpp | 43 ---------------- .../costmap/cost_map.processing.cpp | 49 ------------------- src/perception/costmap/pch.hpp | 5 -- 4 files changed, 129 deletions(-) delete mode 100644 src/perception/costmap/cost_map.cpp delete mode 100644 src/perception/costmap/cost_map.hpp delete mode 100644 src/perception/costmap/cost_map.processing.cpp delete mode 100644 src/perception/costmap/pch.hpp diff --git a/src/perception/costmap/cost_map.cpp b/src/perception/costmap/cost_map.cpp deleted file mode 100644 index ed0565dc0..000000000 --- a/src/perception/costmap/cost_map.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include "cost_map.hpp" - -namespace mrover { - - void CostMapNodelet::onInit() { - mNh = getMTNodeHandle(); - mPnh = getMTPrivateNodeHandle(); - mNh.param("publish_cost_maps", publishCostMaps, true); - mNh.param("verbose", verbose, false); - mNh.param("element_size", element_size, 1); - mNh.param("map_width", map_width, 1); - mNh.param("map_height", map_height, 1); - mNh.param("cutoff", cutoff, 1); - - - //mCostMapPub = mCmt.advertise("cost_maps", 1); - - mPcSub = mNh.subscribe("camera/left/points", 1, &CostMapNodelet::pointCloudCallback, this); - - } -} - -/* -TODO: -- Nodelet takes in pointcloud -PARALLELIZE -- Get cost of point -- Transform point to nav basis -- Stitch to nav grid -END - -*/ \ No newline at end of file diff --git a/src/perception/costmap/cost_map.hpp b/src/perception/costmap/cost_map.hpp deleted file mode 100644 index 04d618665..000000000 --- a/src/perception/costmap/cost_map.hpp +++ /dev/null @@ -1,43 +0,0 @@ -#include "pch.hpp" - -namespace mrover { - - struct CostMapNode { - cv::Point2f position{}; - double cost = 0; - }; - /* - (2,3) -> 100 - (3,4) -> 0 - (1,1) -> 100 - - -1 -1 -1 -1 -1 - -1 100 -1 -1 -1 - -1 -1 -1 100 -1 - -1 -1 -1 -1 0 - -1 -1 -1 -1 -1 - - */ - - class CostMapNodelet : public nodelet::Nodelet { - private: - ros::NodeHandle mNh, mPnh, mCmt; - ros::Publisher mCostMapPub; - ros::Subscriber mPcSub; - std::vector costMapPointList; - void onInit() override; - - bool publishCostMaps = false; - bool verbose = false; - float element_size = 1; - int map_width = 1; - int map_height = 1; - float cutoff = 1; - - public: - CostMapNodelet() = default; - ~CostMapNodelet() override = default; - void pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg); - - }; -} \ No newline at end of file diff --git a/src/perception/costmap/cost_map.processing.cpp b/src/perception/costmap/cost_map.processing.cpp deleted file mode 100644 index d2f968814..000000000 --- a/src/perception/costmap/cost_map.processing.cpp +++ /dev/null @@ -1,49 +0,0 @@ -#include "cost_map.hpp" - -#include "../point.hpp" -#include -#include - -namespace mrover { - - /** - * Calculate the costs of each point in pointcloud - * and stitch to occupancy grid. - * - * @param msg Point cloud message - */ - void CostMapNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg){ - assert(msg); - assert(msg->height > 0); - assert(msg->width > 0); - - auto* pointPtr = reinterpret_cast(msg->data.data()); - // Question: Determine number of points in pointcloud in msg - int size = 0; //TODO - std::for_each(std::execution::par_unseq, pointPtr, pointPtr + size, [&cutoff](Point& point) - { - float curv = point.curvature; - int cost = 0; - if (curv >= cutoff) - { - cost = 100; - } - // Question: How do we get the transformation matrix to get to the correct basis? - - - - }); - - } -} - -/* -TODO: -- Nodelet takes in pointcloud -PARALLELIZE -- Get cost of point -- Transform point to nav basis -- Stitch to nav grid -END - -*/ \ No newline at end of file diff --git a/src/perception/costmap/pch.hpp b/src/perception/costmap/pch.hpp deleted file mode 100644 index c7d149d7a..000000000 --- a/src/perception/costmap/pch.hpp +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once - -#include -#include -#include \ No newline at end of file From 01cca13fb21a3e5bd9376e168858cc2d15632998 Mon Sep 17 00:00:00 2001 From: RyanAspen Date: Sun, 5 Nov 2023 11:48:57 -0500 Subject: [PATCH 05/76] Initial README.md --- src/perception/cost_map/README.md | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 src/perception/cost_map/README.md diff --git a/src/perception/cost_map/README.md b/src/perception/cost_map/README.md new file mode 100644 index 000000000..2e0587a98 --- /dev/null +++ b/src/perception/cost_map/README.md @@ -0,0 +1,14 @@ +(WIP) + +### Code Layout + +- [cost_map.cpp](./cost_map.cpp) Mainly ROS node setup (topics, parameters, etc.) +- [cost_map.processing.cpp](./cost_map.processing.cpp) Processes pointclouds to continuously update the global terrain cost map + +### Explanation + +The goal of this code is to take in pointcloud data from the zed and output a cost map / occupancy grid that represents the difficulty of passing terrain. The cost at a particular location is found by the following function: + +$ c(x) + +To prevent erratic planning, \ No newline at end of file From c61599b3a97eccef69fe2be76e2f8dccf057acf4 Mon Sep 17 00:00:00 2001 From: Ryan Aspenleiter Date: Sun, 5 Nov 2023 12:02:23 -0500 Subject: [PATCH 06/76] Update README.md --- src/perception/cost_map/README.md | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/perception/cost_map/README.md b/src/perception/cost_map/README.md index 2e0587a98..fd5be929b 100644 --- a/src/perception/cost_map/README.md +++ b/src/perception/cost_map/README.md @@ -7,8 +7,10 @@ ### Explanation -The goal of this code is to take in pointcloud data from the zed and output a cost map / occupancy grid that represents the difficulty of passing terrain. The cost at a particular location is found by the following function: +The goal of this code is to take in pointcloud data from the zed and output a cost map / occupancy grid that represents the difficulty of passing terrain. -$ c(x) +When a new pointcloud is received, the nodelet first calculates the costs associated with each point in the pointcloud. The cost at a particular location is 100 if the curvature estimate is above our threshold, 0 if it is not, or -1 if there is no data for that location. This discreteness is helpful for the navigation functions using this costmap, as it minimizes erratic movement to account for new data. -To prevent erratic planning, \ No newline at end of file +After costs are calculated, each point's location is transformed from the local basis (relative to the rover) to the gobal basis (relative to the starting location). + +Processed cost point data is then used to update the rover's costmap. The resolution (size of each grid space in physical units), grid width (in physical units), and grid height (in physical units) is used to determine the size of the costmap and to which grid space a given cost point is used to update. For simplicity, the new grid space's cost is equal to the maximum cost among the point cloud points that fall within the grid space. The cost map is then published for use by navigation nodelets. From c3bd12f349cd374e825e8bb2e4333f413d711855 Mon Sep 17 00:00:00 2001 From: Ryan Aspenleiter Date: Sun, 5 Nov 2023 12:02:38 -0500 Subject: [PATCH 07/76] Update README.md --- src/perception/cost_map/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/perception/cost_map/README.md b/src/perception/cost_map/README.md index fd5be929b..ce4b7edfb 100644 --- a/src/perception/cost_map/README.md +++ b/src/perception/cost_map/README.md @@ -11,6 +11,6 @@ The goal of this code is to take in pointcloud data from the zed and output a co When a new pointcloud is received, the nodelet first calculates the costs associated with each point in the pointcloud. The cost at a particular location is 100 if the curvature estimate is above our threshold, 0 if it is not, or -1 if there is no data for that location. This discreteness is helpful for the navigation functions using this costmap, as it minimizes erratic movement to account for new data. -After costs are calculated, each point's location is transformed from the local basis (relative to the rover) to the gobal basis (relative to the starting location). +After costs are calculated, each point's location is transformed from the local basis (relative to the rover) to the global basis (relative to the starting location). Processed cost point data is then used to update the rover's costmap. The resolution (size of each grid space in physical units), grid width (in physical units), and grid height (in physical units) is used to determine the size of the costmap and to which grid space a given cost point is used to update. For simplicity, the new grid space's cost is equal to the maximum cost among the point cloud points that fall within the grid space. The cost map is then published for use by navigation nodelets. From 6eb8d2b0984ac3d42dc1b9367470b9495d9c5c1a Mon Sep 17 00:00:00 2001 From: Ryan Aspenleiter Date: Thu, 9 Nov 2023 19:10:26 -0500 Subject: [PATCH 08/76] Update README.md to account for clarification on project specifications --- src/perception/cost_map/README.md | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/perception/cost_map/README.md b/src/perception/cost_map/README.md index ce4b7edfb..f424670ea 100644 --- a/src/perception/cost_map/README.md +++ b/src/perception/cost_map/README.md @@ -3,14 +3,19 @@ ### Code Layout - [cost_map.cpp](./cost_map.cpp) Mainly ROS node setup (topics, parameters, etc.) -- [cost_map.processing.cpp](./cost_map.processing.cpp) Processes pointclouds to continuously update the global terrain cost map +- [cost_map.processing.cpp](./cost_map.processing.cpp) Processes ~~pointclouds~~ occupancy grids to continuously update the global terrain occupancy grid ### Explanation -The goal of this code is to take in pointcloud data from the zed and output a cost map / occupancy grid that represents the difficulty of passing terrain. +The goal of this code is to take in a local occupancy grid / costmap and output a global occupancy grid. -When a new pointcloud is received, the nodelet first calculates the costs associated with each point in the pointcloud. The cost at a particular location is 100 if the curvature estimate is above our threshold, 0 if it is not, or -1 if there is no data for that location. This discreteness is helpful for the navigation functions using this costmap, as it minimizes erratic movement to account for new data. +~~The goal of this code is to take in pointcloud data from the zed and output a cost map / occupancy grid that represents the difficulty of passing terrain.~~ -After costs are calculated, each point's location is transformed from the local basis (relative to the rover) to the global basis (relative to the starting location). +When a new local occupancy grid is received, the nodelet transforms it into the global frame using tf_tree information about the rover's frame relative to a chosen origin. The resolution (size of each grid space in physical units), grid width (in physical units), and grid height (in physical units) is used to determine the size of the costmap and to which grid space a given cost point is used to update. For simplicity, the new grid space's cost is equal to the maximum cost among the point cloud points that fall within the grid space. The cost map is then published for use by navigation nodelets. + +~~When a new pointcloud is received, the nodelet first calculates the costs associated with each point in the pointcloud. The cost at a particular location is 100 if the curvature estimate is above our threshold, 0 if it is not, or -1 if there is no data for that location. This discreteness is helpful for the navigation functions using this costmap, as it minimizes erratic movement to account for new data.~~ + +~~After costs are calculated, each point's location is transformed from the local basis (relative to the rover) to the global basis (relative to the starting location).~~ + +~~Processed cost point data is then used to update the rover's costmap. The resolution (size of each grid space in physical units), grid width (in physical units), and grid height (in physical units) is used to determine the size of the costmap and to which grid space a given cost point is used to update. For simplicity, the new grid space's cost is equal to the maximum cost among the point cloud points that fall within the grid space. The cost map is then published for use by navigation nodelets.~~ -Processed cost point data is then used to update the rover's costmap. The resolution (size of each grid space in physical units), grid width (in physical units), and grid height (in physical units) is used to determine the size of the costmap and to which grid space a given cost point is used to update. For simplicity, the new grid space's cost is equal to the maximum cost among the point cloud points that fall within the grid space. The cost map is then published for use by navigation nodelets. From d945109a8ff270e3bec0e480e9d20632b82606eb Mon Sep 17 00:00:00 2001 From: Ryan Aspenleiter Date: Thu, 9 Nov 2023 19:23:28 -0500 Subject: [PATCH 09/76] Added clarification --- src/perception/cost_map/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/perception/cost_map/README.md b/src/perception/cost_map/README.md index f424670ea..b5046ea95 100644 --- a/src/perception/cost_map/README.md +++ b/src/perception/cost_map/README.md @@ -7,7 +7,7 @@ ### Explanation -The goal of this code is to take in a local occupancy grid / costmap and output a global occupancy grid. +The goal of this code is to take in a local occupancy grid / costmap (using http://docs.ros.org/en/melodic/api/nav_msgs/html/msg/OccupancyGrid.html) and output a global occupancy grid (again using http://docs.ros.org/en/melodic/api/nav_msgs/html/msg/OccupancyGrid.html). ~~The goal of this code is to take in pointcloud data from the zed and output a cost map / occupancy grid that represents the difficulty of passing terrain.~~ From 7c2c227db773c390aa3b878334d2018c755da52b Mon Sep 17 00:00:00 2001 From: Prabhleen Pawar Date: Sun, 12 Nov 2023 19:02:33 +0000 Subject: [PATCH 10/76] Created water_bottle_search.py --- src/navigation/water_bottle_search.py | 80 +++++++++++++++++++++++++++ 1 file changed, 80 insertions(+) create mode 100644 src/navigation/water_bottle_search.py diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py new file mode 100644 index 000000000..a3476aa1b --- /dev/null +++ b/src/navigation/water_bottle_search.py @@ -0,0 +1,80 @@ +from __future__ import annotations + +from dataclasses import dataclass +from typing import Optional + +import numpy as np +from mrover.msg import GPSPointList + +from util.ros_utils import get_rosparam +from util.state_lib.state import State + +from navigation import approach_post, recovery, waypoint +from navigation.context import convert_cartesian_to_gps +from navigation.trajectory import Trajectory +from navigation.search import SearchTrajectory + +# TODO: Right now this is a copy of search state, we will need to change this + +class WaterBottleSearchState(State): + traj: SearchTrajectory + prev_target: Optional[np.ndarray] = None + is_recovering: bool = False + + # TODO: add rosparam names to waterbottlesearch/... in the navigation.yaml file + STOP_THRESH = get_rosparam("search/stop_thresh", 0.2) + DRIVE_FWD_THRESH = get_rosparam("search/drive_fwd_thresh", 0.34) + SPIRAL_COVERAGE_RADIUS = get_rosparam("search/coverage_radius", 10) + SEGMENTS_PER_ROTATION = get_rosparam("search/segments_per_rotation", 8) # TODO: after testing, might need to change + DISTANCE_BETWEEN_SPIRALS = get_rosparam("search/distance_between_spirals", 1.25) # TODO: after testing, might need to change + + + # TODO: Make call_back function to push into data structure + + + def on_enter(self, context) -> None: + search_center = context.course.current_waypoint() + if not self.is_recovering: + self.traj = SearchTrajectory.spiral_traj( + context.rover.get_pose().position[0:2], + self.SPIRAL_COVERAGE_RADIUS, + self.DISTANCE_BETWEEN_SPIRALS, + self.SEGMENTS_PER_ROTATION, + search_center.fiducial_id, + ) + self.prev_target = None + + def on_exit(self, context) -> None: + pass + + def on_loop(self, context) -> State: + # continue executing the path from wherever it left off + target_pos = self.traj.get_cur_pt() + cmd_vel, arrived = context.rover.driver.get_drive_command( + target_pos, + context.rover.get_pose(), + self.STOP_THRESH, + self.DRIVE_FWD_THRESH, + path_start=self.prev_target, + ) + if arrived: + self.prev_target = target_pos + # if we finish the spiral without seeing the fiducial, move on with course + if self.traj.increment_point(): + return waypoint.WaypointState() + + if context.rover.stuck: + context.rover.previous_state = self + self.is_recovering = True + return recovery.RecoveryState() + else: + self.is_recovering = False + + context.search_point_publisher.publish( + GPSPointList([convert_cartesian_to_gps(pt) for pt in self.traj.coordinates]) + ) + context.rover.send_drive_command(cmd_vel) + + if context.env.current_fid_pos() is not None and context.course.look_for_post(): + return approach_post.ApproachPostState() + return self From 6d084896b5ec9f0a63cb68ab9b6b579a3353d86b Mon Sep 17 00:00:00 2001 From: RyanAspen Date: Tue, 14 Nov 2023 18:57:26 -0500 Subject: [PATCH 11/76] Updated code for new project scope --- src/perception/cost_map/cost_map.cpp | 6 +- src/perception/cost_map/cost_map.hpp | 7 ++- .../cost_map/cost_map.processing.cpp | 58 +++++++------------ 3 files changed, 28 insertions(+), 43 deletions(-) diff --git a/src/perception/cost_map/cost_map.cpp b/src/perception/cost_map/cost_map.cpp index ed0565dc0..2dce1026f 100644 --- a/src/perception/cost_map/cost_map.cpp +++ b/src/perception/cost_map/cost_map.cpp @@ -1,4 +1,5 @@ #include "cost_map.hpp" +#include namespace mrover { @@ -11,14 +12,13 @@ namespace mrover { mNh.param("map_width", map_width, 1); mNh.param("map_height", map_height, 1); mNh.param("cutoff", cutoff, 1); - + //mCostMapPub = mCmt.advertise("cost_maps", 1); mPcSub = mNh.subscribe("camera/left/points", 1, &CostMapNodelet::pointCloudCallback, this); - } -} +} // namespace mrover /* TODO: diff --git a/src/perception/cost_map/cost_map.hpp b/src/perception/cost_map/cost_map.hpp index 04d618665..9a99de951 100644 --- a/src/perception/cost_map/cost_map.hpp +++ b/src/perception/cost_map/cost_map.hpp @@ -1,3 +1,4 @@ +#include "nav_msgs/OccupancyGrid.h" #include "pch.hpp" namespace mrover { @@ -33,11 +34,11 @@ namespace mrover { int map_width = 1; int map_height = 1; float cutoff = 1; + const tf2_ros::Buffer tf_buffer = tf2_ros::Buffer(); public: CostMapNodelet() = default; ~CostMapNodelet() override = default; - void pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg); - + void occupancyGridCallback(nav_msgs::OccupancyGrid const& msg); }; -} \ No newline at end of file +} // namespace mrover \ No newline at end of file diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index d2f968814..af492d38a 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -1,49 +1,33 @@ #include "cost_map.hpp" - #include "../point.hpp" + #include #include +#include namespace mrover { /** - * Calculate the costs of each point in pointcloud - * and stitch to occupancy grid. + * Stitches local occupancy grid to global occupancy grid * - * @param msg Point cloud message + * @param msg Local Occupancy Grid Mesasge */ - void CostMapNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg){ - assert(msg); - assert(msg->height > 0); - assert(msg->width > 0); - - auto* pointPtr = reinterpret_cast(msg->data.data()); - // Question: Determine number of points in pointcloud in msg - int size = 0; //TODO - std::for_each(std::execution::par_unseq, pointPtr, pointPtr + size, [&cutoff](Point& point) - { - float curv = point.curvature; - int cost = 0; - if (curv >= cutoff) - { - cost = 100; - } - // Question: How do we get the transformation matrix to get to the correct basis? - - + void CostMapNodelet::occupancyGridCallback(nav_msgs::OccupancyGrid const& msg) { + + // Make sure message is valid + assert(msg.info.width > 0); + assert(msg.info.height > 0); + assert(msg.info.resolution > 0); + + // Using tf_tree, get the transformation + + SE3 base_to_map = SE3::fromTfTree(tf_buffer, "base_link", "map"); + SE3 zed_to_base = SE3::fromTfTree(tf_buffer, "zed2i_left_camera_frame", "base_link"); + Eigen::Matrix4d transformMatrix = zed_to_base.matrix() * base_to_map.matrix(); // This is the matrix transform from zed to the map + + auto* pointPtr = &msg.data; + std::for_each(std::execution::par_unseq, &msg.data, &msg.data + msg.info.width, [&](int& point) { }); - - } -} - -/* -TODO: -- Nodelet takes in pointcloud -PARALLELIZE -- Get cost of point -- Transform point to nav basis -- Stitch to nav grid -END - -*/ \ No newline at end of file + } +} // namespace mrover From db012d21dccb11b5f7c7929314d7bd04b080e517 Mon Sep 17 00:00:00 2001 From: RyanAspen Date: Thu, 16 Nov 2023 19:18:54 -0500 Subject: [PATCH 12/76] Added more complete logic for stitching --- src/perception/cost_map/cost_map.cpp | 22 +++++++-- src/perception/cost_map/cost_map.hpp | 32 +++--------- .../cost_map/cost_map.processing.cpp | 49 ++++++++++++++++++- 3 files changed, 72 insertions(+), 31 deletions(-) diff --git a/src/perception/cost_map/cost_map.cpp b/src/perception/cost_map/cost_map.cpp index 2dce1026f..3e30f1a92 100644 --- a/src/perception/cost_map/cost_map.cpp +++ b/src/perception/cost_map/cost_map.cpp @@ -1,22 +1,34 @@ #include "cost_map.hpp" +#include #include namespace mrover { void CostMapNodelet::onInit() { mNh = getMTNodeHandle(); - mPnh = getMTPrivateNodeHandle(); + mPnh = getMTPrivateNodeHandle(); // Unused for now mNh.param("publish_cost_maps", publishCostMaps, true); mNh.param("verbose", verbose, false); - mNh.param("element_size", element_size, 1); + mNh.param("resolution", resolution, 1); mNh.param("map_width", map_width, 1); mNh.param("map_height", map_height, 1); - mNh.param("cutoff", cutoff, 1); - //mCostMapPub = mCmt.advertise("cost_maps", 1); + mCostMapPub = mCmt.advertise("cost_maps", 1); // We publish our results to "cost_maps" - mPcSub = mNh.subscribe("camera/left/points", 1, &CostMapNodelet::pointCloudCallback, this); + //TODO: Change topic to whatever the occupancy grid creating node publishes to + mPcSub = mNh.subscribe("camera/left/points", 1, &CostMapNodelet::occupancyGridCallback, this); + + globalGridMsg.info.resolution = resolution; + globalGridMsg.info.width = map_width; + globalGridMsg.info.height = map_height; + //TODO: What do we choose as our original load time and origin? + //globalGridMsg.info.map_load_time = + //globalGridMsg.info.origin = + + //TODO: Why is int8 not recognized? + auto* initial_map = new uint8[map_width * map_height]; + globalGridMsg.data = initial_map; } } // namespace mrover diff --git a/src/perception/cost_map/cost_map.hpp b/src/perception/cost_map/cost_map.hpp index 9a99de951..f965f74a2 100644 --- a/src/perception/cost_map/cost_map.hpp +++ b/src/perception/cost_map/cost_map.hpp @@ -3,39 +3,23 @@ namespace mrover { - struct CostMapNode { - cv::Point2f position{}; - double cost = 0; - }; - /* - (2,3) -> 100 - (3,4) -> 0 - (1,1) -> 100 - - -1 -1 -1 -1 -1 - -1 100 -1 -1 -1 - -1 -1 -1 100 -1 - -1 -1 -1 -1 0 - -1 -1 -1 -1 -1 - - */ - class CostMapNodelet : public nodelet::Nodelet { private: ros::NodeHandle mNh, mPnh, mCmt; ros::Publisher mCostMapPub; ros::Subscriber mPcSub; - std::vector costMapPointList; void onInit() override; - bool publishCostMaps = false; - bool verbose = false; - float element_size = 1; - int map_width = 1; - int map_height = 1; - float cutoff = 1; + bool publishCostMaps = false; // Do we publish our global cost maps? + bool verbose = false; // Do we print extra information for debugging? + float resolution = 1; // In m/cell + int map_width = 1; // Number of cells wide + int map_height = 1; // Number of cells high const tf2_ros::Buffer tf_buffer = tf2_ros::Buffer(); + nav_msgs::OccupancyGrid globalGridMsg; + + public: CostMapNodelet() = default; ~CostMapNodelet() override = default; diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index af492d38a..2f371a2bc 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -1,6 +1,7 @@ #include "cost_map.hpp" #include "../point.hpp" +#include #include #include #include @@ -25,9 +26,53 @@ namespace mrover { SE3 zed_to_base = SE3::fromTfTree(tf_buffer, "zed2i_left_camera_frame", "base_link"); Eigen::Matrix4d transformMatrix = zed_to_base.matrix() * base_to_map.matrix(); // This is the matrix transform from zed to the map - auto* pointPtr = &msg.data; - std::for_each(std::execution::par_unseq, &msg.data, &msg.data + msg.info.width, [&](int& point) { + auto* pointPtr = reinterpret_cast(msg.data.data()); + int width = static_cast(msg.info.width); + int height = static_cast(msg.info.width); + auto x_origin_local = static_cast(msg.info.origin.position.x); + auto y_origin_local = static_cast(msg.info.origin.position.y); + auto resolution_local = static_cast(msg.info.resolution); // m/cell + int width_global = static_cast(globalGridMsg.info.width); + int height_global = static_cast(globalGridMsg.info.height); + auto x_origin_global = static_cast(globalGridMsg.info.origin.position.x); + auto y_origin_global = static_cast(globalGridMsg.info.origin.position.y); + auto resolution_global = static_cast(globalGridMsg.info.resolution); + + auto data_global = static_cast(globalGridMsg.data); + + // Do we update load time? + //globalGridMsg.info.map_load_time + + //For each through full array + std::for_each(std::execution::par_unseq, &pointPtr, &pointPtr + msg.info.height * msg.info.width, [&](auto point) { + // Get x and y relative to the local frame + int cost = point; + int i = &point - pointPtr; + int x_index = i % width; + int y_index = i / height; + double x_local = x_origin_local + (resolution_local * x_index); + double y_local = y_origin_local + (resolution_local * y_index); + + // Convert to homogeneous coordinates + Eigen::MatrixXd point_local(4, 1); + point_local(0, 0) = x_local; + point_local(1, 0) = y_local; + point_local(2, 0) = 0; + point_local(3, 0) = 1; + + // Transform using our transformation matrix + auto xy_global = transformMatrix * point_local; + + // Calculate which index in the global frame the point is in + double x_global = xy_global(0, 0); + double y_global = xy_global(1, 0); + int x_index_global = floor((x_global - x_origin_global) / resolution_global); + int y_index_global = floor((y_global - y_origin_global) / resolution_global); + int i_global = (y_index_global * width_global) + x_index_global; + + // Update our global map by taking the maximum cost at the given point + //int prev }); } } // namespace mrover From 5bf78fa3c9a3b2688fde0825642194667067abd4 Mon Sep 17 00:00:00 2001 From: RyanAspen Date: Thu, 16 Nov 2023 19:43:19 -0500 Subject: [PATCH 13/76] Removed Line which kills Clang servers --- .../cost_map/cost_map.processing.cpp | 51 ++++++++++++++++++- 1 file changed, 50 insertions(+), 1 deletion(-) diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 2f371a2bc..e9b0fe502 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -1,6 +1,7 @@ #include "cost_map.hpp" #include "../point.hpp" +#include #include #include #include @@ -39,6 +40,54 @@ namespace mrover { auto y_origin_global = static_cast(globalGridMsg.info.origin.position.y); auto resolution_global = static_cast(globalGridMsg.info.resolution); + // Do we update load time? + //globalGridMsg.info.map_load_time + + //For each through full array + std::for_each(std::execution::par_unseq, &pointPtr, &pointPtr + msg.info.height * msg.info.width, [&](auto point) { + // Get x and y relative to the local frame + int cost = point; + int i = &point - pointPtr; + int x_index = i % width; + int y_index = i / height; + double x_local = x_origin_local + (resolution_local * x_index); + double y_local = y_origin_local + (resolution_local * y_index); + + // Convert to homogeneous coordinates + Eigen::MatrixXd point_local(4, 1); + point_local(0, 0) = x_local; + point_local(1, 0) = y_local; + point_local(2, 0) = 0; + point_local(3, 0) = 1; + + // Transform using our transformation matrix + auto xy_global = transformMatrix * point_local; + + // Calculate which index in the global frame the point is in + double x_global = xy_global(0, 0); + double y_global = xy_global(1, 0); + int x_index_global = floor((x_global - x_origin_global) / resolution_global); + int y_index_global = floor((y_global - y_origin_global) / resolution_global); + int i_global = (y_index_global * width_global) + x_index_global; + + // Update our global map by taking the maximum cost at the given point + int prev_cost = static_cast(globalGridMsg.data[i_global]); + if (cost > prev_cost) { + globalGridMsg.data[i_global] = cost; + } + auto* pointPtr = reinterpret_cast(msg.data.data()); + int width = static_cast(msg.info.width); + int height = static_cast(msg.info.width); + auto x_origin_local = static_cast(msg.info.origin.position.x); + auto y_origin_local = static_cast(msg.info.origin.position.y); + auto resolution_local = static_cast(msg.info.resolution); // m/cell + + int width_global = static_cast(globalGridMsg.info.width); + int height_global = static_cast(globalGridMsg.info.height); + auto x_origin_global = static_cast(globalGridMsg.info.origin.position.x); + auto y_origin_global = static_cast(globalGridMsg.info.origin.position.y); + auto resolution_global = static_cast(globalGridMsg.info.resolution); + auto data_global = static_cast(globalGridMsg.data); // Do we update load time? @@ -75,4 +124,4 @@ namespace mrover { //int prev }); } -} // namespace mrover +} // namespace mrover \ No newline at end of file From 0e6360df4e432b0bbeb2e2dc2e62ce27fc175fe7 Mon Sep 17 00:00:00 2001 From: Prabhleen Pawar Date: Sun, 19 Nov 2023 17:25:55 +0000 Subject: [PATCH 14/76] Added rostopic and started callback function --- config/navigation.yaml | 9 +++++++- src/navigation/water_bottle_search.py | 31 ++++++++++++++++++++++----- 2 files changed, 34 insertions(+), 6 deletions(-) diff --git a/config/navigation.yaml b/config/navigation.yaml index fe5a6adfb..ffb76514b 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -22,6 +22,13 @@ search: segments_per_rotation: 8 distance_between_spirals: 3 +water_bottle_search: + stop_thresh: 0.5 + drive_fwd_thresh: 0.34 + coverage_radius: 10 + segments_per_rotation: 8 + distance_between_spirals: 1.25 + single_fiducial: stop_thresh: 1.0 fiducial_stop_threshold: 1.75 @@ -46,5 +53,5 @@ failure_identification: watchdog: window_size: 100 #size of window we are looking at for being stuck - angular_threshold: 0.1 + angular_threshold: 0.1 linear_threshold: 0.2 diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index a3476aa1b..95238a582 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -4,8 +4,9 @@ from typing import Optional import numpy as np +import rospy from mrover.msg import GPSPointList - +from nav_msgs.msg import OccupancyGrid from util.ros_utils import get_rosparam from util.state_lib.state import State @@ -15,24 +16,44 @@ from navigation.search import SearchTrajectory # TODO: Right now this is a copy of search state, we will need to change this +# REFERENCE: https://docs.google.com/document/d/18GjDWxIu5f5-N5t5UgbrZGdEyaDj9ZMEUuXex8-NKrA/edit class WaterBottleSearchState(State): - traj: SearchTrajectory + #Spiral + traj: SearchTrajectory + #when we are moving along the spiral prev_target: Optional[np.ndarray] = None is_recovering: bool = False - # TODO: add rosparam names to waterbottlesearch/... in the navigation.yaml file - STOP_THRESH = get_rosparam("search/stop_thresh", 0.2) + # TODO: add rosparam names to water_bottle_search/... in the navigation.yaml file + STOP_THRESH = get_rosparam("search/stop_thresh", 0.5) DRIVE_FWD_THRESH = get_rosparam("search/drive_fwd_thresh", 0.34) SPIRAL_COVERAGE_RADIUS = get_rosparam("search/coverage_radius", 10) SEGMENTS_PER_ROTATION = get_rosparam("search/segments_per_rotation", 8) # TODO: after testing, might need to change DISTANCE_BETWEEN_SPIRALS = get_rosparam("search/distance_between_spirals", 1.25) # TODO: after testing, might need to change - + + + # TODO: Data Structure to store the information given from the map (Look at navigation) + + #2D list + costMap = [] + # TODO: Make call_back function to push into data structure + def costmap_callback(self, msg:OccupancyGrid): + #update data structure + costMap = OccupancyGrid.data + + #Call A-STAR + pass + + #TODO: A-STAR Algorithm: f(n) = g(n) + h(n) + #def a_star(): + def on_enter(self, context) -> None: + self.listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) search_center = context.course.current_waypoint() if not self.is_recovering: self.traj = SearchTrajectory.spiral_traj( From 95761c07466c518e524967c9041869d3da7549fb Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Sun, 19 Nov 2023 13:04:19 -0500 Subject: [PATCH 15/76] add costmap nodelet in cmakelists --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 98c9465f3..886e05e2d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -208,6 +208,7 @@ mrover_add_node(sim_arm_bridge src/simulator/arm_bridge/*.cpp) ## Perception mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp) +mrover_add_nodelet(costmap src/perception/cost_map/*.cpp src/perception/cost_map src/perception/cost_map/pch.hpp) mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc tbb lie) if (ZED_FOUND) From d82f70a9de56c353ec0bb99c703572bcbb720014 Mon Sep 17 00:00:00 2001 From: Prabhleen Pawar Date: Sun, 19 Nov 2023 18:53:26 +0000 Subject: [PATCH 16/76] Added a debug file for testng the occupency grid message --- src/navigation/debug_water_bottle_search.py | 41 +++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 src/navigation/debug_water_bottle_search.py diff --git a/src/navigation/debug_water_bottle_search.py b/src/navigation/debug_water_bottle_search.py new file mode 100644 index 000000000..f54425134 --- /dev/null +++ b/src/navigation/debug_water_bottle_search.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python3 + +import numpy as np + +import rospy +from nav_msgs.msg import OccupancyGrid, MapMetaData +from geometry_msgs.msg import Pose +from std_msgs.msg import Header +from util.course_publish_helpers import publish_waypoints + + +""" +The purpose of this file is for testing the water bottle search state. +Specifically the occupancy grid message. +""" + +if __name__ == "__main__": + rospy.init_node("debug_water_bottle") + try: + # int8[] data + test_grid = OccupancyGrid() + test_grid.data = np.array([1, 2, 3, 4, 5, 6, 7, 8, 9], dtype=np.int8) + + # nav_msgs/MapMetaData info + metadata = MapMetaData() + metadata.map_load_time = rospy.Time.now() + metadata.resolution = 3 + metadata.width = 3 + metadata.height = 3 + metadata.origin = Pose() + test_grid.info = metadata + + # std_msgs/Header header + header = Header() + test_grid.header = header + + two_d_list = [[(i * 3) + j + 1 for j in range(3)] for i in range(3)] + flattened_list = [element for row in two_d_list for element in row] + + except rospy.Servicexception as e: + print(f"Service call failed: {e}") From 2ff15052e82c42f21369c55c3c23188584cc1805 Mon Sep 17 00:00:00 2001 From: Mackenzie Date: Sun, 19 Nov 2023 14:04:02 -0500 Subject: [PATCH 17/76] changed costmap removed redundant code --- src/perception/cost_map/cost_map.hpp | 5 +- .../cost_map/cost_map.processing.cpp | 68 +++---------------- src/perception/cost_map/pch.hpp | 6 +- 3 files changed, 17 insertions(+), 62 deletions(-) diff --git a/src/perception/cost_map/cost_map.hpp b/src/perception/cost_map/cost_map.hpp index f965f74a2..e2c38a177 100644 --- a/src/perception/cost_map/cost_map.hpp +++ b/src/perception/cost_map/cost_map.hpp @@ -13,8 +13,9 @@ namespace mrover { bool publishCostMaps = false; // Do we publish our global cost maps? bool verbose = false; // Do we print extra information for debugging? float resolution = 1; // In m/cell - int map_width = 1; // Number of cells wide - int map_height = 1; // Number of cells high + int cell_width = 1; // Number of cells wide + int cell_height = 1; // Number of cells high + int global_dim = 15; const tf2_ros::Buffer tf_buffer = tf2_ros::Buffer(); nav_msgs::OccupancyGrid globalGridMsg; diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index e9b0fe502..d36da1b6e 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -23,28 +23,25 @@ namespace mrover { // Using tf_tree, get the transformation - SE3 base_to_map = SE3::fromTfTree(tf_buffer, "base_link", "map"); - SE3 zed_to_base = SE3::fromTfTree(tf_buffer, "zed2i_left_camera_frame", "base_link"); - Eigen::Matrix4d transformMatrix = zed_to_base.matrix() * base_to_map.matrix(); // This is the matrix transform from zed to the map + SE3 zed_to_map = SE3::fromTfTree(tf_buffer, "zed2i_left_camera_frame", "map"); //transform straight from zed to map w/o base + Eigen::Matrix4d transformMatrix = zed_to_map.matrix(); auto* pointPtr = reinterpret_cast(msg.data.data()); - int width = static_cast(msg.info.width); - int height = static_cast(msg.info.width); + cell_width = static_cast(msg.info.width); //hard set? don't want changing cell dims + cell_height = static_cast(msg.info.width); auto x_origin_local = static_cast(msg.info.origin.position.x); auto y_origin_local = static_cast(msg.info.origin.position.y); - auto resolution_local = static_cast(msg.info.resolution); // m/cell + auto resolution_local = static_cast(msg.info.resolution); // m/cell //defined in cost_map.hpp? - int width_global = static_cast(globalGridMsg.info.width); - int height_global = static_cast(globalGridMsg.info.height); auto x_origin_global = static_cast(globalGridMsg.info.origin.position.x); auto y_origin_global = static_cast(globalGridMsg.info.origin.position.y); - auto resolution_global = static_cast(globalGridMsg.info.resolution); + auto resolution_global = static_cast(globalGridMsg.info.resolution); //defined in cost_map.hpp? // Do we update load time? //globalGridMsg.info.map_load_time //For each through full array - std::for_each(std::execution::par_unseq, &pointPtr, &pointPtr + msg.info.height * msg.info.width, [&](auto point) { + std::for_each(std::execution::par_unseq, &pointPtr, &pointPtr + msg.info.height * msg.info.width, [&](auto point)) { // Get x and y relative to the local frame int cost = point; int i = &point - pointPtr; @@ -68,60 +65,13 @@ namespace mrover { double y_global = xy_global(1, 0); int x_index_global = floor((x_global - x_origin_global) / resolution_global); int y_index_global = floor((y_global - y_origin_global) / resolution_global); - int i_global = (y_index_global * width_global) + x_index_global; + int i_global = (y_index_global * global_dim) + x_index_global; // Update our global map by taking the maximum cost at the given point int prev_cost = static_cast(globalGridMsg.data[i_global]); if (cost > prev_cost) { globalGridMsg.data[i_global] = cost; } - auto* pointPtr = reinterpret_cast(msg.data.data()); - int width = static_cast(msg.info.width); - int height = static_cast(msg.info.width); - auto x_origin_local = static_cast(msg.info.origin.position.x); - auto y_origin_local = static_cast(msg.info.origin.position.y); - auto resolution_local = static_cast(msg.info.resolution); // m/cell - - int width_global = static_cast(globalGridMsg.info.width); - int height_global = static_cast(globalGridMsg.info.height); - auto x_origin_global = static_cast(globalGridMsg.info.origin.position.x); - auto y_origin_global = static_cast(globalGridMsg.info.origin.position.y); - auto resolution_global = static_cast(globalGridMsg.info.resolution); - - auto data_global = static_cast(globalGridMsg.data); - - // Do we update load time? - //globalGridMsg.info.map_load_time - - //For each through full array - std::for_each(std::execution::par_unseq, &pointPtr, &pointPtr + msg.info.height * msg.info.width, [&](auto point) { - // Get x and y relative to the local frame - int cost = point; - int i = &point - pointPtr; - int x_index = i % width; - int y_index = i / height; - double x_local = x_origin_local + (resolution_local * x_index); - double y_local = y_origin_local + (resolution_local * y_index); - - // Convert to homogeneous coordinates - Eigen::MatrixXd point_local(4, 1); - point_local(0, 0) = x_local; - point_local(1, 0) = y_local; - point_local(2, 0) = 0; - point_local(3, 0) = 1; - - // Transform using our transformation matrix - auto xy_global = transformMatrix * point_local; - - // Calculate which index in the global frame the point is in - double x_global = xy_global(0, 0); - double y_global = xy_global(1, 0); - int x_index_global = floor((x_global - x_origin_global) / resolution_global); - int y_index_global = floor((y_global - y_origin_global) / resolution_global); - int i_global = (y_index_global * width_global) + x_index_global; - - // Update our global map by taking the maximum cost at the given point - //int prev - }); + } } } // namespace mrover \ No newline at end of file diff --git a/src/perception/cost_map/pch.hpp b/src/perception/cost_map/pch.hpp index c7d149d7a..347e7491d 100644 --- a/src/perception/cost_map/pch.hpp +++ b/src/perception/cost_map/pch.hpp @@ -2,4 +2,8 @@ #include #include -#include \ No newline at end of file +#include +#include +#include +#include +#include \ No newline at end of file From 9a82b0ca24d543facc4a9a88ffbc39e47eca7b01 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Sun, 26 Nov 2023 12:47:06 -0500 Subject: [PATCH 18/76] add costmap to cmakelists, fix some includes compatibility issues --- CMakeLists.txt | 6 +- src/perception/cost_map/cost_map.cpp | 23 +++---- src/perception/cost_map/cost_map.hpp | 20 +++--- .../cost_map/cost_map.processing.cpp | 68 +++++++++++++++++-- src/perception/cost_map/pch.hpp | 11 +-- 5 files changed, 95 insertions(+), 33 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 886e05e2d..7130c0012 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,6 +25,7 @@ set(MROVER_ROS_DEPENDENCIES tf2_ros tf2_geometry_msgs gazebo_ros + pcl_ros ) # Search a path for all files matching a glob pattern and extract the filenames @@ -141,6 +142,7 @@ find_package(OpenCV REQUIRED) find_package(ZED 2 QUIET) find_package(gazebo REQUIRED) find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED) if (ZED_FOUND) # Anything newer than C++17 combined with libstdc++13 is not supported just yet by NVCC (the CUDA compiler) set(CMAKE_CUDA_STANDARD 17) @@ -208,8 +210,10 @@ mrover_add_node(sim_arm_bridge src/simulator/arm_bridge/*.cpp) ## Perception mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp) -mrover_add_nodelet(costmap src/perception/cost_map/*.cpp src/perception/cost_map src/perception/cost_map/pch.hpp) mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc tbb lie) +mrover_add_nodelet(costmap src/perception/cost_map/*.cpp src/perception/cost_map src/perception/cost_map/pch.hpp) +mrover_nodelet_include_directories(costmap ${PCL_LIBRARY_DIRS}) +mrover_nodelet_link_libraries(costmap ${PCL_LIBRARIES} lie) if (ZED_FOUND) mrover_add_nodelet(zed src/perception/zed_wrapper/*.c* src/perception/zed_wrapper src/perception/zed_wrapper/pch.hpp) diff --git a/src/perception/cost_map/cost_map.cpp b/src/perception/cost_map/cost_map.cpp index 3e30f1a92..71ec436df 100644 --- a/src/perception/cost_map/cost_map.cpp +++ b/src/perception/cost_map/cost_map.cpp @@ -1,34 +1,29 @@ #include "cost_map.hpp" -#include -#include namespace mrover { void CostMapNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); // Unused for now - mNh.param("publish_cost_maps", publishCostMaps, true); - mNh.param("verbose", verbose, false); - mNh.param("resolution", resolution, 1); - mNh.param("map_width", map_width, 1); - mNh.param("map_height", map_height, 1); + mNh.param("publish_cost_map", mPublishCostMap, true); + mNh.param("resolution", mResolution, 2); + mNh.param("map_width", mDimension, 15); - - mCostMapPub = mCmt.advertise("cost_maps", 1); // We publish our results to "cost_maps" + mCostMapPub = mCmt.advertise("costmap", 1); // We publish our results to "costmap" //TODO: Change topic to whatever the occupancy grid creating node publishes to mPcSub = mNh.subscribe("camera/left/points", 1, &CostMapNodelet::occupancyGridCallback, this); - globalGridMsg.info.resolution = resolution; - globalGridMsg.info.width = map_width; - globalGridMsg.info.height = map_height; + mGlobalGridMsg.info.resolution = mResolution; + mGlobalGridMsg.info.width = mDimension; + mGlobalGridMsg.info.height = mDimension; //TODO: What do we choose as our original load time and origin? //globalGridMsg.info.map_load_time = //globalGridMsg.info.origin = //TODO: Why is int8 not recognized? - auto* initial_map = new uint8[map_width * map_height]; - globalGridMsg.data = initial_map; + auto* initial_map = new uint[mDimension * mDimension]; + mGlobalGridMsg.data = initial_map; } } // namespace mrover diff --git a/src/perception/cost_map/cost_map.hpp b/src/perception/cost_map/cost_map.hpp index e2c38a177..76e0ca02d 100644 --- a/src/perception/cost_map/cost_map.hpp +++ b/src/perception/cost_map/cost_map.hpp @@ -1,8 +1,10 @@ -#include "nav_msgs/OccupancyGrid.h" #include "pch.hpp" namespace mrover { + using PointCloud = pcl::PointCloud; + using PointCloudPtr = PointCloud::Ptr; + class CostMapNodelet : public nodelet::Nodelet { private: ros::NodeHandle mNh, mPnh, mCmt; @@ -10,20 +12,22 @@ namespace mrover { ros::Subscriber mPcSub; void onInit() override; - bool publishCostMaps = false; // Do we publish our global cost maps? - bool verbose = false; // Do we print extra information for debugging? - float resolution = 1; // In m/cell - int cell_width = 1; // Number of cells wide - int cell_height = 1; // Number of cells high - int global_dim = 15; + bool mPublishCostMap{}; // If set, publish the global costmap + float mResolution{}; // Cells per meter + int mDimension{}; // Dimensions of the square costmap in meters const tf2_ros::Buffer tf_buffer = tf2_ros::Buffer(); - nav_msgs::OccupancyGrid globalGridMsg; + std::optional mPreviousPose; + nav_msgs::OccupancyGrid mGlobalGridMsg; public: CostMapNodelet() = default; + ~CostMapNodelet() override = default; + void occupancyGridCallback(nav_msgs::OccupancyGrid const& msg); + void configCallback(); + void pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg); }; } // namespace mrover \ No newline at end of file diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index d36da1b6e..ba7aaf82a 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -1,14 +1,22 @@ #include "cost_map.hpp" #include "../point.hpp" -#include #include #include #include #include +#include namespace mrover { + void CostMapNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) { + assert(msg); + assert(msg->height > 0); + assert(msg->width > 0); + + if (!mPublishCostMap) return; + } + /** * Stitches local occupancy grid to global occupancy grid * @@ -27,15 +35,14 @@ namespace mrover { Eigen::Matrix4d transformMatrix = zed_to_map.matrix(); auto* pointPtr = reinterpret_cast(msg.data.data()); - cell_width = static_cast(msg.info.width); //hard set? don't want changing cell dims - cell_height = static_cast(msg.info.width); + mDimension = static_cast(msg.info.width); auto x_origin_local = static_cast(msg.info.origin.position.x); auto y_origin_local = static_cast(msg.info.origin.position.y); auto resolution_local = static_cast(msg.info.resolution); // m/cell //defined in cost_map.hpp? - auto x_origin_global = static_cast(globalGridMsg.info.origin.position.x); - auto y_origin_global = static_cast(globalGridMsg.info.origin.position.y); - auto resolution_global = static_cast(globalGridMsg.info.resolution); //defined in cost_map.hpp? + auto x_origin_global = static_cast(mGlobalGridMsg.info.origin.position.x); + auto y_origin_global = static_cast(mGlobalGridMsg.info.origin.position.y); + auto resolution_global = static_cast(mGlobalGridMsg.info.resolution); //defined in cost_map.hpp? // Do we update load time? //globalGridMsg.info.map_load_time @@ -72,6 +79,55 @@ namespace mrover { if (cost > prev_cost) { globalGridMsg.data[i_global] = cost; } + + auto* pointPtr = reinterpret_cast(msg.data.data()); + int width = static_cast(msg.info.width); + int height = static_cast(msg.info.width); + auto x_origin_local = static_cast(msg.info.origin.position.x); + auto y_origin_local = static_cast(msg.info.origin.position.y); + auto resolution_local = static_cast(msg.info.resolution); // m/cell + + int width_global = static_cast(globalGridMsg.info.width); + int height_global = static_cast(globalGridMsg.info.height); + auto x_origin_global = static_cast(globalGridMsg.info.origin.position.x); + auto y_origin_global = static_cast(globalGridMsg.info.origin.position.y); + auto resolution_global = static_cast(globalGridMsg.info.resolution); + + auto data_global = static_cast(globalGridMsg.data); + + // Do we update load time? + //globalGridMsg.info.map_load_time + + //For each through full array + std::for_each(std::execution::par_unseq, &pointPtr, &pointPtr + msg.info.height * msg.info.width, [&](auto point) { + // Get x and y relative to the local frame + int cost = point; + int i = &point - pointPtr; + int x_index = i % width; + int y_index = i / height; + double x_local = x_origin_local + (resolution_local * x_index); + double y_local = y_origin_local + (resolution_local * y_index); + + // Convert to homogeneous coordinates + Eigen::MatrixXd point_local(4, 1); + point_local(0, 0) = x_local; + point_local(1, 0) = y_local; + point_local(2, 0) = 0; + point_local(3, 0) = 1; + + // Transform using our transformation matrix + auto xy_global = transformMatrix * point_local; + + // Calculate which index in the global frame the point is in + double x_global = xy_global(0, 0); + double y_global = xy_global(1, 0); + int x_index_global = floor((x_global - x_origin_global) / resolution_global); + int y_index_global = floor((y_global - y_origin_global) / resolution_global); + int i_global = (y_index_global * width_global) + x_index_global; + + // Update our global map by taking the maximum cost at the given point + //int prev + }); } } } // namespace mrover \ No newline at end of file diff --git a/src/perception/cost_map/pch.hpp b/src/perception/cost_map/pch.hpp index 347e7491d..e45a989d6 100644 --- a/src/perception/cost_map/pch.hpp +++ b/src/perception/cost_map/pch.hpp @@ -1,9 +1,12 @@ #pragma once +#include +#include +#include +#include +#include #include #include +#include #include -#include -#include -#include -#include \ No newline at end of file +#include \ No newline at end of file From 782e22a00468bec516a739457773cccb54299036 Mon Sep 17 00:00:00 2001 From: Prabhleen Pawar Date: Sun, 26 Nov 2023 19:11:23 +0000 Subject: [PATCH 19/76] Started publisher for msg debugging --- src/navigation/debug_water_bottle_search.py | 13 +++++++------ src/navigation/water_bottle_search.py | 3 +-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/navigation/debug_water_bottle_search.py b/src/navigation/debug_water_bottle_search.py index f54425134..1d59c3990 100644 --- a/src/navigation/debug_water_bottle_search.py +++ b/src/navigation/debug_water_bottle_search.py @@ -20,7 +20,7 @@ # int8[] data test_grid = OccupancyGrid() test_grid.data = np.array([1, 2, 3, 4, 5, 6, 7, 8, 9], dtype=np.int8) - + # nav_msgs/MapMetaData info metadata = MapMetaData() metadata.map_load_time = rospy.Time.now() @@ -33,9 +33,10 @@ # std_msgs/Header header header = Header() test_grid.header = header - - two_d_list = [[(i * 3) + j + 1 for j in range(3)] for i in range(3)] - flattened_list = [element for row in two_d_list for element in row] + two_d_list: list[list[int]] = [[(i * 3) + j + 1 for j in range(3)] for i in range(3)] + + costpub = rospy.Publisher("costmap", OccupancyGrid, queue_size=1) + costpub.publisher(test_grid) - except rospy.Servicexception as e: - print(f"Service call failed: {e}") + except rospy.ROSInterruptException as e: + print(f"Didn't work to publish or retrieve message from ros") diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index 95238a582..d1c634a69 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -6,7 +6,6 @@ import numpy as np import rospy from mrover.msg import GPSPointList -from nav_msgs.msg import OccupancyGrid from util.ros_utils import get_rosparam from util.state_lib.state import State @@ -49,7 +48,7 @@ def costmap_callback(self, msg:OccupancyGrid): pass #TODO: A-STAR Algorithm: f(n) = g(n) + h(n) - #def a_star(): + #def a_star():j def on_enter(self, context) -> None: From bf8c75f137dfce51dae7b0272ca18b7b93f24ae9 Mon Sep 17 00:00:00 2001 From: Prabhleen Pawar Date: Wed, 29 Nov 2023 00:01:30 +0000 Subject: [PATCH 20/76] Debug course publisher update with test, also added temporary subscriber changes in context --- .../debug_water_bottle_search.py | 10 +++-- src/navigation/context.py | 11 +++++ src/navigation/water_bottle_search.py | 43 +++++++++++-------- 3 files changed, 42 insertions(+), 22 deletions(-) rename {src/navigation => scripts}/debug_water_bottle_search.py (89%) diff --git a/src/navigation/debug_water_bottle_search.py b/scripts/debug_water_bottle_search.py similarity index 89% rename from src/navigation/debug_water_bottle_search.py rename to scripts/debug_water_bottle_search.py index 1d59c3990..ff9ff5145 100644 --- a/src/navigation/debug_water_bottle_search.py +++ b/scripts/debug_water_bottle_search.py @@ -33,10 +33,14 @@ # std_msgs/Header header header = Header() test_grid.header = header - two_d_list: list[list[int]] = [[(i * 3) + j + 1 for j in range(3)] for i in range(3)] - + + costpub = rospy.Publisher("costmap", OccupancyGrid, queue_size=1) - costpub.publisher(test_grid) + for i in range(10): + costpub.publish(test_grid) + + rospy.spin() + except rospy.ROSInterruptException as e: print(f"Didn't work to publish or retrieve message from ros") diff --git a/src/navigation/context.py b/src/navigation/context.py index 3c242edc5..24ad8ddce 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -10,6 +10,7 @@ import rospy import tf2_ros from geometry_msgs.msg import Twist +from nav_msgs.msg import OccupancyGrid, MapMetaData from mrover.msg import Waypoint, GPSWaypoint, EnableAuton, WaypointType, GPSPointList from std_msgs.msg import Time, Bool from visualization_msgs.msg import Marker @@ -223,6 +224,7 @@ def __init__(self): self.search_point_publisher = rospy.Publisher("search_path", GPSPointList, queue_size=1) self.enable_auton_service = rospy.Service("enable_auton", mrover.srv.PublishEnableAuton, self.recv_enable_auton) self.stuck_listener = rospy.Subscriber("nav_stuck", Bool, self.stuck_callback) + self.costmap_listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) self.course = None self.rover = Rover(self, False, "") self.env = Environment(self) @@ -242,3 +244,12 @@ def recv_enable_auton(self, req: mrover.srv.PublishEnableAutonRequest) -> mrover def stuck_callback(self, msg: Bool): self.rover.stuck = msg.data + + def costmap_callback(self, msg: OccupancyGrid): + # update data structure + height = msg.info.height + width = msg.info.width + rospy.loginfo(f"height: {height}, width: {width}") + costmap2D = np.array(msg.data) + costmap2D.reshape(height, width) + rospy.loginfo(f"2D costmap: {costmap2D}") diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index d1c634a69..dc9e9d0e2 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -9,18 +9,20 @@ from util.ros_utils import get_rosparam from util.state_lib.state import State +from nav_msgs.msg import OccupancyGrid, MapMetaData from navigation import approach_post, recovery, waypoint from navigation.context import convert_cartesian_to_gps from navigation.trajectory import Trajectory from navigation.search import SearchTrajectory -# TODO: Right now this is a copy of search state, we will need to change this +# TODO: Right now this is a copy of search state, we will need to change this # REFERENCE: https://docs.google.com/document/d/18GjDWxIu5f5-N5t5UgbrZGdEyaDj9ZMEUuXex8-NKrA/edit + class WaterBottleSearchState(State): - #Spiral - traj: SearchTrajectory - #when we are moving along the spiral + # Spiral + traj: SearchTrajectory + # when we are moving along the spiral prev_target: Optional[np.ndarray] = None is_recovering: bool = False @@ -28,28 +30,31 @@ class WaterBottleSearchState(State): STOP_THRESH = get_rosparam("search/stop_thresh", 0.5) DRIVE_FWD_THRESH = get_rosparam("search/drive_fwd_thresh", 0.34) SPIRAL_COVERAGE_RADIUS = get_rosparam("search/coverage_radius", 10) - SEGMENTS_PER_ROTATION = get_rosparam("search/segments_per_rotation", 8) # TODO: after testing, might need to change - DISTANCE_BETWEEN_SPIRALS = get_rosparam("search/distance_between_spirals", 1.25) # TODO: after testing, might need to change - - + SEGMENTS_PER_ROTATION = get_rosparam("search/segments_per_rotation", 8) # TODO: after testing, might need to change + DISTANCE_BETWEEN_SPIRALS = get_rosparam( + "search/distance_between_spirals", 1.25 + ) # TODO: after testing, might need to change # TODO: Data Structure to store the information given from the map (Look at navigation) - #2D list + # 2D list costMap = [] # TODO: Make call_back function to push into data structure - def costmap_callback(self, msg:OccupancyGrid): - #update data structure - costMap = OccupancyGrid.data - - #Call A-STAR - pass - - #TODO: A-STAR Algorithm: f(n) = g(n) + h(n) - #def a_star():j - + def costmap_callback(self, msg: OccupancyGrid): + # update data structure + height = msg.info.height + width = msg.info.width + rospy.loginfo(f"height: {height}, width: {width}") + costmap2D = np.array(msg.data) + costmap2D.reshape(height, width) + rospy.loginfo(f"2D costmap: {costmap2D}") + + # Call A-STAR + + # TODO: A-STAR Algorithm: f(n) = g(n) + h(n) + # def a_star():j def on_enter(self, context) -> None: self.listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) From de8ce87fa2f693864fa34ab17357c8ee3150d8c6 Mon Sep 17 00:00:00 2001 From: Prabhleen Pawar Date: Wed, 29 Nov 2023 00:53:59 +0000 Subject: [PATCH 21/76] Callback function done and tested, data retrieved properly --- scripts/debug_water_bottle_search.py | 6 ++++-- src/navigation/context.py | 10 ++++++---- 2 files changed, 10 insertions(+), 6 deletions(-) mode change 100644 => 100755 scripts/debug_water_bottle_search.py diff --git a/scripts/debug_water_bottle_search.py b/scripts/debug_water_bottle_search.py old mode 100644 new mode 100755 index ff9ff5145..c9a038009 --- a/scripts/debug_water_bottle_search.py +++ b/scripts/debug_water_bottle_search.py @@ -3,6 +3,7 @@ import numpy as np import rospy +import time from nav_msgs.msg import OccupancyGrid, MapMetaData from geometry_msgs.msg import Pose from std_msgs.msg import Header @@ -34,11 +35,12 @@ header = Header() test_grid.header = header - + rospy.loginfo(f"Before publish") costpub = rospy.Publisher("costmap", OccupancyGrid, queue_size=1) for i in range(10): costpub.publish(test_grid) - + time.sleep(1) + rospy.loginfo(f"After publish") rospy.spin() diff --git a/src/navigation/context.py b/src/navigation/context.py index 24ad8ddce..baf0830c4 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -203,6 +203,7 @@ class Context: vis_publisher: rospy.Publisher course_listener: rospy.Subscriber stuck_listener: rospy.Subscriber + costmap_listener: rospy.Subscriber # Use these as the primary interfaces in states course: Optional[Course] @@ -249,7 +250,8 @@ def costmap_callback(self, msg: OccupancyGrid): # update data structure height = msg.info.height width = msg.info.width - rospy.loginfo(f"height: {height}, width: {width}") - costmap2D = np.array(msg.data) - costmap2D.reshape(height, width) - rospy.loginfo(f"2D costmap: {costmap2D}") + rospy.logerr(f"height: {height}, width: {width}") + costmap1D = np.array(msg.data) + costmap2D = costmap1D.reshape((height, width)) + rospy.logerr(f"2D costmap: {costmap2D.shape}") + rospy.logerr(f"2D costmap: {costmap2D}") From 234315fb27ccf06d1c8b8252cca1a402264d9e3a Mon Sep 17 00:00:00 2001 From: Depicar Date: Thu, 30 Nov 2023 14:13:28 -0500 Subject: [PATCH 22/76] Removed duplicate code --- .../cost_map/cost_map.processing.cpp | 96 +++++++++---------- 1 file changed, 48 insertions(+), 48 deletions(-) diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index ba7aaf82a..dbd2d20b2 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -80,54 +80,54 @@ namespace mrover { globalGridMsg.data[i_global] = cost; } - auto* pointPtr = reinterpret_cast(msg.data.data()); - int width = static_cast(msg.info.width); - int height = static_cast(msg.info.width); - auto x_origin_local = static_cast(msg.info.origin.position.x); - auto y_origin_local = static_cast(msg.info.origin.position.y); - auto resolution_local = static_cast(msg.info.resolution); // m/cell - - int width_global = static_cast(globalGridMsg.info.width); - int height_global = static_cast(globalGridMsg.info.height); - auto x_origin_global = static_cast(globalGridMsg.info.origin.position.x); - auto y_origin_global = static_cast(globalGridMsg.info.origin.position.y); - auto resolution_global = static_cast(globalGridMsg.info.resolution); - - auto data_global = static_cast(globalGridMsg.data); - - // Do we update load time? - //globalGridMsg.info.map_load_time - - //For each through full array - std::for_each(std::execution::par_unseq, &pointPtr, &pointPtr + msg.info.height * msg.info.width, [&](auto point) { - // Get x and y relative to the local frame - int cost = point; - int i = &point - pointPtr; - int x_index = i % width; - int y_index = i / height; - double x_local = x_origin_local + (resolution_local * x_index); - double y_local = y_origin_local + (resolution_local * y_index); - - // Convert to homogeneous coordinates - Eigen::MatrixXd point_local(4, 1); - point_local(0, 0) = x_local; - point_local(1, 0) = y_local; - point_local(2, 0) = 0; - point_local(3, 0) = 1; - - // Transform using our transformation matrix - auto xy_global = transformMatrix * point_local; - - // Calculate which index in the global frame the point is in - double x_global = xy_global(0, 0); - double y_global = xy_global(1, 0); - int x_index_global = floor((x_global - x_origin_global) / resolution_global); - int y_index_global = floor((y_global - y_origin_global) / resolution_global); - int i_global = (y_index_global * width_global) + x_index_global; - - // Update our global map by taking the maximum cost at the given point - //int prev - }); + // auto* pointPtr = reinterpret_cast(msg.data.data()); + // int width = static_cast(msg.info.width); + // int height = static_cast(msg.info.width); + // auto x_origin_local = static_cast(msg.info.origin.position.x); + // auto y_origin_local = static_cast(msg.info.origin.position.y); + // auto resolution_local = static_cast(msg.info.resolution); // m/cell + + // int width_global = static_cast(globalGridMsg.info.width); + // int height_global = static_cast(globalGridMsg.info.height); + // auto x_origin_global = static_cast(globalGridMsg.info.origin.position.x); + // auto y_origin_global = static_cast(globalGridMsg.info.origin.position.y); + // auto resolution_global = static_cast(globalGridMsg.info.resolution); + + // auto data_global = static_cast(globalGridMsg.data); + + // // Do we update load time? + // //globalGridMsg.info.map_load_time + + // //For each through full array + // std::for_each(std::execution::par_unseq, &pointPtr, &pointPtr + msg.info.height * msg.info.width, [&](auto point) { + // // Get x and y relative to the local frame + // int cost = point; + // int i = &point - pointPtr; + // int x_index = i % width; + // int y_index = i / height; + // double x_local = x_origin_local + (resolution_local * x_index); + // double y_local = y_origin_local + (resolution_local * y_index); + + // // Convert to homogeneous coordinates + // Eigen::MatrixXd point_local(4, 1); + // point_local(0, 0) = x_local; + // point_local(1, 0) = y_local; + // point_local(2, 0) = 0; + // point_local(3, 0) = 1; + + // // Transform using our transformation matrix + // auto xy_global = transformMatrix * point_local; + + // // Calculate which index in the global frame the point is in + // double x_global = xy_global(0, 0); + // double y_global = xy_global(1, 0); + // int x_index_global = floor((x_global - x_origin_global) / resolution_global); + // int y_index_global = floor((y_global - y_origin_global) / resolution_global); + // int i_global = (y_index_global * width_global) + x_index_global; + + // // Update our global map by taking the maximum cost at the given point + // //int prev + // }); } } } // namespace mrover \ No newline at end of file From 6a448ebf2515a0595dd99306ddab6c2c38e629fa Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Sun, 3 Dec 2023 12:42:31 -0500 Subject: [PATCH 23/76] remove pcl from build --- CMakeLists.txt | 5 +---- src/perception/cost_map/cost_map.hpp | 3 --- src/perception/cost_map/pch.hpp | 1 - 3 files changed, 1 insertion(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7130c0012..664e59b27 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,7 +25,6 @@ set(MROVER_ROS_DEPENDENCIES tf2_ros tf2_geometry_msgs gazebo_ros - pcl_ros ) # Search a path for all files matching a glob pattern and extract the filenames @@ -142,7 +141,6 @@ find_package(OpenCV REQUIRED) find_package(ZED 2 QUIET) find_package(gazebo REQUIRED) find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED) if (ZED_FOUND) # Anything newer than C++17 combined with libstdc++13 is not supported just yet by NVCC (the CUDA compiler) set(CMAKE_CUDA_STANDARD 17) @@ -212,8 +210,7 @@ mrover_add_node(sim_arm_bridge src/simulator/arm_bridge/*.cpp) mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp) mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc tbb lie) mrover_add_nodelet(costmap src/perception/cost_map/*.cpp src/perception/cost_map src/perception/cost_map/pch.hpp) -mrover_nodelet_include_directories(costmap ${PCL_LIBRARY_DIRS}) -mrover_nodelet_link_libraries(costmap ${PCL_LIBRARIES} lie) +mrover_nodelet_link_libraries(costmap lie) if (ZED_FOUND) mrover_add_nodelet(zed src/perception/zed_wrapper/*.c* src/perception/zed_wrapper src/perception/zed_wrapper/pch.hpp) diff --git a/src/perception/cost_map/cost_map.hpp b/src/perception/cost_map/cost_map.hpp index 76e0ca02d..747ed32f2 100644 --- a/src/perception/cost_map/cost_map.hpp +++ b/src/perception/cost_map/cost_map.hpp @@ -2,9 +2,6 @@ namespace mrover { - using PointCloud = pcl::PointCloud; - using PointCloudPtr = PointCloud::Ptr; - class CostMapNodelet : public nodelet::Nodelet { private: ros::NodeHandle mNh, mPnh, mCmt; diff --git a/src/perception/cost_map/pch.hpp b/src/perception/cost_map/pch.hpp index e45a989d6..c34257915 100644 --- a/src/perception/cost_map/pch.hpp +++ b/src/perception/cost_map/pch.hpp @@ -4,7 +4,6 @@ #include #include #include -#include #include #include #include From c06b6f33b541c73e4c246df19549204318012053 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Sun, 3 Dec 2023 13:27:57 -0500 Subject: [PATCH 24/76] worked on grid filling --- CMakeLists.txt | 2 - src/perception/cost_map/cost_map.cpp | 5 +- src/perception/cost_map/cost_map.hpp | 4 +- .../cost_map/cost_map.processing.cpp | 125 +++--------------- src/perception/cost_map/pch.hpp | 1 - src/perception/point.hpp | 2 + 6 files changed, 22 insertions(+), 117 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7130c0012..211dcc06e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,7 +25,6 @@ set(MROVER_ROS_DEPENDENCIES tf2_ros tf2_geometry_msgs gazebo_ros - pcl_ros ) # Search a path for all files matching a glob pattern and extract the filenames @@ -142,7 +141,6 @@ find_package(OpenCV REQUIRED) find_package(ZED 2 QUIET) find_package(gazebo REQUIRED) find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED) if (ZED_FOUND) # Anything newer than C++17 combined with libstdc++13 is not supported just yet by NVCC (the CUDA compiler) set(CMAKE_CUDA_STANDARD 17) diff --git a/src/perception/cost_map/cost_map.cpp b/src/perception/cost_map/cost_map.cpp index 71ec436df..cf0f60ae7 100644 --- a/src/perception/cost_map/cost_map.cpp +++ b/src/perception/cost_map/cost_map.cpp @@ -12,7 +12,7 @@ namespace mrover { mCostMapPub = mCmt.advertise("costmap", 1); // We publish our results to "costmap" //TODO: Change topic to whatever the occupancy grid creating node publishes to - mPcSub = mNh.subscribe("camera/left/points", 1, &CostMapNodelet::occupancyGridCallback, this); + mPcSub = mNh.subscribe("camera/left/points", 1, &CostMapNodelet::pointCloudCallback, this); mGlobalGridMsg.info.resolution = mResolution; mGlobalGridMsg.info.width = mDimension; @@ -22,7 +22,8 @@ namespace mrover { //globalGridMsg.info.origin = //TODO: Why is int8 not recognized? - auto* initial_map = new uint[mDimension * mDimension]; + std::vector initial_map; + std::fill(initial_map.begin(), initial_map.end(), -1); mGlobalGridMsg.data = initial_map; } } // namespace mrover diff --git a/src/perception/cost_map/cost_map.hpp b/src/perception/cost_map/cost_map.hpp index 76e0ca02d..89d3a3419 100644 --- a/src/perception/cost_map/cost_map.hpp +++ b/src/perception/cost_map/cost_map.hpp @@ -2,8 +2,6 @@ namespace mrover { - using PointCloud = pcl::PointCloud; - using PointCloudPtr = PointCloud::Ptr; class CostMapNodelet : public nodelet::Nodelet { private: @@ -15,6 +13,7 @@ namespace mrover { bool mPublishCostMap{}; // If set, publish the global costmap float mResolution{}; // Cells per meter int mDimension{}; // Dimensions of the square costmap in meters + float mNormalThreshold = 0.5; const tf2_ros::Buffer tf_buffer = tf2_ros::Buffer(); std::optional mPreviousPose; @@ -26,7 +25,6 @@ namespace mrover { ~CostMapNodelet() override = default; - void occupancyGridCallback(nav_msgs::OccupancyGrid const& msg); void configCallback(); void pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg); }; diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index ba7aaf82a..16237e065 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -15,119 +15,26 @@ namespace mrover { assert(msg->width > 0); if (!mPublishCostMap) return; - } - - /** - * Stitches local occupancy grid to global occupancy grid - * - * @param msg Local Occupancy Grid Mesasge - */ - void CostMapNodelet::occupancyGridCallback(nav_msgs::OccupancyGrid const& msg) { - - // Make sure message is valid - assert(msg.info.width > 0); - assert(msg.info.height > 0); - assert(msg.info.resolution > 0); - - // Using tf_tree, get the transformation - - SE3 zed_to_map = SE3::fromTfTree(tf_buffer, "zed2i_left_camera_frame", "map"); //transform straight from zed to map w/o base - Eigen::Matrix4d transformMatrix = zed_to_map.matrix(); - - auto* pointPtr = reinterpret_cast(msg.data.data()); - mDimension = static_cast(msg.info.width); - auto x_origin_local = static_cast(msg.info.origin.position.x); - auto y_origin_local = static_cast(msg.info.origin.position.y); - auto resolution_local = static_cast(msg.info.resolution); // m/cell //defined in cost_map.hpp? - - auto x_origin_global = static_cast(mGlobalGridMsg.info.origin.position.x); - auto y_origin_global = static_cast(mGlobalGridMsg.info.origin.position.y); - auto resolution_global = static_cast(mGlobalGridMsg.info.resolution); //defined in cost_map.hpp? - - // Do we update load time? - //globalGridMsg.info.map_load_time - - //For each through full array - std::for_each(std::execution::par_unseq, &pointPtr, &pointPtr + msg.info.height * msg.info.width, [&](auto point)) { - // Get x and y relative to the local frame - int cost = point; - int i = &point - pointPtr; - int x_index = i % width; - int y_index = i / height; - double x_local = x_origin_local + (resolution_local * x_index); - double y_local = y_origin_local + (resolution_local * y_index); - - // Convert to homogeneous coordinates - Eigen::MatrixXd point_local(4, 1); - point_local(0, 0) = x_local; - point_local(1, 0) = y_local; - point_local(2, 0) = 0; - point_local(3, 0) = 1; - - // Transform using our transformation matrix - auto xy_global = transformMatrix * point_local; - - // Calculate which index in the global frame the point is in - double x_global = xy_global(0, 0); - double y_global = xy_global(1, 0); - int x_index_global = floor((x_global - x_origin_global) / resolution_global); - int y_index_global = floor((y_global - y_origin_global) / resolution_global); - int i_global = (y_index_global * global_dim) + x_index_global; - - // Update our global map by taking the maximum cost at the given point - int prev_cost = static_cast(globalGridMsg.data[i_global]); - if (cost > prev_cost) { - globalGridMsg.data[i_global] = cost; - } - - auto* pointPtr = reinterpret_cast(msg.data.data()); - int width = static_cast(msg.info.width); - int height = static_cast(msg.info.width); - auto x_origin_local = static_cast(msg.info.origin.position.x); - auto y_origin_local = static_cast(msg.info.origin.position.y); - auto resolution_local = static_cast(msg.info.resolution); // m/cell - - int width_global = static_cast(globalGridMsg.info.width); - int height_global = static_cast(globalGridMsg.info.height); - auto x_origin_global = static_cast(globalGridMsg.info.origin.position.x); - auto y_origin_global = static_cast(globalGridMsg.info.origin.position.y); - auto resolution_global = static_cast(globalGridMsg.info.resolution); - - auto data_global = static_cast(globalGridMsg.data); - - // Do we update load time? - //globalGridMsg.info.map_load_time - //For each through full array - std::for_each(std::execution::par_unseq, &pointPtr, &pointPtr + msg.info.height * msg.info.width, [&](auto point) { - // Get x and y relative to the local frame - int cost = point; - int i = &point - pointPtr; - int x_index = i % width; - int y_index = i / height; - double x_local = x_origin_local + (resolution_local * x_index); - double y_local = y_origin_local + (resolution_local * y_index); + SE3 zed_to_map = SE3::fromTfTree(tf_buffer, "zed2i_left_camera_frame", "map"); - // Convert to homogeneous coordinates - Eigen::MatrixXd point_local(4, 1); - point_local(0, 0) = x_local; - point_local(1, 0) = y_local; - point_local(2, 0) = 0; - point_local(3, 0) = 1; + auto* points = reinterpret_cast(msg->data.data()); + std::for_each(points, points + msg->width * msg->height, [&](auto& point) { + SE3 point_in_zed{R3{point.x, point.y, 0.0}}; + SE3 point_in_map = zed_to_map * point_in_zed; - // Transform using our transformation matrix - auto xy_global = transformMatrix * point_local; + int x_index = floor((point_in_map.position().x() - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); + int y_index = floor((point_in_map.position().y() - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); + auto i = mGlobalGridMsg.info.width * y_index + x_index; - // Calculate which index in the global frame the point is in - double x_global = xy_global(0, 0); - double y_global = xy_global(1, 0); - int x_index_global = floor((x_global - x_origin_global) / resolution_global); - int y_index_global = floor((y_global - y_origin_global) / resolution_global); - int i_global = (y_index_global * width_global) + x_index_global; + // dot unit normal with <0, 0, 1> + R3 normal{point.normal_x, point.normal_y, point.normal_z}; + normal.normalize(); + double dot_product = normal.z(); + signed char cost = dot_product < mNormalThreshold ? 100 : 0; - // Update our global map by taking the maximum cost at the given point - //int prev - }); - } + mGlobalGridMsg.data[i] = std::max(mGlobalGridMsg.data[i], cost); + }); + mCostMapPub.publish(mGlobalGridMsg); } } // namespace mrover \ No newline at end of file diff --git a/src/perception/cost_map/pch.hpp b/src/perception/cost_map/pch.hpp index e45a989d6..c34257915 100644 --- a/src/perception/cost_map/pch.hpp +++ b/src/perception/cost_map/pch.hpp @@ -4,7 +4,6 @@ #include #include #include -#include #include #include #include diff --git a/src/perception/point.hpp b/src/perception/point.hpp index 2bfa9925d..71ba7cb3f 100644 --- a/src/perception/point.hpp +++ b/src/perception/point.hpp @@ -1,5 +1,7 @@ #pragma once +#include +#include namespace mrover { /** From ea4471e66b39f8abea7cd5c339a01426ef478030 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Tue, 16 Jan 2024 18:03:08 -0500 Subject: [PATCH 25/76] fixed build and header --- .../cost_map/cost_map.processing.cpp | 24 +++++++++++++++---- src/perception/cost_map/pch.hpp | 11 +++++++-- src/perception/point.hpp | 2 -- src/perception/zed_wrapper/zed_wrapper.cpp | 3 +++ src/perception/zed_wrapper/zed_wrapper.hpp | 3 ++- 5 files changed, 33 insertions(+), 10 deletions(-) diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 16237e065..7038fa0b1 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -2,10 +2,7 @@ #include "../point.hpp" #include -#include #include -#include -#include namespace mrover { @@ -20,7 +17,7 @@ namespace mrover { auto* points = reinterpret_cast(msg->data.data()); std::for_each(points, points + msg->width * msg->height, [&](auto& point) { - SE3 point_in_zed{R3{point.x, point.y, 0.0}}; + SE3 point_in_zed{R3{point.x, point.y, 0.0}, {}}; SE3 point_in_map = zed_to_map * point_in_zed; int x_index = floor((point_in_map.position().x() - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); @@ -37,4 +34,21 @@ namespace mrover { }); mCostMapPub.publish(mGlobalGridMsg); } -} // namespace mrover \ No newline at end of file +} // namespace mrover + +int main(int argc, char** argv) { + ros::init(argc, argv, "costmap"); + + // Start Costmap Nodelet + nodelet::Loader nodelet; + nodelet.load(ros::this_node::getName(), "mrover/CostMapNodelet", ros::names::getRemappings(), {}); + + ros::spin(); + + return EXIT_SUCCESS; +} + +#ifdef MROVER_IS_NODELET +#include +PLUGINLIB_EXPORT_CLASS(mrover::CostMapNodelet, nodelet::Nodelet) +#endif \ No newline at end of file diff --git a/src/perception/cost_map/pch.hpp b/src/perception/cost_map/pch.hpp index c34257915..4f4b3a77c 100644 --- a/src/perception/cost_map/pch.hpp +++ b/src/perception/cost_map/pch.hpp @@ -1,6 +1,10 @@ #pragma once -#include +#include +#include + +#include + #include #include #include @@ -8,4 +12,7 @@ #include #include #include -#include \ No newline at end of file +#include +#include + +#include \ No newline at end of file diff --git a/src/perception/point.hpp b/src/perception/point.hpp index 71ba7cb3f..2bfa9925d 100644 --- a/src/perception/point.hpp +++ b/src/perception/point.hpp @@ -1,7 +1,5 @@ #pragma once -#include -#include namespace mrover { /** diff --git a/src/perception/zed_wrapper/zed_wrapper.cpp b/src/perception/zed_wrapper/zed_wrapper.cpp index f69ef66ba..e04fda56f 100644 --- a/src/perception/zed_wrapper/zed_wrapper.cpp +++ b/src/perception/zed_wrapper/zed_wrapper.cpp @@ -70,6 +70,7 @@ namespace mrover { mImageResolution = sl::Resolution(imageWidth, imageHeight); mPointResolution = sl::Resolution(imageWidth, imageHeight); + mNormalsResolution = sl::Resolution(imageWidth, imageHeight); NODELET_INFO("Resolution: %s image: %zux%zu points: %zux%zu", grabResolutionString.c_str(), mImageResolution.width, mImageResolution.height, mPointResolution.width, mPointResolution.height); @@ -229,6 +230,8 @@ namespace mrover { throw std::runtime_error("ZED failed to retrieve left image"); if (mZed.retrieveMeasure(mGrabMeasures.leftPoints, sl::MEASURE::XYZ, sl::MEM::GPU, mPointResolution) != sl::ERROR_CODE::SUCCESS) throw std::runtime_error("ZED failed to retrieve point cloud"); + if (mZed.retrieveMeasure(mGrabMeasures.leftNormals, sl::MEASURE::XYZRGBA, sl::MEM::GPU, mNormalsResolution) != sl::ERROR_CODE::SUCCESS) + throw std::runtime_error("ZED failed to retrieve point cloud normals"); assert(mGrabMeasures.leftImage.timestamp == mGrabMeasures.leftPoints.timestamp); diff --git a/src/perception/zed_wrapper/zed_wrapper.hpp b/src/perception/zed_wrapper/zed_wrapper.hpp index cde5d0aaa..cabbe808c 100644 --- a/src/perception/zed_wrapper/zed_wrapper.hpp +++ b/src/perception/zed_wrapper/zed_wrapper.hpp @@ -15,6 +15,7 @@ namespace mrover { sl::Mat leftImage; sl::Mat rightImage; sl::Mat leftPoints; + sl::Mat leftNormals; Measures() = default; @@ -34,7 +35,7 @@ namespace mrover { PointCloudGpu mPointCloudGpu; - sl::Resolution mImageResolution, mPointResolution; + sl::Resolution mImageResolution, mPointResolution, mNormalsResolution; sl::String mSvoPath; int mGrabTargetFps{}; int mDepthConfidence{}; From 70aa5f02621e6454769163ae3f7ebddd82357e7d Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Thu, 18 Jan 2024 19:06:21 -0500 Subject: [PATCH 26/76] add normals to zed processing --- src/perception/zed_wrapper/zed_wrapper.bridge.cu | 11 ++++++++--- src/perception/zed_wrapper/zed_wrapper.cpp | 5 +++-- src/perception/zed_wrapper/zed_wrapper.hpp | 2 +- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/perception/zed_wrapper/zed_wrapper.bridge.cu b/src/perception/zed_wrapper/zed_wrapper.bridge.cu index 5ebab9f35..e7647c150 100644 --- a/src/perception/zed_wrapper/zed_wrapper.bridge.cu +++ b/src/perception/zed_wrapper/zed_wrapper.bridge.cu @@ -11,7 +11,7 @@ namespace mrover { /** * @brief Runs on the GPU, interleaving the XYZ and BGRA buffers into a single buffer of #Point structs. */ - __global__ void fillPointCloudMessageKernel(sl::float4* xyzGpuPtr, sl::uchar4* bgraGpuPtr, Point* pcGpuPtr, size_t size) { + __global__ void fillPointCloudMessageKernel(sl::float4* xyzGpuPtr, sl::uchar4* bgraGpuPtr, sl::float4* normalsGpuPtr, Point* pcGpuPtr, size_t size) { // This function is invoked once per element at index #i in the point cloud size_t i = blockIdx.x * blockDim.x + threadIdx.x; if (i >= size) return; @@ -23,6 +23,10 @@ namespace mrover { pcGpuPtr[i].g = bgraGpuPtr[i].g; pcGpuPtr[i].r = bgraGpuPtr[i].b; pcGpuPtr[i].a = bgraGpuPtr[i].a; + pcGpuPtr[i].normal_x = normalsGpuPtr[i].x; + pcGpuPtr[i].normal_x = normalsGpuPtr[i].y; + pcGpuPtr[i].normal_x = normalsGpuPtr[i].z; + // ROS_WARN(xyzGpuPtr[i].x); } /** @@ -33,7 +37,7 @@ namespace mrover { * @param pcGpu Point cloud buffer on the GPU (@see Point) * @param msg Point cloud message with buffer on the CPU */ - void fillPointCloudMessageFromGpu(sl::Mat& xyzGpu, sl::Mat& bgraGpu, PointCloudGpu& pcGpu, sensor_msgs::PointCloud2Ptr const& msg) { + void fillPointCloudMessageFromGpu(sl::Mat& xyzGpu, sl::Mat& bgraGpu, sl::Mat& normalsGpu, PointCloudGpu& pcGpu, sensor_msgs::PointCloud2Ptr const& msg) { assert(bgraGpu.getWidth() >= xyzGpu.getWidth()); assert(bgraGpu.getHeight() >= xyzGpu.getHeight()); assert(bgraGpu.getChannels() == 4); @@ -42,6 +46,7 @@ namespace mrover { auto* bgraGpuPtr = bgraGpu.getPtr(sl::MEM::GPU); auto* xyzGpuPtr = xyzGpu.getPtr(sl::MEM::GPU); + auto* normalsGpuPtr = normalsGpu.getPtr(sl::MEM::GPU); msg->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; msg->is_dense = true; msg->height = bgraGpu.getHeight(); @@ -53,7 +58,7 @@ namespace mrover { Point* pcGpuPtr = pcGpu.data().get(); dim3 threadsPerBlock{BLOCK_SIZE}; dim3 numBlocks{static_cast(std::ceil(static_cast(size) / BLOCK_SIZE))}; - fillPointCloudMessageKernel<<>>(xyzGpuPtr, bgraGpuPtr, pcGpuPtr, size); + fillPointCloudMessageKernel<<>>(xyzGpuPtr, bgraGpuPtr, normalsGpuPtr, pcGpuPtr, size); checkCudaError(cudaPeekAtLastError()); checkCudaError(cudaMemcpy(msg->data.data(), pcGpuPtr, size * sizeof(Point), cudaMemcpyDeviceToHost)); } diff --git a/src/perception/zed_wrapper/zed_wrapper.cpp b/src/perception/zed_wrapper/zed_wrapper.cpp index e04fda56f..c5cd7182e 100644 --- a/src/perception/zed_wrapper/zed_wrapper.cpp +++ b/src/perception/zed_wrapper/zed_wrapper.cpp @@ -142,7 +142,7 @@ namespace mrover { mIsSwapReady = false; mPcThreadProfiler.measureEvent("Wait"); - fillPointCloudMessageFromGpu(mPcMeasures.leftPoints, mPcMeasures.leftImage, mPointCloudGpu, pointCloudMsg); + fillPointCloudMessageFromGpu(mPcMeasures.leftPoints, mPcMeasures.leftImage, mPcMeasures.leftNormals, mPointCloudGpu, pointCloudMsg); pointCloudMsg->header.seq = mPointCloudUpdateTick; pointCloudMsg->header.stamp = mPcMeasures.time; pointCloudMsg->header.frame_id = "zed2i_left_camera_frame"; @@ -230,7 +230,7 @@ namespace mrover { throw std::runtime_error("ZED failed to retrieve left image"); if (mZed.retrieveMeasure(mGrabMeasures.leftPoints, sl::MEASURE::XYZ, sl::MEM::GPU, mPointResolution) != sl::ERROR_CODE::SUCCESS) throw std::runtime_error("ZED failed to retrieve point cloud"); - if (mZed.retrieveMeasure(mGrabMeasures.leftNormals, sl::MEASURE::XYZRGBA, sl::MEM::GPU, mNormalsResolution) != sl::ERROR_CODE::SUCCESS) + if (mZed.retrieveMeasure(mGrabMeasures.leftNormals, sl::MEASURE::NORMALS, sl::MEM::GPU, mNormalsResolution) != sl::ERROR_CODE::SUCCESS) throw std::runtime_error("ZED failed to retrieve point cloud normals"); assert(mGrabMeasures.leftImage.timestamp == mGrabMeasures.leftPoints.timestamp); @@ -320,6 +320,7 @@ namespace mrover { sl::Mat::swap(other.leftImage, leftImage); sl::Mat::swap(other.rightImage, rightImage); sl::Mat::swap(other.leftPoints, leftPoints); + sl::Mat::swap(other.leftNormals, leftNormals); std::swap(time, other.time); return *this; } diff --git a/src/perception/zed_wrapper/zed_wrapper.hpp b/src/perception/zed_wrapper/zed_wrapper.hpp index cabbe808c..7e80329cb 100644 --- a/src/perception/zed_wrapper/zed_wrapper.hpp +++ b/src/perception/zed_wrapper/zed_wrapper.hpp @@ -74,7 +74,7 @@ namespace mrover { ros::Time slTime2Ros(sl::Timestamp t); - void fillPointCloudMessageFromGpu(sl::Mat& xyzGpu, sl::Mat& bgraGpu, PointCloudGpu& pcGpu, sensor_msgs::PointCloud2Ptr const& msg); + void fillPointCloudMessageFromGpu(sl::Mat& xyzGpu, sl::Mat& bgraGpu, sl::Mat& normalsGpu, PointCloudGpu& pcGpu, sensor_msgs::PointCloud2Ptr const& msg); void fillCameraInfoMessages(sl::CalibrationParameters& calibration, sl::Resolution const& resolution, sensor_msgs::CameraInfoPtr const& leftInfoMsg, sensor_msgs::CameraInfoPtr const& rightInfoMsg); From d452ba443a4496176082e8a406615614a1ac5da7 Mon Sep 17 00:00:00 2001 From: Prabhleen Pawar Date: Sun, 21 Jan 2024 19:04:37 +0000 Subject: [PATCH 27/76] Cartesian Coordinates COnverting Function --- src/navigation/water_bottle_search.py | 43 ++++++++++++++++++++++----- 1 file changed, 36 insertions(+), 7 deletions(-) diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index dc9e9d0e2..0ef1634df 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -37,29 +37,58 @@ class WaterBottleSearchState(State): # TODO: Data Structure to store the information given from the map (Look at navigation) - # 2D list + # 2D list and map parts costMap = [] + height = 0 + width = 0 + resolution = 0 + + #Shfited Center + origin = [] + center = [] # TODO: Make call_back function to push into data structure def costmap_callback(self, msg: OccupancyGrid): # update data structure - height = msg.info.height - width = msg.info.width - rospy.loginfo(f"height: {height}, width: {width}") + self.height = msg.info.height * msg.info.resolution + self.width = msg.info.width * msg.info.resolution + self.resolution = msg.info.resolution + rospy.loginfo(f"height: {self.height}, width: {self.width}") costmap2D = np.array(msg.data) - costmap2D.reshape(height, width) + costmap2D.reshape(self.height, self.width) rospy.loginfo(f"2D costmap: {costmap2D}") - + self.origin = self.center - [self.width/2, self.height/2] # Call A-STAR - # TODO: A-STAR Algorithm: f(n) = g(n) + h(n) + + """ + # TODO: A-STAR Algorithm: f(n) = g(n) + h(n) # def a_star():j + # start = rover pose (Cartesian) + # end = next point in the spiral + """ + def a_star(self, costmap2d, start, end) -> list | None: + #Convert start and end to local cartesian coordinates + + + #Do the check for high cost for the end point + + pass + + """ + #TODO: Convert global to i and j (Occupancy grid) + + """ + #def cartesian_convert(self, cart_coord: np.ndarray) -> np.ndarray: + + def on_enter(self, context) -> None: self.listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) search_center = context.course.current_waypoint() if not self.is_recovering: + self.center = context.rover.get_pose().position[0:2] self.traj = SearchTrajectory.spiral_traj( context.rover.get_pose().position[0:2], self.SPIRAL_COVERAGE_RADIUS, From beae1071cd9b5cd31e0856f7785cb904169edde0 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Tue, 23 Jan 2024 18:50:38 -0500 Subject: [PATCH 28/76] Fix compile --- src/perception/cost_map/cost_map.cpp | 2 +- .../cost_map/cost_map.processing.cpp | 19 +------------------ 2 files changed, 2 insertions(+), 19 deletions(-) diff --git a/src/perception/cost_map/cost_map.cpp b/src/perception/cost_map/cost_map.cpp index cf0f60ae7..0d75bb10b 100644 --- a/src/perception/cost_map/cost_map.cpp +++ b/src/perception/cost_map/cost_map.cpp @@ -37,4 +37,4 @@ PARALLELIZE - Stitch to nav grid END -*/ \ No newline at end of file +*/ diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 7038fa0b1..8b014e151 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -15,7 +15,7 @@ namespace mrover { SE3 zed_to_map = SE3::fromTfTree(tf_buffer, "zed2i_left_camera_frame", "map"); - auto* points = reinterpret_cast(msg->data.data()); + auto* points = reinterpret_cast(msg->data.data()); std::for_each(points, points + msg->width * msg->height, [&](auto& point) { SE3 point_in_zed{R3{point.x, point.y, 0.0}, {}}; SE3 point_in_map = zed_to_map * point_in_zed; @@ -35,20 +35,3 @@ namespace mrover { mCostMapPub.publish(mGlobalGridMsg); } } // namespace mrover - -int main(int argc, char** argv) { - ros::init(argc, argv, "costmap"); - - // Start Costmap Nodelet - nodelet::Loader nodelet; - nodelet.load(ros::this_node::getName(), "mrover/CostMapNodelet", ros::names::getRemappings(), {}); - - ros::spin(); - - return EXIT_SUCCESS; -} - -#ifdef MROVER_IS_NODELET -#include -PLUGINLIB_EXPORT_CLASS(mrover::CostMapNodelet, nodelet::Nodelet) -#endif \ No newline at end of file From 40acab387fd32f81f6c5ea6b7c53659e4797be6d Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Tue, 23 Jan 2024 19:20:27 -0500 Subject: [PATCH 29/76] Update --- src/perception/cost_map/cost_map.hpp | 3 +- .../cost_map/cost_map.processing.cpp | 44 ++++++++++--------- src/perception/cost_map/main.cpp | 22 ++++++++++ 3 files changed, 47 insertions(+), 22 deletions(-) create mode 100644 src/perception/cost_map/main.cpp diff --git a/src/perception/cost_map/cost_map.hpp b/src/perception/cost_map/cost_map.hpp index 89d3a3419..3524e1a5c 100644 --- a/src/perception/cost_map/cost_map.hpp +++ b/src/perception/cost_map/cost_map.hpp @@ -14,7 +14,8 @@ namespace mrover { float mResolution{}; // Cells per meter int mDimension{}; // Dimensions of the square costmap in meters float mNormalThreshold = 0.5; - const tf2_ros::Buffer tf_buffer = tf2_ros::Buffer(); + tf2_ros::Buffer tf_buffer; + tf2_ros::TransformListener tf_listener{tf_buffer}; std::optional mPreviousPose; nav_msgs::OccupancyGrid mGlobalGridMsg; diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 8b014e151..b9f879c3b 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -12,26 +12,28 @@ namespace mrover { assert(msg->width > 0); if (!mPublishCostMap) return; - - SE3 zed_to_map = SE3::fromTfTree(tf_buffer, "zed2i_left_camera_frame", "map"); - - auto* points = reinterpret_cast(msg->data.data()); - std::for_each(points, points + msg->width * msg->height, [&](auto& point) { - SE3 point_in_zed{R3{point.x, point.y, 0.0}, {}}; - SE3 point_in_map = zed_to_map * point_in_zed; - - int x_index = floor((point_in_map.position().x() - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); - int y_index = floor((point_in_map.position().y() - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); - auto i = mGlobalGridMsg.info.width * y_index + x_index; - - // dot unit normal with <0, 0, 1> - R3 normal{point.normal_x, point.normal_y, point.normal_z}; - normal.normalize(); - double dot_product = normal.z(); - signed char cost = dot_product < mNormalThreshold ? 100 : 0; - - mGlobalGridMsg.data[i] = std::max(mGlobalGridMsg.data[i], cost); - }); - mCostMapPub.publish(mGlobalGridMsg); + try { + SE3 zed_to_map = SE3::fromTfTree(tf_buffer, "zed2i_left_camera_frame", "map"); + auto* points = reinterpret_cast(msg->data.data()); + // std::for_each(points, points + msg->width * msg->height, [&](auto& point) { + for (auto point = points; point - points < msg->width * msg->height; point++) { + SE3 point_in_zed{R3{point->x, point->y, 0.0}, {}}; + SE3 point_in_map = zed_to_map * point_in_zed; + + int x_index = floor((point_in_map.position().x() - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); + int y_index = floor((point_in_map.position().y() - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); + auto i = mGlobalGridMsg.info.width * y_index + x_index; + + // dot unit normal with <0, 0, 1> + R3 normal{point->normal_x, point->normal_y, point->normal_z}; + normal.normalize(); + double dot_product = normal.z(); + signed char cost = dot_product < mNormalThreshold ? 100 : 0; + + mGlobalGridMsg.data[i] = std::max(mGlobalGridMsg.data[i], cost); + } + mCostMapPub.publish(mGlobalGridMsg); + } catch (...) { + } } } // namespace mrover diff --git a/src/perception/cost_map/main.cpp b/src/perception/cost_map/main.cpp new file mode 100644 index 000000000..1850df82a --- /dev/null +++ b/src/perception/cost_map/main.cpp @@ -0,0 +1,22 @@ +#include "cost_map.hpp" + +#ifdef MROVER_IS_NODELET + +#include +PLUGINLIB_EXPORT_CLASS(mrover::CostMapNodelet, nodelet::Nodelet) + +#else + +int main(int argc, char** argv) { + ros::init(argc, argv, "costmap"); + + // Start Costmap Nodelet + nodelet::Loader nodelet; + nodelet.load(ros::this_node::getName(), "mrover/CostMapNodelet", ros::names::getRemappings(), {}); + + ros::spin(); + + return EXIT_SUCCESS; +} + +#endif From 41c468183526660d250415e576f94a57fb379d07 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Tue, 23 Jan 2024 20:01:44 -0500 Subject: [PATCH 30/76] costmap stuff --- src/perception/cost_map/cost_map.cpp | 13 ++++++------- src/perception/cost_map/cost_map.hpp | 2 +- src/perception/cost_map/cost_map.processing.cpp | 6 ++++-- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/perception/cost_map/cost_map.cpp b/src/perception/cost_map/cost_map.cpp index 0d75bb10b..41539636b 100644 --- a/src/perception/cost_map/cost_map.cpp +++ b/src/perception/cost_map/cost_map.cpp @@ -6,8 +6,8 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); // Unused for now mNh.param("publish_cost_map", mPublishCostMap, true); - mNh.param("resolution", mResolution, 2); - mNh.param("map_width", mDimension, 15); + mNh.param("resolution", mResolution, 1); + mNh.param("map_width", mDimension, 30); mCostMapPub = mCmt.advertise("costmap", 1); // We publish our results to "costmap" @@ -15,16 +15,15 @@ namespace mrover { mPcSub = mNh.subscribe("camera/left/points", 1, &CostMapNodelet::pointCloudCallback, this); mGlobalGridMsg.info.resolution = mResolution; - mGlobalGridMsg.info.width = mDimension; - mGlobalGridMsg.info.height = mDimension; + mGlobalGridMsg.info.width = (int) (mDimension / mResolution); + mGlobalGridMsg.info.height = (int) (mDimension / mResolution); //TODO: What do we choose as our original load time and origin? //globalGridMsg.info.map_load_time = //globalGridMsg.info.origin = //TODO: Why is int8 not recognized? - std::vector initial_map; - std::fill(initial_map.begin(), initial_map.end(), -1); - mGlobalGridMsg.data = initial_map; + mGlobalGridMsg.data.resize(mGlobalGridMsg.info.width * mGlobalGridMsg.info.height); + std::fill(mGlobalGridMsg.data.begin(), mGlobalGridMsg.data.end(), -1); } } // namespace mrover diff --git a/src/perception/cost_map/cost_map.hpp b/src/perception/cost_map/cost_map.hpp index 3524e1a5c..9e83e92bc 100644 --- a/src/perception/cost_map/cost_map.hpp +++ b/src/perception/cost_map/cost_map.hpp @@ -12,7 +12,7 @@ namespace mrover { bool mPublishCostMap{}; // If set, publish the global costmap float mResolution{}; // Cells per meter - int mDimension{}; // Dimensions of the square costmap in meters + float mDimension{}; // Dimensions of the square costmap in meters float mNormalThreshold = 0.5; tf2_ros::Buffer tf_buffer; tf2_ros::TransformListener tf_listener{tf_buffer}; diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index b9f879c3b..fe0fee922 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -20,8 +20,10 @@ namespace mrover { SE3 point_in_zed{R3{point->x, point->y, 0.0}, {}}; SE3 point_in_map = zed_to_map * point_in_zed; - int x_index = floor((point_in_map.position().x() - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); - int y_index = floor((point_in_map.position().y() - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); + double x = point_in_map.position().x(); + double y = point_in_map.position().y(); + int x_index = floor((x - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); + int y_index = floor((y - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); auto i = mGlobalGridMsg.info.width * y_index + x_index; // dot unit normal with <0, 0, 1> From 3039c9c8074fbf7d63fecbdada75846bdf340ead Mon Sep 17 00:00:00 2001 From: qhdwight Date: Wed, 24 Jan 2024 16:24:54 -0500 Subject: [PATCH 31/76] Remove old file --- src/perception/cost_map/pch.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/perception/cost_map/pch.hpp b/src/perception/cost_map/pch.hpp index 4f4b3a77c..a66e0ce0c 100644 --- a/src/perception/cost_map/pch.hpp +++ b/src/perception/cost_map/pch.hpp @@ -3,8 +3,6 @@ #include #include -#include - #include #include #include From 36c1351c498fadeb4b15364ce1c8750e527d1bc7 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Thu, 25 Jan 2024 19:19:07 -0500 Subject: [PATCH 32/76] costmap node --- package.xml | 1 + plugins/costmap.xml | 6 ++++++ src/perception/cost_map/cost_map.processing.cpp | 7 ++++--- src/simulator/simulator.cpp | 2 +- 4 files changed, 12 insertions(+), 4 deletions(-) create mode 100644 plugins/costmap.xml diff --git a/package.xml b/package.xml index 0839b8d8f..624da5068 100644 --- a/package.xml +++ b/package.xml @@ -82,6 +82,7 @@ + diff --git a/plugins/costmap.xml b/plugins/costmap.xml new file mode 100644 index 000000000..2873e5822 --- /dev/null +++ b/plugins/costmap.xml @@ -0,0 +1,6 @@ + + + + diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index fe0fee922..166428fb6 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -26,11 +26,12 @@ namespace mrover { int y_index = floor((y - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); auto i = mGlobalGridMsg.info.width * y_index + x_index; - // dot unit normal with <0, 0, 1> R3 normal{point->normal_x, point->normal_y, point->normal_z}; normal.normalize(); - double dot_product = normal.z(); - signed char cost = dot_product < mNormalThreshold ? 100 : 0; + // get vertical component of (unit) normal vector + double z_comp = normal.z(); + // small z component indicates largely horizontal normal (surface is vertical) + signed char cost = z_comp < mNormalThreshold ? 100 : 0; mGlobalGridMsg.data[i] = std::max(mGlobalGridMsg.data[i], cost); } diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index 0566d7147..99f25cda9 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -67,7 +67,7 @@ namespace mrover { #else glfwPollEvents(); #endif - // glfwSetInputMode(mWindow.get(), GLFW_CURSOR, mInGui ? GLFW_CURSOR_NORMAL : GLFW_CURSOR_DISABLED); + glfwSetInputMode(mWindow.get(), GLFW_CURSOR, mInGui ? GLFW_CURSOR_NORMAL : GLFW_CURSOR_DISABLED); mLoopProfiler.measureEvent("GLFW Events"); userControls(dt); From 996575b61599f75125612cfe3e7220ec44837285 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Thu, 25 Jan 2024 19:54:10 -0500 Subject: [PATCH 33/76] normal tweaks --- src/perception/cost_map/cost_map.processing.cpp | 7 ++++--- urdf/rover/rover.urdf.xacro | 2 +- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 166428fb6..fb1b56980 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -26,10 +26,11 @@ namespace mrover { int y_index = floor((y - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); auto i = mGlobalGridMsg.info.width * y_index + x_index; - R3 normal{point->normal_x, point->normal_y, point->normal_z}; - normal.normalize(); + R3 normal_in_zed{point->normal_x, point->normal_y, point->normal_z}; + R3 normal_in_map = zed_to_map.rotation().quaternion() * normal_in_zed; + normal_in_map.normalize(); // get vertical component of (unit) normal vector - double z_comp = normal.z(); + double z_comp = normal_in_map.z(); // small z component indicates largely horizontal normal (surface is vertical) signed char cost = z_comp < mNormalThreshold ? 100 : 0; diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 7869e4836..952c76118 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -278,7 +278,7 @@ - + From 52a5bd8d18176d8fb8458002fc6b95c702a33a31 Mon Sep 17 00:00:00 2001 From: Prabhleen Pawar Date: Fri, 26 Jan 2024 01:04:16 +0000 Subject: [PATCH 34/76] Conversion function (Real World -> Occupancy Grid) & (Occupancy Grid -> Real World) --- src/navigation/water_bottle_search.py | 25 ++++++++++++++++++++++--- 1 file changed, 22 insertions(+), 3 deletions(-) diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index 0ef1634df..8a1e683e5 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -4,6 +4,7 @@ from typing import Optional import numpy as np +import math import rospy from mrover.msg import GPSPointList from util.ros_utils import get_rosparam @@ -77,11 +78,28 @@ def a_star(self, costmap2d, start, end) -> list | None: pass """ - #TODO: Convert global to i and j (Occupancy grid) + #Convert global(Real World) to i and j (Occupancy grid) + cart_coord: These are the i and j coordinates + width_height: These are the width and height list (m/cell) + floor(v - (WP + [-W/2, H/2]) / r) [1, -1] + return: i and j coordinates for the occupancy grid + """ + def cartesian_convert(self, cart_coord: np.ndarray) -> np.ndarray: + width_height = [-self.width / 2, self.height / 2] + converted_coord = (cart_coord - (self.origin + (width_height) / self.resolution))* [1,-1] + return math.floor(converted_coord) """ - #def cartesian_convert(self, cart_coord: np.ndarray) -> np.ndarray: - + #Convert Occupancy grid(i, j) to x and y coordinates + real_coord: These are the x and y coordinates + width_height: These are the width and height list (m/cell) + (WP - [W/2, H/2]) + [j x r, -i x r] + [r/2, -r/2] + """ + def real_world_convert(self, real_coord : np.ndarray) -> np.ndarray: + width_height = [self.width/2, self.height/2] + resolution_conversion = [-real_coord * self.resolution] + half_res = [self.resolution/2, -self.resolution/2] + return self.origin - width_height + resolution_conversion + half_res def on_enter(self, context) -> None: @@ -96,6 +114,7 @@ def on_enter(self, context) -> None: self.SEGMENTS_PER_ROTATION, search_center.fiducial_id, ) + self.origin = context.rover.get_pose().position[0:2] self.prev_target = None def on_exit(self, context) -> None: From 4a4afa87f95149837e427743428a7e3fe2c7d935 Mon Sep 17 00:00:00 2001 From: Prabhleen Pawar Date: Fri, 26 Jan 2024 19:17:54 -0500 Subject: [PATCH 35/76] Added setup for the A* program --- src/navigation/water_bottle_search.py | 74 +++++++++++++++++++++------ 1 file changed, 58 insertions(+), 16 deletions(-) diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index 8a1e683e5..b138b7baa 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -2,7 +2,7 @@ from dataclasses import dataclass from typing import Optional - +import heapq import numpy as np import math import rospy @@ -21,6 +21,23 @@ class WaterBottleSearchState(State): + class Node: + def __init__(self, parent=None, position=None): + self.parent = parent + self.position = position + + self.g = 0 + self.h = 0 + self.f = 0 + def __eq__(self, other): + return self.position == other.position + # defining less than for purposes of heap queue + def __lt__(self, other): + return self.f < other.f + + # defining greater than for purposes of heap queue + def __gt__(self, other): + return self.f > other.f # Spiral traj: SearchTrajectory # when we are moving along the spiral @@ -62,21 +79,6 @@ def costmap_callback(self, msg: OccupancyGrid): self.origin = self.center - [self.width/2, self.height/2] # Call A-STAR - - """ - # TODO: A-STAR Algorithm: f(n) = g(n) + h(n) - # def a_star():j - # start = rover pose (Cartesian) - # end = next point in the spiral - """ - def a_star(self, costmap2d, start, end) -> list | None: - #Convert start and end to local cartesian coordinates - - - #Do the check for high cost for the end point - - pass - """ #Convert global(Real World) to i and j (Occupancy grid) cart_coord: These are the i and j coordinates @@ -100,6 +102,46 @@ def real_world_convert(self, real_coord : np.ndarray) -> np.ndarray: resolution_conversion = [-real_coord * self.resolution] half_res = [self.resolution/2, -self.resolution/2] return self.origin - width_height + resolution_conversion + half_res + + """ + # TODO: A-STAR Algorithm: f(n) = g(n) + h(n) + # def a_star():j + # start = rover pose (Cartesian) + # end = next point in the spiral + """ + def a_star(self, costmap2d, start: np.ndarray, end: np.ndarray) -> list | None: + #Convert start and end to local cartesian coordinates + startij = self.cartesian_convert(start) + endij = self.cartesian_convert(end) + + #Do the check for high cost for the end point + if costmap2d[endij[0]][endij[1]] >= 100: + if not self.traj.increment_point(): + end = self.traj.get_cur_pt + #TODO What do we do in this case where we don't have another point in da spiral + else: + pass + + #Intialize nodes: + start_node = self.Node(None, startij) + start_node.g = start_node.h = start_node.f = 0 + end_node = self.Node(None, endij) + end_node.g = end_node.h = end_node.f = 0 + + # Check if end node is within range, if it is, check cost. Return None for path so we can call astar with new incremented end goal + # if end_node.position[0] <= (len(self.width) - 1) and end_node.position[0] >= 0 and end_node.position[1] <= (self.height) and end_node.position[1] >= 0: + # if costmap2d[end_node.position[0]][end_node.position[1]] == 100: + # print("Target has high cost!") + # return None + + # Initialize both open and closed list + open_list = [] + closed_list = [] + + # Heapify the open_list and Add the start node + heapq.heapify(open_list) + heapq.heappush(open_list, start_node) + def on_enter(self, context) -> None: From 04654f3939051a235f313f5ff4f185faec0ecb8d Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Tue, 30 Jan 2024 19:28:01 -0500 Subject: [PATCH 36/76] pointcloud working --- src/perception/cost_map/cost_map.cpp | 10 ++--- src/perception/cost_map/cost_map.hpp | 2 +- .../cost_map/cost_map.processing.cpp | 39 +++++++++++-------- src/util/lie/se3.cpp | 4 +- urdf/meshes/bottle.fbx | 2 +- urdf/meshes/ground.fbx | 4 +- 6 files changed, 32 insertions(+), 29 deletions(-) diff --git a/src/perception/cost_map/cost_map.cpp b/src/perception/cost_map/cost_map.cpp index 41539636b..7d3dfa46d 100644 --- a/src/perception/cost_map/cost_map.cpp +++ b/src/perception/cost_map/cost_map.cpp @@ -6,22 +6,20 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); // Unused for now mNh.param("publish_cost_map", mPublishCostMap, true); - mNh.param("resolution", mResolution, 1); + mNh.param("resolution", mResolution, 0.5); mNh.param("map_width", mDimension, 30); mCostMapPub = mCmt.advertise("costmap", 1); // We publish our results to "costmap" - //TODO: Change topic to whatever the occupancy grid creating node publishes to mPcSub = mNh.subscribe("camera/left/points", 1, &CostMapNodelet::pointCloudCallback, this); mGlobalGridMsg.info.resolution = mResolution; mGlobalGridMsg.info.width = (int) (mDimension / mResolution); mGlobalGridMsg.info.height = (int) (mDimension / mResolution); - //TODO: What do we choose as our original load time and origin? - //globalGridMsg.info.map_load_time = - //globalGridMsg.info.origin = + // make center of map at (0, 0) + mGlobalGridMsg.info.origin.position.x = -1 * mDimension / 2; + mGlobalGridMsg.info.origin.position.y = -1 * mDimension / 2; - //TODO: Why is int8 not recognized? mGlobalGridMsg.data.resize(mGlobalGridMsg.info.width * mGlobalGridMsg.info.height); std::fill(mGlobalGridMsg.data.begin(), mGlobalGridMsg.data.end(), -1); } diff --git a/src/perception/cost_map/cost_map.hpp b/src/perception/cost_map/cost_map.hpp index 9e83e92bc..21ca1b742 100644 --- a/src/perception/cost_map/cost_map.hpp +++ b/src/perception/cost_map/cost_map.hpp @@ -11,7 +11,7 @@ namespace mrover { void onInit() override; bool mPublishCostMap{}; // If set, publish the global costmap - float mResolution{}; // Cells per meter + float mResolution{}; // Meters per cell float mDimension{}; // Dimensions of the square costmap in meters float mNormalThreshold = 0.5; tf2_ros::Buffer tf_buffer; diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index fb1b56980..68d73c216 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -1,8 +1,11 @@ #include "cost_map.hpp" #include "../point.hpp" +#include "loop_profiler.hpp" +#include #include #include +#include namespace mrover { @@ -13,28 +16,30 @@ namespace mrover { if (!mPublishCostMap) return; try { - SE3 zed_to_map = SE3::fromTfTree(tf_buffer, "zed2i_left_camera_frame", "map"); + SE3 zed_to_map = SE3::fromTfTree(tf_buffer, "map", "zed2i_left_camera_frame"); auto* points = reinterpret_cast(msg->data.data()); - // std::for_each(points, points + msg->width * msg->height, [&](auto& point) { + // std::for_each(points, points + msg->width * msg->height, [&](auto* point) { for (auto point = points; point - points < msg->width * msg->height; point++) { - SE3 point_in_zed{R3{point->x, point->y, 0.0}, {}}; - SE3 point_in_map = zed_to_map * point_in_zed; + Eigen::Vector4d point_in_map = zed_to_map.matrix() * Eigen::Vector4d{point->x, point->y, 0, 1}; - double x = point_in_map.position().x(); - double y = point_in_map.position().y(); - int x_index = floor((x - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); - int y_index = floor((y - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); - auto i = mGlobalGridMsg.info.width * y_index + x_index; + double x = point_in_map.x(); + double y = point_in_map.y(); + if (x >= -1 * mDimension / 2 && x <= mDimension / 2 && + y >= -1 * mDimension / 2 && y <= mDimension / 2) { + int x_index = floor((x - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); + int y_index = floor((y - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); + auto i = mGlobalGridMsg.info.width * y_index + x_index; - R3 normal_in_zed{point->normal_x, point->normal_y, point->normal_z}; - R3 normal_in_map = zed_to_map.rotation().quaternion() * normal_in_zed; - normal_in_map.normalize(); - // get vertical component of (unit) normal vector - double z_comp = normal_in_map.z(); - // small z component indicates largely horizontal normal (surface is vertical) - signed char cost = z_comp < mNormalThreshold ? 100 : 0; + Eigen::Vector3d normal_in_zed{point->normal_x, point->normal_y, point->normal_z}; + Eigen::Vector3d normal_in_map = zed_to_map.rotation() * normal_in_zed; + normal_in_map.normalize(); + // get vertical component of (unit) normal vector + double z_comp = normal_in_map.z(); + // small z component indicates largely horizontal normal (surface is vertical) + signed char cost = z_comp < mNormalThreshold ? 100 : 0; - mGlobalGridMsg.data[i] = std::max(mGlobalGridMsg.data[i], cost); + mGlobalGridMsg.data[i] = std::max(mGlobalGridMsg.data[i], cost); + } } mCostMapPub.publish(mGlobalGridMsg); } catch (...) { diff --git a/src/util/lie/se3.cpp b/src/util/lie/se3.cpp index ce4e4593d..f93e1e791 100644 --- a/src/util/lie/se3.cpp +++ b/src/util/lie/se3.cpp @@ -1,7 +1,7 @@ #include "se3.hpp" -SE3 SE3::fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) { - geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{}); +SE3 SE3::fromTfTree(tf2_ros::Buffer const& buffer, std::string const& toFrameId, std::string const& fromFrameId) { + geometry_msgs::TransformStamped transform = buffer.lookupTransform(toFrameId, fromFrameId, ros::Time{}); return fromTf(transform.transform); } diff --git a/urdf/meshes/bottle.fbx b/urdf/meshes/bottle.fbx index 25d7a8eed..5602ef9c2 100644 --- a/urdf/meshes/bottle.fbx +++ b/urdf/meshes/bottle.fbx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:df9dfe054c07005d820d7c81906136bd1bf621ebf4e4dcf3fbfb52f7e0fb4716 +oid sha256:a6eda3d1f92892e01fcf2ad3dff4421d59ef1f4688fbe8e7e700151065a5b6be size 17596 diff --git a/urdf/meshes/ground.fbx b/urdf/meshes/ground.fbx index 05b93f260..1e06dd797 100644 --- a/urdf/meshes/ground.fbx +++ b/urdf/meshes/ground.fbx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:8323c262d0e44f0f1d128a3c68752ef95b70a9b4602de2ff06f43cef106d4b31 -size 43772 +oid sha256:eb9df0b23202bdbaf6319b67e2e723197edec84fba09b53ee2c18ffbdf454626 +size 39708 From e987b17a34de35af449e158c771185a7c6fea66d Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Tue, 30 Jan 2024 19:56:48 -0500 Subject: [PATCH 37/76] fast costmap --- src/perception/cost_map/cost_map.cpp | 2 +- src/perception/cost_map/cost_map.hpp | 2 +- .../cost_map/cost_map.processing.cpp | 30 ++++++++++++++----- 3 files changed, 24 insertions(+), 10 deletions(-) diff --git a/src/perception/cost_map/cost_map.cpp b/src/perception/cost_map/cost_map.cpp index 7d3dfa46d..bceeafc09 100644 --- a/src/perception/cost_map/cost_map.cpp +++ b/src/perception/cost_map/cost_map.cpp @@ -7,7 +7,7 @@ namespace mrover { mPnh = getMTPrivateNodeHandle(); // Unused for now mNh.param("publish_cost_map", mPublishCostMap, true); mNh.param("resolution", mResolution, 0.5); - mNh.param("map_width", mDimension, 30); + mNh.param("map_width", mDimension, 60); mCostMapPub = mCmt.advertise("costmap", 1); // We publish our results to "costmap" diff --git a/src/perception/cost_map/cost_map.hpp b/src/perception/cost_map/cost_map.hpp index 21ca1b742..d80365925 100644 --- a/src/perception/cost_map/cost_map.hpp +++ b/src/perception/cost_map/cost_map.hpp @@ -13,7 +13,7 @@ namespace mrover { bool mPublishCostMap{}; // If set, publish the global costmap float mResolution{}; // Meters per cell float mDimension{}; // Dimensions of the square costmap in meters - float mNormalThreshold = 0.5; + float mNormalThreshold = 0.9; tf2_ros::Buffer tf_buffer; tf2_ros::TransformListener tf_listener{tf_buffer}; diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 68d73c216..07e96b615 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -4,6 +4,7 @@ #include "loop_profiler.hpp" #include #include +#include #include #include @@ -18,23 +19,36 @@ namespace mrover { try { SE3 zed_to_map = SE3::fromTfTree(tf_buffer, "map", "zed2i_left_camera_frame"); auto* points = reinterpret_cast(msg->data.data()); + int step = 2; + uint32_t num_points = msg->width * msg->height / step; + Eigen::MatrixXd point_matrix(4, num_points); + Eigen::MatrixXd normal_matrix(4, num_points); // std::for_each(points, points + msg->width * msg->height, [&](auto* point) { - for (auto point = points; point - points < msg->width * msg->height; point++) { - Eigen::Vector4d point_in_map = zed_to_map.matrix() * Eigen::Vector4d{point->x, point->y, 0, 1}; + for (auto point = points; point - points < msg->width * msg->height; point += step) { + Eigen::Vector4d p{point->x, point->y, 0, 1}; + point_matrix.col(point - points) = p; + Eigen::Vector4d normal{point->normal_x, point->normal_y, point->normal_z, 0}; + normal_matrix.col(point - points) = normal; + } + + point_matrix = zed_to_map.matrix() * point_matrix; + normal_matrix = zed_to_map.matrix() * normal_matrix; + + for (uint32_t i = 0; i < num_points; i++) { + double x = point_matrix(0, i); + double y = point_matrix(1, i); + Eigen::Vector4d n = normal_matrix.col(i); - double x = point_in_map.x(); - double y = point_in_map.y(); if (x >= -1 * mDimension / 2 && x <= mDimension / 2 && y >= -1 * mDimension / 2 && y <= mDimension / 2) { int x_index = floor((x - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); int y_index = floor((y - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); auto i = mGlobalGridMsg.info.width * y_index + x_index; - Eigen::Vector3d normal_in_zed{point->normal_x, point->normal_y, point->normal_z}; - Eigen::Vector3d normal_in_map = zed_to_map.rotation() * normal_in_zed; - normal_in_map.normalize(); + Eigen::Vector3d normal{n.x(), n.y(), n.z()}; + normal.normalize(); // get vertical component of (unit) normal vector - double z_comp = normal_in_map.z(); + double z_comp = normal.z(); // small z component indicates largely horizontal normal (surface is vertical) signed char cost = z_comp < mNormalThreshold ? 100 : 0; From 99bb27103b5fb8309e74289f3ed2628268345f99 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Tue, 30 Jan 2024 19:58:45 -0500 Subject: [PATCH 38/76] fixed costmap --- src/perception/cost_map/cost_map.processing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 07e96b615..4f4b6f29d 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -19,7 +19,7 @@ namespace mrover { try { SE3 zed_to_map = SE3::fromTfTree(tf_buffer, "map", "zed2i_left_camera_frame"); auto* points = reinterpret_cast(msg->data.data()); - int step = 2; + int step = 1; uint32_t num_points = msg->width * msg->height / step; Eigen::MatrixXd point_matrix(4, num_points); Eigen::MatrixXd normal_matrix(4, num_points); From 227170c5a55d1ece294275ea4f376ba462c3e24d Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Thu, 1 Feb 2024 18:16:23 -0500 Subject: [PATCH 39/76] faster costmap --- src/perception/cost_map/cost_map.processing.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 4f4b6f29d..7e115a4aa 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -19,16 +19,16 @@ namespace mrover { try { SE3 zed_to_map = SE3::fromTfTree(tf_buffer, "map", "zed2i_left_camera_frame"); auto* points = reinterpret_cast(msg->data.data()); - int step = 1; + int step = 4; uint32_t num_points = msg->width * msg->height / step; Eigen::MatrixXd point_matrix(4, num_points); Eigen::MatrixXd normal_matrix(4, num_points); // std::for_each(points, points + msg->width * msg->height, [&](auto* point) { for (auto point = points; point - points < msg->width * msg->height; point += step) { Eigen::Vector4d p{point->x, point->y, 0, 1}; - point_matrix.col(point - points) = p; + point_matrix.col((point - points) / step) = p; Eigen::Vector4d normal{point->normal_x, point->normal_y, point->normal_z, 0}; - normal_matrix.col(point - points) = normal; + normal_matrix.col((point - points) / step) = normal; } point_matrix = zed_to_map.matrix() * point_matrix; From 6b745317c25f1f21ff0e56904ed55fef137eb92f Mon Sep 17 00:00:00 2001 From: Prabhleen Pawar Date: Thu, 1 Feb 2024 19:15:07 -0500 Subject: [PATCH 40/76] Finished adding A-Start --- src/navigation/water_bottle_search.py | 128 +++++++++++++++++--------- 1 file changed, 82 insertions(+), 46 deletions(-) diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index b138b7baa..df11d4fa4 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -1,31 +1,26 @@ from __future__ import annotations - from dataclasses import dataclass from typing import Optional import heapq import numpy as np -import math +import random +import math import rospy from mrover.msg import GPSPointList from util.ros_utils import get_rosparam from util.state_lib.state import State - from nav_msgs.msg import OccupancyGrid, MapMetaData from navigation import approach_post, recovery, waypoint from navigation.context import convert_cartesian_to_gps from navigation.trajectory import Trajectory from navigation.search import SearchTrajectory - # TODO: Right now this is a copy of search state, we will need to change this # REFERENCE: https://docs.google.com/document/d/18GjDWxIu5f5-N5t5UgbrZGdEyaDj9ZMEUuXex8-NKrA/edit - - class WaterBottleSearchState(State): - class Node: + class Node: def __init__(self, parent=None, position=None): self.parent = parent self.position = position - self.g = 0 self.h = 0 self.f = 0 @@ -34,7 +29,6 @@ def __eq__(self, other): # defining less than for purposes of heap queue def __lt__(self, other): return self.f < other.f - # defining greater than for purposes of heap queue def __gt__(self, other): return self.f > other.f @@ -43,7 +37,6 @@ def __gt__(self, other): # when we are moving along the spiral prev_target: Optional[np.ndarray] = None is_recovering: bool = False - # TODO: add rosparam names to water_bottle_search/... in the navigation.yaml file STOP_THRESH = get_rosparam("search/stop_thresh", 0.5) DRIVE_FWD_THRESH = get_rosparam("search/drive_fwd_thresh", 0.34) @@ -52,21 +45,17 @@ def __gt__(self, other): DISTANCE_BETWEEN_SPIRALS = get_rosparam( "search/distance_between_spirals", 1.25 ) # TODO: after testing, might need to change - - # TODO: Data Structure to store the information given from the map (Look at navigation) - + + # 2D list and map parts costMap = [] height = 0 width = 0 resolution = 0 - #Shfited Center origin = [] center = [] - # TODO: Make call_back function to push into data structure - def costmap_callback(self, msg: OccupancyGrid): # update data structure self.height = msg.info.height * msg.info.resolution @@ -78,10 +67,11 @@ def costmap_callback(self, msg: OccupancyGrid): rospy.loginfo(f"2D costmap: {costmap2D}") self.origin = self.center - [self.width/2, self.height/2] # Call A-STAR - + + """ #Convert global(Real World) to i and j (Occupancy grid) - cart_coord: These are the i and j coordinates + cart_coord: These are the i and j coordinates width_height: These are the width and height list (m/cell) floor(v - (WP + [-W/2, H/2]) / r) [1, -1] return: i and j coordinates for the occupancy grid @@ -90,60 +80,111 @@ def cartesian_convert(self, cart_coord: np.ndarray) -> np.ndarray: width_height = [-self.width / 2, self.height / 2] converted_coord = (cart_coord - (self.origin + (width_height) / self.resolution))* [1,-1] return math.floor(converted_coord) - """ #Convert Occupancy grid(i, j) to x and y coordinates real_coord: These are the x and y coordinates width_height: These are the width and height list (m/cell) (WP - [W/2, H/2]) + [j x r, -i x r] + [r/2, -r/2] - """ - def real_world_convert(self, real_coord : np.ndarray) -> np.ndarray: + """ + def real_world_convert(self, real_coord : np.ndarray) -> np.ndarray: width_height = [self.width/2, self.height/2] resolution_conversion = [-real_coord * self.resolution] half_res = [self.resolution/2, -self.resolution/2] return self.origin - width_height + resolution_conversion + half_res + def return_path(current_node): + path = [] + current = current_node + while current is not None: + path.append(current.position) + current = current.parent + return path[::-1] # Return reversed path` """ - # TODO: A-STAR Algorithm: f(n) = g(n) + h(n) + # TODO: A-STAR Algorithm: f(n) = g(n) + h(n) # def a_star():j # start = rover pose (Cartesian) # end = next point in the spiral + # return: list of A-STAR coordinates """ def a_star(self, costmap2d, start: np.ndarray, end: np.ndarray) -> list | None: - #Convert start and end to local cartesian coordinates - startij = self.cartesian_convert(start) + #Convert start and end to local cartesian coordinates + startij = self.cartesian_convert(start) endij = self.cartesian_convert(end) - #Do the check for high cost for the end point if costmap2d[endij[0]][endij[1]] >= 100: - if not self.traj.increment_point(): + if not self.traj.increment_point(): end = self.traj.get_cur_pt #TODO What do we do in this case where we don't have another point in da spiral - else: + else: pass - - #Intialize nodes: + #Intialize nodes: start_node = self.Node(None, startij) start_node.g = start_node.h = start_node.f = 0 end_node = self.Node(None, endij) end_node.g = end_node.h = end_node.f = 0 - - # Check if end node is within range, if it is, check cost. Return None for path so we can call astar with new incremented end goal - # if end_node.position[0] <= (len(self.width) - 1) and end_node.position[0] >= 0 and end_node.position[1] <= (self.height) and end_node.position[1] >= 0: - # if costmap2d[end_node.position[0]][end_node.position[1]] == 100: - # print("Target has high cost!") - # return None - # Initialize both open and closed list open_list = [] closed_list = [] - # Heapify the open_list and Add the start node - heapq.heapify(open_list) + heapq.heapify(open_list) heapq.heappush(open_list, start_node) - - - + # Adding a stop condition + outer_iterations = 0 + max_iterations = (len(costmap2d[0]) * len(costmap2d) // 2) + #Movements: + adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1)) + adjacent_square_pick_index = [0, 1, 2, 3, 4, 5, 6, 7] + + # Loop until you find the end + while len(open_list) > 0: + #Randomize the order of the adjacent_squares_pick_index to avoid a decision making bias + random.shuffle(adjacent_square_pick_index) + outer_iterations += 1 + if outer_iterations > max_iterations: + # if we hit this point return the path such as it is + # it will not contain the destination + #warn("giving up on pathfinding too many iterations") + return self.return_path(current_node) + # Get the current node + current_node = heapq.heappop(open_list) + closed_list.append(current_node) + # Found the goal + if current_node == end_node: + return self.return_path(current_node) + # Generate children + children = [] + for pick_index in adjacent_square_pick_index: + new_position = adjacent_squares[pick_index] + # Get node position + node_position = (current_node.position[0] + new_position[0], current_node.position[1] + new_position[1]) + # Make sure within range + if node_position[0] > (len(costmap2d) - 1) or node_position[0] < 0 or node_position[1] > (len(costmap2d[len(costmap2d)-1]) -1) or node_position[1] < 0: + continue + # Make sure walkable terrain + # if costmap2d[node_position[0]][node_position[1]] != 0: + # continue + # Create new node + new_node = self.Node(current_node, node_position) + # Append + children.append(new_node) + # Loop through children + for child in children: + # Child is on the closed list + if len([closed_child for closed_child in closed_list if closed_child == child]) > 0: + continue + # Create the f, g, and h values + child.g = current_node.g + costmap2d[child.position[0]][child.position[1]] + child.h = ((child.position[0] - end_node.position[0]) ** 2) + ((child.position[1] - end_node.position[1]) ** 2) + child.f = child.g + child.h + # Child is already in the open list + if len([open_node for open_node in open_list if child.position == open_node.position and child.g > open_node.g]) > 0: + continue + # Add the child to the open list + heapq.heappush(open_list, child) + + return self.return_path(current_node) + + def on_enter(self, context) -> None: self.listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) search_center = context.course.current_waypoint() @@ -158,10 +199,8 @@ def on_enter(self, context) -> None: ) self.origin = context.rover.get_pose().position[0:2] self.prev_target = None - def on_exit(self, context) -> None: pass - def on_loop(self, context) -> State: # continue executing the path from wherever it left off target_pos = self.traj.get_cur_pt() @@ -177,19 +216,16 @@ def on_loop(self, context) -> State: # if we finish the spiral without seeing the fiducial, move on with course if self.traj.increment_point(): return waypoint.WaypointState() - if context.rover.stuck: context.rover.previous_state = self self.is_recovering = True return recovery.RecoveryState() else: self.is_recovering = False - context.search_point_publisher.publish( GPSPointList([convert_cartesian_to_gps(pt) for pt in self.traj.coordinates]) ) context.rover.send_drive_command(cmd_vel) - if context.env.current_fid_pos() is not None and context.course.look_for_post(): return approach_post.ApproachPostState() return self From 52df6b2e37dc28b613eec14319c63d39f1afc203 Mon Sep 17 00:00:00 2001 From: Prabhleen Pawar Date: Thu, 1 Feb 2024 19:45:30 -0500 Subject: [PATCH 41/76] Added A-Star call, but we need to do something with the list returned --- src/navigation/water_bottle_search.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index df11d4fa4..79ea77b60 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -66,8 +66,13 @@ def costmap_callback(self, msg: OccupancyGrid): costmap2D.reshape(self.height, self.width) rospy.loginfo(f"2D costmap: {costmap2D}") self.origin = self.center - [self.width/2, self.height/2] - # Call A-STAR + rover_pose = self.context.rover.get_pose().position[0:2] + self.traj.increment_point() + end_point = self.traj.get_cur_pt() + + # Call A-STAR + self.a_star(costmap2d = costmap2D,start = rover_pose, end = end_point) """ #Convert global(Real World) to i and j (Occupancy grid) @@ -92,13 +97,17 @@ def real_world_convert(self, real_coord : np.ndarray) -> np.ndarray: half_res = [self.resolution/2, -self.resolution/2] return self.origin - width_height + resolution_conversion + half_res + """ + # It returns the path given from A-Star in reverse + """ def return_path(current_node): path = [] current = current_node while current is not None: path.append(current.position) current = current.parent - return path[::-1] # Return reversed path` + return path[::-1] # Return reversed path + """ # TODO: A-STAR Algorithm: f(n) = g(n) + h(n) # def a_star():j From be439f38a7cef5f21fe4bc2cd4e9b61497eb9fe4 Mon Sep 17 00:00:00 2001 From: Prabhleen Pawar Date: Sat, 3 Feb 2024 14:50:16 -0500 Subject: [PATCH 42/76] Added reset cur point in trajector.py and made small changes to conversion functions --- src/navigation/trajectory.py | 3 + src/navigation/water_bottle_search.py | 128 ++++++++++++++------------ 2 files changed, 71 insertions(+), 60 deletions(-) diff --git a/src/navigation/trajectory.py b/src/navigation/trajectory.py index a44dceb3a..564a9f229 100644 --- a/src/navigation/trajectory.py +++ b/src/navigation/trajectory.py @@ -20,3 +20,6 @@ def increment_point(self) -> bool: """ self.cur_pt += 1 return self.cur_pt >= len(self.coordinates) + + def reset_cur_pt(self) -> None: + self.cur_pt = 0 diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index 79ea77b60..f5aab9f5d 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -14,6 +14,8 @@ from navigation.context import convert_cartesian_to_gps from navigation.trajectory import Trajectory from navigation.search import SearchTrajectory + + # TODO: Right now this is a copy of search state, we will need to change this # REFERENCE: https://docs.google.com/document/d/18GjDWxIu5f5-N5t5UgbrZGdEyaDj9ZMEUuXex8-NKrA/edit class WaterBottleSearchState(State): @@ -32,32 +34,30 @@ def __lt__(self, other): # defining greater than for purposes of heap queue def __gt__(self, other): return self.f > other.f - # Spiral - traj: SearchTrajectory + + traj: SearchTrajectory # spiral + star_traj: Trajectory # returned by astar # when we are moving along the spiral prev_target: Optional[np.ndarray] = None is_recovering: bool = False - # TODO: add rosparam names to water_bottle_search/... in the navigation.yaml file - STOP_THRESH = get_rosparam("search/stop_thresh", 0.5) - DRIVE_FWD_THRESH = get_rosparam("search/drive_fwd_thresh", 0.34) - SPIRAL_COVERAGE_RADIUS = get_rosparam("search/coverage_radius", 10) - SEGMENTS_PER_ROTATION = get_rosparam("search/segments_per_rotation", 8) # TODO: after testing, might need to change + height: int = 0 # height of occupancy grid + width: int = 0 # width of occupancy grid + resolution: int = 0 # resolution of occupancy + origin: np.ndarray = np.ndarray([]) # hold the inital rover pose + + STOP_THRESH = get_rosparam("water_bottle_search/stop_thresh", 0.5) + DRIVE_FWD_THRESH = get_rosparam("water_bottle_search/drive_fwd_thresh", 0.34) + SPIRAL_COVERAGE_RADIUS = get_rosparam("water_bottle_search/coverage_radius", 10) + SEGMENTS_PER_ROTATION = get_rosparam("water_bottle_search/segments_per_rotation", 8) # TODO: after testing, might need to change DISTANCE_BETWEEN_SPIRALS = get_rosparam( - "search/distance_between_spirals", 1.25 + "water_bottle_search/distance_between_spirals", 1.25 ) # TODO: after testing, might need to change - - - # 2D list and map parts - costMap = [] - height = 0 - width = 0 - resolution = 0 - #Shfited Center - origin = [] - center = [] - # TODO: Make call_back function to push into data structure + def costmap_callback(self, msg: OccupancyGrid): - # update data structure + """ + Callback function + :param msg: Occupancy Grid representative of a 30 x 30m square area with origin at GNSS waypoint. Values are 0, 1, -1 + """ self.height = msg.info.height * msg.info.resolution self.width = msg.info.width * msg.info.resolution self.resolution = msg.info.resolution @@ -65,42 +65,50 @@ def costmap_callback(self, msg: OccupancyGrid): costmap2D = np.array(msg.data) costmap2D.reshape(self.height, self.width) rospy.loginfo(f"2D costmap: {costmap2D}") - self.origin = self.center - [self.width/2, self.height/2] rover_pose = self.context.rover.get_pose().position[0:2] self.traj.increment_point() end_point = self.traj.get_cur_pt() - # Call A-STAR - self.a_star(costmap2d = costmap2D,start = rover_pose, end = end_point) + # call A-STAR + occupancy_list = self.a_star(costmap2d = costmap2D, start = rover_pose, end = end_point) + if occupancy_list is not None: + self.star_traj = self.ij_to_cartesian(np.array(occupancy_list)) + self.star_traj = (map(self.ij_to_cartesian, occupancy_list)) + else: + self.star_traj = [] + # reset current point to start of list + self.star_traj.reset_cur_pt() - """ - #Convert global(Real World) to i and j (Occupancy grid) - cart_coord: These are the i and j coordinates - width_height: These are the width and height list (m/cell) - floor(v - (WP + [-W/2, H/2]) / r) [1, -1] - return: i and j coordinates for the occupancy grid - """ - def cartesian_convert(self, cart_coord: np.ndarray) -> np.ndarray: - width_height = [-self.width / 2, self.height / 2] - converted_coord = (cart_coord - (self.origin + (width_height) / self.resolution))* [1,-1] + + def cartesian_to_ij(self, cart_coord: np.ndarray) -> np.ndarray: + """ + Convert real world cartesian coordinates (x, y) to coordinates in the occupancy grid (i, j) + :param cart_coord: array of x and y cartesian coordinates + :return: array of i and j coordinates for the occupancy grid + """ + width_height = [-self.width / 2, self.height / 2] # these are the width and height list (m/cell) + converted_coord = (cart_coord - (self.origin + (width_height) / self.resolution))* [1,-1] return math.floor(converted_coord) - """ - #Convert Occupancy grid(i, j) to x and y coordinates - real_coord: These are the x and y coordinates - width_height: These are the width and height list (m/cell) - (WP - [W/2, H/2]) + [j x r, -i x r] + [r/2, -r/2] - """ - def real_world_convert(self, real_coord : np.ndarray) -> np.ndarray: - width_height = [self.width/2, self.height/2] - resolution_conversion = [-real_coord * self.resolution] - half_res = [self.resolution/2, -self.resolution/2] + + def ij_to_cartesian(self, ij_coords : np.ndarray) -> np.ndarray: + """ + Convert coordinates in the occupancy grid (i, j) to real world cartesian coordinates (x, y) + using formula (WP - [W/2, H/2]) + [j x r, -i x r] + [r/2, -r/2] where WP is the origin. + :param ij_coords: array of i and j occupancy grid coordinates + :return: array of x and y coordinates in the real world + """ + width_height = [self.width/2, self.height/2] # [W/2, H/2] width and height list (m/cell) + resolution_conversion = ij_coords * [self.resolution, -1 * self.resolution] # [j x r, -i x r] + half_res = [self.resolution/2, -self.resolution/2] # [r/2, -r/2] return self.origin - width_height + resolution_conversion + half_res - """ - # It returns the path given from A-Star in reverse - """ + def return_path(current_node): + """ + It returns the path given from A-Star in reverse through current node's parents + :param current_node: end point of path and contains parents to retrieve path + """ path = [] current = current_node while current is not None: @@ -108,24 +116,23 @@ def return_path(current_node): current = current.parent return path[::-1] # Return reversed path - """ - # TODO: A-STAR Algorithm: f(n) = g(n) + h(n) - # def a_star():j - # start = rover pose (Cartesian) - # end = next point in the spiral - # return: list of A-STAR coordinates - """ def a_star(self, costmap2d, start: np.ndarray, end: np.ndarray) -> list | None: + """ + A-STAR Algorithm: f(n) = g(n) + h(n) + :param start: rover pose (cartesian) + :param end: next point in the spiral from traj (cartesian) + :return: list of A-STAR coordinates in the occupancy grid coordinates (i,j) + """ #Convert start and end to local cartesian coordinates - startij = self.cartesian_convert(start) - endij = self.cartesian_convert(end) + startij = self.cartesian_to_ij(start) + endij = self.cartesian_to_ij(end) #Do the check for high cost for the end point if costmap2d[endij[0]][endij[1]] >= 100: if not self.traj.increment_point(): end = self.traj.get_cur_pt #TODO What do we do in this case where we don't have another point in da spiral else: - pass + return None #Intialize nodes: start_node = self.Node(None, startij) start_node.g = start_node.h = start_node.f = 0 @@ -198,7 +205,6 @@ def on_enter(self, context) -> None: self.listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) search_center = context.course.current_waypoint() if not self.is_recovering: - self.center = context.rover.get_pose().position[0:2] self.traj = SearchTrajectory.spiral_traj( context.rover.get_pose().position[0:2], self.SPIRAL_COVERAGE_RADIUS, @@ -212,7 +218,9 @@ def on_exit(self, context) -> None: pass def on_loop(self, context) -> State: # continue executing the path from wherever it left off - target_pos = self.traj.get_cur_pt() + #if self.star_traj is None: + + target_pos = self.star_traj.get_cur_pt() cmd_vel, arrived = context.rover.driver.get_drive_command( target_pos, context.rover.get_pose(), @@ -223,7 +231,7 @@ def on_loop(self, context) -> State: if arrived: self.prev_target = target_pos # if we finish the spiral without seeing the fiducial, move on with course - if self.traj.increment_point(): + if self.star_traj.increment_point(): return waypoint.WaypointState() if context.rover.stuck: context.rover.previous_state = self @@ -232,7 +240,7 @@ def on_loop(self, context) -> State: else: self.is_recovering = False context.search_point_publisher.publish( - GPSPointList([convert_cartesian_to_gps(pt) for pt in self.traj.coordinates]) + GPSPointList([convert_cartesian_to_gps(pt) for pt in self.star_traj.coordinates]) ) context.rover.send_drive_command(cmd_vel) if context.env.current_fid_pos() is not None and context.course.look_for_post(): From e8b314ca6cf5fe7340b72460714ccb99e97a9666 Mon Sep 17 00:00:00 2001 From: Emerson-Ramey Date: Sat, 3 Feb 2024 23:32:28 -0500 Subject: [PATCH 43/76] reorganize and add path logic to on_loop --- src/navigation/trajectory.py | 3 - src/navigation/water_bottle_search.py | 230 ++++++++++++++------------ 2 files changed, 120 insertions(+), 113 deletions(-) diff --git a/src/navigation/trajectory.py b/src/navigation/trajectory.py index 564a9f229..a44dceb3a 100644 --- a/src/navigation/trajectory.py +++ b/src/navigation/trajectory.py @@ -20,6 +20,3 @@ def increment_point(self) -> bool: """ self.cur_pt += 1 return self.cur_pt >= len(self.coordinates) - - def reset_cur_pt(self) -> None: - self.cur_pt = 0 diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index f5aab9f5d..cb5be0f61 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -4,7 +4,6 @@ import heapq import numpy as np import random -import math import rospy from mrover.msg import GPSPointList from util.ros_utils import get_rosparam @@ -15,26 +14,28 @@ from navigation.trajectory import Trajectory from navigation.search import SearchTrajectory +class Node: + """ + Node class for astar pathfinding + """ + def __init__(self, parent=None, position=None): + self.parent = parent + self.position = position + self.g = 0 + self.h = 0 + self.f = 0 + def __eq__(self, other): + return self.position == other.position + # defining less than for purposes of heap queue + def __lt__(self, other): + return self.f < other.f + # defining greater than for purposes of heap queue + def __gt__(self, other): + return self.f > other.f # TODO: Right now this is a copy of search state, we will need to change this # REFERENCE: https://docs.google.com/document/d/18GjDWxIu5f5-N5t5UgbrZGdEyaDj9ZMEUuXex8-NKrA/edit class WaterBottleSearchState(State): - class Node: - def __init__(self, parent=None, position=None): - self.parent = parent - self.position = position - self.g = 0 - self.h = 0 - self.f = 0 - def __eq__(self, other): - return self.position == other.position - # defining less than for purposes of heap queue - def __lt__(self, other): - return self.f < other.f - # defining greater than for purposes of heap queue - def __gt__(self, other): - return self.f > other.f - traj: SearchTrajectory # spiral star_traj: Trajectory # returned by astar # when we are moving along the spiral @@ -43,7 +44,7 @@ def __gt__(self, other): height: int = 0 # height of occupancy grid width: int = 0 # width of occupancy grid resolution: int = 0 # resolution of occupancy - origin: np.ndarray = np.ndarray([]) # hold the inital rover pose + origin: np.ndarray = np.array([]) # hold the inital rover pose STOP_THRESH = get_rosparam("water_bottle_search/stop_thresh", 0.5) DRIVE_FWD_THRESH = get_rosparam("water_bottle_search/drive_fwd_thresh", 0.34) @@ -52,62 +53,34 @@ def __gt__(self, other): DISTANCE_BETWEEN_SPIRALS = get_rosparam( "water_bottle_search/distance_between_spirals", 1.25 ) # TODO: after testing, might need to change - - def costmap_callback(self, msg: OccupancyGrid): - """ - Callback function - :param msg: Occupancy Grid representative of a 30 x 30m square area with origin at GNSS waypoint. Values are 0, 1, -1 - """ - self.height = msg.info.height * msg.info.resolution - self.width = msg.info.width * msg.info.resolution - self.resolution = msg.info.resolution - rospy.loginfo(f"height: {self.height}, width: {self.width}") - costmap2D = np.array(msg.data) - costmap2D.reshape(self.height, self.width) - rospy.loginfo(f"2D costmap: {costmap2D}") - - rover_pose = self.context.rover.get_pose().position[0:2] - self.traj.increment_point() - end_point = self.traj.get_cur_pt() - - # call A-STAR - occupancy_list = self.a_star(costmap2d = costmap2D, start = rover_pose, end = end_point) - if occupancy_list is not None: - self.star_traj = self.ij_to_cartesian(np.array(occupancy_list)) - self.star_traj = (map(self.ij_to_cartesian, occupancy_list)) - else: - self.star_traj = [] - # reset current point to start of list - self.star_traj.reset_cur_pt() - def cartesian_to_ij(self, cart_coord: np.ndarray) -> np.ndarray: """ Convert real world cartesian coordinates (x, y) to coordinates in the occupancy grid (i, j) + using formula floor(v - (WP + [-W/2, H/2]) / r) * [1, -1] where v is an (x,y) coordinate and WP is the origin. :param cart_coord: array of x and y cartesian coordinates :return: array of i and j coordinates for the occupancy grid """ - width_height = [-self.width / 2, self.height / 2] # these are the width and height list (m/cell) - converted_coord = (cart_coord - (self.origin + (width_height) / self.resolution))* [1,-1] - return math.floor(converted_coord) + width_height = np.array([-1 * self.width / 2, self.height / 2]) # [-W/2, H/2] + converted_coord = np.floor(cart_coord - (self.origin + (width_height / self.resolution))) + return converted_coord * np.array([1, -1]) def ij_to_cartesian(self, ij_coords : np.ndarray) -> np.ndarray: """ Convert coordinates in the occupancy grid (i, j) to real world cartesian coordinates (x, y) - using formula (WP - [W/2, H/2]) + [j x r, -i x r] + [r/2, -r/2] where WP is the origin. + using formula (WP - [W/2, H/2]) + [j * r, -i * r] + [r/2, -r/2] where WP is the origin. :param ij_coords: array of i and j occupancy grid coordinates :return: array of x and y coordinates in the real world """ - width_height = [self.width/2, self.height/2] # [W/2, H/2] width and height list (m/cell) - resolution_conversion = ij_coords * [self.resolution, -1 * self.resolution] # [j x r, -i x r] - half_res = [self.resolution/2, -self.resolution/2] # [r/2, -r/2] - return self.origin - width_height + resolution_conversion + half_res - + width_height = np.array([self.width/2, self.height/2]) # [W/2, H/2] + resolution_conversion = ij_coords * [self.resolution, -1 * self.resolution] # [j * r, -i * r] + half_res = np.array([self.resolution/2, -self.resolution/2]) # [r/2, -r/2] + return (self.origin - width_height) + resolution_conversion + half_res - def return_path(current_node): + def return_path(self, current_node): """ - It returns the path given from A-Star in reverse through current node's parents - :param current_node: end point of path and contains parents to retrieve path + Return the path given from A-Star in reverse through current node's parents + :param current_node: end point of path which contains parents to retrieve path """ path = [] current = current_node @@ -118,89 +91,119 @@ def return_path(current_node): def a_star(self, costmap2d, start: np.ndarray, end: np.ndarray) -> list | None: """ - A-STAR Algorithm: f(n) = g(n) + h(n) - :param start: rover pose (cartesian) - :param end: next point in the spiral from traj (cartesian) + A-STAR Algorithm: f(n) = g(n) + h(n) to find a path from the given start to the given end in the given costmap + :param costmap2d: occupancy grid in the form of an array + :param start: rover pose in cartesian coordinates + :param end: next point in the spiral from traj in cartesian coordinates :return: list of A-STAR coordinates in the occupancy grid coordinates (i,j) """ - #Convert start and end to local cartesian coordinates + # convert start and end to occupancy grid coordinates startij = self.cartesian_to_ij(start) endij = self.cartesian_to_ij(end) - #Do the check for high cost for the end point - if costmap2d[endij[0]][endij[1]] >= 100: - if not self.traj.increment_point(): - end = self.traj.get_cur_pt - #TODO What do we do in this case where we don't have another point in da spiral - else: - return None - #Intialize nodes: - start_node = self.Node(None, startij) - start_node.g = start_node.h = start_node.f = 0 - end_node = self.Node(None, endij) - end_node.g = end_node.h = end_node.f = 0 - # Initialize both open and closed list + + # initialize start and end nodes + start_node = Node(None, startij) + end_node = Node(None, endij) + + # check if end node is within range, if it is, check if it has a high cost + if end_node.position[0] <= (costmap2d.shape[0] - 1) and end_node.position[0] >= 0 and end_node.position[1] <= (costmap2d.shape[1] - 1) and end_node.position[1] >= 0: + if costmap2d[end_node.position[0]][end_node.position[1]] >= 1: + # True if the trajectory is finished so return None + if self.traj.increment_point(): + return None + # update end point to be the next point in the search spiral + endij = self.cartesian_to_ij(self.traj.get_cur_pt) + end_node = Node(None, endij) + + # initialize both open and closed list open_list = [] closed_list = [] - # Heapify the open_list and Add the start node + + # heapify the open_list and add the start node heapq.heapify(open_list) heapq.heappush(open_list, start_node) - # Adding a stop condition + + # add a stop condition outer_iterations = 0 - max_iterations = (len(costmap2d[0]) * len(costmap2d) // 2) - #Movements: + max_iterations = (costmap2d.shape[0] * costmap2d.shape[1] // 2) + + # movements/squares we can search adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1)) adjacent_square_pick_index = [0, 1, 2, 3, 4, 5, 6, 7] - # Loop until you find the end + # loop until you find the end while len(open_list) > 0: - #Randomize the order of the adjacent_squares_pick_index to avoid a decision making bias + # randomize the order of the adjacent_squares_pick_index to avoid a decision making bias random.shuffle(adjacent_square_pick_index) + outer_iterations += 1 + if outer_iterations > max_iterations: - # if we hit this point return the path such as it is - # it will not contain the destination - #warn("giving up on pathfinding too many iterations") + # if we hit this point return the path such as it is. It will not contain the destination + print("giving up on pathfinding too many iterations") return self.return_path(current_node) - # Get the current node + + # get the current node current_node = heapq.heappop(open_list) closed_list.append(current_node) - # Found the goal + + # found the goal if current_node == end_node: return self.return_path(current_node) - # Generate children + + # generate children children = [] for pick_index in adjacent_square_pick_index: new_position = adjacent_squares[pick_index] - # Get node position node_position = (current_node.position[0] + new_position[0], current_node.position[1] + new_position[1]) - # Make sure within range - if node_position[0] > (len(costmap2d) - 1) or node_position[0] < 0 or node_position[1] > (len(costmap2d[len(costmap2d)-1]) -1) or node_position[1] < 0: + # make sure within range + if node_position[0] > (costmap2d.shape[0] - 1) or node_position[0] < 0 or node_position[1] > (costmap2d.shape[1] - 1) or node_position[1] < 0: continue - # Make sure walkable terrain - # if costmap2d[node_position[0]][node_position[1]] != 0: - # continue - # Create new node - new_node = self.Node(current_node, node_position) - # Append + + # create new node and append it + new_node = Node(current_node, node_position) children.append(new_node) - # Loop through children + + # loop through children for child in children: - # Child is on the closed list + # child is on the closed list if len([closed_child for closed_child in closed_list if closed_child == child]) > 0: continue - # Create the f, g, and h values + # create the f, g, and h values child.g = current_node.g + costmap2d[child.position[0]][child.position[1]] child.h = ((child.position[0] - end_node.position[0]) ** 2) + ((child.position[1] - end_node.position[1]) ** 2) child.f = child.g + child.h - # Child is already in the open list + # child is already in the open list if len([open_node for open_node in open_list if child.position == open_node.position and child.g > open_node.g]) > 0: continue - # Add the child to the open list + # add the child to the open list heapq.heappush(open_list, child) + + print("Couldn't find a path to destination") + return None - return self.return_path(current_node) - - + def costmap_callback(self, msg: OccupancyGrid): + """ + Callback function for the occupancy grid perception sends + :param msg: Occupancy Grid representative of a 30 x 30m square area with origin at GNSS waypoint. Values are 0, 1, -1 + """ + self.resolution = msg.info.resolution # meters/cell + self.height = msg.info.height * self.resolution # meters + self.width = msg.info.width * self.resolution # meters + rospy.loginfo(f"height: {self.height}, width: {self.width}") + costmap2D = np.array(msg.data).reshape(self.height, self.width) + rospy.loginfo(f"2D costmap: {costmap2D}") + + cur_rover_pose = self.context.rover.get_pose().position[0:2] + end_point = self.traj.get_cur_pt() + + # call A-STAR + occupancy_list = self.a_star(costmap2D, cur_rover_pose, end_point) + if occupancy_list is None: + self.star_traj = Trajectory(np.array([])) + else: + self.star_traj = Trajectory(self.ij_to_cartesian(np.array(occupancy_list))) # current point gets set back to 0 + def on_enter(self, context) -> None: self.listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) search_center = context.course.current_waypoint() @@ -214,13 +217,16 @@ def on_enter(self, context) -> None: ) self.origin = context.rover.get_pose().position[0:2] self.prev_target = None + def on_exit(self, context) -> None: pass + def on_loop(self, context) -> State: # continue executing the path from wherever it left off - #if self.star_traj is None: - - target_pos = self.star_traj.get_cur_pt() + target_pos = self.traj.get_cur_pt() + # if there is an alternate path we need to take to avoid the obstacle, use that trajectory + if len(self.star_traj.coordinates) != 0: + target_pos = self.star_traj.get_cur_pt() cmd_vel, arrived = context.rover.driver.get_drive_command( target_pos, context.rover.get_pose(), @@ -230,9 +236,12 @@ def on_loop(self, context) -> State: ) if arrived: self.prev_target = target_pos - # if we finish the spiral without seeing the fiducial, move on with course + # if we finish the astar path, then reset astar and increment the spiral if self.star_traj.increment_point(): - return waypoint.WaypointState() + self.star_traj = Trajectory(np.array([])) + # if we finish the spiral without seeing the fiducial, move on with course + if self.traj.increment_point(): + return waypoint.WaypointState() if context.rover.stuck: context.rover.previous_state = self self.is_recovering = True @@ -240,9 +249,10 @@ def on_loop(self, context) -> State: else: self.is_recovering = False context.search_point_publisher.publish( - GPSPointList([convert_cartesian_to_gps(pt) for pt in self.star_traj.coordinates]) + GPSPointList([convert_cartesian_to_gps(pt) for pt in self.traj.coordinates]) ) context.rover.send_drive_command(cmd_vel) + # TODO: change so that if we are looking for the water bottle we will transition into ApproachObjectState if context.env.current_fid_pos() is not None and context.course.look_for_post(): return approach_post.ApproachPostState() return self From caec28bcf2c194678d83c78ec29373fa93c2ec87 Mon Sep 17 00:00:00 2001 From: Emerson-Ramey Date: Sun, 4 Feb 2024 14:02:21 -0500 Subject: [PATCH 44/76] change incrementing of paths --- src/navigation/water_bottle_search.py | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index cb5be0f61..cf9f8e96a 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -224,9 +224,11 @@ def on_exit(self, context) -> None: def on_loop(self, context) -> State: # continue executing the path from wherever it left off target_pos = self.traj.get_cur_pt() + traj_target = True # if there is an alternate path we need to take to avoid the obstacle, use that trajectory if len(self.star_traj.coordinates) != 0: target_pos = self.star_traj.get_cur_pt() + traj_target = False cmd_vel, arrived = context.rover.driver.get_drive_command( target_pos, context.rover.get_pose(), @@ -236,12 +238,17 @@ def on_loop(self, context) -> State: ) if arrived: self.prev_target = target_pos - # if we finish the astar path, then reset astar and increment the spiral - if self.star_traj.increment_point(): - self.star_traj = Trajectory(np.array([])) - # if we finish the spiral without seeing the fiducial, move on with course + # if our target was the search spiral point, only increment the spiral path + if traj_target: + # if we finish the spiral without seeing the object, move on with course if self.traj.increment_point(): return waypoint.WaypointState() + else: # otherwise, increment the astar path + # if we finish the astar path, then reset astar and increment the spiral path + if self.star_traj.increment_point(): + self.star_traj = Trajectory(np.array([])) + if self.traj.increment_point(): + return waypoint.WaypointState() if context.rover.stuck: context.rover.previous_state = self self.is_recovering = True From 3f9c09c30d2116471190acd433a12ce841138378 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Tue, 6 Feb 2024 18:17:15 -0500 Subject: [PATCH 45/76] minor tweaks --- src/perception/cost_map/cost_map.processing.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 7e115a4aa..6966eb5f4 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -25,7 +25,7 @@ namespace mrover { Eigen::MatrixXd normal_matrix(4, num_points); // std::for_each(points, points + msg->width * msg->height, [&](auto* point) { for (auto point = points; point - points < msg->width * msg->height; point += step) { - Eigen::Vector4d p{point->x, point->y, 0, 1}; + Eigen::Vector4d p{point->x, point->y, point->z, 1}; point_matrix.col((point - points) / step) = p; Eigen::Vector4d normal{point->normal_x, point->normal_y, point->normal_z, 0}; normal_matrix.col((point - points) / step) = normal; @@ -46,7 +46,7 @@ namespace mrover { auto i = mGlobalGridMsg.info.width * y_index + x_index; Eigen::Vector3d normal{n.x(), n.y(), n.z()}; - normal.normalize(); + // normal.normalize(); // get vertical component of (unit) normal vector double z_comp = normal.z(); // small z component indicates largely horizontal normal (surface is vertical) From 57a5b43e0b42db4ba4cd2e96032fd86b25e18755 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Tue, 6 Feb 2024 18:33:35 -0500 Subject: [PATCH 46/76] removed allocations from hot-path --- src/perception/cost_map/cost_map.hpp | 6 ++++++ src/perception/cost_map/cost_map.processing.cpp | 12 ++++-------- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/perception/cost_map/cost_map.hpp b/src/perception/cost_map/cost_map.hpp index d80365925..c456c20fb 100644 --- a/src/perception/cost_map/cost_map.hpp +++ b/src/perception/cost_map/cost_map.hpp @@ -1,4 +1,5 @@ #include "pch.hpp" +#include namespace mrover { @@ -14,6 +15,11 @@ namespace mrover { float mResolution{}; // Meters per cell float mDimension{}; // Dimensions of the square costmap in meters float mNormalThreshold = 0.9; + int mDownSamplingFactor = 4; + uint32_t mNumPoints = 640 * 480 / mDownSamplingFactor; + Eigen::MatrixXd point_matrix{4, mNumPoints}; + Eigen::MatrixXd normal_matrix{4, mNumPoints}; + tf2_ros::Buffer tf_buffer; tf2_ros::TransformListener tf_listener{tf_buffer}; diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 6966eb5f4..01c04d242 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -19,22 +19,18 @@ namespace mrover { try { SE3 zed_to_map = SE3::fromTfTree(tf_buffer, "map", "zed2i_left_camera_frame"); auto* points = reinterpret_cast(msg->data.data()); - int step = 4; - uint32_t num_points = msg->width * msg->height / step; - Eigen::MatrixXd point_matrix(4, num_points); - Eigen::MatrixXd normal_matrix(4, num_points); // std::for_each(points, points + msg->width * msg->height, [&](auto* point) { - for (auto point = points; point - points < msg->width * msg->height; point += step) { + for (auto point = points; point - points < msg->width * msg->height; point += mDownSamplingFactor) { Eigen::Vector4d p{point->x, point->y, point->z, 1}; - point_matrix.col((point - points) / step) = p; + point_matrix.col((point - points) / mDownSamplingFactor) = p; Eigen::Vector4d normal{point->normal_x, point->normal_y, point->normal_z, 0}; - normal_matrix.col((point - points) / step) = normal; + normal_matrix.col((point - points) / mDownSamplingFactor) = normal; } point_matrix = zed_to_map.matrix() * point_matrix; normal_matrix = zed_to_map.matrix() * normal_matrix; - for (uint32_t i = 0; i < num_points; i++) { + for (uint32_t i = 0; i < mNumPoints; i++) { double x = point_matrix(0, i); double y = point_matrix(1, i); Eigen::Vector4d n = normal_matrix.col(i); From 973f25af50d2edbec6d5a67f5da338790f6448eb Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Tue, 6 Feb 2024 18:37:47 -0500 Subject: [PATCH 47/76] cleaned up starter project stuff --- starter_project/autonomy/msg/StarterProjectTag.msg | 4 ---- starter_project/autonomy/src/perception.cpp | 2 -- 2 files changed, 6 deletions(-) delete mode 100644 starter_project/autonomy/msg/StarterProjectTag.msg diff --git a/starter_project/autonomy/msg/StarterProjectTag.msg b/starter_project/autonomy/msg/StarterProjectTag.msg deleted file mode 100644 index fb3b6f843..000000000 --- a/starter_project/autonomy/msg/StarterProjectTag.msg +++ /dev/null @@ -1,4 +0,0 @@ -int32 tagId -float32 xTagCenterPixel -float32 yTagCenterPixel -float32 closenessMetric \ No newline at end of file diff --git a/starter_project/autonomy/src/perception.cpp b/starter_project/autonomy/src/perception.cpp index 5df36999d..986b4a6a4 100644 --- a/starter_project/autonomy/src/perception.cpp +++ b/starter_project/autonomy/src/perception.cpp @@ -51,8 +51,6 @@ namespace mrover { // hint: you have mTagDictionary, mTagCorners, mTagIds, and mTagDetectorParams member variables already defined! // hint: write and use the "getCenterFromTagCorners" and "getClosenessMetricFromTagCorners" functions - cv::aruco::detectMarkers(image, mTagDictionary, mTagCorners, mTagIds, mTagDetectorParams); - tags.clear(); // Clear old tags in output vector // TODO: implement me! From aea06653c96d1d4c772c7264210935ba08097a4c Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Tue, 6 Feb 2024 19:20:17 -0500 Subject: [PATCH 48/76] small changes for nav --- src/perception/cost_map/cost_map.cpp | 2 +- src/perception/cost_map/cost_map.processing.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/perception/cost_map/cost_map.cpp b/src/perception/cost_map/cost_map.cpp index bceeafc09..7d3dfa46d 100644 --- a/src/perception/cost_map/cost_map.cpp +++ b/src/perception/cost_map/cost_map.cpp @@ -7,7 +7,7 @@ namespace mrover { mPnh = getMTPrivateNodeHandle(); // Unused for now mNh.param("publish_cost_map", mPublishCostMap, true); mNh.param("resolution", mResolution, 0.5); - mNh.param("map_width", mDimension, 60); + mNh.param("map_width", mDimension, 30); mCostMapPub = mCmt.advertise("costmap", 1); // We publish our results to "costmap" diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 01c04d242..56e1d0391 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -46,7 +46,7 @@ namespace mrover { // get vertical component of (unit) normal vector double z_comp = normal.z(); // small z component indicates largely horizontal normal (surface is vertical) - signed char cost = z_comp < mNormalThreshold ? 100 : 0; + signed char cost = z_comp < mNormalThreshold ? 1 : 0; mGlobalGridMsg.data[i] = std::max(mGlobalGridMsg.data[i], cost); } From 3cfd481860e1ffab083a5db4cc7ba9a875116e0e Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Sat, 10 Feb 2024 02:02:29 -0500 Subject: [PATCH 49/76] fix errors when running and add transitions --- config/navigation.yaml | 2 +- scripts/debug_course_publisher.py | 33 +++-- src/navigation/context.py | 16 --- src/navigation/navigation.py | 14 +- src/navigation/search.py | 68 +-------- src/navigation/trajectory.py | 66 +++++++++ src/navigation/water_bottle_search.py | 194 +++++++++++++++++--------- src/navigation/waypoint.py | 10 +- 8 files changed, 234 insertions(+), 169 deletions(-) diff --git a/config/navigation.yaml b/config/navigation.yaml index ffb76514b..bc30a2838 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -27,7 +27,7 @@ water_bottle_search: drive_fwd_thresh: 0.34 coverage_radius: 10 segments_per_rotation: 8 - distance_between_spirals: 1.25 + distance_between_spirals: 3 single_fiducial: stop_thresh: 1.0 diff --git a/scripts/debug_course_publisher.py b/scripts/debug_course_publisher.py index 35162f32f..f137176f1 100755 --- a/scripts/debug_course_publisher.py +++ b/scripts/debug_course_publisher.py @@ -16,17 +16,22 @@ if __name__ == "__main__": rospy.init_node("debug_course_publisher") - publish_waypoints([convert_waypoint_to_gps(waypoint) for waypoint in [ - ( - Waypoint(tag_id=0, type=WaypointType(val=WaypointType.NO_SEARCH)), - SE3(position=np.array([4, 4, 0])), - ), - ( - Waypoint(tag_id=0, type=WaypointType(val=WaypointType.POST)), - SE3(position=np.array([-2, -2, 0])), - ), - ( - Waypoint(tag_id=1, type=WaypointType(val=WaypointType.POST)), - SE3(position=np.array([11, -10, 0])), - ) - ]]) + publish_waypoints( + [ + convert_waypoint_to_gps(waypoint) + for waypoint in [ + ( + Waypoint(tag_id=0, type=WaypointType(val=WaypointType.WATER_BOTTLE)), + SE3(position=np.array([6, 0, 0])), + ) + # ( + # Waypoint(tag_id=0, type=WaypointType(val=WaypointType.POST)), + # SE3(position=np.array([-2, -2, 0])), + # ), + # ( + # Waypoint(tag_id=1, type=WaypointType(val=WaypointType.POST)), + # SE3(position=np.array([11, -10, 0])), + # ) + ] + ] + ) diff --git a/src/navigation/context.py b/src/navigation/context.py index ec1109382..df2ed514d 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -9,10 +9,6 @@ import rospy import tf2_ros from geometry_msgs.msg import Twist -from nav_msgs.msg import OccupancyGrid, MapMetaData -from mrover.msg import Waypoint, GPSWaypoint, EnableAuton, WaypointType, GPSPointList -from std_msgs.msg import Time, Bool -from visualization_msgs.msg import Marker from util.SE3 import SE3 @@ -207,7 +203,6 @@ class Context: vis_publisher: rospy.Publisher course_listener: rospy.Subscriber stuck_listener: rospy.Subscriber - costmap_listener: rospy.Subscriber # Use these as the primary interfaces in states course: Optional[Course] @@ -229,7 +224,6 @@ def __init__(self): self.search_point_publisher = rospy.Publisher("search_path", GPSPointList, queue_size=1) self.enable_auton_service = rospy.Service("enable_auton", EnableAuton, self.recv_enable_auton) self.stuck_listener = rospy.Subscriber("nav_stuck", Bool, self.stuck_callback) - self.costmap_listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) self.course = None self.rover = Rover(self, False, "") self.env = Environment(self) @@ -248,13 +242,3 @@ def recv_enable_auton(self, req: EnableAutonRequest) -> EnableAutonResponse: def stuck_callback(self, msg: Bool): self.rover.stuck = msg.data - - def costmap_callback(self, msg: OccupancyGrid): - # update data structure - height = msg.info.height - width = msg.info.width - rospy.logerr(f"height: {height}, width: {width}") - costmap1D = np.array(msg.data) - costmap2D = costmap1D.reshape((height, width)) - rospy.logerr(f"2D costmap: {costmap2D.shape}") - rospy.logerr(f"2D costmap: {costmap2D}") diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py index 82eeee331..197ea24d4 100755 --- a/src/navigation/navigation.py +++ b/src/navigation/navigation.py @@ -14,6 +14,7 @@ from navigation.search import SearchState from navigation.state import DoneState, OffState, off_check from navigation.waypoint import WaypointState +from navigation.water_bottle_search import WaterBottleSearchState class Navigation(threading.Thread): @@ -35,9 +36,20 @@ def __init__(self, context: Context): self.state_machine.add_transitions(SearchState(), [ApproachPostState(), WaypointState(), RecoveryState()]) self.state_machine.add_transitions(DoneState(), [WaypointState()]) self.state_machine.add_transitions( - WaypointState(), [PostBackupState(), ApproachPostState(), SearchState(), RecoveryState(), DoneState()] + WaypointState(), + [ + PostBackupState(), + ApproachPostState(), + SearchState(), + RecoveryState(), + DoneState(), + WaterBottleSearchState(), + ], ) self.state_machine.add_transitions(OffState(), [WaypointState(), DoneState()]) + self.state_machine.add_transitions( + WaterBottleSearchState(), [WaypointState(), RecoveryState(), ApproachPostState()] + ) self.state_machine.configure_off_switch(OffState(), off_check) self.state_machine_server = StatePublisher(self.state_machine, "nav_structure", 1, "nav_state", 10) diff --git a/src/navigation/search.py b/src/navigation/search.py index 2b0d8ee3b..f336d1539 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -11,73 +11,7 @@ from navigation import approach_post, recovery, waypoint from navigation.context import convert_cartesian_to_gps -from navigation.trajectory import Trajectory - - -@dataclass -class SearchTrajectory(Trajectory): - # Associated fiducial for this trajectory - fid_id: int - - @classmethod - def gen_spiral_coordinates( - cls, coverage_radius: float, distance_between_spirals: float, num_segments_per_rotation: int - ) -> np.ndarray: - """ - Generates a set of coordinates for a spiral search pattern centered at the origin - :param coverage_radius: radius of the spiral search pattern (float) - :param distance_between_spirals: distance between each spiralradii = angles * (distance_between_spirals / (2*np.pi)) (float) - :param num_segments_per_rotation: number of segments that the spiral has per rotation (int) - :return: np.ndarray of coordinates - """ - # the number of spirals should ensure coverage of the entire radius. - # We add 1 to ensure that the last spiral covers the radius along the entire rotation, - # as otherwise we will just make the outermost point touch the radius - num_spirals = np.ceil(coverage_radius / distance_between_spirals).astype("int") + 1 - # the angles are evenly spaced between 0 and 2pi*num_segments_per_rotation (add one to the number of points because N+1 points make N segments) - angles = np.linspace(0, 2 * np.pi * num_spirals, num_segments_per_rotation * num_spirals + 1) - # radii are computed via following polar formula. - # This is correct because you want the radius to increase by 'distance_between_spirals' every 2pi radians (one rotation) - radii = angles * (distance_between_spirals / (2 * np.pi)) - # convert to cartesian coordinates - xcoords = np.cos(angles) * radii - ycoords = np.sin(angles) * radii - # we want to return as a 2D matrix where each row is a coordinate pair - # so we reshape x and y coordinates to be (n, 1) matricies then stack horizontally to get (n, 2) matrix - return np.hstack((xcoords.reshape(-1, 1), ycoords.reshape(-1, 1))) - - @classmethod - def spiral_traj( - cls, - center: np.ndarray, - coverage_radius: float, - distance_between_spirals: float, - segments_per_rotation: int, - fid_id: int, - ) -> SearchTrajectory: - """ - Generates a square spiral search pattern around a center position, assumes rover is at the center position - :param center: position to center spiral on (np.ndarray) - :param coverage_radius: radius of the spiral search pattern (float) - :param distance_between_spirals: distance between each spiral (float) - :param segments_per_rotation: number of segments per spiral (int), for example, 4 segments per rotation would be a square spiral, 8 segments per rotation would be an octagonal spiral - :param fid_id: tag id to associate with this trajectory (int) - :return: SearchTrajectory object - """ - zero_centered_spiral_r2 = cls.gen_spiral_coordinates( - coverage_radius, distance_between_spirals, segments_per_rotation - ) - - # numpy broadcasting magic to add center to each row of the spiral coordinates - spiral_coordinates_r2 = zero_centered_spiral_r2 + center - # add a column of zeros to make it 3D - spiral_coordinates_r3 = np.hstack( - (spiral_coordinates_r2, np.zeros(spiral_coordinates_r2.shape[0]).reshape(-1, 1)) - ) - return SearchTrajectory( - spiral_coordinates_r3, - fid_id, - ) +from navigation.trajectory import SearchTrajectory class SearchState(State): diff --git a/src/navigation/trajectory.py b/src/navigation/trajectory.py index a44dceb3a..8df26a20e 100644 --- a/src/navigation/trajectory.py +++ b/src/navigation/trajectory.py @@ -20,3 +20,69 @@ def increment_point(self) -> bool: """ self.cur_pt += 1 return self.cur_pt >= len(self.coordinates) + + +@dataclass +class SearchTrajectory(Trajectory): + # Associated fiducial for this trajectory + fid_id: int + + @classmethod + def gen_spiral_coordinates( + cls, coverage_radius: float, distance_between_spirals: float, num_segments_per_rotation: int + ) -> np.ndarray: + """ + Generates a set of coordinates for a spiral search pattern centered at the origin + :param coverage_radius: radius of the spiral search pattern (float) + :param distance_between_spirals: distance between each spiralradii = angles * (distance_between_spirals / (2*np.pi)) (float) + :param num_segments_per_rotation: number of segments that the spiral has per rotation (int) + :return: np.ndarray of coordinates + """ + # the number of spirals should ensure coverage of the entire radius. + # We add 1 to ensure that the last spiral covers the radius along the entire rotation, + # as otherwise we will just make the outermost point touch the radius + num_spirals = np.ceil(coverage_radius / distance_between_spirals).astype("int") + 1 + # the angles are evenly spaced between 0 and 2pi*num_segments_per_rotation (add one to the number of points because N+1 points make N segments) + angles = np.linspace(0, 2 * np.pi * num_spirals, num_segments_per_rotation * num_spirals + 1) + # radii are computed via following polar formula. + # This is correct because you want the radius to increase by 'distance_between_spirals' every 2pi radians (one rotation) + radii = angles * (distance_between_spirals / (2 * np.pi)) + # convert to cartesian coordinates + xcoords = np.cos(angles) * radii + ycoords = np.sin(angles) * radii + # we want to return as a 2D matrix where each row is a coordinate pair + # so we reshape x and y coordinates to be (n, 1) matricies then stack horizontally to get (n, 2) matrix + return np.hstack((xcoords.reshape(-1, 1), ycoords.reshape(-1, 1))) + + @classmethod + def spiral_traj( + cls, + center: np.ndarray, + coverage_radius: float, + distance_between_spirals: float, + segments_per_rotation: int, + fid_id: int, + ): + """ + Generates a square spiral search pattern around a center position, assumes rover is at the center position + :param center: position to center spiral on (np.ndarray) + :param coverage_radius: radius of the spiral search pattern (float) + :param distance_between_spirals: distance between each spiral (float) + :param segments_per_rotation: number of segments per spiral (int), for example, 4 segments per rotation would be a square spiral, 8 segments per rotation would be an octagonal spiral + :param fid_id: tag id to associate with this trajectory (int) + :return: SearchTrajectory object + """ + zero_centered_spiral_r2 = cls.gen_spiral_coordinates( + coverage_radius, distance_between_spirals, segments_per_rotation + ) + + # numpy broadcasting magic to add center to each row of the spiral coordinates + spiral_coordinates_r2 = zero_centered_spiral_r2 + center + # add a column of zeros to make it 3D + spiral_coordinates_r3 = np.hstack( + (spiral_coordinates_r2, np.zeros(spiral_coordinates_r2.shape[0]).reshape(-1, 1)) + ) + return SearchTrajectory( + spiral_coordinates_r3, + fid_id, + ) diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index cf9f8e96a..f08c49c5b 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -11,75 +11,95 @@ from nav_msgs.msg import OccupancyGrid, MapMetaData from navigation import approach_post, recovery, waypoint from navigation.context import convert_cartesian_to_gps -from navigation.trajectory import Trajectory -from navigation.search import SearchTrajectory +from navigation.trajectory import Trajectory, SearchTrajectory +from navigation.context import Context + class Node: """ Node class for astar pathfinding """ + def __init__(self, parent=None, position=None): self.parent = parent self.position = position self.g = 0 self.h = 0 self.f = 0 + def __eq__(self, other): return self.position == other.position + # defining less than for purposes of heap queue def __lt__(self, other): return self.f < other.f + # defining greater than for purposes of heap queue def __gt__(self, other): return self.f > other.f + # TODO: Right now this is a copy of search state, we will need to change this # REFERENCE: https://docs.google.com/document/d/18GjDWxIu5f5-N5t5UgbrZGdEyaDj9ZMEUuXex8-NKrA/edit class WaterBottleSearchState(State): - traj: SearchTrajectory # spiral - star_traj: Trajectory # returned by astar + traj: SearchTrajectory # spiral + star_traj: Trajectory # returned by astar # when we are moving along the spiral prev_target: Optional[np.ndarray] = None is_recovering: bool = False - height: int = 0 # height of occupancy grid - width: int = 0 # width of occupancy grid - resolution: int = 0 # resolution of occupancy - origin: np.ndarray = np.array([]) # hold the inital rover pose - + height: int = 0 # height of occupancy grid + width: int = 0 # width of occupancy grid + resolution: int = 0 # resolution of occupancy + origin: np.ndarray = np.array([]) # hold the inital rover pose + context: Context + STOP_THRESH = get_rosparam("water_bottle_search/stop_thresh", 0.5) DRIVE_FWD_THRESH = get_rosparam("water_bottle_search/drive_fwd_thresh", 0.34) SPIRAL_COVERAGE_RADIUS = get_rosparam("water_bottle_search/coverage_radius", 10) - SEGMENTS_PER_ROTATION = get_rosparam("water_bottle_search/segments_per_rotation", 8) # TODO: after testing, might need to change + SEGMENTS_PER_ROTATION = get_rosparam( + "water_bottle_search/segments_per_rotation", 8 + ) # TODO: after testing, might need to change DISTANCE_BETWEEN_SPIRALS = get_rosparam( - "water_bottle_search/distance_between_spirals", 1.25 + "water_bottle_search/distance_between_spirals", 3 ) # TODO: after testing, might need to change - + def cartesian_to_ij(self, cart_coord: np.ndarray) -> np.ndarray: """ Convert real world cartesian coordinates (x, y) to coordinates in the occupancy grid (i, j) - using formula floor(v - (WP + [-W/2, H/2]) / r) * [1, -1] where v is an (x,y) coordinate and WP is the origin. + using formula floor(v - (WP + [-W/2, H/2]) / r) * [1, -1] + v: (x,y) coordinate + WP: origin + W, H: grid width, height (meters) + r: resolution (meters/cell) :param cart_coord: array of x and y cartesian coordinates :return: array of i and j coordinates for the occupancy grid """ - width_height = np.array([-1 * self.width / 2, self.height / 2]) # [-W/2, H/2] - converted_coord = np.floor(cart_coord - (self.origin + (width_height / self.resolution))) - return converted_coord * np.array([1, -1]) - - def ij_to_cartesian(self, ij_coords : np.ndarray) -> np.ndarray: + width = self.width * self.resolution + height = self.height * self.resolution + width_height = np.array([-1 * width / 2, height / 2]) # [-W/2, H/2] + converted_coord = np.floor((cart_coord[0:2] - (self.origin + width_height)) / self.resolution) + return converted_coord.astype(np.int8) * np.array([1, -1]) + + def ij_to_cartesian(self, ij_coords: np.ndarray) -> np.ndarray: """ Convert coordinates in the occupancy grid (i, j) to real world cartesian coordinates (x, y) - using formula (WP - [W/2, H/2]) + [j * r, -i * r] + [r/2, -r/2] where WP is the origin. + using formula (WP - [W/2, H/2]) + [j * r, i * r] + [r/2, -r/2] + WP: origin + W, H: grid width, height (meters) + r: resolution (meters/cell) :param ij_coords: array of i and j occupancy grid coordinates :return: array of x and y coordinates in the real world """ - width_height = np.array([self.width/2, self.height/2]) # [W/2, H/2] - resolution_conversion = ij_coords * [self.resolution, -1 * self.resolution] # [j * r, -i * r] - half_res = np.array([self.resolution/2, -self.resolution/2]) # [r/2, -r/2] - return (self.origin - width_height) + resolution_conversion + half_res - + width = self.width * self.resolution + height = self.height * self.resolution + width_height = np.array([width / 2, height / 2]) # [W/2, H/2] + resolution_conversion = ij_coords * [self.resolution, self.resolution] # [j * r, i * r] + half_res = np.array([self.resolution / 2, -1 * self.resolution / 2]) # [r/2, -r/2] + return ((self.origin - width_height) + resolution_conversion + half_res) * np.array([1, -1]) + def return_path(self, current_node): """ - Return the path given from A-Star in reverse through current node's parents + Return the path given from A-Star in reverse through current node's parents :param current_node: end point of path which contains parents to retrieve path """ path = [] @@ -87,34 +107,48 @@ def return_path(self, current_node): while current is not None: path.append(current.position) current = current.parent - return path[::-1] # Return reversed path - - def a_star(self, costmap2d, start: np.ndarray, end: np.ndarray) -> list | None: + reversed_path = path[::-1] + print("ij:", reversed_path[1:]) + return reversed_path[1:] # Return reversed path except the starting point (we are already there) + + def a_star(self, costmap2d: np.ndarray, start: np.ndarray, end: np.ndarray) -> list | None: """ A-STAR Algorithm: f(n) = g(n) + h(n) to find a path from the given start to the given end in the given costmap - :param costmap2d: occupancy grid in the form of an array + :param costmap2d: occupancy grid in the form of an 2D array of floats :param start: rover pose in cartesian coordinates :param end: next point in the spiral from traj in cartesian coordinates :return: list of A-STAR coordinates in the occupancy grid coordinates (i,j) """ # convert start and end to occupancy grid coordinates - startij = self.cartesian_to_ij(start) + startij = self.cartesian_to_ij(start) endij = self.cartesian_to_ij(end) # initialize start and end nodes - start_node = Node(None, startij) - end_node = Node(None, endij) + start_node = Node(None, (startij[0], startij[1])) + end_node = Node(None, (endij[0], endij[1])) + + if start_node == end_node: + return None + + print(f"start: {start}, end: {end}") + print(f"startij: {startij}, endij: {endij}") # check if end node is within range, if it is, check if it has a high cost - if end_node.position[0] <= (costmap2d.shape[0] - 1) and end_node.position[0] >= 0 and end_node.position[1] <= (costmap2d.shape[1] - 1) and end_node.position[1] >= 0: - if costmap2d[end_node.position[0]][end_node.position[1]] >= 1: + if ( + end_node.position[0] <= (costmap2d.shape[0] - 1) + and end_node.position[0] >= 0 + and end_node.position[1] <= (costmap2d.shape[1] - 1) + and end_node.position[1] >= 0 + ): + if costmap2d[end_node.position[0], end_node.position[1]] >= 1: # True if the trajectory is finished so return None if self.traj.increment_point(): - return None + return None # update end point to be the next point in the search spiral - endij = self.cartesian_to_ij(self.traj.get_cur_pt) - end_node = Node(None, endij) - + endij = self.cartesian_to_ij(self.traj.get_cur_pt()) + end_node = Node(None, (endij[0], endij[1])) + print(f"End has high cost! new end: {endij}") + # initialize both open and closed list open_list = [] closed_list = [] @@ -125,12 +159,12 @@ def a_star(self, costmap2d, start: np.ndarray, end: np.ndarray) -> list | None: # add a stop condition outer_iterations = 0 - max_iterations = (costmap2d.shape[0] * costmap2d.shape[1] // 2) - + max_iterations = costmap2d.shape[0] * costmap2d.shape[1] // 2 + # movements/squares we can search adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1)) adjacent_square_pick_index = [0, 1, 2, 3, 4, 5, 6, 7] - + # loop until you find the end while len(open_list) > 0: # randomize the order of the adjacent_squares_pick_index to avoid a decision making bias @@ -142,22 +176,28 @@ def a_star(self, costmap2d, start: np.ndarray, end: np.ndarray) -> list | None: # if we hit this point return the path such as it is. It will not contain the destination print("giving up on pathfinding too many iterations") return self.return_path(current_node) - + # get the current node current_node = heapq.heappop(open_list) closed_list.append(current_node) # found the goal if current_node == end_node: + print("Found goal!") return self.return_path(current_node) - + # generate children children = [] for pick_index in adjacent_square_pick_index: new_position = adjacent_squares[pick_index] node_position = (current_node.position[0] + new_position[0], current_node.position[1] + new_position[1]) # make sure within range - if node_position[0] > (costmap2d.shape[0] - 1) or node_position[0] < 0 or node_position[1] > (costmap2d.shape[1] - 1) or node_position[1] < 0: + if ( + node_position[0] > (costmap2d.shape[0] - 1) + or node_position[0] < 0 + or node_position[1] > (costmap2d.shape[1] - 1) + or node_position[1] < 0 + ): continue # create new node and append it @@ -170,42 +210,57 @@ def a_star(self, costmap2d, start: np.ndarray, end: np.ndarray) -> list | None: if len([closed_child for closed_child in closed_list if closed_child == child]) > 0: continue # create the f, g, and h values - child.g = current_node.g + costmap2d[child.position[0]][child.position[1]] - child.h = ((child.position[0] - end_node.position[0]) ** 2) + ((child.position[1] - end_node.position[1]) ** 2) + child.g = current_node.g + costmap2d[child.position[0], child.position[1]] + child.h = ((child.position[0] - end_node.position[0]) ** 2) + ( + (child.position[1] - end_node.position[1]) ** 2 + ) child.f = child.g + child.h # child is already in the open list - if len([open_node for open_node in open_list if child.position == open_node.position and child.g > open_node.g]) > 0: + if ( + len( + [ + open_node + for open_node in open_list + if child.position == open_node.position and child.g > open_node.g + ] + ) + > 0 + ): continue # add the child to the open list heapq.heappush(open_list, child) - - print("Couldn't find a path to destination") + + print("Couldn't find a path to destination") return None - + def costmap_callback(self, msg: OccupancyGrid): """ - Callback function for the occupancy grid perception sends + Callback function for the occupancy grid perception sends :param msg: Occupancy Grid representative of a 30 x 30m square area with origin at GNSS waypoint. Values are 0, 1, -1 """ - self.resolution = msg.info.resolution # meters/cell - self.height = msg.info.height * self.resolution # meters - self.width = msg.info.width * self.resolution # meters - rospy.loginfo(f"height: {self.height}, width: {self.width}") - costmap2D = np.array(msg.data).reshape(self.height, self.width) - rospy.loginfo(f"2D costmap: {costmap2D}") - + self.resolution = msg.info.resolution # meters/cell + self.height = msg.info.height # cells + self.width = msg.info.width # cells + costmap2D = np.array(msg.data).reshape(int(self.height), int(self.width)).astype(np.float32) + # change all unidentified points to have a cost of 0.5 + costmap2D[costmap2D == -1.0] = 0.5 + cur_rover_pose = self.context.rover.get_pose().position[0:2] end_point = self.traj.get_cur_pt() - + # call A-STAR - occupancy_list = self.a_star(costmap2D, cur_rover_pose, end_point) + occupancy_list = self.a_star(costmap2D, cur_rover_pose, end_point[0:2]) if occupancy_list is None: self.star_traj = Trajectory(np.array([])) else: - self.star_traj = Trajectory(self.ij_to_cartesian(np.array(occupancy_list))) # current point gets set back to 0 - + cartesian_coords = self.ij_to_cartesian(np.array(occupancy_list)) + print(f"{cartesian_coords}, shape: {cartesian_coords.shape}") + self.star_traj = Trajectory( + np.hstack((cartesian_coords, np.zeros((cartesian_coords.shape[0], 1)))) + ) # current point gets set back to 0 + def on_enter(self, context) -> None: - self.listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) + self.context = context search_center = context.course.current_waypoint() if not self.is_recovering: self.traj = SearchTrajectory.spiral_traj( @@ -213,11 +268,14 @@ def on_enter(self, context) -> None: self.SPIRAL_COVERAGE_RADIUS, self.DISTANCE_BETWEEN_SPIRALS, self.SEGMENTS_PER_ROTATION, - search_center.fiducial_id, + search_center.tag_id, ) self.origin = context.rover.get_pose().position[0:2] + print(f"ORIGIN: {self.origin}") self.prev_target = None - + self.star_traj = Trajectory(np.array([])) + self.listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) + def on_exit(self, context) -> None: pass @@ -227,7 +285,7 @@ def on_loop(self, context) -> State: traj_target = True # if there is an alternate path we need to take to avoid the obstacle, use that trajectory if len(self.star_traj.coordinates) != 0: - target_pos = self.star_traj.get_cur_pt() + target_pos = self.star_traj.get_cur_pt() traj_target = False cmd_vel, arrived = context.rover.driver.get_drive_command( target_pos, @@ -240,12 +298,14 @@ def on_loop(self, context) -> State: self.prev_target = target_pos # if our target was the search spiral point, only increment the spiral path if traj_target: + print("arrived at sprial point") # if we finish the spiral without seeing the object, move on with course if self.traj.increment_point(): return waypoint.WaypointState() - else: # otherwise, increment the astar path + else: # otherwise, increment the astar path # if we finish the astar path, then reset astar and increment the spiral path if self.star_traj.increment_point(): + print("arrived at end of astar") self.star_traj = Trajectory(np.array([])) if self.traj.increment_point(): return waypoint.WaypointState() diff --git a/src/navigation/waypoint.py b/src/navigation/waypoint.py index 0a964dc48..ec602ecfb 100644 --- a/src/navigation/waypoint.py +++ b/src/navigation/waypoint.py @@ -3,7 +3,8 @@ from util.ros_utils import get_rosparam from util.state_lib.state import State -from navigation import search, recovery, approach_post, post_backup, state +from navigation import search, recovery, approach_post, post_backup, state, water_bottle_search +from mrover.msg import WaypointType class WaypointState(State): @@ -48,11 +49,14 @@ def on_loop(self, context) -> State: self.DRIVE_FWD_THRESH, ) if arrived: - if not context.course.look_for_post(): + if current_waypoint.type.val == WaypointType.NO_SEARCH: # We finished a regular waypoint, go onto the next one context.course.increment_waypoint() + elif current_waypoint.type.val == WaypointType.WATER_BOTTLE: + # We finished a waypoint associated with the water bottle, but we have not seen it yet + return water_bottle_search.WaterBottleSearchState() else: - # We finished a waypoint associated with a tag id, but we have not seen it yet. + # We finished a waypoint associated with a tag id or the mallet, but we have not seen it yet. return search.SearchState() if context.rover.stuck: From 890cb257beea944a2953c54d93fa60031a1d40b3 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Sun, 11 Feb 2024 12:26:16 -0500 Subject: [PATCH 50/76] minor fixes --- src/perception/cost_map/cost_map.processing.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 56e1d0391..1796462ad 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -35,8 +35,8 @@ namespace mrover { double y = point_matrix(1, i); Eigen::Vector4d n = normal_matrix.col(i); - if (x >= -1 * mDimension / 2 && x <= mDimension / 2 && - y >= -1 * mDimension / 2 && y <= mDimension / 2) { + if (x >= mGlobalGridMsg.info.origin.position.x && x <= mGlobalGridMsg.info.origin.position.x + mDimension && + y >= mGlobalGridMsg.info.origin.position.y && y <= mGlobalGridMsg.info.origin.position.y + mDimension) { int x_index = floor((x - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); int y_index = floor((y - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); auto i = mGlobalGridMsg.info.width * y_index + x_index; From 17b3a55c944edd0fda7ca71e4089f5ec5105875e Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Sun, 11 Feb 2024 13:27:36 -0500 Subject: [PATCH 51/76] fix driving to same target point --- launch/autonomy.launch | 2 +- src/navigation/drive.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/launch/autonomy.launch b/launch/autonomy.launch index 411a9e202..fb213b3c4 100644 --- a/launch/autonomy.launch +++ b/launch/autonomy.launch @@ -2,7 +2,7 @@ - + diff --git a/src/navigation/drive.py b/src/navigation/drive.py index 2ed873124..80c630899 100644 --- a/src/navigation/drive.py +++ b/src/navigation/drive.py @@ -141,6 +141,7 @@ def get_lookahead_pt( :return: the lookahead point """ # compute the vector from the previous target position to the current target position + path_vec = path_end - path_start # compute the vector from the previous target position to the rover position rover_vec = rover_pos - path_start @@ -192,7 +193,7 @@ def get_drive_command( self.reset() return Twist(), True - if path_start is not None: + if path_start is not None and np.linalg.norm(path_start - target_pos) > LOOKAHEAD_DISTANCE: target_pos = self.get_lookahead_pt(path_start, target_pos, rover_pos, LOOKAHEAD_DISTANCE) target_dir = target_pos - rover_pos From ce364ac26db0f077c1cbffdf2fa0ab07555fd267 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Sun, 11 Feb 2024 14:32:01 -0500 Subject: [PATCH 52/76] small fixes --- src/perception/cost_map/cost_map.processing.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 1796462ad..6942d5c90 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -33,13 +33,15 @@ namespace mrover { for (uint32_t i = 0; i < mNumPoints; i++) { double x = point_matrix(0, i); double y = point_matrix(1, i); + double z = point_matrix(2, i); Eigen::Vector4d n = normal_matrix.col(i); if (x >= mGlobalGridMsg.info.origin.position.x && x <= mGlobalGridMsg.info.origin.position.x + mDimension && - y >= mGlobalGridMsg.info.origin.position.y && y <= mGlobalGridMsg.info.origin.position.y + mDimension) { + y >= mGlobalGridMsg.info.origin.position.y && y <= mGlobalGridMsg.info.origin.position.y + mDimension && + z < 2) { int x_index = floor((x - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); int y_index = floor((y - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); - auto i = mGlobalGridMsg.info.width * y_index + x_index; + auto ind = mGlobalGridMsg.info.width * y_index + x_index; Eigen::Vector3d normal{n.x(), n.y(), n.z()}; // normal.normalize(); @@ -48,7 +50,7 @@ namespace mrover { // small z component indicates largely horizontal normal (surface is vertical) signed char cost = z_comp < mNormalThreshold ? 1 : 0; - mGlobalGridMsg.data[i] = std::max(mGlobalGridMsg.data[i], cost); + mGlobalGridMsg.data[ind] = std::max(mGlobalGridMsg.data[ind], cost); } } mCostMapPub.publish(mGlobalGridMsg); From 2be051ede432736fa171dcfb65e95a107dae50c7 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Sun, 11 Feb 2024 14:36:07 -0500 Subject: [PATCH 53/76] got rid of z limit (might be needed in real world though) --- src/perception/cost_map/cost_map.processing.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 6942d5c90..bda7f5d71 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -37,8 +37,7 @@ namespace mrover { Eigen::Vector4d n = normal_matrix.col(i); if (x >= mGlobalGridMsg.info.origin.position.x && x <= mGlobalGridMsg.info.origin.position.x + mDimension && - y >= mGlobalGridMsg.info.origin.position.y && y <= mGlobalGridMsg.info.origin.position.y + mDimension && - z < 2) { + y >= mGlobalGridMsg.info.origin.position.y && y <= mGlobalGridMsg.info.origin.position.y + mDimension) { int x_index = floor((x - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); int y_index = floor((y - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); auto ind = mGlobalGridMsg.info.width * y_index + x_index; From 572e81890c56f3a3535981e4ccbcc75d4aa53736 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Sun, 18 Feb 2024 16:01:40 -0500 Subject: [PATCH 54/76] not done - testing path planning --- src/navigation/water_bottle_search.py | 110 ++++++++++++++++++++------ 1 file changed, 85 insertions(+), 25 deletions(-) diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index f08c49c5b..49ec694cb 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -5,14 +5,17 @@ import numpy as np import random import rospy -from mrover.msg import GPSPointList +from mrover.msg import GPSPointList, Costmap2D from util.ros_utils import get_rosparam from util.state_lib.state import State -from nav_msgs.msg import OccupancyGrid, MapMetaData +from nav_msgs.msg import OccupancyGrid, MapMetaData, Path +from geometry_msgs.msg import Pose, PoseStamped from navigation import approach_post, recovery, waypoint from navigation.context import convert_cartesian_to_gps from navigation.trajectory import Trajectory, SearchTrajectory from navigation.context import Context +from threading import Lock +from util.SE3 import SE3 class Node: @@ -52,6 +55,11 @@ class WaterBottleSearchState(State): resolution: int = 0 # resolution of occupancy origin: np.ndarray = np.array([]) # hold the inital rover pose context: Context + listener: rospy.Subscriber + costmap_pub: rospy.Publisher + time_last_updated: rospy.Time + costmap_lock: Lock + path_pub: rospy.Publisher STOP_THRESH = get_rosparam("water_bottle_search/stop_thresh", 0.5) DRIVE_FWD_THRESH = get_rosparam("water_bottle_search/drive_fwd_thresh", 0.34) @@ -97,7 +105,7 @@ def ij_to_cartesian(self, ij_coords: np.ndarray) -> np.ndarray: half_res = np.array([self.resolution / 2, -1 * self.resolution / 2]) # [r/2, -r/2] return ((self.origin - width_height) + resolution_conversion + half_res) * np.array([1, -1]) - def return_path(self, current_node): + def return_path(self, current_node, maze): """ Return the path given from A-Star in reverse through current node's parents :param current_node: end point of path which contains parents to retrieve path @@ -109,6 +117,27 @@ def return_path(self, current_node): current = current.parent reversed_path = path[::-1] print("ij:", reversed_path[1:]) + + for step in reversed_path[1:]: + maze[step[0]][step[1]] = 2 + maze[reversed_path[1][0]][reversed_path[1][1]] = 3 # start + maze[reversed_path[-1][0]][reversed_path[-1][1]] = 4 # end + + for row in maze: + line = [] + for col in row: + if col == 1: + line.append("\u2588") + elif col == 0: + line.append(" ") + elif col == 2: + line.append(".") + elif col == 3: + line.append("S") + elif col == 4: + line.append("E") + print("".join(line)) + return reversed_path[1:] # Return reversed path except the starting point (we are already there) def a_star(self, costmap2d: np.ndarray, start: np.ndarray, end: np.ndarray) -> list | None: @@ -183,8 +212,7 @@ def a_star(self, costmap2d: np.ndarray, start: np.ndarray, end: np.ndarray) -> l # found the goal if current_node == end_node: - print("Found goal!") - return self.return_path(current_node) + return self.return_path(current_node, costmap2d) # generate children children = [] @@ -238,29 +266,58 @@ def costmap_callback(self, msg: OccupancyGrid): Callback function for the occupancy grid perception sends :param msg: Occupancy Grid representative of a 30 x 30m square area with origin at GNSS waypoint. Values are 0, 1, -1 """ - self.resolution = msg.info.resolution # meters/cell - self.height = msg.info.height # cells - self.width = msg.info.width # cells - costmap2D = np.array(msg.data).reshape(int(self.height), int(self.width)).astype(np.float32) - # change all unidentified points to have a cost of 0.5 - costmap2D[costmap2D == -1.0] = 0.5 - - cur_rover_pose = self.context.rover.get_pose().position[0:2] - end_point = self.traj.get_cur_pt() - - # call A-STAR - occupancy_list = self.a_star(costmap2D, cur_rover_pose, end_point[0:2]) - if occupancy_list is None: - self.star_traj = Trajectory(np.array([])) - else: - cartesian_coords = self.ij_to_cartesian(np.array(occupancy_list)) - print(f"{cartesian_coords}, shape: {cartesian_coords.shape}") - self.star_traj = Trajectory( - np.hstack((cartesian_coords, np.zeros((cartesian_coords.shape[0], 1)))) - ) # current point gets set back to 0 + if rospy.get_time() - self.time_last_updated > 5: + print("RUN ASTAR") + self.resolution = msg.info.resolution # meters/cell + self.height = msg.info.height # cells + self.width = msg.info.width # cells + with self.costmap_lock: + costmap2D = np.array(msg.data).reshape(int(self.height), int(self.width)).astype(np.float32) + # change all unidentified points to have a cost of 0.5 + print("SHAPE", costmap2D.shape) + costmap2D[costmap2D == -1.0] = 0.5 + + cur_rover_pose = self.context.rover.get_pose().position[0:2] + end_point = self.traj.get_cur_pt() + + print("XXXXXXXXXXXXXXXXXXXXX START COSTMAP XXXXXXXXXXXXXXXXXXXXX") + for row in costmap2D: + line = [] + for col in row: + if col == 1: + line.append("\u2588") + elif col == 0: + line.append(" ") + print("".join(line)) + print("XXXXXXXXXXXXXXXXXXXXX END COSTMAP XXXXXXXXXXXXXXXXXXXXX") + + # call A-STAR + occupancy_list = self.a_star(costmap2D, cur_rover_pose, end_point[0:2]) + if occupancy_list is None: + self.star_traj = Trajectory(np.array([])) + else: + cartesian_coords = self.ij_to_cartesian(np.array(occupancy_list)) + print(f"{cartesian_coords}, shape: {cartesian_coords.shape}") + self.star_traj = Trajectory( + np.hstack((cartesian_coords, np.zeros((cartesian_coords.shape[0], 1)))) + ) # current point gets set back to 0 + self.costmap_pub.publish(costmap2D.flatten()) + + path = Path() + poses = [] + for coord in cartesian_coords: + pose = SE3.from_pos_quat(coord, [0, 0, 0, 1]) + pose_stamped = PoseStamped() + pose_stamped.pose = Pose(pose.position, pose.rotation.quaternion) + poses.append(pose_stamped) + path.poses = poses + self.path_pub.publish(path) + + self.time_last_updated = rospy.get_time() def on_enter(self, context) -> None: self.context = context + self.costmap_lock = Lock() search_center = context.course.current_waypoint() if not self.is_recovering: self.traj = SearchTrajectory.spiral_traj( @@ -274,7 +331,10 @@ def on_enter(self, context) -> None: print(f"ORIGIN: {self.origin}") self.prev_target = None self.star_traj = Trajectory(np.array([])) + self.time_last_updated = rospy.get_time() self.listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) + self.costmap_pub = rospy.Publisher("costmap2D", Costmap2D, queue_size=1) + self.path_pub = rospy.Publisher("path", Path, queue_size=1) def on_exit(self, context) -> None: pass From bc8d1f345ad106b8550a4a8f130c739230d52ee5 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Mon, 19 Feb 2024 21:36:07 -0500 Subject: [PATCH 55/76] Update to use manif --- src/perception/cost_map/cost_map.cpp | 4 ++-- src/perception/cost_map/cost_map.hpp | 17 +++++++------- .../cost_map/cost_map.processing.cpp | 23 ++++++++----------- src/perception/cost_map/pch.hpp | 9 ++++++-- 4 files changed, 27 insertions(+), 26 deletions(-) diff --git a/src/perception/cost_map/cost_map.cpp b/src/perception/cost_map/cost_map.cpp index 7d3dfa46d..b400a9f01 100644 --- a/src/perception/cost_map/cost_map.cpp +++ b/src/perception/cost_map/cost_map.cpp @@ -14,8 +14,8 @@ namespace mrover { mPcSub = mNh.subscribe("camera/left/points", 1, &CostMapNodelet::pointCloudCallback, this); mGlobalGridMsg.info.resolution = mResolution; - mGlobalGridMsg.info.width = (int) (mDimension / mResolution); - mGlobalGridMsg.info.height = (int) (mDimension / mResolution); + mGlobalGridMsg.info.width = static_cast(mDimension / mResolution); + mGlobalGridMsg.info.height = static_cast(mDimension / mResolution); // make center of map at (0, 0) mGlobalGridMsg.info.origin.position.x = -1 * mDimension / 2; mGlobalGridMsg.info.origin.position.y = -1 * mDimension / 2; diff --git a/src/perception/cost_map/cost_map.hpp b/src/perception/cost_map/cost_map.hpp index c456c20fb..833d4c11a 100644 --- a/src/perception/cost_map/cost_map.hpp +++ b/src/perception/cost_map/cost_map.hpp @@ -1,15 +1,14 @@ +#pragma once + #include "pch.hpp" -#include namespace mrover { + class CostMapNodelet final : public nodelet::Nodelet { - class CostMapNodelet : public nodelet::Nodelet { - private: ros::NodeHandle mNh, mPnh, mCmt; ros::Publisher mCostMapPub; ros::Subscriber mPcSub; - void onInit() override; bool mPublishCostMap{}; // If set, publish the global costmap float mResolution{}; // Meters per cell @@ -20,12 +19,13 @@ namespace mrover { Eigen::MatrixXd point_matrix{4, mNumPoints}; Eigen::MatrixXd normal_matrix{4, mNumPoints}; - tf2_ros::Buffer tf_buffer; - tf2_ros::TransformListener tf_listener{tf_buffer}; + tf2_ros::Buffer mTfBuffer; + tf2_ros::TransformListener mTfListener{mTfBuffer}; - std::optional mPreviousPose; + std::optional mPreviousPose; nav_msgs::OccupancyGrid mGlobalGridMsg; + void onInit() override; public: CostMapNodelet() = default; @@ -35,4 +35,5 @@ namespace mrover { void configCallback(); void pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg); }; -} // namespace mrover \ No newline at end of file + +} // namespace mrover diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index bda7f5d71..1344f7250 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -1,12 +1,4 @@ #include "cost_map.hpp" -#include "../point.hpp" - -#include "loop_profiler.hpp" -#include -#include -#include -#include -#include namespace mrover { @@ -16,8 +8,9 @@ namespace mrover { assert(msg->width > 0); if (!mPublishCostMap) return; + try { - SE3 zed_to_map = SE3::fromTfTree(tf_buffer, "map", "zed2i_left_camera_frame"); + SE3d zed_to_map = SE3Conversions::fromTfTree(mTfBuffer, "map", "zed_left_camera_frame"); auto* points = reinterpret_cast(msg->data.data()); // std::for_each(points, points + msg->width * msg->height, [&](auto* point) { for (auto point = points; point - points < msg->width * msg->height; point += mDownSamplingFactor) { @@ -27,19 +20,19 @@ namespace mrover { normal_matrix.col((point - points) / mDownSamplingFactor) = normal; } - point_matrix = zed_to_map.matrix() * point_matrix; - normal_matrix = zed_to_map.matrix() * normal_matrix; + // TODO(neven): Make sure this still works with manif + point_matrix = zed_to_map.transform() * point_matrix; + normal_matrix = zed_to_map.transform() * normal_matrix; for (uint32_t i = 0; i < mNumPoints; i++) { double x = point_matrix(0, i); double y = point_matrix(1, i); - double z = point_matrix(2, i); Eigen::Vector4d n = normal_matrix.col(i); if (x >= mGlobalGridMsg.info.origin.position.x && x <= mGlobalGridMsg.info.origin.position.x + mDimension && y >= mGlobalGridMsg.info.origin.position.y && y <= mGlobalGridMsg.info.origin.position.y + mDimension) { - int x_index = floor((x - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); - int y_index = floor((y - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); + int x_index = std::floor((x - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); + int y_index = std::floor((y - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); auto ind = mGlobalGridMsg.info.width * y_index + x_index; Eigen::Vector3d normal{n.x(), n.y(), n.z()}; @@ -54,6 +47,8 @@ namespace mrover { } mCostMapPub.publish(mGlobalGridMsg); } catch (...) { + // TODO(neven): Catch TF exceptions only, and log warn them } } + } // namespace mrover diff --git a/src/perception/cost_map/pch.hpp b/src/perception/cost_map/pch.hpp index a66e0ce0c..807581900 100644 --- a/src/perception/cost_map/pch.hpp +++ b/src/perception/cost_map/pch.hpp @@ -3,14 +3,19 @@ #include #include +#include + #include #include #include #include #include -#include #include #include #include +#include + +#include +#include -#include \ No newline at end of file +#include From a13a89dfb8b1b4323f1463980b3b87634110b17d Mon Sep 17 00:00:00 2001 From: qhdwight Date: Mon, 19 Feb 2024 21:40:11 -0500 Subject: [PATCH 56/76] Formatting --- scripts/debug_water_bottle_search.py | 5 ++--- src/perception/cost_map/cost_map.processing.cpp | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/scripts/debug_water_bottle_search.py b/scripts/debug_water_bottle_search.py index c9a038009..b7b922ec7 100755 --- a/scripts/debug_water_bottle_search.py +++ b/scripts/debug_water_bottle_search.py @@ -3,7 +3,7 @@ import numpy as np import rospy -import time +import time from nav_msgs.msg import OccupancyGrid, MapMetaData from geometry_msgs.msg import Pose from std_msgs.msg import Header @@ -34,7 +34,7 @@ # std_msgs/Header header header = Header() test_grid.header = header - + rospy.loginfo(f"Before publish") costpub = rospy.Publisher("costmap", OccupancyGrid, queue_size=1) for i in range(10): @@ -42,7 +42,6 @@ time.sleep(1) rospy.loginfo(f"After publish") rospy.spin() - except rospy.ROSInterruptException as e: print(f"Didn't work to publish or retrieve message from ros") diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 1344f7250..d1026d0a5 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -2,7 +2,7 @@ namespace mrover { - void CostMapNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) { + auto CostMapNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void { assert(msg); assert(msg->height > 0); assert(msg->width > 0); From 60fa76b1ecab41c662a4adb3975d0f98e4512193 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Tue, 20 Feb 2024 12:14:32 -0500 Subject: [PATCH 57/76] Fix nav imports --- src/navigation/water_bottle_search.py | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index 49ec694cb..31f6075c9 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -1,21 +1,22 @@ from __future__ import annotations -from dataclasses import dataclass -from typing import Optional + import heapq -import numpy as np import random +from threading import Lock +from typing import Optional + +import numpy as np + import rospy -from mrover.msg import GPSPointList, Costmap2D -from util.ros_utils import get_rosparam -from util.state_lib.state import State -from nav_msgs.msg import OccupancyGrid, MapMetaData, Path from geometry_msgs.msg import Pose, PoseStamped +from mrover.msg import GPSPointList +from nav_msgs.msg import OccupancyGrid, Path from navigation import approach_post, recovery, waypoint -from navigation.context import convert_cartesian_to_gps +from navigation.context import convert_cartesian_to_gps, Context from navigation.trajectory import Trajectory, SearchTrajectory -from navigation.context import Context -from threading import Lock from util.SE3 import SE3 +from util.ros_utils import get_rosparam +from util.state_lib.state import State class Node: From e5d9092b7735fd15758e92045e13dda4e1b94fab Mon Sep 17 00:00:00 2001 From: qhdwight Date: Tue, 20 Feb 2024 15:33:58 -0500 Subject: [PATCH 58/76] Correct frame --- src/perception/cost_map/cost_map.processing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index d1026d0a5..6b8727e34 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -10,7 +10,7 @@ namespace mrover { if (!mPublishCostMap) return; try { - SE3d zed_to_map = SE3Conversions::fromTfTree(mTfBuffer, "map", "zed_left_camera_frame"); + SE3d zed_to_map = SE3Conversions::fromTfTree(mTfBuffer, "zed_left_camera_frame", "map"); auto* points = reinterpret_cast(msg->data.data()); // std::for_each(points, points + msg->width * msg->height, [&](auto* point) { for (auto point = points; point - points < msg->width * msg->height; point += mDownSamplingFactor) { From b877028e0aae025987e021e206d338dec43d49f7 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Tue, 20 Feb 2024 15:53:24 -0500 Subject: [PATCH 59/76] Enable ekf --- launch/autonomy.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/autonomy.launch b/launch/autonomy.launch index bbfc5b521..2312591c3 100644 --- a/launch/autonomy.launch +++ b/launch/autonomy.launch @@ -3,7 +3,7 @@ - + From 3e3b54589f632965fe9a6b6f4123e7e0ba772a28 Mon Sep 17 00:00:00 2001 From: kayleyge Date: Thu, 22 Feb 2024 19:10:40 -0500 Subject: [PATCH 60/76] changed debug course publisher for waterbottle --- astar.py | 218 +++++++++++++++++++++++++++ scripts/debug_course_publisher.py | 22 +-- scripts/debug_water_bottle_search.py | 14 +- 3 files changed, 236 insertions(+), 18 deletions(-) create mode 100644 astar.py diff --git a/astar.py b/astar.py new file mode 100644 index 000000000..d032359ad --- /dev/null +++ b/astar.py @@ -0,0 +1,218 @@ +# Credit for this: Nicholas Swift +# as found at https://medium.com/@nicholas.w.swift/easy-a-star-pathfinding-7e6689c7f7b2 +from warnings import warn +import heapq +import random + +class Node: + """ + A node class for A* Pathfinding + """ + + def __init__(self, parent=None, position=None): + self.parent = parent + self.position = position + + self.g = 0 + self.h = 0 + self.f = 0 + + def __eq__(self, other): + return self.position == other.position + + def __repr__(self): + return f"{self.position} - g: {self.g} h: {self.h} f: {self.f}" + + # defining less than for purposes of heap queue + def __lt__(self, other): + return self.f < other.f + + # defining greater than for purposes of heap queue + def __gt__(self, other): + return self.f > other.f + +def return_path(current_node): + path = [] + current = current_node + while current is not None: + path.append(current.position) + current = current.parent + return path[::-1] # Return reversed path + + +def astar(maze, start, end, allow_diagonal_movement = True): + """ + Returns a list of tuples as a path from the given start to the given end in the given maze + :param maze: + :param start: + :param end: + :return: + """ + + # Create start and end node + start_node = Node(None, start) + start_node.g = start_node.h = start_node.f = 0 + end_node = Node(None, end) + end_node.g = end_node.h = end_node.f = 0 + + # Check if end node is within range, if it is, check cost. Return None for path so we can call astar with new incremented end goal + if end_node.position[0] <= (len(maze) - 1) and end_node.position[0] >= 0 and end_node.position[1] <= (len(maze[len(maze)-1]) -1) and end_node.position[1] >= 0: + if maze[end_node.position[0]][end_node.position[1]] == 100: + print("Target has high cost!") + return None + + # Initialize both open and closed list + open_list = [] + closed_list = [] + + # Heapify the open_list and Add the start node + heapq.heapify(open_list) + heapq.heappush(open_list, start_node) + + # Adding a stop condition + outer_iterations = 0 + max_iterations = (len(maze[0]) * len(maze) // 2) + + # what squares do we search + if allow_diagonal_movement: + adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1)) + adjacent_square_pick_index = [0, 1, 2, 3, 4, 5, 6, 7] + else: + adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0)) + adjacent_square_pick_index = [0, 1, 2, 3] + + # Loop until you find the end + while len(open_list) > 0: + #Randomize the order of the adjacent_squares_pick_index to avoid a decision making bias + random.shuffle(adjacent_square_pick_index) + + outer_iterations += 1 + + if outer_iterations > max_iterations: + # if we hit this point return the path such as it is + # it will not contain the destination + warn("giving up on pathfinding too many iterations") + return return_path(current_node) + + # Get the current node + current_node = heapq.heappop(open_list) + closed_list.append(current_node) + + # Found the goal + if current_node == end_node: + return return_path(current_node) + + # Generate children + children = [] + + for pick_index in adjacent_square_pick_index: + new_position = adjacent_squares[pick_index] + + # Get node position + node_position = (current_node.position[0] + new_position[0], current_node.position[1] + new_position[1]) + + # Make sure within range + if node_position[0] > (len(maze) - 1) or node_position[0] < 0 or node_position[1] > (len(maze[len(maze)-1]) -1) or node_position[1] < 0: + continue + + # Make sure walkable terrain + #if maze[node_position[0]][node_position[1]] != 0: + # continue + + # Create new node + new_node = Node(current_node, node_position) + + # Append + children.append(new_node) + + # Loop through children + for child in children: + # Child is on the closed list + if len([closed_child for closed_child in closed_list if closed_child == child]) > 0: + continue + + # Create the f, g, and h values + child.g = current_node.g + maze[child.position[0]][child.position[1]] + child.h = ((child.position[0] - end_node.position[0]) ** 2) + ((child.position[1] - end_node.position[1]) ** 2) + child.f = child.g + child.h + + # Child is already in the open list + if len([open_node for open_node in open_list if child.position == open_node.position and child.g > open_node.g]) > 0: + continue + + # Add the child to the open list + heapq.heappush(open_list, child) + + warn("Couldn't get a path to destination") + return None + +def example(print_maze = True): + + maze = [ + [0,0,0,0,0,1,0,0,1,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,1,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,0,0,0,1,0,1,0,1,1,1,0,0,0,0,0,0,0,1,1,0,0,0,0,0,0], + [0,0,0,0,0,0,0,1,1,1,1,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0], + [0,0,0,1,0,0,0,1,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,1,0], + [0,0,0,1,0,1,1,1,1,0,1,1,0,0,1,1,1,0,0,0,1,1,0,1,1,0,1,0,0,0], + [0,0,0,1,0,1,0,0,0,0,1,1,0,0,0,0,1,1,0,1,0,0,0,0,0,0,1,1,1,0], + [0,0,0,1,0,1,1,0,1,1,0,1,1,1,0,0,0,0,0,1,0,0,1,1,1,1,1,0,0,0], + [0,0,0,1,0,1,0,0,0,0,0,0,0,1,1,1,1,1,1,1,0,0,1,1,1,1,1,1,1,1], + [0,0,0,1,0,1,0,1,1,0,1,1,1,1,0,0,1,1,1,1,1,1,1,0,1,0,1,0,0,0], + [0,0,0,1,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,1,0,1,1,1,0], + [0,0,0,1,0,1,1,1,1,0,1,0,0,1,1,1,0,1,1,1,1,0,1,1,1,0,1,0,0,0], + [0,0,0,1,0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,1,1], + [0,0,0,1,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0,0,1,0,0,0], + [1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0],] + + maze1 = [ + [0,100,100,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,100,100,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,0,0,0,100,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,0,0,0,0,0,0,0,0,100,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], + [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],] + + # start = (0, 0) + # end = (len(maze)-1, len(maze[0])-1) + + start = (1, 1) + end = (10, 28) + + path = astar(maze, start, end) + + if path is None: + new_end = (0,17) + path = astar(maze, start, new_end) + + if print_maze and path is not None: + for step in path: + maze[step[0]][step[1]] = 2 + + for row in maze: + line = [] + for col in row: + if col == 1: + line.append("\u2588") + elif col == 0: + line.append(" ") + elif col == 2: + line.append(".") + print("".join(line)) + + print(path) + + +if __name__ == "__main__": + example() \ No newline at end of file diff --git a/scripts/debug_course_publisher.py b/scripts/debug_course_publisher.py index ff5b34384..734b953f4 100755 --- a/scripts/debug_course_publisher.py +++ b/scripts/debug_course_publisher.py @@ -21,17 +21,17 @@ convert_waypoint_to_gps(waypoint) for waypoint in [ ( - Waypoint(tag_id=0, type=WaypointType(val=WaypointType.NO_SEARCH)), - SE3(position=np.array([4, 4, 0])), - ), - ( - Waypoint(tag_id=0, type=WaypointType(val=WaypointType.POST)), - SE3(position=np.array([-2, -2, 0])), - ), - ( - Waypoint(tag_id=1, type=WaypointType(val=WaypointType.POST)), - SE3(position=np.array([11, -10, 0])), - ), + Waypoint(tag_id=-1, type=WaypointType(val=WaypointType.WATER_BOTTLE)), + SE3(position=np.array([0, 0, 0])), + )#, + # ( + # Waypoint(tag_id=0, type=WaypointType(val=WaypointType.POST)), + # SE3(position=np.array([-2, -2, 0])), + # ), + # ( + # Waypoint(tag_id=1, type=WaypointType(val=WaypointType.POST)), + # SE3(position=np.array([11, -10, 0])), + # ), ] ] ) diff --git a/scripts/debug_water_bottle_search.py b/scripts/debug_water_bottle_search.py index b7b922ec7..942fb927e 100755 --- a/scripts/debug_water_bottle_search.py +++ b/scripts/debug_water_bottle_search.py @@ -35,13 +35,13 @@ header = Header() test_grid.header = header - rospy.loginfo(f"Before publish") - costpub = rospy.Publisher("costmap", OccupancyGrid, queue_size=1) - for i in range(10): - costpub.publish(test_grid) - time.sleep(1) - rospy.loginfo(f"After publish") - rospy.spin() + # rospy.loginfo(f"Before publish") + # #costpub = rospy.Publisher("costmap", OccupancyGrid, queue_size=1) + # for i in range(10): + # costpub.publish(test_grid) + # time.sleep(1) + # rospy.loginfo(f"After publish") + # rospy.spin() except rospy.ROSInterruptException as e: print(f"Didn't work to publish or retrieve message from ros") From 3e279860806ecfcaa1ec63b587a6d9ca89c17edc Mon Sep 17 00:00:00 2001 From: Quintin Date: Mon, 26 Feb 2024 13:00:38 -0500 Subject: [PATCH 61/76] Costmap touchup (#652) * Start refactor * Working with manif * Invert lol will fix in integration * Remove manif prefix * Remove bad it --- src/perception/cost_map/cost_map.cpp | 17 +----- src/perception/cost_map/cost_map.hpp | 13 ++-- .../cost_map/cost_map.processing.cpp | 61 +++++++++---------- src/perception/cost_map/pch.hpp | 3 +- 4 files changed, 41 insertions(+), 53 deletions(-) diff --git a/src/perception/cost_map/cost_map.cpp b/src/perception/cost_map/cost_map.cpp index b400a9f01..987a30df5 100644 --- a/src/perception/cost_map/cost_map.cpp +++ b/src/perception/cost_map/cost_map.cpp @@ -2,7 +2,7 @@ namespace mrover { - void CostMapNodelet::onInit() { + auto CostMapNodelet::onInit() -> void { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); // Unused for now mNh.param("publish_cost_map", mPublishCostMap, true); @@ -20,18 +20,7 @@ namespace mrover { mGlobalGridMsg.info.origin.position.x = -1 * mDimension / 2; mGlobalGridMsg.info.origin.position.y = -1 * mDimension / 2; - mGlobalGridMsg.data.resize(mGlobalGridMsg.info.width * mGlobalGridMsg.info.height); - std::fill(mGlobalGridMsg.data.begin(), mGlobalGridMsg.data.end(), -1); + mGlobalGridMsg.data.resize(mGlobalGridMsg.info.width * mGlobalGridMsg.info.height, UNKNOWN_COST); } -} // namespace mrover -/* -TODO: -- Nodelet takes in pointcloud -PARALLELIZE -- Get cost of point -- Transform point to nav basis -- Stitch to nav grid -END - -*/ +} // namespace mrover \ No newline at end of file diff --git a/src/perception/cost_map/cost_map.hpp b/src/perception/cost_map/cost_map.hpp index 833d4c11a..3fe1adc77 100644 --- a/src/perception/cost_map/cost_map.hpp +++ b/src/perception/cost_map/cost_map.hpp @@ -6,6 +6,8 @@ namespace mrover { class CostMapNodelet final : public nodelet::Nodelet { + constexpr static std::int8_t UNKNOWN_COST = -1, FREE_COST = 0, OCCUPIED_COST = 1; + ros::NodeHandle mNh, mPnh, mCmt; ros::Publisher mCostMapPub; ros::Subscriber mPcSub; @@ -13,11 +15,11 @@ namespace mrover { bool mPublishCostMap{}; // If set, publish the global costmap float mResolution{}; // Meters per cell float mDimension{}; // Dimensions of the square costmap in meters - float mNormalThreshold = 0.9; + double mNormalThreshold = 0.9; int mDownSamplingFactor = 4; - uint32_t mNumPoints = 640 * 480 / mDownSamplingFactor; - Eigen::MatrixXd point_matrix{4, mNumPoints}; - Eigen::MatrixXd normal_matrix{4, mNumPoints}; + std::uint32_t mNumPoints = 640 * 480 / mDownSamplingFactor / mDownSamplingFactor; + Eigen::MatrixXf mPointsInMap{4, mNumPoints}; + Eigen::MatrixXf mNormalsInMap{4, mNumPoints}; tf2_ros::Buffer mTfBuffer; tf2_ros::TransformListener mTfListener{mTfBuffer}; @@ -32,8 +34,7 @@ namespace mrover { ~CostMapNodelet() override = default; - void configCallback(); - void pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg); + auto pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void; }; } // namespace mrover diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 6b8727e34..91573e037 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -1,3 +1,5 @@ +#include + #include "cost_map.hpp" namespace mrover { @@ -10,39 +12,36 @@ namespace mrover { if (!mPublishCostMap) return; try { - SE3d zed_to_map = SE3Conversions::fromTfTree(mTfBuffer, "zed_left_camera_frame", "map"); + SE3f cameraToMap = SE3Conversions::fromTfTree(mTfBuffer, "zed2i_left_camera_frame", "map").cast(); auto* points = reinterpret_cast(msg->data.data()); - // std::for_each(points, points + msg->width * msg->height, [&](auto* point) { - for (auto point = points; point - points < msg->width * msg->height; point += mDownSamplingFactor) { - Eigen::Vector4d p{point->x, point->y, point->z, 1}; - point_matrix.col((point - points) / mDownSamplingFactor) = p; - Eigen::Vector4d normal{point->normal_x, point->normal_y, point->normal_z, 0}; - normal_matrix.col((point - points) / mDownSamplingFactor) = normal; + for (std::size_t i = 0, r = 0; r < msg->height; r += mDownSamplingFactor) { + for (std::size_t c = 0; c < msg->width; c += mDownSamplingFactor) { + auto* point = points + r * msg->width + c; + Eigen::Vector4f pointInCamera{point->x, point->y, point->z, 1}; + Eigen::Vector4f normalInCamera{point->normal_x, point->normal_y, point->normal_z, 0}; + + mPointsInMap.col(static_cast(i)) = cameraToMap.transform() * pointInCamera; + mNormalsInMap.col(static_cast(i)) = cameraToMap.transform() * normalInCamera; + i++; + } } - // TODO(neven): Make sure this still works with manif - point_matrix = zed_to_map.transform() * point_matrix; - normal_matrix = zed_to_map.transform() * normal_matrix; - - for (uint32_t i = 0; i < mNumPoints; i++) { - double x = point_matrix(0, i); - double y = point_matrix(1, i); - Eigen::Vector4d n = normal_matrix.col(i); - - if (x >= mGlobalGridMsg.info.origin.position.x && x <= mGlobalGridMsg.info.origin.position.x + mDimension && - y >= mGlobalGridMsg.info.origin.position.y && y <= mGlobalGridMsg.info.origin.position.y + mDimension) { - int x_index = std::floor((x - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); - int y_index = std::floor((y - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); - auto ind = mGlobalGridMsg.info.width * y_index + x_index; - - Eigen::Vector3d normal{n.x(), n.y(), n.z()}; - // normal.normalize(); - // get vertical component of (unit) normal vector - double z_comp = normal.z(); - // small z component indicates largely horizontal normal (surface is vertical) - signed char cost = z_comp < mNormalThreshold ? 1 : 0; - - mGlobalGridMsg.data[ind] = std::max(mGlobalGridMsg.data[ind], cost); + for (Eigen::Index i = 0; i < mNumPoints; i++) { + double xInMap = mPointsInMap.col(i).x(); + double yInMap = mPointsInMap.col(i).y(); + Eigen::Vector3f normalInMap = mNormalsInMap.col(i).head<3>(); + + if (xInMap >= mGlobalGridMsg.info.origin.position.x && xInMap <= mGlobalGridMsg.info.origin.position.x + mDimension && + yInMap >= mGlobalGridMsg.info.origin.position.y && yInMap <= mGlobalGridMsg.info.origin.position.y + mDimension) { + int xIndex = std::floor((xInMap - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); + int yIndex = std::floor((yInMap - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); + auto costMapIndex = mGlobalGridMsg.info.width * yIndex + xIndex; + + // Z is the vertical component of of hte normal + // A small Z component indicates largely horizontal normal (surface is vertical) + std::int8_t cost = normalInMap.z() < mNormalThreshold ? OCCUPIED_COST : FREE_COST; + + mGlobalGridMsg.data[costMapIndex] = std::max(mGlobalGridMsg.data[costMapIndex], cost); } } mCostMapPub.publish(mGlobalGridMsg); @@ -51,4 +50,4 @@ namespace mrover { } } -} // namespace mrover +} // namespace mrover \ No newline at end of file diff --git a/src/perception/cost_map/pch.hpp b/src/perception/cost_map/pch.hpp index 807581900..b76cbe13a 100644 --- a/src/perception/cost_map/pch.hpp +++ b/src/perception/cost_map/pch.hpp @@ -2,6 +2,7 @@ #include #include +#include #include @@ -17,5 +18,3 @@ #include #include - -#include From 1cbff9959cc26bfa345307744ae051355ebd0729 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Tue, 27 Feb 2024 16:56:04 -0500 Subject: [PATCH 62/76] fix zed frame name --- config/rviz/auton_sim.rviz | 4 ++-- config/rviz/zed_test.rviz | 6 +++--- src/perception/tag_detector/tag_detector.cpp | 2 +- src/perception/tag_detector/tag_detector.processing.cpp | 2 +- src/perception/tag_detector/tag_detector.threshold.cpp | 2 +- src/perception/zed_wrapper/zed_wrapper.cpp | 4 ++-- src/simulator/urdf.cpp | 2 +- urdf/rover/rover.urdf.xacro | 6 +++--- 8 files changed, 14 insertions(+), 14 deletions(-) diff --git a/config/rviz/auton_sim.rviz b/config/rviz/auton_sim.rviz index 8ed1993dc..d16b7139a 100644 --- a/config/rviz/auton_sim.rviz +++ b/config/rviz/auton_sim.rviz @@ -82,7 +82,7 @@ Visualization Manager: Value: true right_camera_link: Value: true - zed2i_left_camera_frame: + zed_left_camera_frame: Value: true Marker Alpha: 1 Marker Scale: 1 @@ -112,7 +112,7 @@ Visualization Manager: {} right_camera_link: {} - zed2i_left_camera_frame: + zed_left_camera_frame: {} Update Interval: 0 Value: true diff --git a/config/rviz/zed_test.rviz b/config/rviz/zed_test.rviz index 104b8410d..03d5807a4 100644 --- a/config/rviz/zed_test.rviz +++ b/config/rviz/zed_test.rviz @@ -72,7 +72,7 @@ Visualization Manager: Value: false zed2i_camera_center: Value: false - zed2i_left_camera_frame: + zed_left_camera_frame: Value: false zed2i_left_camera_optical_frame: Value: false @@ -102,7 +102,7 @@ Visualization Manager: zed2i_camera_center: zed2i_baro_link: {} - zed2i_left_camera_frame: + zed_left_camera_frame: zed2i_left_camera_optical_frame: {} zed2i_temp_left_link: @@ -145,7 +145,7 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - zed2i_left_camera_frame: + zed_left_camera_frame: Alpha: 1 Show Axes: false Show Trail: false diff --git a/src/perception/tag_detector/tag_detector.cpp b/src/perception/tag_detector/tag_detector.cpp index 066dd61d5..d59d08d5c 100644 --- a/src/perception/tag_detector/tag_detector.cpp +++ b/src/perception/tag_detector/tag_detector.cpp @@ -12,7 +12,7 @@ namespace mrover { mNh.param("use_odom_frame", mUseOdom, false); mNh.param("odom_frame", mOdomFrameId, "odom"); mNh.param("world_frame", mMapFrameId, "map"); - mNh.param("camera_frame", mCameraFrameId, "zed2i_left_camera_frame"); + mNh.param("camera_frame", mCameraFrameId, "zed_left_camera_frame"); mPnh.param("publish_images", mPublishImages, true); mPnh.param("dictionary", dictionaryNumber, static_cast(cv::aruco::DICT_4X4_50)); diff --git a/src/perception/tag_detector/tag_detector.processing.cpp b/src/perception/tag_detector/tag_detector.processing.cpp index bf4026da8..1c16d3080 100644 --- a/src/perception/tag_detector/tag_detector.processing.cpp +++ b/src/perception/tag_detector/tag_detector.processing.cpp @@ -139,7 +139,7 @@ namespace mrover { } mImgMsg.header.seq = mSeqNum; mImgMsg.header.stamp = ros::Time::now(); - mImgMsg.header.frame_id = "zed2i_left_camera_frame"; + mImgMsg.header.frame_id = "zed_left_camera_frame"; mImgMsg.height = mImg.rows; mImgMsg.width = mImg.cols; mImgMsg.encoding = sensor_msgs::image_encodings::BGR8; diff --git a/src/perception/tag_detector/tag_detector.threshold.cpp b/src/perception/tag_detector/tag_detector.threshold.cpp index 41ff4a527..54576fac6 100644 --- a/src/perception/tag_detector/tag_detector.threshold.cpp +++ b/src/perception/tag_detector/tag_detector.threshold.cpp @@ -37,7 +37,7 @@ namespace mrover { mThreshMsg.header.seq = mSeqNum; mThreshMsg.header.stamp = ros::Time::now(); - mThreshMsg.header.frame_id = "zed2i_left_camera_frame"; + mThreshMsg.header.frame_id = "zed_left_camera_frame"; mThreshMsg.height = mGrayImg.rows; mThreshMsg.width = mGrayImg.cols; mThreshMsg.encoding = sensor_msgs::image_encodings::MONO8; diff --git a/src/perception/zed_wrapper/zed_wrapper.cpp b/src/perception/zed_wrapper/zed_wrapper.cpp index 5ab6cc6bf..b7dc4f5db 100644 --- a/src/perception/zed_wrapper/zed_wrapper.cpp +++ b/src/perception/zed_wrapper/zed_wrapper.cpp @@ -146,7 +146,7 @@ namespace mrover { fillPointCloudMessageFromGpu(mPcMeasures.leftPoints, mPcMeasures.leftImage, mPcMeasures.leftNormals, mPointCloudGpu, pointCloudMsg); pointCloudMsg->header.seq = mPointCloudUpdateTick; pointCloudMsg->header.stamp = mPcMeasures.time; - pointCloudMsg->header.frame_id = "zed2i_left_camera_frame"; + pointCloudMsg->header.frame_id = "zed_left_camera_frame"; mPcThreadProfiler.measureEvent("Fill Message"); if (mLeftImgPub.getNumSubscribers()) { @@ -260,7 +260,7 @@ namespace mrover { try { SE3d leftCameraInOdom{{translation.x, translation.y, translation.z}, Eigen::Quaterniond{orientation.w, orientation.x, orientation.y, orientation.z}.normalized()}; - SE3d baseLinkToLeftCamera = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "zed2i_left_camera_frame"); + SE3d baseLinkToLeftCamera = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "zed_left_camera_frame"); SE3d baseLinkInOdom = leftCameraInOdom * baseLinkToLeftCamera; SE3Conversions::pushToTfTree(mTfBroadcaster, "base_link", "odom", baseLinkInOdom); } catch (tf2::TransformException& e) { diff --git a/src/simulator/urdf.cpp b/src/simulator/urdf.cpp index 981c40b25..8607d8358 100644 --- a/src/simulator/urdf.cpp +++ b/src/simulator/urdf.cpp @@ -183,7 +183,7 @@ namespace mrover { if (link->name.contains("camera"sv)) { Camera camera = makeCameraForLink(simulator, &multiBody->getLink(linkIndex)); if (link->name.contains("zed"sv)) { - camera.frameId = "zed2i_left_camera_frame"; + camera.frameId = "zed_left_camera_frame"; camera.pub = simulator.mNh.advertise("camera/left/image", 1); camera.fov = 60; StereoCamera stereoCamera; diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 952c76118..d0d4ef21e 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -263,13 +263,13 @@ - + - + - + From cd678cf913517e0796245e4d2f82048dc4377304 Mon Sep 17 00:00:00 2001 From: Emerson-Ramey Date: Sun, 3 Mar 2024 22:12:07 -0500 Subject: [PATCH 63/76] fix navigation merge conflicts --- config/navigation.yaml | 4 +--- src/navigation/context.py | 6 ------ src/navigation/navigation.py | 11 +---------- src/navigation/waypoint.py | 16 ++-------------- 4 files changed, 4 insertions(+), 33 deletions(-) diff --git a/config/navigation.yaml b/config/navigation.yaml index 0492888bb..fdd968811 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -22,16 +22,14 @@ search: segments_per_rotation: 8 distance_between_spirals: 3 -<<<<<<< HEAD water_bottle_search: stop_thresh: 0.5 drive_fwd_thresh: 0.34 coverage_radius: 10 segments_per_rotation: 8 -======= + object_search: coverage_radius: 10 ->>>>>>> origin/integration distance_between_spirals: 3 single_fiducial: diff --git a/src/navigation/context.py b/src/navigation/context.py index 7c67016c0..db24ba878 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -9,12 +9,7 @@ import rospy import tf2_ros from geometry_msgs.msg import Twist -<<<<<<< HEAD - from util.SE3 import SE3 - -from mrover.msg import Waypoint, GPSWaypoint, WaypointType, GPSPointList, Course as CourseMsg -======= from mrover.msg import ( Waypoint, GPSWaypoint, @@ -24,7 +19,6 @@ LongRangeTag, LongRangeTags, ) ->>>>>>> origin/integration from mrover.srv import EnableAuton, EnableAutonRequest, EnableAutonResponse from navigation.drive import DriveController from navigation import approach_post, long_range, approach_object diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py index b14f4f972..72d7fe9b2 100755 --- a/src/navigation/navigation.py +++ b/src/navigation/navigation.py @@ -14,12 +14,9 @@ from navigation.search import SearchState from navigation.state import DoneState, OffState, off_check from navigation.waypoint import WaypointState -<<<<<<< HEAD from navigation.water_bottle_search import WaterBottleSearchState -======= from navigation.approach_object import ApproachObjectState from navigation.long_range import LongRangeState ->>>>>>> origin/integration class Navigation(threading.Thread): @@ -56,18 +53,12 @@ def __init__(self, context: Context): [ PostBackupState(), ApproachPostState(), -<<<<<<< HEAD - SearchState(), - RecoveryState(), - DoneState(), - WaterBottleSearchState(), -======= ApproachObjectState(), + WaterBottleSearchState(), LongRangeState(), SearchState(), RecoveryState(), DoneState(), ->>>>>>> origin/integration ], ) self.state_machine.add_transitions(ApproachObjectState(), [DoneState(), SearchState(), RecoveryState()]) diff --git a/src/navigation/waypoint.py b/src/navigation/waypoint.py index 0c9cf68ac..6ccdbda2e 100644 --- a/src/navigation/waypoint.py +++ b/src/navigation/waypoint.py @@ -3,12 +3,8 @@ from util.ros_utils import get_rosparam from util.state_lib.state import State -<<<<<<< HEAD -from navigation import search, recovery, approach_post, post_backup, state, water_bottle_search +from navigation import search, recovery, approach_post, post_backup, state, approach_object, long_range, water_bottle_search from mrover.msg import WaypointType -======= -from navigation import search, recovery, approach_post, post_backup, state, approach_object, long_range ->>>>>>> origin/integration class WaypointState(State): @@ -53,22 +49,14 @@ def on_loop(self, context) -> State: self.DRIVE_FWD_THRESH, ) if arrived: -<<<<<<< HEAD - if current_waypoint.type.val == WaypointType.NO_SEARCH: + if not context.course.look_for_post() and not context.course.look_for_object(): # We finished a regular waypoint, go onto the next one context.course.increment_waypoint() elif current_waypoint.type.val == WaypointType.WATER_BOTTLE: # We finished a waypoint associated with the water bottle, but we have not seen it yet return water_bottle_search.WaterBottleSearchState() else: - # We finished a waypoint associated with a tag id or the mallet, but we have not seen it yet. -======= - if not context.course.look_for_post() and not context.course.look_for_object(): - # We finished a regular waypoint, go onto the next one - context.course.increment_waypoint() - elif context.course.look_for_post() or context.course.look_for_object(): # We finished a waypoint associated with a post or mallet, but we have not seen it yet. ->>>>>>> origin/integration return search.SearchState() if context.rover.stuck: From 3728c6d4e48eb38544e23376587677af494bc4e9 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Mon, 4 Mar 2024 16:13:19 -0500 Subject: [PATCH 64/76] add message file --- astar.py | 218 ---------------------------------------------- msg/Costmap2D.msg | 1 + 2 files changed, 1 insertion(+), 218 deletions(-) delete mode 100644 astar.py create mode 100644 msg/Costmap2D.msg diff --git a/astar.py b/astar.py deleted file mode 100644 index d032359ad..000000000 --- a/astar.py +++ /dev/null @@ -1,218 +0,0 @@ -# Credit for this: Nicholas Swift -# as found at https://medium.com/@nicholas.w.swift/easy-a-star-pathfinding-7e6689c7f7b2 -from warnings import warn -import heapq -import random - -class Node: - """ - A node class for A* Pathfinding - """ - - def __init__(self, parent=None, position=None): - self.parent = parent - self.position = position - - self.g = 0 - self.h = 0 - self.f = 0 - - def __eq__(self, other): - return self.position == other.position - - def __repr__(self): - return f"{self.position} - g: {self.g} h: {self.h} f: {self.f}" - - # defining less than for purposes of heap queue - def __lt__(self, other): - return self.f < other.f - - # defining greater than for purposes of heap queue - def __gt__(self, other): - return self.f > other.f - -def return_path(current_node): - path = [] - current = current_node - while current is not None: - path.append(current.position) - current = current.parent - return path[::-1] # Return reversed path - - -def astar(maze, start, end, allow_diagonal_movement = True): - """ - Returns a list of tuples as a path from the given start to the given end in the given maze - :param maze: - :param start: - :param end: - :return: - """ - - # Create start and end node - start_node = Node(None, start) - start_node.g = start_node.h = start_node.f = 0 - end_node = Node(None, end) - end_node.g = end_node.h = end_node.f = 0 - - # Check if end node is within range, if it is, check cost. Return None for path so we can call astar with new incremented end goal - if end_node.position[0] <= (len(maze) - 1) and end_node.position[0] >= 0 and end_node.position[1] <= (len(maze[len(maze)-1]) -1) and end_node.position[1] >= 0: - if maze[end_node.position[0]][end_node.position[1]] == 100: - print("Target has high cost!") - return None - - # Initialize both open and closed list - open_list = [] - closed_list = [] - - # Heapify the open_list and Add the start node - heapq.heapify(open_list) - heapq.heappush(open_list, start_node) - - # Adding a stop condition - outer_iterations = 0 - max_iterations = (len(maze[0]) * len(maze) // 2) - - # what squares do we search - if allow_diagonal_movement: - adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1)) - adjacent_square_pick_index = [0, 1, 2, 3, 4, 5, 6, 7] - else: - adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0)) - adjacent_square_pick_index = [0, 1, 2, 3] - - # Loop until you find the end - while len(open_list) > 0: - #Randomize the order of the adjacent_squares_pick_index to avoid a decision making bias - random.shuffle(adjacent_square_pick_index) - - outer_iterations += 1 - - if outer_iterations > max_iterations: - # if we hit this point return the path such as it is - # it will not contain the destination - warn("giving up on pathfinding too many iterations") - return return_path(current_node) - - # Get the current node - current_node = heapq.heappop(open_list) - closed_list.append(current_node) - - # Found the goal - if current_node == end_node: - return return_path(current_node) - - # Generate children - children = [] - - for pick_index in adjacent_square_pick_index: - new_position = adjacent_squares[pick_index] - - # Get node position - node_position = (current_node.position[0] + new_position[0], current_node.position[1] + new_position[1]) - - # Make sure within range - if node_position[0] > (len(maze) - 1) or node_position[0] < 0 or node_position[1] > (len(maze[len(maze)-1]) -1) or node_position[1] < 0: - continue - - # Make sure walkable terrain - #if maze[node_position[0]][node_position[1]] != 0: - # continue - - # Create new node - new_node = Node(current_node, node_position) - - # Append - children.append(new_node) - - # Loop through children - for child in children: - # Child is on the closed list - if len([closed_child for closed_child in closed_list if closed_child == child]) > 0: - continue - - # Create the f, g, and h values - child.g = current_node.g + maze[child.position[0]][child.position[1]] - child.h = ((child.position[0] - end_node.position[0]) ** 2) + ((child.position[1] - end_node.position[1]) ** 2) - child.f = child.g + child.h - - # Child is already in the open list - if len([open_node for open_node in open_list if child.position == open_node.position and child.g > open_node.g]) > 0: - continue - - # Add the child to the open list - heapq.heappush(open_list, child) - - warn("Couldn't get a path to destination") - return None - -def example(print_maze = True): - - maze = [ - [0,0,0,0,0,1,0,0,1,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,1,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,0,0,0,1,0,1,0,1,1,1,0,0,0,0,0,0,0,1,1,0,0,0,0,0,0], - [0,0,0,0,0,0,0,1,1,1,1,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0], - [0,0,0,1,0,0,0,1,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,1,0], - [0,0,0,1,0,1,1,1,1,0,1,1,0,0,1,1,1,0,0,0,1,1,0,1,1,0,1,0,0,0], - [0,0,0,1,0,1,0,0,0,0,1,1,0,0,0,0,1,1,0,1,0,0,0,0,0,0,1,1,1,0], - [0,0,0,1,0,1,1,0,1,1,0,1,1,1,0,0,0,0,0,1,0,0,1,1,1,1,1,0,0,0], - [0,0,0,1,0,1,0,0,0,0,0,0,0,1,1,1,1,1,1,1,0,0,1,1,1,1,1,1,1,1], - [0,0,0,1,0,1,0,1,1,0,1,1,1,1,0,0,1,1,1,1,1,1,1,0,1,0,1,0,0,0], - [0,0,0,1,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,1,0,1,1,1,0], - [0,0,0,1,0,1,1,1,1,0,1,0,0,1,1,1,0,1,1,1,1,0,1,1,1,0,1,0,0,0], - [0,0,0,1,0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,1,1], - [0,0,0,1,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0,0,1,0,0,0], - [1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0],] - - maze1 = [ - [0,100,100,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,100,100,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,0,0,0,100,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,0,0,0,0,0,0,0,0,100,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], - [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],] - - # start = (0, 0) - # end = (len(maze)-1, len(maze[0])-1) - - start = (1, 1) - end = (10, 28) - - path = astar(maze, start, end) - - if path is None: - new_end = (0,17) - path = astar(maze, start, new_end) - - if print_maze and path is not None: - for step in path: - maze[step[0]][step[1]] = 2 - - for row in maze: - line = [] - for col in row: - if col == 1: - line.append("\u2588") - elif col == 0: - line.append(" ") - elif col == 2: - line.append(".") - print("".join(line)) - - print(path) - - -if __name__ == "__main__": - example() \ No newline at end of file diff --git a/msg/Costmap2D.msg b/msg/Costmap2D.msg new file mode 100644 index 000000000..6aba68c66 --- /dev/null +++ b/msg/Costmap2D.msg @@ -0,0 +1 @@ +float32[] map \ No newline at end of file From fe8ab310f5294ad2cb2196c0716666aeadf4e780 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Thu, 7 Mar 2024 19:06:05 -0500 Subject: [PATCH 65/76] fix percep topic name and testing nav path planning --- src/navigation/water_bottle_search.py | 52 +++++++++++-------- .../cost_map/cost_map.processing.cpp | 2 +- 2 files changed, 30 insertions(+), 24 deletions(-) diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index 31f6075c9..1d3dab180 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -8,8 +8,9 @@ import numpy as np import rospy -from geometry_msgs.msg import Pose, PoseStamped -from mrover.msg import GPSPointList +from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion +from std_msgs.msg import Header +from mrover.msg import GPSPointList, Costmap2D from nav_msgs.msg import OccupancyGrid, Path from navigation import approach_post, recovery, waypoint from navigation.context import convert_cartesian_to_gps, Context @@ -119,9 +120,9 @@ def return_path(self, current_node, maze): reversed_path = path[::-1] print("ij:", reversed_path[1:]) - for step in reversed_path[1:]: - maze[step[0]][step[1]] = 2 - maze[reversed_path[1][0]][reversed_path[1][1]] = 3 # start + for step in reversed_path: + maze[step[0]][step[1]] = 2 # path (.) + maze[reversed_path[0][0]][reversed_path[0][1]] = 3 # start maze[reversed_path[-1][0]][reversed_path[-1][1]] = 4 # end for row in maze: @@ -267,30 +268,30 @@ def costmap_callback(self, msg: OccupancyGrid): Callback function for the occupancy grid perception sends :param msg: Occupancy Grid representative of a 30 x 30m square area with origin at GNSS waypoint. Values are 0, 1, -1 """ - if rospy.get_time() - self.time_last_updated > 5: + if rospy.get_time() - self.time_last_updated > 1: print("RUN ASTAR") self.resolution = msg.info.resolution # meters/cell self.height = msg.info.height # cells self.width = msg.info.width # cells with self.costmap_lock: - costmap2D = np.array(msg.data).reshape(int(self.height), int(self.width)).astype(np.float32) + costmap2D = np.reshape(np.array(msg.data),(int(self.height), int(self.width))).astype(np.float32) + # costmap2D = np.fliplr(costmap2D) # change all unidentified points to have a cost of 0.5 - print("SHAPE", costmap2D.shape) costmap2D[costmap2D == -1.0] = 0.5 cur_rover_pose = self.context.rover.get_pose().position[0:2] end_point = self.traj.get_cur_pt() - print("XXXXXXXXXXXXXXXXXXXXX START COSTMAP XXXXXXXXXXXXXXXXXXXXX") - for row in costmap2D: - line = [] - for col in row: - if col == 1: - line.append("\u2588") - elif col == 0: - line.append(" ") - print("".join(line)) - print("XXXXXXXXXXXXXXXXXXXXX END COSTMAP XXXXXXXXXXXXXXXXXXXXX") + # print("XXXXXXXXXXXXXXXXXXXXX START COSTMAP XXXXXXXXXXXXXXXXXXXXX") + # for row in costmap2D:msg.data + # line = [] + # for col in row: + # if col == 1: + # line.append("\u2588").2517715 + # elif col == 0: + # line.append(" ") + # print("".join(line)) + # print("XXXXXXXXXXXXXXXXXXXXX END COSTMAP XXXXXXXXXXXXXXXXXXXXX") # call A-STAR occupancy_list = self.a_star(costmap2D, cur_rover_pose, end_point[0:2]) @@ -306,10 +307,15 @@ def costmap_callback(self, msg: OccupancyGrid): path = Path() poses = [] + path.header = Header() + path.header.frame_id = "map" for coord in cartesian_coords: - pose = SE3.from_pos_quat(coord, [0, 0, 0, 1]) pose_stamped = PoseStamped() - pose_stamped.pose = Pose(pose.position, pose.rotation.quaternion) + pose_stamped.header = Header() + pose_stamped.header.frame_id = "map" + point = Point(coord[0], coord[1], 0) + quat = Quaternion(0,0,0,1) + pose_stamped.pose = Pose(point, quat) poses.append(pose_stamped) path.poses = poses self.path_pub.publish(path) @@ -333,9 +339,9 @@ def on_enter(self, context) -> None: self.prev_target = None self.star_traj = Trajectory(np.array([])) self.time_last_updated = rospy.get_time() - self.listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) self.costmap_pub = rospy.Publisher("costmap2D", Costmap2D, queue_size=1) - self.path_pub = rospy.Publisher("path", Path, queue_size=1) + self.listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) + self.path_pub = rospy.Publisher("path", Path, queue_size=10) def on_exit(self, context) -> None: pass @@ -381,6 +387,6 @@ def on_loop(self, context) -> State: ) context.rover.send_drive_command(cmd_vel) # TODO: change so that if we are looking for the water bottle we will transition into ApproachObjectState - if context.env.current_fid_pos() is not None and context.course.look_for_post(): + if context.env.current_target_pos() is not None and context.course.look_for_post(): return approach_post.ApproachPostState() return self diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 91573e037..61c433463 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -12,7 +12,7 @@ namespace mrover { if (!mPublishCostMap) return; try { - SE3f cameraToMap = SE3Conversions::fromTfTree(mTfBuffer, "zed2i_left_camera_frame", "map").cast(); + SE3f cameraToMap = SE3Conversions::fromTfTree(mTfBuffer, "zed_left_camera_frame", "map").cast(); auto* points = reinterpret_cast(msg->data.data()); for (std::size_t i = 0, r = 0; r < msg->height; r += mDownSamplingFactor) { for (std::size_t c = 0; c < msg->width; c += mDownSamplingFactor) { From d794ae435106c801cd650a263e3cf4d7468717a2 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Thu, 7 Mar 2024 22:42:06 -0500 Subject: [PATCH 66/76] fix orientation of costmap and start to consider average of neighbor cells --- src/navigation/water_bottle_search.py | 53 +++++++++++++++------------ 1 file changed, 29 insertions(+), 24 deletions(-) diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index 1d3dab180..c1644c277 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -106,12 +106,28 @@ def ij_to_cartesian(self, ij_coords: np.ndarray) -> np.ndarray: resolution_conversion = ij_coords * [self.resolution, self.resolution] # [j * r, i * r] half_res = np.array([self.resolution / 2, -1 * self.resolution / 2]) # [r/2, -r/2] return ((self.origin - width_height) + resolution_conversion + half_res) * np.array([1, -1]) + + def avg_cell(self, costmap2D, cell) -> float: + """ + Finds the average cost of the surrounding neighbors of the cell in the costmap + Pads costmap2D with zeros for edge cases + :param costmap2D: current costmap + :param cell: cell we are wanting to find average of neighbors including self + :return: average value + """ + padded_costmap = np.pad(costmap2D, pad_width=1, mode='constant', constant_values=0) + i, j = cell + neighbors = padded_costmap[i:i+3, j:j+3] # Extract surrounding neighbors + return np.mean(neighbors) - def return_path(self, current_node, maze): + def return_path(self, current_node, costmap2D): """ Return the path given from A-Star in reverse through current node's parents :param current_node: end point of path which contains parents to retrieve path + :param costmap2D: current costmap + :return: reversed path except the starting point (we are already there) """ + costmap2D = np.copy(costmap2D) path = [] current = current_node while current is not None: @@ -121,16 +137,16 @@ def return_path(self, current_node, maze): print("ij:", reversed_path[1:]) for step in reversed_path: - maze[step[0]][step[1]] = 2 # path (.) - maze[reversed_path[0][0]][reversed_path[0][1]] = 3 # start - maze[reversed_path[-1][0]][reversed_path[-1][1]] = 4 # end + costmap2D[step[0]][step[1]] = 2 # path (.) + costmap2D[reversed_path[0][0]][reversed_path[0][1]] = 3 # start + costmap2D[reversed_path[-1][0]][reversed_path[-1][1]] = 4 # end - for row in maze: + for row in costmap2D: line = [] for col in row: - if col == 1: + if col == 1.0: line.append("\u2588") - elif col == 0: + elif col == 0.0 or col == 0.5: line.append(" ") elif col == 2: line.append(".") @@ -140,7 +156,7 @@ def return_path(self, current_node, maze): line.append("E") print("".join(line)) - return reversed_path[1:] # Return reversed path except the starting point (we are already there) + return reversed_path[1:] def a_star(self, costmap2d: np.ndarray, start: np.ndarray, end: np.ndarray) -> list | None: """ @@ -164,14 +180,14 @@ def a_star(self, costmap2d: np.ndarray, start: np.ndarray, end: np.ndarray) -> l print(f"start: {start}, end: {end}") print(f"startij: {startij}, endij: {endij}") - # check if end node is within range, if it is, check if it has a high cost + # check if end node is within range, if it is, check if it has a high cost or high surrounding cost if ( end_node.position[0] <= (costmap2d.shape[0] - 1) and end_node.position[0] >= 0 and end_node.position[1] <= (costmap2d.shape[1] - 1) and end_node.position[1] >= 0 ): - if costmap2d[end_node.position[0], end_node.position[1]] >= 1: + if costmap2d[end_node.position[0], end_node.position[1]] >= 1 or self.avg_cell(costmap2d, [end_node.position[0], end_node.position[1]]) >= 1/3: # True if the trajectory is finished so return None if self.traj.increment_point(): return None @@ -240,7 +256,7 @@ def a_star(self, costmap2d: np.ndarray, start: np.ndarray, end: np.ndarray) -> l if len([closed_child for closed_child in closed_list if closed_child == child]) > 0: continue # create the f, g, and h values - child.g = current_node.g + costmap2d[child.position[0], child.position[1]] + child.g = current_node.g + costmap2d[child.position[0], child.position[1]] + (self.avg_cell(costmap2d, [child.position[0], child.position[1]])*30) # TODO change thresholds of how much importance we want to give each element child.h = ((child.position[0] - end_node.position[0]) ** 2) + ( (child.position[1] - end_node.position[1]) ** 2 ) @@ -274,25 +290,14 @@ def costmap_callback(self, msg: OccupancyGrid): self.height = msg.info.height # cells self.width = msg.info.width # cells with self.costmap_lock: - costmap2D = np.reshape(np.array(msg.data),(int(self.height), int(self.width))).astype(np.float32) - # costmap2D = np.fliplr(costmap2D) + costmap2D = np.array(msg.data).reshape((int(self.height), int(self.width))).astype(np.float32) # change all unidentified points to have a cost of 0.5 costmap2D[costmap2D == -1.0] = 0.5 + costmap2D = np.rot90(costmap2D, k=3, axes=(0,1)) # rotate 90 degress clockwise cur_rover_pose = self.context.rover.get_pose().position[0:2] end_point = self.traj.get_cur_pt() - # print("XXXXXXXXXXXXXXXXXXXXX START COSTMAP XXXXXXXXXXXXXXXXXXXXX") - # for row in costmap2D:msg.data - # line = [] - # for col in row: - # if col == 1: - # line.append("\u2588").2517715 - # elif col == 0: - # line.append(" ") - # print("".join(line)) - # print("XXXXXXXXXXXXXXXXXXXXX END COSTMAP XXXXXXXXXXXXXXXXXXXXX") - # call A-STAR occupancy_list = self.a_star(costmap2D, cur_rover_pose, end_point[0:2]) if occupancy_list is None: From eaa6467510ca56bb7d3186bc9ac1a27fcc9ee72e Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Tue, 12 Mar 2024 19:19:45 -0400 Subject: [PATCH 67/76] update printed map, add kernel, traversable terrian --- pyproject.toml | 1 + src/navigation/water_bottle_search.py | 32 ++++++++++++++++++++++----- 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 65b9e2ce0..ccef04f85 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -26,6 +26,7 @@ dependencies = [ "aenum==3.1.15", "daphne==4.0.0", "channels==4.0.0", + "scipy==1.12.0" ] [project.optional-dependencies] diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index c1644c277..c9a7943c7 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -4,6 +4,8 @@ import random from threading import Lock from typing import Optional +from scipy.signal import convolve2d + import numpy as np @@ -146,7 +148,13 @@ def return_path(self, current_node, costmap2D): for col in row: if col == 1.0: line.append("\u2588") - elif col == 0.0 or col == 0.5: + elif col < 1.0 and col >= 0.8: + line.append("\u2593") + elif col < 0.8 and col >= 0.5: + line.append("\u2592") + elif col < 0.5 and col >= 0.2: + line.append("\u2591") + elif col < 0.2: line.append(" ") elif col == 2: line.append(".") @@ -187,7 +195,7 @@ def a_star(self, costmap2d: np.ndarray, start: np.ndarray, end: np.ndarray) -> l and end_node.position[1] <= (costmap2d.shape[1] - 1) and end_node.position[1] >= 0 ): - if costmap2d[end_node.position[0], end_node.position[1]] >= 1 or self.avg_cell(costmap2d, [end_node.position[0], end_node.position[1]]) >= 1/3: + if costmap2d[end_node.position[0], end_node.position[1]] >= 0.3: # TODO figure out value # True if the trajectory is finished so return None if self.traj.increment_point(): return None @@ -222,7 +230,7 @@ def a_star(self, costmap2d: np.ndarray, start: np.ndarray, end: np.ndarray) -> l if outer_iterations > max_iterations: # if we hit this point return the path such as it is. It will not contain the destination print("giving up on pathfinding too many iterations") - return self.return_path(current_node) + return self.return_path(current_node, costmap2d) # get the current node current_node = heapq.heappop(open_list) @@ -246,6 +254,9 @@ def a_star(self, costmap2d: np.ndarray, start: np.ndarray, end: np.ndarray) -> l ): continue + if costmap2d[node_position[0]][node_position[1]] >= 0.3: + continue + # create new node and append it new_node = Node(current_node, node_position) children.append(new_node) @@ -256,7 +267,7 @@ def a_star(self, costmap2d: np.ndarray, start: np.ndarray, end: np.ndarray) -> l if len([closed_child for closed_child in closed_list if closed_child == child]) > 0: continue # create the f, g, and h values - child.g = current_node.g + costmap2d[child.position[0], child.position[1]] + (self.avg_cell(costmap2d, [child.position[0], child.position[1]])*30) # TODO change thresholds of how much importance we want to give each element + child.g = current_node.g + costmap2d[child.position[0], child.position[1]] child.h = ((child.position[0] - end_node.position[0]) ** 2) + ( (child.position[1] - end_node.position[1]) ** 2 ) @@ -286,15 +297,24 @@ def costmap_callback(self, msg: OccupancyGrid): """ if rospy.get_time() - self.time_last_updated > 1: print("RUN ASTAR") - self.resolution = msg.info.resolution # meters/cell + + self.resolution = msg.info.resolution # meters/cell self.height = msg.info.height # cells self.width = msg.info.width # cells with self.costmap_lock: costmap2D = np.array(msg.data).reshape((int(self.height), int(self.width))).astype(np.float32) + # change all unidentified points to have a cost of 0.5 costmap2D[costmap2D == -1.0] = 0.5 costmap2D = np.rot90(costmap2D, k=3, axes=(0,1)) # rotate 90 degress clockwise + #TODO: Figure out threshold and maybe leave 1s as 1s + #Apply Kernel to average the map with zero padding + mask = (costmap2D == 1.0) + kernel = np.ones((3,3), dtype = np.float32) / 9 + costmap2D = convolve2d(costmap2D, kernel, mode = "same") + costmap2D = np.where(mask, 1.0, costmap2D) + cur_rover_pose = self.context.rover.get_pose().position[0:2] end_point = self.traj.get_cur_pt() @@ -309,7 +329,7 @@ def costmap_callback(self, msg: OccupancyGrid): np.hstack((cartesian_coords, np.zeros((cartesian_coords.shape[0], 1)))) ) # current point gets set back to 0 self.costmap_pub.publish(costmap2D.flatten()) - + path = Path() poses = [] path.header = Header() From e5179c7416145c917105a1119acb9df5012d0256 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Sat, 16 Mar 2024 01:06:52 -0400 Subject: [PATCH 68/76] add transition to approach object, add exceptions and reset trajectory --- msg/Costmap2D.msg | 1 - src/navigation/context.py | 2 +- src/navigation/navigation.py | 2 +- src/navigation/trajectory.py | 6 ++ src/navigation/water_bottle_search.py | 94 +++++++++++++++------------ 5 files changed, 59 insertions(+), 46 deletions(-) delete mode 100644 msg/Costmap2D.msg diff --git a/msg/Costmap2D.msg b/msg/Costmap2D.msg deleted file mode 100644 index 6aba68c66..000000000 --- a/msg/Costmap2D.msg +++ /dev/null @@ -1 +0,0 @@ -float32[] map \ No newline at end of file diff --git a/src/navigation/context.py b/src/navigation/context.py index db24ba878..aeb035cbb 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -120,7 +120,7 @@ def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray] return self.get_target_pos(f"fiducial{current_waypoint.tag_id}", in_odom) elif current_waypoint.type.val == WaypointType.MALLET: return self.get_target_pos("hammer", in_odom) - elif current_waypoint.type == WaypointType.WATER_BOTTLE: + elif current_waypoint.type.val == WaypointType.WATER_BOTTLE: return self.get_target_pos("bottle", in_odom) else: return None diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py index 72d7fe9b2..0ef028ec9 100755 --- a/src/navigation/navigation.py +++ b/src/navigation/navigation.py @@ -65,7 +65,7 @@ def __init__(self, context: Context): self.state_machine.add_transitions(LongRangeState(), [ApproachPostState(), SearchState(), RecoveryState()]) self.state_machine.add_transitions(OffState(), [WaypointState(), DoneState()]) self.state_machine.add_transitions( - WaterBottleSearchState(), [WaypointState(), RecoveryState(), ApproachPostState()] + WaterBottleSearchState(), [WaypointState(), RecoveryState(), ApproachObjectState()] ) self.state_machine.configure_off_switch(OffState(), off_check) self.state_machine_server = StatePublisher(self.state_machine, "nav_structure", 1, "nav_state", 10) diff --git a/src/navigation/trajectory.py b/src/navigation/trajectory.py index 8df26a20e..a4baab287 100644 --- a/src/navigation/trajectory.py +++ b/src/navigation/trajectory.py @@ -20,6 +20,12 @@ def increment_point(self) -> bool: """ self.cur_pt += 1 return self.cur_pt >= len(self.coordinates) + + def reset(self)-> None: + """ + Resets the trajectory back to its start + """ + self.cur_pt = 0 @dataclass diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index c9a7943c7..57379abdd 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -12,9 +12,9 @@ import rospy from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion from std_msgs.msg import Header -from mrover.msg import GPSPointList, Costmap2D +from mrover.msg import GPSPointList from nav_msgs.msg import OccupancyGrid, Path -from navigation import approach_post, recovery, waypoint +from navigation import approach_object, recovery, waypoint from navigation.context import convert_cartesian_to_gps, Context from navigation.trajectory import Trajectory, SearchTrajectory from util.SE3 import SE3 @@ -44,9 +44,20 @@ def __lt__(self, other): # defining greater than for purposes of heap queue def __gt__(self, other): return self.f > other.f + +class SpiralEnd(Exception): + """ + Raise when there are no more points left in the search spiral + """ + pass + +class NoPath(Exception): + """ + Raise when an A* path could not be found + """ + pass -# TODO: Right now this is a copy of search state, we will need to change this # REFERENCE: https://docs.google.com/document/d/18GjDWxIu5f5-N5t5UgbrZGdEyaDj9ZMEUuXex8-NKrA/edit class WaterBottleSearchState(State): traj: SearchTrajectory # spiral @@ -56,11 +67,10 @@ class WaterBottleSearchState(State): is_recovering: bool = False height: int = 0 # height of occupancy grid width: int = 0 # width of occupancy grid - resolution: int = 0 # resolution of occupancy - origin: np.ndarray = np.array([]) # hold the inital rover pose + resolution: int = 0 # resolution of occupancy grid + origin: np.ndarray = np.array([]) # hold the inital rover pose (waypoint of water bottle) context: Context listener: rospy.Subscriber - costmap_pub: rospy.Publisher time_last_updated: rospy.Time costmap_lock: Lock path_pub: rospy.Publisher @@ -95,7 +105,7 @@ def cartesian_to_ij(self, cart_coord: np.ndarray) -> np.ndarray: def ij_to_cartesian(self, ij_coords: np.ndarray) -> np.ndarray: """ Convert coordinates in the occupancy grid (i, j) to real world cartesian coordinates (x, y) - using formula (WP - [W/2, H/2]) + [j * r, i * r] + [r/2, -r/2] + using formula (WP - [W/2, H/2]) + [j * r, i * r] + [r/2, -r/2] * [1, -1] WP: origin W, H: grid width, height (meters) r: resolution (meters/cell) @@ -108,19 +118,6 @@ def ij_to_cartesian(self, ij_coords: np.ndarray) -> np.ndarray: resolution_conversion = ij_coords * [self.resolution, self.resolution] # [j * r, i * r] half_res = np.array([self.resolution / 2, -1 * self.resolution / 2]) # [r/2, -r/2] return ((self.origin - width_height) + resolution_conversion + half_res) * np.array([1, -1]) - - def avg_cell(self, costmap2D, cell) -> float: - """ - Finds the average cost of the surrounding neighbors of the cell in the costmap - Pads costmap2D with zeros for edge cases - :param costmap2D: current costmap - :param cell: cell we are wanting to find average of neighbors including self - :return: average value - """ - padded_costmap = np.pad(costmap2D, pad_width=1, mode='constant', constant_values=0) - i, j = cell - neighbors = padded_costmap[i:i+3, j:j+3] # Extract surrounding neighbors - return np.mean(neighbors) def return_path(self, current_node, costmap2D): """ @@ -138,6 +135,8 @@ def return_path(self, current_node, costmap2D): reversed_path = path[::-1] print("ij:", reversed_path[1:]) + # Print visual of costmap with path and start (S) and end (E) points + # The lighter the block, the more costly it is for step in reversed_path: costmap2D[step[0]][step[1]] = 2 # path (.) costmap2D[reversed_path[0][0]][reversed_path[0][1]] = 3 # start @@ -188,17 +187,17 @@ def a_star(self, costmap2d: np.ndarray, start: np.ndarray, end: np.ndarray) -> l print(f"start: {start}, end: {end}") print(f"startij: {startij}, endij: {endij}") - # check if end node is within range, if it is, check if it has a high cost or high surrounding cost + # check if end node is within range, if it is, check if it has a high cost if ( end_node.position[0] <= (costmap2d.shape[0] - 1) and end_node.position[0] >= 0 and end_node.position[1] <= (costmap2d.shape[1] - 1) and end_node.position[1] >= 0 ): - if costmap2d[end_node.position[0], end_node.position[1]] >= 0.3: # TODO figure out value - # True if the trajectory is finished so return None + while(costmap2d[end_node.position[0], end_node.position[1]] >= 0.2): # TODO: find optimal value + # True if the trajectory is finished if self.traj.increment_point(): - return None + raise SpiralEnd() # update end point to be the next point in the search spiral endij = self.cartesian_to_ij(self.traj.get_cur_pt()) end_node = Node(None, (endij[0], endij[1])) @@ -254,7 +253,8 @@ def a_star(self, costmap2d: np.ndarray, start: np.ndarray, end: np.ndarray) -> l ): continue - if costmap2d[node_position[0]][node_position[1]] >= 0.3: + # make sure it is traversable terrian (not too high of a cost) + if costmap2d[node_position[0]][node_position[1]] >= 0.2: # TODO: find optimal value continue # create new node and append it @@ -266,7 +266,7 @@ def a_star(self, costmap2d: np.ndarray, start: np.ndarray, end: np.ndarray) -> l # child is on the closed list if len([closed_child for closed_child in closed_list if closed_child == child]) > 0: continue - # create the f, g, and h values + # create the f (total), g (cost in map), and h (euclidean distance) values child.g = current_node.g + costmap2d[child.position[0], child.position[1]] child.h = ((child.position[0] - end_node.position[0]) ** 2) + ( (child.position[1] - end_node.position[1]) ** 2 @@ -288,14 +288,15 @@ def a_star(self, costmap2d: np.ndarray, start: np.ndarray, end: np.ndarray) -> l heapq.heappush(open_list, child) print("Couldn't find a path to destination") - return None + raise NoPath() def costmap_callback(self, msg: OccupancyGrid): """ Callback function for the occupancy grid perception sends :param msg: Occupancy Grid representative of a 30 x 30m square area with origin at GNSS waypoint. Values are 0, 1, -1 """ - if rospy.get_time() - self.time_last_updated > 1: + # only update our costmap every 0.5 seconds + if rospy.get_time() - self.time_last_updated > 0.5: print("RUN ASTAR") self.resolution = msg.info.resolution # meters/cell @@ -304,22 +305,31 @@ def costmap_callback(self, msg: OccupancyGrid): with self.costmap_lock: costmap2D = np.array(msg.data).reshape((int(self.height), int(self.width))).astype(np.float32) - # change all unidentified points to have a cost of 0.5 - costmap2D[costmap2D == -1.0] = 0.5 + # change all unidentified points to have a slight cost + costmap2D[costmap2D == -1.0] = 0.1 # TODO: find optimal value costmap2D = np.rot90(costmap2D, k=3, axes=(0,1)) # rotate 90 degress clockwise - #TODO: Figure out threshold and maybe leave 1s as 1s - #Apply Kernel to average the map with zero padding - mask = (costmap2D == 1.0) - kernel = np.ones((3,3), dtype = np.float32) / 9 + # apply kernel to average the map with zero padding + kernel_shape = (7,7) # TODO: find optimal kernel size + kernel = np.ones(kernel_shape, dtype = np.float32) / (kernel_shape[0]*kernel_shape[1]) costmap2D = convolve2d(costmap2D, kernel, mode = "same") - costmap2D = np.where(mask, 1.0, costmap2D) cur_rover_pose = self.context.rover.get_pose().position[0:2] end_point = self.traj.get_cur_pt() # call A-STAR - occupancy_list = self.a_star(costmap2D, cur_rover_pose, end_point[0:2]) + try: + occupancy_list = self.a_star(costmap2D, cur_rover_pose, end_point[0:2]) + except SpiralEnd as spiral_error: + # TODO: what to do in this case + self.traj.reset() + occupancy_list = None + except NoPath as path_error: + # increment end point + if self.traj.increment_point(): + # TODO: what to do in this case + self.traj.reset() + occupancy_list = None if occupancy_list is None: self.star_traj = Trajectory(np.array([])) else: @@ -328,7 +338,6 @@ def costmap_callback(self, msg: OccupancyGrid): self.star_traj = Trajectory( np.hstack((cartesian_coords, np.zeros((cartesian_coords.shape[0], 1)))) ) # current point gets set back to 0 - self.costmap_pub.publish(costmap2D.flatten()) path = Path() poses = [] @@ -364,12 +373,11 @@ def on_enter(self, context) -> None: self.prev_target = None self.star_traj = Trajectory(np.array([])) self.time_last_updated = rospy.get_time() - self.costmap_pub = rospy.Publisher("costmap2D", Costmap2D, queue_size=1) - self.listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) + self.costmap_listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) self.path_pub = rospy.Publisher("path", Path, queue_size=10) def on_exit(self, context) -> None: - pass + self.costmap_listener.unregister() def on_loop(self, context) -> State: # continue executing the path from wherever it left off @@ -411,7 +419,7 @@ def on_loop(self, context) -> State: GPSPointList([convert_cartesian_to_gps(pt) for pt in self.traj.coordinates]) ) context.rover.send_drive_command(cmd_vel) - # TODO: change so that if we are looking for the water bottle we will transition into ApproachObjectState - if context.env.current_target_pos() is not None and context.course.look_for_post(): - return approach_post.ApproachPostState() + + if context.env.current_target_pos() is not None and context.course.look_for_object(): + return approach_object.ApproachObjectState() return self From 7e69d19c62d149bd648337d9febbb1110e43b215 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Thu, 21 Mar 2024 20:09:05 -0400 Subject: [PATCH 69/76] add extra points to water bottle search spiral --- src/navigation/search.py | 2 ++ src/navigation/trajectory.py | 25 ++++++++++++++++++++++--- src/navigation/water_bottle_search.py | 3 ++- 3 files changed, 26 insertions(+), 4 deletions(-) diff --git a/src/navigation/search.py b/src/navigation/search.py index ea99869fd..e4ec19e39 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -40,6 +40,7 @@ def on_enter(self, context) -> None: self.DISTANCE_BETWEEN_SPIRALS, self.SEGMENTS_PER_ROTATION, search_center.tag_id, + False ) else: # water bottle or mallet self.traj = SearchTrajectory.spiral_traj( @@ -48,6 +49,7 @@ def on_enter(self, context) -> None: self.OBJECT_DISTANCE_BETWEEN_SPIRALS, self.SEGMENTS_PER_ROTATION, search_center.tag_id, + False ) self.prev_target = None diff --git a/src/navigation/trajectory.py b/src/navigation/trajectory.py index a4baab287..15c627bff 100644 --- a/src/navigation/trajectory.py +++ b/src/navigation/trajectory.py @@ -35,7 +35,7 @@ class SearchTrajectory(Trajectory): @classmethod def gen_spiral_coordinates( - cls, coverage_radius: float, distance_between_spirals: float, num_segments_per_rotation: int + cls, coverage_radius: float, distance_between_spirals: float, num_segments_per_rotation: int, insert_extra: bool ) -> np.ndarray: """ Generates a set of coordinates for a spiral search pattern centered at the origin @@ -58,7 +58,25 @@ def gen_spiral_coordinates( ycoords = np.sin(angles) * radii # we want to return as a 2D matrix where each row is a coordinate pair # so we reshape x and y coordinates to be (n, 1) matricies then stack horizontally to get (n, 2) matrix - return np.hstack((xcoords.reshape(-1, 1), ycoords.reshape(-1, 1))) + vertices = np.hstack((xcoords.reshape(-1, 1), ycoords.reshape(-1, 1))) + all_points = [] + print("VERTICES") + print(repr(vertices)) + if insert_extra: + for i in range(len(vertices) - 1): + all_points.append(vertices[i]) + vector = vertices[i+1] - vertices[i] + magnitude = np.linalg.norm(vector) + unit_vector = vector / magnitude + count = 0.0 + while(count < magnitude - 3.5): + all_points.append(all_points[-1]+(unit_vector*2.5)) + count += 2.5 + print("ALL POINTS:") + print(repr(np.array(all_points))) + return np.array(all_points) + + return vertices @classmethod def spiral_traj( @@ -68,6 +86,7 @@ def spiral_traj( distance_between_spirals: float, segments_per_rotation: int, fid_id: int, + insert_extra: bool ): """ Generates a square spiral search pattern around a center position, assumes rover is at the center position @@ -79,7 +98,7 @@ def spiral_traj( :return: SearchTrajectory object """ zero_centered_spiral_r2 = cls.gen_spiral_coordinates( - coverage_radius, distance_between_spirals, segments_per_rotation + coverage_radius, distance_between_spirals, segments_per_rotation, insert_extra ) # numpy broadcasting magic to add center to each row of the spiral coordinates diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index 57379abdd..e3a1b09a1 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -296,7 +296,7 @@ def costmap_callback(self, msg: OccupancyGrid): :param msg: Occupancy Grid representative of a 30 x 30m square area with origin at GNSS waypoint. Values are 0, 1, -1 """ # only update our costmap every 0.5 seconds - if rospy.get_time() - self.time_last_updated > 0.5: + if rospy.get_time() - self.time_last_updated > 1: print("RUN ASTAR") self.resolution = msg.info.resolution # meters/cell @@ -367,6 +367,7 @@ def on_enter(self, context) -> None: self.DISTANCE_BETWEEN_SPIRALS, self.SEGMENTS_PER_ROTATION, search_center.tag_id, + True ) self.origin = context.rover.get_pose().position[0:2] print(f"ORIGIN: {self.origin}") From bb27616b9029730261f7ba5a8bd397a6b3d5ee09 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Tue, 26 Mar 2024 19:57:28 -0400 Subject: [PATCH 70/76] create new file for astar, move costmap callback to context --- config/navigation.yaml | 2 +- src/navigation/astar.py | 245 +++++++++++++++++ src/navigation/context.py | 36 ++- src/navigation/water_bottle_search.py | 379 +++++--------------------- 4 files changed, 350 insertions(+), 312 deletions(-) create mode 100644 src/navigation/astar.py diff --git a/config/navigation.yaml b/config/navigation.yaml index fdd968811..c1ca5e300 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -26,7 +26,7 @@ water_bottle_search: stop_thresh: 0.5 drive_fwd_thresh: 0.34 coverage_radius: 10 - segments_per_rotation: 8 + segments_per_rotation: 10 object_search: coverage_radius: 10 diff --git a/src/navigation/astar.py b/src/navigation/astar.py new file mode 100644 index 000000000..fefce2159 --- /dev/null +++ b/src/navigation/astar.py @@ -0,0 +1,245 @@ +from __future__ import annotations + +import heapq +import random +from threading import Lock + +import numpy as np + +from navigation.context import Context + +class SpiralEnd(Exception): + """ + Raise when there are no more points left in the search spiral + """ + pass + +class NoPath(Exception): + """ + Raise when an A* path could not be found + """ + pass + +class AStar: + origin: np.ndarray = np.array([]) # holds the inital rover pose (waypoint of water bottle) + context: Context + costmap_lock: Lock + + def __init__(self, origin: np.ndarray, context: Context)-> None: + self.origin = origin + self.context = context + self.costmap_lock = Lock() + + class Node: + """ + Node class for astar pathfinding + """ + def __init__(self, parent=None, position=None): + self.parent = parent + self.position = position + self.g = 0 + self.h = 0 + self.f = 0 + + def __eq__(self, other): + return self.position == other.position + + # defining less than for purposes of heap queue + def __lt__(self, other): + return self.f < other.f + + # defining greater than for purposes of heap queue + def __gt__(self, other): + return self.f > other.f + + def cartesian_to_ij(self, cart_coord: np.ndarray) -> np.ndarray: + """ + Convert real world cartesian coordinates (x, y) to coordinates in the occupancy grid (i, j) + using formula floor(v - (WP + [-W/2, H/2]) / r) * [1, -1] + v: (x,y) coordinate + WP: origin + W, H: grid width, height (meters) + r: resolution (meters/cell) + :param cart_coord: array of x and y cartesian coordinates + :return: array of i and j coordinates for the occupancy grid + """ + width = self.context.env.cost_map.width * self.context.env.cost_map.resolution + height = self.context.env.cost_map.height * self.context.env.cost_map.resolution + width_height = np.array([-1 * width / 2, height / 2]) # [-W/2, H/2] + converted_coord = np.floor((cart_coord[0:2] - (self.origin + width_height)) / self.context.env.cost_map.resolution) + return converted_coord.astype(np.int8) * np.array([1, -1]) + + def ij_to_cartesian(self, ij_coords: np.ndarray) -> np.ndarray: + """ + Convert coordinates in the occupancy grid (i, j) to real world cartesian coordinates (x, y) + using formula (WP - [W/2, H/2]) + [j * r, i * r] + [r/2, -r/2] * [1, -1] + WP: origin + W, H: grid width, height (meters) + r: resolution (meters/cell) + :param ij_coords: array of i and j occupancy grid coordinates + :return: array of x and y coordinates in the real world + """ + width = self.context.env.cost_map.width * self.context.env.cost_map.resolution + height = self.context.env.cost_map.height * self.context.env.cost_map.resolution + width_height = np.array([width / 2, height / 2]) # [W/2, H/2] + resolution_conversion = ij_coords * [self.context.env.cost_map.resolution, self.context.env.cost_map.resolution] # [j * r, i * r] + half_res = np.array([self.context.env.cost_map.resolution / 2, -1 * self.context.env.cost_map.resolution / 2]) # [r/2, -r/2] + return ((self.origin - width_height) + resolution_conversion + half_res) * np.array([1, -1]) + + def return_path(self, current_node: Node) -> list: + """ + Return the path given from A-Star in reverse through current node's parents + :param current_node: end point of path which contains parents to retrieve path + :param costmap2D: current costmap + :return: reversed path except the starting point (we are already there) + """ + costmap2D = np.copy(self.context.env.cost_map.data) + path = [] + current = current_node + while current is not None: + path.append(current.position) + current = current.parent + reversed_path = path[::-1] + print("ij:", reversed_path[1:]) + + # Print visual of costmap with path and start (S) and end (E) points + # The lighter the block, the more costly it is + for step in reversed_path: + costmap2D[step[0]][step[1]] = 2 # path (.) + costmap2D[reversed_path[0][0]][reversed_path[0][1]] = 3 # start + costmap2D[reversed_path[-1][0]][reversed_path[-1][1]] = 4 # end + + for row in costmap2D: + line = [] + for col in row: + if col == 1.0: + line.append("\u2588") + elif col < 1.0 and col >= 0.8: + line.append("\u2593") + elif col < 0.8 and col >= 0.5: + line.append("\u2592") + elif col < 0.5 and col >= 0.2: + line.append("\u2591") + elif col < 0.2: + line.append(" ") + elif col == 2: + line.append(".") + elif col == 3: + line.append("S") + elif col == 4: + line.append("E") + print("".join(line)) + + return reversed_path[1:] + + def a_star(self, start: np.ndarray, end: np.ndarray) -> list | None: + """ + A-STAR Algorithm: f(n) = g(n) + h(n) to find a path from the given start to the given end in the given costmap + :param costmap2d: occupancy grid in the form of an 2D array of floats + :param start: rover pose in cartesian coordinates + :param end: next point in the spiral from traj in cartesian coordinates + :return: list of A-STAR coordinates in the occupancy grid coordinates (i,j) + """ + with self.costmap_lock: + costmap2d = self.context.env.cost_map.data + # convert start and end to occupancy grid coordinates + startij = self.cartesian_to_ij(start) + endij = self.cartesian_to_ij(end) + + # initialize start and end nodes + start_node = self.Node(None, (startij[0], startij[1])) + end_node = self.Node(None, (endij[0], endij[1])) + + if start_node == end_node: + return None + + print(f"start: {start}, end: {end}") + print(f"startij: {startij}, endij: {endij}") + + # initialize both open and closed list + open_list = [] + closed_list = [] + + # heapify the open_list and add the start node + heapq.heapify(open_list) + heapq.heappush(open_list, start_node) + + # add a stop condition + outer_iterations = 0 + max_iterations = costmap2d.shape[0] * costmap2d.shape[1] // 2 + + # movements/squares we can search + adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1)) + adjacent_square_pick_index = [0, 1, 2, 3, 4, 5, 6, 7] + + # loop until you find the end + while len(open_list) > 0: + # randomize the order of the adjacent_squares_pick_index to avoid a decision making bias + random.shuffle(adjacent_square_pick_index) + + outer_iterations += 1 + + if outer_iterations > max_iterations: + # if we hit this point return the path such as it is. It will not contain the destination + print("giving up on pathfinding too many iterations") + return self.return_path(current_node) + + # get the current node + current_node = heapq.heappop(open_list) + closed_list.append(current_node) + + # found the goal + if current_node == end_node: + return self.return_path(current_node) + + # generate children + children = [] + for pick_index in adjacent_square_pick_index: + new_position = adjacent_squares[pick_index] + node_position = (current_node.position[0] + new_position[0], current_node.position[1] + new_position[1]) + # make sure within range + if ( + node_position[0] > (costmap2d.shape[0] - 1) + or node_position[0] < 0 + or node_position[1] > (costmap2d.shape[1] - 1) + or node_position[1] < 0 + ): + continue + + # make sure it is traversable terrian (not too high of a cost) + if costmap2d[node_position[0]][node_position[1]] >= 0.2: # TODO: find optimal value + continue + + # create new node and append it + new_node = self.Node(current_node, node_position) + children.append(new_node) + + # loop through children + for child in children: + # child is on the closed list + if len([closed_child for closed_child in closed_list if closed_child == child]) > 0: + continue + # create the f (total), g (cost in map), and h (euclidean distance) values + child.g = current_node.g + costmap2d[child.position[0], child.position[1]] + child.h = ((child.position[0] - end_node.position[0]) ** 2) + ( + (child.position[1] - end_node.position[1]) ** 2 + ) + child.f = child.g + child.h + # child is already in the open list + if ( + len( + [ + open_node + for open_node in open_list + if child.position == open_node.position and child.g > open_node.g + ] + ) + > 0 + ): + continue + # add the child to the open list + heapq.heappush(open_list, child) + + print("Couldn't find a path to destination") + raise NoPath() + \ No newline at end of file diff --git a/src/navigation/context.py b/src/navigation/context.py index aeb035cbb..33033f8f0 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -2,6 +2,7 @@ from dataclasses import dataclass from typing import ClassVar, Optional, List, Tuple +from scipy.signal import convolve2d import numpy as np import pymap3d @@ -22,6 +23,7 @@ from mrover.srv import EnableAuton, EnableAutonRequest, EnableAutonResponse from navigation.drive import DriveController from navigation import approach_post, long_range, approach_object +from nav_msgs.msg import OccupancyGrid from std_msgs.msg import Time, Bool from util.SE3 import SE3 from visualization_msgs.msg import Marker @@ -78,6 +80,7 @@ class Environment: ctx: Context long_range_tags: LongRangeTagStore + cost_map: CostMap NO_FIDUCIAL: ClassVar[int] = -1 arrived_at_target: bool = False last_target_location: Optional[np.ndarray] = None @@ -177,6 +180,15 @@ def get(self, tag_id: int) -> Optional[LongRangeTag]: return None +class CostMap: + """ + Context class to represent the costmap generated around the water bottle waypoint + """ + data: np.ndarray + resolution: int + height: int + width: int + @dataclass class Course: ctx: Context @@ -305,6 +317,7 @@ class Context: course_listener: rospy.Subscriber stuck_listener: rospy.Subscriber tag_data_listener: rospy.Subscriber + costmap_listener: rospy.Subscriber # Use these as the primary interfaces in states course: Optional[Course] @@ -328,13 +341,14 @@ def __init__(self): self.stuck_listener = rospy.Subscriber("nav_stuck", Bool, self.stuck_callback) self.course = None self.rover = Rover(self, False, "") - self.env = Environment(self, long_range_tags=LongRangeTagStore(self)) + self.env = Environment(self, long_range_tags=LongRangeTagStore(self), cost_map=CostMap()) self.disable_requested = False self.use_odom = rospy.get_param("use_odom_frame") self.world_frame = rospy.get_param("world_frame") self.odom_frame = rospy.get_param("odom_frame") self.rover_frame = rospy.get_param("rover_frame") self.tag_data_listener = rospy.Subscriber("tags", LongRangeTags, self.tag_data_callback) + self.costmap_listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) def recv_enable_auton(self, req: EnableAutonRequest) -> EnableAutonResponse: if req.enable: @@ -348,3 +362,23 @@ def stuck_callback(self, msg: Bool): def tag_data_callback(self, tags: LongRangeTags) -> None: self.env.long_range_tags.push_frame(tags.longRangeTags) + + def costmap_callback(self, msg: OccupancyGrid): + """ + Callback function for the occupancy grid perception sends + :param msg: Occupancy Grid representative of a 30 x 30m square area with origin at GNSS waypoint. Values are 0, 1, -1 + """ + self.env.cost_map.resolution = msg.info.resolution # meters/cell + self.env.cost_map.height = msg.info.height # cells + self.env.cost_map.width = msg.info.width # cells + self.env.cost_map.data = np.array(msg.data).reshape((int(self.env.cost_map.height), int(self.env.cost_map.width))).astype(np.float32) + + # change all unidentified points to have a slight cost + self.env.cost_map.data[self.env.cost_map.data == -1.0] = 0.1 # TODO: find optimal value + self.env.cost_map.data = np.rot90(self.env.cost_map.data, k=3, axes=(0,1)) # rotate 90 degress clockwise + + # apply kernel to average the map with zero padding + kernel_shape = (7,7) # TODO: find optimal kernel size + kernel = np.ones(kernel_shape, dtype = np.float32) / (kernel_shape[0]*kernel_shape[1]) + self.env.cost_map.data = convolve2d(self.env.cost_map.data, kernel, mode = "same") + diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py index e3a1b09a1..e011cc30f 100644 --- a/src/navigation/water_bottle_search.py +++ b/src/navigation/water_bottle_search.py @@ -1,11 +1,6 @@ from __future__ import annotations -import heapq -import random -from threading import Lock from typing import Optional -from scipy.signal import convolve2d - import numpy as np @@ -13,179 +8,50 @@ from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion from std_msgs.msg import Header from mrover.msg import GPSPointList -from nav_msgs.msg import OccupancyGrid, Path +from nav_msgs.msg import Path from navigation import approach_object, recovery, waypoint from navigation.context import convert_cartesian_to_gps, Context from navigation.trajectory import Trajectory, SearchTrajectory -from util.SE3 import SE3 +from navigation.astar import AStar, SpiralEnd, NoPath from util.ros_utils import get_rosparam from util.state_lib.state import State -class Node: - """ - Node class for astar pathfinding - """ - - def __init__(self, parent=None, position=None): - self.parent = parent - self.position = position - self.g = 0 - self.h = 0 - self.f = 0 - - def __eq__(self, other): - return self.position == other.position - - # defining less than for purposes of heap queue - def __lt__(self, other): - return self.f < other.f - - # defining greater than for purposes of heap queue - def __gt__(self, other): - return self.f > other.f - -class SpiralEnd(Exception): - """ - Raise when there are no more points left in the search spiral - """ - pass - -class NoPath(Exception): - """ - Raise when an A* path could not be found - """ - pass - - # REFERENCE: https://docs.google.com/document/d/18GjDWxIu5f5-N5t5UgbrZGdEyaDj9ZMEUuXex8-NKrA/edit class WaterBottleSearchState(State): + """ + State when searching for the water bottle + Follows a search spiral but uses A* to avoid obstacles + """ traj: SearchTrajectory # spiral star_traj: Trajectory # returned by astar - # when we are moving along the spiral prev_target: Optional[np.ndarray] = None is_recovering: bool = False - height: int = 0 # height of occupancy grid - width: int = 0 # width of occupancy grid - resolution: int = 0 # resolution of occupancy grid - origin: np.ndarray = np.array([]) # hold the inital rover pose (waypoint of water bottle) context: Context - listener: rospy.Subscriber time_last_updated: rospy.Time - costmap_lock: Lock path_pub: rospy.Publisher + astar: AStar STOP_THRESH = get_rosparam("water_bottle_search/stop_thresh", 0.5) DRIVE_FWD_THRESH = get_rosparam("water_bottle_search/drive_fwd_thresh", 0.34) SPIRAL_COVERAGE_RADIUS = get_rosparam("water_bottle_search/coverage_radius", 10) SEGMENTS_PER_ROTATION = get_rosparam( - "water_bottle_search/segments_per_rotation", 8 + "water_bottle_search/segments_per_rotation", 10 ) # TODO: after testing, might need to change DISTANCE_BETWEEN_SPIRALS = get_rosparam( "water_bottle_search/distance_between_spirals", 3 - ) # TODO: after testing, might need to change + ) # TODO: after testing, might need to change - def cartesian_to_ij(self, cart_coord: np.ndarray) -> np.ndarray: - """ - Convert real world cartesian coordinates (x, y) to coordinates in the occupancy grid (i, j) - using formula floor(v - (WP + [-W/2, H/2]) / r) * [1, -1] - v: (x,y) coordinate - WP: origin - W, H: grid width, height (meters) - r: resolution (meters/cell) - :param cart_coord: array of x and y cartesian coordinates - :return: array of i and j coordinates for the occupancy grid - """ - width = self.width * self.resolution - height = self.height * self.resolution - width_height = np.array([-1 * width / 2, height / 2]) # [-W/2, H/2] - converted_coord = np.floor((cart_coord[0:2] - (self.origin + width_height)) / self.resolution) - return converted_coord.astype(np.int8) * np.array([1, -1]) - - def ij_to_cartesian(self, ij_coords: np.ndarray) -> np.ndarray: - """ - Convert coordinates in the occupancy grid (i, j) to real world cartesian coordinates (x, y) - using formula (WP - [W/2, H/2]) + [j * r, i * r] + [r/2, -r/2] * [1, -1] - WP: origin - W, H: grid width, height (meters) - r: resolution (meters/cell) - :param ij_coords: array of i and j occupancy grid coordinates - :return: array of x and y coordinates in the real world - """ - width = self.width * self.resolution - height = self.height * self.resolution - width_height = np.array([width / 2, height / 2]) # [W/2, H/2] - resolution_conversion = ij_coords * [self.resolution, self.resolution] # [j * r, i * r] - half_res = np.array([self.resolution / 2, -1 * self.resolution / 2]) # [r/2, -r/2] - return ((self.origin - width_height) + resolution_conversion + half_res) * np.array([1, -1]) - - def return_path(self, current_node, costmap2D): - """ - Return the path given from A-Star in reverse through current node's parents - :param current_node: end point of path which contains parents to retrieve path - :param costmap2D: current costmap - :return: reversed path except the starting point (we are already there) - """ - costmap2D = np.copy(costmap2D) - path = [] - current = current_node - while current is not None: - path.append(current.position) - current = current.parent - reversed_path = path[::-1] - print("ij:", reversed_path[1:]) - - # Print visual of costmap with path and start (S) and end (E) points - # The lighter the block, the more costly it is - for step in reversed_path: - costmap2D[step[0]][step[1]] = 2 # path (.) - costmap2D[reversed_path[0][0]][reversed_path[0][1]] = 3 # start - costmap2D[reversed_path[-1][0]][reversed_path[-1][1]] = 4 # end - - for row in costmap2D: - line = [] - for col in row: - if col == 1.0: - line.append("\u2588") - elif col < 1.0 and col >= 0.8: - line.append("\u2593") - elif col < 0.8 and col >= 0.5: - line.append("\u2592") - elif col < 0.5 and col >= 0.2: - line.append("\u2591") - elif col < 0.2: - line.append(" ") - elif col == 2: - line.append(".") - elif col == 3: - line.append("S") - elif col == 4: - line.append("E") - print("".join(line)) - - return reversed_path[1:] - - def a_star(self, costmap2d: np.ndarray, start: np.ndarray, end: np.ndarray) -> list | None: + def find_endpoint(self, end: np.ndarray) -> np.ndarray: """ A-STAR Algorithm: f(n) = g(n) + h(n) to find a path from the given start to the given end in the given costmap - :param costmap2d: occupancy grid in the form of an 2D array of floats - :param start: rover pose in cartesian coordinates :param end: next point in the spiral from traj in cartesian coordinates - :return: list of A-STAR coordinates in the occupancy grid coordinates (i,j) + :return: the end point in cartesian coordinates """ - # convert start and end to occupancy grid coordinates - startij = self.cartesian_to_ij(start) - endij = self.cartesian_to_ij(end) - - # initialize start and end nodes - start_node = Node(None, (startij[0], startij[1])) - end_node = Node(None, (endij[0], endij[1])) - - if start_node == end_node: - return None - - print(f"start: {start}, end: {end}") - print(f"startij: {startij}, endij: {endij}") + costmap2d = self.context.env.cost_map.data + # convert end to occupancy grid coordinates then node + endij = self.astar.cartesian_to_ij(end) + end_node = self.astar.Node(None, (endij[0], endij[1])) # check if end node is within range, if it is, check if it has a high cost if ( @@ -199,166 +65,13 @@ def a_star(self, costmap2d: np.ndarray, start: np.ndarray, end: np.ndarray) -> l if self.traj.increment_point(): raise SpiralEnd() # update end point to be the next point in the search spiral - endij = self.cartesian_to_ij(self.traj.get_cur_pt()) - end_node = Node(None, (endij[0], endij[1])) + endij = self.astar.cartesian_to_ij(self.traj.get_cur_pt()) + end_node = self.astar.Node(None, (endij[0], endij[1])) print(f"End has high cost! new end: {endij}") - - # initialize both open and closed list - open_list = [] - closed_list = [] - - # heapify the open_list and add the start node - heapq.heapify(open_list) - heapq.heappush(open_list, start_node) - - # add a stop condition - outer_iterations = 0 - max_iterations = costmap2d.shape[0] * costmap2d.shape[1] // 2 - - # movements/squares we can search - adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1)) - adjacent_square_pick_index = [0, 1, 2, 3, 4, 5, 6, 7] - - # loop until you find the end - while len(open_list) > 0: - # randomize the order of the adjacent_squares_pick_index to avoid a decision making bias - random.shuffle(adjacent_square_pick_index) - - outer_iterations += 1 - - if outer_iterations > max_iterations: - # if we hit this point return the path such as it is. It will not contain the destination - print("giving up on pathfinding too many iterations") - return self.return_path(current_node, costmap2d) - - # get the current node - current_node = heapq.heappop(open_list) - closed_list.append(current_node) - - # found the goal - if current_node == end_node: - return self.return_path(current_node, costmap2d) - - # generate children - children = [] - for pick_index in adjacent_square_pick_index: - new_position = adjacent_squares[pick_index] - node_position = (current_node.position[0] + new_position[0], current_node.position[1] + new_position[1]) - # make sure within range - if ( - node_position[0] > (costmap2d.shape[0] - 1) - or node_position[0] < 0 - or node_position[1] > (costmap2d.shape[1] - 1) - or node_position[1] < 0 - ): - continue - - # make sure it is traversable terrian (not too high of a cost) - if costmap2d[node_position[0]][node_position[1]] >= 0.2: # TODO: find optimal value - continue - - # create new node and append it - new_node = Node(current_node, node_position) - children.append(new_node) - - # loop through children - for child in children: - # child is on the closed list - if len([closed_child for closed_child in closed_list if closed_child == child]) > 0: - continue - # create the f (total), g (cost in map), and h (euclidean distance) values - child.g = current_node.g + costmap2d[child.position[0], child.position[1]] - child.h = ((child.position[0] - end_node.position[0]) ** 2) + ( - (child.position[1] - end_node.position[1]) ** 2 - ) - child.f = child.g + child.h - # child is already in the open list - if ( - len( - [ - open_node - for open_node in open_list - if child.position == open_node.position and child.g > open_node.g - ] - ) - > 0 - ): - continue - # add the child to the open list - heapq.heappush(open_list, child) - - print("Couldn't find a path to destination") - raise NoPath() - - def costmap_callback(self, msg: OccupancyGrid): - """ - Callback function for the occupancy grid perception sends - :param msg: Occupancy Grid representative of a 30 x 30m square area with origin at GNSS waypoint. Values are 0, 1, -1 - """ - # only update our costmap every 0.5 seconds - if rospy.get_time() - self.time_last_updated > 1: - print("RUN ASTAR") - - self.resolution = msg.info.resolution # meters/cell - self.height = msg.info.height # cells - self.width = msg.info.width # cells - with self.costmap_lock: - costmap2D = np.array(msg.data).reshape((int(self.height), int(self.width))).astype(np.float32) - - # change all unidentified points to have a slight cost - costmap2D[costmap2D == -1.0] = 0.1 # TODO: find optimal value - costmap2D = np.rot90(costmap2D, k=3, axes=(0,1)) # rotate 90 degress clockwise - - # apply kernel to average the map with zero padding - kernel_shape = (7,7) # TODO: find optimal kernel size - kernel = np.ones(kernel_shape, dtype = np.float32) / (kernel_shape[0]*kernel_shape[1]) - costmap2D = convolve2d(costmap2D, kernel, mode = "same") - - cur_rover_pose = self.context.rover.get_pose().position[0:2] - end_point = self.traj.get_cur_pt() - - # call A-STAR - try: - occupancy_list = self.a_star(costmap2D, cur_rover_pose, end_point[0:2]) - except SpiralEnd as spiral_error: - # TODO: what to do in this case - self.traj.reset() - occupancy_list = None - except NoPath as path_error: - # increment end point - if self.traj.increment_point(): - # TODO: what to do in this case - self.traj.reset() - occupancy_list = None - if occupancy_list is None: - self.star_traj = Trajectory(np.array([])) - else: - cartesian_coords = self.ij_to_cartesian(np.array(occupancy_list)) - print(f"{cartesian_coords}, shape: {cartesian_coords.shape}") - self.star_traj = Trajectory( - np.hstack((cartesian_coords, np.zeros((cartesian_coords.shape[0], 1)))) - ) # current point gets set back to 0 - - path = Path() - poses = [] - path.header = Header() - path.header.frame_id = "map" - for coord in cartesian_coords: - pose_stamped = PoseStamped() - pose_stamped.header = Header() - pose_stamped.header.frame_id = "map" - point = Point(coord[0], coord[1], 0) - quat = Quaternion(0,0,0,1) - pose_stamped.pose = Pose(point, quat) - poses.append(pose_stamped) - path.poses = poses - self.path_pub.publish(path) - - self.time_last_updated = rospy.get_time() - + return self.traj.get_cur_pt() + def on_enter(self, context) -> None: self.context = context - self.costmap_lock = Lock() search_center = context.course.current_waypoint() if not self.is_recovering: self.traj = SearchTrajectory.spiral_traj( @@ -369,18 +82,64 @@ def on_enter(self, context) -> None: search_center.tag_id, True ) - self.origin = context.rover.get_pose().position[0:2] - print(f"ORIGIN: {self.origin}") + origin = context.rover.get_pose().position[0:2] + self.astar = AStar(origin, context) + print(f"ORIGIN: {origin}") self.prev_target = None self.star_traj = Trajectory(np.array([])) self.time_last_updated = rospy.get_time() - self.costmap_listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) self.path_pub = rospy.Publisher("path", Path, queue_size=10) def on_exit(self, context) -> None: - self.costmap_listener.unregister() + self.context.costmap_listener.unregister() def on_loop(self, context) -> State: + # only update our costmap every 1 second + if rospy.get_time() - self.time_last_updated > 1: + cur_rover_pose = self.context.rover.get_pose().position[0:2] + end_point = self.find_endpoint(self.traj.get_cur_pt()[0:2]) + + # call A-STAR + print("RUN ASTAR") + try: + occupancy_list = self.astar.a_star(cur_rover_pose, end_point[0:2]) + except SpiralEnd as spiral_error: + # TODO: what to do in this case + self.traj.reset() + occupancy_list = None + except NoPath as path_error: + # increment end point + if self.traj.increment_point(): + # TODO: what to do in this case + self.traj.reset() + occupancy_list = None + if occupancy_list is None: + self.star_traj = Trajectory(np.array([])) + else: + cartesian_coords = self.astar.ij_to_cartesian(np.array(occupancy_list)) + print(f"{cartesian_coords}, shape: {cartesian_coords.shape}") + self.star_traj = Trajectory( + np.hstack((cartesian_coords, np.zeros((cartesian_coords.shape[0], 1)))) + ) # current point gets set back to 0 + + # create path type to publish planned path segments to see in rviz + path = Path() + poses = [] + path.header = Header() + path.header.frame_id = "map" + for coord in cartesian_coords: + pose_stamped = PoseStamped() + pose_stamped.header = Header() + pose_stamped.header.frame_id = "map" + point = Point(coord[0], coord[1], 0) + quat = Quaternion(0,0,0,1) + pose_stamped.pose = Pose(point, quat) + poses.append(pose_stamped) + path.poses = poses + self.path_pub.publish(path) + + self.time_last_updated = rospy.get_time() + # continue executing the path from wherever it left off target_pos = self.traj.get_cur_pt() traj_target = True From 40d8b3e240a7116174d3d61bddb785e3666ea5ed Mon Sep 17 00:00:00 2001 From: Prabhleen Pawar Date: Thu, 28 Mar 2024 19:28:17 -0400 Subject: [PATCH 71/76] Added ground1 test One mountain test --- other.f | 0 urdf/meshes/ground1.fbx | 3 +++ urdf/staging/ground1.blend | 3 +++ 3 files changed, 6 insertions(+) create mode 100644 other.f create mode 100644 urdf/meshes/ground1.fbx create mode 100644 urdf/staging/ground1.blend diff --git a/other.f b/other.f new file mode 100644 index 000000000..e69de29bb diff --git a/urdf/meshes/ground1.fbx b/urdf/meshes/ground1.fbx new file mode 100644 index 000000000..974af7809 --- /dev/null +++ b/urdf/meshes/ground1.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fd516cce23b7a3d54276cfc32f67bdfae3403503aba14ab34c26f4f3018049e1 +size 42716 diff --git a/urdf/staging/ground1.blend b/urdf/staging/ground1.blend new file mode 100644 index 000000000..d511af33c --- /dev/null +++ b/urdf/staging/ground1.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2a5fb55c5430c92fe6e19df5cc28eb95fea74533f88893cb63e9f3e2dfefded7 +size 1004132 From ca76f897a6cd8e4eb5fecc3c965c7ca4445af6c1 Mon Sep 17 00:00:00 2001 From: ARLEEN CHEEMA Date: Thu, 28 Mar 2024 19:41:05 -0400 Subject: [PATCH 72/76] Adding ground2.fbx and ground2.blend for testing and there's some testing of hills and valleys. --- urdf/meshes/ground2.fbx | 3 +++ urdf/staging/ground2.blend | 3 +++ 2 files changed, 6 insertions(+) create mode 100644 urdf/meshes/ground2.fbx create mode 100644 urdf/staging/ground2.blend diff --git a/urdf/meshes/ground2.fbx b/urdf/meshes/ground2.fbx new file mode 100644 index 000000000..a31d1d1ce --- /dev/null +++ b/urdf/meshes/ground2.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9eb0d8e3c5e9c991dbe2cbc4cb9466ea6856f05aad38cc72f954fe2803202cc6 +size 43356 diff --git a/urdf/staging/ground2.blend b/urdf/staging/ground2.blend new file mode 100644 index 000000000..62484d1b6 --- /dev/null +++ b/urdf/staging/ground2.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:16bc19bebe0a6930db803f45cc1a45cd6d1dda0a3e353085a438712d3e4897c4 +size 999012 From 3effc25ba193b0f5e1257fa7e1075069e3dfc70c Mon Sep 17 00:00:00 2001 From: kayleyge Date: Sun, 31 Mar 2024 13:22:47 -0400 Subject: [PATCH 73/76] added the arena for the 70th hunger games as a map --- urdf/meshes/groundka.fbx | 3 +++ urdf/staging/groundka.blend | 3 +++ 2 files changed, 6 insertions(+) create mode 100644 urdf/meshes/groundka.fbx create mode 100644 urdf/staging/groundka.blend diff --git a/urdf/meshes/groundka.fbx b/urdf/meshes/groundka.fbx new file mode 100644 index 000000000..ae7048eb3 --- /dev/null +++ b/urdf/meshes/groundka.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4f9c96351ab4c6198aee86fa5387078f9e24b2829581a8a14dc41e889b04a467 +size 43468 diff --git a/urdf/staging/groundka.blend b/urdf/staging/groundka.blend new file mode 100644 index 000000000..f324660d2 --- /dev/null +++ b/urdf/staging/groundka.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8df22a59b1326ec66b3f3dd8f45d144368ec30f6b2ed5fef11abcc1577eeff8e +size 999012 From f7c85ea1de271ced3c5015b58301abd8fd4bc42f Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Thu, 4 Apr 2024 01:01:51 -0400 Subject: [PATCH 74/76] update grounds --- other.f | 0 urdf/meshes/ground1.fbx | 3 --- urdf/meshes/ground2.fbx | 3 --- urdf/meshes/ground4.fbx | 3 +++ urdf/meshes/ground5.fbx | 3 +++ urdf/meshes/groundkay.fbx | 3 +++ urdf/staging/ground1.blend | 3 --- urdf/staging/ground2.blend | 3 --- urdf/staging/ground4.blend | 3 +++ urdf/staging/ground5.blend | 3 +++ urdf/staging/groundkay.blend | 3 +++ 11 files changed, 18 insertions(+), 12 deletions(-) delete mode 100644 other.f delete mode 100644 urdf/meshes/ground1.fbx delete mode 100644 urdf/meshes/ground2.fbx create mode 100644 urdf/meshes/ground4.fbx create mode 100644 urdf/meshes/ground5.fbx create mode 100644 urdf/meshes/groundkay.fbx delete mode 100644 urdf/staging/ground1.blend delete mode 100644 urdf/staging/ground2.blend create mode 100644 urdf/staging/ground4.blend create mode 100644 urdf/staging/ground5.blend create mode 100644 urdf/staging/groundkay.blend diff --git a/other.f b/other.f deleted file mode 100644 index e69de29bb..000000000 diff --git a/urdf/meshes/ground1.fbx b/urdf/meshes/ground1.fbx deleted file mode 100644 index 974af7809..000000000 --- a/urdf/meshes/ground1.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:fd516cce23b7a3d54276cfc32f67bdfae3403503aba14ab34c26f4f3018049e1 -size 42716 diff --git a/urdf/meshes/ground2.fbx b/urdf/meshes/ground2.fbx deleted file mode 100644 index a31d1d1ce..000000000 --- a/urdf/meshes/ground2.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9eb0d8e3c5e9c991dbe2cbc4cb9466ea6856f05aad38cc72f954fe2803202cc6 -size 43356 diff --git a/urdf/meshes/ground4.fbx b/urdf/meshes/ground4.fbx new file mode 100644 index 000000000..536474571 --- /dev/null +++ b/urdf/meshes/ground4.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:aa35ccd62d8ed61e394b19c204f09bb708d9fbe41877a2069a9550034bc54bc6 +size 36396 diff --git a/urdf/meshes/ground5.fbx b/urdf/meshes/ground5.fbx new file mode 100644 index 000000000..ba78f106c --- /dev/null +++ b/urdf/meshes/ground5.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:48d228d1446e1daff8c87564fb6db10961cb629866f98f31855283460ff7af2f +size 39180 diff --git a/urdf/meshes/groundkay.fbx b/urdf/meshes/groundkay.fbx new file mode 100644 index 000000000..ebe4d5e8c --- /dev/null +++ b/urdf/meshes/groundkay.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8838030f2b4c57a71b66c9ffcc29487d4bb8fee2f546decb61dd4100621dac1a +size 40380 diff --git a/urdf/staging/ground1.blend b/urdf/staging/ground1.blend deleted file mode 100644 index d511af33c..000000000 --- a/urdf/staging/ground1.blend +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2a5fb55c5430c92fe6e19df5cc28eb95fea74533f88893cb63e9f3e2dfefded7 -size 1004132 diff --git a/urdf/staging/ground2.blend b/urdf/staging/ground2.blend deleted file mode 100644 index 62484d1b6..000000000 --- a/urdf/staging/ground2.blend +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:16bc19bebe0a6930db803f45cc1a45cd6d1dda0a3e353085a438712d3e4897c4 -size 999012 diff --git a/urdf/staging/ground4.blend b/urdf/staging/ground4.blend new file mode 100644 index 000000000..1e812cf51 --- /dev/null +++ b/urdf/staging/ground4.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ab0ee4bf7fef788a2b7773fec19bb1c561bc79592b19bd5b7b21a2f8f9c984c8 +size 1007984 diff --git a/urdf/staging/ground5.blend b/urdf/staging/ground5.blend new file mode 100644 index 000000000..a398c12b1 --- /dev/null +++ b/urdf/staging/ground5.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4a50e943e8573b690a57d3ba0894b31c8f050b1465d909f05da19241bbff49c2 +size 999228 diff --git a/urdf/staging/groundkay.blend b/urdf/staging/groundkay.blend new file mode 100644 index 000000000..f07b5a190 --- /dev/null +++ b/urdf/staging/groundkay.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:56f0bcf481f686ba191e57787b5f0a43ccc5824d93f62558dbe0d44bcbb5d832 +size 993092 From 93fce06ed7a4dbce402f5cf7ddf38b962c92aa01 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Thu, 11 Apr 2024 19:18:25 -0400 Subject: [PATCH 75/76] worked on moving costmap --- src/navigation/waypoint.py | 13 ++++++++++++- src/perception/cost_map/cost_map.cpp | 9 +++++++++ src/perception/cost_map/cost_map.hpp | 3 +++ src/perception/cost_map/cost_map.processing.cpp | 2 +- src/perception/cost_map/pch.hpp | 1 + 5 files changed, 26 insertions(+), 2 deletions(-) diff --git a/src/navigation/waypoint.py b/src/navigation/waypoint.py index 6ccdbda2e..26515542e 100644 --- a/src/navigation/waypoint.py +++ b/src/navigation/waypoint.py @@ -1,10 +1,11 @@ import tf2_ros - +import rospy from util.ros_utils import get_rosparam from util.state_lib.state import State from navigation import search, recovery, approach_post, post_backup, state, approach_object, long_range, water_bottle_search from mrover.msg import WaypointType +from mrover.srv import MoveCostMap class WaypointState(State): @@ -13,6 +14,16 @@ class WaypointState(State): NO_FIDUCIAL = get_rosparam("waypoint/no_fiducial", -1) def on_enter(self, context) -> None: + #TODO: In context find the access for type of waypoint + + current_waypoint = context.course.current_waypoint() + if current_waypoint.type.val == WaypointType.WATER_BOTTLE: + rospy.wait_for_service("move_cost_map") + move_cost_map = rospy.ServiceProxy("move_cost_map", MoveCostMap) + try: + resp = move_cost_map(f"course{context.course.waypoint_index}") + except rospy.ServiceException as exc: + rospy.logerr(f"Service call failed: {exc}") pass def on_exit(self, context) -> None: diff --git a/src/perception/cost_map/cost_map.cpp b/src/perception/cost_map/cost_map.cpp index 987a30df5..81f39f1b1 100644 --- a/src/perception/cost_map/cost_map.cpp +++ b/src/perception/cost_map/cost_map.cpp @@ -11,6 +11,8 @@ namespace mrover { mCostMapPub = mCmt.advertise("costmap", 1); // We publish our results to "costmap" + mServer = mNh.advertiseService("move_cost_map", &CostMapNodelet::moveCostMapCallback, this); + mPcSub = mNh.subscribe("camera/left/points", 1, &CostMapNodelet::pointCloudCallback, this); mGlobalGridMsg.info.resolution = mResolution; @@ -23,4 +25,11 @@ namespace mrover { mGlobalGridMsg.data.resize(mGlobalGridMsg.info.width * mGlobalGridMsg.info.height, UNKNOWN_COST); } + auto CostMapNodelet::moveCostMapCallback(MoveCostMap::Request& req, MoveCostMap::Response& res) -> bool { + SE3d waypointPos = SE3Conversions::fromTfTree(mTfBuffer, req.course, "map"); + mGlobalGridMsg.info.origin.position.x = waypointPos.x() - 1 * mDimension / 2; + mGlobalGridMsg.info.origin.position.y = waypointPos.y() - 1 * mDimension / 2; + res.success = true; + return true; + } } // namespace mrover \ No newline at end of file diff --git a/src/perception/cost_map/cost_map.hpp b/src/perception/cost_map/cost_map.hpp index 3fe1adc77..c2b67486c 100644 --- a/src/perception/cost_map/cost_map.hpp +++ b/src/perception/cost_map/cost_map.hpp @@ -29,12 +29,15 @@ namespace mrover { void onInit() override; + ros::ServiceServer mServer; + public: CostMapNodelet() = default; ~CostMapNodelet() override = default; auto pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void; + auto moveCostMapCallback(MoveCostMap::Request& req, MoveCostMap::Response& res) -> bool; }; } // namespace mrover diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp index 61c433463..7f411dfb8 100644 --- a/src/perception/cost_map/cost_map.processing.cpp +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -37,7 +37,7 @@ namespace mrover { int yIndex = std::floor((yInMap - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); auto costMapIndex = mGlobalGridMsg.info.width * yIndex + xIndex; - // Z is the vertical component of of hte normal + // Z is the vertical component of the normal // A small Z component indicates largely horizontal normal (surface is vertical) std::int8_t cost = normalInMap.z() < mNormalThreshold ? OCCUPIED_COST : FREE_COST; diff --git a/src/perception/cost_map/pch.hpp b/src/perception/cost_map/pch.hpp index b76cbe13a..63ff6fe5f 100644 --- a/src/perception/cost_map/pch.hpp +++ b/src/perception/cost_map/pch.hpp @@ -18,3 +18,4 @@ #include #include +#include From 87bbb85424a6edd01b46fca8a7036bb0938f1326 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Thu, 11 Apr 2024 19:21:18 -0400 Subject: [PATCH 76/76] forgot to add something --- srv/MoveCostMap.srv | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 srv/MoveCostMap.srv diff --git a/srv/MoveCostMap.srv b/srv/MoveCostMap.srv new file mode 100644 index 000000000..f4b1b49d8 --- /dev/null +++ b/srv/MoveCostMap.srv @@ -0,0 +1,3 @@ +string course +--- +bool success \ No newline at end of file