Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
4 changes: 2 additions & 2 deletions .clang-tidy
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@
# readability-implicit-bool-conversion: We have decided that !ptr-type is cleaner than ptr-type==nullptr
# readability-magic-numbers: This encourages useless variables and extra lint lines
# performance-unnecessary-value-param: TODO(RSDK-3007) remove this and fix all lints.
# performance-move-const-arg: TODO(RSDK-3019)
# misc-unused-parameters: TODO(RSDK-3008) remove this and fix all lints.
# misc-include-cleaner: TODO(RSDK-5479) this is overly finnicky, add IWYU support and fix.
# readability-function-cognitive-complexity: No, complexity is subjective and sometimes necessary.
Expand All @@ -29,7 +28,6 @@ Checks: >
-readability-implicit-bool-conversion,
-readability-magic-numbers,
-performance-unnecessary-value-param,
-performance-move-const-arg,
-misc-unused-parameters,
-misc-include-cleaner,
-readability-function-cognitive-complexity,
Expand All @@ -41,3 +39,5 @@ CheckOptions:
value: true
- key: readability-function-cognitive-complexity.Threshold
value: 30
- key: performance-move-const-arg.CheckTriviallyCopyableMove
value: false
6 changes: 3 additions & 3 deletions src/viam/sdk/registry/registry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ const Model& ModelRegistration::model() const {
};

