Skip to content

Commit

Permalink
Create semantic component for poses
Browse files Browse the repository at this point in the history
  • Loading branch information
RobertWilbrandt committed Oct 6, 2024
1 parent ab84b74 commit 7a358ea
Show file tree
Hide file tree
Showing 5 changed files with 278 additions and 0 deletions.
10 changes: 10 additions & 0 deletions controller_interface/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@ target_compile_definitions(controller_interface PRIVATE "CONTROLLER_INTERFACE_BU

if(BUILD_TESTING)
find_package(ament_cmake_gmock REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)

ament_add_gmock(test_controller_interface test/test_controller_interface.cpp)
Expand Down Expand Up @@ -74,6 +75,15 @@ if(BUILD_TESTING)
ament_target_dependencies(test_imu_sensor
sensor_msgs
)

ament_add_gmock(test_pose_sensor test/test_pose_sensor.cpp)
target_link_libraries(test_pose_sensor
controller_interface
hardware_interface::hardware_interface
)
ament_target_dependencies(test_pose_sensor
geometry_msgs
)
endif()

install(
Expand Down
110 changes: 110 additions & 0 deletions controller_interface/include/semantic_components/pose_sensor.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,110 @@
// Copyright 2024 FZI Forschungszentrum Informatik
//
// 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__POSE_SENSOR_HPP_
#define SEMANTIC_COMPONENTS__POSE_SENSOR_HPP_

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

#include "geometry_msgs/msg/pose.hpp"
#include "semantic_components/semantic_component_interface.hpp"

namespace semantic_components
{

class PoseSensor : public SemanticComponentInterface<geometry_msgs::msg::Pose>
{
public:
/// Constructor for a standard pose sensor with interface names set based on sensor name.
explicit PoseSensor(const std::string & name) : SemanticComponentInterface{name, 7}
{
// Use standard interface names
interface_names_.emplace_back(name_ + '/' + "position.x");
interface_names_.emplace_back(name_ + '/' + "position.y");
interface_names_.emplace_back(name_ + '/' + "position.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");

// Set all sensor values to default value NaN
std::fill(position_.begin(), position_.end(), std::numeric_limits<double>::quiet_NaN());
std::fill(orientation_.begin(), orientation_.end(), std::numeric_limits<double>::quiet_NaN());
}

virtual ~PoseSensor() = default;

/// Update and return position.
/*!
* Update and return current pose position from state interfaces.
*
* \return Array of position coordinates.
*/
std::array<double, 3> get_position()
{
for (size_t i = 0; i < 3; ++i)
{
position_[i] = state_interfaces_[i].get().get_value();
}

return position_;
}

/// Update and return orientation
/*!
* Update and return current pose orientation from state interfaces.
*
* \return Array of orientation coordinates in xyzw convention.
*/
std::array<double, 4> get_orientation()
{
for (size_t i = 3; i < 7; ++i)
{
orientation_[i - 3] = state_interfaces_[i].get().get_value();
}

return orientation_;
}

/// Fill pose message with current values.
/**
* Fill a pose message with current position and orientation from the state interfaces.
*/
bool get_values_as_message(geometry_msgs::msg::Pose & message)
{
// Update state from state interfaces
get_position();
get_orientation();

// Set message values from current state
message.position.x = position_[0];
message.position.y = position_[1];
message.position.z = position_[2];
message.orientation.x = orientation_[0];
message.orientation.y = orientation_[1];
message.orientation.z = orientation_[2];
message.orientation.w = orientation_[3];

return true;
}

protected:
std::array<double, 3> position_;
std::array<double, 4> orientation_;
};

} // namespace semantic_components

#endif // SEMANTIC_COMPONENTS__POSE_SENSOR_HPP_
1 change: 1 addition & 0 deletions controller_interface/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
<build_export_depend>rclcpp_lifecycle</build_export_depend>

<test_depend>ament_cmake_gmock</test_depend>
<test_depend>geometry_msgs</test_depend>
<test_depend>sensor_msgs</test_depend>

<export>
Expand Down
98 changes: 98 additions & 0 deletions controller_interface/test/test_pose_sensor.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,98 @@
// Copyright (c) 2024, FZI Forschungszentrum Informatik
//
// 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 "test_pose_sensor.hpp"

void PoseSensorTest::SetUp()
{
full_interface_names_.reserve(size_);
for (const auto & interface_name : interface_names_)
{
full_interface_names_.emplace_back(sensor_name_ + '/' + interface_name);
}
}

void PoseSensorTest::TearDown() { pose_sensor_.reset(nullptr); }

TEST_F(PoseSensorTest, validate_all)
{
// Create sensor
pose_sensor_ = std::make_unique<TestablePoseSensor>(sensor_name_);
EXPECT_EQ(pose_sensor_->name_, sensor_name_);

// Validate reserved space for interface_names_ and state_interfaces_
// As state_interfaces_ are not defined yet, use capacity()
ASSERT_EQ(pose_sensor_->interface_names_.size(), size_);
ASSERT_EQ(pose_sensor_->state_interfaces_.capacity(), size_);

// Validate default interface_names_
EXPECT_TRUE(std::equal(
pose_sensor_->interface_names_.cbegin(), pose_sensor_->interface_names_.cend(),
full_interface_names_.cbegin(), full_interface_names_.cend()));

// Get interface names
std::vector<std::string> interface_names = pose_sensor_->get_state_interface_names();

// Assign values to position
hardware_interface::StateInterface position_x{
sensor_name_, interface_names_[0], &position_values_[0]};
hardware_interface::StateInterface position_y{
sensor_name_, interface_names_[1], &position_values_[1]};
hardware_interface::StateInterface position_z{
sensor_name_, interface_names_[2], &position_values_[2]};

// Assign values to orientation
hardware_interface::StateInterface orientation_x{
sensor_name_, interface_names_[3], &orientation_values_[0]};
hardware_interface::StateInterface orientation_y{
sensor_name_, interface_names_[4], &orientation_values_[1]};
hardware_interface::StateInterface orientation_z{
sensor_name_, interface_names_[5], &orientation_values_[2]};
hardware_interface::StateInterface orientation_w{
sensor_name_, interface_names_[6], &orientation_values_[3]};

// Create state interface vector in jumbled order
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
temp_state_interfaces.reserve(7);

temp_state_interfaces.emplace_back(position_z);
temp_state_interfaces.emplace_back(orientation_y);
temp_state_interfaces.emplace_back(orientation_x);
temp_state_interfaces.emplace_back(position_x);
temp_state_interfaces.emplace_back(orientation_w);
temp_state_interfaces.emplace_back(position_y);
temp_state_interfaces.emplace_back(orientation_z);

// Assign interfaces
pose_sensor_->assign_loaned_state_interfaces(temp_state_interfaces);
EXPECT_EQ(pose_sensor_->state_interfaces_.size(), size_);

// Validate correct position and orientation
EXPECT_EQ(pose_sensor_->get_position(), position_values_);
EXPECT_EQ(pose_sensor_->get_orientation(), orientation_values_);

// Validate generated message
geometry_msgs::msg::Pose temp_message;
ASSERT_TRUE(pose_sensor_->get_values_as_message(temp_message));
EXPECT_EQ(temp_message.position.x, position_values_[0]);
EXPECT_EQ(temp_message.position.y, position_values_[1]);
EXPECT_EQ(temp_message.position.z, position_values_[2]);
EXPECT_EQ(temp_message.orientation.x, orientation_values_[0]);
EXPECT_EQ(temp_message.orientation.y, orientation_values_[1]);
EXPECT_EQ(temp_message.orientation.z, orientation_values_[2]);
EXPECT_EQ(temp_message.orientation.w, orientation_values_[3]);

// Release state interfaces
pose_sensor_->release_interfaces();
ASSERT_EQ(pose_sensor_->state_interfaces_.size(), 0);
}
59 changes: 59 additions & 0 deletions controller_interface/test/test_pose_sensor.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
// Copyright (c) 2024, FZI Forschungszentrum Informatik
//
// 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 TEST_POSE_SENSOR_HPP_
#define TEST_POSE_SENSOR_HPP_

#include <gmock/gmock.h>

#include <array>
#include <memory>
#include <string>
#include <vector>

#include "semantic_components/pose_sensor.hpp"

class TestablePoseSensor : public semantic_components::PoseSensor
{
FRIEND_TEST(PoseSensorTest, validate_all);

public:
// Use default interface names
explicit TestablePoseSensor(const std::string & name) : PoseSensor{name} {}

virtual ~TestablePoseSensor() = default;
};

class PoseSensorTest : public ::testing::Test
{
public:
void SetUp();
void TearDown();

protected:
const size_t size_ = 7;
const std::string sensor_name_ = "test_pose_sensor";

std::vector<std::string> full_interface_names_;
const std::vector<std::string> interface_names_ = {
"position.x", "position.y", "position.z", "orientation.x",
"orientation.y", "orientation.z", "orientation.w"};

std::array<double, 3> position_values_ = {{1.1, 2.2, 3.3}};
std::array<double, 4> orientation_values_ = {{4.4, 5.5, 6.6, 7.7}};

std::unique_ptr<TestablePoseSensor> pose_sensor_;
};

#endif // TEST_POSE_SENSOR_HPP_

0 comments on commit 7a358ea

Please sign in to comment.