From 652e23891adcdf0a08c90d865b7f4ff1a95d5afd Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 16 Dec 2022 07:21:01 +0800 Subject: [PATCH] Add multichannel lookup for environment sensors. (#1814) Often when sensing we may be interested in vector quantities. This PR extends the environmental_sensor to support vector quantities. Some additional work was required handle transformations. In particular if the data is something like Ocean Currents or Wind then the data has to be transformed to the robot's local coordinate frame and the robot's velocity also has to be accounted for. Signed-off-by: Arjo Chakravarty --- .../CMakeLists.txt | 7 + .../EnvironmentalSensorSystem.cc | 237 ++++++++++++++---- .../EnvironmentalSensorSystem.hh | 30 +++ .../TransformTypes.hh | 92 +++++++ .../TransformTypes_TEST.cc | 73 ++++++ .../environmental_sensor_system.cc | 85 +++++-- test/worlds/environmental_sensor.csv | 34 +-- test/worlds/environmental_sensor.sdf.in | 26 +- 8 files changed, 496 insertions(+), 88 deletions(-) create mode 100644 src/systems/environmental_sensor_system/TransformTypes.hh create mode 100644 src/systems/environmental_sensor_system/TransformTypes_TEST.cc diff --git a/src/systems/environmental_sensor_system/CMakeLists.txt b/src/systems/environmental_sensor_system/CMakeLists.txt index 89f30b71dd..c589a464bf 100644 --- a/src/systems/environmental_sensor_system/CMakeLists.txt +++ b/src/systems/environmental_sensor_system/CMakeLists.txt @@ -6,3 +6,10 @@ gz_add_system(environmental-sensor gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} gz-math${GZ_MATH_VER}::eigen3 ) + +gz_build_tests(TYPE UNIT + SOURCES + TransformTypes_TEST.cc + LIB_DEPS + gz-math${GZ_MATH_VER}::gz-math${GZ_MATH_VER} +) diff --git a/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc b/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc index 85268b7fce..b966fa9b88 100644 --- a/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc +++ b/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc @@ -15,6 +15,7 @@ * */ #include "EnvironmentalSensorSystem.hh" +#include "TransformTypes.hh" #include @@ -27,6 +28,7 @@ #include #include #include +#include #include #include @@ -76,19 +78,89 @@ class EnvironmentalSensor : public gz::sensors::Sensor // Load common sensor params gz::sensors::Sensor::Load(_sdf); - this->pub = this->node.Advertise(this->Topic()); + // Handle the field type + if (_sdf.Element() != nullptr && + _sdf.Element()->HasElement("output_format")) + { + std::string format = _sdf.Element()->Get("output_format"); + if (format == "scalar") + { + this->numberOfFields = 1; + } + else if (format == "vector3") + { + this->numberOfFields = 3; + } + else + { + gzerr << "Invalid output format " << format << "given." + << "valid options are scalar or vector3. " + << "Defaulting to 1." << std::endl; + } + } + + // Handle the transform type + if (_sdf.Element() != nullptr && + _sdf.Element()->HasElement("transform_type")) + { + auto res = + getTransformType(_sdf.Element()->Get("transform_type")); + + if (!res.has_value()) + { + gzerr << "Invalid transform type of " + << _sdf.Element()->Get("transform_type") + << " given. Valid types are ADD_VELOCITY_LOCAL, ADD_VELOCITY_GLOBAL," + << " LOCAL, and GLOBAL. Defaulting to GLOBAL." << std::endl; + } + else + { + this->transformType = res.value(); + } + } + + switch(this->numberOfFields) { + case 1: + this->pub = this->node.Advertise(this->Topic()); + break; + case 3: + this->pub = this->node.Advertise(this->Topic()); + break; + default: + gzerr << "Unable to create publisher too many fields" << "\n"; + } // If "environment_variable" is defined then remap // sensor from existing data. if (_sdf.Element() != nullptr && + this->numberOfFields == 1 && _sdf.Element()->HasElement("environment_variable")) { - this->fieldName = + this->fieldName[0] = _sdf.Element()->Get("environment_variable"); } + else if (_sdf.Element() != nullptr && + this->numberOfFields == 3) + { + if (_sdf.Element()->HasElement("environment_variable_x")) + { + this->fieldName[0] = + _sdf.Element()->Get("environment_variable_x"); + } + if (_sdf.Element()->HasElement("environment_variable_y")) + { + this->fieldName[1] = + _sdf.Element()->Get("environment_variable_y"); + } + if (_sdf.Element()->HasElement("environment_variable_z")) + { + this->fieldName[2] = + _sdf.Element()->Get("environment_variable_z"); + } + } else { - this->fieldName = type.substr(strlen(SENSOR_TYPE_PREFIX)); + this->fieldName[0] = type.substr(strlen(SENSOR_TYPE_PREFIX)); } // Allow setting custom frame_ids @@ -98,8 +170,18 @@ class EnvironmentalSensor : public gz::sensors::Sensor this->frameId = _sdf.Element()->Get("frame_id"); } - gzdbg << "Loaded environmental sensor for " << this->fieldName - << " publishing on " << this->Topic() << std::endl; + if (this->numberOfFields == 1) + { + gzdbg << "Loaded environmental sensor for " << this->fieldName[0] + << " publishing on " << this->Topic() << std::endl; + } + else + { + gzdbg << "Loaded environmental sensor for [" << this->fieldName[0] << ", " + << this->fieldName[1] << ", " << this->fieldName[2] + << "] publishing on " << this->Topic() << std::endl; + } + return true; } @@ -113,30 +195,72 @@ class EnvironmentalSensor : public gz::sensors::Sensor { if (!this->ready) return false; - if (!this->session.has_value()) return false; - // Step time if its not static - if (!this->gridField->staticTime) - this->session = this->gridField->frame[this->fieldName].StepTo( - this->session.value(), std::chrono::duration(_now).count()); + std::optional dataPoints[3]; + for (std::size_t i = 0; i < this->numberOfFields; ++i) + { + if (this->fieldName[i] == "") + { + // Empty field name means the column should default to zero. + dataPoints[i] = 0; + continue; + } + + if (!this->session[i].has_value()) return false; + + // Step time if its not static + if (!this->gridField->staticTime) + this->session[i] = this->gridField->frame[this->fieldName[i]].StepTo( + this->session[i].value(), + std::chrono::duration(_now).count()); + + if (!this->session[i].has_value()) return false; - if (!this->session.has_value()) return false; + dataPoints[i] = this->gridField->frame[this->fieldName[i]].LookUp( + this->session[i].value(), this->position); + } - gz::msgs::Double msg; - *msg.mutable_header()->mutable_stamp() = gz::msgs::Convert(_now); - auto frame = msg.mutable_header()->add_data(); - frame->set_key("frame_id"); - frame->add_value((this->frameId == "") ? this->Name() : this->frameId); - auto data = this->gridField->frame[this->fieldName].LookUp( - this->session.value(), this->position); - if (!data.has_value()) + if (this->numberOfFields == 1) { + gz::msgs::Double msg; + *msg.mutable_header()->mutable_stamp() = gz::msgs::Convert(_now); + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value((this->frameId == "") ? this->Name() : this->frameId); + auto data = dataPoints[0]; + if (!data.has_value()) + { + gzwarn << "Failed to acquire value perhaps out of field?\n"; + return false; + } + msg.set_data(data.value()); + // TODO(anyone) Add sensor noise. + this->pub.Publish(msg); + } + else if (this->numberOfFields == 3) { - gzwarn << "Failed to acquire value perhaps out of field?\n"; - return false; + gz::msgs::Vector3d msg; + *msg.mutable_header()->mutable_stamp() = gz::msgs::Convert(_now); + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value((this->frameId == "") ? this->Name() : this->frameId); + + if (!dataPoints[0].has_value() || !dataPoints[1].has_value() + || !dataPoints[2].has_value()) + { + gzwarn << "Failed to acquire value perhaps out of field?\n"; + return false; + } + + math::Vector3d vector( + dataPoints[0].value(), dataPoints[1].value(), dataPoints[2].value()); + auto transformed = transformFrame(this->transformType, this->objectPose, + this->velocity, vector); + + msg.set_x(transformed.X()); + msg.set_y(transformed.Y()); + msg.set_z(transformed.Z()); + this->pub.Publish(msg); } - msg.set_data(data.value()); - // TODO(anyone) Add sensor noise. - this->pub.Publish(msg); return true; } @@ -150,28 +274,42 @@ class EnvironmentalSensor : public gz::sensors::Sensor { gzdbg << "Setting new data table\n"; auto data = _data->Data(); - if(!data->frame.Has(this->fieldName)) + for (std::size_t i = 0; i < this->numberOfFields; ++i) { - gzwarn << "Environmental sensor could not find field " - << this->fieldName << "\n"; - this->ready = false; - return; + // If field name is empty it was intentionally left out so the column + // should default to zero. Otherwise if wrong name throw an error and + // disable output. + if (this->fieldName[i] != "" && !data->frame.Has(this->fieldName[i])) + { + gzwarn << "Environmental sensor could not find field " + << this->fieldName[i] << "\n"; + this->ready = false; + return; + } } this->gridField = data; - this->session = this->gridField->frame[this->fieldName].CreateSession(); - if (!this->gridField->staticTime) + for (std::size_t i = 0; i < this->numberOfFields; ++i) { - this->session = this->gridField->frame[this->fieldName].StepTo( - *this->session, - std::chrono::duration(_curr_time).count()); - } - this->ready = true; + if (this->fieldName[i] == "") + continue; - if(!this->session.has_value()) - { - gzerr << "Exceeded time stamp." << std::endl; + this->session[i] = + this->gridField->frame[this->fieldName[i]].CreateSession(); + if (!this->gridField->staticTime) + { + this->session[i] = this->gridField->frame[this->fieldName[i]].StepTo( + *this->session[i], + std::chrono::duration(_curr_time).count()); + } + + if(!this->session[i].has_value()) + { + gzerr << "Exceeded time stamp." << std::endl; + } } + + this->ready = true; } //////////////////////////////////////////////////////////////// @@ -186,7 +324,8 @@ class EnvironmentalSensor : public gz::sensors::Sensor { if (!this->ready) return false; - const auto worldPosition = worldPose(_entity, _ecm).Pos(); + this->objectPose = worldPose(_entity, _ecm); + const auto worldPosition = this->objectPose.Pos(); auto lookupCoords = getGridFieldCoordinates(_ecm, worldPosition, this->gridField); @@ -195,6 +334,11 @@ class EnvironmentalSensor : public gz::sensors::Sensor return false; } + auto vel = _ecm.Component(_entity); + if (vel != nullptr) + { + this->velocity = vel->Data(); + } this->position = lookupCoords.value(); return true; } @@ -202,16 +346,19 @@ class EnvironmentalSensor : public gz::sensors::Sensor //////////////////////////////////////////////////////////////// public: std::string Field() const { - return fieldName; + return fieldName[0]; } private: bool ready {false}; - private: math::Vector3d position; - private: std::string fieldName; + private: math::Vector3d position, velocity; + private: math::Pose3d objectPose; + private: std::size_t numberOfFields{1}; + private: std::string fieldName[3]; private: std::string frameId; - private: std::optional> session; + private: std::optional> session[3]; private: std::shared_ptr gridField; + private: TransformType transformType{TransformType::GLOBAL}; }; class gz::sim::EnvironmentalSensorSystemPrivate { @@ -313,6 +460,8 @@ void EnvironmentalSensorSystem::PreUpdate(const gz::sim::UpdateInfo &_info, gzerr << "No sensor data loaded\n"; } + enableComponent(_ecm, _entity); + // Keep track of this sensor this->dataPtr->entitySensorMap.insert(std::make_pair(_entity, std::move(sensor))); diff --git a/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.hh b/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.hh index ecb7ad9845..3570f25037 100644 --- a/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.hh +++ b/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.hh @@ -34,6 +34,36 @@ class EnvironmentalSensorSystemPrivate; /// field_name refers to the type of data you would like to output. /// Alternatively, if you would like to specify a custom field name you may do /// so using the tag. +/// +/// Additionally, the environment sensor supports scenarios where the data is in +/// the form of vector fields. For instance in the case of wind or ocean +/// currents. +/// +/// Tags: +/// - Either "scalar" or "vector3" depending on type +/// of output data desired. +/// - Only for scalar type. The name of the column of +/// the CSV file to be used. +/// - The name of the field to be used as the x value +/// in global frame for vector3 fields. Note: If this is left out and +/// vector3 is set, then the x value defaults to zero. +/// - The name of the field to be used as the y value +/// in global frame for vector3 fields. Note: If this is left out and +/// vector3 is set, then the y value defaults to zero. +/// - The name of the field to be used as the z value +/// in global frame vector3 fields. Note: If this is left out and +/// vector3 is set, then the z value defaults to zero. +/// - When handling vector2 or vector3 types it may be in our +/// interest to transform them to local coordinate frames. For instance +/// measurements of ocean currents or wind depend on the orientation and +/// velocity of the sensor. This field can have 4 values: +/// * GLOBAL - Don't transform the vectors to a local frame. +/// * LOCAL - Transform the vector to a local frame. +/// * ADD_VELOCITY_GLOBAL - Don't transform to local frame but account for +/// velocity change. +/// * ADD_VELOCITY_LOCAL - Transform to local frame and account for sensor +/// velocity. If you're working with wind or ocean currents, this is +/// probably the option you want. class EnvironmentalSensorSystem: public gz::sim::System, public gz::sim::ISystemConfigure, diff --git a/src/systems/environmental_sensor_system/TransformTypes.hh b/src/systems/environmental_sensor_system/TransformTypes.hh new file mode 100644 index 0000000000..94104f02bb --- /dev/null +++ b/src/systems/environmental_sensor_system/TransformTypes.hh @@ -0,0 +1,92 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef GZ_ENVIRONMENTAL_SYSTEM_TRANFORM_TYPE_HH_ +#define GZ_ENVIRONMENTAL_SYSTEM_TRANFORM_TYPE_HH_ + +#include +#include + +#include +#include + +namespace gz { +namespace sim { +/// \brief Transform Type +enum TransformType { + /// \brief Add the sensor's velocity and retrieve the value after + /// transforming the vector to the local frame. (Example would be for a + /// wind speed sensor or current sensor). + ADD_VELOCITY_LOCAL, + /// \brief Add the sensor's velocity and retrieve the value in a global + /// frame. (For instance to get Airspeed in a global frame) + ADD_VELOCITY_GLOBAL, + /// \brief The vector doesn't change with speed but we want it in local + /// frame. Example could be magnetic field lines in local frame. + LOCAL, + /// \brief The vector doesn't change with speed but we want it in local + /// frame. Example could be magnetic field lines in global frame. + GLOBAL +}; + +/// \brief Given a string return the type of transform +/// \param[in] _str - input string +/// \return std::nullopt if string invalid, else corresponding transform +std::optional getTransformType(const std::string &_str) +{ + if(_str == "ADD_VELOCITY_LOCAL") + return TransformType::ADD_VELOCITY_LOCAL; + if(_str == "ADD_VELOCITY_GLOBAL") + return TransformType::ADD_VELOCITY_GLOBAL; + if(_str == "LOCAL") + return TransformType::LOCAL; + if(_str == "GLOBAL") + return TransformType::GLOBAL; + return std::nullopt; +} + +/// \brief Given a string return the type of transform +/// \param[in] _type - Transform type. +/// \param[in] _pose - Global pose of frame to be transformed into. +/// \param[in] _velocity - Velocity of current frame. +/// \param[in] _reading - vector to be transformed. +/// \return transformed vector. +math::Vector3d transformFrame( + const TransformType _type, const math::Pose3d& _pose, + const math::Vector3d& _velocity, const math::Vector3d& _reading) +{ + math::Vector3d result; + math::Vector3d offset{0, 0, 0}; + + if (_type == ADD_VELOCITY_LOCAL || _type == ADD_VELOCITY_GLOBAL) + { + offset = -_velocity; + } + + switch (_type) { + case ADD_VELOCITY_LOCAL: + case LOCAL: + result = _pose.Rot().Inverse() * (_reading + offset); + break; + default: + result = (_reading + offset); + } + + return result; +} +} +} +#endif diff --git a/src/systems/environmental_sensor_system/TransformTypes_TEST.cc b/src/systems/environmental_sensor_system/TransformTypes_TEST.cc new file mode 100644 index 0000000000..452952d0b5 --- /dev/null +++ b/src/systems/environmental_sensor_system/TransformTypes_TEST.cc @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include +#include "TransformTypes.hh" + +using namespace gz; +using namespace sim; + +TEST(TransformationTypes, transformFrame) +{ + // Create a vehicle rotated 90 degrees in the yaw axis + math::Pose3d pose(math::Vector3d(0, 0, 0), math::Quaterniond(0, 0, GZ_PI_2)); + + // Vehicle should be moving at 1m/s along x-axis (In global frame) + math::Vector3d velocity(1, 0, 0); + + // Create a current moving against the vehicle + math::Vector3d current(-3, 0, 0); + + // Test transforms + // Global transformFrame should keep things unaffected. + auto res = transformFrame(TransformType::GLOBAL, pose, velocity, current); + EXPECT_EQ(res, current); + + res = transformFrame( + TransformType::ADD_VELOCITY_GLOBAL, pose, velocity, current); + EXPECT_EQ(res, math::Vector3d(-4, 0, 0)); + + // Tranforming to local frame without accounting for velocity. Considering + // robot is facing Y axis. + // Top down view: + // vvvvvvvvv Current direction + // ^ (Global X, Robot -Y, Robot Direction of motion) + // | + // | Robot coords + // -----------> (Global Y, Robot +X heading) --->+x + // | | + // | v +y + // + res = transformFrame(TransformType::LOCAL, pose, velocity, current); + EXPECT_LT((res - math::Vector3d(0, 3, 0)).Length(), 1e-5); + + // Tranforming with vehicle's velocity in account + res = transformFrame( + TransformType::ADD_VELOCITY_LOCAL, pose, velocity, current); + EXPECT_LT((res - math::Vector3d(0, 4, 0)).Length(), 1e-5); +} + +TEST(TransformationTypes, getTransformType) +{ + EXPECT_EQ( + getTransformType("ADD_VELOCITY_LOCAL"), TransformType::ADD_VELOCITY_LOCAL); + EXPECT_EQ( + getTransformType("ADD_VELOCITY_GLOBAL"), + TransformType::ADD_VELOCITY_GLOBAL); + EXPECT_EQ(getTransformType("LOCAL"), TransformType::LOCAL); + EXPECT_EQ(getTransformType("GLOBAL"), TransformType::GLOBAL); + EXPECT_EQ(getTransformType("nonsense"), std::nullopt); +} diff --git a/test/integration/environmental_sensor_system.cc b/test/integration/environmental_sensor_system.cc index 71e4fda198..429af8f31b 100644 --- a/test/integration/environmental_sensor_system.cc +++ b/test/integration/environmental_sensor_system.cc @@ -24,6 +24,8 @@ #include "gz/sim/TestFixture.hh" #include +#include +#include #include "gz/transport/Node.hh" #include "test_config.hh" @@ -36,32 +38,61 @@ using namespace sim; /// \brief Test EnvironmentPreload system class EnvironmentSensorTest : public InternalFixture<::testing::Test> { - ///////////////////////////////////////////////// - public: EnvironmentSensorTest() - { - node.Subscribe("/world/environmental_sensor_test/" - "model/model_with_sensor/link/link/sensor/custom_sensor/" - "environmental_sensor/humidity", - &EnvironmentSensorTest::OnReceiveMsg, - this); - } - - ///////////////////////////////////////////////// - public: void OnReceiveMsg(const gz::msgs::Double& msg) - { - // Data set is such that sensor value == time for the first second. - double nsec = msg.header().stamp().nsec(); - double sec = static_cast(msg.header().stamp().sec()); - auto time = nsec * 1e-9 + sec; - EXPECT_NEAR(time, msg.data(), 1e-9); - this->receivedData = true; - } - - ///////////////////////////////////////////////// - public: std::atomic receivedData{false}; - - ///////////////////////////////////////////////// - public: transport::Node node; + ///////////////////////////////////////////////// + public: EnvironmentSensorTest() + { + node.Subscribe("/world/environmental_sensor_test/" + "model/model_with_sensor/link/link/sensor/custom_sensor/" + "environmental_sensor/humidity", + &EnvironmentSensorTest::OnReceiveMsg, + this); + + node.Subscribe("/wind_speed2d", &EnvironmentSensorTest::OnWind2d, this); + node.Subscribe("/wind_speed3d", &EnvironmentSensorTest::OnWind3d, this); + } + + ///////////////////////////////////////////////// + public: void OnReceiveMsg(const gz::msgs::Double& _msg) + { + // Data set is such that sensor value == time for the first second. + double nsec = _msg.header().stamp().nsec(); + double sec = static_cast(_msg.header().stamp().sec()); + auto time = nsec * 1e-9 + sec; + EXPECT_NEAR(time, _msg.data(), 1e-9); + this->receivedData = true; + } + + ///////////////////////////////////////////////// + public: void OnWind2d(const gz::msgs::Vector3d& _msg) + { + EXPECT_NEAR(_msg.x(), 1, 1e-6); + EXPECT_NEAR(_msg.y(), 0, 1e-6); + EXPECT_NEAR(_msg.z(), 0, 1e-6); + this->receivedWind2dData = true; + } + + ///////////////////////////////////////////////// + public: void OnWind3d(const gz::msgs::Vector3d& _msg) + { + // Robot is rotated 90 degrees in yaw and has transform type set to local. + // Wind is in global +x and therefore in robot's local frame it will be -y + EXPECT_NEAR(_msg.x(), 0, 1e-3); + EXPECT_NEAR(_msg.y(), -1, 1e-3); + EXPECT_NEAR(_msg.z(), 0, 1e-3); + this->receivedWind3dData = true; + } + + ///////////////////////////////////////////////// + public: std::atomic receivedData{false}; + + ///////////////////////////////////////////////// + public: std::atomic receivedWind2dData{false}; + + ///////////////////////////////////////////////// + public: std::atomic receivedWind3dData{false}; + + ///////////////////////////////////////////////// + public: transport::Node node; }; @@ -81,4 +112,6 @@ TEST_F(EnvironmentSensorTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(CanPreload)) // Run server server.Run(true, 1000, false); EXPECT_TRUE(this->receivedData.load()); + EXPECT_TRUE(this->receivedWind2dData.load()); + EXPECT_TRUE(this->receivedWind3dData.load()); } diff --git a/test/worlds/environmental_sensor.csv b/test/worlds/environmental_sensor.csv index 3ec2c0a571..e5610e9c2f 100644 --- a/test/worlds/environmental_sensor.csv +++ b/test/worlds/environmental_sensor.csv @@ -1,17 +1,17 @@ -timestamp,humidity,x,y,z -0,0,-1,-1,-1 -0,0,-1,-1, 1 -0,0,-1, 1,-1 -0,0,-1, 1, 1 -0,0, 1,-1,-1 -0,0, 1,-1, 1 -0,0, 1, 1,-1 -0,0, 1, 1, 1 -1,1,-1,-1,-1 -1,1,-1,-1, 1 -1,1,-1, 1,-1 -1,1,-1, 1, 1 -1,1, 1,-1,-1 -1,1, 1,-1, 1 -1,1, 1, 1,-1 -1,1, 1, 1, 1 +timestamp,humidity,x,y,z,wind_speed_x,wind_speed_y,wind_speed_z +0,0,-1,-1,-1,1,0,0 +0,0,-1,-1, 1,1,0,0 +0,0,-1, 1,-1,1,0,0 +0,0,-1, 1, 1,1,0,0 +0,0, 1,-1,-1,1,0,0 +0,0, 1,-1, 1,1,0,0 +0,0, 1, 1,-1,1,0,0 +0,0, 1, 1, 1,1,0,0 +1,1,-1,-1,-1,1,0,0 +1,1,-1,-1, 1,1,0,0 +1,1,-1, 1,-1,1,0,0 +1,1,-1, 1, 1,1,0,0 +1,1, 1,-1,-1,1,0,0 +1,1, 1,-1, 1,1,0,0 +1,1, 1, 1,-1,1,0,0 +1,1, 1, 1, 1,1,0,0 diff --git a/test/worlds/environmental_sensor.sdf.in b/test/worlds/environmental_sensor.sdf.in index 46b3673ea7..85e0de6150 100644 --- a/test/worlds/environmental_sensor.sdf.in +++ b/test/worlds/environmental_sensor.sdf.in @@ -75,7 +75,7 @@ - 0 0 0.05 0 0 0 + 0 0 0.05 0 0 1.57 0.1 @@ -105,6 +105,30 @@ 30 true + + + + 1 + 30 + true + vector3 + wind_speed2d + wind_speed_x + wind_speed_y + + + + + 1 + 30 + true + vector3 + wind_speed3d + LOCAL + wind_speed_x + wind_speed_y + wind_speed_z +