Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions src/extlibs/er_force_sim/config/simulator/default.txt
Original file line number Diff line number Diff line change
Expand Up @@ -13,3 +13,4 @@ missing_ball_detections: 0
vision_delay: 0
vision_processing_time: 0
simulate_dribbling: false
command_delay: 0
1 change: 1 addition & 0 deletions src/extlibs/er_force_sim/config/simulator/realistic.txt
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ missing_ball_detections: 0.02
vision_delay: 35000000 # ns
vision_processing_time: 10000000 # ns
simulate_dribbling: false
command_delay: 3000000 # ns

enable_robot_localizer: true
robot_sensor_noise {
Expand Down
54 changes: 36 additions & 18 deletions src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -81,6 +81,7 @@ Simulator::Simulator(const amun::SimulatorSetup& setup)
m_data->robotReplyPacketLoss = 0;
m_data->missingBallDetections = 0;
m_data->dribblePerfect = false;
m_data->commandDelay = 0;

// no robots after initialisation
}
Expand All @@ -100,33 +101,45 @@ std::vector<robot::RadioResponse> Simulator::acceptYellowRobotControlCommand(
std::vector<robot::RadioResponse> Simulator::acceptRobotControlCommand(
const SSLSimulationProto::RobotControl& control, bool isBlue)
{
std::deque<QueuedRobotControl>& queue = isBlue ? m_blueCommands : m_yellowCommands;
queue.push_back({control, m_time});

// collect responses from robots
std::vector<robot::RadioResponse> responses;

Simulator::RobotMap& robotMap = isBlue ? m_data->robotsBlue : m_data->robotsYellow;

for (const SSLSimulationProto::RobotCommand& command : control.robot_commands())
// only apply commands that the robots have already received, i.e. whose
// command delay has passed
while (!queue.empty() && queue.front().receive_time + m_data->commandDelay <= m_time)
{
if (!robotMap.contains(command.id()))
{
return responses;
}
const SSLSimulationProto::RobotControl due_control = std::move(queue.front().control);
queue.pop_front();

// pass radio command to robot that matches the id
robot::RadioResponse response = robotMap.at(command.id())
->setCommand(command, *m_data->ball, m_charge,
m_data->robotCommandPacketLoss,
m_data->robotReplyPacketLoss);
response.set_time(m_time);
response.set_is_blue(isBlue);

// only collect valid responses
if (response.IsInitialized())
for (const SSLSimulationProto::RobotCommand& command : due_control.robot_commands())
{
if (m_data->robotReplyPacketLoss == 0 ||
m_data->rng.uniformFloat(0, 1) > m_data->robotReplyPacketLoss)
if (!robotMap.contains(command.id()))
{
return responses;
}

// pass radio command to robot that matches the id
robot::RadioResponse response =
robotMap.at(command.id())
->setCommand(command, *m_data->ball, m_charge,
m_data->robotCommandPacketLoss,
m_data->robotReplyPacketLoss);
response.set_time(m_time);
response.set_is_blue(isBlue);

// only collect valid responses
if (response.IsInitialized())
{
responses.emplace_back(response);
if (m_data->robotReplyPacketLoss == 0 ||
m_data->rng.uniformFloat(0, 1) > m_data->robotReplyPacketLoss)
{
responses.emplace_back(response);
}
}
}
}
Expand Down Expand Up @@ -691,6 +704,11 @@ void Simulator::handleSimulatorSetupCommand(const std::unique_ptr<amun::Command>
m_data->dribblePerfect = !realism.simulate_dribbling();
teamOrPerfectDribbleChanged = true;
}

if (realism.has_command_delay())
{
m_data->commandDelay = std::max<int64_t>(0l, realism.command_delay());
}
}

if (sim.has_ssl_control())
Expand Down
12 changes: 12 additions & 0 deletions src/extlibs/er_force_sim/src/amun/simulator/simulator.h
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#ifndef SIMULATOR_H
#define SIMULATOR_H

#include <deque>
#include <memory>
#include <random>
#include <utility>
Expand Down Expand Up @@ -128,6 +129,16 @@ class camun::simulator::Simulator
void initializeDetection(SSLProto::SSL_DetectionFrame& detection, size_t cameraId);

private:
// a robot control message waiting for its command delay to pass
struct QueuedRobotControl
{
SSLSimulationProto::RobotControl control;
int64_t receive_time; // simulator time [ns] at which it was received
};

std::deque<QueuedRobotControl> m_blueCommands;
std::deque<QueuedRobotControl> m_yellowCommands;

std::unique_ptr<SimulatorData> m_data;

bool m_enabled;
Expand Down Expand Up @@ -204,6 +215,7 @@ struct camun::simulator::SimulatorData
float robotReplyPacketLoss;
float missingBallDetections;
bool dribblePerfect;
int64_t commandDelay; // [ns] time for a robot to receive a command
};

#endif // SIMULATOR_H
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,9 @@ message RealismConfigErForce
// Simulates an offset of all reported object positions (robots, ball) at this
// magnitude [m]
optional float object_position_offset = 16;
// The time it takes for the robot to receive a command after the simulator
// receives it [ns]. Field number matches the upstream ER-Force framework.
optional int64 command_delay = 18;

// Config for Thunderbots' robot and sensors
// 100 in case er force have some updates w new fields at 17, ...
Expand Down
Loading