Program Listing for File imu_sensor.hpp
↰ Return to documentation for file (include/semantic_components/imu_sensor.hpp
)
// Copyright 2021 PAL Robotics SL.
//
// 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 SEMANTIC_COMPONENTS__IMU_SENSOR_HPP_
#define SEMANTIC_COMPONENTS__IMU_SENSOR_HPP_
#include <limits>
#include <string>
#include <vector>
#include "semantic_components/semantic_component_interface.hpp"
#include "sensor_msgs/msg/imu.hpp"
namespace semantic_components
{
class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
{
public:
explicit IMUSensor(const std::string & name) : SemanticComponentInterface(name, 10)
{
interface_names_.emplace_back(name_ + "/" + "orientation.x");
interface_names_.emplace_back(name_ + "/" + "orientation.y");
interface_names_.emplace_back(name_ + "/" + "orientation.z");
interface_names_.emplace_back(name_ + "/" + "orientation.w");
interface_names_.emplace_back(name_ + "/" + "angular_velocity.x");
interface_names_.emplace_back(name_ + "/" + "angular_velocity.y");
interface_names_.emplace_back(name_ + "/" + "angular_velocity.z");
interface_names_.emplace_back(name_ + "/" + "linear_acceleration.x");
interface_names_.emplace_back(name_ + "/" + "linear_acceleration.y");
interface_names_.emplace_back(name_ + "/" + "linear_acceleration.z");
// Set default values to NaN
orientation_.fill(std::numeric_limits<double>::quiet_NaN());
angular_velocity_.fill(std::numeric_limits<double>::quiet_NaN());
linear_acceleration_.fill(std::numeric_limits<double>::quiet_NaN());
}
virtual ~IMUSensor() = default;
std::array<double, 4> get_orientation()
{
size_t interface_offset = 0;
for (size_t i = 0; i < orientation_.size(); ++i)
{
orientation_[i] = state_interfaces_[interface_offset + i].get().get_value();
}
return orientation_;
}
std::array<double, 3> get_angular_velocity()
{
size_t interface_offset = orientation_.size();
for (size_t i = 0; i < angular_velocity_.size(); ++i)
{
angular_velocity_[i] = state_interfaces_[interface_offset + i].get().get_value();
}
return angular_velocity_;
}
std::array<double, 3> get_linear_acceleration()
{
size_t interface_offset = orientation_.size() + angular_velocity_.size();
for (size_t i = 0; i < linear_acceleration_.size(); ++i)
{
linear_acceleration_[i] = state_interfaces_[interface_offset + i].get().get_value();
}
return linear_acceleration_;
}
bool get_values_as_message(sensor_msgs::msg::Imu & message)
{
// call get_orientation() and get_angular_velocity() get_linear_acceleration() to
// update with the latest values
get_orientation();
get_angular_velocity();
get_linear_acceleration();
// update the message values, covariances unknown
message.orientation.x = orientation_[0];
message.orientation.y = orientation_[1];
message.orientation.z = orientation_[2];
message.orientation.w = orientation_[3];
message.angular_velocity.x = angular_velocity_[0];
message.angular_velocity.y = angular_velocity_[1];
message.angular_velocity.z = angular_velocity_[2];
message.linear_acceleration.x = linear_acceleration_[0];
message.linear_acceleration.y = linear_acceleration_[1];
message.linear_acceleration.z = linear_acceleration_[2];
return true;
}
protected:
// Order is: orientation X,Y,Z,W angular velocity X,Y,Z and linear acceleration X,Y,Z
std::array<double, 4> orientation_;
std::array<double, 3> angular_velocity_;
std::array<double, 3> linear_acceleration_;
};
} // namespace semantic_components
#endif // SEMANTIC_COMPONENTS__IMU_SENSOR_HPP_