fix: wait until connect of the PoulpeRemoteClient (error after 2s) + …

orbita2d_controller/examples/poulpe2d.rs

@@ -99,18 +99,16 @@ fn main() -> Result<(), Box<dyn Error>> {
         // let target_yaw_mat=conversion::intrinsic_roll_pitch_yaw_to_matrix(0.0, 0.0, s);
         // let target=conversion::rotation_matrix_to_quaternion(target_yaw_mat);
 
-        let fb = controller.set_target_orientation_fb([s, 0.0]);
-        let axis = controller.get_axis_sensors();
-        match fb {
-            Ok(fb) => {
-                log::info!("Feedback: {:?}", fb);
-                println!(
-                    "{:?} {:?} {:?} {:?} {:?} {:?}",
-                    t as f64, s, fb.orientation[0], fb.orientation[1], axis[0], axis[1]
-                );
-            }
-            Err(e) => log::error!("Error: {}", e),
-        }
+        let fb = controller.set_target_orientation_fb([s, 0.0]).unwrap();
+        let axis = controller.get_axis_sensors().unwrap();
+
+        log::info!("Feedback: {:?}", fb);
+        log::info!("Axis: {:?}", axis);
+        println!(
+            "{:?} {:?} {:?} {:?} {:?} {:?}",
+            t as f64, s, fb.orientation[0], fb.orientation[1], axis[0], axis[1]
+        );
+
 
         thread::sleep(Duration::from_millis(1));
     }

orbita2d_controller/src/poulpe_ethercat.rs

@@ -59,6 +59,19 @@ impl Orbita2dController {
                 return Err("Error while connecting to the PoulpeRemoteClient".into());
             }
         };
+
+        // wait for the connection to be established
+        let mut trials = 20; // 2s
+        while io.get_state(id).is_err() {
+            thread::sleep(Duration::from_millis(100));
+            if trials == 0 {
+                log::error!("Error: Timeout while connecting to the Orbita2d PoulpeRemoteClient with id {}", id);
+                return Err("Error: Timeout while connecting to the Orbita2d  PoulpeRemoteClient".into());
+            }
+            trials -= 1;
+        }
+        log::info!("Connected to Orbita2d  PoulpeRemoteClient with id {}", id);
+
 
         // set the initial velocity and torque limit to 100%
         io.set_velocity_limit(id, [1.0; 2].to_vec());