AI update based on proto changes

src/viam/sdk/services/motion.hpp

@@ -179,13 +179,23 @@ class Motion : public Service {
         std::vector<allowed_frame_collisions> 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<float> line_tolerance_factor;
+        /// @brief The factor by which to multiply the default orientation tolerance.
+        boost::optional<float> orientation_tolerance_factor;
+    };
+
     /// @struct constraints
     /// @brief Specifies all constraints to be passed to Viam's motion planning, along
     /// with any optional parameters.
     struct constraints {
         std::vector<linear_constraint> linear_constraints;
         std::vector<orientation_constraint> orientation_constraints;
         std::vector<collision_specification> collision_specifications;
+        std::vector<pseudolinear_constraint> pseudolinear_constraints;
     };
 
     API api() const override;
@@ -466,4 +476,4 @@ struct API::traits<Motion> {
 };
 
 }  // namespace sdk
-}  // namespace viam
+}  // namespace viam

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;
 }
 

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<ResourceManager> 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<Motion::pseudolinear_constraint> 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

src/viam/sdk/tests/mocks/mock_motion.cpp

@@ -180,6 +180,13 @@ std::vector<geo_geometry> 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

src/viam/sdk/tests/mocks/mock_motion.hpp

@@ -20,6 +20,7 @@ sdk::geo_point fake_geo_point();
 std::vector<sdk::geo_geometry> fake_obstacles();
 std::shared_ptr<sdk::motion_configuration> fake_motion_configuration();
 std::vector<sdk::geo_geometry> 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

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<WorldState>(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<Motion::constraints>();
+        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