Robothon 2025 protocol

idf/taskboard/extra_ros_packages/robothon_taskboard_msgs/msg/SensorMeasurement.msg

@@ -3,6 +3,7 @@ uint8 SENSOR_MEASUREMENT_TYPE_BOOL = 1
 uint8 SENSOR_MEASUREMENT_TYPE_ANALOG = 2
 uint8 SENSOR_MEASUREMENT_TYPE_VECTOR3 = 3
 uint8 SENSOR_MEASUREMENT_TYPE_INTEGER = 4
+uint8 SENSOR_MEASUREMENT_TYPE_EMPTY = 5
 
 uint8 type
 

idf/taskboard/main/Kconfig.projbuild

@@ -6,7 +6,7 @@ menu "M5STACK controller"
 
 choice CONTROLLER_HARDWARE
     prompt "M5STACK hardware"
-    default M5STACK_STICKC_PLUS2
+    default M5STACK_CORE2
     config M5STACK_STICKC_PLUS2
         bool "M5STACK_STICKC_PLUS2"
     config M5STACK_CORE2

idf/taskboard/main/hal/NonVolatileStorage.hpp

@@ -106,7 +106,7 @@ struct NonVolatileStorage
         // Write CSV line: timestamp, task name, is_human
         fprintf(file, "%s,%lld,%d,%s,",
                 task.name().c_str(),
-                task.elapsed_time(),
+                task.total_task_time(),
                 is_human ? 1 : 0,
                 task.unique_id().c_str());
 
@@ -131,7 +131,7 @@ struct NonVolatileStorage
         fflush(file);
 
         ESP_LOGI(TAG, "Added new register: %s, %lld, %d",
-                task.name().c_str(), task.elapsed_time(), is_human ? 1 : 0);
+                task.name().c_str(), task.total_task_time(), is_human ? 1 : 0);
 
         fclose(file);
     }

idf/taskboard/main/hal/board/TaskBoardDriver_TBv2023.hpp

@@ -254,7 +254,7 @@ struct TaskBoardDriver_v1 :
         update();
 
         // Create default tasks
