diff --git a/include/RobotManager.h b/include/RobotManager.h index 8447b1e..2eea743 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -47,7 +47,8 @@ class RobotManager { void bindMujoco(MujocoContext* mujContext); std::shared_ptr create(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& pos, - const Eigen::Vector3d& ori, const std::string& colorName, const std::shared_ptr team); + const Eigen::Vector3d& ori, const std::string& colorName, const std::shared_ptr team, + const std::string& role = "Striker"); void startContainers(const std::string& fwkCfgPath = spqr::frameworkConfigPath, const std::string& pathsCfgPath = spqr::pathsConfigPath); diff --git a/include/frontend/tools_panel/ToolsPanelGrid.h b/include/frontend/tools_panel/ToolsPanelGrid.h index c50f49d..3c47740 100644 --- a/include/frontend/tools_panel/ToolsPanelGrid.h +++ b/include/frontend/tools_panel/ToolsPanelGrid.h @@ -485,7 +485,7 @@ class ToolsPanelGrid : public QWidget { auto it = sensors.find("joints"); if (it != sensors.end()) { Sensor* jointsSensor = it->second; - Eigen::Vector3d position = dynamic_cast(jointsSensor)->getPosition(); + Eigen::VectorXd position = dynamic_cast(jointsSensor)->getPosition(); plot->addDataPoint("head_yaw", position(0), simTime); plot->addDataPoint("head_pitch", position(1), simTime); plot->addDataPoint("shoulder_left_pitch", position(2), simTime); @@ -514,7 +514,7 @@ class ToolsPanelGrid : public QWidget { auto it = sensors.find("joints"); if (it != sensors.end()) { Sensor* jointsSensor = it->second; - Eigen::Vector3d velocity = dynamic_cast(jointsSensor)->getVelocity(); + Eigen::VectorXd velocity = dynamic_cast(jointsSensor)->getVelocity(); plot->addDataPoint("head_yaw", velocity(0), simTime); plot->addDataPoint("head_pitch", velocity(1), simTime); plot->addDataPoint("shoulder_left_pitch", velocity(2), simTime); @@ -543,7 +543,7 @@ class ToolsPanelGrid : public QWidget { auto it = sensors.find("joints"); if (it != sensors.end()) { Sensor* jointsSensor = it->second; - Eigen::Vector3d acceleration = dynamic_cast(jointsSensor)->getAcceleration(); + Eigen::VectorXd acceleration = dynamic_cast(jointsSensor)->getAcceleration(); plot->addDataPoint("head_yaw", acceleration(0), simTime); plot->addDataPoint("head_pitch", acceleration(1), simTime); plot->addDataPoint("shoulder_left_pitch", acceleration(2), simTime); @@ -572,7 +572,7 @@ class ToolsPanelGrid : public QWidget { auto it = sensors.find("joints"); if (it != sensors.end()) { Sensor* jointsSensor = it->second; - Eigen::Vector3d torque = dynamic_cast(jointsSensor)->getTorque(); + Eigen::VectorXd torque = dynamic_cast(jointsSensor)->getTorque(); plot->addDataPoint("head_yaw", torque(0), simTime); plot->addDataPoint("head_pitch", torque(1), simTime); plot->addDataPoint("shoulder_left_pitch", torque(2), simTime); @@ -655,9 +655,13 @@ class ToolsPanelGrid : public QWidget { int height = depthCamera->getHeight(); if (!depthData.empty() && width > 0 && height > 0) { + // Depth is uint16 millimetres; map the near 10 m to the + // full 8-bit range so the preview keeps usable contrast + constexpr uint32_t kPreviewRangeMm = 10000; std::vector depthImage(depthData.size()); for (size_t i = 0; i < depthData.size(); ++i) { - depthImage[i] = static_cast(depthData[i] / 256); // Scale 16-bit to 8-bit + const uint32_t scaled = static_cast(depthData[i]) * 255u / kPreviewRangeMm; + depthImage[i] = static_cast(scaled > 255u ? 255u : scaled); } imageTool->setImage(depthImage.data(), width, height, 1); } else { diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index 49ac809..e0917bc 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -19,7 +20,9 @@ #include "MujocoContext.h" #include "robots/Robot.h" #include "sensors/CameraDepth.h" +#include "sensors/CameraInfo.h" #include "sensors/CameraRGB.h" +#include "sensors/GroundRelativePosition.h" #include "sensors/ImageSharedMemoryWriter.h" #include "sensors/Imu.h" #include "sensors/Joint.h" @@ -34,11 +37,13 @@ class Team; // Forward declaration class BoosterT1 : public Robot { public: Pose* pose = nullptr; + GroundRelativePosition* headPose = nullptr; Imu* imu = nullptr; Joints* joints = nullptr; Oracle* oracle = nullptr; CameraRGB* rgbCamera; CameraDepth* depthCamera; + CameraInfo* rgbCameraInfo = nullptr; BoosterT1(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& initPosition, const Eigen::Vector3d& initOrientation, const std::string& colorName, const std::shared_ptr& team) @@ -72,6 +77,8 @@ class BoosterT1 : public Robot { void bindMujoco(MujocoContext* mujCtx) override { pose = new Pose(mujCtx->model, mujCtx->data, (name + "_position").c_str(), (name + "_orientation").c_str()); + headPose = new GroundRelativePosition(mujCtx->model, mujCtx->data, (name + "_head_rgb_cam_site").c_str(), + GroundRelativePosition::TargetType::Site, pose); imu = new Imu(mujCtx->model, mujCtx->data, (name + "_linear-acceleration").c_str(), (name + "_angular-velocity").c_str()); joints = new Joints(mujCtx->model, mujCtx->data, joint_map); @@ -103,12 +110,14 @@ class BoosterT1 : public Robot { // Use RGB viewpoint for simulated depth to provide aligned depth-to-color. // This avoids parallax between rgb_cam and depth_cam when unprojecting RGB detections. depthCamera = new CameraDepth(mujCtx, (name + "_rgb_cam").c_str()); + rgbCameraInfo = new CameraInfo(mujCtx->model, (name + "_rgb_cam").c_str()); // Configure the writer for the shared memory file const int width = rgbCamera->getWidth(); const int height = rgbCamera->getHeight(); rgb_writer_.configure(shmFilePath_("rgb"), width, height, 3); - depth_writer_.configure(shmFilePath_("depth"), width, height, 1); + // Depth is 16-bit (16UC1): two bytes per pixel, published without precision loss + depth_writer_.configure(shmFilePath_("depth"), width, height, 2); // Create Oracle with the pose and all robots oracle = new Oracle(mujCtx->model, mujCtx->data, name, pose); @@ -142,13 +151,15 @@ class BoosterT1 : public Robot { std::map msg; msg["robot_name"] = msgpack::object(name, buffer_zone_); msg["pose"] = pose->serialize(buffer_zone_); + msg["head_pose"] = headPose->serialize(buffer_zone_); msg["imu"] = imu->serialize(buffer_zone_); msg["joints"] = joints->serialize(buffer_zone_); msg["oracle"] = oracle->serialize(buffer_zone_); + msg["camera_info"] = rgbCameraInfo->serialize(buffer_zone_); // Write in the shared file the information rgb_writer_.write(rgbCamera->getImage()); - depth_writer_.write(depthCamera->getDepth8bit()); + depth_writer_.write(depthCamera->getDepth16UC1()); return msg; } @@ -156,10 +167,12 @@ class BoosterT1 : public Robot { std::map getSensors() override { std::map sensors; sensors["pose"] = pose; + sensors["head_pose"] = headPose; sensors["imu"] = imu; sensors["joints"] = joints; sensors["rgb_camera"] = rgbCamera; sensors["depth_camera"] = depthCamera; + sensors["camera_info"] = rgbCameraInfo; return sensors; } @@ -170,11 +183,13 @@ class BoosterT1 : public Robot { void update() override { pose->update(); + headPose->update(); imu->update(); joints->update(); oracle->update(); rgbCamera->update(); depthCamera->update(); + rgbCameraInfo->update(); } ~BoosterT1() = default; diff --git a/include/robots/Robot.h b/include/robots/Robot.h index 5fe0e91..306ee93 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -26,8 +26,16 @@ class Team; // Forward declaration class Robot { public: Robot(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& initPosition, - const Eigen::Vector3d& initOrientation, const std::string& colorName, const std::shared_ptr& team) - : name(name), type(type), number(number), initPosition(initPosition), initOrientation(initOrientation), colorName(colorName), team(team) { + const Eigen::Vector3d& initOrientation, const std::string& colorName, const std::shared_ptr& team, + const std::string& role = "Striker") + : name(name), + type(type), + number(number), + initPosition(initPosition), + initOrientation(initOrientation), + colorName(colorName), + team(team), + role(role) { if (colorName == "red") { color = {130, 36, 51}; } else if (colorName == "blue") { @@ -50,6 +58,7 @@ class Robot { Eigen::Vector3d initPosition; Eigen::Vector3d initOrientation; // Euler angles std::string colorName; + std::string role; std::tuple color; std::unique_ptr container; std::shared_ptr team; diff --git a/include/sensors/CameraDepth.h b/include/sensors/CameraDepth.h index 2f802c1..d561d06 100644 --- a/include/sensors/CameraDepth.h +++ b/include/sensors/CameraDepth.h @@ -88,7 +88,9 @@ class CameraDepth : public Sensor { const float extent = static_cast(mujContext->model->stat.extent); const float znear = static_cast(mujContext->model->vis.map.znear) * extent; const float zfar = static_cast(mujContext->model->vis.map.zfar) * extent; - constexpr float kDepthMaxMeters = 10.0f; // Keep in sync with SimBridge mono8 decoding. + // Depth is stored as uint16 millimetres, the ROS 16UC1 convention consumers + // expect (raw / 1000 = metres). Saturates at 65535 mm ~= 65 m. + constexpr float kMillimetresPerMetre = 1000.0f; float max_u16 = static_cast(std::numeric_limits::max()); // Resample offscreen depth to camera resolution and convert to metric depth. @@ -104,8 +106,8 @@ class CameraDepth : public Sensor { float z_converted = (znear * zfar) / (zfar - z_raw * (zfar - znear)); depthNormalized[dstRow + x] = z_converted; - float normalizedDepth = std::clamp(z_converted / kDepthMaxMeters, 0.0f, 1.0f); - depth[dstRow + x] = static_cast(normalizedDepth * max_u16); + const float depthMillimetres = std::clamp(z_converted * kMillimetresPerMetre, 0.0f, max_u16); + depth[dstRow + x] = static_cast(depthMillimetres); } } @@ -134,13 +136,14 @@ class CameraDepth : public Sensor { return depth; } - std::vector getDepth8bit() const { + std::vector getDepth16UC1() const { std::lock_guard lock(depthMutex_); - std::vector depth8(depth.size()); + std::vector bytes(depth.size() * sizeof(uint16_t)); for (size_t i = 0; i < depth.size(); ++i) { - depth8[i] = static_cast(depth[i] >> 8); // Convert uint16 to uint8 + bytes[i * 2 + 0] = static_cast(depth[i] & 0xFF); + bytes[i * 2 + 1] = static_cast((depth[i] >> 8) & 0xFF); } - return depth8; + return bytes; } int getWidth() const { diff --git a/include/sensors/CameraInfo.h b/include/sensors/CameraInfo.h new file mode 100644 index 0000000..7cee4b5 --- /dev/null +++ b/include/sensors/CameraInfo.h @@ -0,0 +1,85 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include "sensors/Sensor.h" + +namespace spqr { + +// Intrinsics parameters of MuJoCo camera. +// Values are derived once from cam_fovy/cam_resolution (ideal pinhole, no distortion, +// since MuJoCo's OpenGL renderer doesn't model lens distortion) and never change at runtime. +class CameraInfo : public Sensor { + public: + CameraInfo(mjModel* mujModel, const char* cameraName) { + int camId = mj_name2id(mujModel, mjOBJ_CAMERA, cameraName); + if (camId < 0) + throw std::runtime_error(std::string("Camera not found: ") + cameraName); + + width_ = mujModel->cam_resolution[2 * camId + 0]; + height_ = mujModel->cam_resolution[2 * camId + 1]; + + double fovyRad = mujModel->cam_fovy[camId] * M_PI / 180.0; + + fx_ = (height_ / 2.0) / std::tan(fovyRad / 2.0); + fy_ = fx_; + cx_ = width_ / 2.0; + cy_ = height_ / 2.0; + } + + void doUpdate() override {} + + msgpack::object doSerialize(msgpack::zone& z) override { + std::map data; + data["height"] = msgpack::object(height_, z); + data["width"] = msgpack::object(width_, z); + data["distortion_model"] = msgpack::object(std::string("plumb_bob"), z); + data["d"] = msgpack::object(std::vector{0.0, 0.0, 0.0, 0.0, 0.0}, z); + data["k"] = msgpack::object(std::vector{fx_, 0.0, cx_, 0.0, fy_, cy_, 0.0, 0.0, 1.0}, z); + data["r"] = msgpack::object(std::vector{1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}, z); + data["p"] = msgpack::object(std::vector{fx_, 0.0, cx_, 0.0, 0.0, fy_, cy_, 0.0, 0.0, 0.0, 1.0, 0.0}, z); + data["binning_x"] = msgpack::object(0, z); + data["binning_y"] = msgpack::object(0, z); + + std::map roi; + roi["x_offset"] = msgpack::object(0, z); + roi["y_offset"] = msgpack::object(0, z); + roi["height"] = msgpack::object(0, z); + roi["width"] = msgpack::object(0, z); + roi["do_rectify"] = msgpack::object(false, z); + data["roi"] = msgpack::object(roi, z); + + return msgpack::object(data, z); + } + + int getWidth() const { + return width_; + } + int getHeight() const { + return height_; + } + double getFx() const { + return fx_; + } + double getFy() const { + return fy_; + } + double getCx() const { + return cx_; + } + double getCy() const { + return cy_; + } + + private: + int width_, height_; + double fx_, fy_, cx_, cy_; +}; + +} // namespace spqr diff --git a/include/sensors/GroundRelativePosition.h b/include/sensors/GroundRelativePosition.h new file mode 100644 index 0000000..466116d --- /dev/null +++ b/include/sensors/GroundRelativePosition.h @@ -0,0 +1,67 @@ +#pragma once + +#include + +#include +#include + +#include "sensors/Pose.h" +#include "sensors/Sensor.h" + +namespace spqr { + +// Position of a body or site relative to the ground projection of a reference Pose. +// Reference frame: (trunk_x, trunk_y, 0) with yaw-only rotation of the trunk. +class GroundRelativePosition : public Sensor { + public: + enum class TargetType { Body, Site }; + + GroundRelativePosition(mjModel* mujModel, mjData* mujData, const char* name, TargetType type, Pose* referencePose) + : mujData_(mujData), type_(type), referencePose_(referencePose) { + if (type_ == TargetType::Site) + targetId_ = mj_name2id(mujModel, mjOBJ_SITE, name); + else + targetId_ = mj_name2id(mujModel, mjOBJ_BODY, name); + } + + void doUpdate() override { + Eigen::Vector3d targetPos; + if (type_ == TargetType::Site) + targetPos = Eigen::Vector3d(mujData_->site_xpos[3 * targetId_], mujData_->site_xpos[3 * targetId_ + 1], + mujData_->site_xpos[3 * targetId_ + 2]); + else + targetPos = Eigen::Vector3d(mujData_->xpos[3 * targetId_], mujData_->xpos[3 * targetId_ + 1], mujData_->xpos[3 * targetId_ + 2]); + + Eigen::Vector4d refQuatVec = referencePose_->getQuatOrientation(); + Eigen::Quaterniond refQuat(refQuatVec(0), refQuatVec(1), refQuatVec(2), refQuatVec(3)); + Eigen::Vector3d refPos = referencePose_->getPosition(); + + double w = refQuat.w(), x = refQuat.x(), y = refQuat.y(), z = refQuat.z(); + double yaw = std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)); + Eigen::Quaterniond groundQuat(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); + Eigen::Vector3d groundPos(refPos.x(), refPos.y(), 0.0); + + position_ = groundQuat.conjugate() * (targetPos - groundPos); + } + + msgpack::object doSerialize(msgpack::zone& z) override { + std::vector position_vec = {position_(0), position_(1), position_(2)}; + std::map data; + data["position"] = msgpack::object(position_vec, z); + return msgpack::object(data, z); + } + + Eigen::Vector3d getPosition() const { + return position_; + } + + private: + mjData* mujData_; + int targetId_; + TargetType type_; + Pose* referencePose_; + + Eigen::Vector3d position_; +}; + +} // namespace spqr diff --git a/resources/robots/Booster-T1/instance.xml b/resources/robots/Booster-T1/instance.xml index a429646..aa1b281 100644 --- a/resources/robots/Booster-T1/instance.xml +++ b/resources/robots/Booster-T1/instance.xml @@ -34,13 +34,15 @@ - - - - - - - + + + + + + + + + @@ -164,7 +166,7 @@ - + true diff --git a/src/Container.cpp b/src/Container.cpp index 852f69d..f5e0a4e 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -76,6 +76,7 @@ void Container::create(const std::shared_ptr& robot, const std::string& i "CIRCUS_PORT=" + std::to_string(frameworkCommunicationPort), "TEAM_NUMBER=" + std::to_string(robot->team->number), "PLAYER_NUMBER=" + std::to_string(robot->number), + "PLAYER_ROLE=" + robot->role, "TEAM_COLOR=" + robot->colorName, "DISPLAY=" + envOrDefault("DISPLAY", ":0"), "QT_X11_NO_MITSHM=1", diff --git a/src/FieldGenerator.cpp b/src/FieldGenerator.cpp index a264ab5..dd9fbfa 100644 --- a/src/FieldGenerator.cpp +++ b/src/FieldGenerator.cpp @@ -150,7 +150,7 @@ void FieldGenerator::addFieldGeometries(pugi::xml_node& worldbodyNode, const Fie addCenterCircle(worldbodyNode, fieldConfig); // Add goals - float halfWidth = fieldConfig.width / 2.0f; + float halfWidth = fieldConfig.width / 2.0f - fieldConfig.line_width / 2.0f; addGoal(worldbodyNode, fieldConfig, "left_goal", -halfWidth, 0.0f); addGoal(worldbodyNode, fieldConfig, "right_goal", halfWidth, static_cast(M_PI)); } @@ -192,8 +192,10 @@ void FieldGenerator::addGroundPlane(pugi::xml_node& worldbodyNode, const FieldCo std::vector FieldGenerator::calculateFieldLines(const FieldConfig& fieldConfig) { std::vector lines; - float halfWidth = fieldConfig.width / 2.0f; - float halfHeight = fieldConfig.height / 2.0f; + // Boundary line centers are inset by half the line width so their outer + // edges sit exactly at ±width/2 and ±height/2 (the declared field dimensions). + float halfWidth = fieldConfig.width / 2.0f - fieldConfig.line_width / 2.0f; + float halfHeight = fieldConfig.height / 2.0f - fieldConfig.line_width / 2.0f; float z = 0.004f; // Slightly above ground float overlap = fieldConfig.line_width / 2.0f; diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 8e72bd2..1c14959 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -53,10 +53,15 @@ void RobotManager::bindMujoco(MujocoContext* mujContext) { } std::shared_ptr RobotManager::create(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& pos, - const Eigen::Vector3d& ori, const std::string& colorName, const std::shared_ptr team) { + const Eigen::Vector3d& ori, const std::string& colorName, const std::shared_ptr team, + const std::string& role) { auto it = robotFactory.find(type); - if (it != robotFactory.end()) - return it->second(name, type, number, pos, ori, colorName, team); + if (it != robotFactory.end()) { + auto robot = it->second(name, type, number, pos, ori, colorName, team); + if (robot) + robot->role = role; + return robot; + } return nullptr; } diff --git a/src/SceneParser.cpp b/src/SceneParser.cpp index 0db7eb7..aa76f8d 100644 --- a/src/SceneParser.cpp +++ b/src/SceneParser.cpp @@ -28,8 +28,20 @@ SceneParser::SceneParser(const string& yamlPath) { if (!sceneRoot["simulation_config"]) throw runtime_error("Scene missing 'simulation_config' entry."); + // Simulation configs are owned by maximus (src/app/sim/simulation_configs). Resolve them from + // the framework tree (FRAMEWORK_PATH / PIXI_PROJECT_ROOT) so circus and maximus stay in sync, + // falling back to circus' own resources for standalone use. string simConfigName = sceneRoot["simulation_config"].as(); - filesystem::path simConfigPath = filesystem::path(PROJECT_ROOT) / "resources" / "config" / "simulation_configs" / (simConfigName + ".yaml"); + const string simConfigFileName = simConfigName + ".yaml"; + filesystem::path simConfigPath; + const char* fwkRoot = std::getenv("FRAMEWORK_PATH") ? std::getenv("FRAMEWORK_PATH") : std::getenv("PIXI_PROJECT_ROOT"); + if (fwkRoot) { + filesystem::path maximusSimConfigPath = filesystem::path(fwkRoot) / "src" / "app" / "sim" / "simulation_configs" / simConfigFileName; + if (filesystem::exists(maximusSimConfigPath)) + simConfigPath = maximusSimConfigPath; + } + if (simConfigPath.empty()) + simConfigPath = filesystem::path(PROJECT_ROOT) / "resources" / "config" / "simulation_configs" / simConfigFileName; if (!filesystem::exists(simConfigPath)) throw runtime_error("Simulation config file does not exist: " + simConfigPath.string()); @@ -60,7 +72,18 @@ SceneParser::SceneParser(const string& yamlPath) { scene.simulationConfig.game.penalty_duration = gameNode["penalty_duration"].as(45); } - filesystem::path fieldPath = filesystem::path(PROJECT_ROOT) / "resources" / "config" / "fields" / (scene.simulationConfig.game.field + ".yaml"); + // Field configs are owned by maximus (src/app/config/fields). Resolve them from the + // framework tree (FRAMEWORK_PATH / PIXI_PROJECT_ROOT) so circus and maximus stay in sync, + // falling back to circus' own resources for standalone use. + const string fieldFileName = scene.simulationConfig.game.field + ".yaml"; + filesystem::path fieldPath; + if (fwkRoot) { + filesystem::path maximusFieldPath = filesystem::path(fwkRoot) / "src" / "app" / "config" / "fields" / fieldFileName; + if (filesystem::exists(maximusFieldPath)) + fieldPath = maximusFieldPath; + } + if (fieldPath.empty()) + fieldPath = filesystem::path(PROJECT_ROOT) / "resources" / "config" / "fields" / fieldFileName; if (!filesystem::exists(fieldPath)) throw runtime_error("Field config file does not exist: " + fieldPath.string()); @@ -109,6 +132,11 @@ SceneParser::SceneParser(const string& yamlPath) { string robotType = robotNode["type"].as(); // complete name - uint8_t robotNumber = robotNode["number"].as(); string robotName = robotNode["name"] ? robotNode["name"].as() : teamName + "_" + robotType + "_" + to_string(typeIndex++); + // Role is taken from the scene if present; otherwise fall back to a number-based default. + string robotRole = robotNode["role"] ? robotNode["role"].as() : + (robotNumber == 1) ? "Goalkeeper" : + (robotNumber == 2) ? "Defender" : + "Striker"; Vector3d pos = Vector3d::Zero(); Vector3d ori = Vector3d::Zero(); @@ -122,7 +150,7 @@ SceneParser::SceneParser(const string& yamlPath) { ori[i] = robotNode["orientation"][i].as(); } - shared_ptr robot = RobotManager::instance().create(robotName, robotType, robotNumber, pos, ori, teamName, teamSpec); + shared_ptr robot = RobotManager::instance().create(robotName, robotType, robotNumber, pos, ori, teamName, teamSpec, robotRole); robotTypes.insert(robotType); teamSpec->robots.push_back(std::move(robot));