void Registry::register_model(std::shared_ptr<const ModelRegistration> resource) {
const std::string reg_key = resource->api().to_string() + "/" + resource->model().to_string();
std::string reg_key = resource->api().to_string() + "/" + resource->model().to_string();
if (resources_.find(reg_key) != resources_.end()) {
const std::string err = "Cannot add resource with name " + reg_key + "as it already exists";
throw std::runtime_error(err);
Expand Down Expand Up @@ -95,14 +95,14 @@ std::shared_ptr<const ModelRegistration> Registry::lookup_model_inlock_(

std::shared_ptr<const ModelRegistration> Registry::lookup_model(const std::string& name) {
const std::lock_guard<std::mutex> lock(lock_);
return lookup_model_inlock_(name, std::move(lock));
return lookup_model_inlock_(name, lock);
}

std::shared_ptr<const ModelRegistration> Registry::lookup_model(const API& api,
const Model& model) {
const std::lock_guard<std::mutex> lock(lock_);
const std::string name = api.to_string() + "/" + model.to_string();
return lookup_model_inlock_(std::move(name), std::move(lock));
return lookup_model_inlock_(name, lock);
}

std::shared_ptr<const ResourceServerRegistration> Registry::lookup_resource_server(const API& api) {
Expand Down
4 changes: 2 additions & 2 deletions src/viam/sdk/resource/resource_api.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -200,12 +200,12 @@ bool operator==(const Model& lhs, const Model& rhs) {
RPCSubtype::RPCSubtype(API api,
std::string proto_service_name,
const google::protobuf::ServiceDescriptor& descriptor)
: descriptor_(std::move(descriptor)),
: descriptor_(descriptor),
proto_service_name_(std::move(proto_service_name)),
api_(std::move(api)) {}

RPCSubtype::RPCSubtype(API api, const google::protobuf::ServiceDescriptor& descriptor)
: descriptor_(std::move(descriptor)), api_(std::move(api)) {}
: descriptor_(descriptor), api_(std::move(api)) {}

const std::string& RPCSubtype::proto_service_name() const {
return proto_service_name_;
Expand Down
29 changes: 14 additions & 15 deletions src/viam/sdk/resource/resource_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -65,36 +65,35 @@ std::string get_shortcut_name(const std::string& name) {
return name_split.at(name_split.size() - 1);
}

void ResourceManager::do_add(const Name& name, const std::shared_ptr<Resource>& resource) {
void ResourceManager::do_add(const Name& name, std::shared_ptr<Resource> resource) {
if (name.name().empty()) {
throw "Empty name used for resource: " + name.to_string();
}
const std::string short_name = name.short_name();
std::string short_name = name.short_name();

do_add(short_name, resource);
do_add(std::move(short_name), std::move(resource));
}

void ResourceManager::do_add(const std::string& name, const std::shared_ptr<Resource>& resource) {
void ResourceManager::do_add(std::string name, std::shared_ptr<Resource> resource) {
if (resources_.find(name) != resources_.end()) {
throw "Attempted to add resource that already existed: " + name;
}

resources_.emplace(name, resource);

const std::string shortcut = get_shortcut_name(name);
std::string shortcut = get_shortcut_name(name);
if (shortcut != name) {
if (short_names_.find(shortcut) != short_names_.end()) {
short_names_.emplace(shortcut, "");
short_names_.emplace(std::move(shortcut), "");
} else {
short_names_.emplace(shortcut, name);
short_names_.emplace(std::move(shortcut), name);
}
}
resources_.emplace(std::move(name), std::move(resource));
}

void ResourceManager::add(const Name& name, const std::shared_ptr<Resource>& resource) {
void ResourceManager::add(const Name& name, std::shared_ptr<Resource> resource) {
const std::lock_guard<std::mutex> lock(lock_);
try {
do_add(name, resource);
do_add(name, std::move(resource));
} catch (std::exception& exc) {
BOOST_LOG_TRIVIAL(error) << "Error adding resource to subtype service: " << exc.what();
}
Expand Down Expand Up @@ -135,11 +134,11 @@ void ResourceManager::remove(const Name& name) {
};
};

void ResourceManager::replace_one(const Name& name, const std::shared_ptr<Resource>& resource) {
void ResourceManager::replace_one(const Name& name, std::shared_ptr<Resource> resource) {
const std::lock_guard<std::mutex> lock(lock_);
try {
do_remove(name);
do_add(name, resource);
do_add(name, std::move(resource));
} catch (std::exception& exc) {
BOOST_LOG_TRIVIAL(error) << "failed to replace resource " << name.to_string() << ": "
<< exc.what();
Expand All @@ -151,10 +150,10 @@ const std::unordered_map<std::string, std::shared_ptr<Resource>>& ResourceManage
return resources_;
}

void ResourceManager::add(const std::string& name, const std::shared_ptr<Resource>& resource) {
void ResourceManager::add(std::string name, std::shared_ptr<Resource> resource) {
const std::lock_guard<std::mutex> lock(lock_);
// NOLINTNEXTLINE(clang-analyzer-cplusplus.NewDeleteLeaks)
do_add(name, resource);
do_add(std::move(name), std::move(resource));
}

} // namespace sdk
Expand Down
10 changes: 5 additions & 5 deletions src/viam/sdk/resource/resource_manager.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,12 +44,12 @@ class ResourceManager {
/// @brief Adds a single resource to the manager.
/// @param name The name of the resource.
/// @param resource The resource being added.
void add(const Name& name, const std::shared_ptr<Resource>& resource);
void add(const Name& name, std::shared_ptr<Resource> resource);

/// @brief Adds a single resource to the manager.
/// @param name The name of the resource.
/// @param resource The resource being added.
void add(const std::string& name, const std::shared_ptr<Resource>& resource);
void add(std::string name, std::shared_ptr<Resource> resource);

/// @brief Remodes a single resource from the manager.
/// @param name The name of the resource to remove.
Expand All @@ -58,7 +58,7 @@ class ResourceManager {
/// @brief Replaces an existing resource. No-op if the named resource does not exist.
/// @param name The name of the resource to replace.
/// @param resource The new resource that is replacing the existing one.
void replace_one(const Name& name, const std::shared_ptr<Resource>& resource);
void replace_one(const Name& name, std::shared_ptr<Resource> resource);

/// @brief Returns a reference to the existing resources within the manager.
const std::unordered_map<std::string, std::shared_ptr<Resource>>& resources() const;
Expand All @@ -70,8 +70,8 @@ class ResourceManager {
std::unordered_map<std::string, std::shared_ptr<Resource>> resources_;
/// @brief `short_names_` is a shortened version of `Name` N of form <remote>:<name>.
std::unordered_map<std::string, std::string> short_names_;
void do_add(const Name& name, const std::shared_ptr<Resource>& resource);
void do_add(const std::string& name, const std::shared_ptr<Resource>& resource);
void do_add(const Name& name, std::shared_ptr<Resource> resource);
void do_add(std::string name, std::shared_ptr<Resource> resource);
void do_remove(const Name& name);
};

Expand Down
5 changes: 3 additions & 2 deletions src/viam/sdk/rpc/server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -47,8 +47,9 @@ void Server::add_resource(std::shared_ptr<Resource> resource) {
<< " but no matching resource server as found";
throw std::runtime_error(buffer.str());
}
auto resource_server = managed_servers_.at(std::move(api));
resource_server->resource_manager()->add(resource->name(), std::move(resource));
auto resource_server = managed_servers_.at(api);
auto name = resource->name();
resource_server->resource_manager()->add(std::move(name), std::move(resource));
}

void Server::start() {
Expand Down
4 changes: 4 additions & 0 deletions src/viam/sdk/services/mlmodel/client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,10 @@ std::shared_ptr<MLModelService::named_tensor_views> MLModelServiceClient::infer(
aav->views.emplace(kv.first, std::move(tensor));
}
auto* const tsav_views = &aav->views;
// This move does nothing pre-C++20 because the `shared_ptr` aliasing constructor takes
// its first arg by `const&`. However, having it here is harmless, and in C++20 this
// move would be correct to minimize refcount modification.
// NOLINTNEXTLINE(performance-move-const-arg)
return {std::move(aav), tsav_views};
}

Expand Down
5 changes: 2 additions & 3 deletions src/viam/sdk/services/mlmodel/server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -85,9 +85,8 @@ ::grpc::Status MLModelServiceServer::Metadata(
*metadata_pb.mutable_type() = std::move(md.type);
*metadata_pb.mutable_description() = std::move(md.description);

const auto pack_tensor_info = [&helper](
auto& target,
const std::vector<MLModelService::tensor_info>& source) {
const auto pack_tensor_info = [&helper](auto& target,
std::vector<MLModelService::tensor_info>& source) {
target.Reserve(source.size());
for (auto&& s : source) {
auto& new_entry = *target.Add();
Expand Down
2 changes: 1 addition & 1 deletion src/viam/sdk/services/motion/client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@ pose_in_frame MotionClient::get_pose(
.with(extra,
[&](auto& request) {
*request.mutable_component_name() = component_name.to_proto();
*request.mutable_destination_frame() = std::move(destination_frame);
*request.mutable_destination_frame() = destination_frame;
for (const auto& transform : supplemental_transforms) {
*request.mutable_supplemental_transforms()->Add() = transform.to_proto();
}
Expand Down
71 changes: 24 additions & 47 deletions src/viam/sdk/services/motion/server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,6 @@ ::grpc::Status MotionServer::Move(::grpc::ServerContext* context,
::viam::service::motion::v1::MoveResponse* response) noexcept {
return make_service_helper<Motion>(
"MotionServer::Move", this, request)([&](auto& helper, auto& motion) {
const pose_in_frame destination = pose_in_frame::from_proto(request->destination());
const Name name = Name::from_proto(request->component_name());
std::shared_ptr<WorldState> ws;
if (request->has_world_state()) {
ws = std::make_shared<WorldState>(WorldState::from_proto(request->world_state()));
Expand All @@ -34,8 +32,8 @@ ::grpc::Status MotionServer::Move(::grpc::ServerContext* context,
Motion::constraints::from_proto(request->constraints()));
}

const bool success = motion->move(std::move(destination),
std::move(name),
const bool success = motion->move(pose_in_frame::from_proto(request->destination()),
Name::from_proto(request->component_name()),
std::move(ws),
std::move(constraints),
helper.getExtra());
Expand All @@ -49,13 +47,11 @@ ::grpc::Status MotionServer::MoveOnMap(
::viam::service::motion::v1::MoveOnMapResponse* response) noexcept {
return make_service_helper<Motion>(
"MotionServer::MoveOnMap", this, request)([&](auto& helper, auto& motion) {
const auto& destination = pose::from_proto(request->destination());
const auto& component_name = Name::from_proto(request->component_name());
const auto& slam_name = Name::from_proto(request->slam_service_name());
const bool success = motion->move_on_map(std::move(destination),
std::move(component_name),
std::move(slam_name),
helper.getExtra());
const auto destination = pose::from_proto(request->destination());
auto component_name = Name::from_proto(request->component_name());
auto slam_name = Name::from_proto(request->slam_service_name());
const bool success = motion->move_on_map(
destination, std::move(component_name), std::move(slam_name), helper.getExtra());

response->set_success(success);
});
Expand All @@ -67,9 +63,9 @@ ::grpc::Status MotionServer::MoveOnGlobe(
::viam::service::motion::v1::MoveOnGlobeResponse* response) noexcept {
return make_service_helper<Motion>(
"MotionServer::MoveOnGlobe", this, request)([&](auto& helper, auto& motion) {
const auto& destination = geo_point::from_proto(request->destination());
const auto& component_name = Name::from_proto(request->component_name());
const auto& movement_sensor_name = Name::from_proto(request->movement_sensor_name());
const auto destination = geo_point::from_proto(request->destination());
auto component_name = Name::from_proto(request->component_name());
auto movement_sensor_name = Name::from_proto(request->movement_sensor_name());
std::vector<geo_obstacle> obstacles;

for (const auto& obstacle : request->obstacles()) {
Expand All @@ -87,8 +83,8 @@ ::grpc::Status MotionServer::MoveOnGlobe(
motion_configuration::from_proto(request->motion_configuration()));
}

const std::string execution_id = motion->move_on_globe(std::move(destination),
std::move(heading),
const std::string execution_id = motion->move_on_globe(destination,
heading,
std::move(component_name),
std::move(movement_sensor_name),
std::move(obstacles),
Expand All @@ -103,37 +99,18 @@ ::grpc::Status MotionServer::GetPose(
::grpc::ServerContext* context,
const ::viam::service::motion::v1::GetPoseRequest* request,
::viam::service::motion::v1::GetPoseResponse* response) noexcept {
if (!request) {
return ::grpc::Status(::grpc::StatusCode::INVALID_ARGUMENT,
"Called [GetPose] without a request");
};

const std::shared_ptr<Resource> rb = resource_manager()->resource(request->name());
if (!rb) {
return grpc::Status(grpc::UNKNOWN, "resource not found: " + request->name());
}

const std::shared_ptr<Motion> motion = std::dynamic_pointer_cast<Motion>(rb);

const auto& component_name = Name::from_proto(request->component_name());
const std::string& destination_frame = request->destination_frame();
std::vector<WorldState::transform> supplemental_transforms;
for (const auto& proto_transform : request->supplemental_transforms()) {
supplemental_transforms.push_back(WorldState::transform::from_proto(proto_transform));
}
AttributeMap extra;
if (request->has_extra()) {
extra = struct_to_map(request->extra());
}

const pose_in_frame pose = motion->get_pose(std::move(component_name),
std::move(destination_frame),
std::move(supplemental_transforms),
std::move(extra));

*response->mutable_pose() = pose.to_proto();

return ::grpc::Status();
return make_service_helper<Motion>(
"MotionServer::GetPose", this, request)([&](auto& helper, auto& motion) {
const auto& component_name = Name::from_proto(request->component_name());
const std::string& destination_frame = request->destination_frame();
std::vector<WorldState::transform> supplemental_transforms;
for (const auto& proto_transform : request->supplemental_transforms()) {
supplemental_transforms.push_back(WorldState::transform::from_proto(proto_transform));
}
const pose_in_frame pose = motion->get_pose(
component_name, destination_frame, supplemental_transforms, helper.getExtra());
*response->mutable_pose() = pose.to_proto();
});
};

::grpc::Status MotionServer::GetPlan(
Expand Down
2 changes: 1 addition & 1 deletion src/viam/sdk/spatialmath/geometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,7 @@ std::vector<GeometryConfig> GeometryConfig::from_proto(
const viam::common::v1::GetGeometriesResponse& proto) {
std::vector<GeometryConfig> response;
for (const auto& geometry : proto.geometries()) {
response.push_back(from_proto(std::move(geometry)));
response.push_back(from_proto(geometry));
}
return response;
}
Expand Down