From 1d508a0ec47ac8856c61ff978c2a3712b49d4f44 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez <shernand@iri.upc.edu>
Date: Fri, 19 Feb 2021 17:48:45 +0100
Subject: [PATCH] Modified the examples to test concurrent operation of all
drivers.
---
src/examples/model_car_actuators_test.cpp | 62 ++++++++------
src/examples/model_car_batteries_test.cpp | 56 +++++++-----
src/examples/model_car_egomotion_test.cpp | 94 +++++++++++----------
src/examples/model_car_ultrasounds_test.cpp | 49 ++++++-----
4 files changed, 145 insertions(+), 116 deletions(-)
diff --git a/src/examples/model_car_actuators_test.cpp b/src/examples/model_car_actuators_test.cpp
index 4559072..b9fed4f 100644
--- a/src/examples/model_car_actuators_test.cpp
+++ b/src/examples/model_car_actuators_test.cpp
@@ -8,30 +8,32 @@ int main(int argc, char *argv[])
CModelCarActuators *act_driver;
double vx,steer;
- try
+ while(1)
{
- act_driver = new CModelCarActuators(name);
- act_driver->open();
+ try
+ {
+ act_driver = new CModelCarActuators(name);
+ act_driver->open();
- act_driver->disable_ultrasounds();
+ act_driver->disable_ultrasounds();
- std::cout << "model_car_actuators_test::main: about to test speed, in 3..." << std::endl;
- sleep(1);
- std::cout << "model_car_actuators_test::main: about to test speed, 2..." << std::endl;
- sleep(1);
- std::cout << "model_car_actuators_test::main: about to test speed, 1..." << std::endl;
- sleep(1);
+ std::cout << "model_car_actuators_test::main: about to test speed, in 3..." << std::endl;
+ sleep(1);
+ std::cout << "model_car_actuators_test::main: about to test speed, 2..." << std::endl;
+ sleep(1);
+ std::cout << "model_car_actuators_test::main: about to test speed, 1..." << std::endl;
+ sleep(1);
- std::cout << "model_car_actuators_test::main: sending speed 10" << std::endl;
- for(unsigned int i=0;i<100;i++)
- {
- usleep(100000);
- vx=20.0*sin((2.0*3.14159*i)/100.0);
- steer=50.0*sin((2.0*3.14159*i)/100.0);
- std::cout << vx << "," << steer << std::endl;
- act_driver->send_command(vx,steer);
- }
- act_driver->send_command(0.0,0.0);
+ std::cout << "model_car_actuators_test::main: sending speed 10" << std::endl;
+ for(unsigned int i=0;i<100;i++)
+ {
+ usleep(100000);
+ vx=20.0*sin((2.0*3.14159*i)/100.0);
+ steer=50.0*sin((2.0*3.14159*i)/100.0);
+ std::cout << vx << "," << steer << std::endl;
+ act_driver->send_command(vx,steer);
+ }
+ act_driver->send_command(0.0,0.0);
/*
if(test_watchdog)
@@ -54,13 +56,17 @@ int main(int argc, char *argv[])
act_driver->send_disable_uss();
}
*/
- act_driver->close();
- }
- catch (CException &e)
- {
- std::cout << "model_car_actuators_test: " << e.what() << std::endl;
+ act_driver->close();
+ delete act_driver;
+ }
+ catch (CException &e)
+ {
+ if(act_driver!=NULL)
+ {
+ delete act_driver;
+ act_driver=NULL;
+ }
+ std::cout << "model_car_actuators_test: " << e.what() << std::endl;
+ }
}
-
- delete act_driver;
-
}
diff --git a/src/examples/model_car_batteries_test.cpp b/src/examples/model_car_batteries_test.cpp
index 7f2892b..ee9ac39 100644
--- a/src/examples/model_car_batteries_test.cpp
+++ b/src/examples/model_car_batteries_test.cpp
@@ -7,33 +7,43 @@ int main(int argc, char *argv[])
std::list<std::string> events;
std::string name = "model_car_driver_bat";
CModelCarBatteries *bat_driver;
- try
+
+ while(1)
{
- bat_driver = new CModelCarBatteries(name);
- bat_driver->open();
- events.push_back(bat_driver->get_new_data_event_id());
+ try
+ {
+ bat_driver = new CModelCarBatteries(name);
+ bat_driver->open();
+ events.clear();
+ events.push_back(bat_driver->get_new_data_event_id());
- while(true)
+ for(unsigned int i=0;i<100;i++)
+ {
+ event_server->wait_all(events,2000);
+ std::cout << " motors battery voltage: " << bat_driver->get_motors_battery_voltage() << std::endl;
+ std::cout << " motors battery cell1 voltage: " << bat_driver->get_motors_cell1_voltage() << std::endl;
+ std::cout << " motors battery cell2 voltage: " << bat_driver->get_motors_cell2_voltage() << std::endl;
+ std::cout << " payload battery voltage: " << bat_driver->get_payload_battery_voltage() << std::endl;
+ std::cout << " payload battery cell1 voltage: " << bat_driver->get_payload_cell1_voltage() << std::endl;
+ std::cout << " payload battery cell2 voltage: " << bat_driver->get_payload_cell2_voltage() << std::endl;
+ std::cout << " payload battery cell3 voltage: " << bat_driver->get_payload_cell3_voltage() << std::endl;
+ std::cout << " payload battery cell4 voltage: " << bat_driver->get_payload_cell4_voltage() << std::endl;
+ std::cout << " payload battery cell5 voltage: " << bat_driver->get_payload_cell5_voltage() << std::endl;
+ std::cout << " payload battery cell6 voltage: " << bat_driver->get_payload_cell6_voltage() << std::endl;
+ std::cout << std::endl;
+ }
+ bat_driver->close();
+ delete bat_driver;
+ }
+ catch (CException &e)
{
- event_server->wait_all(events,2000);
- std::cout << " motors battery voltage: " << bat_driver->get_motors_battery_voltage() << std::endl;
- std::cout << " motors battery cell1 voltage: " << bat_driver->get_motors_cell1_voltage() << std::endl;
- std::cout << " motors battery cell2 voltage: " << bat_driver->get_motors_cell2_voltage() << std::endl;
- std::cout << " payload battery voltage: " << bat_driver->get_payload_battery_voltage() << std::endl;
- std::cout << " payload battery cell1 voltage: " << bat_driver->get_payload_cell1_voltage() << std::endl;
- std::cout << " payload battery cell2 voltage: " << bat_driver->get_payload_cell2_voltage() << std::endl;
- std::cout << " payload battery cell3 voltage: " << bat_driver->get_payload_cell3_voltage() << std::endl;
- std::cout << " payload battery cell4 voltage: " << bat_driver->get_payload_cell4_voltage() << std::endl;
- std::cout << " payload battery cell5 voltage: " << bat_driver->get_payload_cell5_voltage() << std::endl;
- std::cout << " payload battery cell6 voltage: " << bat_driver->get_payload_cell6_voltage() << std::endl;
- std::cout << std::endl;
+ if(bat_driver!=NULL)
+ {
+ delete bat_driver;
+ bat_driver=NULL;
+ }
+ std::cout << e.what() << std::endl;
}
- bat_driver->close();
- }
- catch (CException &e)
- {
- std::cout << e.what() << std::endl;
}
- delete bat_driver;
}
diff --git a/src/examples/model_car_egomotion_test.cpp b/src/examples/model_car_egomotion_test.cpp
index 0a2ca94..1e7125f 100644
--- a/src/examples/model_car_egomotion_test.cpp
+++ b/src/examples/model_car_egomotion_test.cpp
@@ -12,57 +12,63 @@ int main(int argc, char *argv[])
int tach;
bool dir;
- try
+ while(1)
{
- egomotion_driver = new CModelCarEgomotion(name);
- egomotion_driver->open();
- events.push_back(egomotion_driver->get_new_data_event_id());
-
- while(true)
+ try
{
- try
+ egomotion_driver = new CModelCarEgomotion(name);
+ egomotion_driver->open();
+ events.clear();
+ events.push_back(egomotion_driver->get_new_data_event_id());
+
+ for(unsigned int i=0;i<100;i++)
{
- event_server->wait_all(events,1000);
- egomotion_driver->get_left_wheel(tach,dir,timestamp);
- std::cout << " Left Wheel: " << std::endl;
- std::cout << " tachometer: " << tach << std::endl;
- std::cout << " direction: " << dir << std::endl;
- std::cout << " timestamp: " << timestamp << std::endl;
- egomotion_driver->get_right_wheel(tach,dir,timestamp);
- std::cout << " Right Wheel: " << std::endl;
- std::cout << " tachometer: " << tach << std::endl;
- std::cout << " direction: " << dir << std::endl;
- std::cout << " timestamp: " << timestamp << std::endl;
- egomotion_driver->get_imu_accelerometer(ax,ay,az,timestamp);
- std::cout << " IMU: " << std::endl;
- std::cout << " ax: " << ax << std::endl;
- std::cout << " ay: " << ay << std::endl;
- std::cout << " az: " << az << std::endl;
- egomotion_driver->get_imu_gyroscope(gx,gy,gz,timestamp);
- std::cout << " gx: " << gx << std::endl;
- std::cout << " gy: " << gy << std::endl;
- std::cout << " gz: " << gz << std::endl;
- egomotion_driver->get_imu_magnetometer(mx,my,mz,timestamp);
- std::cout << " mx: " << mx << std::endl;
- std::cout << " my: " << my << std::endl;
- std::cout << " mz: " << mz << std::endl;
- std::cout << " timestamp: " << timestamp << std::endl;
- std::cout << std::endl;
+ try
+ {
+ event_server->wait_all(events,1000);
+ egomotion_driver->get_left_wheel(tach,dir,timestamp);
+ std::cout << " Left Wheel: " << std::endl;
+ std::cout << " tachometer: " << tach << std::endl;
+ std::cout << " direction: " << dir << std::endl;
+ std::cout << " timestamp: " << timestamp << std::endl;
+ egomotion_driver->get_right_wheel(tach,dir,timestamp);
+ std::cout << " Right Wheel: " << std::endl;
+ std::cout << " tachometer: " << tach << std::endl;
+ std::cout << " direction: " << dir << std::endl;
+ std::cout << " timestamp: " << timestamp << std::endl;
+ egomotion_driver->get_imu_accelerometer(ax,ay,az,timestamp);
+ std::cout << " IMU: " << std::endl;
+ std::cout << " ax: " << ax << std::endl;
+ std::cout << " ay: " << ay << std::endl;
+ std::cout << " az: " << az << std::endl;
+ egomotion_driver->get_imu_gyroscope(gx,gy,gz,timestamp);
+ std::cout << " gx: " << gx << std::endl;
+ std::cout << " gy: " << gy << std::endl;
+ std::cout << " gz: " << gz << std::endl;
+ egomotion_driver->get_imu_magnetometer(mx,my,mz,timestamp);
+ std::cout << " mx: " << mx << std::endl;
+ std::cout << " my: " << my << std::endl;
+ std::cout << " mz: " << mz << std::endl;
+ std::cout << " timestamp: " << timestamp << std::endl;
+ std::cout << std::endl;
+ }
+ catch(CEventTimeoutException &e){
+ std::cout << e.what() << std::endl;
+ }
}
- catch(CEventTimeoutException &e)
+
+ egomotion_driver->close();
+ delete egomotion_driver;
+ }
+ catch (CException &e)
+ {
+ if(egomotion_driver!=NULL)
{
- std::cout << e.what() << std::endl;
+ delete egomotion_driver;
+ egomotion_driver=NULL;
}
+ std::cout << e.what() << std::endl;
}
-
- std::cout << "model_car_drivers_test_egomotion::main: stopping egomotion_driver" << std::endl;
- egomotion_driver->close();
- std::cout << "model_car_drivers_test_egomotion::main: egomotion_driver stopped" << std::endl;
- }
- catch (CException &e)
- {
- std::cout << e.what() << std::endl;
}
- delete egomotion_driver;
}
diff --git a/src/examples/model_car_ultrasounds_test.cpp b/src/examples/model_car_ultrasounds_test.cpp
index 9816451..fe85be3 100644
--- a/src/examples/model_car_ultrasounds_test.cpp
+++ b/src/examples/model_car_ultrasounds_test.cpp
@@ -7,30 +7,37 @@ int main(int argc, char *argv[])
std::list<std::string> events;
std::string name = "model_car_driver_uss";
CModelCarUltrasounds *uss_driver;
- try
+ while(1)
{
- uss_driver = new CModelCarUltrasounds(name);
- uss_driver->open();
- events.push_back(uss_driver->get_new_data_event_id());
+ try
+ {
+ uss_driver = new CModelCarUltrasounds(name);
+ uss_driver->open();
+ events.clear();
+ events.push_back(uss_driver->get_new_data_event_id());
+
+ for(unsigned int i=0;i<100;i++)
+ {
+ event_server->wait_all(events,1000);
+ std::cout << " Side right ultrasound distance: " << uss_driver->get_side_right_distance() << std::endl;
+ std::cout << " Side left ultrasound distance: " << uss_driver->get_side_left_distance() << std::endl;
+ std::cout << " Rear right ultrasound distance: " << uss_driver->get_rear_right_distance() << std::endl;
+ std::cout << " Rear center ultrasound distance: " << uss_driver->get_rear_center_distance() << std::endl;
+ std::cout << " Rear left ultrasound distance: " << uss_driver->get_rear_left_distance() << std::endl;
+ std::cout << std::endl;
+ }
- while(true)
+ uss_driver->close();
+ delete uss_driver;
+ }
+ catch (CException &e)
{
- event_server->wait_all(events,1000);
- std::cout << " Side right ultrasound distance: " << uss_driver->get_side_right_distance() << std::endl;
- std::cout << " Side left ultrasound distance: " << uss_driver->get_side_left_distance() << std::endl;
- std::cout << " Rear right ultrasound distance: " << uss_driver->get_rear_right_distance() << std::endl;
- std::cout << " Rear center ultrasound distance: " << uss_driver->get_rear_center_distance() << std::endl;
- std::cout << " Rear left ultrasound distance: " << uss_driver->get_rear_left_distance() << std::endl;
- std::cout << std::endl;
+ if(uss_driver!=NULL)
+ {
+ delete uss_driver;
+ uss_driver=NULL;
+ }
+ std::cout << e.what() << std::endl;
}
-
- uss_driver->close();
}
- catch (CException &e)
- {
- std::cout << e.what() << std::endl;
- }
-
- delete uss_driver;
-
}
--
GitLab