Add SelectCommand implementation

- Fixes #14

Author: virtuald

commands2/__init__.py

@@ -27,6 +27,7 @@
     RepeatCommand,
     RunCommand,
     ScheduleCommand,
+    SelectCommand,
     SequentialCommandGroup,
     StartEndCommand,
     Subsystem,
@@ -54,6 +55,7 @@
     "CommandGroupBase",
     "CommandScheduler",
     "ConditionalCommand",
+    "SelectCommand",
     "FunctionalCommand",
     "InstantCommand",
     "MecanumControllerCommand",

commands2/src/SelectCommandKey.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#include <robotpy_build.h>
+
+// this assumes that the __hash__ of the python object isn't going to change
+// once added to the map of the SelectCommand. This is... probably reasonable?
+struct SelectCommandKey {
+
+  SelectCommandKey() = default;
+
+  SelectCommandKey &operator=(const py::handle src) {
+    py::gil_scoped_acquire gil;
+    m_v = py::reinterpret_borrow<py::object>(src);
+    m_hash = m_v.attr("__hash__")().cast<std::size_t>();
+    return *this;
+  }
+
+  operator py::object() const { return m_v; }
+
+  py::object m_v;
+  std::size_t m_hash;
+};
+
+inline bool operator==(const SelectCommandKey &lhs,
+                       const SelectCommandKey &rhs) {
+  py::gil_scoped_acquire gil;
+  return lhs.m_v.attr("__eq__")(rhs.m_v).cast<bool>();
+}
+
+template <> struct std::hash<SelectCommandKey> {
+  std::size_t operator()(const SelectCommandKey &s) const noexcept {
+    return s.m_hash;
+  }
+};
+
+namespace pybind11 {
+namespace detail {
+template <> struct type_caster<SelectCommandKey> {
+  PYBIND11_TYPE_CASTER(SelectCommandKey, const_name("object"));
+  bool load(handle src, bool) {
+    value = src;
+    return true;
+  }
+
+  static handle cast(const SelectCommandKey &src,
+                     return_value_policy /* policy */, handle /* parent */) {
+    return src.m_v;
+  }
+};
+} // namespace detail
+} // namespace pybind11

commands2/src/include/frc2/command/SelectCommand.h

@@ -74,7 +74,7 @@ class SelectCommand : public CommandBase {
       std::vector<std::pair<Key, std::shared_ptr<Command>>>&& commands)
       : m_selector{std::move(selector)} {
     for (auto&& command : commands) {
-      CommandScheduler::GetInstance().RequireUngrouped(command.second.get());
+      CommandScheduler::GetInstance().RequireUngrouped(command.second);
     }
 
     for (auto&& command : commands) {

gen/SelectCommand.yml

@@ -3,6 +3,7 @@
 extra_includes:
 - frc2/command/Command.h
 - frc2/command/Subsystem.h
+- src/SelectCommandKey.h
 
 functions:
   Initialize:
@@ -29,4 +30,4 @@ templates:
   SelectCommand:
     qualname: frc2::SelectCommand
     params:
-      - py::object
+      - SelectCommandKey

pyproject.toml

@@ -110,7 +110,7 @@ RepeatCommand = "frc2/command/RepeatCommand.h"
 RunCommand = "frc2/command/RunCommand.h"
 ScheduleCommand = "frc2/command/ScheduleCommand.h"
 # TODO
-# SelectCommand = "frc2/command/SelectCommand.h"
+SelectCommand = "frc2/command/SelectCommand.h"
 SequentialCommandGroup = "frc2/command/SequentialCommandGroup.h"
 # SetUtilities = "frc2/command/SetUtilities.h"
 StartEndCommand = "frc2/command/StartEndCommand.h"

tests/test_select_command.py

@@ -0,0 +1,20 @@
+import commands2
+from util import ConditionHolder
+
+
+def test_select_command(scheduler: commands2.CommandScheduler):
+    c = ConditionHolder()
+
+    def _assert_false():
+        assert False
+
+    cmd1 = commands2.RunCommand(_assert_false)
+    cmd2 = commands2.RunCommand(c.setTrue)
+    cmd3 = commands2.RunCommand(_assert_false)
+
+    sc = commands2.SelectCommand(lambda: 2, [(1, cmd1), (2, cmd2), (3, cmd3)])
+
+    scheduler.schedule(sc)
+    scheduler.run()
+
+    assert c.cond