Skip to content

Commit

Permalink
Update semantic components to be consistent and functional when no in…
Browse files Browse the repository at this point in the history 
…fo is present
  • Loading branch information
@saikishor
saikishor committed Feb 20, 2025
1 parent 94d0d9d commit f958dfe
Show file tree
Hide file tree
Showing 5 changed files with 179 additions and 92 deletions.
74 changes: 39 additions & 35 deletions controller_interface/include/semantic_components/force_torque_sensor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#ifndef SEMANTIC_COMPONENTS__FORCE_TORQUE_SENSOR_HPP_
#define SEMANTIC_COMPONENTS__FORCE_TORQUE_SENSOR_HPP_

#include <algorithm>
#include <limits>
#include <string>
#include <vector>
Expand All @@ -25,7 +26,6 @@

namespace semantic_components
{
constexpr std::size_t FORCES_SIZE = 3;
class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::Wrench>
{
public:
Expand All @@ -42,6 +42,7 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
{name + "/" + "torque.z"}}),
existing_axes_({{true, true, true, true, true, true}})
{
data_.fill(std::numeric_limits<double>::quiet_NaN());
}

/// Constructor for 6D FTS with custom interface names.
Expand All @@ -59,6 +60,7 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
const std::string & interface_torque_y, const std::string & interface_torque_z)
: SemanticComponentInterface("", 6)
{
data_.fill(std::numeric_limits<double>::quiet_NaN());
auto check_and_add_interface =
[this](const std::string & interface_name, const std::size_t index)
{
Expand Down Expand Up @@ -89,17 +91,9 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
*/
std::array<double, 3> get_forces() const
{
update_data_from_interfaces();
std::array<double, 3> forces;
forces.fill(std::numeric_limits<double>::quiet_NaN());
std::size_t interface_counter{0};
for (auto i = 0u; i < forces.size(); ++i)
{
if (existing_axes_[i])
{
forces[i] = state_interfaces_[interface_counter].get().get_optional().value();
++interface_counter;
}
}
std::copy(data_.begin(), data_.begin() + 3, forces.begin());
return forces;
}

Expand All @@ -111,22 +105,9 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
*/
std::array<double, 3> get_torques() const
{
update_data_from_interfaces();
std::array<double, 3> torques;
torques.fill(std::numeric_limits<double>::quiet_NaN());

// find out how many force interfaces are being used
// torque interfaces will be found from the next index onward
auto torque_interface_counter = static_cast<std::size_t>(
std::count(existing_axes_.begin(), existing_axes_.begin() + FORCES_SIZE, true));

for (auto i = 0u; i < torques.size(); ++i)
{
if (existing_axes_[i + FORCES_SIZE])
{
torques[i] = state_interfaces_[torque_interface_counter].get().get_optional().value();
++torque_interface_counter;
}
}
std::copy(data_.begin() + 3, data_.end(), torques.begin());
return torques;
}

Expand All @@ -140,20 +121,43 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
*/
bool get_values_as_message(geometry_msgs::msg::Wrench & message) const
{
const auto [force_x, force_y, force_z] = get_forces();
const auto [torque_x, torque_y, torque_z] = get_torques();

message.force.x = force_x;
message.force.y = force_y;
message.force.z = force_z;
message.torque.x = torque_x;
message.torque.y = torque_y;
message.torque.z = torque_z;
update_data_from_interfaces();
message.force.x = data_[0];
message.force.y = data_[1];
message.force.z = data_[2];
message.torque.x = data_[3];
message.torque.y = data_[4];
message.torque.z = data_[5];

return true;
}

protected:
/**
* @brief Update the data from the state interfaces.
* @note The method is thread-safe and non-blocking.
* @note This method might return stale data if the data is not updated. This is to ensure that
* the data from the sensor is not discontinuous.
*/
void update_data_from_interfaces() const
{
std::size_t interface_counter{0};
for (auto i = 0u; i < data_.size(); ++i)
{
if (existing_axes_[i])
{
const auto data = state_interfaces_[interface_counter].get().get_optional();
if (data.has_value())
{
data_[i] = data.value();
}
++interface_counter;
}
}
}

/// Array to store the data of the FT sensors
mutable std::array<double, 6> data_;
/// Vector with existing axes for sensors with less then 6D axes.
std::array<bool, 6> existing_axes_;
};
Expand Down
63 changes: 55 additions & 8 deletions controller_interface/include/semantic_components/gps_sensor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#define SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_

#include <array>
#include <limits>
#include <string>

#include "semantic_components/semantic_component_interface.hpp"
Expand Down Expand Up @@ -61,7 +62,12 @@ class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
*/
int8_t get_status() const
{
return static_cast<int8_t>(state_interfaces_[0].get().get_optional().value());
const auto data = state_interfaces_[0].get().get_optional();
if (data.has_value())
{
return static_cast<int8_t>(data.value());
}
return std::numeric_limits<int8_t>::max();
}

/**
Expand All @@ -71,29 +77,58 @@ class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
*/
uint16_t get_service() const
{
return static_cast<uint16_t>(state_interfaces_[1].get().get_optional().value());
const auto data = state_interfaces_[1].get().get_optional();
if (data.has_value())
{
return static_cast<uint16_t>(data.value());
}
return std::numeric_limits<uint16_t>::max();
}

/**
* Return latitude reported by a GPS
*
* \return Latitude.
*/
double get_latitude() const { return state_interfaces_[2].get().get_optional().value(); }
double get_latitude() const
{
const auto data = state_interfaces_[2].get().get_optional();
if (data.has_value())
{
return data.value();
}
return std::numeric_limits<double>::quiet_NaN();
}

/**
* Return longitude reported by a GPS
*
* \return Longitude.
*/
double get_longitude() const { return state_interfaces_[3].get().get_optional().value(); }
double get_longitude() const
{
const auto data = state_interfaces_[3].get().get_optional();
if (data.has_value())
{
return data.value();
}
return std::numeric_limits<double>::quiet_NaN();
}

/**
* Return altitude reported by a GPS
*
* \return Altitude.
*/
double get_altitude() const { return state_interfaces_[4].get().get_optional().value(); }
double get_altitude() const
{
const auto data = state_interfaces_[4].get().get_optional();
if (data.has_value())
{
return data.value();
}
return std::numeric_limits<double>::quiet_NaN();
}

/**
* Return covariance reported by a GPS.
Expand All @@ -105,9 +140,21 @@ class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
typename = std::enable_if_t<sensor_option == GPSSensorOption::WithCovariance, U>>
const std::array<double, 9> & get_covariance()
{
covariance_[0] = state_interfaces_[5].get().get_optional().value();
covariance_[4] = state_interfaces_[6].get().get_optional().value();
covariance_[8] = state_interfaces_[7].get().get_optional().value();
const auto data_1 = state_interfaces_[5].get().get_optional();
const auto data_2 = state_interfaces_[6].get().get_optional();
const auto data_3 = state_interfaces_[7].get().get_optional();
if (data_1.has_value())
{
covariance_[0] = data_1.value();
}
if (data_2.has_value())
{
covariance_[4] = data_2.value();
}
if (data_3.has_value())
{
covariance_[8] = data_3.value();
}
return covariance_;
}

Expand Down
70 changes: 40 additions & 30 deletions controller_interface/include/semantic_components/imu_sensor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#ifndef SEMANTIC_COMPONENTS__IMU_SENSOR_HPP_
#define SEMANTIC_COMPONENTS__IMU_SENSOR_HPP_

#include <algorithm>
#include <limits>
#include <string>
#include <vector>
Expand Down Expand Up @@ -49,11 +50,9 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
*/
std::array<double, 4> get_orientation() const
{
update_data_from_interfaces();
std::array<double, 4> orientation;
for (auto i = 0u; i < orientation.size(); ++i)
{
orientation[i] = state_interfaces_[i].get().get_optional().value();
}
std::copy(data_.begin(), data_.begin() + 4, orientation.begin());
return orientation;
}

Expand All @@ -65,12 +64,9 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
*/
std::array<double, 3> get_angular_velocity() const
{
update_data_from_interfaces();
std::array<double, 3> angular_velocity;
const std::size_t interface_offset{4};
for (auto i = 0u; i < angular_velocity.size(); ++i)
{
angular_velocity[i] = state_interfaces_[interface_offset + i].get().get_optional().value();
}
std::copy(data_.begin() + 4, data_.begin() + 7, angular_velocity.begin());
return angular_velocity;
}

Expand All @@ -82,12 +78,9 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
*/
std::array<double, 3> get_linear_acceleration() const
{
update_data_from_interfaces();
std::array<double, 3> linear_acceleration;
const std::size_t interface_offset{7};
for (auto i = 0u; i < linear_acceleration.size(); ++i)
{
linear_acceleration[i] = state_interfaces_[interface_offset + i].get().get_optional().value();
}
std::copy(data_.begin() + 7, data_.end(), linear_acceleration.begin());
return linear_acceleration;
}

Expand All @@ -98,27 +91,44 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
*/
bool get_values_as_message(sensor_msgs::msg::Imu & message) const
{
const auto [orientation_x, orientation_y, orientation_z, orientation_w] = get_orientation();
const auto [angular_velocity_x, angular_velocity_y, angular_velocity_z] =
get_angular_velocity();
const auto [linear_acceleration_x, linear_acceleration_y, linear_acceleration_z] =
get_linear_acceleration();
update_data_from_interfaces();
message.orientation.x = data_[0];
message.orientation.y = data_[1];
message.orientation.z = data_[2];
message.orientation.w = data_[3];

message.orientation.x = orientation_x;
message.orientation.y = orientation_y;
message.orientation.z = orientation_z;
message.orientation.w = orientation_w;
message.angular_velocity.x = data_[4];
message.angular_velocity.y = data_[5];
message.angular_velocity.z = data_[6];

message.angular_velocity.x = angular_velocity_x;
message.angular_velocity.y = angular_velocity_y;
message.angular_velocity.z = angular_velocity_z;

message.linear_acceleration.x = linear_acceleration_x;
message.linear_acceleration.y = linear_acceleration_y;
message.linear_acceleration.z = linear_acceleration_z;
message.linear_acceleration.x = data_[7];
message.linear_acceleration.y = data_[8];
message.linear_acceleration.z = data_[9];

return true;
}

private:
/**
* @brief Update the data array from the state interfaces.
* @note This method is thread-safe and non-blocking.
* @note This method might return stale data if the data is not updated. This is to ensure that
* the data from the sensor is not discontinuous.
*/
void update_data_from_interfaces() const
{
for (auto i = 0u; i < data_.size(); ++i)
{
const auto data = state_interfaces_[i].get().get_optional();
if (data.has_value())
{
data_[i] = data.value();
}
}
}

// Array to store the data of the IMU sensor
mutable std::array<double, 10> data_{{0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
};

} // namespace semantic_components
Expand Down
Loading

0 comments on commit f958dfe

Please sign in to comment.