Merge changes Ib1ee3415,I195bdac6,I106f1389,I422e3438,I9d840b65
* changes:
Fix arm UI spline editing
Add limit checks to arm UI
Make arm UI angle conventions match C++
Add roll joint to superstructure and arm UI
Add double jointed arm trajectory with roll
diff --git a/aos/starter/starter.sh b/aos/starter/starter.sh
index d4281c5..face963 100755
--- a/aos/starter/starter.sh
+++ b/aos/starter/starter.sh
@@ -13,8 +13,7 @@
elif [[ "$(hostname)" == "pi-"* ]]; then
# We have systemd configured to handle restarting, so just exec.
export PATH="${PATH}:/home/pi/bin"
- rm -rf /dev/shm/aos
- exec starterd --user=pi
+ exec starterd --user=pi --purge_shm_base
else
ROBOT_CODE="${HOME}/bin"
fi
@@ -22,6 +21,5 @@
cd "${ROBOT_CODE}"
export PATH="${PATH}:${ROBOT_CODE}"
while true; do
- rm -rf /dev/shm/aos
- starterd 2>&1
+ starterd --purge_shm_base 2>&1
done
diff --git a/aos/starter/starterd.cc b/aos/starter/starterd.cc
index 54f1bb4..19117de 100644
--- a/aos/starter/starterd.cc
+++ b/aos/starter/starterd.cc
@@ -2,15 +2,25 @@
#include <sys/types.h>
#include "aos/init.h"
+#include "aos/starter/starterd_lib.h"
+#include "aos/util/file.h"
#include "gflags/gflags.h"
-#include "starterd_lib.h"
DEFINE_string(config, "aos_config.json", "File path of aos configuration");
DEFINE_string(user, "",
"Starter runs as though this user ran a SUID binary if set.");
+DECLARE_string(shm_base);
+DEFINE_bool(purge_shm_base, false,
+ "If true, delete everything in --shm_base before starting.");
+
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
+
+ if (FLAGS_purge_shm_base) {
+ aos::util::UnlinkRecursive(FLAGS_shm_base);
+ }
+
if (!FLAGS_user.empty()) {
uid_t uid;
uid_t gid;
diff --git a/frc971/rockpi/contents/etc/fstab b/frc971/rockpi/contents/etc/fstab
index cf62c9f..c148c2d 100644
--- a/frc971/rockpi/contents/etc/fstab
+++ b/frc971/rockpi/contents/etc/fstab
@@ -1,2 +1,2 @@
-/dev/mmcblk0p1 /boot auto defaults 0 2
/dev/mmcblk0p2 / auto errors=remount-ro 0 0
+tmpfs /dev/shm tmpfs rw,nosuid,nodev,size=90% 0 0
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 3186f44..839f11e 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -30,8 +30,11 @@
"//aos:init",
"//aos:json_to_flatbuffer",
"//aos/events:shm_event_loop",
+ "//frc971/constants:constants_sender_lib",
"//frc971/vision:vision_fbs",
"//third_party:opencv",
+ "//y2023/vision:april_debug_fbs",
+ "//y2023/vision:vision_util",
"@com_google_absl//absl/strings",
],
)
diff --git a/y2023/vision/viewer.cc b/y2023/vision/viewer.cc
index 68495b1..7877a57 100644
--- a/y2023/vision/viewer.cc
+++ b/y2023/vision/viewer.cc
@@ -6,7 +6,12 @@
#include "aos/init.h"
#include "aos/json_to_flatbuffer.h"
#include "aos/time/time.h"
+#include "frc971/constants/constants_sender_lib.h"
#include "frc971/vision/vision_generated.h"
+#include "opencv2/calib3d.hpp"
+#include "opencv2/imgproc.hpp"
+#include "y2023/vision/april_debug_generated.h"
+#include "y2023/vision/vision_util.h"
DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
DEFINE_string(channel, "/camera", "Channel name for the image.");
@@ -16,23 +21,32 @@
DEFINE_int32(rate, 100, "Time in milliseconds to wait between images");
-namespace frc971 {
+namespace y2023 {
namespace vision {
namespace {
-aos::Fetcher<CameraImage> image_fetcher;
-bool DisplayLoop() {
+using frc971::vision::CameraImage;
+
+bool DisplayLoop(const cv::Mat intrinsics, const cv::Mat dist_coeffs,
+ aos::Fetcher<CameraImage> *image_fetcher,
+ aos::Fetcher<AprilDebug> *april_debug_fetcher) {
const CameraImage *image;
+ std::optional<const AprilDebug *> april_debug = std::nullopt;
// Read next image
- if (!image_fetcher.Fetch()) {
+ if (!image_fetcher->Fetch()) {
VLOG(2) << "Couldn't fetch next image";
return true;
}
-
- image = image_fetcher.get();
+ image = image_fetcher->get();
CHECK(image != nullptr) << "Couldn't read image";
+ if (april_debug_fetcher->Fetch()) {
+ april_debug = april_debug_fetcher->get();
+ } else {
+ VLOG(2) << "Couldn't fetch next target map";
+ }
+
// Create color image:
cv::Mat image_color_mat(cv::Size(image->cols(), image->rows()), CV_8UC2,
(void *)image->data()->data());
@@ -41,7 +55,8 @@
if (!FLAGS_capture.empty()) {
if (absl::EndsWith(FLAGS_capture, ".bfbs")) {
- aos::WriteFlatbufferToFile(FLAGS_capture, image_fetcher.CopyFlatBuffer());
+ aos::WriteFlatbufferToFile(FLAGS_capture,
+ image_fetcher->CopyFlatBuffer());
} else {
cv::imwrite(FLAGS_capture, bgr_image);
}
@@ -49,7 +64,21 @@
return false;
}
- cv::imshow("Display", bgr_image);
+ cv::Mat undistorted_image;
+ cv::undistort(bgr_image, undistorted_image, intrinsics, dist_coeffs);
+
+ if (april_debug.has_value() && april_debug.value()->corners()->size() > 0) {
+ for (const auto *corners : *april_debug.value()->corners()) {
+ std::vector<cv::Point> points;
+ for (const auto *point_fbs : *corners->points()) {
+ points.emplace_back(point_fbs->x(), point_fbs->y());
+ }
+ cv::polylines(undistorted_image, points, true, cv::Scalar(255, 0, 0), 10);
+ }
+ }
+
+ cv::imshow("Display", undistorted_image);
+
int keystroke = cv::waitKey(1);
if ((keystroke & 0xFF) == static_cast<int>('c')) {
// Convert again, to get clean image
@@ -68,14 +97,26 @@
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig(FLAGS_config);
+ frc971::constants::WaitForConstants<Constants>(&config.message());
+
aos::ShmEventLoop event_loop(&config.message());
- image_fetcher = event_loop.MakeFetcher<CameraImage>(FLAGS_channel);
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher(&event_loop);
+ const auto *calibration_data = FindCameraCalibration(
+ constants_fetcher.constants(), event_loop.node()->name()->string_view());
+ const cv::Mat intrinsics = CameraIntrinsics(calibration_data);
+ const cv::Mat dist_coeffs = CameraDistCoeffs(calibration_data);
+
+ aos::Fetcher<CameraImage> image_fetcher =
+ event_loop.MakeFetcher<CameraImage>(FLAGS_channel);
+ aos::Fetcher<AprilDebug> april_debug_fetcher =
+ event_loop.MakeFetcher<AprilDebug>("/camera");
// Run the display loop
event_loop.AddPhasedLoop(
- [&event_loop](int) {
- if (!DisplayLoop()) {
+ [&](int) {
+ if (!DisplayLoop(intrinsics, dist_coeffs, &image_fetcher,
+ &april_debug_fetcher)) {
LOG(INFO) << "Calling event_loop Exit";
event_loop.Exit();
};
@@ -89,9 +130,9 @@
} // namespace
} // namespace vision
-} // namespace frc971
+} // namespace y2023
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
- frc971::vision::ViewerMain();
+ y2023::vision::ViewerMain();
}