From 837119747e45433f945be18004535831ba765b93 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Tue, 16 Jan 2024 19:25:26 -0500 Subject: [PATCH 01/89] Start lander align --- CMakeLists.txt | 4 ++++ src/teleoperation/lander_align/lander_align.cpp | 3 +++ src/teleoperation/lander_align/lander_align.hpp | 1 + src/teleoperation/lander_align/pch.hpp | 2 ++ 4 files changed, 10 insertions(+) create mode 100644 src/teleoperation/lander_align/lander_align.cpp create mode 100644 src/teleoperation/lander_align/lander_align.hpp create mode 100644 src/teleoperation/lander_align/pch.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 98c9465f3..ec1ce5bc8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -220,6 +220,10 @@ if (ZED_FOUND) ) endif () +## Teleoperation + +mrover_add_nodelet(lander_align src/teleoperation/lander_align/*.cpp src/teleoperation/lander_align src/teleoperation/lander_align/pch.hpp) + ## Simulator mrover_add_gazebo_plugin(differential_drive_plugin_6w src/simulator/differential_drive_6w.cpp src) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp new file mode 100644 index 000000000..704dacc68 --- /dev/null +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -0,0 +1,3 @@ +int main() { + +} \ No newline at end of file diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp new file mode 100644 index 000000000..7f3196a70 --- /dev/null +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -0,0 +1 @@ +#include "pch.hpp" \ No newline at end of file diff --git a/src/teleoperation/lander_align/pch.hpp b/src/teleoperation/lander_align/pch.hpp new file mode 100644 index 000000000..3f59c932d --- /dev/null +++ b/src/teleoperation/lander_align/pch.hpp @@ -0,0 +1,2 @@ +#pragma once + From a1e9bc1f63318bdb53a52a6c1b3e509725ef4c54 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Wed, 17 Jan 2024 15:21:56 -0500 Subject: [PATCH 02/89] Setup nodelet arch --- package.xml | 1 + plugins/lander_align_plugin.xml | 6 +++++ .../lander_align/lander_align.cpp | 27 +++++++++++++++++-- .../lander_align/lander_align.hpp | 14 +++++++++- src/teleoperation/lander_align/pch.hpp | 10 +++++++ 5 files changed, 55 insertions(+), 3 deletions(-) create mode 100644 plugins/lander_align_plugin.xml diff --git a/package.xml b/package.xml index 87ca4865b..e6b735059 100644 --- a/package.xml +++ b/package.xml @@ -87,6 +87,7 @@ + diff --git a/plugins/lander_align_plugin.xml b/plugins/lander_align_plugin.xml new file mode 100644 index 000000000..ead6fac2b --- /dev/null +++ b/plugins/lander_align_plugin.xml @@ -0,0 +1,6 @@ + + + + diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 704dacc68..55d22a49a 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,3 +1,26 @@ -int main() { +#include "lander_align.hpp" -} \ No newline at end of file +namespace mrover { + + void LanderAlignNodelet::onInit() { + mNh = getMTNodeHandle(); + mPnh = getMTPrivateNodeHandle(); + } + +} // namespace mrover + +int main(int argc, char** argv) { + ros::init(argc, argv, "lander_align"); + + nodelet::Loader nodelet; + nodelet.load(ros::this_node::getName(), "mrover/LanderAlignNodelet", ros::names::getRemappings(), {}); + + ros::spin(); + + return EXIT_SUCCESS; +} + +#ifdef MROVER_IS_NODELET +#include +PLUGINLIB_EXPORT_CLASS(mrover::LanderAlignNodelet, nodelet::Nodelet) +#endif diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 7f3196a70..6d60971c7 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -1 +1,13 @@ -#include "pch.hpp" \ No newline at end of file +#pragma once + +#include "pch.hpp" + +namespace mrover { + + class LanderAlignNodelet : public nodelet::Nodelet { + ros::NodeHandle mNh, mPnh; + + void onInit() override; + }; + +} // namespace mrover diff --git a/src/teleoperation/lander_align/pch.hpp b/src/teleoperation/lander_align/pch.hpp index 3f59c932d..3a43be200 100644 --- a/src/teleoperation/lander_align/pch.hpp +++ b/src/teleoperation/lander_align/pch.hpp @@ -1,2 +1,12 @@ #pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include From a6ded2d89246fa559bff5e56a83f91bb7fa56c71 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Wed, 17 Jan 2024 15:26:42 -0500 Subject: [PATCH 03/89] Add more boilerplate --- src/teleoperation/lander_align/lander_align.cpp | 17 +++++++++++++++-- src/teleoperation/lander_align/lander_align.hpp | 8 +++++++- src/teleoperation/lander_align/pch.hpp | 4 ++++ 3 files changed, 26 insertions(+), 3 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 55d22a49a..21b71171c 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -2,14 +2,27 @@ namespace mrover { - void LanderAlignNodelet::onInit() { + auto LanderAlignNodelet::onInit() -> void { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); } + auto LanderAlignNodelet::downsample(sensor_msgs::PointCloud2Ptr const& cloud) -> sensor_msgs::PointCloud2Ptr { + // TODO: make a new point cloud with half the resolution, check the zed wrapper for how to make one + } + + auto LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud) -> void { + // TODO: modify "cloud" in-place, this should be input from the downsample function + } + + auto LanderAlignNodelet::ransac(sensor_msgs::PointCloud2Ptr const& cloud) -> Eigen::Vector3d { + // TODO: use RANSAC to find the lander face, should be the closest, we may need to modify this to output more information, currently the output is the normal + } + + } // namespace mrover -int main(int argc, char** argv) { +auto main(int argc, char** argv) -> int { ros::init(argc, argv, "lander_align"); nodelet::Loader nodelet; diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 6d60971c7..20ff7e104 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -7,7 +7,13 @@ namespace mrover { class LanderAlignNodelet : public nodelet::Nodelet { ros::NodeHandle mNh, mPnh; - void onInit() override; + auto onInit() -> void override; + + auto downsample(sensor_msgs::PointCloud2Ptr const& cloud) -> sensor_msgs::PointCloud2Ptr; + + auto filterNormals(sensor_msgs::PointCloud2Ptr const& cloud) -> void; + + auto ransac(sensor_msgs::PointCloud2Ptr const& cloud) -> Eigen::Vector3d; }; } // namespace mrover diff --git a/src/teleoperation/lander_align/pch.hpp b/src/teleoperation/lander_align/pch.hpp index 3a43be200..0e4f62105 100644 --- a/src/teleoperation/lander_align/pch.hpp +++ b/src/teleoperation/lander_align/pch.hpp @@ -10,3 +10,7 @@ #include #include #include + +#include + +#include "../../perception/point.hpp" From 016a24d10390d4ca99b401bc9d5e48c08b82b181 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 18 Jan 2024 18:36:09 -0500 Subject: [PATCH 04/89] Make compile --- src/teleoperation/lander_align/lander_align.cpp | 17 ----------------- src/teleoperation/lander_align/main.cpp | 17 +++++++++++++++++ src/teleoperation/lander_align/pch.hpp | 2 +- 3 files changed, 18 insertions(+), 18 deletions(-) create mode 100644 src/teleoperation/lander_align/main.cpp diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 21b71171c..2b743f082 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -19,21 +19,4 @@ namespace mrover { // TODO: use RANSAC to find the lander face, should be the closest, we may need to modify this to output more information, currently the output is the normal } - } // namespace mrover - -auto main(int argc, char** argv) -> int { - ros::init(argc, argv, "lander_align"); - - nodelet::Loader nodelet; - nodelet.load(ros::this_node::getName(), "mrover/LanderAlignNodelet", ros::names::getRemappings(), {}); - - ros::spin(); - - return EXIT_SUCCESS; -} - -#ifdef MROVER_IS_NODELET -#include -PLUGINLIB_EXPORT_CLASS(mrover::LanderAlignNodelet, nodelet::Nodelet) -#endif diff --git a/src/teleoperation/lander_align/main.cpp b/src/teleoperation/lander_align/main.cpp new file mode 100644 index 000000000..9da01ba56 --- /dev/null +++ b/src/teleoperation/lander_align/main.cpp @@ -0,0 +1,17 @@ +#include "lander_align.hpp" + +auto main(int argc, char** argv) -> int { + ros::init(argc, argv, "lander_align"); + + nodelet::Loader nodelet; + nodelet.load(ros::this_node::getName(), "mrover/LanderAlignNodelet", ros::names::getRemappings(), {}); + + ros::spin(); + + return EXIT_SUCCESS; +} + +#ifdef MROVER_IS_NODELET +#include +PLUGINLIB_EXPORT_CLASS(mrover::LanderAlignNodelet, nodelet::Nodelet) +#endif diff --git a/src/teleoperation/lander_align/pch.hpp b/src/teleoperation/lander_align/pch.hpp index 0e4f62105..ae1a3a507 100644 --- a/src/teleoperation/lander_align/pch.hpp +++ b/src/teleoperation/lander_align/pch.hpp @@ -13,4 +13,4 @@ #include -#include "../../perception/point.hpp" +#include "point.hpp" From 54a0b7040e959fdcd38ee99780d2690187e93285 Mon Sep 17 00:00:00 2001 From: Joshua Huang Date: Thu, 25 Jan 2024 18:27:40 -0500 Subject: [PATCH 05/89] Redefine filtering/ransac problem --- .../lander_align/lander_align.cpp | 23 +++++++++++++++---- .../lander_align/lander_align.hpp | 7 +++--- 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 2b743f082..b7ed383d2 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,4 +1,6 @@ #include "lander_align.hpp" +#include +#include namespace mrover { @@ -7,16 +9,29 @@ namespace mrover { mPnh = getMTPrivateNodeHandle(); } - auto LanderAlignNodelet::downsample(sensor_msgs::PointCloud2Ptr const& cloud) -> sensor_msgs::PointCloud2Ptr { + // deprecated/not needed anymore + // auto LanderAlignNodelet::downsample(sensor_msgs::PointCloud2Ptr const& cloud) -> sensor_msgs::PointCloud2Ptr { // TODO: make a new point cloud with half the resolution, check the zed wrapper for how to make one - } + // sensor_msgs::PointCloud2 cloudSample; + // cloudSample.header = cloud->header + // cloudSample.height = cloud->height / 2; + // cloudSample.width = cloud->width / 2; + // cloudSample.fields + // } + + auto LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud, const int threshold) -> std::vector { + // TODO: return a vector of Point pointers that correspond to all points in the point cloud that are not the ground + // filter based on whether z-normal valus exceed specified threshold - auto LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud) -> void { - // TODO: modify "cloud" in-place, this should be input from the downsample function + return {}; } auto LanderAlignNodelet::ransac(sensor_msgs::PointCloud2Ptr const& cloud) -> Eigen::Vector3d { // TODO: use RANSAC to find the lander face, should be the closest, we may need to modify this to output more information, currently the output is the normal + auto* cloudData = reinterpret_cast(cloud->fields.data()); + + std::default_random_engine generator; + std::uniform_int_distribution distribution (0, ) } } // namespace mrover diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 20ff7e104..8a146cba0 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -9,11 +9,12 @@ namespace mrover { auto onInit() -> void override; - auto downsample(sensor_msgs::PointCloud2Ptr const& cloud) -> sensor_msgs::PointCloud2Ptr; + // deprecated/not needed anymore + // auto downsample(sensor_msgs::PointCloud2Ptr const& cloud) -> sensor_msgs::PointCloud2Ptr; - auto filterNormals(sensor_msgs::PointCloud2Ptr const& cloud) -> void; + auto filterNormals(sensor_msgs::PointCloud2Ptr const& cloud, const int threshold) -> std::vector; - auto ransac(sensor_msgs::PointCloud2Ptr const& cloud) -> Eigen::Vector3d; + auto ransac(const std::vector& filteredPoints) -> Eigen::Vector3d; }; } // namespace mrover From 4ca2905737b622a07277d251eb58ea6b77a763ae Mon Sep 17 00:00:00 2001 From: Joshua Huang Date: Thu, 25 Jan 2024 20:01:24 -0500 Subject: [PATCH 06/89] Implement RANSAC plane fitting vec math --- .../lander_align/lander_align.cpp | 28 +++++++++++++++++-- .../lander_align/lander_align.hpp | 2 +- 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index b7ed383d2..2002825f2 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,4 +1,5 @@ #include "lander_align.hpp" +#include #include #include @@ -22,16 +23,37 @@ namespace mrover { auto LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud, const int threshold) -> std::vector { // TODO: return a vector of Point pointers that correspond to all points in the point cloud that are not the ground // filter based on whether z-normal valus exceed specified threshold + auto* cloudData = reinterpret_cast(cloud->fields.data()); return {}; } - auto LanderAlignNodelet::ransac(sensor_msgs::PointCloud2Ptr const& cloud) -> Eigen::Vector3d { + auto LanderAlignNodelet::ransac(const std::vector& points, const double distanceThreshold, const int epochs) -> Eigen::Vector3f { // TODO: use RANSAC to find the lander face, should be the closest, we may need to modify this to output more information, currently the output is the normal - auto* cloudData = reinterpret_cast(cloud->fields.data()); + // takes 3 samples for every epoch and terminates after specified number of epochs + Eigen::Vector3f bestPlane; // normal vector representing plane + // define randomizer std::default_random_engine generator; - std::uniform_int_distribution distribution (0, ) + std::uniform_int_distribution distribution (0, (int) points.size() - 1); + + for (int i = 0; i < epochs; ++i) { + // sample 3 random points (potential inliers) + Point* point1 = points[distribution(generator)]; + Point* point2 = points[distribution(generator)]; + Point* point3 = points[distribution(generator)]; + + Eigen::Vector3f vec1{point1->x, point1->y, point1->z}; + Eigen::Vector3f vec2{point2->x, point2->y, point2->z}; + Eigen::Vector3f vec3{point3->x, point3->y, point3->z}; + + // fit a plane to these points TODO change this to vec substract then cross + Eigen::Vector3f u{point1->x - point2->x, point1->y - point2->y, point1->z - point2->z}; + Eigen::Vector3f v{point1->x - point3->x, point1->y - point3->y, point1->z - point3->z}; + // Eigen::Vector3f plane = u.subTo(Dest &dst) + + } + } } // namespace mrover diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 8a146cba0..64b879109 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -14,7 +14,7 @@ namespace mrover { auto filterNormals(sensor_msgs::PointCloud2Ptr const& cloud, const int threshold) -> std::vector; - auto ransac(const std::vector& filteredPoints) -> Eigen::Vector3d; + auto ransac(const std::vector& filteredPoints) -> Eigen::Vector3f; }; } // namespace mrover From 6fbcf43713afd33b2cd5e723470a2189ddc97897 Mon Sep 17 00:00:00 2001 From: Khush Date: Thu, 1 Feb 2024 19:31:00 -0500 Subject: [PATCH 07/89] Remove ground normals --- .../lander_align/lander_align.cpp | 25 ++++++++++++------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 2002825f2..320cf68f5 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -12,30 +12,39 @@ namespace mrover { // deprecated/not needed anymore // auto LanderAlignNodelet::downsample(sensor_msgs::PointCloud2Ptr const& cloud) -> sensor_msgs::PointCloud2Ptr { - // TODO: make a new point cloud with half the resolution, check the zed wrapper for how to make one - // sensor_msgs::PointCloud2 cloudSample; - // cloudSample.header = cloud->header + // TODO: make a new point cloud with half the resolution, check the zed wrapper for how to make one + // sensor_msgs::PointCloud2 cloudSample; + // cloudSample.header = cloud->header // cloudSample.height = cloud->height / 2; // cloudSample.width = cloud->width / 2; // cloudSample.fields // } - auto LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud, const int threshold) -> std::vector { + auto LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud, int const threshold) -> std::vector { // TODO: return a vector of Point pointers that correspond to all points in the point cloud that are not the ground // filter based on whether z-normal valus exceed specified threshold + auto* cloudData = reinterpret_cast(cloud->fields.data()); + std::vector extractedPoints; + + for (auto point = cloudData; point < cloudData + (cloud->height * cloud->width); point++) { + if (point->normal_z < threshold) { + extractedPoints.push_back(point); + } + } - return {}; + + return extractedPoints; } - auto LanderAlignNodelet::ransac(const std::vector& points, const double distanceThreshold, const int epochs) -> Eigen::Vector3f { + auto LanderAlignNodelet::ransac(std::vector const& points, double const distanceThreshold, int const epochs) -> Eigen::Vector3f { // TODO: use RANSAC to find the lander face, should be the closest, we may need to modify this to output more information, currently the output is the normal // takes 3 samples for every epoch and terminates after specified number of epochs Eigen::Vector3f bestPlane; // normal vector representing plane // define randomizer std::default_random_engine generator; - std::uniform_int_distribution distribution (0, (int) points.size() - 1); + std::uniform_int_distribution distribution(0, (int) points.size() - 1); for (int i = 0; i < epochs; ++i) { // sample 3 random points (potential inliers) @@ -51,9 +60,7 @@ namespace mrover { Eigen::Vector3f u{point1->x - point2->x, point1->y - point2->y, point1->z - point2->z}; Eigen::Vector3f v{point1->x - point3->x, point1->y - point3->y, point1->z - point3->z}; // Eigen::Vector3f plane = u.subTo(Dest &dst) - } - } } // namespace mrover From ff6bc8adf0a5881635379c1de6cf158735365f21 Mon Sep 17 00:00:00 2001 From: Khush Date: Thu, 1 Feb 2024 19:51:13 -0500 Subject: [PATCH 08/89] Added TODO --- src/teleoperation/lander_align/lander_align.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 320cf68f5..9929263c6 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -21,8 +21,7 @@ namespace mrover { // } auto LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud, int const threshold) -> std::vector { - // TODO: return a vector of Point pointers that correspond to all points in the point cloud that are not the ground - // filter based on whether z-normal valus exceed specified threshold + // TODO: OPTIMIZE; doing this many push_back calls could slow things down auto* cloudData = reinterpret_cast(cloud->fields.data()); std::vector extractedPoints; From a4705894cd4235692e8f115e1cd949c3c664326e Mon Sep 17 00:00:00 2001 From: Joshua Huang Date: Thu, 1 Feb 2024 20:16:58 -0500 Subject: [PATCH 09/89] Implement RANSAC with inlier count as metric --- .../lander_align/lander_align.cpp | 31 +++++++++++++++---- 1 file changed, 25 insertions(+), 6 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 9929263c6..87f1c4314 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -36,10 +36,10 @@ namespace mrover { return extractedPoints; } - auto LanderAlignNodelet::ransac(std::vector const& points, double const distanceThreshold, int const epochs) -> Eigen::Vector3f { + auto LanderAlignNodelet::ransac(const std::vector& points, const float distanceThreshold, int minInliers, const int epochs) -> Eigen::Vector3f { // TODO: use RANSAC to find the lander face, should be the closest, we may need to modify this to output more information, currently the output is the normal // takes 3 samples for every epoch and terminates after specified number of epochs - Eigen::Vector3f bestPlane; // normal vector representing plane + Eigen::Vector3f bestPlane; // normal vector representing plane (initialize as zero vector?? default ctor??) // define randomizer std::default_random_engine generator; @@ -55,11 +55,30 @@ namespace mrover { Eigen::Vector3f vec2{point2->x, point2->y, point2->z}; Eigen::Vector3f vec3{point3->x, point3->y, point3->z}; - // fit a plane to these points TODO change this to vec substract then cross - Eigen::Vector3f u{point1->x - point2->x, point1->y - point2->y, point1->z - point2->z}; - Eigen::Vector3f v{point1->x - point3->x, point1->y - point3->y, point1->z - point3->z}; - // Eigen::Vector3f plane = u.subTo(Dest &dst) + // fit a plane to these points + Eigen::Vector3f normal = (vec1 - vec2).cross(vec1 - vec3).normalized(); + float offset = -normal.dot(vec1); // calculate offset (D value) using one of the points + + int numInliers = 0; + + for (auto p : points) { + // calculate distance of each point from potential plane + float distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); + + if (distance < distanceThreshold) { + ++numInliers; // count num of inliers that pass the "good enough fit" threshold + } + } + + // update best plane if better inlier count + if (numInliers > minInliers) { + minInliers = numInliers; + bestPlane = normal; + } } + + return bestPlane; // TODO address zero vector null case or bug + } } // namespace mrover From 275c4c16b6223a7da4f34d3afc4455f70495be60 Mon Sep 17 00:00:00 2001 From: Joshua Huang Date: Sat, 3 Feb 2024 13:30:03 -0500 Subject: [PATCH 10/89] Add retry zero-vector case --- .../lander_align/lander_align.cpp | 53 ++++++++++--------- 1 file changed, 27 insertions(+), 26 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 87f1c4314..61903160a 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -39,46 +39,47 @@ namespace mrover { auto LanderAlignNodelet::ransac(const std::vector& points, const float distanceThreshold, int minInliers, const int epochs) -> Eigen::Vector3f { // TODO: use RANSAC to find the lander face, should be the closest, we may need to modify this to output more information, currently the output is the normal // takes 3 samples for every epoch and terminates after specified number of epochs - Eigen::Vector3f bestPlane; // normal vector representing plane (initialize as zero vector?? default ctor??) + Eigen::Vector3f bestPlane(0, 0, 0); // normal vector representing plane (initialize as zero vector?? default ctor??) // define randomizer std::default_random_engine generator; std::uniform_int_distribution distribution(0, (int) points.size() - 1); - for (int i = 0; i < epochs; ++i) { - // sample 3 random points (potential inliers) - Point* point1 = points[distribution(generator)]; - Point* point2 = points[distribution(generator)]; - Point* point3 = points[distribution(generator)]; + do { + for (int i = 0; i < epochs; ++i) { + // sample 3 random points (potential inliers) + Point* point1 = points[distribution(generator)]; + Point* point2 = points[distribution(generator)]; + Point* point3 = points[distribution(generator)]; - Eigen::Vector3f vec1{point1->x, point1->y, point1->z}; - Eigen::Vector3f vec2{point2->x, point2->y, point2->z}; - Eigen::Vector3f vec3{point3->x, point3->y, point3->z}; + Eigen::Vector3f vec1{point1->x, point1->y, point1->z}; + Eigen::Vector3f vec2{point2->x, point2->y, point2->z}; + Eigen::Vector3f vec3{point3->x, point3->y, point3->z}; - // fit a plane to these points - Eigen::Vector3f normal = (vec1 - vec2).cross(vec1 - vec3).normalized(); - float offset = -normal.dot(vec1); // calculate offset (D value) using one of the points + // fit a plane to these points + Eigen::Vector3f normal = (vec1 - vec2).cross(vec1 - vec3).normalized(); + float offset = -normal.dot(vec1); // calculate offset (D value) using one of the points - int numInliers = 0; + int numInliers = 0; - for (auto p : points) { - // calculate distance of each point from potential plane - float distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); + for (auto p : points) { + // calculate distance of each point from potential plane + float distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); - if (distance < distanceThreshold) { - ++numInliers; // count num of inliers that pass the "good enough fit" threshold + if (distance < distanceThreshold) { + ++numInliers; // count num of inliers that pass the "good enough fit" threshold + } } - } - // update best plane if better inlier count - if (numInliers > minInliers) { - minInliers = numInliers; - bestPlane = normal; + // update best plane if better inlier count + if (numInliers > minInliers) { + minInliers = numInliers; + bestPlane = normal; + } } - } - - return bestPlane; // TODO address zero vector null case or bug + } while (bestPlane.isZero()); // keep trying until we get valid result TODO break case after X attempts?? + return bestPlane; } } // namespace mrover From d992ec7afd238986144a37c6e6c3ca0f2b1b8795 Mon Sep 17 00:00:00 2001 From: Joshua Huang Date: Sat, 3 Feb 2024 13:44:53 -0500 Subject: [PATCH 11/89] Fix some semantics --- src/teleoperation/lander_align/lander_align.cpp | 4 ++-- src/teleoperation/lander_align/lander_align.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 61903160a..73305c0b4 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -20,11 +20,11 @@ namespace mrover { // cloudSample.fields // } - auto LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud, int const threshold) -> std::vector { + auto LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud, const float threshold) -> std::vector { // TODO: OPTIMIZE; doing this many push_back calls could slow things down auto* cloudData = reinterpret_cast(cloud->fields.data()); - std::vector extractedPoints; + std::vector extractedPoints; for (auto point = cloudData; point < cloudData + (cloud->height * cloud->width); point++) { if (point->normal_z < threshold) { diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 64b879109..689fe2812 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -12,9 +12,9 @@ namespace mrover { // deprecated/not needed anymore // auto downsample(sensor_msgs::PointCloud2Ptr const& cloud) -> sensor_msgs::PointCloud2Ptr; - auto filterNormals(sensor_msgs::PointCloud2Ptr const& cloud, const int threshold) -> std::vector; + auto filterNormals(sensor_msgs::PointCloud2Ptr const& cloud, float threshold) -> std::vector; - auto ransac(const std::vector& filteredPoints) -> Eigen::Vector3f; + auto ransac(const std::vector& points, float distanceThreshold, int minInliers, int epochs) -> Eigen::Vector3f; }; } // namespace mrover From 8c8a0e366456d26ab6c11a0f8defb342be8edd1b Mon Sep 17 00:00:00 2001 From: Julian Skifstad Date: Sun, 4 Feb 2024 14:42:03 -0500 Subject: [PATCH 12/89] it builds :) --- setup.py | 1 - .../lander_align/lander_align.cpp | 38 +++++++++++++------ .../lander_align/lander_align.hpp | 15 +++++++- src/teleoperation/lander_align/pch.hpp | 6 +-- 4 files changed, 43 insertions(+), 17 deletions(-) diff --git a/setup.py b/setup.py index 5bb0b94e7..48ba751d1 100644 --- a/setup.py +++ b/setup.py @@ -12,7 +12,6 @@ "navigation", "navigation.failure_identification", "esw", - "teleop", "teleoperation.teleoperation", ], package_dir={"": "src"}, diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 73305c0b4..7809ebc84 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,6 +1,9 @@ #include "lander_align.hpp" +#include "pch.hpp" #include +#include #include +#include #include namespace mrover { @@ -8,6 +11,21 @@ namespace mrover { auto LanderAlignNodelet::onInit() -> void { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); + mThreshold = 10; + mVectorSub = mNh.subscribe("/camera/left/points", 1, &LanderAlignNodelet::filterNormals, this); + mDebugVectorPub = mNh.advertise("/lander_align/Pose", 1); + } + + void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { + filterNormals(cloud); + Eigen::Vector3f planeNorm = ransac(mFilteredPoints, 1, 10, 10); + + geometry_msgs::Vector3 vect; + vect.x = planeNorm.x(); + vect.y = planeNorm.y(); + vect.z = planeNorm.z(); + + mDebugVectorPub.publish(vect); } // deprecated/not needed anymore @@ -20,23 +38,20 @@ namespace mrover { // cloudSample.fields // } - auto LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud, const float threshold) -> std::vector { - // TODO: OPTIMIZE; doing this many push_back calls could slow things down + void LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud) { + // TODO: OPTIMIZE; doing this maobject_detector/debug_imgny push_back calls could slow things down auto* cloudData = reinterpret_cast(cloud->fields.data()); - std::vector extractedPoints; + // std::vector extractedPoints; for (auto point = cloudData; point < cloudData + (cloud->height * cloud->width); point++) { - if (point->normal_z < threshold) { - extractedPoints.push_back(point); + if (point->normal_z < mThreshold) { + mFilteredPoints.push_back(point); } } - - - return extractedPoints; } - auto LanderAlignNodelet::ransac(const std::vector& points, const float distanceThreshold, int minInliers, const int epochs) -> Eigen::Vector3f { + auto LanderAlignNodelet::ransac(std::vector const& points, float const distanceThreshold, int minInliers, int const epochs) -> Eigen::Vector3f { // TODO: use RANSAC to find the lander face, should be the closest, we may need to modify this to output more information, currently the output is the normal // takes 3 samples for every epoch and terminates after specified number of epochs Eigen::Vector3f bestPlane(0, 0, 0); // normal vector representing plane (initialize as zero vector?? default ctor??) @@ -62,7 +77,7 @@ namespace mrover { int numInliers = 0; - for (auto p : points) { + for (auto p: points) { // calculate distance of each point from potential plane float distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); @@ -72,7 +87,7 @@ namespace mrover { } // update best plane if better inlier count - if (numInliers > minInliers) { + if (numInliers > minInliers) { minInliers = numInliers; bestPlane = normal; } @@ -82,4 +97,5 @@ namespace mrover { return bestPlane; } + } // namespace mrover diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 689fe2812..d4729eab7 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -7,14 +7,25 @@ namespace mrover { class LanderAlignNodelet : public nodelet::Nodelet { ros::NodeHandle mNh, mPnh; + ros::Publisher mDebugVectorPub; + + ros::Subscriber mVectorSub; + + + float mThreshold; + + std::vector mFilteredPoints; + auto onInit() -> void override; // deprecated/not needed anymore // auto downsample(sensor_msgs::PointCloud2Ptr const& cloud) -> sensor_msgs::PointCloud2Ptr; - auto filterNormals(sensor_msgs::PointCloud2Ptr const& cloud, float threshold) -> std::vector; + void LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud); + + void filterNormals(sensor_msgs::PointCloud2Ptr const& cloud); - auto ransac(const std::vector& points, float distanceThreshold, int minInliers, int epochs) -> Eigen::Vector3f; + auto ransac(std::vector const& points, float distanceThreshold, int minInliers, int epochs) -> Eigen::Vector3f; }; } // namespace mrover diff --git a/src/teleoperation/lander_align/pch.hpp b/src/teleoperation/lander_align/pch.hpp index ae1a3a507..dadc8af0e 100644 --- a/src/teleoperation/lander_align/pch.hpp +++ b/src/teleoperation/lander_align/pch.hpp @@ -4,12 +4,12 @@ #include #include -#include -#include #include -#include #include #include +#include +#include +#include #include From ba0a8c7574a909cd810381d7e68e9a95a403c45c Mon Sep 17 00:00:00 2001 From: John Date: Tue, 6 Feb 2024 18:25:56 -0500 Subject: [PATCH 13/89] Updates --- CMakeLists.txt | 2 +- src/teleoperation/lander_align/lander_align.cpp | 8 ++++---- src/teleoperation/lander_align/lander_align.hpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a06741d2..c56a75cc0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -225,4 +225,4 @@ add_rostest(test/util/SE3_tf_test.test) install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) +) \ No newline at end of file diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 7809ebc84..025f9ad6c 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -63,9 +63,9 @@ namespace mrover { do { for (int i = 0; i < epochs; ++i) { // sample 3 random points (potential inliers) - Point* point1 = points[distribution(generator)]; - Point* point2 = points[distribution(generator)]; - Point* point3 = points[distribution(generator)]; + Point const* point1 = points[distribution(generator)]; + Point const* point2 = points[distribution(generator)]; + Point const* point3 = points[distribution(generator)]; Eigen::Vector3f vec1{point1->x, point1->y, point1->z}; Eigen::Vector3f vec2{point2->x, point2->y, point2->z}; @@ -98,4 +98,4 @@ namespace mrover { } -} // namespace mrover +} // namespace mrover \ No newline at end of file diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index d4729eab7..a3175ceae 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -28,4 +28,4 @@ namespace mrover { auto ransac(std::vector const& points, float distanceThreshold, int minInliers, int epochs) -> Eigen::Vector3f; }; -} // namespace mrover +} // namespace mrover \ No newline at end of file From fd76219a11a293a24a90db53fbe0c2d44c2c8e9e Mon Sep 17 00:00:00 2001 From: Khush Date: Tue, 6 Feb 2024 19:27:41 -0500 Subject: [PATCH 14/89] calculating center point of plane UNTESTED --- src/teleoperation/lander_align/lander_align.cpp | 17 +++++++++++++++++ src/teleoperation/lander_align/lander_align.hpp | 3 +++ 2 files changed, 20 insertions(+) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 025f9ad6c..490492dc6 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -14,6 +14,7 @@ namespace mrover { mThreshold = 10; mVectorSub = mNh.subscribe("/camera/left/points", 1, &LanderAlignNodelet::filterNormals, this); mDebugVectorPub = mNh.advertise("/lander_align/Pose", 1); + mBestCenter(0, 0, 0); } void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { @@ -90,10 +91,26 @@ namespace mrover { if (numInliers > minInliers) { minInliers = numInliers; bestPlane = normal; + mBestOffset = offset; } } } while (bestPlane.isZero()); // keep trying until we get valid result TODO break case after X attempts?? + // Run through one more loop to identify the center of the plane + for (auto p: points) { + // calculate distance of each point from potential plane + float distance = std::abs(bestPlane.x() * p->x + bestPlane.y() * p->y + bestPlane.z() * p->z + mBestOffset); + + if (distance < distanceThreshold) { + mBestCenter(0) += p->x; + mBestCenter(1) += p->y; + mBestCenter(2) += p->z; + ++numInliers; // count num of inliers that pass the "good enough fit" threshold + } + } + + mBestCenter /= numInliers; + return bestPlane; } diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index a3175ceae..b11b98f87 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -13,9 +13,12 @@ namespace mrover { float mThreshold; + float mBestOffset; std::vector mFilteredPoints; + Eigen::Vector3f mBestCenter; + auto onInit() -> void override; // deprecated/not needed anymore From e2b467634ec761f11c3e08fcc2d6f024c31a51ae Mon Sep 17 00:00:00 2001 From: John Date: Tue, 6 Feb 2024 19:40:01 -0500 Subject: [PATCH 15/89] Merged --- CMakeLists.txt | 1 + .../lander_align/lander_align.cpp | 68 +++++++++++++------ .../lander_align/lander_align.hpp | 17 +++-- src/teleoperation/lander_align/pch.hpp | 9 ++- 4 files changed, 69 insertions(+), 26 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c56a75cc0..eaaa3423f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -190,6 +190,7 @@ endif () mrover_add_nodelet(lander_align src/teleoperation/lander_align/*.cpp src/teleoperation/lander_align src/teleoperation/lander_align/pch.hpp) mrover_add_node(arm_controller src/teleoperation/arm_controller/*.cpp) +mrover_nodelet_link_libraries(lander_align lie) ## Simulator diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 490492dc6..e9d87d1e4 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,7 +1,10 @@ #include "lander_align.hpp" #include "pch.hpp" #include +#include #include +#include +#include #include #include #include @@ -12,21 +15,23 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); mThreshold = 10; - mVectorSub = mNh.subscribe("/camera/left/points", 1, &LanderAlignNodelet::filterNormals, this); + mVectorSub = mNh.subscribe("/camera/left/points", 1, &LanderAlignNodelet::LanderCallback, this); mDebugVectorPub = mNh.advertise("/lander_align/Pose", 1); - mBestCenter(0, 0, 0); + + //TF Create the Reference Frames + mNh.param("camera_frame", mCameraFrameId, "zed2i_left_camera_frame"); + mNh.param("world_frame", mMapFrameId, "map"); } void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { filterNormals(cloud); - Eigen::Vector3f planeNorm = ransac(mFilteredPoints, 1, 10, 10); - - geometry_msgs::Vector3 vect; - vect.x = planeNorm.x(); - vect.y = planeNorm.y(); - vect.z = planeNorm.z(); - - mDebugVectorPub.publish(vect); + if (std::optional planeNorm = ransac(mFilteredPoints, 0.2, 10, 100); planeNorm) { + geometry_msgs::Vector3 vect; + vect.x = planeNorm.value().x(); + vect.y = planeNorm.value().y(); + vect.z = planeNorm.value().z(); + mDebugVectorPub.publish(vect); + } } // deprecated/not needed anymore @@ -41,27 +46,35 @@ namespace mrover { void LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud) { // TODO: OPTIMIZE; doing this maobject_detector/debug_imgny push_back calls could slow things down + mFilteredPoints.clear(); + auto* cloudData = reinterpret_cast(cloud->data.data()); - auto* cloudData = reinterpret_cast(cloud->fields.data()); - // std::vector extractedPoints; + // define randomizer + std::default_random_engine generator; + std::uniform_int_distribution distribution(0, (int) 200); - for (auto point = cloudData; point < cloudData + (cloud->height * cloud->width); point++) { - if (point->normal_z < mThreshold) { + for (auto point = cloudData; point < cloudData + (cloud->height * cloud->width); point += distribution(generator)) { + bool isPointInvalid = (!std::isfinite(point->x) || !std::isfinite(point->y) || !std::isfinite(point->z)); + if (point->normal_z < mThreshold && !isPointInvalid) { mFilteredPoints.push_back(point); } } } - auto LanderAlignNodelet::ransac(std::vector const& points, float const distanceThreshold, int minInliers, int const epochs) -> Eigen::Vector3f { + auto LanderAlignNodelet::ransac(std::vector const& points, float const distanceThreshold, int minInliers, int const epochs) -> std::optional { // TODO: use RANSAC to find the lander face, should be the closest, we may need to modify this to output more information, currently the output is the normal // takes 3 samples for every epoch and terminates after specified number of epochs Eigen::Vector3f bestPlane(0, 0, 0); // normal vector representing plane (initialize as zero vector?? default ctor??) + float offset; // define randomizer std::default_random_engine generator; std::uniform_int_distribution distribution(0, (int) points.size() - 1); - do { + if (points.size() < 3) { + return std::nullopt; + } + while (bestPlane.isZero()) { for (int i = 0; i < epochs; ++i) { // sample 3 random points (potential inliers) Point const* point1 = points[distribution(generator)]; @@ -74,7 +87,7 @@ namespace mrover { // fit a plane to these points Eigen::Vector3f normal = (vec1 - vec2).cross(vec1 - vec3).normalized(); - float offset = -normal.dot(vec1); // calculate offset (D value) using one of the points + offset = -normal.dot(vec1); // calculate offset (D value) using one of the points int numInliers = 0; @@ -90,13 +103,13 @@ namespace mrover { // update best plane if better inlier count if (numInliers > minInliers) { minInliers = numInliers; - bestPlane = normal; - mBestOffset = offset; + bestPlane = normal.normalized(); } } - } while (bestPlane.isZero()); // keep trying until we get valid result TODO break case after X attempts?? + } // Run through one more loop to identify the center of the plane + int numInliers = 0; for (auto p: points) { // calculate distance of each point from potential plane float distance = std::abs(bestPlane.x() * p->x + bestPlane.y() * p->y + bestPlane.z() * p->z + mBestOffset); @@ -109,7 +122,20 @@ namespace mrover { } } - mBestCenter /= numInliers; + if (numInliers == 0) { + return std::nullopt; + } + + mBestCenter /= static_cast(numInliers); + + SE3 planeLoc{ + {mBestCenter(0), mBestCenter(1), mBestCenter(2)}, + {}}; + + std::string immediateFrameId = "immediatePlane"; + SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, planeLoc); + + ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestCenter(0), mBestCenter(1), mBestCenter(2), bestPlane.x(), bestPlane.y(), bestPlane.z()); return bestPlane; } diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index b11b98f87..917200863 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -11,14 +11,23 @@ namespace mrover { ros::Subscriber mVectorSub; + //** + float mBestOffset; + + Eigen::Vector3f mBestCenter; + //** + + //TF Member Variables + tf2_ros::Buffer mTfBuffer; + tf2_ros::TransformListener mTfListener{mTfBuffer}; + tf2_ros::TransformBroadcaster mTfBroadcaster; + std::string mCameraFrameId; + std::string mMapFrameId; float mThreshold; - float mBestOffset; std::vector mFilteredPoints; - Eigen::Vector3f mBestCenter; - auto onInit() -> void override; // deprecated/not needed anymore @@ -28,7 +37,7 @@ namespace mrover { void filterNormals(sensor_msgs::PointCloud2Ptr const& cloud); - auto ransac(std::vector const& points, float distanceThreshold, int minInliers, int epochs) -> Eigen::Vector3f; + auto ransac(std::vector const& points, float distanceThreshold, int minInliers, int epochs) -> std::optional; }; } // namespace mrover \ No newline at end of file diff --git a/src/teleoperation/lander_align/pch.hpp b/src/teleoperation/lander_align/pch.hpp index dadc8af0e..00742350f 100644 --- a/src/teleoperation/lander_align/pch.hpp +++ b/src/teleoperation/lander_align/pch.hpp @@ -11,6 +11,13 @@ #include #include +//TF +#include +#include +#include + +#include "lie/se3.hpp" + #include -#include "point.hpp" +#include "point.hpp" \ No newline at end of file From 756a5f75462320963d845819379ccbc46b2abfe6 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 6 Feb 2024 19:52:29 -0500 Subject: [PATCH 16/89] Real Merge with Integration --- src/teleoperation/lander_align/pch.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/teleoperation/lander_align/pch.hpp b/src/teleoperation/lander_align/pch.hpp index 00742350f..703804a69 100644 --- a/src/teleoperation/lander_align/pch.hpp +++ b/src/teleoperation/lander_align/pch.hpp @@ -1,7 +1,5 @@ #pragma once -#include - #include #include #include From fc22a35dbcdbe24855793350f82da125ce345302 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 6 Feb 2024 20:01:11 -0500 Subject: [PATCH 17/89] Removed stuff --- launch/autonomy.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/autonomy.launch b/launch/autonomy.launch index 411a9e202..4dfd4e0da 100644 --- a/launch/autonomy.launch +++ b/launch/autonomy.launch @@ -9,11 +9,11 @@ - + \ No newline at end of file From addf9cff7eff6de43828d20e04921b21704f9f1f Mon Sep 17 00:00:00 2001 From: Joshua Huang Date: Thu, 8 Feb 2024 18:40:14 -0500 Subject: [PATCH 18/89] Fix everything --- src/teleoperation/lander_align/lander_align.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index e9d87d1e4..5ed05becb 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -74,7 +74,7 @@ namespace mrover { if (points.size() < 3) { return std::nullopt; } - while (bestPlane.isZero()) { + while (bestPlane.isZero()) { // TODO add give up condition after X iter for (int i = 0; i < epochs; ++i) { // sample 3 random points (potential inliers) Point const* point1 = points[distribution(generator)]; From 00082d659db634f702931aab53be86ee3f9b8c47 Mon Sep 17 00:00:00 2001 From: Kushal Date: Thu, 8 Feb 2024 18:43:50 -0500 Subject: [PATCH 19/89] Add twist write function WIP --- .../lander_align/lander_align.cpp | 24 +++++++++++++++++++ .../lander_align/lander_align.hpp | 2 ++ 2 files changed, 26 insertions(+) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 5ed05becb..1211f14a8 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -9,6 +9,11 @@ #include #include +#include +#include +#include + + namespace mrover { auto LanderAlignNodelet::onInit() -> void { @@ -140,5 +145,24 @@ namespace mrover { return bestPlane; } + void LanderAlignNodelet::sendTwist(Eigen::Vector3f const& mBestCenter) { + tf2_ros::Buffer mTfBuffer; + tf2_ros::TransformListener mTfListener{mTfBuffer}; + SE3 rover = SE3::fromTfTree(mTfBuffer, "map", "base_link"); + + float linear_thresh = 0.1; + float angular_thresh = 0.1; + Eigen::Vector3f offset = {-2, 0, 0}; + + Eigen::Vector3f target_pos = mBestCenter - offset; + Eigen::Vector3f rover_pos = rover.position(); + Eigen::Vector3f target_dir = target_pos - rover_pos; + auto rover_dir = rover.rotation(); + + while (target_dir.norm() > linear_thresh&&) { + rover_pos = rover.position(); + target_dir = target_pos - rover_pos; + } + } } // namespace mrover \ No newline at end of file diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 917200863..0e0aa8b5f 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -38,6 +38,8 @@ namespace mrover { void filterNormals(sensor_msgs::PointCloud2Ptr const& cloud); auto ransac(std::vector const& points, float distanceThreshold, int minInliers, int epochs) -> std::optional; + + void sendTwist(Eigen::Vector3f const& mBestCenter); }; } // namespace mrover \ No newline at end of file From 4b2429c4e6334457409f520b2b7bc222c4a3f24a Mon Sep 17 00:00:00 2001 From: MyCabbages4 Date: Thu, 8 Feb 2024 19:08:20 -0500 Subject: [PATCH 20/89] Added commented code for detecting center --- src/teleoperation/lander_align/lander_align.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 1211f14a8..7e7863117 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -70,6 +70,7 @@ namespace mrover { // TODO: use RANSAC to find the lander face, should be the closest, we may need to modify this to output more information, currently the output is the normal // takes 3 samples for every epoch and terminates after specified number of epochs Eigen::Vector3f bestPlane(0, 0, 0); // normal vector representing plane (initialize as zero vector?? default ctor??) + // Eigen::Vector3f currentCenter(0, 0, 0); // Keeps track of current center of plane in current epoch float offset; // define randomizer @@ -81,6 +82,7 @@ namespace mrover { } while (bestPlane.isZero()) { // TODO add give up condition after X iter for (int i = 0; i < epochs; ++i) { + // currentCenter *= 0; // Set all vals in current center to zero at the start of each epoch // sample 3 random points (potential inliers) Point const* point1 = points[distribution(generator)]; Point const* point2 = points[distribution(generator)]; @@ -101,6 +103,10 @@ namespace mrover { float distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); if (distance < distanceThreshold) { + // Add points to current planes center + // currentCenter(0) += p->x; + // currentCenter(1) += p->y; + // currentCenter(2) += p->z; ++numInliers; // count num of inliers that pass the "good enough fit" threshold } } @@ -109,11 +115,14 @@ namespace mrover { if (numInliers > minInliers) { minInliers = numInliers; bestPlane = normal.normalized(); + + // If this is the new best plane, set mBestCenter + // mBestCenter = currentCenter / static_cast(minInliers); } } } - // Run through one more loop to identify the center of the plane + // Run through one more loop to identify the center of the plane (one possibility for determining best center) int numInliers = 0; for (auto p: points) { // calculate distance of each point from potential plane From d042f5549d406d8d01123997b733de05a5de73f2 Mon Sep 17 00:00:00 2001 From: Kushal Date: Thu, 8 Feb 2024 19:33:27 -0500 Subject: [PATCH 21/89] Twist Send Updates(WIP) --- src/teleoperation/lander_align/lander_align.cpp | 14 ++++++++++---- src/teleoperation/lander_align/lander_align.hpp | 2 +- starter_project/autonomy/msg/StarterProjectTag.msg | 4 ++++ 3 files changed, 15 insertions(+), 5 deletions(-) create mode 100644 starter_project/autonomy/msg/StarterProjectTag.msg diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 7e7863117..91cd04467 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -154,23 +154,29 @@ namespace mrover { return bestPlane; } - void LanderAlignNodelet::sendTwist(Eigen::Vector3f const& mBestCenter) { + void LanderAlignNodelet::sendTwist(Eigen::Vector3f const& mBestCenter, Eigen::Vector3f const& offset) { tf2_ros::Buffer mTfBuffer; tf2_ros::TransformListener mTfListener{mTfBuffer}; SE3 rover = SE3::fromTfTree(mTfBuffer, "map", "base_link"); float linear_thresh = 0.1; float angular_thresh = 0.1; - Eigen::Vector3f offset = {-2, 0, 0}; Eigen::Vector3f target_pos = mBestCenter - offset; Eigen::Vector3f rover_pos = rover.position(); Eigen::Vector3f target_dir = target_pos - rover_pos; - auto rover_dir = rover.rotation(); + Eigen::Vector3f rover_dir = rover.rotation().matrix().col(0); - while (target_dir.norm() > linear_thresh&&) { + auto linear_error = abs(target_dir.norm()); + auto angular_error = angle_to_rotate(rover_dir, target_dir); + + while (linear_error > linear_thresh && angular_error > angular_thresh) { rover_pos = rover.position(); target_dir = target_pos - rover_pos; + rover_dir = rover.rotation().matrix().col(0); + //Drive Command + linear_error = abs(target_dir.norm()); + angular_error = angle_to_rotate(rover_dir, target_dir); } } diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 0e0aa8b5f..919205338 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -39,7 +39,7 @@ namespace mrover { auto ransac(std::vector const& points, float distanceThreshold, int minInliers, int epochs) -> std::optional; - void sendTwist(Eigen::Vector3f const& mBestCenter); + void sendTwist(Eigen::Vector3f const& mBestCenter, Eigen::Vector3f const& offset); }; } // namespace mrover \ No newline at end of file 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 From 9891483f9052896a4ed010d1cc165318c9750e02 Mon Sep 17 00:00:00 2001 From: MyCabbages4 Date: Thu, 8 Feb 2024 19:36:35 -0500 Subject: [PATCH 22/89] commented angle error function --- .../lander_align/lander_align.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 91cd04467..ba778086a 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,9 +1,13 @@ #include "lander_align.hpp" #include "pch.hpp" #include +#include +#include +#include #include #include #include +#include #include #include #include @@ -180,4 +184,18 @@ namespace mrover { } } + // auto angleToRotate(Eigen::Vector2f v1, Eigen::Vector2f v2) -> float { + // v1.normalize(); + // v2.normalize(); + + // float smallestAngle = std::clamp(acosf(v1.dot(v2)), -1.0f, 1.0f); + // float perpendicularAlignment = v2(0) * -v1(1) + v2(1) * v1(0); + + // int sign = std::signbit(perpendicularAlignment); + + // sign = (sign == 1) ? -1 : 1; + + // return smallestAngle * (float) sign; + // } + } // namespace mrover \ No newline at end of file From a4dc43911d6b46276d21c273263e27463f773067 Mon Sep 17 00:00:00 2001 From: Julian Skifstad Date: Thu, 8 Feb 2024 20:43:17 -0500 Subject: [PATCH 23/89] incomplete PID implementation --- .../lander_align/lander_align.cpp | 39 ++++++++++++++++--- 1 file changed, 33 insertions(+), 6 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 91cd04467..20139a274 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -13,6 +13,8 @@ #include #include +#include <~/mrover/src/esw/fw/bdcmc/Core/Inc/pidf.hpp> + namespace mrover { @@ -154,6 +156,19 @@ namespace mrover { return bestPlane; } + auto angle_to_rotate(Eigen::Vector3f current, Eigen::Vector3f target) { + current[2] = 0; + target[2] = 0; + Eigen::Vector3f u1 = current.normalized(); + Eigen::Vector3f u2 = target.normalized(); + float theta = acos(u1.dot(u2)); + float perp_alignment = u2[0] * -u1[1] + u2[1] * u1[0]; + if (perp_alignment > 0) { + return theta; + } + return -theta; + } + void LanderAlignNodelet::sendTwist(Eigen::Vector3f const& mBestCenter, Eigen::Vector3f const& offset) { tf2_ros::Buffer mTfBuffer; tf2_ros::TransformListener mTfListener{mTfBuffer}; @@ -164,19 +179,31 @@ namespace mrover { Eigen::Vector3f target_pos = mBestCenter - offset; Eigen::Vector3f rover_pos = rover.position(); - Eigen::Vector3f target_dir = target_pos - rover_pos; + Eigen::Vector3f direction_vec = target_pos - rover_pos; Eigen::Vector3f rover_dir = rover.rotation().matrix().col(0); - auto linear_error = abs(target_dir.norm()); - auto angular_error = angle_to_rotate(rover_dir, target_dir); + direction_vec[2] = 0; + rover_dir[2] = 0; + + auto linear_error = abs(direction_vec.norm()); + auto angular_error = angle_to_rotate(rover_dir, direction_vec); + + PIDF position_PID; // also need some time unit, set deadband and errors and stuff + PIDF angle_PID; + while (linear_error > linear_thresh && angular_error > angular_thresh) { rover_pos = rover.position(); - target_dir = target_pos - rover_pos; + direction_vec = target_pos - rover_pos; rover_dir = rover.rotation().matrix().col(0); + //Drive Command - linear_error = abs(target_dir.norm()); - angular_error = angle_to_rotate(rover_dir, target_dir); + linear_error = abs(direction_vec.norm()); + angular_error = angle_to_rotate(rover_dir, direction_vec); + + // PID + float angle_command = position_PID.calculate(linear_error, 0); + float linear_command = angle_PID.calculate(angular_error, 0); } } From f3b9f2f358bfc753b8887fdb88b8b0965ab6d2eb Mon Sep 17 00:00:00 2001 From: Julian Skifstad Date: Sat, 10 Feb 2024 15:16:52 -0500 Subject: [PATCH 24/89] finished basic drive control, added pidf.hpp --- .../lander_align/lander_align.cpp | 120 +++++++++++++----- .../lander_align/lander_align.hpp | 38 +++++- src/util/pidf.hpp | 120 ++++++++++++++++++ 3 files changed, 245 insertions(+), 33 deletions(-) create mode 100644 src/util/pidf.hpp diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 20139a274..5b1d5d1ad 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,20 +1,22 @@ #include "lander_align.hpp" #include "pch.hpp" #include +#include #include +#include #include + #include #include #include #include +#include #include #include #include #include -#include <~/mrover/src/esw/fw/bdcmc/Core/Inc/pidf.hpp> - namespace mrover { @@ -24,6 +26,8 @@ namespace mrover { mThreshold = 10; mVectorSub = mNh.subscribe("/camera/left/points", 1, &LanderAlignNodelet::LanderCallback, this); mDebugVectorPub = mNh.advertise("/lander_align/Pose", 1); + mTwistPub = mNh.advertise("/cmd_vel", 1); + //TF Create the Reference Frames mNh.param("camera_frame", mCameraFrameId, "zed2i_left_camera_frame"); @@ -156,9 +160,12 @@ namespace mrover { return bestPlane; } - auto angle_to_rotate(Eigen::Vector3f current, Eigen::Vector3f target) { - current[2] = 0; - target[2] = 0; + auto LanderAlignNodelet::PID::rotate_speed(float theta) const -> float { + return Angle_P * theta; + } + + + auto LanderAlignNodelet::PID::find_angle(Eigen::Vector3f current, Eigen::Vector3f target) -> float { Eigen::Vector3f u1 = current.normalized(); Eigen::Vector3f u2 = target.normalized(); float theta = acos(u1.dot(u2)); @@ -169,42 +176,91 @@ namespace mrover { return -theta; } - void LanderAlignNodelet::sendTwist(Eigen::Vector3f const& mBestCenter, Eigen::Vector3f const& offset) { - tf2_ros::Buffer mTfBuffer; - tf2_ros::TransformListener mTfListener{mTfBuffer}; - SE3 rover = SE3::fromTfTree(mTfBuffer, "map", "base_link"); + auto LanderAlignNodelet::PID::drive_speed(float distance) -> float { + return distance * Linear_P; + } + + auto LanderAlignNodelet::PID::find_distance(Eigen::Vector3f current, Eigen::Vector3f target) -> float { + Eigen::Vector3f difference = target - current; + float distance = difference.norm(); + return distance; + } + + // auto LanderAlignNodelet::PID::calculate(Eigen::Vector3f& input, Eigen::Vector3f& target) -> std::tuple { + // input[2] = 0; + // target[2] = 0; + + // return {rotate_command(input, target), drive_speed(input, target)}; + // } + + + void LanderAlignNodelet::sendTwist(Eigen::Vector3f const& planeNormal, Eigen::Vector3f const& planeCenterInWorld, Eigen::Vector3f const& offset) { + SE3 rover; + + geometry_msgs::Twist twist; - float linear_thresh = 0.1; - float angular_thresh = 0.1; + float const linear_thresh = 0.1; // could be member variables + float const angular_thresh = 0.1; - Eigen::Vector3f target_pos = mBestCenter - offset; - Eigen::Vector3f rover_pos = rover.position(); - Eigen::Vector3f direction_vec = target_pos - rover_pos; - Eigen::Vector3f rover_dir = rover.rotation().matrix().col(0); + Eigen::Vector3f rover_dir; - direction_vec[2] = 0; - rover_dir[2] = 0; + Eigen::Vector3f targetPosInWorld = planeCenterInWorld - offset; + targetPosInWorld[2] = 0; - auto linear_error = abs(direction_vec.norm()); - auto angular_error = angle_to_rotate(rover_dir, direction_vec); + PID pid(0.1, 0.1); // literally just P -- ugly class and probably needs restructuring in the future - PIDF position_PID; // also need some time unit, set deadband and errors and stuff - PIDF angle_PID; + ros::Rate rate(20); // ROS Rate at 20Hz + while (ros::ok()) { + rover = SE3::fromTfTree(mTfBuffer, "map", "base_link"); + Eigen::Vector3f roverPosInWorld = rover.position(); + roverPosInWorld[2] = 0; + switch (mLoopState) { + case RTRSTATE::turn1: { + rover_dir = rover.rotation().matrix().col(0); + rover_dir[2] = 0; + float angle = pid.find_angle(rover_dir, targetPosInWorld - roverPosInWorld); + float angle_rate = pid.rotate_speed(angle); - while (linear_error > linear_thresh && angular_error > angular_thresh) { - rover_pos = rover.position(); - direction_vec = target_pos - rover_pos; - rover_dir = rover.rotation().matrix().col(0); + twist.angular.z = angle_rate; - //Drive Command - linear_error = abs(direction_vec.norm()); - angular_error = angle_to_rotate(rover_dir, direction_vec); + if (abs(angle) < angular_thresh) { + mLoopState = RTRSTATE::drive; + } + } + + case RTRSTATE::drive: { + Eigen::Vector3f roverPosInWorld = rover.position(); + float distance = pid.find_distance(roverPosInWorld, targetPosInWorld - roverPosInWorld); + float drive_rate = pid.drive_speed(distance); + + twist.linear.x = drive_rate; + + if (abs(distance) < linear_thresh) { + mLoopState = RTRSTATE::turn2; + } + } + + case RTRSTATE::turn2: { + rover_dir = rover.rotation().matrix().col(0); + rover_dir[2] = 0; + float angle = pid.find_angle(rover_dir, planeNormal); + float angle_rate = pid.rotate_speed(angle); - // PID - float angle_command = position_PID.calculate(linear_error, 0); - float linear_command = angle_PID.calculate(angular_error, 0); + twist.angular.z = angle_rate; + + if (abs(angle) < angular_thresh) { + mLoopState = RTRSTATE::done; + } + } + + case RTRSTATE::done: + break; + } + mTwistPub.publish(twist); + + ROS_INFO("Running turn/drive state machine..."); + rate.sleep(); } } - } // namespace mrover \ No newline at end of file diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 919205338..c7fbdca4d 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -1,16 +1,30 @@ #pragma once #include "pch.hpp" +#include +#include namespace mrover { + enum RTRSTATE { + turn1, + drive, + turn2, + done + }; + class LanderAlignNodelet : public nodelet::Nodelet { ros::NodeHandle mNh, mPnh; ros::Publisher mDebugVectorPub; + ros::Publisher mTwistPub; + + ros::Subscriber mVectorSub; + RTRSTATE mLoopState; + //** float mBestOffset; @@ -39,7 +53,29 @@ namespace mrover { auto ransac(std::vector const& points, float distanceThreshold, int minInliers, int epochs) -> std::optional; - void sendTwist(Eigen::Vector3f const& mBestCenter, Eigen::Vector3f const& offset); + void sendTwist(Eigen::Vector3f const& planeNormal, Eigen::Vector3f const& mBestCenter, Eigen::Vector3f const& offset); + + class PID { + private: + float const Angle_P; + float const Linear_P; + + + public: + PID(float angle_P, float linear_P); + + // auto calculate(Eigen::Vector3f input, Eigen::Vector3f target) -> float; + + [[nodiscard]] auto rotate_speed(float theta) const -> float; + + + auto find_angle(Eigen::Vector3f current, Eigen::Vector3f target) -> float; + + + auto find_distance(Eigen::Vector3f current, Eigen::Vector3f target) -> float; + + auto drive_speed(float) -> float; + }; }; } // namespace mrover \ No newline at end of file diff --git a/src/util/pidf.hpp b/src/util/pidf.hpp new file mode 100644 index 000000000..de83b3576 --- /dev/null +++ b/src/util/pidf.hpp @@ -0,0 +1,120 @@ +#pragma once + +#include +#include + +#include + +namespace mrover { + + /** + * A PIDF controller. An output signal is generated based on the error of the current input and the desired input target. + * + * P - Proportional Multiplied by the error + * I - Integral Multiplied by the integral of the error. Alleviates steady state error, use with caution. + * D - Derivative Multiplied by the derivative of the error + * F - Feedforward Multiplied by the target. This is useful for gravity compensation. + * + * @tparam InputUnit Unit of input, usually a sensor reading (for example encoder ticks for an arm) + * @tparam OutputUnit Unit of output, usually a motor command (for example voltage for a motor) + * @tparam TimeUnit Unit of time + */ + template + struct PIDF { + private: + using TotalError = compound_unit; + // Settings + compound_unit> m_p{}; + compound_unit, inverse> m_i{}; + compound_unit>>> m_d{}; + compound_unit> m_f{}; + InputUnit m_dead_band{}; + std::pair m_output_bound{}; + std::optional> m_input_bound; + // State + TotalError m_total_error{}; + InputUnit m_last_error{}; + TimeUnit m_last_time{}; + + auto calculate(InputUnit input, InputUnit target, TimeUnit dt) -> OutputUnit { + InputUnit error = target - input; + + if (m_input_bound) { + auto [in_min, in_max] = m_input_bound.value(); + if (abs(error) > (in_max - in_min) / 2) { + if (error > InputUnit{}) { + error -= in_max - in_min; + } else { + error += in_max - in_min; + } + } + } + + auto [out_min, out_max] = m_output_bound; + if (out_min < error * m_p && error * m_p < out_max) { + m_total_error += error * dt; + } else { + m_total_error = TotalError{}; + } + + InputUnit error_for_p = abs(error) < m_dead_band ? InputUnit{} : error; + OutputUnit result = m_p * error_for_p + m_i * m_total_error + m_d * (error - m_last_error) / dt + m_f * target; + m_last_error = error; + + return clamp(result, out_min, out_max); + } + + public: + /** + * TODO: documentation + * + * @param input Current value + * @param target Desired final value + * @return Output value to control the input to move to the target + */ + auto calculate(InputUnit input, InputUnit target) -> OutputUnit { + double current_ticks = HAL_GetTick(); + IsUnit auto tick_frequency = Hertz{HAL_GetTickFreq()}; + TimeUnit now = current_ticks / tick_frequency; + TimeUnit dt = now - m_last_time; + m_last_time = now; + return calculate(input, target, now); + } + + auto with_p(double p) -> PIDF& { + m_p = p; + return *this; + } + + auto with_i(double i) -> PIDF& { + m_i = i; + return *this; + } + + auto with_d(double d) -> PIDF& { + m_d = d; + return *this; + } + + auto with_f(double f) -> PIDF& { + m_f = f; + return *this; + } + + auto with_dead_band(InputUnit dead_band) -> PIDF& { + m_dead_band = dead_band; + return *this; + } + + auto with_input_bound(InputUnit min, InputUnit max) -> PIDF& { + m_input_bound = {min, max}; + return *this; + } + + auto with_output_bound(double min, double max) -> PIDF& { + m_output_bound = {min, max}; + return *this; + } + }; + +} // namespace mrover From ef1041f2b09dfe780d6184a327096ae096758913 Mon Sep 17 00:00:00 2001 From: Julian Skifstad Date: Sun, 11 Feb 2024 14:06:20 -0500 Subject: [PATCH 25/89] finished initial control loop + ransac --- .../lander_align/lander_align.cpp | 116 +++++++++++------- .../lander_align/lander_align.hpp | 17 ++- src/teleoperation/lander_align/pch.hpp | 2 +- 3 files changed, 84 insertions(+), 51 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 763ac1aed..cb368dcd2 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -37,13 +37,16 @@ namespace mrover { void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { filterNormals(cloud); - if (std::optional planeNorm = ransac(mFilteredPoints, 0.2, 10, 100); planeNorm) { + ransac(0.2, 10, 100); + if (mBestNormal.has_value()) { geometry_msgs::Vector3 vect; - vect.x = planeNorm.value().x(); - vect.y = planeNorm.value().y(); - vect.z = planeNorm.value().z(); + vect.x = mBestNormal.value().x(); + vect.y = mBestNormal.value().y(); + vect.z = mBestNormal.value().z(); mDebugVectorPub.publish(vect); } + + sendTwist({1, 1, 0}); } // deprecated/not needed anymore @@ -73,27 +76,32 @@ namespace mrover { } } - auto LanderAlignNodelet::ransac(std::vector const& points, float const distanceThreshold, int minInliers, int const epochs) -> std::optional { + void LanderAlignNodelet::ransac(float const distanceThreshold, int minInliers, int const epochs) { // TODO: use RANSAC to find the lander face, should be the closest, we may need to modify this to output more information, currently the output is the normal // takes 3 samples for every epoch and terminates after specified number of epochs - Eigen::Vector3f bestPlane(0, 0, 0); // normal vector representing plane (initialize as zero vector?? default ctor??) + // Eigen::Vector3f mBestNormal(0, 0, 0); // normal vector representing plane (initialize as zero vector?? default ctor??) // Eigen::Vector3f currentCenter(0, 0, 0); // Keeps track of current center of plane in current epoch float offset; // define randomizer std::default_random_engine generator; - std::uniform_int_distribution distribution(0, (int) points.size() - 1); + std::uniform_int_distribution distribution(0, (int) mFilteredPoints.size() - 1); - if (points.size() < 3) { - return std::nullopt; + if (mFilteredPoints.size() < 3) { + mBestNormal = std::nullopt; + mBestCenterInZED = std::nullopt; + return; } - while (bestPlane.isZero()) { // TODO add give up condition after X iter + + mBestNormal = std::make_optional(0, 0, 0); + + while (mBestNormal.value().isZero()) { // TODO add give up condition after X iter for (int i = 0; i < epochs; ++i) { // currentCenter *= 0; // Set all vals in current center to zero at the start of each epoch // sample 3 random points (potential inliers) - Point const* point1 = points[distribution(generator)]; - Point const* point2 = points[distribution(generator)]; - Point const* point3 = points[distribution(generator)]; + Point const* point1 = mFilteredPoints[distribution(generator)]; + Point const* point2 = mFilteredPoints[distribution(generator)]; + Point const* point3 = mFilteredPoints[distribution(generator)]; Eigen::Vector3f vec1{point1->x, point1->y, point1->z}; Eigen::Vector3f vec2{point2->x, point2->y, point2->z}; @@ -105,7 +113,7 @@ namespace mrover { int numInliers = 0; - for (auto p: points) { + for (auto p: mFilteredPoints) { // calculate distance of each point from potential plane float distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); @@ -121,44 +129,57 @@ namespace mrover { // update best plane if better inlier count if (numInliers > minInliers) { minInliers = numInliers; - bestPlane = normal.normalized(); + mBestNormal.value() = normal.normalized(); - // If this is the new best plane, set mBestCenter - // mBestCenter = currentCenter / static_cast(minInliers); + // If this is the new best plane, set mBestCenterInZED + // mBestCenterInZED = currentCenter / static_cast(minInliers); } } } // Run through one more loop to identify the center of the plane (one possibility for determining best center) int numInliers = 0; - for (auto p: points) { + for (auto p: mFilteredPoints) { // calculate distance of each point from potential plane - float distance = std::abs(bestPlane.x() * p->x + bestPlane.y() * p->y + bestPlane.z() * p->z + mBestOffset); + float distance = std::abs(mBestNormal.value().x() * p->x + mBestNormal.value().y() * p->y + mBestNormal.value().z() * p->z + mBestOffset); if (distance < distanceThreshold) { - mBestCenter(0) += p->x; - mBestCenter(1) += p->y; - mBestCenter(2) += p->z; + mBestCenterInZED.value().x() += p->x; + mBestCenterInZED.value().y() += p->y; + mBestCenterInZED.value().z() += p->z; ++numInliers; // count num of inliers that pass the "good enough fit" threshold } } if (numInliers == 0) { - return std::nullopt; + mBestNormal = std::nullopt; + mBestCenterInZED = std::nullopt; + return; } - mBestCenter /= static_cast(numInliers); + mBestCenterInZED.value() /= static_cast(numInliers); + + if (mBestNormal.value().x() < 0) { + mBestNormal.value() *= -1; + } - SE3 planeLoc{ - {mBestCenter(0), mBestCenter(1), mBestCenter(2)}, + mPlaneLocInZED = { + {mBestCenterInZED.value().x(), mBestCenterInZED.value().y(), mBestCenterInZED.value().z()}, {}}; - std::string immediateFrameId = "immediatePlane"; - SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, planeLoc); + std::string immediateFrameIdInZED = "immediatePlaneInZED"; + SE3::pushToTfTree(mTfBroadcaster, immediateFrameIdInZED, mCameraFrameId, mPlaneLocInZED); - ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestCenter(0), mBestCenter(1), mBestCenter(2), bestPlane.x(), bestPlane.y(), bestPlane.z()); + mPlaneLocInWorld = SE3::fromTfTree(mTfBuffer, immediateFrameIdInZED, mMapFrameId); - return bestPlane; + std::string immediateFrameIdInWorld = "immediatePlaneInWorld"; + SE3::pushToTfTree(mTfBroadcaster, immediateFrameIdInWorld, mMapFrameId, mPlaneLocInWorld); + + mBestCenterInWorld = { + static_cast(mPlaneLocInWorld.position().x()), + static_cast(mPlaneLocInWorld.position().y()), + static_cast(mPlaneLocInWorld.position().z())}; + ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestCenterInZED.value().x(), mBestCenterInZED.value().y(), mBestCenterInZED.value().z(), mBestNormal.value().x(), mBestNormal.value().y(), mBestNormal.value().z()); } auto LanderAlignNodelet::PID::rotate_speed(float theta) const -> float { @@ -166,10 +187,12 @@ namespace mrover { } - auto LanderAlignNodelet::PID::find_angle(Eigen::Vector3f current, Eigen::Vector3f target) -> float { + auto LanderAlignNodelet::PID::find_angle(Eigen::Vector3f const& current, Eigen::Vector3f const& target) -> float { Eigen::Vector3f u1 = current.normalized(); + u1.z() = 0; Eigen::Vector3f u2 = target.normalized(); - float theta = acos(u1.dot(u2)); + u2.z() = 0; + float theta = fmod(acos(u1.dot(u2)), static_cast(180)); float perp_alignment = u2[0] * -u1[1] + u2[1] * u1[0]; if (perp_alignment > 0) { return theta; @@ -181,7 +204,7 @@ namespace mrover { return distance * Linear_P; } - auto LanderAlignNodelet::PID::find_distance(Eigen::Vector3f current, Eigen::Vector3f target) -> float { + auto LanderAlignNodelet::PID::find_distance(Eigen::Vector3f const& current, Eigen::Vector3f const& target) -> float { Eigen::Vector3f difference = target - current; float distance = difference.norm(); return distance; @@ -195,31 +218,33 @@ namespace mrover { // } - void LanderAlignNodelet::sendTwist(Eigen::Vector3f const& planeNormal, Eigen::Vector3f const& planeCenterInWorld, Eigen::Vector3f const& offset) { + void LanderAlignNodelet::sendTwist(Eigen::Vector3f const& offset) { + + //Locations SE3 rover; + Eigen::Vector3f rover_dir; + Eigen::Vector3f targetPosInWorld = mBestCenterInWorld.value() - offset; + //Final msg geometry_msgs::Twist twist; + //Thr float const linear_thresh = 0.1; // could be member variables float const angular_thresh = 0.1; - Eigen::Vector3f rover_dir; - Eigen::Vector3f targetPosInWorld = planeCenterInWorld - offset; - targetPosInWorld[2] = 0; + targetPosInWorld.z() = 0; PID pid(0.1, 0.1); // literally just P -- ugly class and probably needs restructuring in the future ros::Rate rate(20); // ROS Rate at 20Hz while (ros::ok()) { rover = SE3::fromTfTree(mTfBuffer, "map", "base_link"); - Eigen::Vector3f roverPosInWorld = rover.position(); - roverPosInWorld[2] = 0; + Eigen::Vector3f roverPosInWorld{static_cast(rover.position().x()), static_cast(rover.position().y()), 0}; switch (mLoopState) { case RTRSTATE::turn1: { - rover_dir = rover.rotation().matrix().col(0); - rover_dir[2] = 0; + rover_dir = {static_cast(rover.rotation().matrix().col(0).x()), static_cast(rover.rotation().matrix().col(0).y()), 0}; float angle = pid.find_angle(rover_dir, targetPosInWorld - roverPosInWorld); float angle_rate = pid.rotate_speed(angle); @@ -228,10 +253,10 @@ namespace mrover { if (abs(angle) < angular_thresh) { mLoopState = RTRSTATE::drive; } + ROS_INFO("In state: turning to point..."); } case RTRSTATE::drive: { - Eigen::Vector3f roverPosInWorld = rover.position(); float distance = pid.find_distance(roverPosInWorld, targetPosInWorld - roverPosInWorld); float drive_rate = pid.drive_speed(distance); @@ -240,12 +265,13 @@ namespace mrover { if (abs(distance) < linear_thresh) { mLoopState = RTRSTATE::turn2; } + ROS_INFO("In state: driving to point..."); } case RTRSTATE::turn2: { - rover_dir = rover.rotation().matrix().col(0); + rover_dir = {static_cast(rover.rotation().matrix().col(0).x()), static_cast(rover.rotation().matrix().col(0).y()), 0}; rover_dir[2] = 0; - float angle = pid.find_angle(rover_dir, planeNormal); + float angle = pid.find_angle(rover_dir, -mBestNormal.value()); // normal is pointing "towards" tehe rover, so we need to flip it to find angle float angle_rate = pid.rotate_speed(angle); twist.angular.z = angle_rate; @@ -253,6 +279,7 @@ namespace mrover { if (abs(angle) < angular_thresh) { mLoopState = RTRSTATE::done; } + ROS_INFO("In state: turning to lander..."); } case RTRSTATE::done: @@ -260,7 +287,6 @@ namespace mrover { } mTwistPub.publish(twist); - ROS_INFO("Running turn/drive state machine..."); rate.sleep(); } } diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index c7fbdca4d..94a2fbee1 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -2,6 +2,7 @@ #include "pch.hpp" #include +#include #include namespace mrover { @@ -28,7 +29,13 @@ namespace mrover { //** float mBestOffset; - Eigen::Vector3f mBestCenter; + std::optional mBestCenterInZED; + std::optional mBestCenterInWorld; + + std::optional mBestNormal; + + SE3 mPlaneLocInZED; + SE3 mPlaneLocInWorld; //** //TF Member Variables @@ -51,9 +58,9 @@ namespace mrover { void filterNormals(sensor_msgs::PointCloud2Ptr const& cloud); - auto ransac(std::vector const& points, float distanceThreshold, int minInliers, int epochs) -> std::optional; + void ransac(float distanceThreshold, int minInliers, int epochs); - void sendTwist(Eigen::Vector3f const& planeNormal, Eigen::Vector3f const& mBestCenter, Eigen::Vector3f const& offset); + void sendTwist(Eigen::Vector3f const& offset); class PID { private: @@ -69,10 +76,10 @@ namespace mrover { [[nodiscard]] auto rotate_speed(float theta) const -> float; - auto find_angle(Eigen::Vector3f current, Eigen::Vector3f target) -> float; + auto find_angle(Eigen::Vector3f const& current, Eigen::Vector3f const& target) -> float; - auto find_distance(Eigen::Vector3f current, Eigen::Vector3f target) -> float; + auto find_distance(Eigen::Vector3f const& current, Eigen::Vector3f const& target) -> float; auto drive_speed(float) -> float; }; diff --git a/src/teleoperation/lander_align/pch.hpp b/src/teleoperation/lander_align/pch.hpp index 703804a69..054ba78c2 100644 --- a/src/teleoperation/lander_align/pch.hpp +++ b/src/teleoperation/lander_align/pch.hpp @@ -18,4 +18,4 @@ #include -#include "point.hpp" \ No newline at end of file +#include "point.hpp" From 692ed6f665dcefe8cbfa1431f3767965044fd080 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 11 Feb 2024 14:32:03 -0500 Subject: [PATCH 26/89] Fixing launch --- CMakeLists.txt | 6 ------ launch/simulator.launch | 2 ++ 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4256edfed..a6fc417b1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -193,12 +193,6 @@ if (CUDA_FOUND) endif () if (ZED_FOUND) - mrover_add_library(streaming src/esw/streaming/*.c* src/esw/streaming) - # target_link_libraries(streaming PUBLIC opencv_core opencv_cudacodec) - target_link_libraries(streaming PUBLIC cuda nvidia-encode opencv_core) - target_include_directories(streaming SYSTEM PUBLIC deps/nvenc) - target_compile_definitions(streaming PUBLIC BOOST_ASIO_NO_DEPRECATED) - mrover_add_nodelet(zed src/perception/zed_wrapper/*.c* src/perception/zed_wrapper src/perception/zed_wrapper/pch.hpp) mrover_nodelet_include_directories(zed ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) mrover_nodelet_link_libraries(zed ${ZED_LIBRARIES} ${SPECIAL_OS_LIBS} lie) diff --git a/launch/simulator.launch b/launch/simulator.launch index 78ecfbae8..cf0ca7ef3 100644 --- a/launch/simulator.launch +++ b/launch/simulator.launch @@ -1,5 +1,7 @@ + + From a9e3f1d893a10dab5ff6a32c5b6ed99334097c4f Mon Sep 17 00:00:00 2001 From: Julian Skifstad Date: Tue, 13 Feb 2024 17:59:52 -0500 Subject: [PATCH 27/89] opt issue fix --- src/teleoperation/lander_align/lander_align.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index cb368dcd2..c65b3efbc 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -94,6 +94,8 @@ namespace mrover { } mBestNormal = std::make_optional(0, 0, 0); + mBestCenterInZED = std::make_optional(0, 0, 0); + while (mBestNormal.value().isZero()) { // TODO add give up condition after X iter for (int i = 0; i < epochs; ++i) { From a4a82fd93baed0089e032d7f7b3a87408b1c84b1 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 13 Feb 2024 18:02:02 -0500 Subject: [PATCH 28/89] Sim not publishing --- launch/autonomy.launch | 4 ++-- src/teleoperation/lander_align/lander_align.cpp | 16 ++++++++++------ 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/launch/autonomy.launch b/launch/autonomy.launch index 4dfd4e0da..b710276aa 100644 --- a/launch/autonomy.launch +++ b/launch/autonomy.launch @@ -9,11 +9,11 @@ - - --> + \ No newline at end of file diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index cb368dcd2..e553c94c0 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -46,7 +46,7 @@ namespace mrover { mDebugVectorPub.publish(vect); } - sendTwist({1, 1, 0}); + //sendTwist({1, 1, 0}); } // deprecated/not needed anymore @@ -94,6 +94,7 @@ namespace mrover { } mBestNormal = std::make_optional(0, 0, 0); + mBestCenterInZED = std::make_optional(0, 0, 0); while (mBestNormal.value().isZero()) { // TODO add give up condition after X iter for (int i = 0; i < epochs; ++i) { @@ -170,10 +171,10 @@ namespace mrover { std::string immediateFrameIdInZED = "immediatePlaneInZED"; SE3::pushToTfTree(mTfBroadcaster, immediateFrameIdInZED, mCameraFrameId, mPlaneLocInZED); - mPlaneLocInWorld = SE3::fromTfTree(mTfBuffer, immediateFrameIdInZED, mMapFrameId); + //mPlaneLocInWorld = SE3::fromTfTree(mTfBuffer, immediateFrameIdInZED, mMapFrameId); - std::string immediateFrameIdInWorld = "immediatePlaneInWorld"; - SE3::pushToTfTree(mTfBroadcaster, immediateFrameIdInWorld, mMapFrameId, mPlaneLocInWorld); + //std::string immediateFrameIdInWorld = "immediatePlaneInWorld"; + //SE3::pushToTfTree(mTfBroadcaster, immediateFrameIdInWorld, mMapFrameId, mPlaneLocInWorld); mBestCenterInWorld = { static_cast(mPlaneLocInWorld.position().x()), @@ -210,6 +211,9 @@ namespace mrover { return distance; } + LanderAlignNodelet::PID::PID(float angle_P, float linear_P) : Angle_P(angle_P), Linear_P(linear_P) { + } + // auto LanderAlignNodelet::PID::calculate(Eigen::Vector3f& input, Eigen::Vector3f& target) -> std::tuple { // input[2] = 0; // target[2] = 0; @@ -234,9 +238,9 @@ namespace mrover { targetPosInWorld.z() = 0; - + ROS_INFO("Here"); PID pid(0.1, 0.1); // literally just P -- ugly class and probably needs restructuring in the future - + ROS_INFO("Here"); ros::Rate rate(20); // ROS Rate at 20Hz while (ros::ok()) { rover = SE3::fromTfTree(mTfBuffer, "map", "base_link"); From 80cd9ed7ae56d7cd091076b9a7a0e23a51fbeae6 Mon Sep 17 00:00:00 2001 From: Julian Skifstad Date: Tue, 13 Feb 2024 20:03:20 -0500 Subject: [PATCH 29/89] ransac plane normals don't make sense, randomly started freezing --- .../lander_align/lander_align.cpp | 36 ++++++++++++------- .../lander_align/lander_align.hpp | 2 +- 2 files changed, 24 insertions(+), 14 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 5f9a8939b..582a6f2e5 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -10,8 +10,10 @@ #include #include #include +#include #include #include +#include #include #include @@ -24,7 +26,7 @@ namespace mrover { auto LanderAlignNodelet::onInit() -> void { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mThreshold = 10; + mZThreshold = .2; mVectorSub = mNh.subscribe("/camera/left/points", 1, &LanderAlignNodelet::LanderCallback, this); mDebugVectorPub = mNh.advertise("/lander_align/Pose", 1); mTwistPub = mNh.advertise("/cmd_vel", 1); @@ -37,7 +39,7 @@ namespace mrover { void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { filterNormals(cloud); - ransac(0.2, 10, 100); + ransac(0.05, 10, 100); if (mBestNormal.has_value()) { geometry_msgs::Vector3 vect; vect.x = mBestNormal.value().x(); @@ -46,7 +48,7 @@ namespace mrover { mDebugVectorPub.publish(vect); } - //sendTwist({1, 1, 0}); + // sendTwist({1, 1, 0});> } // deprecated/not needed anymore @@ -63,17 +65,20 @@ namespace mrover { // TODO: OPTIMIZE; doing this maobject_detector/debug_imgny push_back calls could slow things down mFilteredPoints.clear(); auto* cloudData = reinterpret_cast(cloud->data.data()); + ROS_INFO("Point cloud size: %i", cloud->height * cloud->width); // define randomizer std::default_random_engine generator; - std::uniform_int_distribution distribution(0, (int) 200); + std::uniform_int_distribution distribution(0, (int) 10); for (auto point = cloudData; point < cloudData + (cloud->height * cloud->width); point += distribution(generator)) { bool isPointInvalid = (!std::isfinite(point->x) || !std::isfinite(point->y) || !std::isfinite(point->z)); - if (point->normal_z < mThreshold && !isPointInvalid) { + if (abs(point->normal_z) < mZThreshold && !isPointInvalid) { mFilteredPoints.push_back(point); + // ROS_INFO("Filtered point: %f, %f, %f", point->normal_x, point->normal_y, point->normal_z); } } + ROS_INFO("Filtered vector size: %lu", mFilteredPoints.size()); } void LanderAlignNodelet::ransac(float const distanceThreshold, int minInliers, int const epochs) { @@ -131,7 +136,7 @@ namespace mrover { // update best plane if better inlier count if (numInliers > minInliers) { minInliers = numInliers; - mBestNormal.value() = normal.normalized(); + mBestNormal.value() = normal; // If this is the new best plane, set mBestCenterInZED // mBestCenterInZED = currentCenter / static_cast(minInliers); @@ -161,7 +166,7 @@ namespace mrover { mBestCenterInZED.value() /= static_cast(numInliers); - if (mBestNormal.value().x() < 0) { + if (mBestNormal.value().x() < 0) { // ALSO NEED TO FLIP THE Y VALUE mBestNormal.value() *= -1; } @@ -172,15 +177,18 @@ namespace mrover { std::string immediateFrameIdInZED = "immediatePlaneInZED"; SE3::pushToTfTree(mTfBroadcaster, immediateFrameIdInZED, mCameraFrameId, mPlaneLocInZED); - //mPlaneLocInWorld = SE3::fromTfTree(mTfBuffer, immediateFrameIdInZED, mMapFrameId); + ros::Duration(.1).sleep(); // THIS IS INCREDIBLY FUCKED please actually do math :) + mPlaneLocInWorld = SE3::fromTfTree(mTfBuffer, immediateFrameIdInZED, mMapFrameId); - //std::string immediateFrameIdInWorld = "immediatePlaneInWorld"; - //SE3::pushToTfTree(mTfBroadcaster, immediateFrameIdInWorld, mMapFrameId, mPlaneLocInWorld); + std::string immediateFrameIdInWorld = "immediatePlaneInWorld"; + SE3::pushToTfTree(mTfBroadcaster, immediateFrameIdInWorld, mMapFrameId, mPlaneLocInWorld); mBestCenterInWorld = { static_cast(mPlaneLocInWorld.position().x()), static_cast(mPlaneLocInWorld.position().y()), static_cast(mPlaneLocInWorld.position().z())}; + + ROS_INFO("Max inliers: %i", minInliers); ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestCenterInZED.value().x(), mBestCenterInZED.value().y(), mBestCenterInZED.value().z(), mBestNormal.value().x(), mBestNormal.value().y(), mBestNormal.value().z()); } @@ -258,7 +266,7 @@ namespace mrover { if (abs(angle) < angular_thresh) { mLoopState = RTRSTATE::drive; } - ROS_INFO("In state: turning to point..."); + // ROS_INFO("In state: turning to point..."); } case RTRSTATE::drive: { @@ -270,7 +278,7 @@ namespace mrover { if (abs(distance) < linear_thresh) { mLoopState = RTRSTATE::turn2; } - ROS_INFO("In state: driving to point..."); + // ROS_INFO("In state: driving to point..."); } case RTRSTATE::turn2: { @@ -284,12 +292,14 @@ namespace mrover { if (abs(angle) < angular_thresh) { mLoopState = RTRSTATE::done; } - ROS_INFO("In state: turning to lander..."); + // ROS_INFO("In state: turning to lander..."); } case RTRSTATE::done: break; } + ROS_INFO("THE TWIST IS: Angular: %f, with linear %f,", twist.angular.z, twist.linear.x); + mTwistPub.publish(twist); rate.sleep(); diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 94a2fbee1..56a840e05 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -45,7 +45,7 @@ namespace mrover { std::string mCameraFrameId; std::string mMapFrameId; - float mThreshold; + float mZThreshold; std::vector mFilteredPoints; From 9713e9d0d66b2897733e8b7c0538ec0705101f3d Mon Sep 17 00:00:00 2001 From: Julian Skifstad Date: Thu, 15 Feb 2024 20:32:30 -0500 Subject: [PATCH 30/89] Fixed a couple of bugs with ransac, but added a bunch of issues trying to convert types --- .../lander_align/lander_align.cpp | 80 +++++++++++-------- .../lander_align/lander_align.hpp | 16 ++-- 2 files changed, 58 insertions(+), 38 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 582a6f2e5..710d784cc 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,6 +1,7 @@ #include "lander_align.hpp" #include "pch.hpp" #include +#include #include #include #include @@ -40,15 +41,14 @@ namespace mrover { void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { filterNormals(cloud); ransac(0.05, 10, 100); - if (mBestNormal.has_value()) { + if (mBestNormalInZED.has_value()) { geometry_msgs::Vector3 vect; - vect.x = mBestNormal.value().x(); - vect.y = mBestNormal.value().y(); - vect.z = mBestNormal.value().z(); + vect.x = mBestNormalInZED.value().x(); + vect.y = mBestNormalInZED.value().y(); + vect.z = mBestNormalInZED.value().z(); mDebugVectorPub.publish(vect); + sendTwist(10.0); } - - // sendTwist({1, 1, 0});> } // deprecated/not needed anymore @@ -93,16 +93,21 @@ namespace mrover { std::uniform_int_distribution distribution(0, (int) mFilteredPoints.size() - 1); if (mFilteredPoints.size() < 3) { - mBestNormal = std::nullopt; + mBestNormalInZED = std::nullopt; mBestCenterInZED = std::nullopt; return; } - mBestNormal = std::make_optional(0, 0, 0); - mBestCenterInZED = std::make_optional(0, 0, 0); + ROS_INFO("Here1"); + + mBestNormalInZED = std::make_optional(0, 0, 0); + mBestCenterInZED = std::make_optional(0, 0, 0); + ROS_INFO("Here2"); - while (mBestNormal.value().isZero()) { // TODO add give up condition after X iter + while (mBestNormalInZED.value().isZero()) { // TODO add give up condition after X iter + ROS_INFO("Here3"); + for (int i = 0; i < epochs; ++i) { // currentCenter *= 0; // Set all vals in current center to zero at the start of each epoch // sample 3 random points (potential inliers) @@ -117,9 +122,13 @@ namespace mrover { // fit a plane to these points Eigen::Vector3f normal = (vec1 - vec2).cross(vec1 - vec3).normalized(); offset = -normal.dot(vec1); // calculate offset (D value) using one of the points + ROS_INFO("Here4"); int numInliers = 0; + // assert(normal.x() != 0 && normal.y() != 0 && normal.z() != 0); + // In some situations we get the 0 vector with surprising frequency + for (auto p: mFilteredPoints) { // calculate distance of each point from potential plane float distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); @@ -134,21 +143,23 @@ namespace mrover { } // update best plane if better inlier count - if (numInliers > minInliers) { + if (numInliers > minInliers && normal.x() != 0 && normal.y() != 0 && normal.z() != 0) { minInliers = numInliers; - mBestNormal.value() = normal; + mBestNormalInZED.value() = normal; // If this is the new best plane, set mBestCenterInZED // mBestCenterInZED = currentCenter / static_cast(minInliers); } } } + ROS_INFO("Out of zero loop"); + // Run through one more loop to identify the center of the plane (one possibility for determining best center) int numInliers = 0; for (auto p: mFilteredPoints) { // calculate distance of each point from potential plane - float distance = std::abs(mBestNormal.value().x() * p->x + mBestNormal.value().y() * p->y + mBestNormal.value().z() * p->z + mBestOffset); + double distance = std::abs(mBestNormalInZED.value().x() * p->x + mBestNormalInZED.value().y() * p->y + mBestNormalInZED.value().z() * p->z + mBestOffset); if (distance < distanceThreshold) { mBestCenterInZED.value().x() += p->x; @@ -159,37 +170,42 @@ namespace mrover { } if (numInliers == 0) { - mBestNormal = std::nullopt; + mBestNormalInZED = std::nullopt; mBestCenterInZED = std::nullopt; return; } mBestCenterInZED.value() /= static_cast(numInliers); - if (mBestNormal.value().x() < 0) { // ALSO NEED TO FLIP THE Y VALUE - mBestNormal.value() *= -1; + if (mBestNormalInZED.value().x() < 0) { // ALSO NEED TO FLIP THE Y VALUE + mBestNormalInZED.value() *= -1; } - mPlaneLocInZED = { - {mBestCenterInZED.value().x(), mBestCenterInZED.value().y(), mBestCenterInZED.value().z()}, - {}}; + mPlaneLocInZED = + {mBestCenterInZED.value().x(), mBestCenterInZED.value().y(), mBestCenterInZED.value().z()}; + + // std::string immediateFrameIdInZED = "immediatePlaneInZED"; + // SE3::pushToTfTree(mTfBroadcaster, immediateFrameIdInZED, mCameraFrameId, mPlaneLocInZED); + + // ros::Duration(.1).sleep(); // THIS IS INCREDIBLY FUCKED please actually do math :) + // mPlaneLocInWorld = SE3::fromTfTree(mTfBuffer, immediateFrameIdInZED, mMapFrameId); + + // std::string immediateFrameIdInWorld = "immediatePlaneInWorld"; + // SE3::pushToTfTree(mTfBroadcaster, immediateFrameIdInWorld, mMapFrameId, mPlaneLocInWorld); - std::string immediateFrameIdInZED = "immediatePlaneInZED"; - SE3::pushToTfTree(mTfBroadcaster, immediateFrameIdInZED, mCameraFrameId, mPlaneLocInZED); + SE3 zedToMap = SE3::fromTfTree(mTfBuffer, mCameraFrameId, mMapFrameId); - ros::Duration(.1).sleep(); // THIS IS INCREDIBLY FUCKED please actually do math :) - mPlaneLocInWorld = SE3::fromTfTree(mTfBuffer, immediateFrameIdInZED, mMapFrameId); - std::string immediateFrameIdInWorld = "immediatePlaneInWorld"; - SE3::pushToTfTree(mTfBroadcaster, immediateFrameIdInWorld, mMapFrameId, mPlaneLocInWorld); + mBestNormalInWorld.value() = zedToMap.rotation().matrix() * mBestNormalInZED.value(); + mPlaneLocInWorld = zedToMap.rotation().matrix() * mBestNormalInZED.value() + zedToMap.position().matrix() * mPlaneLocInZED; mBestCenterInWorld = { - static_cast(mPlaneLocInWorld.position().x()), - static_cast(mPlaneLocInWorld.position().y()), - static_cast(mPlaneLocInWorld.position().z())}; + static_cast(mPlaneLocInWorld.x()), + static_cast(mPlaneLocInWorld.y()), + static_cast(mPlaneLocInWorld.z())}; ROS_INFO("Max inliers: %i", minInliers); - ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestCenterInZED.value().x(), mBestCenterInZED.value().y(), mBestCenterInZED.value().z(), mBestNormal.value().x(), mBestNormal.value().y(), mBestNormal.value().z()); + ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestCenterInZED.value().x(), mBestCenterInZED.value().y(), mBestCenterInZED.value().z(), mBestNormalInZED.value().x(), mBestNormalInZED.value().y(), mBestNormalInZED.value().z()); } auto LanderAlignNodelet::PID::rotate_speed(float theta) const -> float { @@ -231,12 +247,12 @@ namespace mrover { // } - void LanderAlignNodelet::sendTwist(Eigen::Vector3f const& offset) { + void LanderAlignNodelet::sendTwist(float const& offset) { //Locations SE3 rover; Eigen::Vector3f rover_dir; - Eigen::Vector3f targetPosInWorld = mBestCenterInWorld.value() - offset; + Eigen::Vector3f targetPosInWorld = mBestCenterInWorld.value() + mBestNormalInWorld.value() * offset; // I don't trust subtracting this //Final msg geometry_msgs::Twist twist; @@ -284,7 +300,7 @@ namespace mrover { case RTRSTATE::turn2: { rover_dir = {static_cast(rover.rotation().matrix().col(0).x()), static_cast(rover.rotation().matrix().col(0).y()), 0}; rover_dir[2] = 0; - float angle = pid.find_angle(rover_dir, -mBestNormal.value()); // normal is pointing "towards" tehe rover, so we need to flip it to find angle + float angle = pid.find_angle(rover_dir, -mBestNormalInWorld.value()); // normal is pointing "towards" the rover, so we need to flip it to find angle float angle_rate = pid.rotate_speed(angle); twist.angular.z = angle_rate; diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 56a840e05..c5d49e92d 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -29,13 +29,17 @@ namespace mrover { //** float mBestOffset; - std::optional mBestCenterInZED; - std::optional mBestCenterInWorld; + std::optional mBestCenterInZED; + std::optional mBestCenterInWorld; - std::optional mBestNormal; + std::optional mBestNormalInZED; + std::optional mBestNormalInWorld; - SE3 mPlaneLocInZED; - SE3 mPlaneLocInWorld; + + // SE3 mPlaneLocInZED; + // SE3 mPlaneLocInWorld; + Eigen::Vector3d mPlaneLocInZED; + Eigen::Vector3d mPlaneLocInWorld; //** //TF Member Variables @@ -60,7 +64,7 @@ namespace mrover { void ransac(float distanceThreshold, int minInliers, int epochs); - void sendTwist(Eigen::Vector3f const& offset); + void sendTwist(float const& offset); class PID { private: From 197f3df21f6663f8dea11283593b225dd6205991 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 18 Feb 2024 15:04:26 -0500 Subject: [PATCH 31/89] Why is the location weird --- .../lander_align/lander_align.cpp | 99 ++++++++----------- .../lander_align/lander_align.hpp | 16 +-- src/teleoperation/lander_align/pch.hpp | 2 +- 3 files changed, 49 insertions(+), 68 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 710d784cc..daf8191db 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,4 +1,5 @@ #include "lander_align.hpp" +#include "lie.hpp" #include "pch.hpp" #include #include @@ -7,8 +8,7 @@ #include #include -#include -#include +#include #include #include #include @@ -17,7 +17,6 @@ #include #include -#include #include #include @@ -27,7 +26,7 @@ namespace mrover { auto LanderAlignNodelet::onInit() -> void { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mZThreshold = .2; + mZThreshold = .9; mVectorSub = mNh.subscribe("/camera/left/points", 1, &LanderAlignNodelet::LanderCallback, this); mDebugVectorPub = mNh.advertise("/lander_align/Pose", 1); mTwistPub = mNh.advertise("/cmd_vel", 1); @@ -36,18 +35,21 @@ namespace mrover { //TF Create the Reference Frames mNh.param("camera_frame", mCameraFrameId, "zed2i_left_camera_frame"); mNh.param("world_frame", mMapFrameId, "map"); + + mBestLocationInWorld = std::make_optional(0, 0, 0); + mBestNormalInWorld = std::make_optional(0, 0, 0); } void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { filterNormals(cloud); - ransac(0.05, 10, 100); + ransac(0.2, 10, 100); if (mBestNormalInZED.has_value()) { geometry_msgs::Vector3 vect; vect.x = mBestNormalInZED.value().x(); vect.y = mBestNormalInZED.value().y(); vect.z = mBestNormalInZED.value().z(); mDebugVectorPub.publish(vect); - sendTwist(10.0); + //sendTwist(10.0); } } @@ -84,8 +86,8 @@ namespace mrover { void LanderAlignNodelet::ransac(float const distanceThreshold, int minInliers, int const epochs) { // TODO: use RANSAC to find the lander face, should be the closest, we may need to modify this to output more information, currently the output is the normal // takes 3 samples for every epoch and terminates after specified number of epochs - // Eigen::Vector3f mBestNormal(0, 0, 0); // normal vector representing plane (initialize as zero vector?? default ctor??) - // Eigen::Vector3f currentCenter(0, 0, 0); // Keeps track of current center of plane in current epoch + // Eigen::Vector3d mBestNormal(0, 0, 0); // normal vector representing plane (initialize as zero vector?? default ctor??) + // Eigen::Vector3d currentCenter(0, 0, 0); // Keeps track of current center of plane in current epoch float offset; // define randomizer @@ -94,20 +96,16 @@ namespace mrover { if (mFilteredPoints.size() < 3) { mBestNormalInZED = std::nullopt; - mBestCenterInZED = std::nullopt; + mBestLocationInZED = std::nullopt; return; } - ROS_INFO("Here1"); mBestNormalInZED = std::make_optional(0, 0, 0); - mBestCenterInZED = std::make_optional(0, 0, 0); - ROS_INFO("Here2"); + mBestLocationInZED = std::make_optional(0, 0, 0); while (mBestNormalInZED.value().isZero()) { // TODO add give up condition after X iter - ROS_INFO("Here3"); - for (int i = 0; i < epochs; ++i) { // currentCenter *= 0; // Set all vals in current center to zero at the start of each epoch // sample 3 random points (potential inliers) @@ -115,14 +113,13 @@ namespace mrover { Point const* point2 = mFilteredPoints[distribution(generator)]; Point const* point3 = mFilteredPoints[distribution(generator)]; - Eigen::Vector3f vec1{point1->x, point1->y, point1->z}; - Eigen::Vector3f vec2{point2->x, point2->y, point2->z}; - Eigen::Vector3f vec3{point3->x, point3->y, point3->z}; + Eigen::Vector3d vec1{point1->x, point1->y, point1->z}; + Eigen::Vector3d vec2{point2->x, point2->y, point2->z}; + Eigen::Vector3d vec3{point3->x, point3->y, point3->z}; // fit a plane to these points - Eigen::Vector3f normal = (vec1 - vec2).cross(vec1 - vec3).normalized(); + Eigen::Vector3d normal = (vec1 - vec2).cross(vec1 - vec3).normalized(); offset = -normal.dot(vec1); // calculate offset (D value) using one of the points - ROS_INFO("Here4"); int numInliers = 0; @@ -162,50 +159,42 @@ namespace mrover { double distance = std::abs(mBestNormalInZED.value().x() * p->x + mBestNormalInZED.value().y() * p->y + mBestNormalInZED.value().z() * p->z + mBestOffset); if (distance < distanceThreshold) { - mBestCenterInZED.value().x() += p->x; - mBestCenterInZED.value().y() += p->y; - mBestCenterInZED.value().z() += p->z; + mBestLocationInZED.value().x() += p->x; + mBestLocationInZED.value().y() += p->y; + mBestLocationInZED.value().z() += p->z; ++numInliers; // count num of inliers that pass the "good enough fit" threshold } } if (numInliers == 0) { mBestNormalInZED = std::nullopt; - mBestCenterInZED = std::nullopt; + mBestLocationInZED = std::nullopt; return; } - mBestCenterInZED.value() /= static_cast(numInliers); + mBestLocationInZED.value() /= static_cast(numInliers); if (mBestNormalInZED.value().x() < 0) { // ALSO NEED TO FLIP THE Y VALUE - mBestNormalInZED.value() *= -1; + mBestNormalInZED.value().x() *= -1; } - mPlaneLocInZED = - {mBestCenterInZED.value().x(), mBestCenterInZED.value().y(), mBestCenterInZED.value().z()}; - - // std::string immediateFrameIdInZED = "immediatePlaneInZED"; - // SE3::pushToTfTree(mTfBroadcaster, immediateFrameIdInZED, mCameraFrameId, mPlaneLocInZED); - - // ros::Duration(.1).sleep(); // THIS IS INCREDIBLY FUCKED please actually do math :) - // mPlaneLocInWorld = SE3::fromTfTree(mTfBuffer, immediateFrameIdInZED, mMapFrameId); + if(mBestNormalInZED.value().y() < 0){ + mBestNormalInZED.value().y() *= -1; + } - // std::string immediateFrameIdInWorld = "immediatePlaneInWorld"; - // SE3::pushToTfTree(mTfBroadcaster, immediateFrameIdInWorld, mMapFrameId, mPlaneLocInWorld); + manif::SE3d mPlaneLocInZED = {{mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z()}, manif::SO3d{1,1,1}};//TODO: THIS IS A RANDOM ROTATION MATRIX - SE3 zedToMap = SE3::fromTfTree(mTfBuffer, mCameraFrameId, mMapFrameId); + manif::SE3d zedToMap = SE3Conversions::fromTfTree(mTfBuffer, mMapFrameId, mCameraFrameId); + mBestNormalInWorld = std::make_optional(zedToMap.rotation().matrix() * mBestNormalInZED.value()); + manif::SE3d planeLocationSE3 = (zedToMap * mPlaneLocInZED); + mBestLocationInWorld = std::make_optional(planeLocationSE3.translation()); - mBestNormalInWorld.value() = zedToMap.rotation().matrix() * mBestNormalInZED.value(); - mPlaneLocInWorld = zedToMap.rotation().matrix() * mBestNormalInZED.value() + zedToMap.position().matrix() * mPlaneLocInZED; + SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, planeLocationSE3); - mBestCenterInWorld = { - static_cast(mPlaneLocInWorld.x()), - static_cast(mPlaneLocInWorld.y()), - static_cast(mPlaneLocInWorld.z())}; ROS_INFO("Max inliers: %i", minInliers); - ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestCenterInZED.value().x(), mBestCenterInZED.value().y(), mBestCenterInZED.value().z(), mBestNormalInZED.value().x(), mBestNormalInZED.value().y(), mBestNormalInZED.value().z()); + ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z(), mBestNormalInZED.value().x(), mBestNormalInZED.value().y(), mBestNormalInZED.value().z()); } auto LanderAlignNodelet::PID::rotate_speed(float theta) const -> float { @@ -213,10 +202,10 @@ namespace mrover { } - auto LanderAlignNodelet::PID::find_angle(Eigen::Vector3f const& current, Eigen::Vector3f const& target) -> float { - Eigen::Vector3f u1 = current.normalized(); + auto LanderAlignNodelet::PID::find_angle(Eigen::Vector3d const& current, Eigen::Vector3d const& target) -> float { + Eigen::Vector3d u1 = current.normalized(); u1.z() = 0; - Eigen::Vector3f u2 = target.normalized(); + Eigen::Vector3d u2 = target.normalized(); u2.z() = 0; float theta = fmod(acos(u1.dot(u2)), static_cast(180)); float perp_alignment = u2[0] * -u1[1] + u2[1] * u1[0]; @@ -230,8 +219,8 @@ namespace mrover { return distance * Linear_P; } - auto LanderAlignNodelet::PID::find_distance(Eigen::Vector3f const& current, Eigen::Vector3f const& target) -> float { - Eigen::Vector3f difference = target - current; + auto LanderAlignNodelet::PID::find_distance(Eigen::Vector3d const& current, Eigen::Vector3d const& target) -> float { + Eigen::Vector3d difference = target - current; float distance = difference.norm(); return distance; } @@ -239,7 +228,7 @@ namespace mrover { LanderAlignNodelet::PID::PID(float angle_P, float linear_P) : Angle_P(angle_P), Linear_P(linear_P) { } - // auto LanderAlignNodelet::PID::calculate(Eigen::Vector3f& input, Eigen::Vector3f& target) -> std::tuple { + // auto LanderAlignNodelet::PID::calculate(Eigen::Vector3d& input, Eigen::Vector3d& target) -> std::tuple { // input[2] = 0; // target[2] = 0; @@ -250,9 +239,9 @@ namespace mrover { void LanderAlignNodelet::sendTwist(float const& offset) { //Locations - SE3 rover; - Eigen::Vector3f rover_dir; - Eigen::Vector3f targetPosInWorld = mBestCenterInWorld.value() + mBestNormalInWorld.value() * offset; // I don't trust subtracting this + manif::SE3d rover; + Eigen::Vector3d rover_dir; + Eigen::Vector3d targetPosInWorld = mBestLocationInWorld.value() + mBestNormalInWorld.value() * offset; // I don't trust subtracting this //Final msg geometry_msgs::Twist twist; @@ -263,13 +252,11 @@ namespace mrover { targetPosInWorld.z() = 0; - ROS_INFO("Here"); PID pid(0.1, 0.1); // literally just P -- ugly class and probably needs restructuring in the future - ROS_INFO("Here"); ros::Rate rate(20); // ROS Rate at 20Hz while (ros::ok()) { - rover = SE3::fromTfTree(mTfBuffer, "map", "base_link"); - Eigen::Vector3f roverPosInWorld{static_cast(rover.position().x()), static_cast(rover.position().y()), 0}; + rover = SE3Conversions::fromTfTree(mTfBuffer, "map", "base_link"); + Eigen::Vector3d roverPosInWorld{static_cast(rover.translation().x()), static_cast(rover.translation().y()), 0}; switch (mLoopState) { case RTRSTATE::turn1: { diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index c5d49e92d..6d8f7a842 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -29,17 +29,11 @@ namespace mrover { //** float mBestOffset; - std::optional mBestCenterInZED; - std::optional mBestCenterInWorld; + std::optional mBestLocationInZED; + std::optional mBestLocationInWorld; std::optional mBestNormalInZED; std::optional mBestNormalInWorld; - - - // SE3 mPlaneLocInZED; - // SE3 mPlaneLocInWorld; - Eigen::Vector3d mPlaneLocInZED; - Eigen::Vector3d mPlaneLocInWorld; //** //TF Member Variables @@ -75,15 +69,15 @@ namespace mrover { public: PID(float angle_P, float linear_P); - // auto calculate(Eigen::Vector3f input, Eigen::Vector3f target) -> float; + // auto calculate(Eigen::Vector3d input, Eigen::Vector3d target) -> float; [[nodiscard]] auto rotate_speed(float theta) const -> float; - auto find_angle(Eigen::Vector3f const& current, Eigen::Vector3f const& target) -> float; + auto find_angle(Eigen::Vector3d const& current, Eigen::Vector3d const& target) -> float; - auto find_distance(Eigen::Vector3f const& current, Eigen::Vector3f const& target) -> float; + auto find_distance(Eigen::Vector3d const& current, Eigen::Vector3d const& target) -> float; auto drive_speed(float) -> float; }; diff --git a/src/teleoperation/lander_align/pch.hpp b/src/teleoperation/lander_align/pch.hpp index 054ba78c2..221b30aad 100644 --- a/src/teleoperation/lander_align/pch.hpp +++ b/src/teleoperation/lander_align/pch.hpp @@ -14,7 +14,7 @@ #include #include -#include "lie/se3.hpp" +#include "lie.hpp" #include From 3c5c9566fb40807ac3ff14e851ebfd81a07b83a6 Mon Sep 17 00:00:00 2001 From: Joshua Huang Date: Tue, 20 Feb 2024 19:06:39 -0500 Subject: [PATCH 32/89] Update cmake for action files --- CMakeLists.txt | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 45bd1a95d..d00d6f70f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,15 +54,19 @@ set(MROVER_ROS_DEPENDENCIES tf2 tf2_ros tf2_geometry_msgs + actionlib_msgs ) extract_filenames(msg/*.msg MROVER_MESSAGE_FILES) extract_filenames(srv/*.srv MROVER_SERVICE_FILES) +extract_filenames(action/*.action MROVER_ACTION_FILES) + set(MROVER_MESSAGE_DEPENDENCIES std_msgs sensor_msgs + actionlib_msgs ) set(MROVER_PARAMETERS @@ -123,13 +127,15 @@ endforeach () set(BUILD_SHARED_LIBS ON) ### ======== ### -### Messages ### +### Messages & Actions & Reconfigure ### ### ======== ### add_message_files(FILES ${MROVER_MESSAGE_FILES}) add_service_files(FILES ${MROVER_SERVICE_FILES}) +add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) + generate_messages(DEPENDENCIES ${MROVER_MESSAGE_DEPENDENCIES}) generate_dynamic_reconfigure_options(${MROVER_PARAMETERS}) From affc9155ea1d5ca416c70a05548be7371383d2d5 Mon Sep 17 00:00:00 2001 From: Julian Skifstad Date: Tue, 20 Feb 2024 20:19:21 -0500 Subject: [PATCH 33/89] normals work ransac dpeosnt;sd;dofhifoasdf --- src/simulator/shaders/shaders.wgsl | 3 +- src/simulator/simulator.hpp | 2 +- .../lander_align/lander_align.cpp | 45 ++++++++++++------- .../lander_align/lander_align.hpp | 4 +- urdf/meshes/ground.fbx | 4 +- 5 files changed, 37 insertions(+), 21 deletions(-) diff --git a/src/simulator/shaders/shaders.wgsl b/src/simulator/shaders/shaders.wgsl index d751dcc23..eb7e12a4c 100644 --- a/src/simulator/shaders/shaders.wgsl +++ b/src/simulator/shaders/shaders.wgsl @@ -64,7 +64,8 @@ struct OutFragment { let spec = pow(max(dot(viewDirInWolrd, reflectDir), 0.0), 32); let specular = specularStrength * spec * su.lightColor; // Combination - out.color = vec4(((ambient + diffuse + specular) * baseColor).rgb, 1); + // out.color = vec4(((ambient + diffuse + specular) * baseColor).rgb, 1); + out.color = vec4(in.normalInWorld.x, in.normalInWorld.y, in.normalInWorld.z, 1); return out; } diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index e37a5aae3..876000e02 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -227,7 +227,7 @@ namespace mrover { // TODO: make variances configurable std::default_random_engine mRNG; - std::normal_distribution mGPSDist{0, 0.2}, + std::normal_distribution mGPSDist{0, 0.0}, mAccelDist{0, 0.05}, mGyroDist{0, 0.02}, mMagDist{0, 0.1}, diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index daf8191db..091a8d2ba 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -26,7 +26,7 @@ namespace mrover { auto LanderAlignNodelet::onInit() -> void { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mZThreshold = .9; + mZThreshold = .1; mVectorSub = mNh.subscribe("/camera/left/points", 1, &LanderAlignNodelet::LanderCallback, this); mDebugVectorPub = mNh.advertise("/lander_align/Pose", 1); mTwistPub = mNh.advertise("/cmd_vel", 1); @@ -43,14 +43,14 @@ namespace mrover { void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { filterNormals(cloud); ransac(0.2, 10, 100); - if (mBestNormalInZED.has_value()) { - geometry_msgs::Vector3 vect; - vect.x = mBestNormalInZED.value().x(); - vect.y = mBestNormalInZED.value().y(); - vect.z = mBestNormalInZED.value().z(); - mDebugVectorPub.publish(vect); - //sendTwist(10.0); - } + // if (mBestNormalInZED.has_value()) { + // geometry_msgs::Vector3 vect; + // vect.x = mBestNormalInZED.value().x(); + // vect.y = mBestNormalInZED.value().y(); + // vect.z = mBestNormalInZED.value().z(); + // mDebugVectorPub.publish(vect); + // //sendTwist(10.0); + // } } // deprecated/not needed anymore @@ -69,18 +69,27 @@ namespace mrover { auto* cloudData = reinterpret_cast(cloud->data.data()); ROS_INFO("Point cloud size: %i", cloud->height * cloud->width); + std::vector pts = {new Point{1, 2, 3, 0, 0, 0, 0, 0.3, 0.5, 0.01, 0}, new Point{1, 2, 3, 0, 0, 0, 0, 0.3, 0.5, 0.1, 0}, new Point{1, 2, 3, 0, 0, 0, 0, 0.3, 0.5, 0.07, 0}}; + // define randomizer std::default_random_engine generator; std::uniform_int_distribution distribution(0, (int) 10); for (auto point = cloudData; point < cloudData + (cloud->height * cloud->width); point += distribution(generator)) { + // for (Point * point : pts) { bool isPointInvalid = (!std::isfinite(point->x) || !std::isfinite(point->y) || !std::isfinite(point->z)); + // ROS_INFO("Filtered point: %f, %f, %f", point->normal_x, point->normal_y, point->normal_z); + if (abs(point->normal_z) < mZThreshold && !isPointInvalid) { mFilteredPoints.push_back(point); // ROS_INFO("Filtered point: %f, %f, %f", point->normal_x, point->normal_y, point->normal_z); } } ROS_INFO("Filtered vector size: %lu", mFilteredPoints.size()); + + for (Point * p : pts) { + delete p; + } } void LanderAlignNodelet::ransac(float const distanceThreshold, int minInliers, int const epochs) { @@ -88,7 +97,7 @@ namespace mrover { // takes 3 samples for every epoch and terminates after specified number of epochs // Eigen::Vector3d mBestNormal(0, 0, 0); // normal vector representing plane (initialize as zero vector?? default ctor??) // Eigen::Vector3d currentCenter(0, 0, 0); // Keeps track of current center of plane in current epoch - float offset; + double offset; // define randomizer std::default_random_engine generator; @@ -106,6 +115,7 @@ namespace mrover { while (mBestNormalInZED.value().isZero()) { // TODO add give up condition after X iter + ROS_INFO("in zero loop"); for (int i = 0; i < epochs; ++i) { // currentCenter *= 0; // Set all vals in current center to zero at the start of each epoch // sample 3 random points (potential inliers) @@ -128,7 +138,7 @@ namespace mrover { for (auto p: mFilteredPoints) { // calculate distance of each point from potential plane - float distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); + float distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); // if (distance < distanceThreshold) { // Add points to current planes center @@ -143,6 +153,7 @@ namespace mrover { if (numInliers > minInliers && normal.x() != 0 && normal.y() != 0 && normal.z() != 0) { minInliers = numInliers; mBestNormalInZED.value() = normal; + mBestOffset = offset; // If this is the new best plane, set mBestCenterInZED // mBestCenterInZED = currentCenter / static_cast(minInliers); @@ -157,7 +168,7 @@ namespace mrover { for (auto p: mFilteredPoints) { // calculate distance of each point from potential plane double distance = std::abs(mBestNormalInZED.value().x() * p->x + mBestNormalInZED.value().y() * p->y + mBestNormalInZED.value().z() * p->z + mBestOffset); - + ROS_INFO("Distance %f", distance); if (distance < distanceThreshold) { mBestLocationInZED.value().x() += p->x; mBestLocationInZED.value().y() += p->y; @@ -167,6 +178,8 @@ namespace mrover { } if (numInliers == 0) { + ROS_INFO("zero inliers"); + mBestNormalInZED = std::nullopt; mBestLocationInZED = std::nullopt; return; @@ -182,6 +195,8 @@ namespace mrover { mBestNormalInZED.value().y() *= -1; } + ROS_INFO("here"); + manif::SE3d mPlaneLocInZED = {{mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z()}, manif::SO3d{1,1,1}};//TODO: THIS IS A RANDOM ROTATION MATRIX manif::SE3d zedToMap = SE3Conversions::fromTfTree(mTfBuffer, mMapFrameId, mCameraFrameId); @@ -219,9 +234,9 @@ namespace mrover { return distance * Linear_P; } - auto LanderAlignNodelet::PID::find_distance(Eigen::Vector3d const& current, Eigen::Vector3d const& target) -> float { + auto LanderAlignNodelet::PID::find_distance(Eigen::Vector3d const& current, Eigen::Vector3d const& target) -> double { Eigen::Vector3d difference = target - current; - float distance = difference.norm(); + double distance = difference.norm(); return distance; } @@ -241,7 +256,7 @@ namespace mrover { //Locations manif::SE3d rover; Eigen::Vector3d rover_dir; - Eigen::Vector3d targetPosInWorld = mBestLocationInWorld.value() + mBestNormalInWorld.value() * offset; // I don't trust subtracting this + Eigen::Vector3d targetPosInWorld = mBestLocationInWorld.value() + mBestNormalInWorld.value() * offset; //Final msg geometry_msgs::Twist twist; diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 6d8f7a842..84ec70397 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -27,7 +27,7 @@ namespace mrover { RTRSTATE mLoopState; //** - float mBestOffset; + double mBestOffset; std::optional mBestLocationInZED; std::optional mBestLocationInWorld; @@ -77,7 +77,7 @@ namespace mrover { auto find_angle(Eigen::Vector3d const& current, Eigen::Vector3d const& target) -> float; - auto find_distance(Eigen::Vector3d const& current, Eigen::Vector3d const& target) -> float; + auto find_distance(Eigen::Vector3d const& current, Eigen::Vector3d const& target) -> double; auto drive_speed(float) -> float; }; 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 08fb7368a899b8fd4443460793c3cf5f6d7b410a Mon Sep 17 00:00:00 2001 From: John Date: Thu, 22 Feb 2024 18:21:34 -0500 Subject: [PATCH 34/89] pc vizualized --- CMakeLists.txt | 4 +- .../lander_align/lander_align.cpp | 80 ++++++++++++++++--- .../lander_align/lander_align.hpp | 3 + 3 files changed, 72 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d00d6f70f..5f8ef6b90 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,7 +61,7 @@ extract_filenames(msg/*.msg MROVER_MESSAGE_FILES) extract_filenames(srv/*.srv MROVER_SERVICE_FILES) -extract_filenames(action/*.action MROVER_ACTION_FILES) +#extract_filenames(action/*.action MROVER_ACTION_FILES) set(MROVER_MESSAGE_DEPENDENCIES std_msgs @@ -134,7 +134,7 @@ add_message_files(FILES ${MROVER_MESSAGE_FILES}) add_service_files(FILES ${MROVER_SERVICE_FILES}) -add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) +#add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) generate_messages(DEPENDENCIES ${MROVER_MESSAGE_DEPENDENCIES}) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 091a8d2ba..7d06f62e3 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -5,14 +5,21 @@ #include #include #include +#include +#include #include #include #include +#include +#include #include +#include #include #include #include +#include +#include #include #include #include @@ -30,6 +37,7 @@ namespace mrover { mVectorSub = mNh.subscribe("/camera/left/points", 1, &LanderAlignNodelet::LanderCallback, this); mDebugVectorPub = mNh.advertise("/lander_align/Pose", 1); mTwistPub = mNh.advertise("/cmd_vel", 1); + mDebugPCPub = mNh.advertise("/lander_align/debugPC", 1); //TF Create the Reference Frames @@ -113,7 +121,7 @@ namespace mrover { mBestNormalInZED = std::make_optional(0, 0, 0); mBestLocationInZED = std::make_optional(0, 0, 0); - + int numInliers = 0; while (mBestNormalInZED.value().isZero()) { // TODO add give up condition after X iter ROS_INFO("in zero loop"); for (int i = 0; i < epochs; ++i) { @@ -131,7 +139,7 @@ namespace mrover { Eigen::Vector3d normal = (vec1 - vec2).cross(vec1 - vec3).normalized(); offset = -normal.dot(vec1); // calculate offset (D value) using one of the points - int numInliers = 0; + numInliers = 0; // assert(normal.x() != 0 && normal.y() != 0 && normal.z() != 0); // In some situations we get the 0 vector with surprising frequency @@ -162,13 +170,13 @@ namespace mrover { } ROS_INFO("Out of zero loop"); + // Run through one more loop to identify the center of the plane (one possibility for determining best center) - int numInliers = 0; + numInliers = 0; for (auto p: mFilteredPoints) { // calculate distance of each point from potential plane double distance = std::abs(mBestNormalInZED.value().x() * p->x + mBestNormalInZED.value().y() * p->y + mBestNormalInZED.value().z() * p->z + mBestOffset); - ROS_INFO("Distance %f", distance); if (distance < distanceThreshold) { mBestLocationInZED.value().x() += p->x; mBestLocationInZED.value().y() += p->y; @@ -176,6 +184,37 @@ namespace mrover { ++numInliers; // count num of inliers that pass the "good enough fit" threshold } } + //Average pnts + mBestLocationInZED.value() /= static_cast(numInliers); + + + auto debugPointCloudPtr = boost::make_shared(); + fillPointCloudMessageHeader(debugPointCloudPtr); + debugPointCloudPtr->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; + debugPointCloudPtr->is_dense = true; + debugPointCloudPtr->height = 1; + debugPointCloudPtr->width = numInliers; + debugPointCloudPtr->header.seq = 0; + debugPointCloudPtr->header.stamp = ros::Time(); + debugPointCloudPtr->header.frame_id = "zed2i_left_camera_frame"; + debugPointCloudPtr->data.resize((numInliers * sizeof(Point))/sizeof(uchar)); + auto pcPtr = reinterpret_cast(debugPointCloudPtr->data.data()); + size_t i = 0; + for (auto p: mFilteredPoints) { + // calculate distance of each point from potential plane + double distance = std::abs(mBestNormalInZED.value().x() * p->x + mBestNormalInZED.value().y() * p->y + mBestNormalInZED.value().z() * p->z + mBestOffset); + if (distance < distanceThreshold) { + pcPtr[i].x = p->x; + pcPtr[i].y = p->y; + pcPtr[i].z = p->z; + pcPtr[i].b = p->r; + pcPtr[i].g = p->g; + pcPtr[i].r = p->b; + pcPtr[i].a = p->a; + ++i; + } + } + mDebugPCPub.publish(debugPointCloudPtr); if (numInliers == 0) { ROS_INFO("zero inliers"); @@ -185,19 +224,34 @@ namespace mrover { return; } - mBestLocationInZED.value() /= static_cast(numInliers); - if (mBestNormalInZED.value().x() < 0) { // ALSO NEED TO FLIP THE Y VALUE - mBestNormalInZED.value().x() *= -1; - } + // if (mBestNormalInZED.value().x() < 0) { // ALSO NEED TO FLIP THE Y VALUE + // mBestNormalInZED.value().x() *= -1; + // } - if(mBestNormalInZED.value().y() < 0){ - mBestNormalInZED.value().y() *= -1; - } + // if(mBestNormalInZED.value().y() < 0){ + // mBestNormalInZED.value().y() *= -1; + // } - ROS_INFO("here"); + //Calculate the other three rotation vectors + Eigen::Matrix3d rot; + { + if(mBestNormalInZED.value().x() > 0) mBestNormalInZED.value() *=-1; + + Eigen::Vector3d x; + Eigen::Vector3d y; + Eigen::Vector3d z; + Eigen::Vector3d dummy{mBestNormalInZED->x()+1,mBestLocationInZED->y(),mBestNormalInZED->z()}; + x = dummy.cross(mBestNormalInZED.value()).normalized(); + y = x.cross(mBestNormalInZED.value()).normalized(); + z = mBestNormalInZED.value().normalized(); + rot << x.x(),y.x(),z.x(), + x.y(),y.y(),z.y(), + x.z(),y.z(),z.z(); + std::cout << "rot matrix " << rot << std::endl; + } - manif::SE3d mPlaneLocInZED = {{mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z()}, manif::SO3d{1,1,1}};//TODO: THIS IS A RANDOM ROTATION MATRIX + manif::SE3d mPlaneLocInZED = {{mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};//TODO: THIS IS A RANDOM ROTATION MATRIX manif::SE3d zedToMap = SE3Conversions::fromTfTree(mTfBuffer, mMapFrameId, mCameraFrameId); diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 84ec70397..6c3a5302f 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -19,6 +19,9 @@ namespace mrover { ros::Publisher mDebugVectorPub; + ros::Publisher mDebugPCPub; + + ros::Publisher mTwistPub; From 3ace1d65173e2529a2514fa2ff8312909f301564 Mon Sep 17 00:00:00 2001 From: Joshua Huang Date: Thu, 22 Feb 2024 18:35:18 -0500 Subject: [PATCH 35/89] Fix action build --- action/LanderAlign.action | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 action/LanderAlign.action diff --git a/action/LanderAlign.action b/action/LanderAlign.action new file mode 100644 index 000000000..c1024312e --- /dev/null +++ b/action/LanderAlign.action @@ -0,0 +1,4 @@ +float32 rotateSpeed +float32 driveSpeed +--- +--- From 7b4e884af15ca57f7334a7a04c4a04691ca1c5e9 Mon Sep 17 00:00:00 2001 From: Joshua Huang Date: Thu, 22 Feb 2024 19:11:36 -0500 Subject: [PATCH 36/89] Uncomment action configs in CMakeLists.txt --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5f8ef6b90..d00d6f70f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,7 +61,7 @@ extract_filenames(msg/*.msg MROVER_MESSAGE_FILES) extract_filenames(srv/*.srv MROVER_SERVICE_FILES) -#extract_filenames(action/*.action MROVER_ACTION_FILES) +extract_filenames(action/*.action MROVER_ACTION_FILES) set(MROVER_MESSAGE_DEPENDENCIES std_msgs @@ -134,7 +134,7 @@ add_message_files(FILES ${MROVER_MESSAGE_FILES}) add_service_files(FILES ${MROVER_SERVICE_FILES}) -#add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) +add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) generate_messages(DEPENDENCIES ${MROVER_MESSAGE_DEPENDENCIES}) From 60f37474ebd6606c3c23e559b48d0eb153efc72f Mon Sep 17 00:00:00 2001 From: Julian Skifstad Date: Thu, 22 Feb 2024 20:43:32 -0500 Subject: [PATCH 37/89] qrcooking --- src/simulator/shaders/shaders.wgsl | 4 +-- .../lander_align/lander_align.cpp | 31 ++++++++++++------- 2 files changed, 21 insertions(+), 14 deletions(-) diff --git a/src/simulator/shaders/shaders.wgsl b/src/simulator/shaders/shaders.wgsl index eb7e12a4c..02b8c2062 100644 --- a/src/simulator/shaders/shaders.wgsl +++ b/src/simulator/shaders/shaders.wgsl @@ -64,8 +64,8 @@ struct OutFragment { let spec = pow(max(dot(viewDirInWolrd, reflectDir), 0.0), 32); let specular = specularStrength * spec * su.lightColor; // Combination - // out.color = vec4(((ambient + diffuse + specular) * baseColor).rgb, 1); - out.color = vec4(in.normalInWorld.x, in.normalInWorld.y, in.normalInWorld.z, 1); + out.color = vec4(((ambient + diffuse + specular) * baseColor).rgb, 1); + // out.color = vec4(in.normalInWorld.x, in.normalInWorld.y, in.normalInWorld.z, 1); return out; } diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 7d06f62e3..06d8bac1a 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -27,6 +27,7 @@ #include #include +#include namespace mrover { @@ -238,22 +239,28 @@ namespace mrover { { if(mBestNormalInZED.value().x() > 0) mBestNormalInZED.value() *=-1; - Eigen::Vector3d x; - Eigen::Vector3d y; - Eigen::Vector3d z; - Eigen::Vector3d dummy{mBestNormalInZED->x()+1,mBestLocationInZED->y(),mBestNormalInZED->z()}; - x = dummy.cross(mBestNormalInZED.value()).normalized(); - y = x.cross(mBestNormalInZED.value()).normalized(); - z = mBestNormalInZED.value().normalized(); - rot << x.x(),y.x(),z.x(), - x.y(),y.y(),z.y(), - x.z(),y.z(),z.z(); + // Eigen::Vector3d x; + // Eigen::Vector3d y; + // Eigen::Vector3d z; + // Eigen::Vector3d dummy{mBestNormalInZED->x()+1,mBestLocationInZED->y(),mBestNormalInZED->z()}; + Eigen::Vector3d n = mBestNormalInZED.value().normalized(); + // y = dummy.cross(mBestNormalInZED.value()).normalized(); + // z = x.cross(mBestNormalInZED.value()).normalized(); + rot << n.x(),0,0, + n.y(),0,1, + n.z(),1,0; std::cout << "rot matrix " << rot << std::endl; + + Eigen::HouseholderQR qr{rot}; + Eigen::Matrix3d q = qr.householderQ(); + rot.col(0) = q.col(0); + rot.col(1) = q.col(2); + rot.col(2) = q.col(1); } manif::SE3d mPlaneLocInZED = {{mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};//TODO: THIS IS A RANDOM ROTATION MATRIX - manif::SE3d zedToMap = SE3Conversions::fromTfTree(mTfBuffer, mMapFrameId, mCameraFrameId); + manif::SE3d zedToMap = SE3Conversions::fromTfTree(mTfBuffer, mCameraFrameId, mMapFrameId); mBestNormalInWorld = std::make_optional(zedToMap.rotation().matrix() * mBestNormalInZED.value()); manif::SE3d planeLocationSE3 = (zedToMap * mPlaneLocInZED); @@ -324,7 +331,7 @@ namespace mrover { PID pid(0.1, 0.1); // literally just P -- ugly class and probably needs restructuring in the future ros::Rate rate(20); // ROS Rate at 20Hz while (ros::ok()) { - rover = SE3Conversions::fromTfTree(mTfBuffer, "map", "base_link"); + rover = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); Eigen::Vector3d roverPosInWorld{static_cast(rover.translation().x()), static_cast(rover.translation().y()), 0}; switch (mLoopState) { From 0ac0792ac013ceb19623f96fe418e7bee04fda38 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 5 Mar 2024 17:23:03 -0500 Subject: [PATCH 38/89] Clean up and fix build errors --- .../lander_align/lander_align.cpp | 100 +++++++----------- .../lander_align/lander_align.hpp | 9 +- 2 files changed, 42 insertions(+), 67 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 06d8bac1a..ea0e5fc5c 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -62,46 +62,56 @@ namespace mrover { // } } - // deprecated/not needed anymore - // auto LanderAlignNodelet::downsample(sensor_msgs::PointCloud2Ptr const& cloud) -> sensor_msgs::PointCloud2Ptr { - // TODO: make a new point cloud with half the resolution, check the zed wrapper for how to make one - // sensor_msgs::PointCloud2 cloudSample; - // cloudSample.header = cloud->header - // cloudSample.height = cloud->height / 2; - // cloudSample.width = cloud->width / 2; - // cloudSample.fields - // } - void LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud) { // TODO: OPTIMIZE; doing this maobject_detector/debug_imgny push_back calls could slow things down mFilteredPoints.clear(); auto* cloudData = reinterpret_cast(cloud->data.data()); ROS_INFO("Point cloud size: %i", cloud->height * cloud->width); - std::vector pts = {new Point{1, 2, 3, 0, 0, 0, 0, 0.3, 0.5, 0.01, 0}, new Point{1, 2, 3, 0, 0, 0, 0, 0.3, 0.5, 0.1, 0}, new Point{1, 2, 3, 0, 0, 0, 0, 0.3, 0.5, 0.07, 0}}; - // define randomizer std::default_random_engine generator; - std::uniform_int_distribution distribution(0, (int) 10); + std::uniform_int_distribution pointDistribution(0, (int) mLeastSamplingDistribution); - for (auto point = cloudData; point < cloudData + (cloud->height * cloud->width); point += distribution(generator)) { - // for (Point * point : pts) { + for (auto point = cloudData; point < cloudData + (cloud->height * cloud->width); point += pointDistribution(generator)) { bool isPointInvalid = (!std::isfinite(point->x) || !std::isfinite(point->y) || !std::isfinite(point->z)); - // ROS_INFO("Filtered point: %f, %f, %f", point->normal_x, point->normal_y, point->normal_z); - if (abs(point->normal_z) < mZThreshold && !isPointInvalid) { mFilteredPoints.push_back(point); // ROS_INFO("Filtered point: %f, %f, %f", point->normal_x, point->normal_y, point->normal_z); } } - ROS_INFO("Filtered vector size: %lu", mFilteredPoints.size()); - - for (Point * p : pts) { - delete p; - } } - void LanderAlignNodelet::ransac(float const distanceThreshold, int minInliers, int const epochs) { + void LanderAlignNodelet::uploadPC(int numInliers, double distanceThreshold){ + auto debugPointCloudPtr = boost::make_shared(); + fillPointCloudMessageHeader(debugPointCloudPtr); + debugPointCloudPtr->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; + debugPointCloudPtr->is_dense = true; + debugPointCloudPtr->height = 1; + debugPointCloudPtr->width = numInliers; + debugPointCloudPtr->header.seq = 0; + debugPointCloudPtr->header.stamp = ros::Time(); + debugPointCloudPtr->header.frame_id = "zed2i_left_camera_frame"; + debugPointCloudPtr->data.resize((numInliers * sizeof(Point))/sizeof(uchar)); + auto pcPtr = reinterpret_cast(debugPointCloudPtr->data.data()); + size_t i = 0; + for (auto p: mFilteredPoints) { + // calculate distance of each point from potential plane + double distance = std::abs(mBestNormalInZED.value().x() * p->x + mBestNormalInZED.value().y() * p->y + mBestNormalInZED.value().z() * p->z + mBestOffset); + if (distance < distanceThreshold) { + pcPtr[i].x = p->x; + pcPtr[i].y = p->y; + pcPtr[i].z = p->z; + pcPtr[i].b = p->r; + pcPtr[i].g = p->g; + pcPtr[i].r = p->b; + pcPtr[i].a = p->a; + ++i; + } + } + mDebugPCPub.publish(debugPointCloudPtr); + } + + void LanderAlignNodelet::ransac(double const distanceThreshold, int minInliers, int const epochs) { // TODO: use RANSAC to find the lander face, should be the closest, we may need to modify this to output more information, currently the output is the normal // takes 3 samples for every epoch and terminates after specified number of epochs // Eigen::Vector3d mBestNormal(0, 0, 0); // normal vector representing plane (initialize as zero vector?? default ctor??) @@ -147,7 +157,7 @@ namespace mrover { for (auto p: mFilteredPoints) { // calculate distance of each point from potential plane - float distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); // + double distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); // if (distance < distanceThreshold) { // Add points to current planes center @@ -169,9 +179,6 @@ namespace mrover { } } } - ROS_INFO("Out of zero loop"); - - // Run through one more loop to identify the center of the plane (one possibility for determining best center) numInliers = 0; @@ -189,33 +196,7 @@ namespace mrover { mBestLocationInZED.value() /= static_cast(numInliers); - auto debugPointCloudPtr = boost::make_shared(); - fillPointCloudMessageHeader(debugPointCloudPtr); - debugPointCloudPtr->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; - debugPointCloudPtr->is_dense = true; - debugPointCloudPtr->height = 1; - debugPointCloudPtr->width = numInliers; - debugPointCloudPtr->header.seq = 0; - debugPointCloudPtr->header.stamp = ros::Time(); - debugPointCloudPtr->header.frame_id = "zed2i_left_camera_frame"; - debugPointCloudPtr->data.resize((numInliers * sizeof(Point))/sizeof(uchar)); - auto pcPtr = reinterpret_cast(debugPointCloudPtr->data.data()); - size_t i = 0; - for (auto p: mFilteredPoints) { - // calculate distance of each point from potential plane - double distance = std::abs(mBestNormalInZED.value().x() * p->x + mBestNormalInZED.value().y() * p->y + mBestNormalInZED.value().z() * p->z + mBestOffset); - if (distance < distanceThreshold) { - pcPtr[i].x = p->x; - pcPtr[i].y = p->y; - pcPtr[i].z = p->z; - pcPtr[i].b = p->r; - pcPtr[i].g = p->g; - pcPtr[i].r = p->b; - pcPtr[i].a = p->a; - ++i; - } - } - mDebugPCPub.publish(debugPointCloudPtr); + uploadPC(numInliers, distanceThreshold); if (numInliers == 0) { ROS_INFO("zero inliers"); @@ -225,15 +206,6 @@ namespace mrover { return; } - - // if (mBestNormalInZED.value().x() < 0) { // ALSO NEED TO FLIP THE Y VALUE - // mBestNormalInZED.value().x() *= -1; - // } - - // if(mBestNormalInZED.value().y() < 0){ - // mBestNormalInZED.value().y() *= -1; - // } - //Calculate the other three rotation vectors Eigen::Matrix3d rot; { @@ -384,4 +356,4 @@ namespace mrover { rate.sleep(); } } -} // namespace mrover \ No newline at end of file +} // namespace mrover diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 6c3a5302f..258e99720 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -46,8 +46,9 @@ namespace mrover { std::string mCameraFrameId; std::string mMapFrameId; + //Ransac Params float mZThreshold; - + int mLeastSamplingDistribution = 10; std::vector mFilteredPoints; auto onInit() -> void override; @@ -59,10 +60,12 @@ namespace mrover { void filterNormals(sensor_msgs::PointCloud2Ptr const& cloud); - void ransac(float distanceThreshold, int minInliers, int epochs); + void ransac(double distanceThreshold, int minInliers, int epochs); void sendTwist(float const& offset); + void uploadPC(int numInliers, double distanceThreshold); + class PID { private: float const Angle_P; @@ -86,4 +89,4 @@ namespace mrover { }; }; -} // namespace mrover \ No newline at end of file +} // namespace mrover From 30eb8441a1c1f82ae9fef3bec1431e6d2e4e6085 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 5 Mar 2024 17:55:09 -0500 Subject: [PATCH 39/89] WHYYY --- src/teleoperation/lander_align/lander_align.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index ea0e5fc5c..9e48eca58 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -42,7 +42,7 @@ namespace mrover { //TF Create the Reference Frames - mNh.param("camera_frame", mCameraFrameId, "zed2i_left_camera_frame"); + mNh.param("camera_frame", mCameraFrameId, "zed_left_camera_frame"); mNh.param("world_frame", mMapFrameId, "map"); mBestLocationInWorld = std::make_optional(0, 0, 0); @@ -90,7 +90,7 @@ namespace mrover { debugPointCloudPtr->width = numInliers; debugPointCloudPtr->header.seq = 0; debugPointCloudPtr->header.stamp = ros::Time(); - debugPointCloudPtr->header.frame_id = "zed2i_left_camera_frame"; + debugPointCloudPtr->header.frame_id = "zed_left_camera_frame"; debugPointCloudPtr->data.resize((numInliers * sizeof(Point))/sizeof(uchar)); auto pcPtr = reinterpret_cast(debugPointCloudPtr->data.data()); size_t i = 0; @@ -209,7 +209,8 @@ namespace mrover { //Calculate the other three rotation vectors Eigen::Matrix3d rot; { - if(mBestNormalInZED.value().x() > 0) mBestNormalInZED.value() *=-1; + if(mBestNormalInZED.value().x() < 0) mBestNormalInZED.value() *=-1; + ROS_INFO("normal x: %.7f", static_cast(mBestNormalInZED.value().x())); // Eigen::Vector3d x; // Eigen::Vector3d y; From eb7f6b211d85e3f81dfef2f5fe3e227a9b5db136 Mon Sep 17 00:00:00 2001 From: Julian Skifstad Date: Tue, 5 Mar 2024 18:24:48 -0500 Subject: [PATCH 40/89] bad optional access --- .../lander_align/lander_align.cpp | 21 +++++++++++++++---- .../lander_align/lander_align.hpp | 3 +++ 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 9e48eca58..a64780c4a 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -101,9 +101,9 @@ namespace mrover { pcPtr[i].x = p->x; pcPtr[i].y = p->y; pcPtr[i].z = p->z; - pcPtr[i].b = p->r; + pcPtr[i].b = p->b; pcPtr[i].g = p->g; - pcPtr[i].r = p->b; + pcPtr[i].r = p->r; pcPtr[i].a = p->a; ++i; } @@ -194,6 +194,10 @@ namespace mrover { } //Average pnts mBestLocationInZED.value() /= static_cast(numInliers); + if(mBestNormalInZED.value().x() > 0) mBestNormalInZED.value() *=-1; + + mBestOffsetInZED.value() = mBestLocationInZED.value() + mBestNormalInZED.value(); + if(mBestNormalInZED.value().x() < 0) mBestNormalInZED.value() *=-1; uploadPC(numInliers, distanceThreshold); @@ -209,14 +213,15 @@ namespace mrover { //Calculate the other three rotation vectors Eigen::Matrix3d rot; { - if(mBestNormalInZED.value().x() < 0) mBestNormalInZED.value() *=-1; - ROS_INFO("normal x: %.7f", static_cast(mBestNormalInZED.value().x())); + if(mBestNormalInZED.value().x() > 0) mBestNormalInZED.value() *=-1; // Eigen::Vector3d x; // Eigen::Vector3d y; // Eigen::Vector3d z; // Eigen::Vector3d dummy{mBestNormalInZED->x()+1,mBestLocationInZED->y(),mBestNormalInZED->z()}; Eigen::Vector3d n = mBestNormalInZED.value().normalized(); + ROS_INFO("normal x: %.7f", static_cast(n.x())); + // y = dummy.cross(mBestNormalInZED.value()).normalized(); // z = x.cross(mBestNormalInZED.value()).normalized(); rot << n.x(),0,0, @@ -241,6 +246,14 @@ namespace mrover { SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, planeLocationSE3); + //For the offset + manif::SE3d mOffsetLocInZED = {{mBestOffsetInZED.value().x(), mBestOffsetInZED.value().y(), mBestOffsetInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};//TODO: THIS IS A RANDOM ROTATION MATRIX + + mBestNormalInWorld = std::make_optional(zedToMap.rotation().matrix() * mBestNormalInZED.value()); + manif::SE3d offsetLocationSE3 = (zedToMap * mPlaneLocInZED); + + SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, offsetLocationSE3); + ROS_INFO("Max inliers: %i", minInliers); ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z(), mBestNormalInZED.value().x(), mBestNormalInZED.value().y(), mBestNormalInZED.value().z()); diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 258e99720..6caf5e892 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -35,6 +35,9 @@ namespace mrover { std::optional mBestLocationInZED; std::optional mBestLocationInWorld; + std::optional mBestOffsetInZED; + std::optional mBestOffsetInWorld; + std::optional mBestNormalInZED; std::optional mBestNormalInWorld; //** From 2d965e9edc6cb2fe5b64f3f6040a78de888c85d2 Mon Sep 17 00:00:00 2001 From: MyCabbages4 Date: Tue, 5 Mar 2024 19:13:50 -0500 Subject: [PATCH 41/89] bad optional fixed --- .../lander_align/lander_align.cpp | 25 ++++++++++--------- 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index a64780c4a..98393dbc5 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -192,15 +192,6 @@ namespace mrover { ++numInliers; // count num of inliers that pass the "good enough fit" threshold } } - //Average pnts - mBestLocationInZED.value() /= static_cast(numInliers); - if(mBestNormalInZED.value().x() > 0) mBestNormalInZED.value() *=-1; - - mBestOffsetInZED.value() = mBestLocationInZED.value() + mBestNormalInZED.value(); - if(mBestNormalInZED.value().x() < 0) mBestNormalInZED.value() *=-1; - - - uploadPC(numInliers, distanceThreshold); if (numInliers == 0) { ROS_INFO("zero inliers"); @@ -210,6 +201,17 @@ namespace mrover { return; } + //Average pnts + mBestLocationInZED.value() /= static_cast(numInliers); + + if(mBestNormalInZED.value().x() > 0) mBestNormalInZED.value() *=-1; + + mBestOffsetInZED = std::make_optional(mBestLocationInZED.value() + mBestNormalInZED.value()); + if(mBestNormalInZED.value().x() < 0) mBestNormalInZED.value() *=-1; + + + uploadPC(numInliers, distanceThreshold); + //Calculate the other three rotation vectors Eigen::Matrix3d rot; { @@ -249,10 +251,9 @@ namespace mrover { //For the offset manif::SE3d mOffsetLocInZED = {{mBestOffsetInZED.value().x(), mBestOffsetInZED.value().y(), mBestOffsetInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};//TODO: THIS IS A RANDOM ROTATION MATRIX - mBestNormalInWorld = std::make_optional(zedToMap.rotation().matrix() * mBestNormalInZED.value()); - manif::SE3d offsetLocationSE3 = (zedToMap * mPlaneLocInZED); + manif::SE3d offsetLocationSE3 = (zedToMap * mOffsetLocInZED); - SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, offsetLocationSE3); + SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, offsetLocationSE3); ROS_INFO("Max inliers: %i", minInliers); From f21a22681793305ca9a8893ef51b6a8252ab2948 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Tue, 5 Mar 2024 19:34:18 -0500 Subject: [PATCH 42/89] plane good offset work --- src/teleoperation/lander_align/lander_align.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 98393dbc5..8e339420b 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,4 +1,4 @@ -#include "lander_align.hpp" + #include "lander_align.hpp" #include "lie.hpp" #include "pch.hpp" #include From 111bf1d9ffcbf441ec0e437dbf57ccfa93660355 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Tue, 5 Mar 2024 20:06:57 -0500 Subject: [PATCH 43/89] stuff --- src/teleoperation/lander_align/lander_align.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 8e339420b..c8738ed22 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -255,6 +255,11 @@ namespace mrover { SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, offsetLocationSE3); + //For the normal + manif::SE3d mNormalLocInZED = {{mBestNormalInWorld.value().x(), mBestNormalInWorld.value().y(), mBestNormalInWorld.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};//TODO: THIS IS A RANDOM ROTATION MATRIX + + SE3Conversions::pushToTfTree(mTfBroadcaster, "normal", mMapFrameId, mNormalLocInZED); + ROS_INFO("Max inliers: %i", minInliers); ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z(), mBestNormalInZED.value().x(), mBestNormalInZED.value().y(), mBestNormalInZED.value().z()); From eb03a9041a5a1432677c6625554bff40e902c4b0 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 5 Mar 2024 22:44:17 -0500 Subject: [PATCH 44/89] Fixed some stuff --- .../lander_align/lander_align.cpp | 51 +++++++++---------- 1 file changed, 24 insertions(+), 27 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index c8738ed22..192ecb067 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,4 +1,4 @@ - #include "lander_align.hpp" +#include "lander_align.hpp" #include "lie.hpp" #include "pch.hpp" #include @@ -207,22 +207,19 @@ namespace mrover { if(mBestNormalInZED.value().x() > 0) mBestNormalInZED.value() *=-1; mBestOffsetInZED = std::make_optional(mBestLocationInZED.value() + mBestNormalInZED.value()); - if(mBestNormalInZED.value().x() < 0) mBestNormalInZED.value() *=-1; - uploadPC(numInliers, distanceThreshold); - //Calculate the other three rotation vectors + manif::SE3d zedToMap = SE3Conversions::fromTfTree(mTfBuffer, mCameraFrameId, mMapFrameId); + + //Calculate the normal in the world frame + mBestNormalInWorld = std::make_optional(zedToMap.rotation().matrix() * mBestNormalInZED.value()); + + //Calculate the SO3 in the world frame Eigen::Matrix3d rot; { - if(mBestNormalInZED.value().x() > 0) mBestNormalInZED.value() *=-1; - - // Eigen::Vector3d x; - // Eigen::Vector3d y; - // Eigen::Vector3d z; - // Eigen::Vector3d dummy{mBestNormalInZED->x()+1,mBestLocationInZED->y(),mBestNormalInZED->z()}; - Eigen::Vector3d n = mBestNormalInZED.value().normalized(); - ROS_INFO("normal x: %.7f", static_cast(n.x())); + Eigen::Vector3d n = mBestNormalInWorld.value().normalized(); + ROS_INFO("normal x: %.7f", static_cast(mBestNormalInZED.value().x())); // y = dummy.cross(mBestNormalInZED.value()).normalized(); // z = x.cross(mBestNormalInZED.value()).normalized(); @@ -237,32 +234,32 @@ namespace mrover { rot.col(1) = q.col(2); rot.col(2) = q.col(1); } + //Calculate the plane location in the world frame + mBestLocationInWorld = std::make_optional(zedToMap.rotation().matrix() * mBestLocationInZED.value()); + manif::SE3d plane_loc_in_world = {{mBestLocationInWorld.value().x(), mBestLocationInWorld.value().y(), mBestLocationInWorld.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; - manif::SE3d mPlaneLocInZED = {{mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};//TODO: THIS IS A RANDOM ROTATION MATRIX + //Calculate the offset location in the world frame + mBestOffsetInWorld = std::make_optional(zedToMap.rotation().matrix() * mBestOffsetInZED.value()); + manif::SE3d offset_loc_in_world = {{mBestOffsetInWorld.value().x(), mBestOffsetInWorld.value().y(), mBestOffsetInWorld.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; - manif::SE3d zedToMap = SE3Conversions::fromTfTree(mTfBuffer, mCameraFrameId, mMapFrameId); - - mBestNormalInWorld = std::make_optional(zedToMap.rotation().matrix() * mBestNormalInZED.value()); - manif::SE3d planeLocationSE3 = (zedToMap * mPlaneLocInZED); - mBestLocationInWorld = std::make_optional(planeLocationSE3.translation()); - SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, planeLocationSE3); + //Push to the tf tree + SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, plane_loc_in_world); + SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, offset_loc_in_world); - //For the offset - manif::SE3d mOffsetLocInZED = {{mBestOffsetInZED.value().x(), mBestOffsetInZED.value().y(), mBestOffsetInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};//TODO: THIS IS A RANDOM ROTATION MATRIX - - manif::SE3d offsetLocationSE3 = (zedToMap * mOffsetLocInZED); + //For the normal + manif::SE3d mNormalLocInWorld = {{mBestNormalInWorld.value().x(), mBestNormalInWorld.value().y(), mBestNormalInWorld.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};//TODO: THIS IS A RANDOM ROTATION MATRIX - SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, offsetLocationSE3); + SE3Conversions::pushToTfTree(mTfBroadcaster, "normalInWorld", mMapFrameId, mNormalLocInWorld); //For the normal - manif::SE3d mNormalLocInZED = {{mBestNormalInWorld.value().x(), mBestNormalInWorld.value().y(), mBestNormalInWorld.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};//TODO: THIS IS A RANDOM ROTATION MATRIX + manif::SE3d mNormalLocInZED = {{mBestNormalInZED.value().x(), mBestNormalInZED.value().y(), mBestNormalInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};//TODO: THIS IS A RANDOM ROTATION MATRIX - SE3Conversions::pushToTfTree(mTfBroadcaster, "normal", mMapFrameId, mNormalLocInZED); + SE3Conversions::pushToTfTree(mTfBroadcaster, "normalInZED", mMapFrameId, mNormalLocInZED); ROS_INFO("Max inliers: %i", minInliers); - ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z(), mBestNormalInZED.value().x(), mBestNormalInZED.value().y(), mBestNormalInZED.value().z()); + ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z(), mBestNormalInWorld.value().x(), mBestNormalInWorld.value().y(), mBestNormalInWorld.value().z()); } auto LanderAlignNodelet::PID::rotate_speed(float theta) const -> float { From a5fcd3f3642ee653c407bf1d919c1da847808c5c Mon Sep 17 00:00:00 2001 From: John Date: Wed, 6 Mar 2024 11:24:21 -0500 Subject: [PATCH 45/89] Cooked --- src/teleoperation/lander_align/lander_align.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 192ecb067..b07153f84 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -234,13 +234,15 @@ namespace mrover { rot.col(1) = q.col(2); rot.col(2) = q.col(1); } - //Calculate the plane location in the world frame - mBestLocationInWorld = std::make_optional(zedToMap.rotation().matrix() * mBestLocationInZED.value()); + //Calculate the plane location in the world frame` manif::SE3d plane_loc_in_world = {{mBestLocationInWorld.value().x(), mBestLocationInWorld.value().y(), mBestLocationInWorld.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; + mBestLocationInWorld = std::make_optional(zedToMap * plane_loc_in_world); + plane_loc_in_world.rotation().matrix() = rot; //Calculate the offset location in the world frame - mBestOffsetInWorld = std::make_optional(zedToMap.rotation().matrix() * mBestOffsetInZED.value()); manif::SE3d offset_loc_in_world = {{mBestOffsetInWorld.value().x(), mBestOffsetInWorld.value().y(), mBestOffsetInWorld.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; + mBestOffsetInWorld = std::make_optional(zedToMap * offset_loc_in_world); + offset_loc_in_world.rotation().matrix() = rot; //Push to the tf tree From ace3ecad32b47c181c892e09d49fcb39a868664e Mon Sep 17 00:00:00 2001 From: John Date: Thu, 7 Mar 2024 10:40:25 -0500 Subject: [PATCH 46/89] Not as cooked --- src/teleoperation/lander_align/lander_align.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index b07153f84..29915a2f5 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -236,12 +236,12 @@ namespace mrover { } //Calculate the plane location in the world frame` manif::SE3d plane_loc_in_world = {{mBestLocationInWorld.value().x(), mBestLocationInWorld.value().y(), mBestLocationInWorld.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; - mBestLocationInWorld = std::make_optional(zedToMap * plane_loc_in_world); + plane_loc_in_world = zedToMap * plane_loc_in_world; plane_loc_in_world.rotation().matrix() = rot; //Calculate the offset location in the world frame manif::SE3d offset_loc_in_world = {{mBestOffsetInWorld.value().x(), mBestOffsetInWorld.value().y(), mBestOffsetInWorld.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; - mBestOffsetInWorld = std::make_optional(zedToMap * offset_loc_in_world); + offset_loc_in_world = zedToMap * offset_loc_in_world; offset_loc_in_world.rotation().matrix() = rot; From 2596de1fc96a9e80aeb7e747fcf4d5e975dbf590 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 7 Mar 2024 14:03:14 -0500 Subject: [PATCH 47/89] Not finished --- src/teleoperation/lander_align/lander_align.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 29915a2f5..7f918795a 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -235,19 +235,19 @@ namespace mrover { rot.col(2) = q.col(1); } //Calculate the plane location in the world frame` - manif::SE3d plane_loc_in_world = {{mBestLocationInWorld.value().x(), mBestLocationInWorld.value().y(), mBestLocationInWorld.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; + manif::SE3d plane_loc_in_world = {{mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; plane_loc_in_world = zedToMap * plane_loc_in_world; - plane_loc_in_world.rotation().matrix() = rot; + manif::SE3d plane_loc_in_world_final = {{plane_loc_in_world.translation().x(),plane_loc_in_world.translation().y(),plane_loc_in_world.translation().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; //Calculate the offset location in the world frame - manif::SE3d offset_loc_in_world = {{mBestOffsetInWorld.value().x(), mBestOffsetInWorld.value().y(), mBestOffsetInWorld.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; + manif::SE3d offset_loc_in_world = {{mBestOffsetInZED.value().x(), mBestOffsetInZED.value().y(), mBestOffsetInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; offset_loc_in_world = zedToMap * offset_loc_in_world; - offset_loc_in_world.rotation().matrix() = rot; + manif::SE3d offset_loc_in_world_final = {{offset_loc_in_world.translation().x(),offset_loc_in_world.translation().y(),offset_loc_in_world.translation().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; //Push to the tf tree - SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, plane_loc_in_world); - SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, offset_loc_in_world); + SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, plane_loc_in_world_final); + SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, offset_loc_in_world_final); //For the normal manif::SE3d mNormalLocInWorld = {{mBestNormalInWorld.value().x(), mBestNormalInWorld.value().y(), mBestNormalInWorld.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};//TODO: THIS IS A RANDOM ROTATION MATRIX From 6f3446dbd0dc9a30c023f8524140a964e9d91336 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 7 Mar 2024 14:37:03 -0500 Subject: [PATCH 48/89] Make No Sense -YoungBoy Never Broke again --- src/teleoperation/lander_align/lander_align.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 7f918795a..a3591d174 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -226,15 +226,21 @@ namespace mrover { rot << n.x(),0,0, n.y(),0,1, n.z(),1,0; - std::cout << "rot matrix " << rot << std::endl; + std::cout << "rot matrix 2 s" << std::endl << rot << std::endl; + Eigen::HouseholderQR qr{rot}; Eigen::Matrix3d q = qr.householderQ(); rot.col(0) = q.col(0); rot.col(1) = q.col(2); rot.col(2) = q.col(1); + std::cout << "rot matrix " << std::endl << rot << std::endl; + rot << 1,0,0, + 0,-1,0, + 0,0,1; + } - //Calculate the plane location in the world frame` + //Calculate the plane location in the world frame manif::SE3d plane_loc_in_world = {{mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; plane_loc_in_world = zedToMap * plane_loc_in_world; manif::SE3d plane_loc_in_world_final = {{plane_loc_in_world.translation().x(),plane_loc_in_world.translation().y(),plane_loc_in_world.translation().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; From 1b6f70017712186de361113364d98cc29266c496 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 7 Mar 2024 14:39:24 -0500 Subject: [PATCH 49/89] Maybe? --- .../lander_align/lander_align.cpp | 20 ++++++++----------- 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index a3591d174..076c6cb83 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -52,14 +52,14 @@ namespace mrover { void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { filterNormals(cloud); ransac(0.2, 10, 100); - // if (mBestNormalInZED.has_value()) { - // geometry_msgs::Vector3 vect; - // vect.x = mBestNormalInZED.value().x(); - // vect.y = mBestNormalInZED.value().y(); - // vect.z = mBestNormalInZED.value().z(); - // mDebugVectorPub.publish(vect); - // //sendTwist(10.0); - // } + if (mBestNormalInZED.has_value()) { + geometry_msgs::Vector3 vect; + vect.x = mBestNormalInZED.value().x(); + vect.y = mBestNormalInZED.value().y(); + vect.z = mBestNormalInZED.value().z(); + mDebugVectorPub.publish(vect); + //sendTwist(10.0); + } } void LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud) { @@ -235,10 +235,6 @@ namespace mrover { rot.col(1) = q.col(2); rot.col(2) = q.col(1); std::cout << "rot matrix " << std::endl << rot << std::endl; - rot << 1,0,0, - 0,-1,0, - 0,0,1; - } //Calculate the plane location in the world frame manif::SE3d plane_loc_in_world = {{mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; From 9a7c0c65e0866bb444889efb4c27c7de5974d5c8 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 7 Mar 2024 17:21:59 -0500 Subject: [PATCH 50/89] Switch Cooked --- .../lander_align/lander_align.cpp | 27 ++++++++++--------- .../lander_align/lander_align.hpp | 17 +++++++++--- 2 files changed, 27 insertions(+), 17 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 076c6cb83..f8dce5c9c 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -30,6 +30,10 @@ #include namespace mrover { + auto operator<<(std::ostream &ostream, RTRSTATE state) -> std::ostream& { + ostream << RTRSTRINGS[state]; + return ostream; + } auto LanderAlignNodelet::onInit() -> void { mNh = getMTNodeHandle(); @@ -40,13 +44,14 @@ namespace mrover { mTwistPub = mNh.advertise("/cmd_vel", 1); mDebugPCPub = mNh.advertise("/lander_align/debugPC", 1); - //TF Create the Reference Frames mNh.param("camera_frame", mCameraFrameId, "zed_left_camera_frame"); mNh.param("world_frame", mMapFrameId, "map"); mBestLocationInWorld = std::make_optional(0, 0, 0); mBestNormalInWorld = std::make_optional(0, 0, 0); + + mLoopState = RTRSTATE::turn1; } void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { @@ -58,7 +63,7 @@ namespace mrover { vect.y = mBestNormalInZED.value().y(); vect.z = mBestNormalInZED.value().z(); mDebugVectorPub.publish(vect); - //sendTwist(10.0); + sendTwist(10.0); } } @@ -134,7 +139,6 @@ namespace mrover { int numInliers = 0; while (mBestNormalInZED.value().isZero()) { // TODO add give up condition after X iter - ROS_INFO("in zero loop"); for (int i = 0; i < epochs; ++i) { // currentCenter *= 0; // Set all vals in current center to zero at the start of each epoch // sample 3 random points (potential inliers) @@ -194,8 +198,6 @@ namespace mrover { } if (numInliers == 0) { - ROS_INFO("zero inliers"); - mBestNormalInZED = std::nullopt; mBestLocationInZED = std::nullopt; return; @@ -219,14 +221,13 @@ namespace mrover { Eigen::Matrix3d rot; { Eigen::Vector3d n = mBestNormalInWorld.value().normalized(); - ROS_INFO("normal x: %.7f", static_cast(mBestNormalInZED.value().x())); // y = dummy.cross(mBestNormalInZED.value()).normalized(); // z = x.cross(mBestNormalInZED.value()).normalized(); rot << n.x(),0,0, n.y(),0,1, n.z(),1,0; - std::cout << "rot matrix 2 s" << std::endl << rot << std::endl; + //std::cout << "rot matrix 2 s" << std::endl << rot << std::endl; Eigen::HouseholderQR qr{rot}; @@ -234,7 +235,6 @@ namespace mrover { rot.col(0) = q.col(0); rot.col(1) = q.col(2); rot.col(2) = q.col(1); - std::cout << "rot matrix " << std::endl << rot << std::endl; } //Calculate the plane location in the world frame manif::SE3d plane_loc_in_world = {{mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; @@ -262,8 +262,8 @@ namespace mrover { SE3Conversions::pushToTfTree(mTfBroadcaster, "normalInZED", mMapFrameId, mNormalLocInZED); - ROS_INFO("Max inliers: %i", minInliers); - ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z(), mBestNormalInWorld.value().x(), mBestNormalInWorld.value().y(), mBestNormalInWorld.value().z()); + //ROS_INFO("Max inliers: %i", minInliers); + //ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z(), mBestNormalInWorld.value().x(), mBestNormalInWorld.value().y(), mBestNormalInWorld.value().z()); } auto LanderAlignNodelet::PID::rotate_speed(float theta) const -> float { @@ -331,12 +331,13 @@ namespace mrover { case RTRSTATE::turn1: { rover_dir = {static_cast(rover.rotation().matrix().col(0).x()), static_cast(rover.rotation().matrix().col(0).y()), 0}; float angle = pid.find_angle(rover_dir, targetPosInWorld - roverPosInWorld); + ROS_INFO("angle %f", angle); float angle_rate = pid.rotate_speed(angle); twist.angular.z = angle_rate; if (abs(angle) < angular_thresh) { - mLoopState = RTRSTATE::drive; + //mLoopState = RTRSTATE::drive; } // ROS_INFO("In state: turning to point..."); } @@ -371,8 +372,8 @@ namespace mrover { break; } ROS_INFO("THE TWIST IS: Angular: %f, with linear %f,", twist.angular.z, twist.linear.x); - - mTwistPub.publish(twist); + ROS_INFO_STREAM(mLoopState); + //mTwistPub.publish(twist); rate.sleep(); } diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 6caf5e892..74c63ad6e 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -3,17 +3,26 @@ #include "pch.hpp" #include #include +#include #include namespace mrover { enum RTRSTATE { - turn1, - drive, - turn2, - done + turn1 = 0, + drive = 1, + turn2 = 2, + done = 3 }; + constexpr char RTRSTRINGS[4][6] = { + "turn1", + "drive", + "turn2", + "done " + }; + + class LanderAlignNodelet : public nodelet::Nodelet { ros::NodeHandle mNh, mPnh; From 6fd4ed5f800d9062b5113dcae72f522fb1297f64 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 7 Mar 2024 17:47:41 -0500 Subject: [PATCH 51/89] refactoring sendTwist --- .../lander_align/lander_align.cpp | 20 +++++++++++-------- .../lander_align/lander_align.hpp | 2 +- 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index f8dce5c9c..d3adcdb2f 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -2,6 +2,7 @@ #include "lie.hpp" #include "pch.hpp" #include +#include #include #include #include @@ -63,7 +64,7 @@ namespace mrover { vect.y = mBestNormalInZED.value().y(); vect.z = mBestNormalInZED.value().z(); mDebugVectorPub.publish(vect); - sendTwist(10.0); + sendTwist(); } } @@ -240,11 +241,13 @@ namespace mrover { manif::SE3d plane_loc_in_world = {{mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; plane_loc_in_world = zedToMap * plane_loc_in_world; manif::SE3d plane_loc_in_world_final = {{plane_loc_in_world.translation().x(),plane_loc_in_world.translation().y(),plane_loc_in_world.translation().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; + mBestLocationInWorld = std::make_optional(plane_loc_in_world_final.translation()); //Calculate the offset location in the world frame manif::SE3d offset_loc_in_world = {{mBestOffsetInZED.value().x(), mBestOffsetInZED.value().y(), mBestOffsetInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; offset_loc_in_world = zedToMap * offset_loc_in_world; manif::SE3d offset_loc_in_world_final = {{offset_loc_in_world.translation().x(),offset_loc_in_world.translation().y(),offset_loc_in_world.translation().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; + mBestOffsetInWorld = std::make_optional(offset_loc_in_world_final.translation()); //Push to the tf tree @@ -305,28 +308,26 @@ namespace mrover { // } - void LanderAlignNodelet::sendTwist(float const& offset) { - + void LanderAlignNodelet::sendTwist() { //Locations - manif::SE3d rover; Eigen::Vector3d rover_dir; - Eigen::Vector3d targetPosInWorld = mBestLocationInWorld.value() + mBestNormalInWorld.value() * offset; //Final msg geometry_msgs::Twist twist; - //Thr + //Threhsolds float const linear_thresh = 0.1; // could be member variables float const angular_thresh = 0.1; - targetPosInWorld.z() = 0; PID pid(0.1, 0.1); // literally just P -- ugly class and probably needs restructuring in the future ros::Rate rate(20); // ROS Rate at 20Hz while (ros::ok()) { - rover = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); + manif::SE3d rover = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); Eigen::Vector3d roverPosInWorld{static_cast(rover.translation().x()), static_cast(rover.translation().y()), 0}; + Eigen::Vector3d targetPosInWorld = mBestOffsetInWorld.value(); + targetPosInWorld.z() = 0; switch (mLoopState) { case RTRSTATE::turn1: { rover_dir = {static_cast(rover.rotation().matrix().col(0).x()), static_cast(rover.rotation().matrix().col(0).y()), 0}; @@ -340,6 +341,7 @@ namespace mrover { //mLoopState = RTRSTATE::drive; } // ROS_INFO("In state: turning to point..."); + break; } case RTRSTATE::drive: { @@ -352,6 +354,7 @@ namespace mrover { mLoopState = RTRSTATE::turn2; } // ROS_INFO("In state: driving to point..."); + break; } case RTRSTATE::turn2: { @@ -366,6 +369,7 @@ namespace mrover { mLoopState = RTRSTATE::done; } // ROS_INFO("In state: turning to lander..."); + break; } case RTRSTATE::done: diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 74c63ad6e..fb567afcc 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -74,7 +74,7 @@ namespace mrover { void ransac(double distanceThreshold, int minInliers, int epochs); - void sendTwist(float const& offset); + void sendTwist(); void uploadPC(int numInliers, double distanceThreshold); From 3cdc0c09939dbd9cfd929815e79cdcf9842b7576 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 7 Mar 2024 18:13:43 -0500 Subject: [PATCH 52/89] pushed --- .../lander_align/lander_align.cpp | 44 +++++++++---------- 1 file changed, 20 insertions(+), 24 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index d3adcdb2f..f4b5375e6 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -58,14 +58,14 @@ namespace mrover { void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { filterNormals(cloud); ransac(0.2, 10, 100); - if (mBestNormalInZED.has_value()) { - geometry_msgs::Vector3 vect; - vect.x = mBestNormalInZED.value().x(); - vect.y = mBestNormalInZED.value().y(); - vect.z = mBestNormalInZED.value().z(); - mDebugVectorPub.publish(vect); - sendTwist(); - } + // if (mBestNormalInZED.has_value()) { + // geometry_msgs::Vector3 vect; + // vect.x = mBestNormalInZED.value().x(); + // vect.y = mBestNormalInZED.value().y(); + // vect.z = mBestNormalInZED.value().z(); + // mDebugVectorPub.publish(vect); + // sendTwist(); + // } } void LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud) { @@ -209,7 +209,7 @@ namespace mrover { if(mBestNormalInZED.value().x() > 0) mBestNormalInZED.value() *=-1; - mBestOffsetInZED = std::make_optional(mBestLocationInZED.value() + mBestNormalInZED.value()); + mBestOffsetInZED = std::make_optional(mBestLocationInZED.value() + mBestNormalInZED.value()); uploadPC(numInliers, distanceThreshold); @@ -221,21 +221,17 @@ namespace mrover { //Calculate the SO3 in the world frame Eigen::Matrix3d rot; { - Eigen::Vector3d n = mBestNormalInWorld.value().normalized(); - - // y = dummy.cross(mBestNormalInZED.value()).normalized(); - // z = x.cross(mBestNormalInZED.value()).normalized(); - rot << n.x(),0,0, - n.y(),0,1, - n.z(),1,0; - //std::cout << "rot matrix 2 s" << std::endl << rot << std::endl; - - - Eigen::HouseholderQR qr{rot}; - Eigen::Matrix3d q = qr.householderQ(); - rot.col(0) = q.col(0); - rot.col(1) = q.col(2); - rot.col(2) = q.col(1); + Eigen::Vector3d forward = mBestNormalInWorld.value().normalized(); + Eigen::Vector3d worldUp = Eigen::Vector3d::UnitZ(); + Eigen::Vector3d left = forward.cross(worldUp); + Eigen::Vector3d up = left.cross(forward); + ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z(), forward.x(), forward.y(), forward.z()); + + + Eigen::Matrix3d rot; + rot.col(0) = forward; + rot.col(1) = left; + rot.col(2) = up; } //Calculate the plane location in the world frame manif::SE3d plane_loc_in_world = {{mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; From c04a088fb96affc015d06eb9d5ef00e310038353 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 7 Mar 2024 20:51:32 -0500 Subject: [PATCH 53/89] Working Lande Align Rough --- .../lander_align/lander_align.cpp | 187 +++++++++++++----- .../lander_align/lander_align.hpp | 6 +- src/util/lie/lie.cpp | 9 + src/util/lie/lie.hpp | 2 + 4 files changed, 156 insertions(+), 48 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index f4b5375e6..9daaa0d0b 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -3,6 +3,7 @@ #include "pch.hpp" #include #include +#include #include #include #include @@ -11,7 +12,10 @@ #include #include +#include #include +#include +#include #include #include #include @@ -58,14 +62,14 @@ namespace mrover { void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { filterNormals(cloud); ransac(0.2, 10, 100); - // if (mBestNormalInZED.has_value()) { - // geometry_msgs::Vector3 vect; - // vect.x = mBestNormalInZED.value().x(); - // vect.y = mBestNormalInZED.value().y(); - // vect.z = mBestNormalInZED.value().z(); - // mDebugVectorPub.publish(vect); - // sendTwist(); - // } + if (mBestNormalInZED.has_value()) { + geometry_msgs::Vector3 vect; + vect.x = mBestNormalInZED.value().x(); + vect.y = mBestNormalInZED.value().y(); + vect.z = mBestNormalInZED.value().z(); + mDebugVectorPub.publish(vect); + sendTwist(); + } } void LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud) { @@ -220,19 +224,16 @@ namespace mrover { //Calculate the SO3 in the world frame Eigen::Matrix3d rot; - { - Eigen::Vector3d forward = mBestNormalInWorld.value().normalized(); - Eigen::Vector3d worldUp = Eigen::Vector3d::UnitZ(); - Eigen::Vector3d left = forward.cross(worldUp); - Eigen::Vector3d up = left.cross(forward); - ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z(), forward.x(), forward.y(), forward.z()); - - - Eigen::Matrix3d rot; - rot.col(0) = forward; - rot.col(1) = left; - rot.col(2) = up; - } + Eigen::Vector3d forward = mBestNormalInWorld.value().normalized(); + Eigen::Vector3d worldUp = Eigen::Vector3d::UnitZ(); + Eigen::Vector3d left = worldUp.cross(forward); + Eigen::Vector3d up = forward.cross(left); + ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z(), forward.x(), forward.y(), forward.z()); + + rot.col(0) = forward; + rot.col(1) = left; + rot.col(2) = up; + //Calculate the plane location in the world frame manif::SE3d plane_loc_in_world = {{mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; plane_loc_in_world = zedToMap * plane_loc_in_world; @@ -265,7 +266,7 @@ namespace mrover { //ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z(), mBestNormalInWorld.value().x(), mBestNormalInWorld.value().y(), mBestNormalInWorld.value().z()); } - auto LanderAlignNodelet::PID::rotate_speed(float theta) const -> float { + auto LanderAlignNodelet::PID::rotate_speed(double theta) const -> double { return Angle_P * theta; } @@ -313,69 +314,161 @@ namespace mrover { //Threhsolds float const linear_thresh = 0.1; // could be member variables - float const angular_thresh = 0.1; + float const angular_thresh = 0.005; PID pid(0.1, 0.1); // literally just P -- ugly class and probably needs restructuring in the future - ros::Rate rate(20); // ROS Rate at 20Hz + ros::Rate rate(20); // ROS Rate at 20Hzn::Matrix3d roverToPlaneNorm; + // Eigen::Vector3d Nup = Eigen::Vector3d::UnitZ(); + // Eigen::Vector3d Nleft = Nup.cross(mBestNormalInWorld.value()); + + // roverToPlaneNorm.col(0) = mBestNormalInWorld.value(); + // roverToPlaneNorm.col(1) = Nup; + // roverToPlaneNorm.col(2) = Nleft; + // manif::SO3d roverToP while (ros::ok()) { manif::SE3d rover = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); - Eigen::Vector3d roverPosInWorld{static_cast(rover.translation().x()), static_cast(rover.translation().y()), 0}; - - Eigen::Vector3d targetPosInWorld = mBestOffsetInWorld.value(); + Eigen::Vector3d roverPosInWorld{static_cast(rover.translation().x()), static_cast(rover.translation().y()), 0.0}; + Eigen::Vector3d targetPosInWorld; + if(mLoopState == RTRSTATE::turn1){ + targetPosInWorld = mBestOffsetInWorld.value(); + }else if(mLoopState == RTRSTATE::turn2){ + targetPosInWorld = mBestLocationInWorld.value(); + } targetPosInWorld.z() = 0; + + Eigen::Vector3d roverToTargetForward = targetPosInWorld - roverPosInWorld; + roverToTargetForward.normalize(); + + Eigen::Vector3d up = Eigen::Vector3d::UnitZ(); + Eigen::Vector3d left = up.cross(roverToTargetForward); + + Eigen::Matrix3d roverToTargetMat; + roverToTargetMat.col(0) = roverToTargetForward; + roverToTargetMat.col(1) = up; + roverToTargetMat.col(2) = left; + + //SO3 Matrices for lie algebra + manif::SO3d roverToTargetSO3 = SE3Conversions::fromColumns(roverToTargetForward, left, up); + manif::SO3d roverSO3 = rover.asSO3(); + + Eigen::Vector3d distanceToTargetVector = targetPosInWorld - Eigen::Vector3d{rover.translation().x(), rover.translation().y(), rover.translation().z()}; + + double distanceToTarget = std::abs(distanceToTargetVector.norm()); + + manif::SO3Tangentd SO3tan = roverToTargetSO3 - roverSO3; // 3 x 1 matrix of angular velocities (x,y,z) + switch (mLoopState) { case RTRSTATE::turn1: { - rover_dir = {static_cast(rover.rotation().matrix().col(0).x()), static_cast(rover.rotation().matrix().col(0).y()), 0}; - float angle = pid.find_angle(rover_dir, targetPosInWorld - roverPosInWorld); - ROS_INFO("angle %f", angle); - float angle_rate = pid.rotate_speed(angle); + if (distanceToTarget < 0.5) mLoopState = RTRSTATE::turn2; + + double angle_rate = mAngleP * SO3tan.z(); + + ROS_INFO("w_z velocity %f", SO3tan.z()); + twist.angular.z = angle_rate; - if (abs(angle) < angular_thresh) { - //mLoopState = RTRSTATE::drive; + if (std::abs(SO3tan.z()) < angular_thresh) { + mLoopState = RTRSTATE::drive; + twist.angular.z = 0; + twist.linear.x = 0; + ROS_INFO("Done spinning"); + } // ROS_INFO("In state: turning to point..."); break; } case RTRSTATE::drive: { - float distance = pid.find_distance(roverPosInWorld, targetPosInWorld - roverPosInWorld); - float drive_rate = pid.drive_speed(distance); + if (std::abs(SO3tan.z()) > angular_thresh) { + mLoopState = RTRSTATE::turn1; + ROS_INFO("Rotation got off"); + twist.linear.x = 0; + + } + double driveRate = mLinearP * distanceToTarget; + + twist.linear.x = driveRate; - twist.linear.x = drive_rate; + ROS_INFO("distance to target %f", distanceToTarget); - if (abs(distance) < linear_thresh) { + if (std::abs(distanceToTarget) < linear_thresh) { mLoopState = RTRSTATE::turn2; + twist.linear.x = 0; + twist.angular.z = 0; } // ROS_INFO("In state: driving to point..."); break; } - + case RTRSTATE::turn2: { - rover_dir = {static_cast(rover.rotation().matrix().col(0).x()), static_cast(rover.rotation().matrix().col(0).y()), 0}; - rover_dir[2] = 0; - float angle = pid.find_angle(rover_dir, -mBestNormalInWorld.value()); // normal is pointing "towards" the rover, so we need to flip it to find angle - float angle_rate = pid.rotate_speed(angle); + // Eigen::Matrix3d roverToPlaneNorm; + // Eigen::Vector3d Nup = Eigen::Vector3d::UnitZ(); + // Eigen::Vector3d Nleft = Nup.cross(mBestNormalInWorld.value()); + + // roverToPlaneNorm.col(0) = mBestNormalInWorld.value(); + // roverToPlaneNorm.col(1) = Nup; + // roverToPlaneNorm.col(2) = Nleft; + // manif::SO3d roverToPlaneSO3 = SE3Conversions::fromColumns(roverToTargetForward, left, up); + double angle_rate = mAngleP * SO3tan.z(); twist.angular.z = angle_rate; + - if (abs(angle) < angular_thresh) { + if (std::abs(SO3tan.z()) < angular_thresh) { mLoopState = RTRSTATE::done; + twist.angular.z = 0; + twist.linear.x = angle_rate; + // ROS_INFO("Done turning to lander"); + } // ROS_INFO("In state: turning to lander..."); break; } - case RTRSTATE::done: + case RTRSTATE::done: { + twist.linear.x = 0; + twist.angular.z = 0; break; + } } - ROS_INFO("THE TWIST IS: Angular: %f, with linear %f,", twist.angular.z, twist.linear.x); + + // case RTRSTATE::turn2: { + // Eigen::Matrix3d roverToPlaneNorm; + // Eigen::Vector3d Nup = Eigen::Vector3d::UnitZ(); + // Eigen::Vector3d Nleft = up.cross(mBestNormalInWorld); + + // roverToPlaneNorm.col(0) = mBestNormalInWorld; + // roverToPlaneNorm.col(1) = Nup; + // roverToPlaneNorm.col(2) = Nleft; + // manif::SO3d roverToPlaneSO3 = SE3Conversions::fromColumns(roverToTargetForward, left, up); + + // manif::SO3Tangentd SO3tan = roverToPlaneSO3 - roverSO3; + // double angle_rate = mAngleP * SO3tan.z(); + // twist.angular.z = angle_rate; + + + + // if (std::abs(SO3tan.z()) < angular_thresh) { + // mLoopState = RTRSTATE::done; + // twist.angular.z = 0; + // twist.linear.x = angle_rate; + // // ROS_INFO("Done turning to lander"); + + // } + // // ROS_INFO("In state: turning to lander..."); + // break; + // } + + // case RTRSTATE::done: + // break; + // } + // ROS_INFO("THE TWIST IS: Angular: %f, with linear %f,", twist.angular.z, twist.linear.x); ROS_INFO_STREAM(mLoopState); - //mTwistPub.publish(twist); + mTwistPub.publish(twist); - rate.sleep(); + // rate.sleep(); } } } // namespace mrover diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index fb567afcc..d48f6f968 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -24,6 +24,10 @@ namespace mrover { class LanderAlignNodelet : public nodelet::Nodelet { + + //PID CONSTANTS + double const mAngleP = 1; + double const mLinearP = 0.1; ros::NodeHandle mNh, mPnh; ros::Publisher mDebugVectorPub; @@ -89,7 +93,7 @@ namespace mrover { // auto calculate(Eigen::Vector3d input, Eigen::Vector3d target) -> float; - [[nodiscard]] auto rotate_speed(float theta) const -> float; + [[nodiscard]] auto rotate_speed(double theta) const -> double; auto find_angle(Eigen::Vector3d const& current, Eigen::Vector3d const& target) -> float; diff --git a/src/util/lie/lie.cpp b/src/util/lie/lie.cpp index 664e03820..cfb7f2625 100644 --- a/src/util/lie/lie.cpp +++ b/src/util/lie/lie.cpp @@ -1,4 +1,5 @@ #include "lie.hpp" +#include SIM3::SIM3(SE3d const& se3, R3 const& scale) { mTransform.fromPositionOrientationScale(se3.translation(), se3.rotation(), scale); @@ -73,3 +74,11 @@ auto SE3Conversions::toTransformStamped(SE3d const& tf, std::string const& child transform.child_frame_id = childFrame; return transform; } + +auto SE3Conversions::fromColumns(R3 const &c1, R3 const &c2, R3 const &c3) -> SO3d { + Eigen::Matrix3d m; + m.col(0) = c1; + m.col(1) = c2; + m.col(2) = c3; + return {Eigen::Quaterniond{m}.normalized()}; +} diff --git a/src/util/lie/lie.hpp b/src/util/lie/lie.hpp index 59ec1d2b8..aefbb7e15 100644 --- a/src/util/lie/lie.hpp +++ b/src/util/lie/lie.hpp @@ -51,6 +51,8 @@ class SE3Conversions { * \param transform The transform or pose represented by an SE3 lie group element */ static auto pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& fromFrame, std::string const& toFrame, SE3d const& transform) -> void; + + static auto fromColumns(R3 const& c1, R3 const& c2, R3 const& c3) -> SO3d; }; class SIM3 { From e04ac3599c22eb1ccaeefaac5f4b92e88d3af165 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 10 Mar 2024 12:51:12 -0400 Subject: [PATCH 54/89] lander respurces --- config/simulator/simulator.yaml | 4 ++++ urdf/meshes/lander.fbx | 3 +++ urdf/staging/lander.blend | 3 +++ urdf/world/lander.urdf.xacro | 19 +++++++++++++++++++ 4 files changed, 29 insertions(+) create mode 100644 urdf/meshes/lander.fbx create mode 100644 urdf/staging/lander.blend create mode 100644 urdf/world/lander.urdf.xacro diff --git a/config/simulator/simulator.yaml b/config/simulator/simulator.yaml index 5df26b848..c29a30ca0 100644 --- a/config/simulator/simulator.yaml +++ b/config/simulator/simulator.yaml @@ -38,3 +38,7 @@ objects: name: rock4 uri: package://mrover/urdf/world/rock.urdf.xacro translation: [9, -2, 0.2] + - type: urdf + name: lander + uri: package://mrover/urdf/world/lander.urdf.xacro + translation: [10, 10, 10] diff --git a/urdf/meshes/lander.fbx b/urdf/meshes/lander.fbx new file mode 100644 index 000000000..a25c775e3 --- /dev/null +++ b/urdf/meshes/lander.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fab8ba4467811dcee2efd5256a88c3dada38f66da36c02544d23b19cd2c05636 +size 489356 diff --git a/urdf/staging/lander.blend b/urdf/staging/lander.blend new file mode 100644 index 000000000..71fcd340b --- /dev/null +++ b/urdf/staging/lander.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d23056f5ba9f7bd3a1c6ccf8375b1613495a466f3f4e9ce347da0c3730c0aee7 +size 2011888 diff --git a/urdf/world/lander.urdf.xacro b/urdf/world/lander.urdf.xacro new file mode 100644 index 000000000..ab31a942e --- /dev/null +++ b/urdf/world/lander.urdf.xacro @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + \ No newline at end of file From f9bb85eb06e73c79029777cee51cf14e056668d6 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 10 Mar 2024 14:24:10 -0400 Subject: [PATCH 55/89] need better path planning --- config/simulator/simulator.yaml | 2 +- src/teleoperation/lander_align/lander_align.cpp | 14 +++++++++----- src/teleoperation/lander_align/lander_align.hpp | 2 +- urdf/meshes/lander.fbx | 4 ++-- urdf/staging/lander.blend | 4 ++-- 5 files changed, 15 insertions(+), 11 deletions(-) diff --git a/config/simulator/simulator.yaml b/config/simulator/simulator.yaml index c29a30ca0..7743af2f3 100644 --- a/config/simulator/simulator.yaml +++ b/config/simulator/simulator.yaml @@ -41,4 +41,4 @@ objects: - type: urdf name: lander uri: package://mrover/urdf/world/lander.urdf.xacro - translation: [10, 10, 10] + translation: [8.5, 0.5, 0] diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 9daaa0d0b..caed2b778 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -61,7 +61,7 @@ namespace mrover { void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { filterNormals(cloud); - ransac(0.2, 10, 100); + ransac(0.1, 10, 100); if (mBestNormalInZED.has_value()) { geometry_msgs::Vector3 vect; vect.x = mBestNormalInZED.value().x(); @@ -213,7 +213,7 @@ namespace mrover { if(mBestNormalInZED.value().x() > 0) mBestNormalInZED.value() *=-1; - mBestOffsetInZED = std::make_optional(mBestLocationInZED.value() + mBestNormalInZED.value()); + mBestOffsetInZED = std::make_optional(mBestLocationInZED.value() + 2 * mBestNormalInZED.value()); uploadPC(numInliers, distanceThreshold); @@ -314,7 +314,7 @@ namespace mrover { //Threhsolds float const linear_thresh = 0.1; // could be member variables - float const angular_thresh = 0.005; + float const angular_thresh = 0.001; PID pid(0.1, 0.1); // literally just P -- ugly class and probably needs restructuring in the future @@ -364,6 +364,8 @@ namespace mrover { double angle_rate = mAngleP * SO3tan.z(); + + ROS_INFO("w_z velocity %f", SO3tan.z()); @@ -387,7 +389,9 @@ namespace mrover { twist.linear.x = 0; } - double driveRate = mLinearP * distanceToTarget; + double driveRate = std::min(mLinearP * distanceToTarget, 1.0); + + ROS_INFO("distance: %f", distanceToTarget); twist.linear.x = driveRate; @@ -419,7 +423,7 @@ namespace mrover { if (std::abs(SO3tan.z()) < angular_thresh) { mLoopState = RTRSTATE::done; twist.angular.z = 0; - twist.linear.x = angle_rate; + twist.linear.x = 0; // ROS_INFO("Done turning to lander"); } diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index d48f6f968..5991f0ce0 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -27,7 +27,7 @@ namespace mrover { //PID CONSTANTS double const mAngleP = 1; - double const mLinearP = 0.1; + double const mLinearP = 0.3; ros::NodeHandle mNh, mPnh; ros::Publisher mDebugVectorPub; diff --git a/urdf/meshes/lander.fbx b/urdf/meshes/lander.fbx index a25c775e3..9afebcec6 100644 --- a/urdf/meshes/lander.fbx +++ b/urdf/meshes/lander.fbx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:fab8ba4467811dcee2efd5256a88c3dada38f66da36c02544d23b19cd2c05636 -size 489356 +oid sha256:9ae7d51a81922c7d4ea01e3abdf79ff8b2ad733bee6294d71cd287cec056f5da +size 490268 diff --git a/urdf/staging/lander.blend b/urdf/staging/lander.blend index 71fcd340b..2cf538d52 100644 --- a/urdf/staging/lander.blend +++ b/urdf/staging/lander.blend @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:d23056f5ba9f7bd3a1c6ccf8375b1613495a466f3f4e9ce347da0c3730c0aee7 -size 2011888 +oid sha256:c403cc5bf23baf1166e1cefc162177cff07226dc80c2e20f93ccf62a14be69e9 +size 1991040 From 6ab3af47cc4038271cfc26b3697ec3ba2a25c717 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 14 Mar 2024 17:44:48 -0400 Subject: [PATCH 56/89] Pre Reframing --- .../lander_align/lander_align.cpp | 207 +++++++++--------- .../lander_align/lander_align.hpp | 21 +- 2 files changed, 122 insertions(+), 106 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index caed2b778..9675a6608 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -53,23 +54,77 @@ namespace mrover { mNh.param("camera_frame", mCameraFrameId, "zed_left_camera_frame"); mNh.param("world_frame", mMapFrameId, "map"); - mBestLocationInWorld = std::make_optional(0, 0, 0); - mBestNormalInWorld = std::make_optional(0, 0, 0); + mLocationInWorldVector = std::make_optional(0, 0, 0); + mNormalInWorldVector = std::make_optional(0, 0, 0); mLoopState = RTRSTATE::turn1; } void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { filterNormals(cloud); - ransac(0.1, 10, 100); - if (mBestNormalInZED.has_value()) { - geometry_msgs::Vector3 vect; - vect.x = mBestNormalInZED.value().x(); - vect.y = mBestNormalInZED.value().y(); - vect.z = mBestNormalInZED.value().z(); - mDebugVectorPub.publish(vect); + ransac(0.1, 10, 100, 2.5); + if (mNormalInZEDVector.has_value()) { sendTwist(); } + mLoopState = RTRSTATE::turn1; + ROS_INFO("Point cloud size"); + filterNormals(cloud); + ransac(0.1, 10, 100, 1); + if (mNormalInZEDVector.has_value()) { + sendTwist(); + } + + } + + //Returns angle (yaw) around the z axis + auto LanderAlignNodelet::calcAngleWithWorldX(Eigen::Vector3d xHeading) -> double{ //I want to be editing this variable so it should not be const or & + xHeading.z() = 0; + xHeading.normalize(); + Eigen::Vector3d xAxisWorld{1, 0, 0}; + double angle = std::acos(xHeading.dot(xAxisWorld)); + if(xHeading.y() >= 0){ + return angle; + }else{ + return angle + std::numbers::pi; + } + } + + void LanderAlignNodelet::calcMotion(double desiredVelocity, double desiredOmega){ + manif::SE3d roverInWorld = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); + Eigen::Vector3d xAxisRotation = roverInWorld.rotation().col(0); + double roverHeading = calcAngleWithWorldX(xAxisRotation); + double targetHeading = calcAngleWithWorldX(-mNormalInWorldVector.value()); + + //Current State + Eigen::Vector3d currState{roverInWorld.translation().x(), roverInWorld.translation().y(), roverHeading}; + + //Target State + Eigen::Vector3d tarState{mOffsetInWorldVector.value().x(), mOffsetInWorldVector.value().y(), targetHeading}; + + //Constants + double Kx = 1; + double Ky = 1; + double Ktheta = 1; + + Eigen::Matrix3d rotation; + rotation << std::cos(roverHeading), std::sin(roverHeading), 0, + -std::sin(roverHeading), std::cos(roverHeading), 0, + 0, 0, 1; + + Eigen::Vector3d errState = rotation * (tarState - currState); + + //I think this is rad/s + double zRotation = desiredOmega + (desiredVelocity / 2) * (Ky * (errState.y() + Ktheta*errState.z()) + (1/Ky) * std::sin(errState.z())); + + //I think this is unit/s + double xSpeed = Kx * errState.x() + desiredVelocity * std::cos(errState.z()) - Ktheta * errState.z() * zRotation; + + geometry_msgs::Twist twist; + twist.angular.z = zRotation; + twist.linear.x = xSpeed; + + mTwistPub.publish(twist); + } void LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud) { @@ -106,7 +161,9 @@ namespace mrover { size_t i = 0; for (auto p: mFilteredPoints) { // calculate distance of each point from potential plane - double distance = std::abs(mBestNormalInZED.value().x() * p->x + mBestNormalInZED.value().y() * p->y + mBestNormalInZED.value().z() * p->z + mBestOffset); + double distance = std::abs(mNormalInZEDVector.value().x() * p->x + mNormalInZEDVector.value().y() * p->y + mNormalInZEDVector.value().z() * p->z + mBestOffset); + // double distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); // + if (distance < distanceThreshold) { pcPtr[i].x = p->x; pcPtr[i].y = p->y; @@ -121,7 +178,7 @@ namespace mrover { mDebugPCPub.publish(debugPointCloudPtr); } - void LanderAlignNodelet::ransac(double const distanceThreshold, int minInliers, int const epochs) { + void LanderAlignNodelet::ransac(double const distanceThreshold, int minInliers, int const epochs, double offsetFactor) { // TODO: use RANSAC to find the lander face, should be the closest, we may need to modify this to output more information, currently the output is the normal // takes 3 samples for every epoch and terminates after specified number of epochs // Eigen::Vector3d mBestNormal(0, 0, 0); // normal vector representing plane (initialize as zero vector?? default ctor??) @@ -133,17 +190,17 @@ namespace mrover { std::uniform_int_distribution distribution(0, (int) mFilteredPoints.size() - 1); if (mFilteredPoints.size() < 3) { - mBestNormalInZED = std::nullopt; - mBestLocationInZED = std::nullopt; + mNormalInZEDVector = std::nullopt; + mLocationInZEDVector = std::nullopt; return; } - mBestNormalInZED = std::make_optional(0, 0, 0); - mBestLocationInZED = std::make_optional(0, 0, 0); + mNormalInZEDVector = std::make_optional(0, 0, 0); + mLocationInZEDVector = std::make_optional(0, 0, 0); int numInliers = 0; - while (mBestNormalInZED.value().isZero()) { // TODO add give up condition after X iter + while (mNormalInZEDVector.value().isZero()) { // TODO add give up condition after X iter for (int i = 0; i < epochs; ++i) { // currentCenter *= 0; // Set all vals in current center to zero at the start of each epoch // sample 3 random points (potential inliers) @@ -180,7 +237,7 @@ namespace mrover { // update best plane if better inlier count if (numInliers > minInliers && normal.x() != 0 && normal.y() != 0 && normal.z() != 0) { minInliers = numInliers; - mBestNormalInZED.value() = normal; + mNormalInZEDVector.value() = normal; mBestOffset = offset; // If this is the new best plane, set mBestCenterInZED @@ -191,79 +248,69 @@ namespace mrover { // Run through one more loop to identify the center of the plane (one possibility for determining best center) numInliers = 0; + mLocationInZEDVector = std::make_optional(Eigen::Vector3d::Zero()); for (auto p: mFilteredPoints) { // calculate distance of each point from potential plane - double distance = std::abs(mBestNormalInZED.value().x() * p->x + mBestNormalInZED.value().y() * p->y + mBestNormalInZED.value().z() * p->z + mBestOffset); + double distance = std::abs(mNormalInZEDVector.value().x() * p->x + mNormalInZEDVector.value().y() * p->y + mNormalInZEDVector.value().z() * p->z + mBestOffset); if (distance < distanceThreshold) { - mBestLocationInZED.value().x() += p->x; - mBestLocationInZED.value().y() += p->y; - mBestLocationInZED.value().z() += p->z; + mLocationInZEDVector.value().x() += p->x; + mLocationInZEDVector.value().y() += p->y; + mLocationInZEDVector.value().z() += p->z; ++numInliers; // count num of inliers that pass the "good enough fit" threshold } } if (numInliers == 0) { - mBestNormalInZED = std::nullopt; - mBestLocationInZED = std::nullopt; + mNormalInZEDVector = std::nullopt; + mLocationInZEDVector = std::nullopt; return; } //Average pnts - mBestLocationInZED.value() /= static_cast(numInliers); + mLocationInZEDVector.value() /= static_cast(numInliers); - if(mBestNormalInZED.value().x() > 0) mBestNormalInZED.value() *=-1; + + uploadPC(numInliers, distanceThreshold); + + if(mNormalInZEDVector.value().x() > 0) mNormalInZEDVector.value() *=-1; + + + mOffsetInZEDVector = std::make_optional(mLocationInZEDVector.value() + offsetFactor * mNormalInZEDVector.value()); - mBestOffsetInZED = std::make_optional(mBestLocationInZED.value() + 2 * mBestNormalInZED.value()); - uploadPC(numInliers, distanceThreshold); manif::SE3d zedToMap = SE3Conversions::fromTfTree(mTfBuffer, mCameraFrameId, mMapFrameId); //Calculate the normal in the world frame - mBestNormalInWorld = std::make_optional(zedToMap.rotation().matrix() * mBestNormalInZED.value()); + mNormalInWorldVector = std::make_optional(zedToMap.rotation().matrix() * mNormalInZEDVector.value()); //Calculate the SO3 in the world frame Eigen::Matrix3d rot; - Eigen::Vector3d forward = mBestNormalInWorld.value().normalized(); + Eigen::Vector3d forward = mNormalInWorldVector.value().normalized(); Eigen::Vector3d worldUp = Eigen::Vector3d::UnitZ(); Eigen::Vector3d left = worldUp.cross(forward); Eigen::Vector3d up = forward.cross(left); - ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z(), forward.x(), forward.y(), forward.z()); + ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mLocationInZEDVector.value().x(), mLocationInZEDVector.value().y(), mLocationInZEDVector.value().z(), forward.x(), forward.y(), forward.z()); rot.col(0) = forward; rot.col(1) = left; rot.col(2) = up; //Calculate the plane location in the world frame - manif::SE3d plane_loc_in_world = {{mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; - plane_loc_in_world = zedToMap * plane_loc_in_world; - manif::SE3d plane_loc_in_world_final = {{plane_loc_in_world.translation().x(),plane_loc_in_world.translation().y(),plane_loc_in_world.translation().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; - mBestLocationInWorld = std::make_optional(plane_loc_in_world_final.translation()); + manif::SE3d plane_loc_in_zed = {{mLocationInZEDVector.value().x(), mLocationInZEDVector.value().y(), mLocationInZEDVector.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; + manif::SE3d plane_loc_in_world = zedToMap * plane_loc_in_zed; + plane_loc_in_world_final = {{plane_loc_in_world.translation().x(),plane_loc_in_world.translation().y(),plane_loc_in_world.translation().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; + mLocationInWorldVector = std::make_optional(plane_loc_in_world_final.translation()); //Calculate the offset location in the world frame - manif::SE3d offset_loc_in_world = {{mBestOffsetInZED.value().x(), mBestOffsetInZED.value().y(), mBestOffsetInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; + manif::SE3d offset_loc_in_world = {{mOffsetInZEDVector.value().x(), mOffsetInZEDVector.value().y(), mOffsetInZEDVector.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; offset_loc_in_world = zedToMap * offset_loc_in_world; - manif::SE3d offset_loc_in_world_final = {{offset_loc_in_world.translation().x(),offset_loc_in_world.translation().y(),offset_loc_in_world.translation().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; - mBestOffsetInWorld = std::make_optional(offset_loc_in_world_final.translation()); - + offset_loc_in_world_final = {{offset_loc_in_world.translation().x(),offset_loc_in_world.translation().y(),offset_loc_in_world.translation().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; + mOffsetInWorldVector = std::make_optional(offset_loc_in_world_final.translation()); //Push to the tf tree SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, plane_loc_in_world_final); SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, offset_loc_in_world_final); - - //For the normal - manif::SE3d mNormalLocInWorld = {{mBestNormalInWorld.value().x(), mBestNormalInWorld.value().y(), mBestNormalInWorld.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};//TODO: THIS IS A RANDOM ROTATION MATRIX - - SE3Conversions::pushToTfTree(mTfBroadcaster, "normalInWorld", mMapFrameId, mNormalLocInWorld); - - //For the normal - manif::SE3d mNormalLocInZED = {{mBestNormalInZED.value().x(), mBestNormalInZED.value().y(), mBestNormalInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};//TODO: THIS IS A RANDOM ROTATION MATRIX - - SE3Conversions::pushToTfTree(mTfBroadcaster, "normalInZED", mMapFrameId, mNormalLocInZED); - - - //ROS_INFO("Max inliers: %i", minInliers); - //ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z(), mBestNormalInWorld.value().x(), mBestNormalInWorld.value().y(), mBestNormalInWorld.value().z()); } auto LanderAlignNodelet::PID::rotate_speed(double theta) const -> double { @@ -327,13 +374,16 @@ namespace mrover { // roverToPlaneNorm.col(2) = Nleft; // manif::SO3d roverToP while (ros::ok()) { + + SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, plane_loc_in_world_final); + SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, offset_loc_in_world_final); manif::SE3d rover = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); Eigen::Vector3d roverPosInWorld{static_cast(rover.translation().x()), static_cast(rover.translation().y()), 0.0}; Eigen::Vector3d targetPosInWorld; if(mLoopState == RTRSTATE::turn1){ - targetPosInWorld = mBestOffsetInWorld.value(); + targetPosInWorld = mOffsetInWorldVector.value(); }else if(mLoopState == RTRSTATE::turn2){ - targetPosInWorld = mBestLocationInWorld.value(); + targetPosInWorld = mLocationInWorldVector.value(); } targetPosInWorld.z() = 0; @@ -364,11 +414,8 @@ namespace mrover { double angle_rate = mAngleP * SO3tan.z(); - - ROS_INFO("w_z velocity %f", SO3tan.z()); - twist.angular.z = angle_rate; if (std::abs(SO3tan.z()) < angular_thresh) { @@ -407,17 +454,9 @@ namespace mrover { } case RTRSTATE::turn2: { - // Eigen::Matrix3d roverToPlaneNorm; - // Eigen::Vector3d Nup = Eigen::Vector3d::UnitZ(); - // Eigen::Vector3d Nleft = Nup.cross(mBestNormalInWorld.value()); - - // roverToPlaneNorm.col(0) = mBestNormalInWorld.value(); - // roverToPlaneNorm.col(1) = Nup; - // roverToPlaneNorm.col(2) = Nleft; - // manif::SO3d roverToPlaneSO3 = SE3Conversions::fromColumns(roverToTargetForward, left, up); - double angle_rate = mAngleP * SO3tan.z(); twist.angular.z = angle_rate; + twist.linear.x = 0; if (std::abs(SO3tan.z()) < angular_thresh) { @@ -437,42 +476,12 @@ namespace mrover { break; } } - - // case RTRSTATE::turn2: { - // Eigen::Matrix3d roverToPlaneNorm; - // Eigen::Vector3d Nup = Eigen::Vector3d::UnitZ(); - // Eigen::Vector3d Nleft = up.cross(mBestNormalInWorld); - - // roverToPlaneNorm.col(0) = mBestNormalInWorld; - // roverToPlaneNorm.col(1) = Nup; - // roverToPlaneNorm.col(2) = Nleft; - // manif::SO3d roverToPlaneSO3 = SE3Conversions::fromColumns(roverToTargetForward, left, up); - - // manif::SO3Tangentd SO3tan = roverToPlaneSO3 - roverSO3; - // double angle_rate = mAngleP * SO3tan.z(); - // twist.angular.z = angle_rate; - - - - // if (std::abs(SO3tan.z()) < angular_thresh) { - // mLoopState = RTRSTATE::done; - // twist.angular.z = 0; - // twist.linear.x = angle_rate; - // // ROS_INFO("Done turning to lander"); - - // } - // // ROS_INFO("In state: turning to lander..."); - // break; - // } - - // case RTRSTATE::done: - // break; - // } // ROS_INFO("THE TWIST IS: Angular: %f, with linear %f,", twist.angular.z, twist.linear.x); ROS_INFO_STREAM(mLoopState); mTwistPub.publish(twist); - - // rate.sleep(); + if(mLoopState == RTRSTATE::done){ + break; + } } } } // namespace mrover diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 5991f0ce0..a2141147a 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -45,14 +45,17 @@ namespace mrover { //** double mBestOffset; - std::optional mBestLocationInZED; - std::optional mBestLocationInWorld; + std::optional mLocationInZEDVector; + std::optional mLocationInWorldVector; - std::optional mBestOffsetInZED; - std::optional mBestOffsetInWorld; + std::optional mOffsetInZEDVector; + std::optional mOffsetInWorldVector; - std::optional mBestNormalInZED; - std::optional mBestNormalInWorld; + std::optional mNormalInZEDVector; + std::optional mNormalInWorldVector; + + manif::SE3d mOffsetLocationInWorldSE3d; + manif::SE3d mPlaneLocationInWorldSE3d; //** //TF Member Variables @@ -76,12 +79,16 @@ namespace mrover { void filterNormals(sensor_msgs::PointCloud2Ptr const& cloud); - void ransac(double distanceThreshold, int minInliers, int epochs); + void ransac(double distanceThreshold, int minInliers, int epochs, double offsetFactor); void sendTwist(); void uploadPC(int numInliers, double distanceThreshold); + void calcMotion(double desiredVelocity, double desiredOmega); + + static auto calcAngleWithWorldX(Eigen::Vector3d xHeading) -> double; + class PID { private: float const Angle_P; From 1ae4adac62ceb93cabfcd667e2c94003913fa1c9 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 14 Mar 2024 17:53:13 -0400 Subject: [PATCH 57/89] Refactoring --- .../lander_align/lander_align.cpp | 49 ++++++++----------- 1 file changed, 21 insertions(+), 28 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 9675a6608..7fad4a1f0 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -63,16 +63,16 @@ namespace mrover { void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { filterNormals(cloud); ransac(0.1, 10, 100, 2.5); - if (mNormalInZEDVector.has_value()) { - sendTwist(); - } - mLoopState = RTRSTATE::turn1; - ROS_INFO("Point cloud size"); - filterNormals(cloud); - ransac(0.1, 10, 100, 1); - if (mNormalInZEDVector.has_value()) { - sendTwist(); - } + // if (mNormalInZEDVector.has_value()) { + // sendTwist(); + // } + // mLoopState = RTRSTATE::turn1; + // ROS_INFO("Point cloud size"); + // filterNormals(cloud); + // ransac(0.1, 10, 100, 1); + // if (mNormalInZEDVector.has_value()) { + // sendTwist(); + // } } @@ -277,16 +277,11 @@ namespace mrover { mOffsetInZEDVector = std::make_optional(mLocationInZEDVector.value() + offsetFactor * mNormalInZEDVector.value()); - - manif::SE3d zedToMap = SE3Conversions::fromTfTree(mTfBuffer, mCameraFrameId, mMapFrameId); - //Calculate the normal in the world frame - mNormalInWorldVector = std::make_optional(zedToMap.rotation().matrix() * mNormalInZEDVector.value()); - //Calculate the SO3 in the world frame Eigen::Matrix3d rot; - Eigen::Vector3d forward = mNormalInWorldVector.value().normalized(); + Eigen::Vector3d forward = mNormalInZEDVector.value().normalized(); Eigen::Vector3d worldUp = Eigen::Vector3d::UnitZ(); Eigen::Vector3d left = worldUp.cross(forward); Eigen::Vector3d up = forward.cross(left); @@ -297,20 +292,18 @@ namespace mrover { rot.col(2) = up; //Calculate the plane location in the world frame - manif::SE3d plane_loc_in_zed = {{mLocationInZEDVector.value().x(), mLocationInZEDVector.value().y(), mLocationInZEDVector.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; - manif::SE3d plane_loc_in_world = zedToMap * plane_loc_in_zed; - plane_loc_in_world_final = {{plane_loc_in_world.translation().x(),plane_loc_in_world.translation().y(),plane_loc_in_world.translation().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; - mLocationInWorldVector = std::make_optional(plane_loc_in_world_final.translation()); + manif::SE3d mPlaneLocationInZEDSE3d = {{mLocationInZEDVector.value().x(), mLocationInZEDVector.value().y(), mLocationInZEDVector.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; + mPlaneLocationInWorldSE3d = zedToMap * mPlaneLocationInZEDSE3d; + mLocationInWorldVector = std::make_optional(mPlaneLocationInWorldSE3d.translation()); //Calculate the offset location in the world frame - manif::SE3d offset_loc_in_world = {{mOffsetInZEDVector.value().x(), mOffsetInZEDVector.value().y(), mOffsetInZEDVector.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; - offset_loc_in_world = zedToMap * offset_loc_in_world; - offset_loc_in_world_final = {{offset_loc_in_world.translation().x(),offset_loc_in_world.translation().y(),offset_loc_in_world.translation().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; - mOffsetInWorldVector = std::make_optional(offset_loc_in_world_final.translation()); + manif::SE3d mOffsetLocationInZEDSE3d = {{mOffsetInZEDVector.value().x(), mOffsetInZEDVector.value().y(), mOffsetInZEDVector.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; + mOffsetLocationInWorldSE3d = zedToMap * mOffsetLocationInZEDSE3d; + mOffsetInWorldVector = std::make_optional(mOffsetLocationInWorldSE3d.translation()); //Push to the tf tree - SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, plane_loc_in_world_final); - SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, offset_loc_in_world_final); + SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, mPlaneLocationInWorldSE3d); + SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, mOffsetLocationInWorldSE3d); } auto LanderAlignNodelet::PID::rotate_speed(double theta) const -> double { @@ -375,8 +368,8 @@ namespace mrover { // manif::SO3d roverToP while (ros::ok()) { - SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, plane_loc_in_world_final); - SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, offset_loc_in_world_final); + SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, mPlaneLocationInWorldSE3d); + SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, mOffsetLocationInWorldSE3d); manif::SE3d rover = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); Eigen::Vector3d roverPosInWorld{static_cast(rover.translation().x()), static_cast(rover.translation().y()), 0.0}; Eigen::Vector3d targetPosInWorld; From e0367bead30f153ca38355f175ac11e71e49c27c Mon Sep 17 00:00:00 2001 From: John Date: Thu, 14 Mar 2024 18:19:12 -0400 Subject: [PATCH 58/89] WIP --- .../lander_align/lander_align.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 7fad4a1f0..5baf020cc 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -63,16 +63,16 @@ namespace mrover { void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { filterNormals(cloud); ransac(0.1, 10, 100, 2.5); - // if (mNormalInZEDVector.has_value()) { - // sendTwist(); - // } - // mLoopState = RTRSTATE::turn1; - // ROS_INFO("Point cloud size"); - // filterNormals(cloud); - // ransac(0.1, 10, 100, 1); - // if (mNormalInZEDVector.has_value()) { - // sendTwist(); - // } + if (mNormalInZEDVector.has_value()) { + sendTwist(); + } + mLoopState = RTRSTATE::turn1; + ROS_INFO("Point cloud size"); + filterNormals(cloud); + ransac(0.1, 10, 100, 1); + if (mNormalInZEDVector.has_value()) { + sendTwist(); + } } From 4ef4d174fe7f6432de0c3931fcba4038fc72cf94 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 14 Mar 2024 19:17:51 -0400 Subject: [PATCH 59/89] Working Align --- config/simulator/simulator.yaml | 2 +- .../lander_align/lander_align.cpp | 57 ++++++++++--------- .../lander_align/lander_align.hpp | 13 +++-- urdf/world/lander.urdf.xacro | 4 +- 4 files changed, 40 insertions(+), 36 deletions(-) diff --git a/config/simulator/simulator.yaml b/config/simulator/simulator.yaml index 7743af2f3..b13c33f14 100644 --- a/config/simulator/simulator.yaml +++ b/config/simulator/simulator.yaml @@ -41,4 +41,4 @@ objects: - type: urdf name: lander uri: package://mrover/urdf/world/lander.urdf.xacro - translation: [8.5, 0.5, 0] + translation: [9.81, -4.73, 2] diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 5baf020cc..0c8435238 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -44,7 +44,9 @@ namespace mrover { auto LanderAlignNodelet::onInit() -> void { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mZThreshold = .1; + mZThreshold = .5; + mXThreshold = .1; + mPlaneOffsetScalar = 2.5; mVectorSub = mNh.subscribe("/camera/left/points", 1, &LanderAlignNodelet::LanderCallback, this); mDebugVectorPub = mNh.advertise("/lander_align/Pose", 1); mTwistPub = mNh.advertise("/cmd_vel", 1); @@ -54,26 +56,25 @@ namespace mrover { mNh.param("camera_frame", mCameraFrameId, "zed_left_camera_frame"); mNh.param("world_frame", mMapFrameId, "map"); - mLocationInWorldVector = std::make_optional(0, 0, 0); + mPlaneLocationInWorldVector = std::make_optional(0, 0, 0); mNormalInWorldVector = std::make_optional(0, 0, 0); mLoopState = RTRSTATE::turn1; } void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { + filterNormals(cloud); - ransac(0.1, 10, 100, 2.5); + ransac(0.1, 10, 100, mPlaneOffsetScalar); if (mNormalInZEDVector.has_value()) { sendTwist(); } - mLoopState = RTRSTATE::turn1; - ROS_INFO("Point cloud size"); - filterNormals(cloud); - ransac(0.1, 10, 100, 1); - if (mNormalInZEDVector.has_value()) { - sendTwist(); + if(mPlaneOffsetScalar == 2.5){ + mPlaneOffsetScalar = 0.4; + mLoopState = RTRSTATE::turn1; + }else{ + mLoopState = RTRSTATE::done; } - } //Returns angle (yaw) around the z axis @@ -99,7 +100,7 @@ namespace mrover { Eigen::Vector3d currState{roverInWorld.translation().x(), roverInWorld.translation().y(), roverHeading}; //Target State - Eigen::Vector3d tarState{mOffsetInWorldVector.value().x(), mOffsetInWorldVector.value().y(), targetHeading}; + Eigen::Vector3d tarState{mOffsetLocationInWorldVector.value().x(), mOffsetLocationInWorldVector.value().y(), targetHeading}; //Constants double Kx = 1; @@ -139,7 +140,7 @@ namespace mrover { for (auto point = cloudData; point < cloudData + (cloud->height * cloud->width); point += pointDistribution(generator)) { bool isPointInvalid = (!std::isfinite(point->x) || !std::isfinite(point->y) || !std::isfinite(point->z)); - if (abs(point->normal_z) < mZThreshold && !isPointInvalid) { + if (abs(point->normal_z) < mZThreshold && !isPointInvalid && abs(point->normal_x) > mXThreshold) { mFilteredPoints.push_back(point); // ROS_INFO("Filtered point: %f, %f, %f", point->normal_x, point->normal_y, point->normal_z); } @@ -191,13 +192,13 @@ namespace mrover { if (mFilteredPoints.size() < 3) { mNormalInZEDVector = std::nullopt; - mLocationInZEDVector = std::nullopt; + mPlaneLocationInZEDVector = std::nullopt; return; } mNormalInZEDVector = std::make_optional(0, 0, 0); - mLocationInZEDVector = std::make_optional(0, 0, 0); + mPlaneLocationInZEDVector = std::make_optional(0, 0, 0); int numInliers = 0; while (mNormalInZEDVector.value().isZero()) { // TODO add give up condition after X iter @@ -248,26 +249,26 @@ namespace mrover { // Run through one more loop to identify the center of the plane (one possibility for determining best center) numInliers = 0; - mLocationInZEDVector = std::make_optional(Eigen::Vector3d::Zero()); + mPlaneLocationInZEDVector = std::make_optional(Eigen::Vector3d::Zero()); for (auto p: mFilteredPoints) { // calculate distance of each point from potential plane double distance = std::abs(mNormalInZEDVector.value().x() * p->x + mNormalInZEDVector.value().y() * p->y + mNormalInZEDVector.value().z() * p->z + mBestOffset); if (distance < distanceThreshold) { - mLocationInZEDVector.value().x() += p->x; - mLocationInZEDVector.value().y() += p->y; - mLocationInZEDVector.value().z() += p->z; + mPlaneLocationInZEDVector.value().x() += p->x; + mPlaneLocationInZEDVector.value().y() += p->y; + mPlaneLocationInZEDVector.value().z() += p->z; ++numInliers; // count num of inliers that pass the "good enough fit" threshold } } if (numInliers == 0) { mNormalInZEDVector = std::nullopt; - mLocationInZEDVector = std::nullopt; + mPlaneLocationInZEDVector = std::nullopt; return; } //Average pnts - mLocationInZEDVector.value() /= static_cast(numInliers); + mPlaneLocationInZEDVector.value() /= static_cast(numInliers); uploadPC(numInliers, distanceThreshold); @@ -275,7 +276,7 @@ namespace mrover { if(mNormalInZEDVector.value().x() > 0) mNormalInZEDVector.value() *=-1; - mOffsetInZEDVector = std::make_optional(mLocationInZEDVector.value() + offsetFactor * mNormalInZEDVector.value()); + mOffsetLocationInZEDVector = std::make_optional(mPlaneLocationInZEDVector.value() + offsetFactor * mNormalInZEDVector.value()); manif::SE3d zedToMap = SE3Conversions::fromTfTree(mTfBuffer, mCameraFrameId, mMapFrameId); @@ -285,21 +286,21 @@ namespace mrover { Eigen::Vector3d worldUp = Eigen::Vector3d::UnitZ(); Eigen::Vector3d left = worldUp.cross(forward); Eigen::Vector3d up = forward.cross(left); - ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mLocationInZEDVector.value().x(), mLocationInZEDVector.value().y(), mLocationInZEDVector.value().z(), forward.x(), forward.y(), forward.z()); + ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mPlaneLocationInZEDVector.value().x(), mPlaneLocationInZEDVector.value().y(), mPlaneLocationInZEDVector.value().z(), forward.x(), forward.y(), forward.z()); rot.col(0) = forward; rot.col(1) = left; rot.col(2) = up; //Calculate the plane location in the world frame - manif::SE3d mPlaneLocationInZEDSE3d = {{mLocationInZEDVector.value().x(), mLocationInZEDVector.value().y(), mLocationInZEDVector.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; + manif::SE3d mPlaneLocationInZEDSE3d = {{mPlaneLocationInZEDVector.value().x(), mPlaneLocationInZEDVector.value().y(), mPlaneLocationInZEDVector.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; mPlaneLocationInWorldSE3d = zedToMap * mPlaneLocationInZEDSE3d; - mLocationInWorldVector = std::make_optional(mPlaneLocationInWorldSE3d.translation()); + mPlaneLocationInWorldVector = std::make_optional(mPlaneLocationInWorldSE3d.translation()); //Calculate the offset location in the world frame - manif::SE3d mOffsetLocationInZEDSE3d = {{mOffsetInZEDVector.value().x(), mOffsetInZEDVector.value().y(), mOffsetInZEDVector.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; + manif::SE3d mOffsetLocationInZEDSE3d = {{mOffsetLocationInZEDVector.value().x(), mOffsetLocationInZEDVector.value().y(), mOffsetLocationInZEDVector.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; mOffsetLocationInWorldSE3d = zedToMap * mOffsetLocationInZEDSE3d; - mOffsetInWorldVector = std::make_optional(mOffsetLocationInWorldSE3d.translation()); + mOffsetLocationInWorldVector = std::make_optional(mOffsetLocationInWorldSE3d.translation()); //Push to the tf tree SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, mPlaneLocationInWorldSE3d); @@ -374,9 +375,9 @@ namespace mrover { Eigen::Vector3d roverPosInWorld{static_cast(rover.translation().x()), static_cast(rover.translation().y()), 0.0}; Eigen::Vector3d targetPosInWorld; if(mLoopState == RTRSTATE::turn1){ - targetPosInWorld = mOffsetInWorldVector.value(); + targetPosInWorld = mOffsetLocationInWorldVector.value(); }else if(mLoopState == RTRSTATE::turn2){ - targetPosInWorld = mLocationInWorldVector.value(); + targetPosInWorld = mPlaneLocationInWorldVector.value(); } targetPosInWorld.z() = 0; diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index a2141147a..4fb2ed7af 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -45,11 +45,13 @@ namespace mrover { //** double mBestOffset; - std::optional mLocationInZEDVector; - std::optional mLocationInWorldVector; + double mPlaneOffsetScalar; - std::optional mOffsetInZEDVector; - std::optional mOffsetInWorldVector; + std::optional mPlaneLocationInZEDVector; + std::optional mPlaneLocationInWorldVector; + + std::optional mOffsetLocationInZEDVector; + std::optional mOffsetLocationInWorldVector; std::optional mNormalInZEDVector; std::optional mNormalInWorldVector; @@ -66,7 +68,8 @@ namespace mrover { std::string mMapFrameId; //Ransac Params - float mZThreshold; + double mZThreshold; + double mXThreshold; int mLeastSamplingDistribution = 10; std::vector mFilteredPoints; diff --git a/urdf/world/lander.urdf.xacro b/urdf/world/lander.urdf.xacro index ab31a942e..912337777 100644 --- a/urdf/world/lander.urdf.xacro +++ b/urdf/world/lander.urdf.xacro @@ -2,8 +2,8 @@ - - + + From 075f071821db514e394fcba8976e324be2e8b172 Mon Sep 17 00:00:00 2001 From: Joshua Huang Date: Sun, 17 Mar 2024 12:34:38 -0400 Subject: [PATCH 60/89] Boilerplate teleop lander align state --- src/teleoperation/backend/consumers.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 42f3539cc..59e7d894c 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -141,6 +141,7 @@ def receive(self, text_data): message = json.loads(text_data) try: if message["type"] == "joystick_values": + if not lander_align self.handle_joystick_message(message) elif message["type"] == "disable_auton_led": self.disable_auton_led() @@ -711,3 +712,6 @@ def flight_attitude_listener(self): self.send(text_data=json.dumps({"type": "flight_attitude", "pitch": pitch, "roll": roll})) rate.sleep() + + def start_lander_align(self) -> None: + pass From 7194f89a0ca1fda4eb7c534750d91a6da2467bda Mon Sep 17 00:00:00 2001 From: John Date: Fri, 15 Mar 2024 15:40:03 -0400 Subject: [PATCH 61/89] Bet --- action/LanderAlign.action | 8 ++++++-- src/teleoperation/lander_align/lander_align.hpp | 11 ++++++----- src/teleoperation/lander_align/pch.hpp | 2 ++ 3 files changed, 14 insertions(+), 7 deletions(-) diff --git a/action/LanderAlign.action b/action/LanderAlign.action index c1024312e..ea5f86238 100644 --- a/action/LanderAlign.action +++ b/action/LanderAlign.action @@ -1,4 +1,8 @@ -float32 rotateSpeed -float32 driveSpeed +# Define the goal +uint32 num # Specify which dishwasher we want to use --- +# Define the result +uint32 num --- +# Define a feedback message +float32 percent_complete \ No newline at end of file diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 4fb2ed7af..7df5b003f 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -8,6 +8,8 @@ namespace mrover { + typedef actionlib::SimpleActionServer Server; + enum RTRSTATE { turn1 = 0, drive = 1, @@ -25,19 +27,21 @@ namespace mrover { class LanderAlignNodelet : public nodelet::Nodelet { + //PID CONSTANTS double const mAngleP = 1; double const mLinearP = 0.3; ros::NodeHandle mNh, mPnh; + Server mActionServer{mNh, "LanderAlignAction", [&] (& const LanderAlignNodelet::LanderCallback), false}; + + ros::Publisher mDebugVectorPub; ros::Publisher mDebugPCPub; - ros::Publisher mTwistPub; - ros::Subscriber mVectorSub; RTRSTATE mLoopState; @@ -75,9 +79,6 @@ namespace mrover { auto onInit() -> void override; - // deprecated/not needed anymore - // auto downsample(sensor_msgs::PointCloud2Ptr const& cloud) -> sensor_msgs::PointCloud2Ptr; - void LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud); void filterNormals(sensor_msgs::PointCloud2Ptr const& cloud); diff --git a/src/teleoperation/lander_align/pch.hpp b/src/teleoperation/lander_align/pch.hpp index 221b30aad..b9a684221 100644 --- a/src/teleoperation/lander_align/pch.hpp +++ b/src/teleoperation/lander_align/pch.hpp @@ -6,8 +6,10 @@ #include #include #include +#include #include #include +#include "mrover/LanderAlignAction.h" //TF #include From 0feb0b569e547631614689370825fa5f42467ee1 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 17 Mar 2024 12:42:20 -0400 Subject: [PATCH 62/89] Action Server stuff --- action/LanderAlign.action | 2 +- src/teleoperation/lander_align/lander_align.cpp | 13 ++++++++++++- src/teleoperation/lander_align/lander_align.hpp | 8 ++++++-- 3 files changed, 19 insertions(+), 4 deletions(-) diff --git a/action/LanderAlign.action b/action/LanderAlign.action index ea5f86238..f70588fba 100644 --- a/action/LanderAlign.action +++ b/action/LanderAlign.action @@ -5,4 +5,4 @@ uint32 num # Specify which dishwasher we want to use uint32 num --- # Define a feedback message -float32 percent_complete \ No newline at end of file +uint32 count_ten \ No newline at end of file diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 0c8435238..86cb55fea 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,5 +1,8 @@ #include "lander_align.hpp" #include "lie.hpp" +#include "mrover/LanderAlignActionFeedback.h" +#include "mrover/LanderAlignActionGoal.h" +#include "mrover/LanderAlignActionResult.h" #include "pch.hpp" #include #include @@ -58,7 +61,6 @@ namespace mrover { mPlaneLocationInWorldVector = std::make_optional(0, 0, 0); mNormalInWorldVector = std::make_optional(0, 0, 0); - mLoopState = RTRSTATE::turn1; } @@ -77,6 +79,15 @@ namespace mrover { } } + void LanderAlignNodelet::ActionServerCallBack(mrover::LanderAlignActionGoalConstPtr const& actionRequest){ + LanderAlignActionFeedback feedback; + LanderAlignActionResult result; + for(int i = 0; i < static_cast(actionRequest->goal.num); i++){ + //feedback.feedback.percent_complete=i; + } + } + + //Returns angle (yaw) around the z axis auto LanderAlignNodelet::calcAngleWithWorldX(Eigen::Vector3d xHeading) -> double{ //I want to be editing this variable so it should not be const or & xHeading.z() = 0; diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 7df5b003f..a874ea19c 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -1,5 +1,7 @@ #pragma once +#include "mrover/LanderAlignAction.h" +#include "mrover/LanderAlignActionGoal.h" #include "pch.hpp" #include #include @@ -8,7 +10,7 @@ namespace mrover { - typedef actionlib::SimpleActionServer Server; + typedef actionlib::SimpleActionServer Server; enum RTRSTATE { turn1 = 0, @@ -33,7 +35,7 @@ namespace mrover { double const mLinearP = 0.3; ros::NodeHandle mNh, mPnh; - Server mActionServer{mNh, "LanderAlignAction", [&] (& const LanderAlignNodelet::LanderCallback), false}; + Server mActionServer = Server(mNh, "LanderAlignAction", [&](const mrover::LanderAlignActionGoalConstPtr& goal){ActionServerCallBack(goal);}, false); ros::Publisher mDebugVectorPub; @@ -80,6 +82,8 @@ namespace mrover { auto onInit() -> void override; void LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud); + + void ActionServerCallBack(LanderAlignActionGoalConstPtr const& actionRequest); void filterNormals(sensor_msgs::PointCloud2Ptr const& cloud); From de073a0ce5a11804f6b6e6cfa0d1e3a06592e3bb Mon Sep 17 00:00:00 2001 From: John Date: Sun, 17 Mar 2024 13:14:07 -0400 Subject: [PATCH 63/89] COOOKED --- src/teleoperation/lander_align/lander_align.cpp | 9 +++++---- src/teleoperation/lander_align/lander_align.hpp | 10 ++-------- src/teleoperation/lander_align/pch.hpp | 2 +- 3 files changed, 8 insertions(+), 13 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 86cb55fea..2e9070c45 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -79,12 +79,13 @@ namespace mrover { } } - void LanderAlignNodelet::ActionServerCallBack(mrover::LanderAlignActionGoalConstPtr const& actionRequest){ + void LanderAlignNodelet::ActionServerCallBack(LanderAlignActionGoalConstPtr const& actionRequest){ LanderAlignActionFeedback feedback; LanderAlignActionResult result; - for(int i = 0; i < static_cast(actionRequest->goal.num); i++){ - //feedback.feedback.percent_complete=i; - } + // while(actionRequest.action_goal.goal.num); + // for(int i = 0; i < static_cast(actionRequest.goal.num); i++){ + // //feedback.feedback.percent_complete=i; + // } } diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index a874ea19c..91eb59aba 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -1,16 +1,10 @@ #pragma once -#include "mrover/LanderAlignAction.h" -#include "mrover/LanderAlignActionGoal.h" #include "pch.hpp" -#include -#include -#include -#include namespace mrover { - typedef actionlib::SimpleActionServer Server; + using Server = actionlib::SimpleActionServer; enum RTRSTATE { turn1 = 0, @@ -35,7 +29,7 @@ namespace mrover { double const mLinearP = 0.3; ros::NodeHandle mNh, mPnh; - Server mActionServer = Server(mNh, "LanderAlignAction", [&](const mrover::LanderAlignActionGoalConstPtr& goal){ActionServerCallBack(goal);}, false); + Server mActionServer = Server(mNh, "LanderAlignAction", [&](mrover::LanderAlignActionGoalConstPtr const& goal){ActionServerCallBack(goal);}, false); ros::Publisher mDebugVectorPub; diff --git a/src/teleoperation/lander_align/pch.hpp b/src/teleoperation/lander_align/pch.hpp index b9a684221..39af1c95b 100644 --- a/src/teleoperation/lander_align/pch.hpp +++ b/src/teleoperation/lander_align/pch.hpp @@ -9,7 +9,7 @@ #include #include #include -#include "mrover/LanderAlignAction.h" +#include //TF #include From 64e5cf3e00a3e89f42e7d26f898812b4920e08df Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 17 Mar 2024 13:37:58 -0400 Subject: [PATCH 64/89] Building --- .../lander_align/lander_align.cpp | 147 +++++++----------- .../lander_align/lander_align.hpp | 47 +++--- src/teleoperation/lander_align/pch.hpp | 10 +- 3 files changed, 81 insertions(+), 123 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 2e9070c45..8dd65355c 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,47 +1,8 @@ #include "lander_align.hpp" -#include "lie.hpp" -#include "mrover/LanderAlignActionFeedback.h" -#include "mrover/LanderAlignActionGoal.h" -#include "mrover/LanderAlignActionResult.h" -#include "pch.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include namespace mrover { - auto operator<<(std::ostream &ostream, RTRSTATE state) -> std::ostream& { - ostream << RTRSTRINGS[state]; - return ostream; + auto operator<<(std::ostream& ostream, RTRSTATE state) -> std::ostream& { + return ostream << RTRSTRINGS[static_cast(state)]; } auto LanderAlignNodelet::onInit() -> void { @@ -55,6 +16,8 @@ namespace mrover { mTwistPub = mNh.advertise("/cmd_vel", 1); mDebugPCPub = mNh.advertise("/lander_align/debugPC", 1); + mActionServer.emplace(mNh, "LanderAlignAction", [&](LanderAlignGoalConstPtr const& goal) { ActionServerCallBack(goal); }, false); + //TF Create the Reference Frames mNh.param("camera_frame", mCameraFrameId, "zed_left_camera_frame"); mNh.param("world_frame", mMapFrameId, "map"); @@ -64,22 +27,22 @@ namespace mrover { mLoopState = RTRSTATE::turn1; } - void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { - + auto LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) -> void { + filterNormals(cloud); ransac(0.1, 10, 100, mPlaneOffsetScalar); if (mNormalInZEDVector.has_value()) { sendTwist(); } - if(mPlaneOffsetScalar == 2.5){ + if (mPlaneOffsetScalar == 2.5) { mPlaneOffsetScalar = 0.4; mLoopState = RTRSTATE::turn1; - }else{ + } else { mLoopState = RTRSTATE::done; } } - void LanderAlignNodelet::ActionServerCallBack(LanderAlignActionGoalConstPtr const& actionRequest){ + auto LanderAlignNodelet::ActionServerCallBack(LanderAlignGoalConstPtr const& actionRequest) -> void { LanderAlignActionFeedback feedback; LanderAlignActionResult result; // while(actionRequest.action_goal.goal.num); @@ -90,20 +53,20 @@ namespace mrover { //Returns angle (yaw) around the z axis - auto LanderAlignNodelet::calcAngleWithWorldX(Eigen::Vector3d xHeading) -> double{ //I want to be editing this variable so it should not be const or & + auto LanderAlignNodelet::calcAngleWithWorldX(Eigen::Vector3d xHeading) -> double { //I want to be editing this variable so it should not be const or & xHeading.z() = 0; xHeading.normalize(); Eigen::Vector3d xAxisWorld{1, 0, 0}; double angle = std::acos(xHeading.dot(xAxisWorld)); - if(xHeading.y() >= 0){ + if (xHeading.y() >= 0) { return angle; - }else{ + } else { return angle + std::numbers::pi; } } - void LanderAlignNodelet::calcMotion(double desiredVelocity, double desiredOmega){ - manif::SE3d roverInWorld = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); + void LanderAlignNodelet::calcMotion(double desiredVelocity, double desiredOmega) { + SE3d roverInWorld = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); Eigen::Vector3d xAxisRotation = roverInWorld.rotation().col(0); double roverHeading = calcAngleWithWorldX(xAxisRotation); double targetHeading = calcAngleWithWorldX(-mNormalInWorldVector.value()); @@ -118,16 +81,16 @@ namespace mrover { double Kx = 1; double Ky = 1; double Ktheta = 1; - + Eigen::Matrix3d rotation; rotation << std::cos(roverHeading), std::sin(roverHeading), 0, - -std::sin(roverHeading), std::cos(roverHeading), 0, - 0, 0, 1; + -std::sin(roverHeading), std::cos(roverHeading), 0, + 0, 0, 1; Eigen::Vector3d errState = rotation * (tarState - currState); //I think this is rad/s - double zRotation = desiredOmega + (desiredVelocity / 2) * (Ky * (errState.y() + Ktheta*errState.z()) + (1/Ky) * std::sin(errState.z())); + double zRotation = desiredOmega + (desiredVelocity / 2) * (Ky * (errState.y() + Ktheta * errState.z()) + (1 / Ky) * std::sin(errState.z())); //I think this is unit/s double xSpeed = Kx * errState.x() + desiredVelocity * std::cos(errState.z()) - Ktheta * errState.z() * zRotation; @@ -137,7 +100,6 @@ namespace mrover { twist.linear.x = xSpeed; mTwistPub.publish(twist); - } void LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud) { @@ -148,7 +110,7 @@ namespace mrover { // define randomizer std::default_random_engine generator; - std::uniform_int_distribution pointDistribution(0, (int) mLeastSamplingDistribution); + std::uniform_int_distribution pointDistribution(0, mLeastSamplingDistribution); for (auto point = cloudData; point < cloudData + (cloud->height * cloud->width); point += pointDistribution(generator)) { bool isPointInvalid = (!std::isfinite(point->x) || !std::isfinite(point->y) || !std::isfinite(point->z)); @@ -159,7 +121,7 @@ namespace mrover { } } - void LanderAlignNodelet::uploadPC(int numInliers, double distanceThreshold){ + void LanderAlignNodelet::uploadPC(int numInliers, double distanceThreshold) { auto debugPointCloudPtr = boost::make_shared(); fillPointCloudMessageHeader(debugPointCloudPtr); debugPointCloudPtr->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; @@ -169,13 +131,13 @@ namespace mrover { debugPointCloudPtr->header.seq = 0; debugPointCloudPtr->header.stamp = ros::Time(); debugPointCloudPtr->header.frame_id = "zed_left_camera_frame"; - debugPointCloudPtr->data.resize((numInliers * sizeof(Point))/sizeof(uchar)); + debugPointCloudPtr->data.resize(numInliers * sizeof(Point)); auto pcPtr = reinterpret_cast(debugPointCloudPtr->data.data()); size_t i = 0; for (auto p: mFilteredPoints) { // calculate distance of each point from potential plane double distance = std::abs(mNormalInZEDVector.value().x() * p->x + mNormalInZEDVector.value().y() * p->y + mNormalInZEDVector.value().z() * p->z + mBestOffset); - // double distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); // + // double distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); // if (distance < distanceThreshold) { pcPtr[i].x = p->x; @@ -189,7 +151,7 @@ namespace mrover { } } mDebugPCPub.publish(debugPointCloudPtr); - } + } void LanderAlignNodelet::ransac(double const distanceThreshold, int minInliers, int const epochs, double offsetFactor) { // TODO: use RANSAC to find the lander face, should be the closest, we may need to modify this to output more information, currently the output is the normal @@ -236,7 +198,7 @@ namespace mrover { for (auto p: mFilteredPoints) { // calculate distance of each point from potential plane - double distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); // + double distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); // if (distance < distanceThreshold) { // Add points to current planes center @@ -282,16 +244,16 @@ namespace mrover { //Average pnts mPlaneLocationInZEDVector.value() /= static_cast(numInliers); - + uploadPC(numInliers, distanceThreshold); - if(mNormalInZEDVector.value().x() > 0) mNormalInZEDVector.value() *=-1; + if (mNormalInZEDVector.value().x() > 0) mNormalInZEDVector.value() *= -1; mOffsetLocationInZEDVector = std::make_optional(mPlaneLocationInZEDVector.value() + offsetFactor * mNormalInZEDVector.value()); - manif::SE3d zedToMap = SE3Conversions::fromTfTree(mTfBuffer, mCameraFrameId, mMapFrameId); - + SE3d zedToMap = SE3Conversions::fromTfTree(mTfBuffer, mCameraFrameId, mMapFrameId); + //Calculate the SO3 in the world frame Eigen::Matrix3d rot; Eigen::Vector3d forward = mNormalInZEDVector.value().normalized(); @@ -304,17 +266,17 @@ namespace mrover { rot.col(1) = left; rot.col(2) = up; - //Calculate the plane location in the world frame - manif::SE3d mPlaneLocationInZEDSE3d = {{mPlaneLocationInZEDVector.value().x(), mPlaneLocationInZEDVector.value().y(), mPlaneLocationInZEDVector.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; + //Calculate the plane location in the world frame + SE3d mPlaneLocationInZEDSE3d = {{mPlaneLocationInZEDVector.value().x(), mPlaneLocationInZEDVector.value().y(), mPlaneLocationInZEDVector.value().z()}, SO3d{Eigen::Quaterniond{rot}.normalized()}}; mPlaneLocationInWorldSE3d = zedToMap * mPlaneLocationInZEDSE3d; mPlaneLocationInWorldVector = std::make_optional(mPlaneLocationInWorldSE3d.translation()); //Calculate the offset location in the world frame - manif::SE3d mOffsetLocationInZEDSE3d = {{mOffsetLocationInZEDVector.value().x(), mOffsetLocationInZEDVector.value().y(), mOffsetLocationInZEDVector.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; - mOffsetLocationInWorldSE3d = zedToMap * mOffsetLocationInZEDSE3d; + SE3d mOffsetLocationInZEDSE3d = {{mOffsetLocationInZEDVector.value().x(), mOffsetLocationInZEDVector.value().y(), mOffsetLocationInZEDVector.value().z()}, SO3d{Eigen::Quaterniond{rot}.normalized()}}; + mOffsetLocationInWorldSE3d = zedToMap * mOffsetLocationInZEDSE3d; mOffsetLocationInWorldVector = std::make_optional(mOffsetLocationInWorldSE3d.translation()); - //Push to the tf tree + //Push to the tf tree SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, mPlaneLocationInWorldSE3d); SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, mOffsetLocationInWorldSE3d); } @@ -370,25 +332,25 @@ namespace mrover { float const angular_thresh = 0.001; - PID pid(0.1, 0.1); // literally just P -- ugly class and probably needs restructuring in the future + PID pid(0.1, 0.1); // literally just P -- ugly class and probably needs restructuring in the future ros::Rate rate(20); // ROS Rate at 20Hzn::Matrix3d roverToPlaneNorm; - // Eigen::Vector3d Nup = Eigen::Vector3d::UnitZ(); - // Eigen::Vector3d Nleft = Nup.cross(mBestNormalInWorld.value()); + // Eigen::Vector3d Nup = Eigen::Vector3d::UnitZ(); + // Eigen::Vector3d Nleft = Nup.cross(mBestNormalInWorld.value()); - // roverToPlaneNorm.col(0) = mBestNormalInWorld.value(); - // roverToPlaneNorm.col(1) = Nup; - // roverToPlaneNorm.col(2) = Nleft; - // manif::SO3d roverToP + // roverToPlaneNorm.col(0) = mBestNormalInWorld.value(); + // roverToPlaneNorm.col(1) = Nup; + // roverToPlaneNorm.col(2) = Nleft; + // manif::SO3d roverToP while (ros::ok()) { SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, mPlaneLocationInWorldSE3d); SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, mOffsetLocationInWorldSE3d); - manif::SE3d rover = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); - Eigen::Vector3d roverPosInWorld{static_cast(rover.translation().x()), static_cast(rover.translation().y()), 0.0}; + SE3d rover = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); + Eigen::Vector3d roverPosInWorld{(rover.translation().x()), (rover.translation().y()), 0.0}; Eigen::Vector3d targetPosInWorld; - if(mLoopState == RTRSTATE::turn1){ + if (mLoopState == RTRSTATE::turn1) { targetPosInWorld = mOffsetLocationInWorldVector.value(); - }else if(mLoopState == RTRSTATE::turn2){ + } else if (mLoopState == RTRSTATE::turn2) { targetPosInWorld = mPlaneLocationInWorldVector.value(); } targetPosInWorld.z() = 0; @@ -403,21 +365,21 @@ namespace mrover { roverToTargetMat.col(0) = roverToTargetForward; roverToTargetMat.col(1) = up; roverToTargetMat.col(2) = left; - + //SO3 Matrices for lie algebra - manif::SO3d roverToTargetSO3 = SE3Conversions::fromColumns(roverToTargetForward, left, up); - manif::SO3d roverSO3 = rover.asSO3(); + SO3d roverToTargetSO3 = SE3Conversions::fromColumns(roverToTargetForward, left, up); + SO3d roverSO3 = rover.asSO3(); Eigen::Vector3d distanceToTargetVector = targetPosInWorld - Eigen::Vector3d{rover.translation().x(), rover.translation().y(), rover.translation().z()}; double distanceToTarget = std::abs(distanceToTargetVector.norm()); - + manif::SO3Tangentd SO3tan = roverToTargetSO3 - roverSO3; // 3 x 1 matrix of angular velocities (x,y,z) switch (mLoopState) { case RTRSTATE::turn1: { if (distanceToTarget < 0.5) mLoopState = RTRSTATE::turn2; - + double angle_rate = mAngleP * SO3tan.z(); ROS_INFO("w_z velocity %f", SO3tan.z()); @@ -429,7 +391,6 @@ namespace mrover { twist.angular.z = 0; twist.linear.x = 0; ROS_INFO("Done spinning"); - } // ROS_INFO("In state: turning to point..."); break; @@ -440,7 +401,6 @@ namespace mrover { mLoopState = RTRSTATE::turn1; ROS_INFO("Rotation got off"); twist.linear.x = 0; - } double driveRate = std::min(mLinearP * distanceToTarget, 1.0); @@ -458,19 +418,18 @@ namespace mrover { // ROS_INFO("In state: driving to point..."); break; } - + case RTRSTATE::turn2: { double angle_rate = mAngleP * SO3tan.z(); twist.angular.z = angle_rate; twist.linear.x = 0; - + if (std::abs(SO3tan.z()) < angular_thresh) { mLoopState = RTRSTATE::done; twist.angular.z = 0; twist.linear.x = 0; - // ROS_INFO("Done turning to lander"); - + // ROS_INFO("Done turning to lander"); } // ROS_INFO("In state: turning to lander..."); break; @@ -485,7 +444,7 @@ namespace mrover { // ROS_INFO("THE TWIST IS: Angular: %f, with linear %f,", twist.angular.z, twist.linear.x); ROS_INFO_STREAM(mLoopState); mTwistPub.publish(twist); - if(mLoopState == RTRSTATE::done){ + if (mLoopState == RTRSTATE::done) { break; } } diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 91eb59aba..b8c3976f4 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -4,33 +4,30 @@ namespace mrover { - using Server = actionlib::SimpleActionServer; + using Server = actionlib::SimpleActionServer; - enum RTRSTATE { + enum struct RTRSTATE { turn1 = 0, drive = 1, turn2 = 2, - done = 3 + done = 3, }; constexpr char RTRSTRINGS[4][6] = { - "turn1", - "drive", - "turn2", - "done " + "turn1", + "drive", + "turn2", + "done ", }; - - - class LanderAlignNodelet : public nodelet::Nodelet { + class LanderAlignNodelet final : public nodelet::Nodelet { //PID CONSTANTS double const mAngleP = 1; double const mLinearP = 0.3; ros::NodeHandle mNh, mPnh; - Server mActionServer = Server(mNh, "LanderAlignAction", [&](mrover::LanderAlignActionGoalConstPtr const& goal){ActionServerCallBack(goal);}, false); - + std::optional mActionServer; ros::Publisher mDebugVectorPub; @@ -40,12 +37,12 @@ namespace mrover { ros::Subscriber mVectorSub; - RTRSTATE mLoopState; + RTRSTATE mLoopState = RTRSTATE::done; //** - double mBestOffset; + double mBestOffset{}; - double mPlaneOffsetScalar; + double mPlaneOffsetScalar{}; std::optional mPlaneLocationInZEDVector; std::optional mPlaneLocationInWorldVector; @@ -56,8 +53,8 @@ namespace mrover { std::optional mNormalInZEDVector; std::optional mNormalInWorldVector; - manif::SE3d mOffsetLocationInWorldSE3d; - manif::SE3d mPlaneLocationInWorldSE3d; + SE3d mOffsetLocationInWorldSE3d; + SE3d mPlaneLocationInWorldSE3d; //** //TF Member Variables @@ -67,17 +64,17 @@ namespace mrover { std::string mCameraFrameId; std::string mMapFrameId; - //Ransac Params - double mZThreshold; - double mXThreshold; + //Ransac Params + double mZThreshold{}; + double mXThreshold{}; int mLeastSamplingDistribution = 10; std::vector mFilteredPoints; auto onInit() -> void override; void LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud); - - void ActionServerCallBack(LanderAlignActionGoalConstPtr const& actionRequest); + + void ActionServerCallBack(LanderAlignGoalConstPtr const& actionRequest); void filterNormals(sensor_msgs::PointCloud2Ptr const& cloud); @@ -85,18 +82,16 @@ namespace mrover { void sendTwist(); - void uploadPC(int numInliers, double distanceThreshold); + void uploadPC(int numInliers, double distanceThreshold); void calcMotion(double desiredVelocity, double desiredOmega); static auto calcAngleWithWorldX(Eigen::Vector3d xHeading) -> double; class PID { - private: float const Angle_P; float const Linear_P; - public: PID(float angle_P, float linear_P); @@ -104,10 +99,8 @@ namespace mrover { [[nodiscard]] auto rotate_speed(double theta) const -> double; - auto find_angle(Eigen::Vector3d const& current, Eigen::Vector3d const& target) -> float; - auto find_distance(Eigen::Vector3d const& current, Eigen::Vector3d const& target) -> double; auto drive_speed(float) -> float; diff --git a/src/teleoperation/lander_align/pch.hpp b/src/teleoperation/lander_align/pch.hpp index 39af1c95b..3b03f293e 100644 --- a/src/teleoperation/lander_align/pch.hpp +++ b/src/teleoperation/lander_align/pch.hpp @@ -1,15 +1,21 @@ #pragma once +#include +#include +#include +#include + +#include +#include +#include #include #include #include #include #include #include -#include #include #include -#include //TF #include From fe7491578c533081412b8de10175b133302eb5a0 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 17 Mar 2024 14:16:51 -0400 Subject: [PATCH 65/89] No topics --- .../lander_align/lander_align.cpp | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 8dd65355c..f86b65781 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,4 +1,7 @@ #include "lander_align.hpp" +#include "mrover/LanderAlignActionFeedback.h" +#include "mrover/LanderAlignActionResult.h" +#include namespace mrover { auto operator<<(std::ostream& ostream, RTRSTATE state) -> std::ostream& { @@ -16,7 +19,7 @@ namespace mrover { mTwistPub = mNh.advertise("/cmd_vel", 1); mDebugPCPub = mNh.advertise("/lander_align/debugPC", 1); - mActionServer.emplace(mNh, "LanderAlignAction", [&](LanderAlignGoalConstPtr const& goal) { ActionServerCallBack(goal); }, false); + mActionServer.emplace(mNh, "/LanderAlignAction", [&](LanderAlignGoalConstPtr const& goal) { ActionServerCallBack(goal); }, false); //TF Create the Reference Frames mNh.param("camera_frame", mCameraFrameId, "zed_left_camera_frame"); @@ -43,12 +46,16 @@ namespace mrover { } auto LanderAlignNodelet::ActionServerCallBack(LanderAlignGoalConstPtr const& actionRequest) -> void { - LanderAlignActionFeedback feedback; - LanderAlignActionResult result; - // while(actionRequest.action_goal.goal.num); - // for(int i = 0; i < static_cast(actionRequest.goal.num); i++){ - // //feedback.feedback.percent_complete=i; - // } + LanderAlignFeedbackPtr feedback; + LanderAlignResult result; + result.num=10; + ros::Rate r(1); + for(int i = 0; i < static_cast(actionRequest->num); i++){ + feedback->count_ten = i; + mActionServer->publishFeedback(feedback); + r.sleep(); + } + mActionServer->setSucceeded(result); } From a754273cd6b3fe4ae79c2179fde01ba69fea9401 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 17 Mar 2024 14:36:40 -0400 Subject: [PATCH 66/89] Working coutn to 10 --- src/teleoperation/lander_align/lander_align.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index f86b65781..cca86842d 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -19,8 +19,8 @@ namespace mrover { mTwistPub = mNh.advertise("/cmd_vel", 1); mDebugPCPub = mNh.advertise("/lander_align/debugPC", 1); - mActionServer.emplace(mNh, "/LanderAlignAction", [&](LanderAlignGoalConstPtr const& goal) { ActionServerCallBack(goal); }, false); - + mActionServer.emplace(mNh, "LanderAlignAction", [&](LanderAlignGoalConstPtr const& goal) { ActionServerCallBack(goal); }, true); + //TF Create the Reference Frames mNh.param("camera_frame", mCameraFrameId, "zed_left_camera_frame"); mNh.param("world_frame", mMapFrameId, "map"); @@ -46,12 +46,12 @@ namespace mrover { } auto LanderAlignNodelet::ActionServerCallBack(LanderAlignGoalConstPtr const& actionRequest) -> void { - LanderAlignFeedbackPtr feedback; + LanderAlignFeedback feedback; LanderAlignResult result; result.num=10; ros::Rate r(1); for(int i = 0; i < static_cast(actionRequest->num); i++){ - feedback->count_ten = i; + feedback.count_ten = i; mActionServer->publishFeedback(feedback); r.sleep(); } From 062a6fa76370b97b98ef88d18e44a6a40e676c33 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 21 Mar 2024 18:15:20 -0400 Subject: [PATCH 67/89] Working in teleop backend --- action/LanderAlign.action | 2 +- config/simulator/simulator.yaml | 2 +- src/teleoperation/backend/consumers.py | 16 ++++- .../lander_align/lander_align.cpp | 66 +++++++++++++------ .../lander_align/lander_align.hpp | 9 ++- src/teleoperation/lander_align/pch.hpp | 1 + 6 files changed, 70 insertions(+), 26 deletions(-) diff --git a/action/LanderAlign.action b/action/LanderAlign.action index f70588fba..ff6bc6d0c 100644 --- a/action/LanderAlign.action +++ b/action/LanderAlign.action @@ -5,4 +5,4 @@ uint32 num # Specify which dishwasher we want to use uint32 num --- # Define a feedback message -uint32 count_ten \ No newline at end of file +string curr_state diff --git a/config/simulator/simulator.yaml b/config/simulator/simulator.yaml index b13c33f14..95b0d1a10 100644 --- a/config/simulator/simulator.yaml +++ b/config/simulator/simulator.yaml @@ -41,4 +41,4 @@ objects: - type: urdf name: lander uri: package://mrover/urdf/world/lander.urdf.xacro - translation: [9.81, -4.73, 2] + translation: [8, 6, 2] diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 59e7d894c..f2c2114c8 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -8,6 +8,7 @@ import rospy import tf2_ros import cv2 +import actionlib from cv_bridge import CvBridge from geometry_msgs.msg import Twist from mrover.msg import ( @@ -35,6 +36,7 @@ from mrover.srv import EnableDevice, AdjustMotor from sensor_msgs.msg import JointState, Joy, NavSatFix from geometry_msgs.msg import Twist, Pose, Point, Quaternion +from mrover.msg import LanderAlignGoal from util.SE3 import SE3 @@ -121,6 +123,9 @@ def connect(self): self.flight_thread = threading.Thread(target=self.flight_attitude_listener) self.flight_thread.start() + #Actions + self.landerClient = actionlib.SimpleActionClient('LanderAlignAction', LanderAlignGoal) + except rospy.ROSException as e: rospy.logerr(e) @@ -141,7 +146,6 @@ def receive(self, text_data): message = json.loads(text_data) try: if message["type"] == "joystick_values": - if not lander_align self.handle_joystick_message(message) elif message["type"] == "disable_auton_led": self.disable_auton_led() @@ -181,6 +185,10 @@ def receive(self, text_data): self.save_basic_waypoint_list(message) elif message["type"] == "get_basic_waypoint_list": self.get_basic_waypoint_list(message) + elif message["type"] == "start_lander_align": + self.start_lander_align(message) + elif message["type"] == "cancel_lander_align": + self.cancel_lander_align(message) except Exception as e: rospy.logerr(e) @@ -714,4 +722,10 @@ def flight_attitude_listener(self): rate.sleep() def start_lander_align(self) -> None: + goal = LanderAlignGoal() + self.landerClient.send_goal(goal) + pass + + def stop_lander_align(self) -> None: + self.landerClient.cancel_goal() pass diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index cca86842d..79a847f8a 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,7 +1,9 @@ #include "lander_align.hpp" #include "mrover/LanderAlignActionFeedback.h" #include "mrover/LanderAlignActionResult.h" +#include #include +#include namespace mrover { auto operator<<(std::ostream& ostream, RTRSTATE state) -> std::ostream& { @@ -32,29 +34,45 @@ namespace mrover { auto LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) -> void { - filterNormals(cloud); - ransac(0.1, 10, 100, mPlaneOffsetScalar); - if (mNormalInZEDVector.has_value()) { - sendTwist(); - } - if (mPlaneOffsetScalar == 2.5) { - mPlaneOffsetScalar = 0.4; - mLoopState = RTRSTATE::turn1; - } else { - mLoopState = RTRSTATE::done; - } + // filterNormals(cloud); + // ransac(0.1, 10, 100, mPlaneOffsetScalar); + // if (mNormalInZEDVector.has_value()) { + // sendTwist(); + // } + // if (mPlaneOffsetScalar == 2.5) { + // mPlaneOffsetScalar = 0.4; + // mLoopState = RTRSTATE::turn1; + // } else { + // mLoopState = RTRSTATE::done; + // } } auto LanderAlignNodelet::ActionServerCallBack(LanderAlignGoalConstPtr const& actionRequest) -> void { - LanderAlignFeedback feedback; LanderAlignResult result; - result.num=10; - ros::Rate r(1); - for(int i = 0; i < static_cast(actionRequest->num); i++){ - feedback.count_ten = i; - mActionServer->publishFeedback(feedback); - r.sleep(); + + //If we haven't yet defined the point cloud we are working with + mCloud = ros::topic::waitForMessage("/camera/left/points", mNh); + filterNormals(mCloud); + ransac(0.1, 10, 100, mPlaneOffsetScalar); + + //If there is a proper normal to drive to + if (mNormalInZEDVector.has_value()) { + sendTwist(); + } + + mPlaneOffsetScalar = 1; + mLoopState = RTRSTATE::turn1; + + //If we haven't yet defined the point cloud we are working with + mCloud = ros::topic::waitForMessage("/camera/left/points", mNh); + filterNormals(mCloud); + ransac(0.1, 10, 100, mPlaneOffsetScalar); + + //If there is a proper normal to drive to + if (mNormalInZEDVector.has_value()) { + sendTwist(); } + mActionServer->setSucceeded(result); } @@ -109,7 +127,7 @@ namespace mrover { mTwistPub.publish(twist); } - void LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2Ptr const& cloud) { + void LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2ConstPtr const& cloud) { // TODO: OPTIMIZE; doing this maobject_detector/debug_imgny push_back calls could slow things down mFilteredPoints.clear(); auto* cloudData = reinterpret_cast(cloud->data.data()); @@ -349,7 +367,14 @@ namespace mrover { // roverToPlaneNorm.col(2) = Nleft; // manif::SO3d roverToP while (ros::ok()) { - + if(mActionServer->isPreemptRequested()){ + mActionServer->setPreempted(); + break; + } + + LanderAlignFeedback feedback; + feedback.curr_state = RTRSTRINGS[static_cast(mLoopState)]; + mActionServer->publishFeedback(feedback); SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, mPlaneLocationInWorldSE3d); SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, mOffsetLocationInWorldSE3d); SE3d rover = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); @@ -446,6 +471,7 @@ namespace mrover { twist.linear.x = 0; twist.angular.z = 0; break; + mCloud = nullptr; } } // ROS_INFO("THE TWIST IS: Angular: %f, with linear %f,", twist.angular.z, twist.linear.x); diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index b8c3976f4..c00d26286 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -1,6 +1,8 @@ #pragma once #include "pch.hpp" +#include +#include namespace mrover { @@ -39,7 +41,9 @@ namespace mrover { RTRSTATE mLoopState = RTRSTATE::done; - //** + //Action Server Variables + sensor_msgs::PointCloud2ConstPtr mCloud; + double mBestOffset{}; double mPlaneOffsetScalar{}; @@ -55,7 +59,6 @@ namespace mrover { SE3d mOffsetLocationInWorldSE3d; SE3d mPlaneLocationInWorldSE3d; - //** //TF Member Variables tf2_ros::Buffer mTfBuffer; @@ -76,7 +79,7 @@ namespace mrover { void ActionServerCallBack(LanderAlignGoalConstPtr const& actionRequest); - void filterNormals(sensor_msgs::PointCloud2Ptr const& cloud); + void filterNormals(sensor_msgs::PointCloud2ConstPtr const& cloud); void ransac(double distanceThreshold, int minInliers, int epochs, double offsetFactor); diff --git a/src/teleoperation/lander_align/pch.hpp b/src/teleoperation/lander_align/pch.hpp index 3b03f293e..c0a6d3a2f 100644 --- a/src/teleoperation/lander_align/pch.hpp +++ b/src/teleoperation/lander_align/pch.hpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include From 39d9cb05ddb8e1cc233a17fa0f7891c509a91d68 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 21 Mar 2024 19:15:10 -0400 Subject: [PATCH 68/89] working action server --- src/teleoperation/lander_align/lander_align.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 79a847f8a..1eecccfce 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -49,7 +49,8 @@ namespace mrover { auto LanderAlignNodelet::ActionServerCallBack(LanderAlignGoalConstPtr const& actionRequest) -> void { LanderAlignResult result; - + mPlaneOffsetScalar = 2.5; + //If we haven't yet defined the point cloud we are working with mCloud = ros::topic::waitForMessage("/camera/left/points", mNh); filterNormals(mCloud); @@ -369,6 +370,9 @@ namespace mrover { while (ros::ok()) { if(mActionServer->isPreemptRequested()){ mActionServer->setPreempted(); + twist.angular.z = 0; + twist.linear.x = 0; + mTwistPub.publish(twist); break; } From 95735a71024813f45337b9a021f3e8ad07be2670 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 21 Mar 2024 19:37:43 -0400 Subject: [PATCH 69/89] flooring + tuning --- src/teleoperation/lander_align/lander_align.cpp | 7 ++++++- src/teleoperation/lander_align/lander_align.hpp | 2 ++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 1eecccfce..c5439641a 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,7 +1,10 @@ #include "lander_align.hpp" #include "mrover/LanderAlignActionFeedback.h" #include "mrover/LanderAlignActionResult.h" +#include #include +#include +#include #include #include @@ -354,7 +357,7 @@ namespace mrover { geometry_msgs::Twist twist; //Threhsolds - float const linear_thresh = 0.1; // could be member variables + float const linear_thresh = 0.01; // could be member variables float const angular_thresh = 0.001; @@ -417,6 +420,7 @@ namespace mrover { if (distanceToTarget < 0.5) mLoopState = RTRSTATE::turn2; double angle_rate = mAngleP * SO3tan.z(); + angle_rate = (std::abs(angle_rate) > mAngleFloor) ? angle_rate : copysign(mAngleFloor, angle_rate); ROS_INFO("w_z velocity %f", SO3tan.z()); @@ -457,6 +461,7 @@ namespace mrover { case RTRSTATE::turn2: { double angle_rate = mAngleP * SO3tan.z(); + angle_rate = (std::abs(angle_rate) > mAngleFloor) ? angle_rate : copysign(mAngleFloor, angle_rate); twist.angular.z = angle_rate; twist.linear.x = 0; diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index c00d26286..96d55541d 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -26,7 +26,9 @@ namespace mrover { //PID CONSTANTS double const mAngleP = 1; + double const mAngleFloor = 0.05; double const mLinearP = 0.3; + ros::NodeHandle mNh, mPnh; std::optional mActionServer; From 17353f13188f89926bcaf55a7c00d00e4a93b632 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 21 Mar 2024 19:51:59 -0400 Subject: [PATCH 70/89] Added Smarter Movement --- src/teleoperation/lander_align/lander_align.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index c5439641a..87c696ea0 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -308,6 +309,9 @@ namespace mrover { //Push to the tf tree SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, mPlaneLocationInWorldSE3d); SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, mOffsetLocationInWorldSE3d); + + //Compare Rover Location to Target Location + if(mOffsetLocationInZEDSE3d.translation().x() < 0) mNormalInZEDVector = std::nullopt; } auto LanderAlignNodelet::PID::rotate_speed(double theta) const -> double { @@ -418,6 +422,7 @@ namespace mrover { switch (mLoopState) { case RTRSTATE::turn1: { if (distanceToTarget < 0.5) mLoopState = RTRSTATE::turn2; + double angle_rate = mAngleP * SO3tan.z(); angle_rate = (std::abs(angle_rate) > mAngleFloor) ? angle_rate : copysign(mAngleFloor, angle_rate); From c679411f89a04eea75c1af51e7ff35607133ccc2 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 21 Mar 2024 20:04:13 -0400 Subject: [PATCH 71/89] Dummy Server Script --- src/teleoperation/lander_align/ClientDummy.py | 53 +++++++++++++++++++ 1 file changed, 53 insertions(+) create mode 100755 src/teleoperation/lander_align/ClientDummy.py diff --git a/src/teleoperation/lander_align/ClientDummy.py b/src/teleoperation/lander_align/ClientDummy.py new file mode 100755 index 000000000..c0388638d --- /dev/null +++ b/src/teleoperation/lander_align/ClientDummy.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python3 +import rospkg +import actionlib + +import os + +# python linear algebra library +import numpy as np + +# library for interacting with ROS and TF tree +import rospy +import mrover + +from mrover.msg import LanderAlignAction +from mrover.msg import LanderAlignGoal + + +class ClientDummy: + def __init__(self): + self.landerClient = actionlib.SimpleActionClient('LanderAlignAction', LanderAlignAction) + self.landerClient.wait_for_server() + print("finished init") + + + def start_lander_align(self) -> None: + print("start") + goal = LanderAlignGoal() + self.landerClient.send_goal(goal) + print("finished start") + pass + + def stop_lander_align(self) -> None: + print("stop") + self.landerClient.cancel_goal() + pass + + + +def main(): + # initialize the node + rospy.init_node("ClientDummy") + dummy = ClientDummy() + + dummy.start_lander_align() + rospy.sleep(4) + dummy.stop_lander_align() + + # let the callback functions run asynchronously in the background + rospy.spin() + + +if __name__ == "__main__": + main() \ No newline at end of file From 1e1d80fd18cd6b8a519f82de380e57a9d405f821 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 24 Mar 2024 12:03:47 -0400 Subject: [PATCH 72/89] Updated Consumers.py --- src/teleoperation/backend/consumers.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index f2c2114c8..aa7f8dddc 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -123,9 +123,9 @@ def connect(self): self.flight_thread = threading.Thread(target=self.flight_attitude_listener) self.flight_thread.start() - #Actions - self.landerClient = actionlib.SimpleActionClient('LanderAlignAction', LanderAlignGoal) - + #Actions Servers + self.landerClient = actionlib.SimpleActionClient('LanderAlignAction', LanderAlignAction) + self.landerClient.wait_for_server() except rospy.ROSException as e: rospy.logerr(e) From eead32a144bfc4d43b5f7716e432bfe2528472b7 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 24 Mar 2024 12:41:52 -0400 Subject: [PATCH 73/89] removed callback --- .../lander_align/lander_align.cpp | 21 +++---------------- .../lander_align/lander_align.hpp | 6 +----- 2 files changed, 4 insertions(+), 23 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 87c696ea0..a5e3f548e 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -20,12 +20,12 @@ namespace mrover { mZThreshold = .5; mXThreshold = .1; mPlaneOffsetScalar = 2.5; - mVectorSub = mNh.subscribe("/camera/left/points", 1, &LanderAlignNodelet::LanderCallback, this); mDebugVectorPub = mNh.advertise("/lander_align/Pose", 1); mTwistPub = mNh.advertise("/cmd_vel", 1); mDebugPCPub = mNh.advertise("/lander_align/debugPC", 1); - mActionServer.emplace(mNh, "LanderAlignAction", [&](LanderAlignGoalConstPtr const& goal) { ActionServerCallBack(goal); }, true); + mActionServer.emplace(mNh, "LanderAlignAction", [&](LanderAlignGoalConstPtr const& goal) { ActionServerCallBack(); }, false); + mActionServer.value().start(); //TF Create the Reference Frames mNh.param("camera_frame", mCameraFrameId, "zed_left_camera_frame"); @@ -36,22 +36,7 @@ namespace mrover { mLoopState = RTRSTATE::turn1; } - auto LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) -> void { - - // filterNormals(cloud); - // ransac(0.1, 10, 100, mPlaneOffsetScalar); - // if (mNormalInZEDVector.has_value()) { - // sendTwist(); - // } - // if (mPlaneOffsetScalar == 2.5) { - // mPlaneOffsetScalar = 0.4; - // mLoopState = RTRSTATE::turn1; - // } else { - // mLoopState = RTRSTATE::done; - // } - } - - auto LanderAlignNodelet::ActionServerCallBack(LanderAlignGoalConstPtr const& actionRequest) -> void { + auto LanderAlignNodelet::ActionServerCallBack() -> void { LanderAlignResult result; mPlaneOffsetScalar = 2.5; diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 96d55541d..faaf2408f 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -39,8 +39,6 @@ namespace mrover { ros::Publisher mTwistPub; - ros::Subscriber mVectorSub; - RTRSTATE mLoopState = RTRSTATE::done; //Action Server Variables @@ -77,9 +75,7 @@ namespace mrover { auto onInit() -> void override; - void LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud); - - void ActionServerCallBack(LanderAlignGoalConstPtr const& actionRequest); + void ActionServerCallBack(); void filterNormals(sensor_msgs::PointCloud2ConstPtr const& cloud); From b6fffc5729e3c3d4e85dbfaee0e29a7ae10f9c17 Mon Sep 17 00:00:00 2001 From: neuenfeldttj Date: Sun, 24 Mar 2024 12:51:30 -0400 Subject: [PATCH 74/89] added toggle --- CMakeLists.txt | 2 +- {action => msg}/LanderAlign.action | 0 src/teleoperation/backend/consumers.py | 11 +++++----- .../frontend/src/components/AutonTask.vue | 22 ++++++++++++++++--- 4 files changed, 26 insertions(+), 9 deletions(-) rename {action => msg}/LanderAlign.action (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 75770d6c2..d28810641 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -108,7 +108,7 @@ add_message_files(FILES ${MROVER_MESSAGE_FILES}) add_service_files(FILES ${MROVER_SERVICE_FILES}) -add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) +# add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) generate_messages(DEPENDENCIES ${MROVER_MESSAGE_DEPENDENCIES}) diff --git a/action/LanderAlign.action b/msg/LanderAlign.action similarity index 100% rename from action/LanderAlign.action rename to msg/LanderAlign.action diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index aa7f8dddc..dbe119dce 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -26,6 +26,8 @@ Velocity, Position, IK, + LanderAlignAction, + LanderAlignGoal ) from mrover.srv import EnableAuton, ChangeCameras, CapturePanorama from sensor_msgs.msg import NavSatFix, Temperature, RelativeHumidity, Image @@ -36,7 +38,6 @@ from mrover.srv import EnableDevice, AdjustMotor from sensor_msgs.msg import JointState, Joy, NavSatFix from geometry_msgs.msg import Twist, Pose, Point, Quaternion -from mrover.msg import LanderAlignGoal from util.SE3 import SE3 @@ -125,7 +126,7 @@ def connect(self): #Actions Servers self.landerClient = actionlib.SimpleActionClient('LanderAlignAction', LanderAlignAction) - self.landerClient.wait_for_server() + except rospy.ROSException as e: rospy.logerr(e) @@ -186,9 +187,9 @@ def receive(self, text_data): elif message["type"] == "get_basic_waypoint_list": self.get_basic_waypoint_list(message) elif message["type"] == "start_lander_align": - self.start_lander_align(message) - elif message["type"] == "cancel_lander_align": - self.cancel_lander_align(message) + self.start_lander_align() + elif message["type"] == "stop_lander_align": + self.stop_lander_align() except Exception as e: rospy.logerr(e) diff --git a/src/teleoperation/frontend/src/components/AutonTask.vue b/src/teleoperation/frontend/src/components/AutonTask.vue index f29cf555f..40176361c 100644 --- a/src/teleoperation/frontend/src/components/AutonTask.vue +++ b/src/teleoperation/frontend/src/components/AutonTask.vue @@ -23,6 +23,12 @@

Joystick Values

+
+ +
@@ -68,6 +74,7 @@ import OdometryReading from './OdometryReading.vue' import JoystickValues from './JoystickValues.vue' import DriveControls from './DriveControls.vue' import MastGimbalControls from './MastGimbalControls.vue' +import ToggleButton from './ToggleButton.vue' import { quaternionToMapAngle } from '../utils.js' import { defineComponent } from 'vue' @@ -84,7 +91,8 @@ export default defineComponent({ OdometryReading, JoystickValues, DriveControls, - MastGimbalControls + MastGimbalControls, + ToggleButton }, // add prop where map has the center property and autontask sends it once it gets it @@ -121,7 +129,9 @@ export default defineComponent({ effort: [] as number[], state: [] as string[], error: [] as string[] - } + }, + + landerAlignEnabled: false, } }, @@ -168,7 +178,13 @@ export default defineComponent({ }, methods: { - ...mapActions('websocket', ['sendMessage']) + ...mapActions('websocket', ['sendMessage']), + + toggleLander() { + this.landerAlignEnabled = !this.landerAlignEnabled; + if(this.landerAlignEnabled) this.sendMessage({"type": "start_lander_align"}); + else this.sendMessage({"type": "stop_lander_align"}); + } }, beforeUnmount: function () { From 915f2b073cd547f67771547bb371ecc48db054d1 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 24 Mar 2024 12:52:54 -0400 Subject: [PATCH 75/89] Some refactoring --- .../lander_align/lander_align.cpp | 29 +++++-------------- .../lander_align/lander_align.hpp | 2 +- 2 files changed, 8 insertions(+), 23 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index a5e3f548e..45e4a0f3a 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -118,20 +118,20 @@ namespace mrover { } void LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2ConstPtr const& cloud) { - // TODO: OPTIMIZE; doing this maobject_detector/debug_imgny push_back calls could slow things down mFilteredPoints.clear(); + + // Pointer to the underlying point cloud data auto* cloudData = reinterpret_cast(cloud->data.data()); - ROS_INFO("Point cloud size: %i", cloud->height * cloud->width); - // define randomizer std::default_random_engine generator; std::uniform_int_distribution pointDistribution(0, mLeastSamplingDistribution); + // Loop over the entire PC for (auto point = cloudData; point < cloudData + (cloud->height * cloud->width); point += pointDistribution(generator)) { + // Make sure all of the values are defined bool isPointInvalid = (!std::isfinite(point->x) || !std::isfinite(point->y) || !std::isfinite(point->z)); - if (abs(point->normal_z) < mZThreshold && !isPointInvalid && abs(point->normal_x) > mXThreshold) { + if (!isPointInvalid && abs(point->normal_z) < mZThreshold && abs(point->normal_x) > mXThreshold) { mFilteredPoints.push_back(point); - // ROS_INFO("Filtered point: %f, %f, %f", point->normal_x, point->normal_y, point->normal_z); } } } @@ -168,11 +168,7 @@ namespace mrover { mDebugPCPub.publish(debugPointCloudPtr); } - void LanderAlignNodelet::ransac(double const distanceThreshold, int minInliers, int const epochs, double offsetFactor) { - // TODO: use RANSAC to find the lander face, should be the closest, we may need to modify this to output more information, currently the output is the normal - // takes 3 samples for every epoch and terminates after specified number of epochs - // Eigen::Vector3d mBestNormal(0, 0, 0); // normal vector representing plane (initialize as zero vector?? default ctor??) - // Eigen::Vector3d currentCenter(0, 0, 0); // Keeps track of current center of plane in current epoch + void LanderAlignNodelet::ransac(double const distanceThreshold, int minInliers, int const epochs) { double offset; // define randomizer @@ -192,7 +188,6 @@ namespace mrover { int numInliers = 0; while (mNormalInZEDVector.value().isZero()) { // TODO add give up condition after X iter for (int i = 0; i < epochs; ++i) { - // currentCenter *= 0; // Set all vals in current center to zero at the start of each epoch // sample 3 random points (potential inliers) Point const* point1 = mFilteredPoints[distribution(generator)]; Point const* point2 = mFilteredPoints[distribution(generator)]; @@ -208,18 +203,11 @@ namespace mrover { numInliers = 0; - // assert(normal.x() != 0 && normal.y() != 0 && normal.z() != 0); - // In some situations we get the 0 vector with surprising frequency - for (auto p: mFilteredPoints) { // calculate distance of each point from potential plane double distance = std::abs(normal.x() * p->x + normal.y() * p->y + normal.z() * p->z + offset); // if (distance < distanceThreshold) { - // Add points to current planes center - // currentCenter(0) += p->x; - // currentCenter(1) += p->y; - // currentCenter(2) += p->z; ++numInliers; // count num of inliers that pass the "good enough fit" threshold } } @@ -229,9 +217,6 @@ namespace mrover { minInliers = numInliers; mNormalInZEDVector.value() = normal; mBestOffset = offset; - - // If this is the new best plane, set mBestCenterInZED - // mBestCenterInZED = currentCenter / static_cast(minInliers); } } } @@ -265,7 +250,7 @@ namespace mrover { if (mNormalInZEDVector.value().x() > 0) mNormalInZEDVector.value() *= -1; - mOffsetLocationInZEDVector = std::make_optional(mPlaneLocationInZEDVector.value() + offsetFactor * mNormalInZEDVector.value()); + mOffsetLocationInZEDVector = std::make_optional(mPlaneLocationInZEDVector.value() + mPlaneOffsetScalar * mNormalInZEDVector.value()); SE3d zedToMap = SE3Conversions::fromTfTree(mTfBuffer, mCameraFrameId, mMapFrameId); diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index faaf2408f..01398efb2 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -79,7 +79,7 @@ namespace mrover { void filterNormals(sensor_msgs::PointCloud2ConstPtr const& cloud); - void ransac(double distanceThreshold, int minInliers, int epochs, double offsetFactor); + void ransac(double distanceThreshold, int minInliers, int epochs); void sendTwist(); From 00b569f3f25c997ffb72b1af9bbaa9d91317c4c6 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 24 Mar 2024 13:44:19 -0400 Subject: [PATCH 76/89] Fixed Backend Error --- src/teleoperation/backend/consumers.py | 4 +++- src/teleoperation/lander_align/lander_align.cpp | 14 ++++++++++---- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index dbe119dce..4448c891d 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -728,5 +728,7 @@ def start_lander_align(self) -> None: pass def stop_lander_align(self) -> None: - self.landerClient.cancel_goal() + if(self.landerClient.get_state() != 3): + self.landerClient.cancel_goal() pass + diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 45e4a0f3a..c23ed8c41 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -37,13 +37,15 @@ namespace mrover { } auto LanderAlignNodelet::ActionServerCallBack() -> void { + // mActionServer->acceptNewGoal(); + LanderAlignResult result; mPlaneOffsetScalar = 2.5; //If we haven't yet defined the point cloud we are working with mCloud = ros::topic::waitForMessage("/camera/left/points", mNh); filterNormals(mCloud); - ransac(0.1, 10, 100, mPlaneOffsetScalar); + ransac(0.1, 10, 100); //If there is a proper normal to drive to if (mNormalInZEDVector.has_value()) { @@ -56,14 +58,18 @@ namespace mrover { //If we haven't yet defined the point cloud we are working with mCloud = ros::topic::waitForMessage("/camera/left/points", mNh); filterNormals(mCloud); - ransac(0.1, 10, 100, mPlaneOffsetScalar); + ransac(0.1, 10, 100); //If there is a proper normal to drive to if (mNormalInZEDVector.has_value()) { sendTwist(); } - mActionServer->setSucceeded(result); + if(mActionServer->isPreemptRequested()){ + mActionServer->setPreempted(); + }else{ + mActionServer->setSucceeded(result); + } } @@ -213,7 +219,7 @@ namespace mrover { } // update best plane if better inlier count - if (numInliers > minInliers && normal.x() != 0 && normal.y() != 0 && normal.z() != 0) { + if (numInliers > minInliers && normal.x() != 0 && normal.y() != 0 && normal.z() != 0) { // this don't make no sense minInliers = numInliers; mNormalInZEDVector.value() = normal; mBestOffset = offset; From e89cad8ab006efccb73dacf5715ee2065dc8042a Mon Sep 17 00:00:00 2001 From: John Date: Sun, 24 Mar 2024 13:53:09 -0400 Subject: [PATCH 77/89] Removed PID --- .../lander_align/lander_align.cpp | 45 +------------------ .../lander_align/lander_align.hpp | 18 -------- 2 files changed, 1 insertion(+), 62 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index c23ed8c41..9e69f93d6 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -37,8 +37,6 @@ namespace mrover { } auto LanderAlignNodelet::ActionServerCallBack() -> void { - // mActionServer->acceptNewGoal(); - LanderAlignResult result; mPlaneOffsetScalar = 2.5; @@ -219,7 +217,7 @@ namespace mrover { } // update best plane if better inlier count - if (numInliers > minInliers && normal.x() != 0 && normal.y() != 0 && normal.z() != 0) { // this don't make no sense + if (numInliers > minInliers && normal.x() != 0) { minInliers = numInliers; mNormalInZEDVector.value() = normal; mBestOffset = offset; @@ -266,7 +264,6 @@ namespace mrover { Eigen::Vector3d worldUp = Eigen::Vector3d::UnitZ(); Eigen::Vector3d left = worldUp.cross(forward); Eigen::Vector3d up = forward.cross(left); - ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mPlaneLocationInZEDVector.value().x(), mPlaneLocationInZEDVector.value().y(), mPlaneLocationInZEDVector.value().z(), forward.x(), forward.y(), forward.z()); rot.col(0) = forward; rot.col(1) = left; @@ -290,45 +287,6 @@ namespace mrover { if(mOffsetLocationInZEDSE3d.translation().x() < 0) mNormalInZEDVector = std::nullopt; } - auto LanderAlignNodelet::PID::rotate_speed(double theta) const -> double { - return Angle_P * theta; - } - - - auto LanderAlignNodelet::PID::find_angle(Eigen::Vector3d const& current, Eigen::Vector3d const& target) -> float { - Eigen::Vector3d u1 = current.normalized(); - u1.z() = 0; - Eigen::Vector3d u2 = target.normalized(); - u2.z() = 0; - float theta = fmod(acos(u1.dot(u2)), static_cast(180)); - float perp_alignment = u2[0] * -u1[1] + u2[1] * u1[0]; - if (perp_alignment > 0) { - return theta; - } - return -theta; - } - - auto LanderAlignNodelet::PID::drive_speed(float distance) -> float { - return distance * Linear_P; - } - - auto LanderAlignNodelet::PID::find_distance(Eigen::Vector3d const& current, Eigen::Vector3d const& target) -> double { - Eigen::Vector3d difference = target - current; - double distance = difference.norm(); - return distance; - } - - LanderAlignNodelet::PID::PID(float angle_P, float linear_P) : Angle_P(angle_P), Linear_P(linear_P) { - } - - // auto LanderAlignNodelet::PID::calculate(Eigen::Vector3d& input, Eigen::Vector3d& target) -> std::tuple { - // input[2] = 0; - // target[2] = 0; - - // return {rotate_command(input, target), drive_speed(input, target)}; - // } - - void LanderAlignNodelet::sendTwist() { //Locations Eigen::Vector3d rover_dir; @@ -341,7 +299,6 @@ namespace mrover { float const angular_thresh = 0.001; - PID pid(0.1, 0.1); // literally just P -- ugly class and probably needs restructuring in the future ros::Rate rate(20); // ROS Rate at 20Hzn::Matrix3d roverToPlaneNorm; // Eigen::Vector3d Nup = Eigen::Vector3d::UnitZ(); // Eigen::Vector3d Nleft = Nup.cross(mBestNormalInWorld.value()); diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 01398efb2..fe511ce75 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -88,24 +88,6 @@ namespace mrover { void calcMotion(double desiredVelocity, double desiredOmega); static auto calcAngleWithWorldX(Eigen::Vector3d xHeading) -> double; - - class PID { - float const Angle_P; - float const Linear_P; - - public: - PID(float angle_P, float linear_P); - - // auto calculate(Eigen::Vector3d input, Eigen::Vector3d target) -> float; - - [[nodiscard]] auto rotate_speed(double theta) const -> double; - - auto find_angle(Eigen::Vector3d const& current, Eigen::Vector3d const& target) -> float; - - auto find_distance(Eigen::Vector3d const& current, Eigen::Vector3d const& target) -> double; - - auto drive_speed(float) -> float; - }; }; } // namespace mrover From 0ef3a040467f092ac8edffb971f48c86fb53a9b7 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 24 Mar 2024 14:07:26 -0400 Subject: [PATCH 78/89] more refactoring --- .../lander_align/lander_align.cpp | 22 +++++++++---------- .../lander_align/lander_align.hpp | 12 ++++++---- 2 files changed, 19 insertions(+), 15 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 9e69f93d6..8f1108987 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -300,14 +300,9 @@ namespace mrover { ros::Rate rate(20); // ROS Rate at 20Hzn::Matrix3d roverToPlaneNorm; - // Eigen::Vector3d Nup = Eigen::Vector3d::UnitZ(); - // Eigen::Vector3d Nleft = Nup.cross(mBestNormalInWorld.value()); - - // roverToPlaneNorm.col(0) = mBestNormalInWorld.value(); - // roverToPlaneNorm.col(1) = Nup; - // roverToPlaneNorm.col(2) = Nleft; - // manif::SO3d roverToP + while (ros::ok()) { + // If the client has cancelled the server stop moving if(mActionServer->isPreemptRequested()){ mActionServer->setPreempted(); twist.angular.z = 0; @@ -316,14 +311,19 @@ namespace mrover { break; } + // Publish the current state of the RTR controller LanderAlignFeedback feedback; feedback.curr_state = RTRSTRINGS[static_cast(mLoopState)]; mActionServer->publishFeedback(feedback); + + // Push plane and offset locations for debugging SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, mPlaneLocationInWorldSE3d); SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, mOffsetLocationInWorldSE3d); - SE3d rover = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); - Eigen::Vector3d roverPosInWorld{(rover.translation().x()), (rover.translation().y()), 0.0}; + + SE3d roverInWorld = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); + Eigen::Vector3d roverPosInWorld{(roverInWorld.translation().x()), (roverInWorld.translation().y()), 0.0}; Eigen::Vector3d targetPosInWorld; + if (mLoopState == RTRSTATE::turn1) { targetPosInWorld = mOffsetLocationInWorldVector.value(); } else if (mLoopState == RTRSTATE::turn2) { @@ -344,9 +344,9 @@ namespace mrover { //SO3 Matrices for lie algebra SO3d roverToTargetSO3 = SE3Conversions::fromColumns(roverToTargetForward, left, up); - SO3d roverSO3 = rover.asSO3(); + SO3d roverSO3 = roverInWorld.asSO3(); - Eigen::Vector3d distanceToTargetVector = targetPosInWorld - Eigen::Vector3d{rover.translation().x(), rover.translation().y(), rover.translation().z()}; + Eigen::Vector3d distanceToTargetVector = targetPosInWorld - Eigen::Vector3d{roverInWorld.translation().x(), roverInWorld.translation().y(), roverInWorld.translation().z()}; double distanceToTarget = std::abs(distanceToTargetVector.norm()); diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index fe511ce75..ed904670c 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -23,7 +23,8 @@ namespace mrover { }; class LanderAlignNodelet final : public nodelet::Nodelet { - + private: + //PID CONSTANTS double const mAngleP = 1; double const mAngleFloor = 0.05; @@ -74,9 +75,7 @@ namespace mrover { std::vector mFilteredPoints; auto onInit() -> void override; - - void ActionServerCallBack(); - + void filterNormals(sensor_msgs::PointCloud2ConstPtr const& cloud); void ransac(double distanceThreshold, int minInliers, int epochs); @@ -88,6 +87,11 @@ namespace mrover { void calcMotion(double desiredVelocity, double desiredOmega); static auto calcAngleWithWorldX(Eigen::Vector3d xHeading) -> double; + + public: + + void ActionServerCallBack(); + }; } // namespace mrover From 1193b2aecbc634609a15082a37de4964c40548ea Mon Sep 17 00:00:00 2001 From: John Date: Tue, 26 Mar 2024 17:57:04 -0400 Subject: [PATCH 79/89] Organizing --- .../lander_align/lander_align.cpp | 9 ---- .../lander_align/lander_align.hpp | 54 +++++++++---------- src/teleoperation/lander_align/pch.hpp | 11 ++++ 3 files changed, 38 insertions(+), 36 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 8f1108987..94eafa7dd 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,13 +1,4 @@ #include "lander_align.hpp" -#include "mrover/LanderAlignActionFeedback.h" -#include "mrover/LanderAlignActionResult.h" -#include -#include -#include -#include -#include -#include -#include namespace mrover { auto operator<<(std::ostream& ostream, RTRSTATE state) -> std::ostream& { diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index ed904670c..9bf6cb418 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -1,8 +1,6 @@ #pragma once #include "pch.hpp" -#include -#include namespace mrover { @@ -24,27 +22,10 @@ namespace mrover { class LanderAlignNodelet final : public nodelet::Nodelet { private: - - //PID CONSTANTS - double const mAngleP = 1; - double const mAngleFloor = 0.05; - double const mLinearP = 0.3; - + //ROS VARS ros::NodeHandle mNh, mPnh; - std::optional mActionServer; - - ros::Publisher mDebugVectorPub; - - ros::Publisher mDebugPCPub; - - ros::Publisher mTwistPub; - - RTRSTATE mLoopState = RTRSTATE::done; - - //Action Server Variables - sensor_msgs::PointCloud2ConstPtr mCloud; - + //RANSAC VARS double mBestOffset{}; double mPlaneOffsetScalar{}; @@ -61,6 +42,31 @@ namespace mrover { SE3d mOffsetLocationInWorldSE3d; SE3d mPlaneLocationInWorldSE3d; + double mZThreshold{}; + double mXThreshold{}; + int mLeastSamplingDistribution = 10; + + ros::Publisher mDebugPCPub; + + std::vector mFilteredPoints; + + //RTR RTR VARS + RTRSTATE mLoopState; + + ros::Publisher mTwistPub; + + //PID CONSTANTS + double const mAngleP = 1; + double const mAngleFloor = 0.05; + double const mLinearP = 0.3; + + ros::Publisher mDebugVectorPub; + + //Action Server Variables + sensor_msgs::PointCloud2ConstPtr mCloud; + + std::optional mActionServer; + //TF Member Variables tf2_ros::Buffer mTfBuffer; tf2_ros::TransformListener mTfListener{mTfBuffer}; @@ -69,10 +75,6 @@ namespace mrover { std::string mMapFrameId; //Ransac Params - double mZThreshold{}; - double mXThreshold{}; - int mLeastSamplingDistribution = 10; - std::vector mFilteredPoints; auto onInit() -> void override; @@ -89,9 +91,7 @@ namespace mrover { static auto calcAngleWithWorldX(Eigen::Vector3d xHeading) -> double; public: - void ActionServerCallBack(); - }; } // namespace mrover diff --git a/src/teleoperation/lander_align/pch.hpp b/src/teleoperation/lander_align/pch.hpp index c0a6d3a2f..a8673fc88 100644 --- a/src/teleoperation/lander_align/pch.hpp +++ b/src/teleoperation/lander_align/pch.hpp @@ -5,6 +5,17 @@ #include #include +#include "mrover/LanderAlignActionFeedback.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "mrover/LanderAlignActionResult.h" #include #include #include From 5c80fd9c25b9ea0f81c691e95c55aa771081afb7 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 26 Mar 2024 18:16:38 -0400 Subject: [PATCH 80/89] stuff --- src/teleoperation/lander_align/lander_align.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 94eafa7dd..43da53fa6 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -8,6 +8,8 @@ namespace mrover { auto LanderAlignNodelet::onInit() -> void { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); + + //Get the values from ros mZThreshold = .5; mXThreshold = .1; mPlaneOffsetScalar = 2.5; From ac3025c77c7a6421fe4acd9089cc517a53202daf Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Tue, 26 Mar 2024 19:34:11 -0400 Subject: [PATCH 81/89] build action files --- CMakeLists.txt | 2 +- {msg => action}/LanderAlign.action | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename {msg => action}/LanderAlign.action (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index d28810641..75770d6c2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -108,7 +108,7 @@ add_message_files(FILES ${MROVER_MESSAGE_FILES}) add_service_files(FILES ${MROVER_SERVICE_FILES}) -# add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) +add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) generate_messages(DEPENDENCIES ${MROVER_MESSAGE_DEPENDENCIES}) diff --git a/msg/LanderAlign.action b/action/LanderAlign.action similarity index 100% rename from msg/LanderAlign.action rename to action/LanderAlign.action From 01496b6dc50b4076ea1cab95286dc98ad9067c8e Mon Sep 17 00:00:00 2001 From: Julian Skifstad Date: Thu, 28 Mar 2024 18:08:29 -0400 Subject: [PATCH 82/89] stuff --- CMakeLists.txt | 2 +- msg/LanderAlign.action | 8 - src/teleoperation/backend/consumers.py | 4 +- .../lander_align/lander_align.cpp | 346 ++++++++++-------- .../lander_align/lander_align.hpp | 2 +- 5 files changed, 190 insertions(+), 172 deletions(-) delete mode 100644 msg/LanderAlign.action diff --git a/CMakeLists.txt b/CMakeLists.txt index d28810641..75770d6c2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -108,7 +108,7 @@ add_message_files(FILES ${MROVER_MESSAGE_FILES}) add_service_files(FILES ${MROVER_SERVICE_FILES}) -# add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) +add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) generate_messages(DEPENDENCIES ${MROVER_MESSAGE_DEPENDENCIES}) diff --git a/msg/LanderAlign.action b/msg/LanderAlign.action deleted file mode 100644 index ff6bc6d0c..000000000 --- a/msg/LanderAlign.action +++ /dev/null @@ -1,8 +0,0 @@ -# Define the goal -uint32 num # Specify which dishwasher we want to use ---- -# Define the result -uint32 num ---- -# Define a feedback message -string curr_state diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 4448c891d..c9eb31937 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -29,7 +29,7 @@ LanderAlignAction, LanderAlignGoal ) -from mrover.srv import EnableAuton, ChangeCameras, CapturePanorama +from mrover.srv import EnableAuton, ChangeCameras from sensor_msgs.msg import NavSatFix, Temperature, RelativeHumidity, Image from mrover.srv import EnableAuton from sensor_msgs.msg import NavSatFix, Temperature, RelativeHumidity @@ -103,7 +103,7 @@ def connect(self): self.cache_enable_limit = rospy.ServiceProxy("cache_enable_limit_switches", SetBool) self.calibrate_service = rospy.ServiceProxy("arm_calibrate", Trigger) self.change_cameras_srv = rospy.ServiceProxy("change_cameras", ChangeCameras) - self.capture_panorama_srv: Any = rospy.ServiceProxy("capture_panorama", CapturePanorama) + # self.capture_panorama_srv = rospy.ServiceProxy("capture_panorama", CapturePanorama) # ROS Parameters self.mappings = rospy.get_param("teleop/joystick_mappings") diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 43da53fa6..15fd6f401 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,4 +1,6 @@ #include "lander_align.hpp" +#include +#include namespace mrover { auto operator<<(std::ostream& ostream, RTRSTATE state) -> std::ostream& { @@ -17,7 +19,7 @@ namespace mrover { mTwistPub = mNh.advertise("/cmd_vel", 1); mDebugPCPub = mNh.advertise("/lander_align/debugPC", 1); - mActionServer.emplace(mNh, "LanderAlignAction", [&](LanderAlignGoalConstPtr const& goal) { ActionServerCallBack(); }, false); + mActionServer.emplace(mNh, "LanderAlignAction", [&](LanderAlignGoalConstPtr const& goal) { ActionServerCallBack(goal); }, false); mActionServer.value().start(); //TF Create the Reference Frames @@ -29,22 +31,22 @@ namespace mrover { mLoopState = RTRSTATE::turn1; } - auto LanderAlignNodelet::ActionServerCallBack() -> void { + auto LanderAlignNodelet::ActionServerCallBack(LanderAlignGoalConstPtr const goal) -> void { LanderAlignResult result; - mPlaneOffsetScalar = 2.5; + // mPlaneOffsetScalar = 2.5; - //If we haven't yet defined the point cloud we are working with - mCloud = ros::topic::waitForMessage("/camera/left/points", mNh); - filterNormals(mCloud); - ransac(0.1, 10, 100); + // //If we haven't yet defined the point cloud we are working with + // mCloud = ros::topic::waitForMessage("/camera/left/points", mNh); + // filterNormals(mCloud); + // ransac(0.1, 10, 100); - //If there is a proper normal to drive to - if (mNormalInZEDVector.has_value()) { - sendTwist(); - } + // //If there is a proper normal to drive to + // if (mNormalInZEDVector.has_value()) { + // sendTwist(); + // } mPlaneOffsetScalar = 1; - mLoopState = RTRSTATE::turn1; + // mLoopState = RTRSTATE::turn1; //If we haven't yet defined the point cloud we are working with mCloud = ros::topic::waitForMessage("/camera/left/points", mNh); @@ -53,7 +55,9 @@ namespace mrover { //If there is a proper normal to drive to if (mNormalInZEDVector.has_value()) { - sendTwist(); + // sendTwist(); + calcMotion(.5,0); + } if(mActionServer->isPreemptRequested()){ @@ -78,40 +82,62 @@ namespace mrover { } void LanderAlignNodelet::calcMotion(double desiredVelocity, double desiredOmega) { + SE3d roverInWorld = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); - Eigen::Vector3d xAxisRotation = roverInWorld.rotation().col(0); - double roverHeading = calcAngleWithWorldX(xAxisRotation); - double targetHeading = calcAngleWithWorldX(-mNormalInWorldVector.value()); - - //Current State - Eigen::Vector3d currState{roverInWorld.translation().x(), roverInWorld.translation().y(), roverHeading}; + + // Inital State + Eigen::Vector2d initState{roverInWorld.translation().x(), roverInWorld.translation().y()}; //Target State + double targetHeading = calcAngleWithWorldX(-mNormalInWorldVector.value()); + Eigen::Vector3d tarState{mOffsetLocationInWorldVector.value().x(), mOffsetLocationInWorldVector.value().y(), targetHeading}; + Eigen::Vector2d distanceToTargetVectorInit{tarState.x() - initState.x(), tarState.y() - initState.y()}; + double distanceToTargetInitial = std::abs(distanceToTargetVectorInit.norm()); + + while (ros::ok()) { + roverInWorld = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); + Eigen::Vector3d xOrientation = roverInWorld.rotation().col(0); + double roverHeading = calcAngleWithWorldX(xOrientation); + Eigen::Vector3d currState{roverInWorld.translation().x(), roverInWorld.translation().y(), roverHeading}; - //Constants - double Kx = 1; - double Ky = 1; - double Ktheta = 1; - Eigen::Matrix3d rotation; - rotation << std::cos(roverHeading), std::sin(roverHeading), 0, - -std::sin(roverHeading), std::cos(roverHeading), 0, - 0, 0, 1; + Eigen::Vector2d distanceToTargetVector{tarState.x() - currState.x(), tarState.y() - currState.y()}; + double distanceToTarget = std::abs(distanceToTargetVector.norm()); + //Constants + double Kx = .2; + double Ky = .2; + double Ktheta = .8; - Eigen::Vector3d errState = rotation * (tarState - currState); + Eigen::Matrix3d rotation; + rotation << std::cos(roverHeading), std::sin(roverHeading), 0, + -std::sin(roverHeading), std::cos(roverHeading), 0, + 0, 0, 1; - //I think this is rad/s - double zRotation = desiredOmega + (desiredVelocity / 2) * (Ky * (errState.y() + Ktheta * errState.z()) + (1 / Ky) * std::sin(errState.z())); + Eigen::Vector3d errState = rotation * (tarState - currState); - //I think this is unit/s - double xSpeed = Kx * errState.x() + desiredVelocity * std::cos(errState.z()) - Ktheta * errState.z() * zRotation; + //I think this is rad/s + // (distanceToTarget / distanceToTargetInitial) * + double zRotation = ( desiredOmega + (desiredVelocity / 2) * (Ky * (errState.y() + Ktheta * errState.z()) + (1 / Ktheta) * std::sin(errState.z()))); - geometry_msgs::Twist twist; - twist.angular.z = zRotation; - twist.linear.x = xSpeed; + //I think this is unit/s + double xSpeed = (distanceToTarget / distanceToTargetInitial) * (Kx * errState.x() + desiredVelocity * std::cos(errState.z()) - Ktheta * errState.z() * zRotation); - mTwistPub.publish(twist); + geometry_msgs::Twist twist; + twist.angular.z = zRotation; + twist.linear.x = xSpeed; + if(mActionServer->isPreemptRequested()){ + twist.angular.z = 0; + twist.linear.x = 0; + mActionServer->setPreempted(); + mTwistPub.publish(twist); + break; + } + if (distanceToTarget < .1) break; + mTwistPub.publish(twist); + } + + } void LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2ConstPtr const& cloud) { @@ -280,146 +306,146 @@ namespace mrover { if(mOffsetLocationInZEDSE3d.translation().x() < 0) mNormalInZEDVector = std::nullopt; } - void LanderAlignNodelet::sendTwist() { - //Locations - Eigen::Vector3d rover_dir; + // void LanderAlignNodelet::sendTwist() { + // //Locations + // Eigen::Vector3d rover_dir; - //Final msg - geometry_msgs::Twist twist; + // //Final msg + // geometry_msgs::Twist twist; - //Threhsolds - float const linear_thresh = 0.01; // could be member variables - float const angular_thresh = 0.001; + // //Threhsolds + // float const linear_thresh = 0.01; // could be member variables + // float const angular_thresh = 0.001; - ros::Rate rate(20); // ROS Rate at 20Hzn::Matrix3d roverToPlaneNorm; + // ros::Rate rate(20); // ROS Rate at 20Hzn::Matrix3d roverToPlaneNorm; - while (ros::ok()) { - // If the client has cancelled the server stop moving - if(mActionServer->isPreemptRequested()){ - mActionServer->setPreempted(); - twist.angular.z = 0; - twist.linear.x = 0; - mTwistPub.publish(twist); - break; - } + // while (ros::ok()) { + // // If the client has cancelled the server stop moving + // if(mActionServer->isPreemptRequested()){ + // mActionServer->setPreempted(); + // twist.angular.z = 0; + // twist.linear.x = 0; + // mTwistPub.publish(twist); + // break; + // } - // Publish the current state of the RTR controller - LanderAlignFeedback feedback; - feedback.curr_state = RTRSTRINGS[static_cast(mLoopState)]; - mActionServer->publishFeedback(feedback); + // // Publish the current state of the RTR controller + // LanderAlignFeedback feedback; + // feedback.curr_state = RTRSTRINGS[static_cast(mLoopState)]; + // mActionServer->publishFeedback(feedback); - // Push plane and offset locations for debugging - SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, mPlaneLocationInWorldSE3d); - SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, mOffsetLocationInWorldSE3d); + // // Push plane and offset locations for debugging + // SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, mPlaneLocationInWorldSE3d); + // SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, mOffsetLocationInWorldSE3d); - SE3d roverInWorld = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); - Eigen::Vector3d roverPosInWorld{(roverInWorld.translation().x()), (roverInWorld.translation().y()), 0.0}; - Eigen::Vector3d targetPosInWorld; - - if (mLoopState == RTRSTATE::turn1) { - targetPosInWorld = mOffsetLocationInWorldVector.value(); - } else if (mLoopState == RTRSTATE::turn2) { - targetPosInWorld = mPlaneLocationInWorldVector.value(); - } - targetPosInWorld.z() = 0; + // SE3d roverInWorld = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); + // Eigen::Vector3d roverPosInWorld{(roverInWorld.translation().x()), (roverInWorld.translation().y()), 0.0}; + // Eigen::Vector3d targetPosInWorld; - Eigen::Vector3d roverToTargetForward = targetPosInWorld - roverPosInWorld; - roverToTargetForward.normalize(); + // if (mLoopState == RTRSTATE::turn1) { + // targetPosInWorld = mOffsetLocationInWorldVector.value(); + // } else if (mLoopState == RTRSTATE::turn2) { + // targetPosInWorld = mPlaneLocationInWorldVector.value(); + // } + // targetPosInWorld.z() = 0; - Eigen::Vector3d up = Eigen::Vector3d::UnitZ(); - Eigen::Vector3d left = up.cross(roverToTargetForward); + // Eigen::Vector3d roverToTargetForward = targetPosInWorld - roverPosInWorld; + // roverToTargetForward.normalize(); - Eigen::Matrix3d roverToTargetMat; - roverToTargetMat.col(0) = roverToTargetForward; - roverToTargetMat.col(1) = up; - roverToTargetMat.col(2) = left; + // Eigen::Vector3d up = Eigen::Vector3d::UnitZ(); + // Eigen::Vector3d left = up.cross(roverToTargetForward); - //SO3 Matrices for lie algebra - SO3d roverToTargetSO3 = SE3Conversions::fromColumns(roverToTargetForward, left, up); - SO3d roverSO3 = roverInWorld.asSO3(); + // Eigen::Matrix3d roverToTargetMat; + // roverToTargetMat.col(0) = roverToTargetForward; + // roverToTargetMat.col(1) = up; + // roverToTargetMat.col(2) = left; - Eigen::Vector3d distanceToTargetVector = targetPosInWorld - Eigen::Vector3d{roverInWorld.translation().x(), roverInWorld.translation().y(), roverInWorld.translation().z()}; + // //SO3 Matrices for lie algebra + // SO3d roverToTargetSO3 = SE3Conversions::fromColumns(roverToTargetForward, left, up); + // SO3d roverSO3 = roverInWorld.asSO3(); - double distanceToTarget = std::abs(distanceToTargetVector.norm()); - - manif::SO3Tangentd SO3tan = roverToTargetSO3 - roverSO3; // 3 x 1 matrix of angular velocities (x,y,z) - - switch (mLoopState) { - case RTRSTATE::turn1: { - if (distanceToTarget < 0.5) mLoopState = RTRSTATE::turn2; - - - double angle_rate = mAngleP * SO3tan.z(); - angle_rate = (std::abs(angle_rate) > mAngleFloor) ? angle_rate : copysign(mAngleFloor, angle_rate); - - ROS_INFO("w_z velocity %f", SO3tan.z()); - - twist.angular.z = angle_rate; - - if (std::abs(SO3tan.z()) < angular_thresh) { - mLoopState = RTRSTATE::drive; - twist.angular.z = 0; - twist.linear.x = 0; - ROS_INFO("Done spinning"); - } - // ROS_INFO("In state: turning to point..."); - break; - } - - case RTRSTATE::drive: { - if (std::abs(SO3tan.z()) > angular_thresh) { - mLoopState = RTRSTATE::turn1; - ROS_INFO("Rotation got off"); - twist.linear.x = 0; - } - double driveRate = std::min(mLinearP * distanceToTarget, 1.0); + // Eigen::Vector3d distanceToTargetVector = targetPosInWorld - Eigen::Vector3d{roverInWorld.translation().x(), roverInWorld.translation().y(), roverInWorld.translation().z()}; - ROS_INFO("distance: %f", distanceToTarget); + // double distanceToTarget = std::abs(distanceToTargetVector.norm()); - twist.linear.x = driveRate; + // manif::SO3Tangentd SO3tan = roverToTargetSO3 - roverSO3; // 3 x 1 matrix of angular velocities (x,y,z) - ROS_INFO("distance to target %f", distanceToTarget); - - if (std::abs(distanceToTarget) < linear_thresh) { - mLoopState = RTRSTATE::turn2; - twist.linear.x = 0; - twist.angular.z = 0; - } - // ROS_INFO("In state: driving to point..."); - break; - } - - case RTRSTATE::turn2: { - double angle_rate = mAngleP * SO3tan.z(); - angle_rate = (std::abs(angle_rate) > mAngleFloor) ? angle_rate : copysign(mAngleFloor, angle_rate); - twist.angular.z = angle_rate; - twist.linear.x = 0; - - - if (std::abs(SO3tan.z()) < angular_thresh) { - mLoopState = RTRSTATE::done; - twist.angular.z = 0; - twist.linear.x = 0; - // ROS_INFO("Done turning to lander"); - } - // ROS_INFO("In state: turning to lander..."); - break; - } + // switch (mLoopState) { + // case RTRSTATE::turn1: { + // if (distanceToTarget < 0.5) mLoopState = RTRSTATE::turn2; + - case RTRSTATE::done: { - twist.linear.x = 0; - twist.angular.z = 0; - break; - mCloud = nullptr; - } - } - // ROS_INFO("THE TWIST IS: Angular: %f, with linear %f,", twist.angular.z, twist.linear.x); - ROS_INFO_STREAM(mLoopState); - mTwistPub.publish(twist); - if (mLoopState == RTRSTATE::done) { - break; - } - } - } + // double angle_rate = mAngleP * SO3tan.z(); + // angle_rate = (std::abs(angle_rate) > mAngleFloor) ? angle_rate : copysign(mAngleFloor, angle_rate); + + // ROS_INFO("w_z velocity %f", SO3tan.z()); + + // twist.angular.z = angle_rate; + + // if (std::abs(SO3tan.z()) < angular_thresh) { + // mLoopState = RTRSTATE::drive; + // twist.angular.z = 0; + // twist.linear.x = 0; + // ROS_INFO("Done spinning"); + // } + // // ROS_INFO("In state: turning to point..."); + // break; + // } + + // case RTRSTATE::drive: { + // if (std::abs(SO3tan.z()) > angular_thresh) { + // mLoopState = RTRSTATE::turn1; + // ROS_INFO("Rotation got off"); + // twist.linear.x = 0; + // } + // double driveRate = std::min(mLinearP * distanceToTarget, 1.0); + + // ROS_INFO("distance: %f", distanceToTarget); + + // twist.linear.x = driveRate; + + // ROS_INFO("distance to target %f", distanceToTarget); + + // if (std::abs(distanceToTarget) < linear_thresh) { + // mLoopState = RTRSTATE::turn2; + // twist.linear.x = 0; + // twist.angular.z = 0; + // } + // // ROS_INFO("In state: driving to point..."); + // break; + // } + + // case RTRSTATE::turn2: { + // double angle_rate = mAngleP * SO3tan.z(); + // angle_rate = (std::abs(angle_rate) > mAngleFloor) ? angle_rate : copysign(mAngleFloor, angle_rate); + // twist.angular.z = angle_rate; + // twist.linear.x = 0; + + + // if (std::abs(SO3tan.z()) < angular_thresh) { + // mLoopState = RTRSTATE::done; + // twist.angular.z = 0; + // twist.linear.x = 0; + // // ROS_INFO("Done turning to lander"); + // } + // // ROS_INFO("In state: turning to lander..."); + // break; + // } + + // case RTRSTATE::done: { + // twist.linear.x = 0; + // twist.angular.z = 0; + // break; + // mCloud = nullptr; + // } + // } + // // ROS_INFO("THE TWIST IS: Angular: %f, with linear %f,", twist.angular.z, twist.linear.x); + // ROS_INFO_STREAM(mLoopState); + // mTwistPub.publish(twist); + // if (mLoopState == RTRSTATE::done) { + // break; + // } + // } + // } } // namespace mrover diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 9bf6cb418..285b6fc22 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -91,7 +91,7 @@ namespace mrover { static auto calcAngleWithWorldX(Eigen::Vector3d xHeading) -> double; public: - void ActionServerCallBack(); + void ActionServerCallBack(LanderAlignGoalConstPtr const goal); }; } // namespace mrover From d463b7be88b3a32399459b0594085c44dc19d6d0 Mon Sep 17 00:00:00 2001 From: Julian Skifstad Date: Thu, 28 Mar 2024 20:21:34 -0400 Subject: [PATCH 83/89] nothing works :) --- .../lander_align/lander_align.cpp | 28 +++++++++++++------ 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 15fd6f401..8df4873ff 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,4 +1,6 @@ #include "lander_align.hpp" +#include +#include #include #include @@ -72,6 +74,7 @@ namespace mrover { auto LanderAlignNodelet::calcAngleWithWorldX(Eigen::Vector3d xHeading) -> double { //I want to be editing this variable so it should not be const or & xHeading.z() = 0; xHeading.normalize(); + // ROS_INFO_STREAM("fucker" << xHeading.x() << " " << xHeading.y() << " " << xHeading.z()); Eigen::Vector3d xAxisWorld{1, 0, 0}; double angle = std::acos(xHeading.dot(xAxisWorld)); if (xHeading.y() >= 0) { @@ -89,11 +92,12 @@ namespace mrover { Eigen::Vector2d initState{roverInWorld.translation().x(), roverInWorld.translation().y()}; //Target State + // double targetHeading = calcAngleWithWorldX({0,1,0}); double targetHeading = calcAngleWithWorldX(-mNormalInWorldVector.value()); - + ROS_INFO_STREAM(mNormalInWorldVector.value().x() << " " << mNormalInWorldVector.value().y() << " " << mNormalInWorldVector.value().z()); Eigen::Vector3d tarState{mOffsetLocationInWorldVector.value().x(), mOffsetLocationInWorldVector.value().y(), targetHeading}; - Eigen::Vector2d distanceToTargetVectorInit{tarState.x() - initState.x(), tarState.y() - initState.y()}; - double distanceToTargetInitial = std::abs(distanceToTargetVectorInit.norm()); + Eigen::Vector2d distanceToTargetVectorInit{tarState.x() - initState.x (), tarState.y() - initState.y()}; + //double distanceToTargetInitial = std::abs(distanceToTargetVectorInit.norm()); while (ros::ok()) { roverInWorld = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); @@ -106,8 +110,8 @@ namespace mrover { double distanceToTarget = std::abs(distanceToTargetVector.norm()); //Constants double Kx = .2; - double Ky = .2; - double Ktheta = .8; + double Ky = .6; + double Ktheta = 0.5; Eigen::Matrix3d rotation; rotation << std::cos(roverHeading), std::sin(roverHeading), 0, @@ -115,13 +119,16 @@ namespace mrover { 0, 0, 1; Eigen::Vector3d errState = rotation * (tarState - currState); - + // desiredVelocity = (distanceToTarget / distanceToTargetInitial) *desiredVelocity; + // ROS_INFO_STREAM("ANGLE ERROR: " << errState.z() << std::endl); + // ROS_INFO_STREAM("Position ERROR x: " << errState.x() << std::endl); + // ROS_INFO_STREAM("Position ERROR y: " << errState.y() << std::endl); //I think this is rad/s // (distanceToTarget / distanceToTargetInitial) * double zRotation = ( desiredOmega + (desiredVelocity / 2) * (Ky * (errState.y() + Ktheta * errState.z()) + (1 / Ktheta) * std::sin(errState.z()))); //I think this is unit/s - double xSpeed = (distanceToTarget / distanceToTargetInitial) * (Kx * errState.x() + desiredVelocity * std::cos(errState.z()) - Ktheta * errState.z() * zRotation); + double xSpeed = (Kx * errState.x() + desiredVelocity * std::cos(errState.z()) - Ktheta * errState.z() * zRotation); geometry_msgs::Twist twist; twist.angular.z = zRotation; @@ -135,6 +142,7 @@ namespace mrover { } if (distanceToTarget < .1) break; mTwistPub.publish(twist); + SE3Conversions::pushToTfTree(mTfBroadcaster, "location_thing", mMapFrameId, SE3d{{mNormalInWorldVector.value().x(), mNormalInWorldVector.value().y(), mNormalInWorldVector.value().z()}, SO3d{Eigen::Quaterniond{0,0,0,0}.normalized()}}); } @@ -272,11 +280,13 @@ namespace mrover { if (mNormalInZEDVector.value().x() > 0) mNormalInZEDVector.value() *= -1; - mOffsetLocationInZEDVector = std::make_optional(mPlaneLocationInZEDVector.value() + mPlaneOffsetScalar * mNormalInZEDVector.value()); SE3d zedToMap = SE3Conversions::fromTfTree(mTfBuffer, mCameraFrameId, mMapFrameId); + + mNormalInWorldVector = std::make_optional(zedToMap.rotation() * mNormalInZEDVector.value()); + //Calculate the SO3 in the world frame Eigen::Matrix3d rot; Eigen::Vector3d forward = mNormalInZEDVector.value().normalized(); @@ -331,7 +341,7 @@ namespace mrover { // } // // Publish the current state of the RTR controller - // LanderAlignFeedback feedback; + // LanderAlignFeedback feedbaSck; // feedback.curr_state = RTRSTRINGS[static_cast(mLoopState)]; // mActionServer->publishFeedback(feedback); From 888241c0437feb3b3266b2e049eec2060440bea1 Mon Sep 17 00:00:00 2001 From: John Date: Sat, 30 Mar 2024 08:47:30 -0400 Subject: [PATCH 84/89] Keeps going past; Not inline with normal --- .../lander_align/lander_align.cpp | 33 ++++++++++--------- 1 file changed, 18 insertions(+), 15 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 8df4873ff..43c0434a5 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -1,5 +1,8 @@ #include "lander_align.hpp" #include +#include +#include +#include #include #include #include @@ -58,7 +61,7 @@ namespace mrover { //If there is a proper normal to drive to if (mNormalInZEDVector.has_value()) { // sendTwist(); - calcMotion(.5,0); + calcMotion(1,0); } @@ -80,7 +83,7 @@ namespace mrover { if (xHeading.y() >= 0) { return angle; } else { - return angle + std::numbers::pi; + return 2 * std::numbers::pi - angle; } } @@ -103,23 +106,23 @@ namespace mrover { roverInWorld = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); Eigen::Vector3d xOrientation = roverInWorld.rotation().col(0); double roverHeading = calcAngleWithWorldX(xOrientation); + ROS_INFO_STREAM("Current Rover Anlge" << roverHeading); Eigen::Vector3d currState{roverInWorld.translation().x(), roverInWorld.translation().y(), roverHeading}; - Eigen::Vector2d distanceToTargetVector{tarState.x() - currState.x(), tarState.y() - currState.y()}; double distanceToTarget = std::abs(distanceToTargetVector.norm()); //Constants - double Kx = .2; - double Ky = .6; - double Ktheta = 0.5; + double Kx = 0.5; + double Ky = 0.5; + double Ktheta = 2; Eigen::Matrix3d rotation; - rotation << std::cos(roverHeading), std::sin(roverHeading), 0, - -std::sin(roverHeading), std::cos(roverHeading), 0, + rotation << std::cos(roverHeading), std::sin(roverHeading), 0, + -std::sin(roverHeading), std::cos(roverHeading), 0, 0, 0, 1; Eigen::Vector3d errState = rotation * (tarState - currState); - // desiredVelocity = (distanceToTarget / distanceToTargetInitial) *desiredVelocity; + //desiredVelocity = (distanceToTarget / distanceToTargetInitial) *desiredVelocity; // ROS_INFO_STREAM("ANGLE ERROR: " << errState.z() << std::endl); // ROS_INFO_STREAM("Position ERROR x: " << errState.x() << std::endl); // ROS_INFO_STREAM("Position ERROR y: " << errState.y() << std::endl); @@ -133,19 +136,15 @@ namespace mrover { geometry_msgs::Twist twist; twist.angular.z = zRotation; twist.linear.x = xSpeed; - if(mActionServer->isPreemptRequested()){ + if(mActionServer->isPreemptRequested() || distanceToTarget < .1){ twist.angular.z = 0; twist.linear.x = 0; mActionServer->setPreempted(); mTwistPub.publish(twist); break; } - if (distanceToTarget < .1) break; mTwistPub.publish(twist); - SE3Conversions::pushToTfTree(mTfBroadcaster, "location_thing", mMapFrameId, SE3d{{mNormalInWorldVector.value().x(), mNormalInWorldVector.value().y(), mNormalInWorldVector.value().z()}, SO3d{Eigen::Quaterniond{0,0,0,0}.normalized()}}); - } - - + } } void LanderAlignNodelet::filterNormals(sensor_msgs::PointCloud2ConstPtr const& cloud) { @@ -298,6 +297,10 @@ namespace mrover { rot.col(1) = left; rot.col(2) = up; + //TODO: Remove this + //SE3d dummy = {{mNormalInWorldVector->x(), mNormalInWorldVector->y(), mNormalInWorldVector->z()}, SO3d{Eigen::Quaterniond{rot}.normalized()}}; + //SE3Conversions::pushToTfTree(mTfBroadcaster, "dummy", mMapFrameId, dummy); + //Calculate the plane location in the world frame SE3d mPlaneLocationInZEDSE3d = {{mPlaneLocationInZEDVector.value().x(), mPlaneLocationInZEDVector.value().y(), mPlaneLocationInZEDVector.value().z()}, SO3d{Eigen::Quaterniond{rot}.normalized()}}; mPlaneLocationInWorldSE3d = zedToMap * mPlaneLocationInZEDSE3d; From 8d19aa621fbfd87be61127186c050a06e7d76a73 Mon Sep 17 00:00:00 2001 From: John Date: Sat, 30 Mar 2024 10:58:52 -0400 Subject: [PATCH 85/89] :( --- src/teleoperation/lander_align/lander_align.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 43c0434a5..b27317dec 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -61,7 +61,7 @@ namespace mrover { //If there is a proper normal to drive to if (mNormalInZEDVector.has_value()) { // sendTwist(); - calcMotion(1,0); + calcMotion(0.25,0); } @@ -77,7 +77,7 @@ namespace mrover { auto LanderAlignNodelet::calcAngleWithWorldX(Eigen::Vector3d xHeading) -> double { //I want to be editing this variable so it should not be const or & xHeading.z() = 0; xHeading.normalize(); - // ROS_INFO_STREAM("fucker" << xHeading.x() << " " << xHeading.y() << " " << xHeading.z()); + Eigen::Vector3d xAxisWorld{1, 0, 0}; double angle = std::acos(xHeading.dot(xAxisWorld)); if (xHeading.y() >= 0) { @@ -113,13 +113,13 @@ namespace mrover { double distanceToTarget = std::abs(distanceToTargetVector.norm()); //Constants double Kx = 0.5; - double Ky = 0.5; - double Ktheta = 2; + double Ky = 10; + double Ktheta = 1; Eigen::Matrix3d rotation; rotation << std::cos(roverHeading), std::sin(roverHeading), 0, -std::sin(roverHeading), std::cos(roverHeading), 0, - 0, 0, 1; + 0, 0, 1; Eigen::Vector3d errState = rotation * (tarState - currState); //desiredVelocity = (distanceToTarget / distanceToTargetInitial) *desiredVelocity; @@ -136,7 +136,7 @@ namespace mrover { geometry_msgs::Twist twist; twist.angular.z = zRotation; twist.linear.x = xSpeed; - if(mActionServer->isPreemptRequested() || distanceToTarget < .1){ + if(mActionServer->isPreemptRequested() || distanceToTarget < .5){ twist.angular.z = 0; twist.linear.x = 0; mActionServer->setPreempted(); From 95c91f476d96ce7c45f01474511538bf365c0064 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 31 Mar 2024 11:13:53 -0400 Subject: [PATCH 86/89] IDK changes or smthn --- src/teleoperation/lander_align/lander_align.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index b27317dec..72e7bc9f5 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -122,11 +122,7 @@ namespace mrover { 0, 0, 1; Eigen::Vector3d errState = rotation * (tarState - currState); - //desiredVelocity = (distanceToTarget / distanceToTargetInitial) *desiredVelocity; - // ROS_INFO_STREAM("ANGLE ERROR: " << errState.z() << std::endl); - // ROS_INFO_STREAM("Position ERROR x: " << errState.x() << std::endl); - // ROS_INFO_STREAM("Position ERROR y: " << errState.y() << std::endl); - //I think this is rad/s + // (distanceToTarget / distanceToTargetInitial) * double zRotation = ( desiredOmega + (desiredVelocity / 2) * (Ky * (errState.y() + Ktheta * errState.z()) + (1 / Ktheta) * std::sin(errState.z()))); From 6658f4b362c3895bccbb48e30e05516f4f2424e4 Mon Sep 17 00:00:00 2001 From: John Date: Sun, 31 Mar 2024 11:22:34 -0400 Subject: [PATCH 87/89] Not sure if its supposed to be line tracking --- src/teleoperation/lander_align/lander_align.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 72e7bc9f5..d4f0be6e3 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -114,7 +114,7 @@ namespace mrover { //Constants double Kx = 0.5; double Ky = 10; - double Ktheta = 1; + double Ktheta = 5; Eigen::Matrix3d rotation; rotation << std::cos(roverHeading), std::sin(roverHeading), 0, From 6648e445a7c3e873f8d162e14e779848e2834726 Mon Sep 17 00:00:00 2001 From: Julian Skifstad Date: Sun, 31 Mar 2024 14:04:43 -0400 Subject: [PATCH 88/89] Beginning some things --- .../lander_align/lander_align.cpp | 273 +++++++++--------- .../lander_align/lander_align.hpp | 2 + 2 files changed, 145 insertions(+), 130 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index d4f0be6e3..25382ec2b 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -85,7 +85,7 @@ namespace mrover { } else { return 2 * std::numbers::pi - angle; } - } + }; void LanderAlignNodelet::calcMotion(double desiredVelocity, double desiredOmega) { @@ -279,7 +279,6 @@ namespace mrover { SE3d zedToMap = SE3Conversions::fromTfTree(mTfBuffer, mCameraFrameId, mMapFrameId); - mNormalInWorldVector = std::make_optional(zedToMap.rotation() * mNormalInZEDVector.value()); //Calculate the SO3 in the world frame @@ -293,10 +292,6 @@ namespace mrover { rot.col(1) = left; rot.col(2) = up; - //TODO: Remove this - //SE3d dummy = {{mNormalInWorldVector->x(), mNormalInWorldVector->y(), mNormalInWorldVector->z()}, SO3d{Eigen::Quaterniond{rot}.normalized()}}; - //SE3Conversions::pushToTfTree(mTfBroadcaster, "dummy", mMapFrameId, dummy); - //Calculate the plane location in the world frame SE3d mPlaneLocationInZEDSE3d = {{mPlaneLocationInZEDVector.value().x(), mPlaneLocationInZEDVector.value().y(), mPlaneLocationInZEDVector.value().z()}, SO3d{Eigen::Quaterniond{rot}.normalized()}}; mPlaneLocationInWorldSE3d = zedToMap * mPlaneLocationInZEDSE3d; @@ -311,150 +306,168 @@ namespace mrover { SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, mPlaneLocationInWorldSE3d); SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, mOffsetLocationInWorldSE3d); + SE3d roverInWorld = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); + Eigen::Vector3d roverPosInWorld{(roverInWorld.translation().x()), (roverInWorld.translation().y()), 0.0}; + Eigen::Vector3d projRover = mNormalInWorldVector.value().dot(roverPosInWorld) * mNormalInWorldVector.value(); + Eigen::Vector3d projPlane = mNormalInWorldVector.value().dot(mPlaneLocationInWorldVector.value()) * mNormalInWorldVector.value(); + double splineLength = (projPlane - projRover).norm(); + //Compare Rover Location to Target Location if(mOffsetLocationInZEDSE3d.translation().x() < 0) mNormalInZEDVector = std::nullopt; } - // void LanderAlignNodelet::sendTwist() { - // //Locations - // Eigen::Vector3d rover_dir; + void LanderAlignNodelet::sendTwist() { + //Locations + Eigen::Vector3d rover_dir; - // //Final msg - // geometry_msgs::Twist twist; + //Final msg + geometry_msgs::Twist twist; - // //Threhsolds - // float const linear_thresh = 0.01; // could be member variables - // float const angular_thresh = 0.001; + //Threhsolds + float const linear_thresh = 0.01; // could be member variables + float const angular_thresh = 0.001; - // ros::Rate rate(20); // ROS Rate at 20Hzn::Matrix3d roverToPlaneNorm; + ros::Rate rate(20); // ROS Rate at 20Hzn::Matrix3d roverToPlaneNorm; - // while (ros::ok()) { - // // If the client has cancelled the server stop moving - // if(mActionServer->isPreemptRequested()){ - // mActionServer->setPreempted(); - // twist.angular.z = 0; - // twist.linear.x = 0; - // mTwistPub.publish(twist); - // break; - // } + while (ros::ok()) { + // If the client has cancelled the server stop moving + if(mActionServer->isPreemptRequested()){ + mActionServer->setPreempted(); + twist.angular.z = 0; + twist.linear.x = 0; + mTwistPub.publish(twist); + break; + } - // // Publish the current state of the RTR controller - // LanderAlignFeedback feedbaSck; - // feedback.curr_state = RTRSTRINGS[static_cast(mLoopState)]; - // mActionServer->publishFeedback(feedback); + // Publish the current state of the RTR controller + LanderAlignFeedback feedback; + feedback.curr_state = RTRSTRINGS[static_cast(mLoopState)]; + mActionServer->publishFeedback(feedback); - // // Push plane and offset locations for debugging - // SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, mPlaneLocationInWorldSE3d); - // SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, mOffsetLocationInWorldSE3d); + // Push plane and offset locations for debugging + SE3Conversions::pushToTfTree(mTfBroadcaster, "plane", mMapFrameId, mPlaneLocationInWorldSE3d); + SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, mOffsetLocationInWorldSE3d); - // SE3d roverInWorld = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); - // Eigen::Vector3d roverPosInWorld{(roverInWorld.translation().x()), (roverInWorld.translation().y()), 0.0}; - // Eigen::Vector3d targetPosInWorld; - - // if (mLoopState == RTRSTATE::turn1) { - // targetPosInWorld = mOffsetLocationInWorldVector.value(); - // } else if (mLoopState == RTRSTATE::turn2) { - // targetPosInWorld = mPlaneLocationInWorldVector.value(); - // } - // targetPosInWorld.z() = 0; + SE3d roverInWorld = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "map"); + Eigen::Vector3d roverPosInWorld{(roverInWorld.translation().x()), (roverInWorld.translation().y()), 0.0}; + Eigen::Vector3d targetPosInWorld; + + if (mLoopState == RTRSTATE::turn1) { + targetPosInWorld = mOffsetLocationInWorldVector.value(); + } else if (mLoopState == RTRSTATE::turn2) { + targetPosInWorld = mPlaneLocationInWorldVector.value(); + } + targetPosInWorld.z() = 0; - // Eigen::Vector3d roverToTargetForward = targetPosInWorld - roverPosInWorld; - // roverToTargetForward.normalize(); + Eigen::Vector3d roverToTargetForward = targetPosInWorld - roverPosInWorld; + roverToTargetForward.normalize(); - // Eigen::Vector3d up = Eigen::Vector3d::UnitZ(); - // Eigen::Vector3d left = up.cross(roverToTargetForward); + Eigen::Vector3d up = Eigen::Vector3d::UnitZ(); + Eigen::Vector3d left = up.cross(roverToTargetForward); - // Eigen::Matrix3d roverToTargetMat; - // roverToTargetMat.col(0) = roverToTargetForward; - // roverToTargetMat.col(1) = up; - // roverToTargetMat.col(2) = left; + Eigen::Matrix3d roverToTargetMat; + roverToTargetMat.col(0) = roverToTargetForward; + roverToTargetMat.col(1) = up; + roverToTargetMat.col(2) = left; - // //SO3 Matrices for lie algebra - // SO3d roverToTargetSO3 = SE3Conversions::fromColumns(roverToTargetForward, left, up); - // SO3d roverSO3 = roverInWorld.asSO3(); + //SO3 Matrices for lie algebra + SO3d roverToTargetSO3 = SE3Conversions::fromColumns(roverToTargetForward, left, up); + SO3d roverSO3 = roverInWorld.asSO3(); - // Eigen::Vector3d distanceToTargetVector = targetPosInWorld - Eigen::Vector3d{roverInWorld.translation().x(), roverInWorld.translation().y(), roverInWorld.translation().z()}; + Eigen::Vector3d distanceToTargetVector = targetPosInWorld - Eigen::Vector3d{roverInWorld.translation().x(), roverInWorld.translation().y(), roverInWorld.translation().z()}; - // double distanceToTarget = std::abs(distanceToTargetVector.norm()); + double distanceToTarget = std::abs(distanceToTargetVector.norm()); - // manif::SO3Tangentd SO3tan = roverToTargetSO3 - roverSO3; // 3 x 1 matrix of angular velocities (x,y,z) + manif::SO3Tangentd SO3tan = roverToTargetSO3 - roverSO3; // 3 x 1 matrix of angular velocities (x,y,z) - // switch (mLoopState) { - // case RTRSTATE::turn1: { - // if (distanceToTarget < 0.5) mLoopState = RTRSTATE::turn2; + switch (mLoopState) { + case RTRSTATE::turn1: { + if (distanceToTarget < 0.5) mLoopState = RTRSTATE::turn2; - // double angle_rate = mAngleP * SO3tan.z(); - // angle_rate = (std::abs(angle_rate) > mAngleFloor) ? angle_rate : copysign(mAngleFloor, angle_rate); - - // ROS_INFO("w_z velocity %f", SO3tan.z()); - - // twist.angular.z = angle_rate; - - // if (std::abs(SO3tan.z()) < angular_thresh) { - // mLoopState = RTRSTATE::drive; - // twist.angular.z = 0; - // twist.linear.x = 0; - // ROS_INFO("Done spinning"); - // } - // // ROS_INFO("In state: turning to point..."); - // break; - // } - - // case RTRSTATE::drive: { - // if (std::abs(SO3tan.z()) > angular_thresh) { - // mLoopState = RTRSTATE::turn1; - // ROS_INFO("Rotation got off"); - // twist.linear.x = 0; - // } - // double driveRate = std::min(mLinearP * distanceToTarget, 1.0); - - // ROS_INFO("distance: %f", distanceToTarget); - - // twist.linear.x = driveRate; - - // ROS_INFO("distance to target %f", distanceToTarget); - - // if (std::abs(distanceToTarget) < linear_thresh) { - // mLoopState = RTRSTATE::turn2; - // twist.linear.x = 0; - // twist.angular.z = 0; - // } - // // ROS_INFO("In state: driving to point..."); - // break; - // } - - // case RTRSTATE::turn2: { - // double angle_rate = mAngleP * SO3tan.z(); - // angle_rate = (std::abs(angle_rate) > mAngleFloor) ? angle_rate : copysign(mAngleFloor, angle_rate); - // twist.angular.z = angle_rate; - // twist.linear.x = 0; - - - // if (std::abs(SO3tan.z()) < angular_thresh) { - // mLoopState = RTRSTATE::done; - // twist.angular.z = 0; - // twist.linear.x = 0; - // // ROS_INFO("Done turning to lander"); - // } - // // ROS_INFO("In state: turning to lander..."); - // break; - // } - - // case RTRSTATE::done: { - // twist.linear.x = 0; - // twist.angular.z = 0; - // break; - // mCloud = nullptr; - // } - // } - // // ROS_INFO("THE TWIST IS: Angular: %f, with linear %f,", twist.angular.z, twist.linear.x); - // ROS_INFO_STREAM(mLoopState); - // mTwistPub.publish(twist); - // if (mLoopState == RTRSTATE::done) { - // break; - // } - // } - // } + double angle_rate = mAngleP * SO3tan.z(); + angle_rate = (std::abs(angle_rate) > mAngleFloor) ? angle_rate : copysign(mAngleFloor, angle_rate); + + ROS_INFO("w_z velocity %f", SO3tan.z()); + + twist.angular.z = angle_rate; + + if (std::abs(SO3tan.z()) < angular_thresh) { + mLoopState = RTRSTATE::drive; + twist.angular.z = 0; + twist.linear.x = 0; + ROS_INFO("Done spinning"); + } + // ROS_INFO("In state: turning to point..."); + break; + } + + case RTRSTATE::drive: { + if (std::abs(SO3tan.z()) > angular_thresh) { + mLoopState = RTRSTATE::turn1; + ROS_INFO("Rotation got off"); + twist.linear.x = 0; + } + double driveRate = std::min(mLinearP * distanceToTarget, 1.0); + + ROS_INFO("distance: %f", distanceToTarget); + + twist.linear.x = driveRate; + + ROS_INFO("distance to target %f", distanceToTarget); + + if (std::abs(distanceToTarget) < linear_thresh) { + mLoopState = RTRSTATE::turn2; + twist.linear.x = 0; + twist.angular.z = 0; + } + // ROS_INFO("In state: driving to point..."); + break; + } + + case RTRSTATE::turn2: { + double angle_rate = mAngleP * SO3tan.z(); + angle_rate = (std::abs(angle_rate) > mAngleFloor) ? angle_rate : copysign(mAngleFloor, angle_rate); + twist.angular.z = angle_rate; + twist.linear.x = 0; + + + if (std::abs(SO3tan.z()) < angular_thresh) { + mLoopState = RTRSTATE::done; + twist.angular.z = 0; + twist.linear.x = 0; + // ROS_INFO("Done turning to lander"); + } + // ROS_INFO("In state: turning to lander..."); + break; + } + + case RTRSTATE::done: { + twist.linear.x = 0; + twist.angular.z = 0; + break; + mCloud = nullptr; + } + } + // ROS_INFO("THE TWIST IS: Angular: %f, with linear %f,", twist.angular.z, twist.linear.x); + ROS_INFO_STREAM(mLoopState); + mTwistPub.publish(twist); + if (mLoopState == RTRSTATE::done) { + break; + } + } + } + + + void LanderAlignNodelet::LandercreateSpline(double splineLength, int density){ + int num = static_cast(std::floor(splineLength) * density);//mNormalInWorldVector + Eigen::Vector3d densityVector = mNormalInWorldVector.value() / density; + + Eigen::Vector3d splinePoint = mPlaneLocationInWorldVector.value(); + for(int i = 0; i < num; i++){ + mPathPoints.push_back(splinePoint); + splinePoint = splinePoint + splinePoint; + } + } } // namespace mrover diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index 285b6fc22..3759c9dab 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -50,6 +50,8 @@ namespace mrover { std::vector mFilteredPoints; + std::vector mPathPoints; + //RTR RTR VARS RTRSTATE mLoopState; From 6e99fad9a1e75bdfb6cb77af74d48158dd5e62c7 Mon Sep 17 00:00:00 2001 From: MyCabbages4 Date: Sun, 31 Mar 2024 14:16:41 -0400 Subject: [PATCH 89/89] continued to start stuff --- src/teleoperation/lander_align/lander_align.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 25382ec2b..47b4759a4 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -461,13 +461,12 @@ namespace mrover { void LanderAlignNodelet::LandercreateSpline(double splineLength, int density){ - int num = static_cast(std::floor(splineLength) * density);//mNormalInWorldVector Eigen::Vector3d densityVector = mNormalInWorldVector.value() / density; Eigen::Vector3d splinePoint = mPlaneLocationInWorldVector.value(); - for(int i = 0; i < num; i++){ + while(splinePoint.norm() < splineLength){ mPathPoints.push_back(splinePoint); - splinePoint = splinePoint + splinePoint; + splinePoint = splinePoint + densityVector; } } } // namespace mrover