-
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-3531 - Motion service wrappers (#141)
- Loading branch information
Showing
24 changed files
with
1,820 additions
and
76 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
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,48 @@ | ||
| #include <viam/sdk/common/pose.hpp> | ||
|
|
||
| #include <common/v1/common.pb.h> | ||
|
|
||
| namespace viam { | ||
| namespace sdk { | ||
|
|
||
| common::v1::PoseInFrame pose_in_frame::to_proto() const { | ||
| common::v1::PoseInFrame pif; | ||
| *pif.mutable_reference_frame() = reference_frame; | ||
| common::v1::Pose proto_pose; | ||
| proto_pose.set_x(pose.coordinates.x); | ||
| proto_pose.set_y(pose.coordinates.y); | ||
| proto_pose.set_z(pose.coordinates.z); | ||
| proto_pose.set_o_x(pose.orientation.o_x); | ||
| proto_pose.set_o_y(pose.orientation.o_y); | ||
| proto_pose.set_o_z(pose.orientation.o_z); | ||
| proto_pose.set_theta(pose.theta); | ||
| *pif.mutable_pose() = std::move(proto_pose); | ||
| return pif; | ||
| }; | ||
|
|
||
| pose_in_frame pose_in_frame::from_proto(const common::v1::PoseInFrame& proto) { | ||
| pose_in_frame pif; | ||
| pif.reference_frame = proto.reference_frame(); | ||
| const auto& proto_pose = proto.pose(); | ||
| pif.pose.orientation.o_x = proto_pose.o_x(); | ||
| pif.pose.orientation.o_y = proto_pose.o_y(); | ||
| pif.pose.orientation.o_z = proto_pose.o_z(); | ||
| pif.pose.coordinates.x = proto_pose.x(); | ||
| pif.pose.coordinates.y = proto_pose.y(); | ||
| pif.pose.coordinates.z = proto_pose.z(); | ||
| pif.pose.theta = proto_pose.theta(); | ||
|
|
||
| return pif; | ||
| } | ||
|
|
||
| bool operator==(const pose_in_frame& lhs, const pose_in_frame& rhs) { | ||
| return lhs.pose == rhs.pose && lhs.reference_frame == rhs.reference_frame; | ||
| } | ||
| std::ostream& operator<<(std::ostream& os, const pose_in_frame& v) { | ||
| os << "{ pose: " << v.pose << ",\n" | ||
| << " reference_frame: " << v.reference_frame << "}"; | ||
| return os; | ||
| } | ||
|
|
||
| } // 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 @@ | ||
| #pragma once | ||
|
|
||
| #include <common/v1/common.pb.h> | ||
|
|
||
| namespace viam { | ||
| namespace sdk { | ||
|
|
||
| struct coordinates { | ||
| double x, y, z; | ||
| friend bool operator==(const coordinates& lhs, const coordinates& rhs); | ||
| }; | ||
|
|
||
| struct pose_orientation { | ||
| double o_x, o_y, o_z; | ||
| friend bool operator==(const pose_orientation& lhs, const pose_orientation& rhs); | ||
| }; | ||
|
|
||
| struct pose { | ||
| struct coordinates coordinates; | ||
| pose_orientation orientation; | ||
| double theta; | ||
|
|
||
| static pose from_proto(const viam::common::v1::Pose& proto); | ||
| viam::common::v1::Pose to_proto() const; | ||
|
|
||
| friend bool operator==(const pose& lhs, const pose& rhs); | ||
| friend std::ostream& operator<<(std::ostream& os, const pose& v); | ||
| }; | ||
|
|
||
| struct pose_in_frame { | ||
| viam::common::v1::PoseInFrame to_proto() const; | ||
| static pose_in_frame from_proto(const viam::common::v1::PoseInFrame& proto); | ||
| pose_in_frame(std::string reference_frame_, struct pose pose_) | ||
| : reference_frame(std::move(reference_frame_)), pose(std::move(pose_)) {} | ||
| pose_in_frame() {} | ||
|
|
||
| std::string reference_frame; | ||
| struct pose pose; | ||
| friend bool operator==(const pose_in_frame& lhs, const pose_in_frame& rhs); | ||
| friend std::ostream& operator<<(std::ostream& os, const pose_in_frame& v); | ||
| }; | ||
|
|
||
| } // 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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,98 @@ | ||
| #include <viam/sdk/common/world_state.hpp> | ||
|
|
||
| #include <common/v1/common.pb.h> | ||
|
|
||
| #include <viam/sdk/common/pose.hpp> | ||
|
|
||
| namespace viam { | ||
| namespace sdk { | ||
|
|
||
| WorldState::geometries_in_frame WorldState::geometries_in_frame::from_proto( | ||
| const common::v1::GeometriesInFrame& proto) { | ||
| geometries_in_frame gif; | ||
| for (const auto& geo : proto.geometries()) { | ||
| gif.geometries.push_back(GeometryConfig::from_proto(geo)); | ||
| } | ||
| gif.reference_frame = proto.reference_frame(); | ||
|
|
||
| return gif; | ||
| } | ||
|
|
||
| common::v1::GeometriesInFrame WorldState::geometries_in_frame::to_proto() const { | ||
| common::v1::GeometriesInFrame proto; | ||
|
|
||
| *proto.mutable_reference_frame() = reference_frame; | ||
| for (const auto& geometry : geometries) { | ||
| *proto.mutable_geometries()->Add() = geometry.to_proto(); | ||
| } | ||
|
|
||
| return proto; | ||
| } | ||
|
|
||
| WorldState WorldState::from_proto(const common::v1::WorldState& ws) { | ||
| const auto& proto_obstacles = ws.obstacles(); | ||
| std::vector<geometries_in_frame> obstacles; | ||
| for (const auto& po : proto_obstacles) { | ||
| obstacles.push_back(geometries_in_frame::from_proto(po)); | ||
| } | ||
|
|
||
| const auto& proto_transforms = ws.transforms(); | ||
| std::vector<transform> transforms; | ||
| for (const auto& pt : proto_transforms) { | ||
| transforms.push_back(WorldState::transform::from_proto(pt)); | ||
| } | ||
|
|
||
| return {obstacles, transforms}; | ||
| } | ||
|
|
||
| common::v1::WorldState WorldState::to_proto() const { | ||
| common::v1::WorldState proto_ws; | ||
| for (const auto& obstacle : obstacles_) { | ||
| *proto_ws.mutable_obstacles()->Add() = obstacle.to_proto(); | ||
| } | ||
| for (const auto& transform : transforms_) { | ||
| *proto_ws.mutable_transforms()->Add() = transform.to_proto(); | ||
| } | ||
|
|
||
| return proto_ws; | ||
| } | ||
|
|
||
| WorldState::transform WorldState::transform::from_proto(const common::v1::Transform& proto) { | ||
| WorldState::transform transform; | ||
| transform.reference_frame = proto.reference_frame(); | ||
| transform.pose_in_observer_frame = pose_in_frame::from_proto(proto.pose_in_observer_frame()); | ||
| if (proto.has_physical_object()) { | ||
| transform.physical_object = | ||
| std::make_shared<GeometryConfig>(GeometryConfig::from_proto(proto.physical_object())); | ||
| } | ||
|
|
||
| return transform; | ||
| } | ||
|
|
||
| common::v1::Transform WorldState::transform::to_proto() const { | ||
| common::v1::Transform proto; | ||
| *proto.mutable_reference_frame() = reference_frame; | ||
| *proto.mutable_pose_in_observer_frame() = pose_in_observer_frame.to_proto(); | ||
| if (physical_object) { | ||
| *proto.mutable_physical_object() = physical_object->to_proto(); | ||
| } | ||
|
|
||
| return proto; | ||
| } | ||
| bool operator==(const WorldState::geometries_in_frame& lhs, | ||
| const WorldState::geometries_in_frame& rhs) { | ||
| return lhs.reference_frame == rhs.reference_frame && lhs.geometries == rhs.geometries; | ||
| } | ||
| bool operator==(const WorldState::transform& lhs, const WorldState::transform& rhs) { | ||
| return lhs.reference_frame == rhs.reference_frame && | ||
| (lhs.physical_object == rhs.physical_object || | ||
| *lhs.physical_object == *rhs.physical_object) && | ||
| lhs.pose_in_observer_frame == rhs.pose_in_observer_frame; | ||
| } | ||
|
|
||
| bool operator==(const WorldState& lhs, const WorldState& rhs) { | ||
| return lhs.obstacles_ == rhs.obstacles_ && lhs.transforms_ == rhs.transforms_; | ||
| } | ||
|
|
||
| } // 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,48 @@ | ||
| #pragma once | ||
|
|
||
| #include <common/v1/common.pb.h> | ||
|
|
||
| #include <viam/sdk/common/pose.hpp> | ||
| #include <viam/sdk/spatialmath/geometry.hpp> | ||
|
|
||
| namespace viam { | ||
| namespace sdk { | ||
|
|
||
| // TODO(RSDK-4553) Add documentation for these types | ||
| class WorldState { | ||
| public: | ||
| struct geometries_in_frame { | ||
| std::vector<GeometryConfig> geometries; | ||
| std::string reference_frame; | ||
| common::v1::GeometriesInFrame to_proto() const; | ||
| static geometries_in_frame from_proto(const common::v1::GeometriesInFrame& proto); | ||
| }; | ||
|
|
||
| struct transform { | ||
| std::string reference_frame; | ||
| pose_in_frame pose_in_observer_frame; | ||
| std::shared_ptr<GeometryConfig> physical_object; | ||
|
|
||
| common::v1::Transform to_proto() const; | ||
| static transform from_proto(const common::v1::Transform& proto); | ||
| transform() {} | ||
| }; | ||
|
|
||
| common::v1::WorldState to_proto() const; | ||
| static WorldState from_proto(const common::v1::WorldState& ws); | ||
|
|
||
| WorldState() {} | ||
| WorldState(std::vector<geometries_in_frame> obstacles, std::vector<transform> transforms) | ||
| : obstacles_(obstacles), transforms_(transforms) {} | ||
|
|
||
| friend bool operator==(const WorldState& lhs, const WorldState& rhs); | ||
| friend bool operator==(const geometries_in_frame& lhs, const geometries_in_frame& rhs); | ||
| friend bool operator==(const transform& lhs, const transform& rhs); | ||
|
|
||
| private: | ||
| std::vector<geometries_in_frame> obstacles_; | ||
| std::vector<transform> transforms_; | ||
| }; | ||
|
|
||
| } // namespace sdk | ||
| } // namespace viam |
Oops, something went wrong.