Skip to content

RSDK-6709 - update move_on_map #217

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Feb 22, 2024
Merged

RSDK-6709 - update move_on_map #217

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions src/viam/sdk/services/motion/client.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,13 +45,19 @@ std::string MotionClient::move_on_map(
const Name& component_name,
const Name& slam_name,
const std::shared_ptr<motion_configuration>& motion_configuration,
const std::vector<GeometryConfig>& obstacles,
Copy link
Member

@nfranczak nfranczak Feb 21, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

my knowledge of cpp is very limited so I want to confirm that vector means list.

Copy link
Member Author

@stuqdog stuqdog Feb 21, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yep! That's correct.

Copy link
Member

@acmorrow acmorrow Feb 22, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not exactly "list" - more like "dynamic array". Both std::vector and std::list are dynamically sized and bidirectionally iterable. The key difference is that std::vector is guaranteed contiguous, but insertions/deletions can cost reallocation / element-by-element copies, whereas std::list is non-contiguous, but insertions/deletions anywhere are cheap since contiguity isn't in play.

const AttributeMap& extra) {
return make_client_helper(this, *stub_, &StubType::MoveOnMap)
.with(extra,
[&](auto& request) {
*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) {
Copy link
Member

@nfranczak nfranczak Feb 21, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[q]: does anything special need to happen if the user chooses to not specify any obstacles e.g. use nil?

Copy link
Member Author

@stuqdog stuqdog Feb 21, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So a user couldn't pass nullptr or anything like that because the arg is not a pointer. If they wanted to not specify any obstacles they would want to pass an empty vec (trivial to do, just pass {} as the arg). Nothing special is required in that case.

*request.mutable_obstacles()->Add() = obstacle.to_proto();
}

if (motion_configuration) {
*request.mutable_motion_configuration() = motion_configuration->to_proto();
}
Expand Down
1 change: 1 addition & 0 deletions src/viam/sdk/services/motion/client.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@ class MotionClient : public Motion {
const Name& component_name,
const Name& slam_name,
const std::shared_ptr<motion_configuration>& motion_configuration,
const std::vector<GeometryConfig>& obstacles,
const AttributeMap& extra) override;

std::string move_on_globe(const geo_point& destination,
Expand Down
7 changes: 5 additions & 2 deletions src/viam/sdk/services/motion/motion.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>& motion_configuration) {
return move_on_map(destination, component_name, slam_name, motion_configuration, {});
const std::shared_ptr<motion_configuration>& motion_configuration,
const std::vector<GeometryConfig>& 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.
Expand All @@ -281,6 +283,7 @@ class Motion : public Service {
const Name& component_name,
const Name& slam_name,
const std::shared_ptr<motion_configuration>& motion_configuration,
const std::vector<GeometryConfig>& obstacles,
const AttributeMap& extra) = 0;

/// @brief Moves any component on the robot to a specific destination on a globe.
Expand Down
10 changes: 8 additions & 2 deletions src/viam/sdk/services/motion/server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,8 +56,14 @@ ::grpc::Status MotionServer::MoveOnMap(
mc = std::make_shared<motion_configuration>(
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<GeometryConfig> 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;
});
Expand Down
3 changes: 3 additions & 0 deletions src/viam/sdk/tests/mocks/mock_motion.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,11 +31,14 @@ std::string MockMotion::move_on_map(
const Name& component_name,
const Name& slam_name,
const std::shared_ptr<motion_configuration>& motion_configuration,
const std::vector<GeometryConfig>& 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";
}
Expand Down
2 changes: 2 additions & 0 deletions src/viam/sdk/tests/mocks/mock_motion.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ class MockMotion : public sdk::Motion {
const sdk::Name& component_name,
const sdk::Name& slam_name,
const std::shared_ptr<sdk::motion_configuration>& motion_configuration,
const std::vector<sdk::GeometryConfig>& obstacles,
const sdk::AttributeMap& extra) override;

std::string move_on_globe(
Expand Down Expand Up @@ -89,6 +90,7 @@ class MockMotion : public sdk::Motion {
double peek_heading;
bool peek_stop_plan_called = false;
std::vector<sdk::geo_obstacle> peek_obstacles;
std::vector<sdk::GeometryConfig> peek_map_obstacles;
std::shared_ptr<constraints> peek_constraints;
std::shared_ptr<sdk::motion_configuration> peek_motion_configuration;
std::shared_ptr<sdk::WorldState> peek_world_state;
Expand Down
11 changes: 8 additions & 3 deletions src/viam/sdk/tests/test_motion.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<viam::sdk::Motion::plan_status_with_id>)
BOOST_TEST_DONT_PRINT_LOG_VALUE(viam::sdk::Motion::plan_with_status)
BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector<viam::sdk::Motion::plan_with_status>)
BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector<viam::sdk::GeometryConfig>)

namespace viam {
namespace sdktests {
Expand Down Expand Up @@ -48,7 +49,7 @@ BOOST_AUTO_TEST_CASE(mock_get_api) {
BOOST_AUTO_TEST_CASE(mock_move_and_get_pose) {
std::shared_ptr<MockMotion> 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<WorldState>(mock_world_state());
Expand All @@ -57,27 +58,30 @@ 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<MockMotion> 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});

std::string execution_id = motion->move_on_map(new_destination,
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) {
Expand Down Expand Up @@ -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");
Expand Down