From f66d8dd9f81fc4fc4bd57b1af9ac0ce5c56e235a Mon Sep 17 00:00:00 2001 From: David Mansolino Date: Tue, 1 Sep 2020 12:29:37 +0200 Subject: [PATCH] Sync with cyberbotics/webots --- package.xml | 2 +- src/complete_test.cpp | 111 ++++++++++++++++++++++++++++++------------ 2 files changed, 80 insertions(+), 33 deletions(-) diff --git a/package.xml b/package.xml index 3dea401..a865d9e 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ webots_ros - 2.1.0 + 2.1.1 The ROS package containing examples for interfacing ROS with the standard ROS controller of Webots http://wiki.ros.org/webots_ros diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 95f2f48..f555ce6 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -133,15 +133,16 @@ using namespace std; static int model_count; static vector model_list; -vector imageColor; -vector imageRangeFinder; -int connectorPresence = 0; -double accelerometerValues[3] = {0, 0, 0}; -double compassValues[3] = {0, 0, 0}; -double GPSValues[3] = {0, 0, 0}; -double GyroValues[3] = {0, 0, 0}; -double inertialUnitValues[4] = {0, 0, 0, 0}; -double touchSensorValues[3] = {0, 0, 0}; +static vector imageColor; +static vector imageRangeFinder; +static int connectorPresence = 0; +static double accelerometerValues[3] = {0, 0, 0}; +static double compassValues[3] = {0, 0, 0}; +static double GPSValues[3] = {0, 0, 0}; +static double GyroValues[3] = {0, 0, 0}; +static double inertialUnitValues[4] = {0, 0, 0, 0}; +static double touchSensorValues[3] = {0, 0, 0}; +static bool callbackCalled = false; ros::ServiceClient time_step_client; webots_ros::set_int time_step_srv; @@ -150,6 +151,7 @@ void modelNameCallback(const std_msgs::String::ConstPtr &name) { model_count++; model_list.push_back(name->data); ROS_INFO("Model #%d: %s.", model_count, model_list.back().c_str()); + callbackCalled = true; } void cameraCallback(const sensor_msgs::Image::ConstPtr &values) { @@ -159,30 +161,36 @@ void cameraCallback(const sensor_msgs::Image::ConstPtr &values) { imageColor[i] = *it; i++; } + callbackCalled = true; } void cameraRecognitionCallback(const webots_ros::RecognitionObject::ConstPtr &object) { ROS_INFO("Camera recognition saw a '%s' (time: %d:%d).", object->model.c_str(), object->header.stamp.sec, object->header.stamp.nsec); + callbackCalled = true; } void joystickCallback(const webots_ros::Int8Stamped::ConstPtr &value) { ROS_INFO("Joystick button pressed: %d (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); + callbackCalled = true; } void keyboardCallback(const webots_ros::Int32Stamped::ConstPtr &value) { ROS_INFO("Keyboard key pressed: %d (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); + callbackCalled = true; } void radarTargetsCallback(const webots_ros::RadarTarget::ConstPtr &target) { ROS_INFO("Received a radar target with distance=%lf received power=%lf speed=%lf azimuth=%lf (time: %d:%d).", target->distance, target->receivedPower, target->speed, target->azimuth, target->header.stamp.sec, target->header.stamp.nsec); + callbackCalled = true; } void radarTargetsNumberCallback(const webots_ros::Int8Stamped::ConstPtr &value) { ROS_INFO("Number of target seen by the radar: %d (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); + callbackCalled = true; } void rangeFinderCallback(const sensor_msgs::Image::ConstPtr &image) { @@ -192,15 +200,18 @@ void rangeFinderCallback(const sensor_msgs::Image::ConstPtr &image) { const float *depth_data = reinterpret_cast(&image->data[0]); for (int i = 0; i < size; ++i) imageRangeFinder[i] = depth_data[i]; + callbackCalled = true; } void lidarCallback(const sensor_msgs::Image::ConstPtr &image) { + callbackCalled = true; } void connectorCallback(const webots_ros::Int8Stamped::ConstPtr &value) { connectorPresence = value->data; ROS_INFO("Connector presence: %d (time: %d:%d).", connectorPresence, value->header.stamp.sec, value->header.stamp.nsec); + callbackCalled = true; } void accelerometerCallback(const sensor_msgs::Imu::ConstPtr &values) { @@ -210,10 +221,12 @@ void accelerometerCallback(const sensor_msgs::Imu::ConstPtr &values) { ROS_INFO("Accelerometer values are x=%f y=%f z=%f (time: %d:%d).", accelerometerValues[0], accelerometerValues[1], accelerometerValues[2], values->header.stamp.sec, values->header.stamp.nsec); + callbackCalled = true; } void battery_sensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) { ROS_INFO("Battery level is %f (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); + callbackCalled = true; } void compassCallback(const sensor_msgs::MagneticField::ConstPtr &values) { @@ -223,10 +236,12 @@ void compassCallback(const sensor_msgs::MagneticField::ConstPtr &values) { ROS_INFO("Compass values are x=%f y=%f z=%f (time: %d:%d).", compassValues[0], compassValues[1], compassValues[2], values->header.stamp.sec, values->header.stamp.nsec); + callbackCalled = true; } void distance_sensorCallback(const sensor_msgs::Range::ConstPtr &value) { ROS_INFO("Distance from object is %f (time: %d:%d).", value->range, value->header.stamp.sec, value->header.stamp.nsec); + callbackCalled = true; } void GPSCallback(const sensor_msgs::NavSatFix::ConstPtr &values) { @@ -236,10 +251,12 @@ void GPSCallback(const sensor_msgs::NavSatFix::ConstPtr &values) { ROS_INFO("GPS values are x=%f y=%f z=%f (time: %d:%d).", GPSValues[0], GPSValues[1], GPSValues[2], values->header.stamp.sec, values->header.stamp.nsec); + callbackCalled = true; } void GPSSpeedCallback(const webots_ros::Float64Stamped::ConstPtr &value) { ROS_INFO("GPS speed is: %fkm/h (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); + callbackCalled = true; } void gyroCallback(const sensor_msgs::Imu::ConstPtr &values) { @@ -249,6 +266,7 @@ void gyroCallback(const sensor_msgs::Imu::ConstPtr &values) { ROS_INFO("Gyro values are x=%f y=%f z=%f (time: %d:%d).", GyroValues[0], GyroValues[1], GyroValues[2], values->header.stamp.sec, values->header.stamp.nsec); + callbackCalled = true; } void inertialUnitCallback(const sensor_msgs::Imu::ConstPtr &values) { @@ -260,26 +278,32 @@ void inertialUnitCallback(const sensor_msgs::Imu::ConstPtr &values) { ROS_INFO("Inertial unit values (quaternions) are x=%f y=%f z=%f w=%f (time: %d:%d).", inertialUnitValues[0], inertialUnitValues[1], inertialUnitValues[2], inertialUnitValues[2], values->header.stamp.sec, values->header.stamp.nsec); + callbackCalled = true; } void lightSensorCallback(const sensor_msgs::Illuminance::ConstPtr &value) { ROS_INFO("Light intensity is %f.", value->illuminance); + callbackCalled = true; } void motorSensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) { ROS_INFO("Motor sensor sent value %f.", value->data); + callbackCalled = true; } void positionSensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) { ROS_INFO("Position sensor sent value %f (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); + callbackCalled = true; } void touchSensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) { ROS_INFO("Touch sensor sent value %f (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); + callbackCalled = true; } void touchSensorBumperCallback(const webots_ros::BoolStamped::ConstPtr &value) { ROS_INFO("Touch sensor sent value %d (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); + callbackCalled = true; } void touchSensor3DCallback(const geometry_msgs::WrenchStamped::ConstPtr &values) { @@ -289,11 +313,13 @@ void touchSensor3DCallback(const geometry_msgs::WrenchStamped::ConstPtr &values) ROS_INFO("Touch sensor values are x = %f, y = %f and z = %f (time: %d:%d).", touchSensorValues[0], touchSensorValues[1], touchSensorValues[2], values->header.stamp.sec, values->header.stamp.nsec); + callbackCalled = true; } void receiverCallback(const webots_ros::StringStamped::ConstPtr &value) { char *message = const_cast(value->data.c_str()); ROS_INFO("Received a message %s.", message); + callbackCalled = true; } void quit(int sig) { @@ -547,8 +573,8 @@ int main(int argc, char **argv) { ROS_INFO("Keyboard of %s has been enabled.", model_name.c_str()); sub_keyboard = n.subscribe(model_name + "/keyboard/key", 1, keyboardCallback); ROS_INFO("Topics for keyboard initialized."); - - while (sub_keyboard.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_keyboard.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -646,7 +672,8 @@ int main(int argc, char **argv) { ROS_INFO("Camera enabled."); sub_camera_color = n.subscribe(model_name + "/camera/image", 1, cameraCallback); ROS_INFO("Topic for camera color initialized."); - while (sub_camera_color.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_camera_color.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -730,7 +757,8 @@ int main(int argc, char **argv) { ROS_INFO("Camera recognition enabled."); sub_camera_recognition = n.subscribe(model_name + "/camera/recognition_objects", 1, cameraRecognitionCallback); ROS_INFO("Topic for camera recognition initialized."); - while (sub_camera_recognition.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_camera_recognition.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -802,7 +830,8 @@ int main(int argc, char **argv) { accelerometer_srv.request.value = 64; if (set_accelerometer_client.call(accelerometer_srv) && accelerometer_srv.response.success) { sub_accelerometer_64 = n.subscribe(model_name + "/accelerometer/values", 1, accelerometerCallback); - while (sub_accelerometer_64.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_accelerometer_64.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -827,7 +856,8 @@ int main(int argc, char **argv) { accelerometer_srv.request.value = 32; if (set_accelerometer_client.call(accelerometer_srv) && accelerometer_srv.response.success) { sub_accelerometer_32 = n.subscribe(model_name + "/accelerometer/values", 1, accelerometerCallback); - while (sub_accelerometer_32.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_accelerometer_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -886,7 +916,8 @@ int main(int argc, char **argv) { if (set_battery_sensor_client.call(battery_sensor_srv) && battery_sensor_srv.response.success) { ROS_INFO("Battery_sensor enabled."); sub_battery_sensor_32 = n.subscribe(model_name + "/battery_sensor/value", 1, battery_sensorCallback); - while (sub_battery_sensor_32.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_battery_sensor_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -934,7 +965,8 @@ int main(int argc, char **argv) { if (set_compass_client.call(compass_srv) && compass_srv.response.success == 1) { ROS_INFO("Compass enabled."); sub_compass_32 = n.subscribe(model_name + "/compass/values", 1, compassCallback); - while (sub_compass_32.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_compass_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -989,7 +1021,8 @@ int main(int argc, char **argv) { if (connector_enable_presence_client.call(connector_srv) && connector_srv.response.success) { ROS_INFO("Connector's presence sensor enabled."); sub_connector = n.subscribe(model_name + "/connector/presence", 1, connectorCallback); - while (sub_connector.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_connector.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -1425,7 +1458,8 @@ int main(int argc, char **argv) { if (set_distance_sensor_client.call(distance_sensor_srv) && distance_sensor_srv.response.success) { ROS_INFO("Distance_sensor enabled."); sub_distance_sensor_32 = n.subscribe(model_name + "/distance_sensor/value", 1, distance_sensorCallback); - while (sub_distance_sensor_32.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_distance_sensor_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -1550,7 +1584,8 @@ int main(int argc, char **argv) { if (set_GPS_client.call(GPS_srv) && GPS_srv.response.success) { ROS_INFO("GPS enabled."); sub_GPS_32 = n.subscribe(model_name + "/gps/values", 1, GPSCallback); - while (sub_GPS_32.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_GPS_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -1558,7 +1593,8 @@ int main(int argc, char **argv) { time_step_client.call(time_step_srv); sub_GPS_speed = n.subscribe(model_name + "/gps/speed", 1, GPSSpeedCallback); - while (sub_GPS_speed.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_GPS_speed.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -1609,7 +1645,8 @@ int main(int argc, char **argv) { if (set_gyro_client.call(gyro_srv) && gyro_srv.response.success) { ROS_INFO("Gyro enabled."); sub_gyro_32 = n.subscribe(model_name + "/gyro/values", 1, gyroCallback); - while (sub_gyro_32.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_gyro_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -1664,7 +1701,8 @@ int main(int argc, char **argv) { if (set_inertial_unit_client.call(inertial_unit_srv) && inertial_unit_srv.response.success) { ROS_INFO("Inertial_unit enabled."); sub_inertial_unit_32 = n.subscribe(model_name + "/inertial_unit/roll_pitch_yaw", 1, inertialUnitCallback); - while (sub_inertial_unit_32.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_inertial_unit_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -1715,9 +1753,10 @@ int main(int argc, char **argv) { if (enable_joystick_client.call(enable_joystick_srv) && enable_joystick_srv.response.success) { ROS_INFO("Joystick of %s has been enabled.", model_name.c_str()); sub_joystick = n.subscribe(model_name + "/joystick/pressed_button", 1, joystickCallback); + callbackCalled = false; ROS_INFO("Topics for joystick initialized."); - while (sub_joystick.getNumPublishers() == 0) { + while (sub_joystick.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -1787,9 +1826,10 @@ int main(int argc, char **argv) { if (set_lidar_client.call(lidar_srv) && lidar_srv.response.success) { ROS_INFO("Lidar enabled."); sub_lidar = n.subscribe(model_name + "/lidar/range_image", 1, lidarCallback); + callbackCalled = false; ROS_INFO("Topic for lidar initialized."); - while (sub_lidar.getNumPublishers() == 0) { + while (sub_lidar.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -1851,7 +1891,8 @@ int main(int argc, char **argv) { if (set_light_sensor_client.call(light_sensor_srv) && light_sensor_srv.response.success) { ROS_INFO("Light_sensor enabled."); sub_light_sensor_32 = n.subscribe(model_name + "/light_sensor/value", 1, lightSensorCallback); - while (sub_light_sensor_32.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_light_sensor_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -2176,7 +2217,8 @@ int main(int argc, char **argv) { if (set_motor_feedback_client.call(motor_feedback_srv) && motor_feedback_srv.response.success) { ROS_INFO("Motor feedback enabled."); sub_motor_feedback_32 = n.subscribe(model_name + "/rotational_motor/torque_feedback", 1, motorSensorCallback); - while (sub_motor_feedback_32.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_motor_feedback_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -2253,7 +2295,8 @@ int main(int argc, char **argv) { if (set_position_sensor_client.call(position_sensor_srv) && position_sensor_srv.response.success) { ROS_INFO("Position_sensor enabled."); sub_position_sensor_32 = n.subscribe(model_name + "/position_sensor/value", 1, positionSensorCallback); - while (sub_position_sensor_32.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_position_sensor_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -2306,9 +2349,10 @@ int main(int argc, char **argv) { ROS_INFO("Radar enabled."); sub_radar_target = n.subscribe(model_name + "/radar/targets", 1, radarTargetsCallback); sub_radar_target_number = n.subscribe(model_name + "/radar/number_of_targets", 1, radarTargetsNumberCallback); + callbackCalled = false; ROS_INFO("Topics for radar initialized."); - while (sub_radar_target.getNumPublishers() == 0 && sub_radar_target_number.getNumPublishers() == 0) { + while (sub_radar_target.getNumPublishers() == 0 && sub_radar_target_number.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -2368,9 +2412,10 @@ int main(int argc, char **argv) { if (set_range_finder_client.call(range_finder_srv) && range_finder_srv.response.success) { ROS_INFO("Range-finder enabled."); sub_range_finder_color = n.subscribe(model_name + "/range_finder/range_image", 1, rangeFinderCallback); + callbackCalled = false; ROS_INFO("Topic for range-finder initialized."); - while (sub_range_finder_color.getNumPublishers() == 0) { + while (sub_range_finder_color.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -2434,7 +2479,8 @@ int main(int argc, char **argv) { if (set_receiver_client.call(receiver_srv) && receiver_srv.response.success) { ROS_INFO("Receiver enabled."); sub_receiver_32 = n.subscribe(model_name + "/receiver/data", 1, receiverCallback); - while (sub_receiver_32.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_receiver_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -2596,7 +2642,8 @@ int main(int argc, char **argv) { sub_touch_sensor_32 = n.subscribe(model_name + "/touch_sensor/value", 1, touchSensorCallback); else sub_touch_sensor_32 = n.subscribe(model_name + "/touch_sensor/values", 1, touchSensor3DCallback); - while (sub_touch_sensor_32.getNumPublishers() == 0) { + callbackCalled = false; + while (sub_touch_sensor_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); }