-        std::vector<const TaskStep*>* precondition_steps = new std::vector<const TaskStep*>
+        std::vector<const TaskStepBase*>* precondition_steps = new std::vector<const TaskStepBase*>
         {
             new TaskStepEqual(*get_sensor_by_name("FADER"), SensorMeasurement(0.0f), 0.1f),
             new TaskStepEqual(*get_sensor_by_name("DOOR_OPEN"), SensorMeasurement(false)),
@@ -272,7 +272,7 @@ struct TaskBoardDriver_v1 :
                 new TaskStepEqualToRandom(*get_sensor_by_name("FADER"), 0.05f);
         // random_fader_step->set_clue_timeout(*get_sensor_by_name("BLUE_BUTTON"), 3000);
 
-        std::vector<const TaskStep*>* main_steps = new std::vector<const TaskStep*>
+        std::vector<const TaskStepBase*>* main_steps = new std::vector<const TaskStepBase*>
         {
             new TaskStepEqual(*get_sensor_by_name("BLUE_BUTTON"), SensorMeasurement(true)),
             timed_fader_operation,

idf/taskboard/main/hal/board/TaskBoardDriver_TBv2025.hpp

@@ -14,6 +14,7 @@
 #include <task/TaskStepEqual.hpp>
 #include <task/TaskStepEqualToRandom.hpp>
 #include <task/TaskStepTraceShape.hpp>
+#include <task/TaskStepWaitRandom.hpp>
 #include <task/SimultaneousConditionTask.hpp>
 #include <task/SequentialTask.hpp>
 #include <util/Timing.hpp>
@@ -282,7 +283,7 @@ struct TaskBoardDriver_v1 :
         update();
 
         // Create default tasks
-        std::vector<const TaskStep*>* precondition_steps = new std::vector<const TaskStep*>
+        std::vector<const TaskStepBase*>* precondition_steps = new std::vector<const TaskStepBase*>
         {
             new TaskStepEqual(*get_sensor_by_name("FADER"), SensorMeasurement(0.0f), 0.1f),
             new TaskStepEqual(*get_sensor_by_name("DOOR_OPEN"), SensorMeasurement(false)),
@@ -300,13 +301,14 @@ struct TaskBoardDriver_v1 :
                 new TaskStepEqualToRandom(*get_sensor_by_name("FADER"), 0.05f);
         // random_fader_step->set_clue_timeout(*get_sensor_by_name("BLUE_BUTTON"), 3000);
 
-        std::vector<const TaskStep*>* main_steps = new std::vector<const TaskStep*>
+        std::vector<const TaskStepBase*>* main_steps = new std::vector<const TaskStepBase*>
         {
             new TaskStepEqual(*get_sensor_by_name("BLUE_BUTTON"), SensorMeasurement(true)),
             timed_fader_operation,
             new TaskStepTraceShape(*get_sensor_by_name("TOUCH_SCREEN_POSITION")),
             random_fader_step,
             new TaskStepEqual(*get_sensor_by_name("FADER_BLUE_BUTTON"), SensorMeasurement(0.2f), 0.05f),
+            new TaskStepWaitRandom("WAIT_FOR_BALL_RELEASE"),
             new TaskStepEqual(*get_sensor_by_name("DOOR_OPEN"), SensorMeasurement(true)),
             new TaskStepEqual(*get_sensor_by_name("PROBE_GOAL"), SensorMeasurement(true)),
             new TaskStepEqual(*get_sensor_by_name("ATTACHED_CABLE"), SensorMeasurement(true)),

idf/taskboard/main/microros/MicroROSTask.hpp

@@ -149,6 +149,6 @@ struct MicroROSTask :
 private:
 
     rclc_action_goal_handle_t const* const goal_handle_;    ///< Pointer to the micro-ROS action goal handle
-    std::vector<const TaskStep*> steps_;                    ///< List of task steps
+    std::vector<const TaskStepBase*> steps_;                ///< List of task steps
     bool steps_from_default_task_ = false;                  ///< Flag to indicate if steps are from default task
 };

idf/taskboard/main/microros/MicroROSTypes.hpp

@@ -116,14 +116,14 @@ struct MicroROSTypes
 
             for (size_t i = 0; i < task.total_steps(); i++)
             {
-                const ::TaskStep& step = task.step(i);
+                const ::TaskStepBase& step = task.step(i);
 
                 robothon_taskboard_msgs__msg__TaskStep& msg_step = microros_msg_.steps.data[i];
                 msg_step = {};
 
-                msg_step.sensor_name.data = (char*)step.sensor().name().c_str();
-                msg_step.sensor_name.size = step.sensor().name().size();
-                msg_step.sensor_name.capacity = step.sensor().name().size() + 1;
+                msg_step.sensor_name.data = (char*)step.name().c_str();
+                msg_step.sensor_name.size = step.name().size();
+                msg_step.sensor_name.capacity = step.name().size() + 1;
 
                 if (step.clue_trigger() != nullptr)
                 {
@@ -168,6 +168,8 @@ struct MicroROSTypes
                         msg_step.target.integer_value.size = 1;
                         msg_step.target.integer_value.capacity = 1;
                         break;
+                    case ::SensorMeasurement::Type::EMPTY:
+                        break;
                 }
 
                 microros_msg_.status.data[i] = task.step_done(i);
@@ -588,6 +590,8 @@ struct MicroROSTypes
                         sensor_data.value.integer_value.capacity = 1;
                         sensor_data.value.integer_value.data[0] = sensor_value.get_integer();
                         break;
+                    case ::SensorMeasurement::Type::EMPTY:
+                        break;
                 }
             }
 
@@ -617,6 +621,8 @@ struct MicroROSTypes
                     case ::SensorMeasurement::Type::INTEGER:
                         delete sensor_data.value.integer_value.data;
                         break;
+                    case ::SensorMeasurement::Type::EMPTY:
+                        break;
                 }
             }
 

idf/taskboard/main/network/JSONHandler.hpp

@@ -150,6 +150,8 @@ struct JSONHandler
                         cJSON_AddItemToObject(sensor, "is_integer", is_integer);
                         break;
                     }
+                    case SensorMeasurement::Type::EMPTY:
+                        break;
                 }
 
                 cJSON_AddItemToArray(sensors_, sensor);
