Skip to content

Commit 0b3dc30

Browse files
committed
feat(simple_sensor_simulator, imu): add gravity vector, tidy up
1 parent aeff9d3 commit 0b3dc30

File tree

4 files changed

+41
-21
lines changed

4 files changed

+41
-21
lines changed

openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -336,6 +336,7 @@ class SimulatorCore
336336
core->attachImuSensor(entity_ref, [&]() {
337337
simulation_api_schema::ImuSensorConfiguration configuration;
338338
configuration.set_entity(entity_ref);
339+
configuration.set_add_gravity(true);
339340
configuration.set_add_noise(false);
340341
configuration.set_use_seed(true);
341342
configuration.set_seed(0);

simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -31,9 +31,9 @@ class ImuSensorBase
3131
{
3232
public:
3333
explicit ImuSensorBase(const simulation_api_schema::ImuSensorConfiguration & configuration)
34-
: entity_name_(configuration.entity()),
34+
: add_gravity_(configuration.add_gravity()),
3535
add_noise_(configuration.add_noise()),
36-
noise_standard_deviation_(configuration.noise_standard_deviation()),
36+
noise_standard_deviation_(add_noise_ ? configuration.noise_standard_deviation() : 0.0),
3737
random_generator_(configuration.use_seed() ? configuration.seed() : std::random_device{}()),
3838
noise_distribution_(0.0, noise_standard_deviation_)
3939
{
@@ -46,7 +46,7 @@ class ImuSensorBase
4646
const std::vector<traffic_simulator_msgs::EntityStatus> & statuses) const -> bool = 0;
4747

4848
protected:
49-
const std::string entity_name_;
49+
const bool add_gravity_;
5050
const bool add_noise_;
5151
const double noise_standard_deviation_;
5252
mutable std::mt19937 random_generator_;
@@ -60,13 +60,13 @@ class ImuSensor : public ImuSensorBase
6060
explicit ImuSensor(
6161
const simulation_api_schema::ImuSensorConfiguration & configuration,
6262
const typename rclcpp::Publisher<MessageType>::SharedPtr & publisher)
63-
: ImuSensorBase(configuration), publisher_(publisher)
63+
: ImuSensorBase(configuration), entity_name_(configuration.entity()), publisher_(publisher)
6464
{
6565
}
6666

6767
auto update(
6868
const rclcpp::Time & current_ros_time,
69-
const std::vector<traffic_simulator_msgs::EntityStatus> & statuses) const -> bool
69+
const std::vector<traffic_simulator_msgs::EntityStatus> & statuses) const -> bool override
7070
{
7171
for (const auto & status : statuses) {
7272
if (status.name() == entity_name_) {
@@ -82,9 +82,10 @@ class ImuSensor : public ImuSensorBase
8282
private:
8383
auto generateMessage(
8484
const rclcpp::Time & current_ros_time,
85-
const traffic_simulator_msgs::msg::EntityStatus & status) const -> const sensor_msgs::msg::Imu;
85+
const traffic_simulator_msgs::msg::EntityStatus & status) const -> const MessageType;
8686

87-
const rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher_;
87+
const std::string entity_name_;
88+
const typename rclcpp::Publisher<MessageType>::SharedPtr publisher_;
8889
};
8990
} // namespace simple_sensor_simulator
9091
#endif // SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__IMU_SENSOR_HPP_

simulation/simple_sensor_simulator/src/sensor_simulation/imu/imu_sensor.cpp

Lines changed: 27 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,10 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15+
#include <tf2/LinearMath/Matrix3x3.h>
16+
#include <tf2/LinearMath/Quaternion.h>
17+
#include <tf2/LinearMath/Transform.h>
18+
1519
#include <geometry/quaternion/euler_to_quaternion.hpp>
1620
#include <geometry/quaternion/quaternion_to_euler.hpp>
1721
#include <simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp>
@@ -24,24 +28,37 @@ auto ImuSensor<sensor_msgs::msg::Imu>::generateMessage(
2428
const traffic_simulator_msgs::msg::EntityStatus & status) const -> const sensor_msgs::msg::Imu
2529
{
2630
const auto applyNoise = [&](geometry_msgs::msg::Vector3 & v) {
27-
if (add_noise_) {
28-
v.x += noise_distribution_(random_generator_);
29-
v.y += noise_distribution_(random_generator_);
30-
v.z += noise_distribution_(random_generator_);
31-
}
31+
v.x += noise_distribution_(random_generator_);
32+
v.y += noise_distribution_(random_generator_);
33+
v.z += noise_distribution_(random_generator_);
3234
};
3335

3436
auto imu_msg = sensor_msgs::msg::Imu();
3537
imu_msg.header.stamp = current_ros_time;
36-
imu_msg.header.frame_id = "imu_frame";
38+
imu_msg.header.frame_id = "imu_link";
3739

38-
// Apply noise
3940
auto orientation_rpy = math::geometry::convertQuaternionToEulerAngle(status.pose.orientation);
40-
applyNoise(orientation_rpy);
4141
auto twist = status.action_status.twist;
42-
applyNoise(twist.angular);
4342
auto accel = status.action_status.accel;
44-
applyNoise(accel.linear);
43+
44+
// Apply noise
45+
if (add_noise_) {
46+
applyNoise(orientation_rpy);
47+
applyNoise(twist.angular);
48+
applyNoise(accel.linear);
49+
}
50+
51+
// Apply gravity
52+
if (add_gravity_) {
53+
tf2::Quaternion orientation_quaternion;
54+
orientation_quaternion.setRPY(orientation_rpy.x, orientation_rpy.y, orientation_rpy.z);
55+
tf2::Matrix3x3 rotation_matrix(orientation_quaternion);
56+
tf2::Vector3 gravity(0.0, 0.0, -9.81);
57+
tf2::Vector3 transformed_gravity = rotation_matrix * gravity;
58+
accel.linear.x += transformed_gravity.x();
59+
accel.linear.y += transformed_gravity.y();
60+
accel.linear.z += transformed_gravity.z();
61+
}
4562

4663
// Set data
4764
imu_msg.orientation = math::geometry::convertEulerAngleToQuaternion(orientation_rpy);

simulation/simulation_interface/proto/simulation_api_schema.proto

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -38,10 +38,11 @@ message PseudoTrafficLightDetectorConfiguration {
3838
**/
3939
message ImuSensorConfiguration {
4040
string entity = 1; // Name of the entity which you want to attach imu.
41-
bool add_noise = 2;
42-
bool use_seed = 3;
43-
int32 seed = 4;
44-
double noise_standard_deviation = 5;
41+
bool add_gravity = 2; // If true, gravity will be added to the acceleration vector
42+
bool add_noise = 3; // If true, noise will be added to every vector published
43+
bool use_seed = 4; // If true, as seed will be used the passed value, if not it will be random
44+
int32 seed = 5; // Seed for random number generator
45+
double noise_standard_deviation = 6; // Standard deviation for noise (normal distribution, mean equal to 0.0)
4546
}
4647

4748
/**

0 commit comments

Comments
 (0)