test: add unit tests for lcmt generators

Author: Meow404

.bazelrc

@@ -51,4 +51,6 @@ build --action_env=LD_LIBRARY_PATH=
 # use python3 by default
 build --python_path=python3
 
-build --define=WITH_GUROBI=ON
+build --define=WITH_GUROBI=ON
+
+build --local_resources=cpu=8

bindings/pyc3/c3_systems_py.cc

@@ -248,9 +248,9 @@ PYBIND11_MODULE(systems, m) {
       .def("get_input_port_c3_solution",
            &lcmt_generators::ContactForceGenerator::get_input_port_c3_solution,
            py::return_value_policy::reference)
-      .def("get_input_port_lcs_contact_info",
+      .def("get_input_port_lcs_contact_descriptions",
            &lcmt_generators::ContactForceGenerator::
-               get_input_port_lcs_contact_info,
+               get_input_port_lcs_contact_descriptions,
            py::return_value_policy::reference)
       .def("get_output_port_contact_force",
            &lcmt_generators::ContactForceGenerator::

core/BUILD.bazel

@@ -82,6 +82,9 @@ cc_library(
     hdrs = [
         "test/c3_cartpole_problem.hpp",
     ],
+    deps = [
+        ":core",
+    ],
     data = [
         ":test_data",
     ]

core/test/core_test.cc

@@ -40,6 +40,7 @@ using namespace c3;
  * | Solve                  |    -    |
  * | SetOsqpSolverOptions   |    -    |
  * | CreatePlaceholderLCS   |   DONE  |
+ * | WarmStartSmokeTest     |   DONE  |
  * | # of regression tests  |    2    |
  *
  * It also has an E2E test for ensuring the "Solve()" function and other
@@ -342,6 +343,16 @@ TEST_F(C3CartpoleTest, CreatePlaceholder) {
   ASSERT_TRUE(placeholder.HasSameDimensionsAs(*pSystem));
 }
 
+// Test if the solver works with warm start enabled (smoke test)
+TEST_F(C3CartpoleTest, WarmStartSmokeTest) {
+  // Enable warm start option
+  options.warm_start = true;
+  C3MIQP optimizer(*pSystem, cost, xdesired, options);
+
+  // Solver should not throw when called with warm start
+  ASSERT_NO_THROW(optimizer.Solve(x0));
+}
+
 template <typename T>
 class C3CartpoleTypedTest : public testing::Test, public C3CartpoleProblem {
  protected:

systems/BUILD.bazel

@@ -153,6 +153,18 @@ cc_test(
     ],
 )
 
+cc_test(
+    name = "generators_test",
+    srcs = [
+        "test/generators_test.cc"
+    ],
+    deps = [
+        ":lcmt_generators",
+        "//core:c3_cartpole_problem",
+        "@gtest//:main",
+    ],
+)
+
 filegroup(
     name = "headers",
     srcs = glob([

systems/lcmt_generators/contact_force_generator.cc

@@ -87,15 +87,15 @@ void ContactForceGenerator::DoCalc(const Context<double>& context,
 LcmPublisherSystem* ContactForceGenerator::AddLcmPublisherToBuilder(
     DiagramBuilder<double>& builder,
     const drake::systems::OutputPort<double>& solution_port,
-    const drake::systems::OutputPort<double>& lcs_contact_info_port,
+    const drake::systems::OutputPort<double>& lcs_contact_descriptions_port,
     const std::string& channel, drake::lcm::DrakeLcmInterface* lcm,
     const drake::systems::TriggerTypeSet& publish_triggers,
     double publish_period, double publish_offset) {
   // Add and connect the ContactForceGenerator system.
   auto force_publisher = builder.AddSystem<ContactForceGenerator>();
   builder.Connect(solution_port, force_publisher->get_input_port_c3_solution());
-  builder.Connect(lcs_contact_info_port,
-                  force_publisher->get_input_port_lcs_contact_info());
+  builder.Connect(lcs_contact_descriptions_port,
+                  force_publisher->get_input_port_lcs_contact_descriptions());
 
   // Add and connect the LCM publisher system.
   auto lcm_force_publisher =

systems/lcmt_generators/contact_force_generator.h

@@ -48,8 +48,8 @@ class ContactForceGenerator : public drake::systems::LeafSystem<double> {
    * @brief Returns the input port for the LCS contact information.
    * @return Reference to the input port for LCS contact info.
    */
-  const drake::systems::InputPort<double>& get_input_port_lcs_contact_info()
-      const {
+  const drake::systems::InputPort<double>&
+  get_input_port_lcs_contact_descriptions() const {
     return this->get_input_port(lcs_contact_info_port_);
   }
 
@@ -82,7 +82,7 @@ class ContactForceGenerator : public drake::systems::LeafSystem<double> {
   static drake::systems::lcm::LcmPublisherSystem* AddLcmPublisherToBuilder(
       drake::systems::DiagramBuilder<double>& builder,
       const drake::systems::OutputPort<double>& solution_port,
-      const drake::systems::OutputPort<double>& lcs_contact_info_port,
+      const drake::systems::OutputPort<double>& lcs_contact_descriptions_port,
       const std::string& channel, drake::lcm::DrakeLcmInterface* lcm,
       const drake::systems::TriggerTypeSet& publish_triggers,
       double publish_period = 0.0, double publish_offset = 0.0);