diff --git a/src/viam/sdk/services/motion/client.cpp b/src/viam/sdk/services/motion/client.cpp index 314641f7d..1303eb78d 100644 --- a/src/viam/sdk/services/motion/client.cpp +++ b/src/viam/sdk/services/motion/client.cpp @@ -45,6 +45,7 @@ std::string MotionClient::move_on_map( const Name& component_name, const Name& slam_name, const std::shared_ptr& motion_configuration, + const std::vector& obstacles, const AttributeMap& extra) { return make_client_helper(this, *stub_, &StubType::MoveOnMap) .with(extra, @@ -52,6 +53,11 @@ std::string MotionClient::move_on_map( *request.mutable_destination() = destination.to_proto(); *request.mutable_component_name() = component_name.to_proto(); *request.mutable_slam_service_name() = slam_name.to_proto(); + + for (const auto& obstacle : obstacles) { + *request.mutable_obstacles()->Add() = obstacle.to_proto(); + } + if (motion_configuration) { *request.mutable_motion_configuration() = motion_configuration->to_proto(); } diff --git a/src/viam/sdk/services/motion/client.hpp b/src/viam/sdk/services/motion/client.hpp index 5bebd3564..6ac24c04d 100644 --- a/src/viam/sdk/services/motion/client.hpp +++ b/src/viam/sdk/services/motion/client.hpp @@ -29,6 +29,7 @@ class MotionClient : public Motion { const Name& component_name, const Name& slam_name, const std::shared_ptr& motion_configuration, + const std::vector& obstacles, const AttributeMap& extra) override; std::string move_on_globe(const geo_point& destination, diff --git a/src/viam/sdk/services/motion/motion.hpp b/src/viam/sdk/services/motion/motion.hpp index 98297555e..53d3d4a4a 100644 --- a/src/viam/sdk/services/motion/motion.hpp +++ b/src/viam/sdk/services/motion/motion.hpp @@ -265,8 +265,10 @@ class Motion : public Service { const pose& destination, const Name& component_name, const Name& slam_name, - const std::shared_ptr& motion_configuration) { - return move_on_map(destination, component_name, slam_name, motion_configuration, {}); + const std::shared_ptr& motion_configuration, + const std::vector& obstacles) { + return move_on_map( + destination, component_name, slam_name, motion_configuration, obstacles, {}); } /// @brief Moves any component on the robot to a specific destination on a SLAM map. @@ -281,6 +283,7 @@ class Motion : public Service { const Name& component_name, const Name& slam_name, const std::shared_ptr& motion_configuration, + const std::vector& obstacles, const AttributeMap& extra) = 0; /// @brief Moves any component on the robot to a specific destination on a globe. diff --git a/src/viam/sdk/services/motion/server.cpp b/src/viam/sdk/services/motion/server.cpp index 21c7048b5..056a4b81b 100644 --- a/src/viam/sdk/services/motion/server.cpp +++ b/src/viam/sdk/services/motion/server.cpp @@ -56,8 +56,14 @@ ::grpc::Status MotionServer::MoveOnMap( mc = std::make_shared( motion_configuration::from_proto(request->motion_configuration())); } - const std::string execution_id = - motion->move_on_map(destination, component_name, slam_name, mc, helper.getExtra()); + + std::vector obstacles; + for (const auto& obstacle : request->obstacles()) { + obstacles.push_back(GeometryConfig::from_proto(obstacle)); + } + + const std::string execution_id = motion->move_on_map( + destination, component_name, slam_name, mc, obstacles, helper.getExtra()); *response->mutable_execution_id() = execution_id; }); diff --git a/src/viam/sdk/tests/mocks/mock_motion.cpp b/src/viam/sdk/tests/mocks/mock_motion.cpp index ee2bc45a5..79a595b92 100644 --- a/src/viam/sdk/tests/mocks/mock_motion.cpp +++ b/src/viam/sdk/tests/mocks/mock_motion.cpp @@ -31,11 +31,14 @@ std::string MockMotion::move_on_map( const Name& component_name, const Name& slam_name, const std::shared_ptr& motion_configuration, + const std::vector& obstacles, const AttributeMap&) { this->peek_current_pose = destination; this->peek_component_name = component_name; this->peek_slam_name = slam_name; this->current_location.pose = destination; + this->peek_motion_configuration = motion_configuration; + this->peek_map_obstacles = obstacles; return "execution-id"; } diff --git a/src/viam/sdk/tests/mocks/mock_motion.hpp b/src/viam/sdk/tests/mocks/mock_motion.hpp index c18a191ca..a57d51fb1 100644 --- a/src/viam/sdk/tests/mocks/mock_motion.hpp +++ b/src/viam/sdk/tests/mocks/mock_motion.hpp @@ -32,6 +32,7 @@ class MockMotion : public sdk::Motion { const sdk::Name& component_name, const sdk::Name& slam_name, const std::shared_ptr& motion_configuration, + const std::vector& obstacles, const sdk::AttributeMap& extra) override; std::string move_on_globe( @@ -89,6 +90,7 @@ class MockMotion : public sdk::Motion { double peek_heading; bool peek_stop_plan_called = false; std::vector peek_obstacles; + std::vector peek_map_obstacles; std::shared_ptr peek_constraints; std::shared_ptr peek_motion_configuration; std::shared_ptr peek_world_state; diff --git a/src/viam/sdk/tests/test_motion.cpp b/src/viam/sdk/tests/test_motion.cpp index 91d38d09e..7f571b746 100644 --- a/src/viam/sdk/tests/test_motion.cpp +++ b/src/viam/sdk/tests/test_motion.cpp @@ -17,6 +17,7 @@ BOOST_TEST_DONT_PRINT_LOG_VALUE(viam::sdk::Motion::plan_status_with_id) BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector) BOOST_TEST_DONT_PRINT_LOG_VALUE(viam::sdk::Motion::plan_with_status) BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector) +BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector) namespace viam { namespace sdktests { @@ -48,7 +49,7 @@ BOOST_AUTO_TEST_CASE(mock_get_api) { BOOST_AUTO_TEST_CASE(mock_move_and_get_pose) { std::shared_ptr motion = MockMotion::get_mock_motion(); - BOOST_CHECK_EQUAL(motion->current_location, std::move(init_fake_pose())); + BOOST_CHECK_EQUAL(motion->current_location, init_fake_pose()); Name fake_name({"acme", "service", "motion"}, "fake-remote", "fake-motion"); auto ws = std::make_shared(mock_world_state()); @@ -57,14 +58,14 @@ BOOST_AUTO_TEST_CASE(mock_move_and_get_pose) { BOOST_TEST(success); BOOST_CHECK_EQUAL(motion->current_location, fake_pose()); - BOOST_CHECK_EQUAL(motion->peek_component_name, std::move(fake_name)); + BOOST_CHECK_EQUAL(motion->peek_component_name, fake_name); BOOST_CHECK_EQUAL(*(motion->peek_world_state), *ws); } BOOST_AUTO_TEST_CASE(mock_move_on_map) { std::shared_ptr motion = MockMotion::get_mock_motion(); - BOOST_CHECK_EQUAL(motion->current_location, std::move(init_fake_pose())); + BOOST_CHECK_EQUAL(motion->current_location, init_fake_pose()); pose new_destination({{3, 4, 5}, {6, 7, 8}, 9}); @@ -72,12 +73,15 @@ BOOST_AUTO_TEST_CASE(mock_move_on_map) { fake_component_name(), fake_slam_name(), fake_motion_configuration(), + fake_geometries(), fake_map()); BOOST_CHECK_EQUAL(execution_id, "execution-id"); BOOST_CHECK_EQUAL(motion->peek_current_pose, std::move(new_destination)); BOOST_CHECK_EQUAL(motion->peek_component_name, fake_component_name()); BOOST_CHECK_EQUAL(motion->peek_slam_name, fake_slam_name()); + BOOST_CHECK_EQUAL(*(motion->peek_motion_configuration), *(fake_motion_configuration())); + BOOST_CHECK_EQUAL(motion->peek_map_obstacles, fake_geometries()); } BOOST_AUTO_TEST_CASE(mock_move_on_globe) { @@ -187,6 +191,7 @@ BOOST_AUTO_TEST_CASE(test_move_on_map) { fake_component_name(), fake_slam_name(), fake_motion_configuration(), + fake_geometries(), fake_map()); BOOST_CHECK_EQUAL(execution_id, "execution-id");