@@ -199,6 +201,8 @@ struct JSONHandler
                         cJSON_AddNumberToObject(root_, sensor_dev->name().c_str(), value);
                         break;
                     }
+                    case SensorMeasurement::Type::EMPTY:
+                        break;
                 }
             }
         }
@@ -246,7 +250,7 @@ struct JSONHandler
         for (size_t i = 0; i < task.total_steps(); i++)
         {
             cJSON* step = cJSON_CreateObject();
-            cJSON_AddStringToObject(step, "sensor", task.step(i).sensor().name().c_str());
+            cJSON_AddStringToObject(step, "sensor", task.step(i).name().c_str());
             cJSON* done = task.step_done(i) ? cJSON_CreateTrue() : cJSON_CreateFalse();
             cJSON_AddItemToObject(step, "done", done);
 

idf/taskboard/main/sensor/SensorMeasurement.hpp

@@ -37,10 +37,11 @@ struct SensorMeasurement
      */
     enum class Type
     {
-        BOOLEAN,    ///< Boolean measurement type
-        ANALOG,     ///< Analog (float) measurement type
-        VECTOR3,    ///< 3D vector measurement type
-        INTEGER     ///< Integer measurement type
+        BOOLEAN = 1,    ///< Boolean measurement type
+        ANALOG  = 2,    ///< Analog (float) measurement type
+        VECTOR3 = 3,    ///< 3D vector measurement type
+        INTEGER = 4,    ///< Integer measurement type
+        EMPTY   = 5     ///< Empty measurement type
     };
 
     /**
@@ -91,6 +92,14 @@ struct SensorMeasurement
     {
     }
 
+    /**
+     * @brief Constructs an empty SensorMeasurement
+     */
+    SensorMeasurement()
+        : type_(Type::EMPTY)
+    {
+    }
+
     /**
      * @brief Gets the type of measurement stored
      * @return Type enum indicating measurement type
@@ -121,6 +130,9 @@ struct SensorMeasurement
             case Type::INTEGER:
                 return std::to_string(integer_value);
                 break;
+            case Type::EMPTY:
+                return "";
+                break;
         }
 
         return "unknown";
@@ -160,6 +172,9 @@ struct SensorMeasurement
                 case Type::INTEGER:
                     ret = a.integer_value == b.integer_value;
                     break;
+                case Type::EMPTY:
+                    ret = true;
+                    break;
             }
         }
 
@@ -198,6 +213,9 @@ struct SensorMeasurement
                 case Type::INTEGER:
                     ret = a.integer_value >= b.integer_value;
                     break;
+                case Type::EMPTY:
+                    ret = true;
+                    break;
             }
         }
 

idf/taskboard/main/task/ParallelTask.hpp

@@ -24,7 +24,7 @@ struct ParallelTask :
      * @param task_name Name identifier for the task
      */
     ParallelTask(
-            const std::vector<const TaskStep*>& steps,
+            const std::vector<const TaskStepBase*>& steps,
             const std::string& task_name = "")
         : Task(steps, task_name, false)
     {

idf/taskboard/main/task/SequentialTask.hpp

@@ -16,6 +16,7 @@
 struct SequentialTask :
     public Task
 {
+    const char* TAG = "SequentialTask";                    ///< Logging tag
     /**
      * @brief Constructs a new SequentialTask object
      *
@@ -24,7 +25,7 @@ struct SequentialTask :
      * @param first_task_init_timer If true, timer starts only after first step completion
      */
     SequentialTask(
-            const std::vector<const TaskStep*>& steps,
+            const std::vector<const TaskStepBase*>& steps,
             const std::string& task_name = "",
             bool first_task_init_timer = false)
         : Task(steps, task_name, first_task_init_timer)
@@ -60,6 +61,8 @@ struct SequentialTask :
         {
             steps_score_.resize(steps_.size(), -1.0f);
             steps_finish_time_.resize(steps_.size(), -1);
+            steps_completion_time_.resize(steps_.size(), -1);
+            steps_time_sum_ = 0;
         }
 
         if (current_step_ < steps_.size())
@@ -68,12 +71,26 @@ struct SequentialTask :
             {
                 steps_score_[current_step_] = steps_[current_step_]->score();
                 steps_finish_time_[current_step_] = elapsed_time();
+                if (current_step_ == 0)
+                {
+                    steps_completion_time_[current_step_] = steps_finish_time_[current_step_];
+                }
+                else
+                {
+                    steps_completion_time_[current_step_] = steps_finish_time_[current_step_] - steps_finish_time_[current_step_ - 1];
+                }
+
+                if (!steps_[current_step_]->is_auto())
+                {
+                    steps_time_sum_ += steps_completion_time_[current_step_];
+                }
+
                 current_step_++;
 
                 // Restart status of the next step
                 if (current_step_ < steps_.size())
                 {
-                    steps_[current_step_]->sensor().start_read();
+                    steps_[current_step_]->restart_step();
                 }
 
                 ret = true;
@@ -112,12 +129,18 @@ struct SequentialTask :
     int64_t step_done_time(
             size_t step) const override
     {
-        if (step >= steps_finish_time_.size())
+        if (step >= steps_completion_time_.size())
         {
             return -1;
         }
 
-        return steps_finish_time_[step];
+        return steps_completion_time_[step];
+    }
+
+    /// Virtual method implementation
+    int64_t total_task_time() const
+    {
+        return steps_time_sum_;
     }
 
     /// Virtual method implementation
@@ -129,7 +152,9 @@ struct SequentialTask :
 
 private:
 
-    size_t current_step_ = 0;                   ///< Index of current step being executed
-    std::vector<float> steps_score_;         ///< Score for each step
-    std::vector<int64_t> steps_finish_time_;    ///< Completion status for each step
+    size_t current_step_ = 0;                       ///< Index of current step being executed
+    std::vector<float> steps_score_;                ///< Score for each step
+    std::vector<int64_t> steps_finish_time_;        ///< Completion status for each step
+    std::vector<int64_t> steps_completion_time_;    ///< Completion time for each step
+    int64_t steps_time_sum_ = 0;                    ///< Calculated task total time
 };

idf/taskboard/main/task/SimultaneousConditionTask.hpp

@@ -27,7 +27,7 @@ struct SimultaneousConditionTask :
      * @param task_name Name identifier for the task
      */
     SimultaneousConditionTask(
-            const std::vector<const TaskStep*>& steps,
+            const std::vector<const TaskStepBase*>& steps,
             const std::string& task_name = "")
         : ParallelTask(steps, task_name)
     {
@@ -64,4 +64,10 @@ struct SimultaneousConditionTask :
         return ret;
     }
 
+    /// Virtual method implementation
+    int64_t total_task_time() const override
+    {
+        return elapsed_time();
+    }
+
 };

idf/taskboard/main/task/Task.hpp

@@ -34,7 +34,7 @@ struct Task
      * @param first_task_init_timer If true, timer starts only after first step completion
      */
     Task(
-            const std::vector<const TaskStep*>& steps,
+            const std::vector<const TaskStepBase*>& steps,
             const std::string& task_name = "",
             bool first_task_init_timer = false)
         : steps_(steps)
@@ -110,16 +110,23 @@ struct Task
         previous_done_state_ = false;
 
         // Restart all sensor reads
-        for (const TaskStep* step : steps_)
+        for (const TaskStepBase* step : steps_)
         {
-            step->sensor().start_read();
+            step->restart_step();
         }
 
         // Regenerate unique ID
         uuid_generate(unique_id_);
         unique_id_str_ = uuid_to_string(unique_id_);
     }
 
+    /**
+     * @brief Gets the total time taken for the task without taking into account automatic steps
+     *
+     * @return Total time in microseconds
+     */
+    virtual int64_t total_task_time() const = 0;
+
     /**
      * @brief Gets the elapsed time since task start
      *
@@ -191,7 +198,7 @@ struct Task
      *
      * @return Const reference to the TaskStep object
      */
-    const TaskStep& step(
+    const TaskStepBase& step(
             size_t step) const
     {
         return *steps_[step];
@@ -235,7 +242,7 @@ struct Task
         task_name_ = task_name;
     }
 
-    const std::vector<const TaskStep*>& steps_;     ///< Sequence of steps in the task
+    const std::vector<const TaskStepBase*>& steps_;     ///< Sequence of steps in the task
 
 private: