Skip to content

Commit

Permalink
Merge branch 'master' into tpo-add_semantic_cmd_interface
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich
christophfroehlich authored Dec 22, 2024
2 parents 342af6a + 03bf0e8 commit 5318268
Show file tree
Hide file tree
Showing 5 changed files with 135 additions and 166 deletions.
94 changes: 43 additions & 51 deletions controller_interface/include/semantic_components/force_torque_sensor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,26 +25,23 @@

namespace semantic_components
{
constexpr std::size_t FORCES_SIZE = 3;
class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::Wrench>
{
public:
/// Constructor for "standard" 6D FTS
explicit ForceTorqueSensor(const std::string & name) : SemanticComponentInterface(name, 6)
explicit ForceTorqueSensor(const std::string & name)
: SemanticComponentInterface(
name,
// If 6D FTS use standard names
{{name + "/" + "force.x"},
{name + "/" + "force.y"},
{name + "/" + "force.z"},
{name + "/" + "torque.x"},
{name + "/" + "torque.y"},
{name + "/" + "torque.z"}}),
existing_axes_({{true, true, true, true, true, true}})
{
// If 6D FTS use standard names
interface_names_.emplace_back(name_ + "/" + "force.x");
interface_names_.emplace_back(name_ + "/" + "force.y");
interface_names_.emplace_back(name_ + "/" + "force.z");
interface_names_.emplace_back(name_ + "/" + "torque.x");
interface_names_.emplace_back(name_ + "/" + "torque.y");
interface_names_.emplace_back(name_ + "/" + "torque.z");

// Set all interfaces existing
std::fill(existing_axes_.begin(), existing_axes_.end(), true);

// Set default force and torque values to NaN
std::fill(forces_.begin(), forces_.end(), std::numeric_limits<double>::quiet_NaN());
std::fill(torques_.begin(), torques_.end(), std::numeric_limits<double>::quiet_NaN());
}

/// Constructor for 6D FTS with custom interface names.
Expand All @@ -62,7 +59,8 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
const std::string & interface_torque_y, const std::string & interface_torque_z)
: SemanticComponentInterface("", 6)
{
auto check_and_add_interface = [this](const std::string & interface_name, const size_t index)
auto check_and_add_interface =
[this](const std::string & interface_name, const std::size_t index)
{
if (!interface_name.empty())
{
Expand All @@ -81,56 +79,55 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
check_and_add_interface(interface_torque_x, 3);
check_and_add_interface(interface_torque_y, 4);
check_and_add_interface(interface_torque_z, 5);

// Set default force and torque values to NaN
std::fill(forces_.begin(), forces_.end(), std::numeric_limits<double>::quiet_NaN());
std::fill(torques_.begin(), torques_.end(), std::numeric_limits<double>::quiet_NaN());
}

virtual ~ForceTorqueSensor() = default;

/// Return forces.
/**
* Return forces of a FTS.
*
* \return array of size 3 with force values.
* \return array of size 3 with force values (x, y, z).
*/
std::array<double, 3> get_forces()
std::array<double, 3> get_forces() const
{
size_t interface_counter = 0;
for (size_t i = 0; i < 3; ++i)
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_value();
forces[i] = state_interfaces_[interface_counter].get().get_value();
++interface_counter;
}
}
return forces_;
return forces;
}

/// Return torque.
/**
* Return torques of a FTS.
*
* \return array of size 3 with torque values.
* \return array of size 3 with torque values (x, y, z).
*/
std::array<double, 3> get_torques()
std::array<double, 3> get_torques() const
{
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<size_t>(std::count(existing_axes_.begin(), existing_axes_.begin() + 3, true));
auto torque_interface_counter = static_cast<std::size_t>(
std::count(existing_axes_.begin(), existing_axes_.begin() + FORCES_SIZE, true));

for (size_t i = 3; i < 6; ++i)
for (auto i = 0u; i < torques.size(); ++i)
{
if (existing_axes_[i])
if (existing_axes_[i + FORCES_SIZE])
{
torques_[i - 3] = state_interfaces_[torque_interface_counter].get().get_value();
torques[i] = state_interfaces_[torque_interface_counter].get().get_value();
++torque_interface_counter;
}
}
return torques_;
return torques;
}

/// Return Wrench message with forces and torques.
Expand All @@ -141,29 +138,24 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
*
* \return wrench message from values;
*/
bool get_values_as_message(geometry_msgs::msg::Wrench & message)
bool get_values_as_message(geometry_msgs::msg::Wrench & message) const
{
// call get_forces() and get_troque() to update with the latest values
get_forces();
get_torques();

// update the message values
message.force.x = forces_[0];
message.force.y = forces_[1];
message.force.z = forces_[2];
message.torque.x = torques_[0];
message.torque.y = torques_[1];
message.torque.z = torques_[2];
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;

return true;
}

protected:
/// Vector with existing axes for sensors with less then 6D axes.
// Order is: force X, force Y, force Z, torque X, torque Y, torque Z.
std::array<bool, 6> existing_axes_;
std::array<double, 3> forces_;
std::array<double, 3> torques_;
};

} // namespace semantic_components
Expand Down
108 changes: 48 additions & 60 deletions controller_interface/include/semantic_components/imu_sensor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,110 +27,98 @@ namespace semantic_components
class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
{
public:
explicit IMUSensor(const std::string & name) : SemanticComponentInterface(name, 10)
explicit IMUSensor(const std::string & name)
: SemanticComponentInterface(
name, {{name + "/" + "orientation.x"},
{name + "/" + "orientation.y"},
{name + "/" + "orientation.z"},
{name + "/" + "orientation.w"},
{name + "/" + "angular_velocity.x"},
{name + "/" + "angular_velocity.y"},
{name + "/" + "angular_velocity.z"},
{name + "/" + "linear_acceleration.x"},
{name + "/" + "linear_acceleration.y"},
{name + "/" + "linear_acceleration.z"}})
{
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;

/// Return orientation.
/**
* Return orientation reported by an IMU
*
* \return array of size 4 with orientation quaternion (x,y,z,w)
* \return Array of size 4 with orientation quaternion (x,y,z,w).
*/
std::array<double, 4> get_orientation()
std::array<double, 4> get_orientation() const
{
size_t interface_offset = 0;
for (size_t i = 0; i < orientation_.size(); ++i)
std::array<double, 4> orientation;
for (auto i = 0u; i < orientation.size(); ++i)
{
orientation_[i] = state_interfaces_[interface_offset + i].get().get_value();
orientation[i] = state_interfaces_[i].get().get_value();
}
return orientation_;
return orientation;
}

/// Return angular velocity.
/**
* Return angular velocity reported by an IMU
*
* \return array of size 3 with angular velocity values.
* \return array of size 3 with angular velocity values (x, y, z).
*/
std::array<double, 3> get_angular_velocity()
std::array<double, 3> get_angular_velocity() const
{
size_t interface_offset = orientation_.size();
for (size_t i = 0; i < angular_velocity_.size(); ++i)
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_value();
angular_velocity[i] = state_interfaces_[interface_offset + i].get().get_value();
}
return angular_velocity_;
return angular_velocity;
}

/// Return linear acceleration.
/**
* Return linear acceleration reported by an IMU
*
* \return array of size 3 with linear acceleration values.
* \return array of size 3 with linear acceleration values (x, y, z).
*/
std::array<double, 3> get_linear_acceleration()
std::array<double, 3> get_linear_acceleration() const
{
size_t interface_offset = orientation_.size() + angular_velocity_.size();
for (size_t i = 0; i < linear_acceleration_.size(); ++i)
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_value();
linear_acceleration[i] = state_interfaces_[interface_offset + i].get().get_value();
}
return linear_acceleration_;
return linear_acceleration;
}

/// Return Imu message with orientation, angular velocity and linear acceleration
/**
* Constructs and return a IMU message from the current values.
* \return imu message from values;
*/
bool get_values_as_message(sensor_msgs::msg::Imu & message)
bool get_values_as_message(sensor_msgs::msg::Imu & message) const
{
// call get_orientation() and get_angular_velocity() get_linear_acceleration() to
// update with the latest values
get_orientation();
get_angular_velocity();
get_linear_acceleration();
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 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.orientation.x = orientation_x;
message.orientation.y = orientation_y;
message.orientation.z = orientation_z;
message.orientation.w = orientation_w;

message.angular_velocity.x = angular_velocity_[0];
message.angular_velocity.y = angular_velocity_[1];
message.angular_velocity.z = angular_velocity_[2];
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_[0];
message.linear_acceleration.y = linear_acceleration_[1];
message.linear_acceleration.z = linear_acceleration_[2];
message.linear_acceleration.x = linear_acceleration_x;
message.linear_acceleration.y = linear_acceleration_y;
message.linear_acceleration.z = linear_acceleration_z;

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
Expand Down
Loading

0 comments on commit 5318268

Please sign in to comment.