Merge "Add pose error ratio filtering to localizer"
diff --git a/WORKSPACE b/WORKSPACE
index 698c066..f411682 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -769,9 +769,9 @@
deps = ["@//third_party/allwpilib/wpimath"],
)
""",
- sha256 = "decff0a28fa4a167696cc2e1122b6a5acd2fef01d3bfd356d8cad25bb487a191",
+ sha256 = "340a9c8e726e2eb365b7a40a722df05fe7c7072c5c4a617fa0218eb6d074ad9f",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/api-cpp/23.0.10/api-cpp-23.0.10-headers.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/api-cpp/23.0.11/api-cpp-23.0.11-headers.zip",
],
)
@@ -793,9 +793,9 @@
target_compatible_with = ['@//tools/platforms/hardware:roborio'],
)
""",
- sha256 = "00aea02c583d109056e2716e73b7d70e84d5c56a6daebd1dc9f4612c430894f8",
+ sha256 = "11f392bebfe54f512be9ef59809e1a10c4497e0ce92970645f054e7e04fe7ef6",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/api-cpp/23.0.10/api-cpp-23.0.10-linuxathena.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/api-cpp/23.0.11/api-cpp-23.0.11-linuxathena.zip",
],
)
@@ -808,9 +808,9 @@
hdrs = glob(['ctre/**/*.h', 'ctre/**/*.hpp']),
)
""",
- sha256 = "3d503df97b711c150c0b50238f644c528e55d5b82418c8e3970c79faa14b749c",
+ sha256 = "7585e1bd9e581dd745e7f040ab521b966b40a04d05bc7fa82d6dafe2fb65764e",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/tools/23.0.10/tools-23.0.10-headers.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/tools/23.0.11/tools-23.0.11-headers.zip",
],
)
@@ -832,9 +832,9 @@
target_compatible_with = ['@//tools/platforms/hardware:roborio'],
)
""",
- sha256 = "4ada1ed9e11c208da9e8a8e8a648a0fe426e6717121ebc2f1392ae3ddc7f2b8c",
+ sha256 = "b1daadfe782c43ed32c2e1a3956998f9604a3fc9282ef866fd8dc1482f3b8cc9",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/tools/23.0.10/tools-23.0.10-linuxathena.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/tools/23.0.11/tools-23.0.11-linuxathena.zip",
],
)
diff --git a/frc971/can_logger/BUILD b/frc971/can_logger/BUILD
new file mode 100644
index 0000000..a3b002a
--- /dev/null
+++ b/frc971/can_logger/BUILD
@@ -0,0 +1,39 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+cc_binary(
+ name = "can_logger",
+ srcs = [
+ "can_logger_main.cc",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":can_logger_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/time",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_library(
+ name = "can_logger_lib",
+ srcs = [
+ "can_logger.cc",
+ "can_logger.h",
+ ],
+ deps = [
+ ":can_logging_fbs",
+ "//aos/events:event_loop",
+ "//aos/scoped:scoped_fd",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "can_logging_fbs",
+ srcs = [
+ "can_logging.fbs",
+ ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
diff --git a/frc971/can_logger/can_logger.cc b/frc971/can_logger/can_logger.cc
new file mode 100644
index 0000000..6b4258a
--- /dev/null
+++ b/frc971/can_logger/can_logger.cc
@@ -0,0 +1,88 @@
+#include "frc971/can_logger/can_logger.h"
+
+namespace frc971 {
+namespace can_logger {
+
+CanLogger::CanLogger(aos::EventLoop *event_loop,
+ std::string_view interface_name)
+ : fd_(socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK, CAN_RAW)),
+ frames_sender_(event_loop->MakeSender<CanFrame>("/can")) {
+ struct ifreq ifr;
+ strcpy(ifr.ifr_name, interface_name.data());
+ PCHECK(ioctl(fd_.get(), SIOCGIFINDEX, &ifr) == 0)
+ << "Failed to get index for interface " << interface_name;
+
+ int enable_canfd = true;
+ PCHECK(setsockopt(fd_.get(), SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd,
+ sizeof(enable_canfd)) == 0)
+ << "Failed to enable CAN FD";
+
+ struct sockaddr_can addr;
+ addr.can_family = AF_CAN;
+ addr.can_ifindex = ifr.ifr_ifindex;
+
+ PCHECK(bind(fd_.get(), reinterpret_cast<struct sockaddr *>(&addr),
+ sizeof(addr)) == 0)
+ << "Failed to bind socket to interface " << interface_name;
+
+ int recieve_buffer_size;
+ socklen_t opt_size = sizeof(recieve_buffer_size);
+ PCHECK(getsockopt(fd_.get(), SOL_SOCKET, SO_RCVBUF, &recieve_buffer_size,
+ &opt_size) == 0);
+ CHECK_EQ(opt_size, sizeof(recieve_buffer_size));
+ VLOG(0) << "CAN recieve bufffer is " << recieve_buffer_size << " bytes large";
+
+ aos::TimerHandler *timer_handler = event_loop->AddTimer([this]() { Poll(); });
+ timer_handler->set_name("CAN logging Loop");
+ timer_handler->Setup(event_loop->monotonic_now(), kPollPeriod);
+}
+
+void CanLogger::Poll() {
+ VLOG(2) << "Polling";
+ int frames_read = 0;
+ while (ReadFrame()) {
+ frames_read++;
+ }
+ VLOG(1) << "Read " << frames_read << " frames to end of buffer";
+}
+
+bool CanLogger::ReadFrame() {
+ errno = 0;
+ struct canfd_frame frame;
+ ssize_t bytes_read = read(fd_.get(), &frame, sizeof(struct canfd_frame));
+
+ if (bytes_read < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)) {
+ // There are no more frames sitting in the recieve buffer.
+ return false;
+ }
+
+ VLOG(2) << "Read " << bytes_read << " bytes";
+ PCHECK(bytes_read > 0);
+ PCHECK(bytes_read == static_cast<ssize_t>(CAN_MTU) ||
+ bytes_read == static_cast<ssize_t>(CANFD_MTU))
+ << "Incomplete can frame";
+
+ struct timeval tv;
+ PCHECK(ioctl(fd_.get(), SIOCGSTAMP, &tv) == 0)
+ << "Failed to get timestamp of CAN frame";
+
+ aos::Sender<CanFrame>::Builder builder = frames_sender_.MakeBuilder();
+
+ auto frame_data = builder.fbb()->CreateVector<uint8_t>(frame.data, frame.len);
+
+ CanFrame::Builder can_frame_builder = builder.MakeBuilder<CanFrame>();
+ can_frame_builder.add_can_id(frame.can_id);
+ can_frame_builder.add_data(frame_data);
+ can_frame_builder.add_monotonic_timestamp_ns(
+ static_cast<std::chrono::nanoseconds>(
+ std::chrono::seconds(tv.tv_sec) +
+ std::chrono::microseconds(tv.tv_usec))
+ .count());
+
+ builder.CheckOk(builder.Send(can_frame_builder.Finish()));
+
+ return true;
+}
+
+} // namespace can_logger
+} // namespace frc971
diff --git a/frc971/can_logger/can_logger.h b/frc971/can_logger/can_logger.h
new file mode 100644
index 0000000..cf37841
--- /dev/null
+++ b/frc971/can_logger/can_logger.h
@@ -0,0 +1,50 @@
+#ifndef FRC971_CAN_LOGGER_CAN_LOGGER_H_
+#define FRC971_CAN_LOGGER_CAN_LOGGER_H_
+
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <linux/sockios.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <sys/time.h>
+
+#include <chrono>
+
+#include "aos/events/event_loop.h"
+#include "aos/realtime.h"
+#include "aos/scoped/scoped_fd.h"
+#include "frc971/can_logger/can_logging_generated.h"
+
+namespace frc971 {
+namespace can_logger {
+
+// This class listens to all the traffic on a SocketCAN interface and sends it
+// on the aos event loop so it can be logged with the aos logging
+// infrastructure.
+class CanLogger {
+ public:
+ static constexpr std::chrono::milliseconds kPollPeriod =
+ std::chrono::milliseconds(100);
+
+ CanLogger(aos::EventLoop *event_loop,
+ std::string_view interface_name = "can0");
+
+ CanLogger(const CanLogger &) = delete;
+ CanLogger &operator=(const CanLogger &) = delete;
+
+ private:
+ void Poll();
+
+ // Read a CAN frame from the socket and send it on the event loop
+ // Returns true if successful and false if the recieve buffer is empty.
+ bool ReadFrame();
+
+ aos::ScopedFD fd_;
+ aos::Sender<CanFrame> frames_sender_;
+};
+
+} // namespace can_logger
+} // namespace frc971
+
+#endif // FRC971_CAN_LOGGER_CAN_LOGGER_H_
diff --git a/frc971/can_logger/can_logger_main.cc b/frc971/can_logger/can_logger_main.cc
new file mode 100644
index 0000000..42c7162
--- /dev/null
+++ b/frc971/can_logger/can_logger_main.cc
@@ -0,0 +1,18 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/can_logger/can_logger.h"
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("aos_config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+
+ frc971::can_logger::CanLogger can_logger(&event_loop, "can0");
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/frc971/can_logger/can_logging.fbs b/frc971/can_logger/can_logging.fbs
new file mode 100644
index 0000000..d6ec8b9
--- /dev/null
+++ b/frc971/can_logger/can_logging.fbs
@@ -0,0 +1,13 @@
+namespace frc971.can_logger;
+
+// A message to represent a single CAN or CAN FD frame.
+table CanFrame {
+ // CAN id + flags
+ can_id:uint32 (id: 0);
+ // The body of the CAN frame up to 64 bytes long.
+ data:[ubyte] (id: 1);
+ // The hardware timestamp of when the frame came in
+ monotonic_timestamp_ns:uint64 (id: 2);
+}
+
+root_type CanFrame;
diff --git a/y2023/BUILD b/y2023/BUILD
index 095b856..08713b7 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -37,6 +37,7 @@
"//y2023/autonomous:binaries",
":joystick_reader",
":wpilib_interface",
+ "//frc971/can_logger",
"//aos/network:message_bridge_client",
"//aos/network:message_bridge_server",
"//y2023/control_loops/drivetrain:drivetrain",
@@ -211,6 +212,7 @@
"//y2023/control_loops/superstructure:superstructure_output_fbs",
"//y2023/control_loops/superstructure:superstructure_position_fbs",
"//y2023/control_loops/superstructure:superstructure_status_fbs",
+ "//frc971/can_logger:can_logging_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index cd52cfd..2f41c91 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -114,16 +114,12 @@
} // namespace
static constexpr int kCANFalconCount = 6;
-static constexpr int kCANSignalsCount = 4;
-static constexpr int kRollerSignalsCount = 4;
static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
class Falcon {
public:
Falcon(int device_id, std::string canbus,
- aos::SizedArray<ctre::phoenixpro::BaseStatusSignalValue *,
- kCANFalconCount * kCANSignalsCount +
- kRollerSignalsCount> *signals)
+ std::vector<ctre::phoenixpro::BaseStatusSignalValue *> *signals)
: talon_(device_id, canbus),
device_id_(device_id),
device_temp_(talon_.GetDeviceTemp()),
@@ -134,9 +130,7 @@
// device temp is not timesynced so don't add it to the list of signals
device_temp_.SetUpdateFrequency(kCANUpdateFreqHz);
- CHECK_EQ(kCANSignalsCount, 4);
CHECK_NOTNULL(signals);
- CHECK_LE(signals->size() + 4u, signals->capacity());
supply_voltage_.SetUpdateFrequency(kCANUpdateFreqHz);
signals->push_back(&supply_voltage_);
@@ -279,8 +273,11 @@
class CANSensorReader {
public:
- CANSensorReader(aos::EventLoop *event_loop)
+ CANSensorReader(
+ aos::EventLoop *event_loop,
+ std::vector<ctre::phoenixpro::BaseStatusSignalValue *> signals_registry)
: event_loop_(event_loop),
+ signals_(signals_registry.begin(), signals_registry.end()),
can_position_sender_(
event_loop->MakeSender<drivetrain::CANPosition>("/drivetrain")),
roller_falcon_data_(std::nullopt) {
@@ -294,12 +291,6 @@
});
}
- aos::SizedArray<ctre::phoenixpro::BaseStatusSignalValue *,
- kCANFalconCount * kCANSignalsCount + kRollerSignalsCount>
- *get_signals_registry() {
- return &signals_;
- };
-
void set_falcons(std::shared_ptr<Falcon> right_front,
std::shared_ptr<Falcon> right_back,
std::shared_ptr<Falcon> right_under,
@@ -323,16 +314,8 @@
private:
void Loop() {
- CHECK_EQ(signals_.size(), 28u);
ctre::phoenix::StatusCode status =
- ctre::phoenixpro::BaseStatusSignalValue::WaitForAll(
- 2000_ms, {signals_[0], signals_[1], signals_[2], signals_[3],
- signals_[4], signals_[5], signals_[6], signals_[7],
- signals_[8], signals_[9], signals_[10], signals_[11],
- signals_[12], signals_[13], signals_[14], signals_[15],
- signals_[16], signals_[17], signals_[18], signals_[19],
- signals_[20], signals_[21], signals_[22], signals_[23],
- signals_[24], signals_[25], signals_[26], signals_[27]});
+ ctre::phoenixpro::BaseStatusSignalValue::WaitForAll(2000_ms, signals_);
if (!status.IsOK()) {
AOS_LOG(ERROR, "Failed to read signals from falcons: %s: %s",
@@ -383,9 +366,7 @@
aos::EventLoop *event_loop_;
- aos::SizedArray<ctre::phoenixpro::BaseStatusSignalValue *,
- kCANFalconCount * kCANSignalsCount + kRollerSignalsCount>
- signals_;
+ const std::vector<ctre::phoenixpro::BaseStatusSignalValue *> signals_;
aos::Sender<drivetrain::CANPosition> can_position_sender_;
std::shared_ptr<Falcon> right_front_, right_back_, right_under_, left_front_,
@@ -934,25 +915,27 @@
std::shared_ptr<frc::DigitalOutput> superstructure_reading =
make_unique<frc::DigitalOutput>(25);
+ std::vector<ctre::phoenixpro::BaseStatusSignalValue *> signals_registry;
+ std::shared_ptr<Falcon> right_front =
+ std::make_shared<Falcon>(1, "Drivetrain Bus", &signals_registry);
+ std::shared_ptr<Falcon> right_back =
+ std::make_shared<Falcon>(2, "Drivetrain Bus", &signals_registry);
+ std::shared_ptr<Falcon> right_under =
+ std::make_shared<Falcon>(3, "Drivetrain Bus", &signals_registry);
+ std::shared_ptr<Falcon> left_front =
+ std::make_shared<Falcon>(4, "Drivetrain Bus", &signals_registry);
+ std::shared_ptr<Falcon> left_back =
+ std::make_shared<Falcon>(5, "Drivetrain Bus", &signals_registry);
+ std::shared_ptr<Falcon> left_under =
+ std::make_shared<Falcon>(6, "Drivetrain Bus", &signals_registry);
+ std::shared_ptr<Falcon> roller =
+ std::make_shared<Falcon>(13, "Drivetrain Bus", &signals_registry);
+
// Thread 3.
::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
can_sensor_reader_event_loop.set_name("CANSensorReader");
- CANSensorReader can_sensor_reader(&can_sensor_reader_event_loop);
-
- std::shared_ptr<Falcon> right_front = std::make_shared<Falcon>(
- 1, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> right_back = std::make_shared<Falcon>(
- 2, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> right_under = std::make_shared<Falcon>(
- 3, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> left_front = std::make_shared<Falcon>(
- 4, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> left_back = std::make_shared<Falcon>(
- 5, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> left_under = std::make_shared<Falcon>(
- 6, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> roller = std::make_shared<Falcon>(
- 13, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
+ CANSensorReader can_sensor_reader(&can_sensor_reader_event_loop,
+ std::move(signals_registry));
can_sensor_reader.set_falcons(right_front, right_back, right_under,
left_front, left_back, left_under, roller);
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index cf3a14e..452a570 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -268,6 +268,14 @@
"max_size": 448
},
{
+ "name": "/can",
+ "type": "frc971.can_logger.CanFrame",
+ "source_node": "roborio",
+ "frequency": 6000,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
"name": "/drivetrain",
"type": "y2023.control_loops.drivetrain.CANPosition",
"source_node": "roborio",
@@ -566,6 +574,13 @@
"nodes": [
"roborio"
]
+ },
+ {
+ "name": "can_logger",
+ "executable_name": "can_logger",
+ "nodes": [
+ "roborio"
+ ]
}
],
"maps": [