-
Notifications
You must be signed in to change notification settings - Fork 22
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
RSDK-8031 Pose tracker component (#258)
- Loading branch information
Showing
13 changed files
with
453 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,16 @@ | ||
| #include <viam/sdk/components/pose_tracker.hpp> | ||
|
|
||
| #include <viam/sdk/common/utils.hpp> | ||
|
|
||
| namespace viam { | ||
| namespace sdk { | ||
| API PoseTracker::api() const { | ||
| return API::get<PoseTracker>(); | ||
| } | ||
|
|
||
| API API::traits<PoseTracker>::api() { | ||
| return {kRDK, kComponent, "pose_tracker"}; | ||
| } | ||
|
|
||
| } // namespace sdk | ||
| } // namespace viam |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,75 @@ | ||
| /// @file components/pose_tracker.hpp | ||
| /// | ||
| /// @brief Defines a `PoseTracker` component | ||
| #pragma once | ||
|
|
||
| #include <string> | ||
| #include <unordered_map> | ||
| #include <vector> | ||
|
|
||
| #include <viam/api/common/v1/common.pb.h> | ||
| #include <viam/api/component/posetracker/v1/pose_tracker.pb.h> | ||
|
|
||
| #include <viam/sdk/common/pose.hpp> | ||
| #include <viam/sdk/components/component.hpp> | ||
| #include <viam/sdk/spatialmath/geometry.hpp> | ||
|
|
||
| namespace viam { | ||
| namespace sdk { | ||
|
|
||
| /// @defgroup PoseTracker Classes related to the PoseTracker component. | ||
|
|
||
| /// @class PoseTracker pose_tracker.hpp "components/pose_tracker.hpp" | ||
| /// @brief A `PoseTracker` represents a physical pose or motion tracking device. | ||
| /// | ||
| /// This acts as an abstract base class for any drivers representing specific | ||
| /// pose tracker implementations | ||
| class PoseTracker : public Component { | ||
| public: | ||
| using pose_map = std::unordered_map<std::string, pose_in_frame>; | ||
|
|
||
| API api() const override; | ||
|
|
||
| /// @brief Get the poses of each body tracked by the pose tracker. | ||
| /// @param tracker_name The name of the pose tracker. | ||
| /// @param body_names Names of bodies whose poses are being requested. If the vector is empty | ||
| /// then all available poses are returned. | ||
| /// @return A mapping of each body to its pose. | ||
| inline pose_map get_poses(const std::vector<std::string>& body_names) { | ||
| return get_poses(body_names, {}); | ||
| } | ||
|
|
||
| /// @brief Get the poses of each body tracked by the pose tracker. | ||
| /// @param tracker_name The name of the pose tracker. | ||
| /// @param body_names Names of bodies whose poses are being requested. If the vector is empty | ||
| /// then all available poses are returned. | ||
| /// @param extra Any additional arguments to the method. | ||
| /// @return A mapping of each body to its pose. | ||
| virtual pose_map get_poses(const std::vector<std::string>& body_names, | ||
| const AttributeMap& extra) = 0; | ||
|
|
||
| /// @brief Send/receive arbitrary commands to the resource. | ||
| /// @param Command the command to execute. | ||
| /// @return The result of the executed command. | ||
| virtual AttributeMap do_command(const AttributeMap& command) = 0; | ||
|
|
||
| /// @brief Returns `GeometryConfig`s associated with the calling pose tracker | ||
| inline std::vector<GeometryConfig> get_geometries() { | ||
| return get_geometries({}); | ||
| } | ||
|
|
||
| /// @brief Returns `GeometryConfig`s associated with the calling pose tracker | ||
| /// @param extra Any additional arguments to the method | ||
| virtual std::vector<GeometryConfig> get_geometries(const AttributeMap& extra) = 0; | ||
|
|
||
| protected: | ||
| using Component::Component; | ||
| }; | ||
|
|
||
| template <> | ||
| struct API::traits<PoseTracker> { | ||
| static API api(); | ||
| }; | ||
|
|
||
| } // namespace sdk | ||
| } // namespace viam |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,49 @@ | ||
| #include <viam/sdk/components/private/pose_tracker_client.hpp> | ||
|
|
||
| #include <viam/api/common/v1/common.pb.h> | ||
| #include <viam/api/component/posetracker/v1/pose_tracker.grpc.pb.h> | ||
| #include <viam/api/component/posetracker/v1/pose_tracker.pb.h> | ||
|
|
||
| #include <viam/sdk/common/client_helper.hpp> | ||
|
|
||
| namespace viam { | ||
| namespace sdk { | ||
| namespace impl { | ||
|
|
||
| PoseTrackerClient::PoseTrackerClient(std::string name, std::shared_ptr<grpc::Channel> channel) | ||
| : PoseTracker(std::move(name)), | ||
| stub_(viam::component::posetracker::v1::PoseTrackerService::NewStub(channel)), | ||
| channel_(std::move(channel)) {} | ||
|
|
||
| PoseTracker::pose_map PoseTrackerClient::get_poses(const std::vector<std::string>& body_names, | ||
| const AttributeMap&) { | ||
| return make_client_helper(this, *stub_, &StubType::GetPoses) | ||
| .with([&](viam::component::posetracker::v1::GetPosesRequest& request) { | ||
| *request.mutable_body_names() = {body_names.begin(), body_names.end()}; | ||
| }) | ||
| .invoke([](const viam::component::posetracker::v1::GetPosesResponse& response) { | ||
| PoseTracker::pose_map result; | ||
|
|
||
| for (const auto& pair : response.body_poses()) { | ||
| result.emplace(pair.first, pose_in_frame::from_proto(pair.second)); | ||
| } | ||
|
|
||
| return result; | ||
| }); | ||
| } | ||
|
|
||
| std::vector<GeometryConfig> PoseTrackerClient::get_geometries(const AttributeMap& extra) { | ||
| return make_client_helper(this, *stub_, &StubType::GetGeometries) | ||
| .with(extra) | ||
| .invoke([](auto& response) { return GeometryConfig::from_proto(response); }); | ||
| } | ||
|
|
||
| AttributeMap PoseTrackerClient::do_command(const AttributeMap& command) { | ||
| return make_client_helper(this, *stub_, &StubType::DoCommand) | ||
| .with([&](auto& request) { *request.mutable_command() = map_to_struct(command); }) | ||
| .invoke([](auto& response) { return struct_to_map(response.result()); }); | ||
| } | ||
|
|
||
| } // namespace impl | ||
| } // namespace sdk | ||
| } // namespace viam |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,43 @@ | ||
| /// @file components/private/pose_tracker_client.hpp | ||
| /// | ||
| /// @brief Implements a gRPC client for the `PoseTracker` component | ||
| #pragma once | ||
|
|
||
| #include <grpcpp/channel.h> | ||
|
|
||
| #include <viam/api/component/posetracker/v1/pose_tracker.grpc.pb.h> | ||
|
|
||
| #include <viam/sdk/components/pose_tracker.hpp> | ||
|
|
||
| namespace viam { | ||
| namespace sdk { | ||
| namespace impl { | ||
|
|
||
| /// @class PoseTrackerClient | ||
| /// @brief gRPC client implementation of a `PoseTracker` component. | ||
| /// @ingroup PoseTracker | ||
| class PoseTrackerClient : public PoseTracker { | ||
| public: | ||
| using interface_type = PoseTracker; | ||
|
|
||
| PoseTrackerClient(std::string name, std::shared_ptr<grpc::Channel> channel); | ||
|
|
||
| PoseTracker::pose_map get_poses(const std::vector<std::string>& body_names, | ||
| const AttributeMap& extra) override; | ||
|
|
||
| AttributeMap do_command(const AttributeMap& command) override; | ||
|
|
||
| std::vector<GeometryConfig> get_geometries(const AttributeMap& extra) override; | ||
|
|
||
| using PoseTracker::get_geometries; | ||
| using PoseTracker::get_poses; | ||
|
|
||
| private: | ||
| using StubType = viam::component::posetracker::v1::PoseTrackerService::StubInterface; | ||
| std::unique_ptr<StubType> stub_; | ||
| std::shared_ptr<grpc::Channel> channel_; | ||
| }; | ||
|
|
||
| } // namespace impl | ||
| } // namespace sdk | ||
| } // namespace viam |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,60 @@ | ||
| #include <viam/sdk/components/private/pose_tracker_server.hpp> | ||
|
|
||
| #include <viam/api/component/posetracker/v1/pose_tracker.pb.h> | ||
|
|
||
| #include <viam/sdk/common/service_helper.hpp> | ||
| #include <viam/sdk/common/utils.hpp> | ||
| #include <viam/sdk/components/pose_tracker.hpp> | ||
| #include <viam/sdk/config/resource.hpp> | ||
| #include <viam/sdk/rpc/server.hpp> | ||
|
|
||
| namespace viam { | ||
| namespace sdk { | ||
| namespace impl { | ||
|
|
||
| PoseTrackerServer::PoseTrackerServer(std::shared_ptr<ResourceManager> manager) | ||
| : ResourceServer(std::move(manager)) {} | ||
|
|
||
| ::grpc::Status PoseTrackerServer::GetPoses( | ||
| ::grpc::ServerContext*, | ||
| const ::viam::component::posetracker::v1::GetPosesRequest* request, | ||
| ::viam::component::posetracker::v1::GetPosesResponse* response) noexcept { | ||
| return make_service_helper<PoseTracker>( | ||
| "PoseTrackerServer::GetPoses", this, request)([&](auto& helper, auto& pose_tracker) { | ||
| const PoseTracker::pose_map result = pose_tracker->get_poses( | ||
| {request->body_names().begin(), request->body_names().end()}, helper.getExtra()); | ||
|
|
||
| for (const auto& pair : result) { | ||
| response->mutable_body_poses()->insert({pair.first, pair.second.to_proto()}); | ||
| } | ||
| }); | ||
| } | ||
|
|
||
| ::grpc::Status PoseTrackerServer::DoCommand( | ||
| grpc::ServerContext*, | ||
| const viam::common::v1::DoCommandRequest* request, | ||
| viam::common::v1::DoCommandResponse* response) noexcept { | ||
| return make_service_helper<PoseTracker>( | ||
| "PoseTrackerServer::DoCommand", this, request)([&](auto&, auto& pose_tracker) { | ||
| const AttributeMap result = pose_tracker->do_command(struct_to_map(request->command())); | ||
| *response->mutable_result() = map_to_struct(result); | ||
| }); | ||
| } | ||
|
|
||
| ::grpc::Status PoseTrackerServer::GetGeometries( | ||
| ::grpc::ServerContext*, | ||
| const ::viam::common::v1::GetGeometriesRequest* request, | ||
| ::viam::common::v1::GetGeometriesResponse* response) noexcept { | ||
| return make_service_helper<PoseTracker>( | ||
| "PoseTrackerServer::GetGeometries", this, request)([&](auto& helper, auto& pose_tracker) { | ||
| const std::vector<GeometryConfig> geometries = | ||
| pose_tracker->get_geometries(helper.getExtra()); | ||
| for (const auto& geometry : geometries) { | ||
| *response->mutable_geometries()->Add() = geometry.to_proto(); | ||
| } | ||
| }); | ||
| } | ||
|
|
||
| } // namespace impl | ||
| } // namespace sdk | ||
| } // namespace viam |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,44 @@ | ||
| /// @file components/private/pose_tracker_server.hpp | ||
| /// | ||
| /// @brief Implements a gRPC server for the `PoseTracker` component. | ||
| #pragma once | ||
|
|
||
| #include <viam/api/common/v1/common.pb.h> | ||
| #include <viam/api/component/posetracker/v1/pose_tracker.grpc.pb.h> | ||
| #include <viam/api/component/posetracker/v1/pose_tracker.pb.h> | ||
|
|
||
| #include <viam/sdk/components/pose_tracker.hpp> | ||
| #include <viam/sdk/resource/resource_manager.hpp> | ||
| #include <viam/sdk/resource/resource_server_base.hpp> | ||
|
|
||
| namespace viam { | ||
| namespace sdk { | ||
| namespace impl { | ||
|
|
||
| /// @class PoseTrackerServer | ||
| /// @brief gRPC server implementation of a `PoseTracker` component. | ||
| /// @ingroup PoseTracker | ||
| class PoseTrackerServer : public ResourceServer, | ||
| public viam::component::posetracker::v1::PoseTrackerService::Service { | ||
| public: | ||
| using interface_type = PoseTracker; | ||
| using service_type = component::posetracker::v1::PoseTrackerService; | ||
| explicit PoseTrackerServer(std::shared_ptr<ResourceManager> manager); | ||
|
|
||
| ::grpc::Status GetPoses( | ||
| ::grpc::ServerContext* context, | ||
| const ::viam::component::posetracker::v1::GetPosesRequest* request, | ||
| ::viam::component::posetracker::v1::GetPosesResponse* response) noexcept override; | ||
|
|
||
| ::grpc::Status DoCommand(::grpc::ServerContext* context, | ||
| const ::viam::common::v1::DoCommandRequest* request, | ||
| ::viam::common::v1::DoCommandResponse* response) noexcept override; | ||
|
|
||
| ::grpc::Status GetGeometries( | ||
| ::grpc::ServerContext* context, | ||
| const ::viam::common::v1::GetGeometriesRequest* request, | ||
| ::viam::common::v1::GetGeometriesResponse* response) noexcept override; | ||
| }; | ||
| } // namespace impl | ||
| } // namespace sdk | ||
| } // namespace viam |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.