|
| 1 | +use bevy::{app::PluginGroupBuilder, ecs::system::RunSystemOnce, prelude::*}; |
| 2 | +use nidhogg::{ |
| 3 | + types::{FillExt, JointArray}, |
| 4 | + DisconnectExt, HardwareInfo, NaoBackend, NaoControlMessage, NaoState, Result, |
| 5 | +}; |
| 6 | + |
| 7 | +use nidhogg::backend::{ConnectWithRetry, ReadHardwareInfo}; |
| 8 | +use yggdrasil::motion::keyframe::manager; |
| 9 | + |
| 10 | +const DEFAULT_STIFFNESS: f32 = 0.8; |
| 11 | + |
| 12 | +pub struct SimulationNaoPlugins; |
| 13 | + |
| 14 | +impl PluginGroup for SimulationNaoPlugins { |
| 15 | + fn build(self) -> PluginGroupBuilder { |
| 16 | + PluginGroupBuilder::start::<Self>() |
| 17 | + .add(SimLolaPlugin) |
| 18 | + .add(cycle::CycleTimePlugin) |
| 19 | + .add(battery_led::BatteryLedPlugin) |
| 20 | + .add(manager::NaoManagerPlugin) |
| 21 | + .add(center_of_mass::CenterOfMassPlugin) |
| 22 | + .add(center_of_pressure::CenterOfPressurePlugin) |
| 23 | + } |
| 24 | +} |
| 25 | + |
| 26 | +/// Plugin that adds systems for reading and writing to the `LoLA` socket using [`nidhogg`]. |
| 27 | +pub(super) struct SimLolaPlugin; |
| 28 | + |
| 29 | +impl Plugin for SimLolaPlugin { |
| 30 | + fn build(&self, app: &mut App) { |
| 31 | + app.init_resource::<NaoControlMessage>(); |
| 32 | + |
| 33 | + app.world_mut() |
| 34 | + .run_system_once(setup_lola) |
| 35 | + .expect("failed to setup lola!"); |
| 36 | + app.world_mut() |
| 37 | + .run_system_once(initialize_nao) |
| 38 | + .expect("failed to initialize nao resources!"); |
| 39 | + |
| 40 | + app.add_systems(Write, sync_hardware); |
| 41 | + } |
| 42 | +} |
| 43 | + |
| 44 | +/// Resource containing the [`SimLolaBackend`]. |
| 45 | +#[derive(Resource, Debug, Deref, DerefMut)] |
| 46 | +pub struct SimLola(SimLolaBackend); |
| 47 | + |
| 48 | +fn setup_lola(mut commands: Commands) { |
| 49 | + commands.insert_resource(SimLola(SimLolaBackend)); |
| 50 | +} |
| 51 | + |
| 52 | +fn initialize_nao(mut commands: Commands, mut lola: ResMut<SimLola>) { |
| 53 | + // let info = RobotInfo::new(&mut lola.0).expect("failed to read robot info from LoLA"); |
| 54 | + |
| 55 | + let info = RobotInfo { |
| 56 | + robot_name: "local".to_string(), |
| 57 | + robot_id: 0, |
| 58 | + head_id: Default::default(), |
| 59 | + head_version: Default::default(), |
| 60 | + body_id: Default::default(), |
| 61 | + body_version: Default::default(), |
| 62 | + initial_joint_positions: Default::default(), |
| 63 | + }; |
| 64 | + |
| 65 | + // Read state and reply with a message. |
| 66 | + let state = lola |
| 67 | + .read_nao_state() |
| 68 | + .expect("failed to read initial state from LoLA"); |
| 69 | + let msg = NaoControlMessage { |
| 70 | + position: info.initial_joint_positions.clone(), |
| 71 | + stiffness: JointArray::fill(DEFAULT_STIFFNESS), |
| 72 | + ..Default::default() |
| 73 | + }; |
| 74 | + lola.send_control_msg(msg) |
| 75 | + .expect("failed to send initial control message to LoLA"); |
| 76 | + |
| 77 | + tracing::info!( |
| 78 | + "Launched yggdrasil on {} with head_id: {}, body_id: {}", |
| 79 | + info.robot_name, |
| 80 | + info.head_id, |
| 81 | + info.body_id |
| 82 | + ); |
| 83 | + |
| 84 | + tracing::info!("Battery level: {}", state.battery.charge); |
| 85 | + |
| 86 | + commands.insert_resource(state); |
| 87 | + commands.insert_resource(info); |
| 88 | +} |
| 89 | + |
| 90 | +pub fn sync_hardware( |
| 91 | + mut nao: ResMut<SimLola>, |
| 92 | + mut robot_state: ResMut<NaoState>, |
| 93 | + update: Res<NaoControlMessage>, |
| 94 | +) { |
| 95 | + nao.send_control_msg(update.clone()) |
| 96 | + .expect("failed to send control message to LoLA"); |
| 97 | + |
| 98 | + *robot_state = nao |
| 99 | + .read_nao_state() |
| 100 | + .expect("failed to read state from LoLA"); |
| 101 | +} |
| 102 | + |
| 103 | +/// `LoLA` backend that communicates with a real NAO V6 through the socket at `/tmp/robocup` |
| 104 | +#[derive(Debug)] |
| 105 | +pub struct SimLolaBackend; |
| 106 | + |
| 107 | +impl NaoBackend for SimLolaBackend { |
| 108 | + /// Connects to a NAO backend |
| 109 | + /// |
| 110 | + /// # Examples |
| 111 | + /// ```no_run |
| 112 | + /// use nidhogg::{NaoBackend, backend::SimLolaBackend}; |
| 113 | + /// |
| 114 | + /// // We connect to a real NAO using the `LoLA` backend |
| 115 | + /// let mut nao = SimLolaBackend::connect().expect("Could not connect to the NAO! 😪"); |
| 116 | + /// ``` |
| 117 | + fn connect() -> Result<Self> { |
| 118 | + Ok(SimLolaBackend) |
| 119 | + } |
| 120 | + |
| 121 | + /// Converts a control message to the format required by the backend and writes it to that backend. |
| 122 | + /// |
| 123 | + /// # Examples |
| 124 | + /// ```no_run |
| 125 | + /// use nidhogg::{NaoBackend, NaoControlMessage, backend::SimLolaBackend, types::color}; |
| 126 | + /// |
| 127 | + /// let mut nao = SimLolaBackend::connect().unwrap(); |
| 128 | + /// |
| 129 | + /// // First, create a new control message where we set the chest color |
| 130 | + /// let msg = NaoControlMessage::builder().chest(color::f32::MAGENTA).build(); |
| 131 | + /// |
| 132 | + /// // Now we send it to the NAO! |
| 133 | + /// nao.send_control_msg(msg).expect("Failed to write control message to backend!"); |
| 134 | + /// ``` |
| 135 | + fn send_control_msg( |
| 136 | + &mut self, |
| 137 | + _: NaoControlMessage, |
| 138 | + ) -> std::result::Result<(), nidhogg::Error> { |
| 139 | + Ok(()) |
| 140 | + } |
| 141 | + |
| 142 | + /// Reads the current sensor data from the chosen backend |
| 143 | + /// |
| 144 | + /// # Examples |
| 145 | + /// ```no_run |
| 146 | + /// use nidhogg::{NaoBackend, backend::SimLolaBackend}; |
| 147 | + /// |
| 148 | + /// let mut nao = SimLolaBackend::connect().unwrap(); |
| 149 | + /// |
| 150 | + /// // Get the current state of the robot |
| 151 | + /// let state = nao.read_nao_state().expect("Failed to retrieve sensor data!"); |
| 152 | + /// ``` |
| 153 | + fn read_nao_state(&mut self) -> Result<NaoState> { |
| 154 | + Ok(NaoState { |
| 155 | + position: Default::default(), |
| 156 | + stiffness: Default::default(), |
| 157 | + accelerometer: Default::default(), |
| 158 | + gyroscope: Default::default(), |
| 159 | + angles: Default::default(), |
| 160 | + sonar: Default::default(), |
| 161 | + fsr: Default::default(), |
| 162 | + touch: Default::default(), |
| 163 | + battery: Default::default(), |
| 164 | + temperature: Default::default(), |
| 165 | + current: Default::default(), |
| 166 | + status: Default::default(), |
| 167 | + }) |
| 168 | + } |
| 169 | +} |
| 170 | + |
| 171 | +impl DisconnectExt for SimLolaBackend { |
| 172 | + fn disconnect(self) -> Result<()> { |
| 173 | + Ok(()) |
| 174 | + } |
| 175 | +} |
| 176 | + |
| 177 | +impl ConnectWithRetry for SimLolaBackend {} |
| 178 | + |
| 179 | +impl ReadHardwareInfo for SimLolaBackend { |
| 180 | + fn read_hardware_info(&mut self) -> Result<HardwareInfo> { |
| 181 | + Ok(HardwareInfo { |
| 182 | + body_id: Default::default(), |
| 183 | + body_version: Default::default(), |
| 184 | + head_id: Default::default(), |
| 185 | + head_version: Default::default(), |
| 186 | + }) |
| 187 | + } |
| 188 | +} |
0 commit comments