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 +