Skip to content

Commit d7191c5

Browse files
FabienDanieauFabienDanieau
committed
add bench mode
1 parent ed88f60 commit d7191c5

File tree

6 files changed

+81
-35
lines changed

6 files changed

+81
-35
lines changed

‎README.md‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -15,8 +15,9 @@ RUST_LOG=debug cargo run --bin grpc-webrtc-rsbridge
1515

1616
### Client
1717

18-
The client sends messages to move both Reachy's arms.
18+
The client sends messages to move both Reachy's arms. Optional arguments are `--frequency`, to
19+
set the base uploading frequency, and `--bench_mode` to increase the frequency until 1500Hz.
1920

2021
```bash
21-
RUST_LOG=debug cargo run --bin simulator
22+
RUST_LOG=debug cargo run --bin simulator -- --frequency 100 --bench_mode
2223
```

‎deps/reachy2-sdk-api‎

‎grpc-webrtc-rsbridge/src/grpc/grpc_client.rs‎

Lines changed: 6 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
use log::{debug, error, trace, warn};
22

3+
use gst::prelude::*;
34
use reachy_api::component::dynamixel_motor::dynamixel_motor_service_client::DynamixelMotorServiceClient;
45
use reachy_api::component::dynamixel_motor::{DynamixelMotorCommand, DynamixelMotorsCommand};
56
use reachy_api::component::ComponentId;
@@ -10,16 +11,14 @@ use reachy_api::reachy::part::mobile::base::mobility::mobile_base_mobility_servi
1011
use reachy_api::reachy::part::mobile::base::utility::mobile_base_utility_service_client::MobileBaseUtilityServiceClient;
1112
use reachy_api::reachy::reachy_service_client::ReachyServiceClient;
1213
use reachy_api::reachy::Reachy;
13-
use reachy_api::reachy::ReachyStreamAuditRequest;
14-
use reachy_api::reachy::ReachyStreamStateRequest;
15-
1614
use reachy_api::reachy::ReachyState;
1715
use reachy_api::reachy::ReachyStatus;
16+
use reachy_api::reachy::ReachyStreamAuditRequest;
17+
use reachy_api::reachy::ReachyStreamStateRequest;
1818
use tokio::runtime::Runtime;
1919
use tonic::transport::Channel;
2020

