diff --git a/src/viam/sdk/services/motion.hpp b/src/viam/sdk/services/motion.hpp index 8da6ee637..04b123163 100644 --- a/src/viam/sdk/services/motion.hpp +++ b/src/viam/sdk/services/motion.hpp @@ -179,6 +179,15 @@ class Motion : public Service { std::vector allows; }; + /// @struct pseudolinear_constraint + /// @brief Specifies that the component being moved should move pseudolinearly to its goal. + struct pseudolinear_constraint { + /// @brief The factor by which to multiply the default line tolerance. + boost::optional line_tolerance_factor; + /// @brief The factor by which to multiply the default orientation tolerance. + boost::optional orientation_tolerance_factor; + }; + /// @struct constraints /// @brief Specifies all constraints to be passed to Viam's motion planning, along /// with any optional parameters. @@ -186,6 +195,7 @@ class Motion : public Service { std::vector linear_constraints; std::vector orientation_constraints; std::vector collision_specifications; + std::vector pseudolinear_constraints; }; API api() const override; @@ -466,4 +476,4 @@ struct API::traits { }; } // namespace sdk -} // namespace viam +} // namespace viam \ No newline at end of file diff --git a/src/viam/sdk/services/private/motion_client.cpp b/src/viam/sdk/services/private/motion_client.cpp index eddcd959e..4e3dc9ca3 100644 --- a/src/viam/sdk/services/private/motion_client.cpp +++ b/src/viam/sdk/services/private/motion_client.cpp @@ -57,6 +57,17 @@ service::motion::v1::MotionConfiguration to_proto(const motion_configuration& mc return proto; } +service::motion::v1::PseudolinearConstraint to_proto(const Motion::pseudolinear_constraint& plc) { + service::motion::v1::PseudolinearConstraint proto; + if (plc.line_tolerance_factor) { + proto.set_line_tolerance_factor(*plc.line_tolerance_factor); + } + if (plc.orientation_tolerance_factor) { + proto.set_orientation_tolerance_factor(*plc.orientation_tolerance_factor); + } + return proto; +} + service::motion::v1::Constraints to_proto(const Motion::constraints& cs) { service::motion::v1::Constraints proto; for (const auto& lc : cs.linear_constraints) { @@ -83,6 +94,10 @@ service::motion::v1::Constraints to_proto(const Motion::constraints& cs) { *proto.mutable_collision_specification()->Add() = std::move(proto_cs); } + for (const auto& plc : cs.pseudolinear_constraints) { + *proto.mutable_pseudolinear_constraint()->Add() = to_proto(plc); + } + return proto; } diff --git a/src/viam/sdk/services/private/motion_server.cpp b/src/viam/sdk/services/private/motion_server.cpp index 9874dc604..7828b70f9 100644 --- a/src/viam/sdk/services/private/motion_server.cpp +++ b/src/viam/sdk/services/private/motion_server.cpp @@ -133,6 +133,17 @@ motion_configuration from_proto(const service::motion::v1::MotionConfiguration& return mc; } +Motion::pseudolinear_constraint from_proto(const service::motion::v1::PseudolinearConstraint& proto) { + Motion::pseudolinear_constraint plc; + if (proto.has_line_tolerance_factor()) { + plc.line_tolerance_factor = proto.line_tolerance_factor(); + } + if (proto.has_orientation_tolerance_factor()) { + plc.orientation_tolerance_factor = proto.orientation_tolerance_factor(); + } + return plc; +} + MotionServer::MotionServer(std::shared_ptr manager) : ResourceServer(std::move(manager)) {} @@ -166,10 +177,16 @@ Motion::constraints from_proto(const service::motion::v1::Constraints& proto) { css.push_back(cs); } + std::vector plcs; + for (const auto& proto_plc : proto.pseudolinear_constraint()) { + plcs.push_back(from_proto(proto_plc)); + } + Motion::constraints constraints; constraints.linear_constraints = lcs; constraints.orientation_constraints = ocs; constraints.collision_specifications = css; + constraints.pseudolineear_constraints = plcs; return constraints; } @@ -358,4 +375,4 @@ ::grpc::Status MotionServer::DoCommand(::grpc::ServerContext*, } // namespace impl } // namespace sdk -} // namespace viam +} // namespace viam \ No newline at end of file diff --git a/src/viam/sdk/tests/mocks/mock_motion.cpp b/src/viam/sdk/tests/mocks/mock_motion.cpp index 413d3cdb6..a93bc9fa4 100644 --- a/src/viam/sdk/tests/mocks/mock_motion.cpp +++ b/src/viam/sdk/tests/mocks/mock_motion.cpp @@ -180,6 +180,13 @@ std::vector fake_bounding_regions() { return {{fake_geo_point(), {std::move(gc)}}}; } +Motion::pseudolinear_constraint fake_pseudolinear_constraint() { + Motion::pseudolinear_constraint plc; + plc.line_tolerance_factor = 0.5f; + plc.orientation_tolerance_factor = 0.75f; + return plc; +} + } // namespace motion } // namespace sdktests -} // namespace viam +} // namespace viam \ No newline at end of file diff --git a/src/viam/sdk/tests/mocks/mock_motion.hpp b/src/viam/sdk/tests/mocks/mock_motion.hpp index 4990e006f..eda689b7c 100644 --- a/src/viam/sdk/tests/mocks/mock_motion.hpp +++ b/src/viam/sdk/tests/mocks/mock_motion.hpp @@ -20,6 +20,7 @@ sdk::geo_point fake_geo_point(); std::vector fake_obstacles(); std::shared_ptr fake_motion_configuration(); std::vector fake_bounding_regions(); +sdk::Motion::pseudolinear_constraint fake_pseudolinear_constraint(); class MockMotion : public sdk::Motion { public: @@ -105,4 +106,4 @@ class MockMotion : public sdk::Motion { } // namespace motion } // namespace sdktests -} // namespace viam +} // namespace viam \ No newline at end of file diff --git a/src/viam/sdk/tests/test_motion.cpp b/src/viam/sdk/tests/test_motion.cpp index 9e06c6aa4..a2a206fed 100644 --- a/src/viam/sdk/tests/test_motion.cpp +++ b/src/viam/sdk/tests/test_motion.cpp @@ -34,6 +34,14 @@ WorldState mock_world_state() { return WorldState({obstacle}, {transform}); } +// Add this function after `fake_bounding_regions()` or similar helper functions. +Motion::pseudolinear_constraint fake_pseudolinear_constraint() { + Motion::pseudolinear_constraint plc; + plc.line_tolerance_factor = 0.5f; + plc.orientation_tolerance_factor = 0.75f; + return plc; +} + BOOST_AUTO_TEST_SUITE(test_mock) BOOST_AUTO_TEST_CASE(mock_get_api) { @@ -175,11 +183,19 @@ BOOST_AUTO_TEST_CASE(test_move_and_get_pose) { BOOST_CHECK_EQUAL(pose, init_fake_pose()); auto ws = std::make_shared(mock_world_state()); - bool success = client.move(fake_pose(), fake_component_name(), ws, nullptr, fake_map()); + // Create a constraints object with the new pseudolinear constraint + auto constraints = std::make_shared(); + constraints->pseudolinear_constraints.push_back(fake_pseudolinear_constraint()); + bool success = client.move(fake_pose(), fake_component_name(), ws, constraints, fake_map()); // Pass the new constraints object BOOST_TEST(success); pose = client.get_pose(fake_component_name(), destination_frame, transforms, fake_map()); BOOST_CHECK_EQUAL(pose, fake_pose()); + // Add checks for the peek_constraints in the mock + BOOST_CHECK(mock->peek_constraints != nullptr); + BOOST_CHECK_EQUAL(mock->peek_constraints->pseudolinear_constraints.size(), 1); + BOOST_CHECK_CLOSE(mock->peek_constraints->pseudolinear_constraints[0].line_tolerance_factor.get(), fake_pseudolinear_constraint().line_tolerance_factor.get(), 0.0001); + BOOST_CHECK_CLOSE(mock->peek_constraints->pseudolinear_constraints[0].orientation_tolerance_factor.get(), fake_pseudolinear_constraint().orientation_tolerance_factor.get(), 0.0001); }); } @@ -281,4 +297,4 @@ BOOST_AUTO_TEST_SUITE_END() } // namespace motion } // namespace sdktests -} // namespace viam +} // namespace viam \ No newline at end of file