2121
use gst::glib::WeakRef;
22-
use gst::prelude::*;
2322
use gstrswebrtc::signaller::Signallable;
2423
use reachy_api::bridge::{
2524
any_command, AnyCommands, ArmCommand, HandCommand, MobileBaseCommand, NeckCommand,
@@ -39,15 +38,15 @@ pub struct GrpcClient {
3938
rt: Runtime, // see https://tokio.rs/tokio/topics/bridging
4039
stop_flag: Arc<AtomicBool>,
4140
aborting: Arc<AtomicBool>,
42-
signaller: WeakRef<Signallable>,
41+
signaller: Option<WeakRef<Signallable>>,
4342
session_id: String,
4443
tx_stop: std::sync::mpsc::Sender<bool>,
4544
}
4645

4746
impl GrpcClient {
4847
pub fn new(
4948
address: String,
50-
signaller: WeakRef<Signallable>,
49+
signaller: Option<WeakRef<Signallable>>,
5150
session_id: Option<String>,
5251
tx_stop: Option<std::sync::mpsc::Sender<bool>>,
5352
) -> Result<Self, tonic::transport::Error> {
@@ -100,7 +99,7 @@ impl GrpcClient {
10099
self.aborting.store(true, Ordering::Relaxed);
101100
warn!("grpc connection lost. aborting session");
102101
self.tx_stop.send(true).unwrap();
103-
self.signaller
102+
self.signaller.as_ref().unwrap()
104103
//.lock()
105104
.upgrade()
106105
.unwrap()
@@ -367,20 +366,6 @@ impl GrpcClient {
367366
trace!("Handling antennas command: {:?}", cmd);
368367

369368
self.rt.block_on(self.dxl_motor_sub.send_command(cmd))?;
370-
371-
/*if let Some(antennas_goal) = cmd.antennas_goal {
372-
trace!("antennas_goal");
373-
self.rt
374-
.block_on(self.head_stub.set_antennas_position(antennas_goal))?;
375-
} else if let Some(turn_on) = cmd.turn_on {
376-
trace!("antennas_turn_on");
377-
self.rt.block_on(self.head_stub.turn_on(turn_on))?;
378-
} else if let Some(turn_off) = cmd.turn_off {
379-
trace!("antennas_turn_off");
380-
self.rt.block_on(self.head_stub.turn_off(turn_off))?;
381-
} else {
382-
warn!("Unknown antennas command: {:?}", cmd);
383-
}*/
384369
Ok(())
385370
}
386371
}

‎grpc-webrtc-rsbridge/src/webrtc/session.rs‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -85,7 +85,7 @@ impl Session {
8585
thread::spawn(move || {
8686
match GrpcClient::new(
8787
grpc_address,
88-
signaller,
88+
Some(signaller),
8989
Some(session_id),
9090
Some(tx_stop_thread_clone),
9191
) {

‎simulator/src/main.rs‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -72,7 +72,13 @@ fn main() {
7272
}
7373
});
7474

75-
let simulator = simulator::Simulator::new(uri, peer_id, rx_stop_signal, args.frequency);
75+
let simulator = simulator::Simulator::new(
76+
uri,
77+
peer_id,
78+
rx_stop_signal,
79+
args.frequency,
80+
args.bench_mode,
81+
);
7682
simulator.run();
7783
} else {
7884
warn!("No peer id found!");

‎simulator/src/simulator.rs‎

Lines changed: 63 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
1+
use ::glib::property::PropertyGet;
12
use gst::glib;
23
use gst::glib::WeakRef;
34
use gst::prelude::*;
@@ -6,7 +7,7 @@ use gstrswebrtc::signaller::SignallableExt;
67
use gstrswebrtc::signaller::Signaller;
78
use gstrswebrtc::signaller::WebRTCSignallerRole;
89
use gstwebrtc::WebRTCDataChannel;
9-
use log::{debug, error, info, warn};
10+
use log::{debug, error, info, trace, warn};
1011
use prost::Message;
1112
use reachy_api::bridge::any_command::Command::ArmCommand;
1213
use reachy_api::bridge::service_response::Response;
@@ -15,6 +16,7 @@ use reachy_api::bridge::{AnyCommand, AnyCommands};
1516
use reachy_api::reachy::kinematics::Matrix4x4;
1617
use reachy_api::reachy::part::arm::ArmCartesianGoal;
1718
use reachy_api::reachy::{Reachy, ReachyState, ReachyStatus};
19+
use std::sync::atomic::{AtomicU64, Ordering};
1820
use std::sync::{Arc, Mutex};
1921
use std::time::Duration;
2022
use std::time::Instant;
@@ -32,6 +34,7 @@ impl Simulator {
3234
peer_id: String,
3335
rx_stop_signal: std::sync::mpsc::Receiver<bool>,
3436
frequency: u16,
37+
bench_mode: bool,
3538
) -> Self {
3639
let main_loop = Arc::new(glib::MainLoop::new(None, false));
3740
let main_loop_clone = main_loop.clone();
@@ -46,8 +49,13 @@ impl Simulator {
4649
signaller.set_property("producer-peer-id", &peer_id);
4750

4851
let _reachy: Arc<Mutex<Option<Reachy>>> = Arc::new(Mutex::new(None));
49-
let (pipeline, webrtcbin) =
50-
Simulator::setup_webrtc(&peer_id, _reachy.clone(), main_loop.clone(), frequency);
52+
let (pipeline, webrtcbin) = Simulator::setup_webrtc(
53+
&peer_id,
54+
_reachy.clone(),
55+
main_loop.clone(),
56+
frequency,
57+
bench_mode,
58+
);
5159

5260
signaller.connect_closure(
5361
"error",
@@ -137,6 +145,7 @@ impl Simulator {
137145
reachy: Arc<Mutex<Option<Reachy>>>,
138146
main_loop: Arc<glib::MainLoop>,
139147
frequency: u16,
148+
bench_mode: bool,
140149
) -> (gst::Pipeline, gst::Element) {
141150
let pipeline = gst::Pipeline::builder()
142151
.name(format!("session-pipeline-{peer_id}"))
@@ -155,7 +164,13 @@ impl Simulator {
155164
match ret {
156165
Ok(gst::StateChangeSuccess::Success) | Ok(gst::StateChangeSuccess::Async) => {
157166
// Pipeline state changed successfully
158-
Simulator::configure_data_channels(webrtcbin_ref, reachy, main_loop, frequency);
167+
Simulator::configure_data_channels(
168+
webrtcbin_ref,
169+
reachy,
170+
main_loop,
171+
frequency,
172+
bench_mode,
173+
);
159174
}
160175
Ok(gst::StateChangeSuccess::NoPreroll) => {
161176
error!("Failed to transition pipeline to PLAYING: No preroll data available");
@@ -172,6 +187,7 @@ impl Simulator {
172187
reachy: Arc<Mutex<Option<Reachy>>>,
173188
main_loop: Arc<glib::MainLoop>,
174189
frequency: u16,
190+
bench_mode: bool,
175191
) {
176192
webrtcbin.upgrade().unwrap().connect_closure(
177193
"on-data-channel",
@@ -196,7 +212,13 @@ impl Simulator {
196212
Simulator::configure_reachy_audit_channel(channel);
197213
} else if label.starts_with("reachy_command_lossy") {
198214
debug!("Received reachy command lossy data channel");
199-
Simulator::send_commands(channel.clone(), reachy, main_loop, frequency);
215+
Simulator::send_commands(
216+
channel.clone(),
217+
reachy,
218+
main_loop,
219+
frequency,
220+
bench_mode,
221+
);
200222
} else if label.starts_with("reachy_command_reliable") {
201223
debug!("Received reachy command reliable data channel");
202224
Simulator::turn_on_arms(channel, reachy);
@@ -213,16 +235,37 @@ impl Simulator {
213235
reachy: Arc<Mutex<Option<Reachy>>>,
214236
main_loop: Arc<glib::MainLoop>,
215237
frequency: u16,
238+
bench_mode: bool,
216239
) {
217240
let radius = 0.2f64; //Circle radius
218241
let fixed_x = 0.4f64; // Fixed x-coordinate
219242
let center_y = 0f64;
220243
let center_z = 0.1f64; // Center of the circle in y-z plane
221-
//let frequency = 100; //Update frequency in Hz
222-
let sample_duration = Duration::from_millis(1000 / frequency as u64);
244+
//let mut frequency = frequency as u64; //Update frequency in Hz
245+
let frequency = Arc::new(AtomicU64::new(frequency as u64));
246+
let frequency_clone = frequency.clone();
247+
//let mut sample_duration = Duration::from_millis(1000 / frequency);
223248
let circle_period = 3f64;
224249
let t0 = Instant::now();
225250

251+
let main_loop_clone = main_loop.clone();
252+
if bench_mode {
253+
std::thread::spawn(move || {
254+
while main_loop_clone.is_running() {
255+
std::thread::sleep(Duration::from_millis(50));
256+
257+
let mut frequency_local = frequency_clone.load(Ordering::Relaxed);
258+
frequency_local += 1;
259+
//debug!("frequency: {}", frequency_local);
260+
if frequency_local > 1500 {
261+
main_loop_clone.quit();
262+
} else {
263+
frequency_clone.store(frequency_local, Ordering::Relaxed);
264+
}
265+
}
266+
});
267+
}
268+
226269
std::thread::spawn(move || {
227270
while main_loop.is_running() {
228271
let elapsed_time = t0.elapsed();
@@ -315,7 +358,18 @@ impl Simulator {
315358
};
316359
let data = glib::Bytes::from_owned(commands.encode_to_vec());
317360
channel.send_data(Some(&data));
361+
let sample_duration =
362+
Duration::from_millis(1000 / frequency.load(Ordering::Relaxed));
318363
std::thread::sleep(sample_duration);
364+
/*if bench_mode {
365+
//sample_duration += Duration::from_millis(100);
366+
frequency += 1;
367+
sample_duration = Duration::from_millis(1000 / frequency);
368+
//debug!("duration millis: {}", sample_duration.as_millis());
369+
if frequency > 1500 {
370+
main_loop.quit();
371+
}
372+
}*/
319373
}
320374
});
321375
}
@@ -377,7 +431,7 @@ impl Simulator {
377431
}
378432
};
379433

380-
debug!("Received state: {:?}", state);
434+
trace!("Received state: {:?}", state);
381435
});
382436
}
383437

@@ -395,7 +449,7 @@ impl Simulator {
395449
}
396450
};
397451

398-
debug!("Received status: {:?}", status);
452+
trace!("Received status: {:?}", status);
399453
});
400454
}
401455

0 commit comments

Comments
 (0)