diff --git a/CMakeLists.txt b/CMakeLists.txt index 59ce01c5..7d66cbef 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ project(mrover VERSION 2025.0.0 LANGUAGES C CXX) set(CMAKE_CXX_STANDARD 23) if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wno-missing-field-initializers) endif () ## Dependencies @@ -32,9 +32,14 @@ find_package(TBB REQUIRED) find_package(manif QUIET) find_package(dawn QUIET) +find_package(PkgConfig REQUIRED) +pkg_search_module(NetLink libnl-3.0 QUIET) +pkg_search_module(NetLinkRoute libnl-route-3.0 QUIET) + add_subdirectory(deps/imgui EXCLUDE_FROM_ALL SYSTEM) add_subdirectory(deps/webgpuhpp EXCLUDE_FROM_ALL SYSTEM) add_subdirectory(deps/glfw3webgpu EXCLUDE_FROM_ALL SYSTEM) +add_subdirectory(deps/mjbots EXCLUDE_FROM_ALL SYSTEM) if (BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -108,6 +113,14 @@ target_link_libraries(arm_controller lie) ament_target_dependencies(arm_controller rclcpp) rosidl_target_interfaces(arm_controller ${PROJECT_NAME} rosidl_typesupport_cpp) +if (NetLink_FOUND AND NetLinkRoute_FOUND) + mrover_add_node(can_bridge esw/can_bridge/*.cpp esw/can_bridge/pch.hpp) + target_link_libraries(can_bridge ${NetLink_LIBRARIES} ${NetLinkRoute_LIBRARIES}) + target_include_directories(can_bridge PRIVATE ${NetLink_INCLUDE_DIRS} ${NetLinkRoute_INCLUDE_DIRS}) + ament_target_dependencies(can_bridge rclcpp) + rosidl_target_interfaces(can_bridge ${PROJECT_NAME} rosidl_typesupport_cpp) +endif () + ## Install install(PROGRAMS diff --git a/deps/mjbots/CMakeLists.txt b/deps/mjbots/CMakeLists.txt new file mode 100644 index 00000000..b5d11d5a --- /dev/null +++ b/deps/mjbots/CMakeLists.txt @@ -0,0 +1,5 @@ +cmake_minimum_required(VERSION 3.16) +project(moteus VERSION 2024.5.20 LANGUAGES CXX) + +add_library(moteus INTERFACE) +target_include_directories(moteus INTERFACE ${CMAKE_CURRENT_LIST_DIR}) diff --git a/deps/mjbots/README.md b/deps/mjbots/README.md new file mode 100644 index 00000000..2a115621 --- /dev/null +++ b/deps/mjbots/README.md @@ -0,0 +1,5 @@ +# Moteus Library + +This is the official mjbots moteus library obtained from: https://github.com/mjbots/moteus + +Everything except the C++ library has been stripped \ No newline at end of file diff --git a/deps/mjbots/moteus/moteus.h b/deps/mjbots/moteus/moteus.h new file mode 100644 index 00000000..058a9274 --- /dev/null +++ b/deps/mjbots/moteus/moteus.h @@ -0,0 +1,1034 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "moteus_optional.h" +#include "moteus_protocol.h" +#include "moteus_tokenizer.h" +#include "moteus_transport.h" + +namespace mjbots { +namespace moteus { + + +/// This is the primary interface to a moteus controller. One +/// instance of this class should be created per controller that is +/// commanded or monitored. +/// +/// The primary control functions each have 3 possible forms: +/// +/// 1. A "Make" variant which constructs a CanFdFrame to be used in a +/// later call to Transport::Cycle. +/// +/// 2. A "Set" variant which sends a command to the controller and +/// waits for a response in a blocking manner. +/// +/// 3. An "Async" variant which starts the process of sending a +/// command and potentially waiting for a response. When this +/// operation is finished, a user-provided callback is invoked. +/// This callback may be called either: +/// a) from an arbitrary thread +/// b) recursively from the calling thread before returning +/// +/// While any async operation is outstanding, it is undefined behavior +/// to start another async operation or execute a blocking operation. +class Controller { + public: + struct Options { + // The ID of the servo to communicate with. + int id = 1; + + // The source ID to use for the commanding node (i.e. the host or + // master). + int source = 0; + + // Which CAN bus to send commands on and look for responses on. + // This may not be used on all transports. + int bus = 0; + + // For each possible primary command, the resolution for all + // command fields has a default set at construction time. + Query::Format query_format; + PositionMode::Format position_format; + VFOCMode::Format vfoc_format; + CurrentMode::Format current_format; + StayWithinMode::Format stay_within_format; + + // Use the given prefix for all CAN IDs. + uint32_t can_prefix = 0x0000; + + // Request the configured set of registers as a query with every + // command. + bool default_query = true; + + int64_t diagnostic_retry_sleep_ns = 200000; + + // Specify a transport to be used. If left unset, a global common + // transport will be constructed to be shared with all Controller + // instances in this process. That will attempt to auto-detect a + // reasonable transport on the system. + std::shared_ptr transport; + + Options() {} + }; + + Controller(const Options& options = {}) : options_(options) { + transport_ = options.transport; + + WriteCanData query_write(&query_frame_); + query_reply_size_ = Query::Make(&query_write, options_.query_format); + } + + const Options& options() const { return options_; } + + Transport* transport() { + if (!transport_) { + transport_ = MakeSingletonTransport({}); + } + + return transport_.get(); + } + + /// Make a transport given the cmdline arguments. + static std::shared_ptr MakeSingletonTransport( + const std::vector& args) { + auto result = ArgumentProcessHelper(args); + return result.first; + } + + /// Require that a default global transport have already been + /// created and return it. + static std::shared_ptr RequireSingletonTransport() { + auto& g_transport = *GlobalTransport(); + if (!g_transport) { + throw std::logic_error("Unexpectedly cannot find global transport"); + } + return g_transport; + } + + struct Result { + CanFdFrame frame; + Query::Result values; + }; + + + ///////////////////////////////////////// + // Query + + CanFdFrame MakeQuery(const Query::Format* format_override = nullptr) { + // We force there to always be an override for the query format, + // because if we're directly asking for a query, we should get it + // no matter the Options::default_query state. + return MakeFrame(EmptyMode(), {}, {}, + format_override == nullptr ? + &options_.query_format : format_override); + } + + // This is prefixed "Set" despite the fact that it sets nothing + // because (a) it is consistent with the other methods with + // "Make/Set/Async" prefixes, and (b) it would otherwise shadow the + // mjbots::moteus::Query structure. + Optional SetQuery(const Query::Format* format_override = nullptr) { + return ExecuteSingleCommand(MakeQuery(format_override)); + } + + void AsyncQuery(Result* result, CompletionCallback callback, + const Query::Format* format_override = nullptr) { + AsyncStartSingleCommand(MakeQuery(format_override), result, callback); + } + + + ///////////////////////////////////////// + // StopMode + + CanFdFrame MakeStop(const Query::Format* query_override = nullptr) { + return MakeFrame(StopMode(), {}, {}, query_override); + } + + Optional SetStop(const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand(MakeStop(query_override)); + } + + void AsyncStop(Result* result, CompletionCallback callback, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand(MakeStop(query_override), result, callback); + } + + + ///////////////////////////////////////// + // BrakeMode + + CanFdFrame MakeBrake(const Query::Format* query_override = nullptr) { + return MakeFrame(BrakeMode(), {}, {}, query_override); + } + + Optional SetBrake(const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand(MakeBrake(query_override)); + } + + void AsyncBrake(Result* result, CompletionCallback callback, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand(MakeBrake(query_override), result, callback); + } + + + ///////////////////////////////////////// + // PositionMode + + CanFdFrame MakePosition(const PositionMode::Command& cmd, + const PositionMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame( + PositionMode(), cmd, + (command_override == nullptr ? + options_.position_format : *command_override), + query_override); + } + + Optional SetPosition( + const PositionMode::Command& cmd, + const PositionMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakePosition(cmd, command_override, query_override)); + } + + void AsyncPosition(const PositionMode::Command& cmd, + Result* result, CompletionCallback callback, + const PositionMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand(MakePosition(cmd, command_override, query_override), + result, callback); + } + + /// Repeatedly send a position command until the reported + /// trajectory_complete flag is true. This will always enable a + /// query and will return the result of the final such response. + Optional SetPositionWaitComplete( + const PositionMode::Command& cmd, + double period_s, + const PositionMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + Query::Format query_format = + query_override == nullptr ? options_.query_format : *query_override; + query_format.trajectory_complete = kInt8; + + int count = 2; + while (true) { + auto maybe_result = SetPosition(cmd, command_override, &query_format); + if (!!maybe_result) { count = std::max(count - 1, 0); } + + if (count == 0 && + !!maybe_result && + maybe_result->values.trajectory_complete) { + return *maybe_result; + } + + ::usleep(static_cast(period_s * 1e6)); + } + + return {}; + } + + + ///////////////////////////////////////// + // VFOCMode + + CanFdFrame MakeVFOC(const VFOCMode::Command& cmd, + const VFOCMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame( + VFOCMode(), cmd, + command_override == nullptr ? options_.vfoc_format : *command_override, + query_override); + } + + Optional SetVFOC(const VFOCMode::Command& cmd, + const VFOCMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand(MakeVFOC(cmd, command_override, query_override)); + } + + void AsyncVFOC(const VFOCMode::Command& cmd, + Result* result, CompletionCallback callback, + const VFOCMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand(MakeVFOC(cmd, command_override, query_override), + result, callback); + } + + + ///////////////////////////////////////// + // CurrentMode + + CanFdFrame MakeCurrent(const CurrentMode::Command& cmd, + const CurrentMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame(CurrentMode(), cmd, + (command_override == nullptr ? + options_.current_format : *command_override), + query_override); + } + + Optional SetCurrent( + const CurrentMode::Command& cmd, + const CurrentMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakeCurrent(cmd, command_override, query_override)); + } + + void AsyncCurrent(const CurrentMode::Command& cmd, + Result* result, CompletionCallback callback, + const CurrentMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand(MakeCurrent(cmd, command_override, query_override), + result, callback); + } + + + ///////////////////////////////////////// + // StayWithinMode + + CanFdFrame MakeStayWithin( + const StayWithinMode::Command& cmd, + const StayWithinMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame(StayWithinMode(), cmd, + (command_override == nullptr ? + options_.stay_within_format : *command_override), + query_override); + } + + Optional SetStayWithin( + const StayWithinMode::Command& cmd, + const StayWithinMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakeStayWithin(cmd, command_override, query_override)); + } + + void AsyncStayWithin(const StayWithinMode::Command& cmd, + Result* result, CompletionCallback callback, + const StayWithinMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand( + MakeStayWithin(cmd, command_override, query_override), result, callback); + } + + + ///////////////////////////////////////// + // OutputNearest + + CanFdFrame MakeOutputNearest(const OutputNearest::Command& cmd, + const OutputNearest::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame(OutputNearest(), cmd, + (command_override == nullptr ? + OutputNearest::Format() : *command_override), + query_override); + } + + Optional SetOutputNearest(const OutputNearest::Command& cmd, + const OutputNearest::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakeOutputNearest(cmd, command_override, query_override)); + } + + void AsyncOutputNearest(const OutputNearest::Command& cmd, + Result* result, CompletionCallback callback, + const OutputNearest::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand( + MakeOutputNearest(cmd, command_override, query_override), + result, callback); + } + + + ///////////////////////////////////////// + // OutputExact + + CanFdFrame MakeOutputExact(const OutputExact::Command& cmd, + const OutputExact::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame(OutputExact(), cmd, + (command_override == nullptr ? + OutputExact::Format() : *command_override), + query_override); + } + + Optional SetOutputExact(const OutputExact::Command& cmd, + const OutputExact::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakeOutputExact(cmd, command_override, query_override)); + } + + void AsyncOutputExact(const OutputExact::Command& cmd, + Result* result, CompletionCallback callback, + const OutputExact::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand( + MakeOutputExact(cmd, command_override, query_override), + result, callback); + } + + + ///////////////////////////////////////// + // RequireReindex + + CanFdFrame MakeRequireReindex(const RequireReindex::Command& cmd = {}, + const RequireReindex::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame(RequireReindex(), cmd, + (command_override == nullptr ? + RequireReindex::Format() : *command_override), + query_override); + } + + Optional SetRequireReindex(const RequireReindex::Command& cmd = {}, + const RequireReindex::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakeRequireReindex(cmd, command_override, query_override)); + } + + void AsyncRequireReindex(const RequireReindex::Command& cmd, + Result* result, CompletionCallback callback, + const RequireReindex::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand( + MakeRequireReindex(cmd, command_override, query_override), + result, callback); + } + + + ///////////////////////////////////////// + // RecapturePositionVelocity + + CanFdFrame MakeRecapturePositionVelocity( + const RecapturePositionVelocity::Command& cmd = {}, + const RecapturePositionVelocity::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame(RecapturePositionVelocity(), cmd, + (command_override == nullptr ? + RecapturePositionVelocity::Format() : *command_override), + query_override); + } + + Optional SetRecapturePositionVelocity( + const RecapturePositionVelocity::Command& cmd = {}, + const RecapturePositionVelocity::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakeRecapturePositionVelocity(cmd, command_override, query_override)); + } + + void AsyncRecapturePositionVelocity( + const RecapturePositionVelocity::Command& cmd, + Result* result, CompletionCallback callback, + const RecapturePositionVelocity::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand( + MakeRecapturePositionVelocity(cmd, command_override, query_override), + result, callback); + } + + + ///////////////////////////////////////// + // ClockTrim + + CanFdFrame MakeClockTrim(const ClockTrim::Command& cmd, + const ClockTrim::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame(ClockTrim(), cmd, + (command_override == nullptr ? + ClockTrim::Format() : *command_override), + query_override); + } + + Optional SetClockTrim(const ClockTrim::Command& cmd, + const ClockTrim::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakeClockTrim(cmd, command_override, query_override)); + } + + void AsyncClockTrim(const ClockTrim::Command& cmd, + Result* result, CompletionCallback callback, + const ClockTrim::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand( + MakeClockTrim(cmd, command_override, query_override), + result, callback); + } + + + ///////////////////////////////////////// + // Diagnostic channel operations + + enum DiagnosticReplyMode { + kExpectOK, + kExpectSingleLine, + }; + + std::string DiagnosticCommand(const std::string& message, + DiagnosticReplyMode reply_mode = kExpectOK) { + BlockingCallback cbk; + std::string response; + AsyncDiagnosticCommand(message, &response, cbk.callback(), reply_mode); + cbk.Wait(); + return response; + } + + void AsyncDiagnosticCommand(const std::string& message, + std::string* result, + CompletionCallback callback, + DiagnosticReplyMode reply_mode = kExpectOK) { + auto context = std::make_shared(); + context->result = result; + context->remaining_command = message + "\n"; + context->controller = this; + context->transport = transport(); + context->callback = callback; + context->reply_mode = reply_mode; + + context->Start(); + } + + void DiagnosticWrite(const std::string& message, int channel = 1) { + BlockingCallback cbk; + AsyncDiagnosticWrite(message, channel, cbk.callback()); + cbk.Wait(); + } + + void AsyncDiagnosticWrite(const std::string& message, + int channel, + CompletionCallback callback) { + auto context = std::make_shared(); + context->message = message; + context->channel = channel; + context->controller = this; + context->transport = transport(); + context->callback = callback; + + context->Start(); + } + + std::string DiagnosticRead(int channel) { + std::string response; + BlockingCallback cbk; + AsyncDiagnosticRead(&response, channel, cbk.callback()); + cbk.Wait(); + return response; + } + + void AsyncDiagnosticRead(std::string* response, + int channel, + CompletionCallback callback) { + DiagnosticRead::Command read; + read.channel = channel; + read.max_length = 48; + + output_frame_ = DefaultFrame(kReplyRequired); + WriteCanData write_frame(output_frame_.data, &output_frame_.size); + DiagnosticRead::Make(&write_frame, read, {}); + + struct Context { + std::string* response = nullptr; + std::vector replies; + CompletionCallback callback; + Transport* transport = nullptr; + }; + auto context = std::make_shared(); + context->response = response; + context->callback = callback; + context->transport = transport(); + + context->transport->Cycle( + &output_frame_, 1, &context->replies, + [context, this](int) { + std::string response; + bool any_response = false; + + for (const auto& frame : context->replies) { + if (frame.destination != options_.source || + frame.source != options_.id || + frame.can_prefix != options_.can_prefix) { + continue; + } + auto maybe_data = DiagnosticResponse::Parse(frame.data, frame.size); + response += std::string( + reinterpret_cast(maybe_data.data), maybe_data.size); + any_response = true; + } + + if (any_response) { + *context->response = response; + context->transport->Post(std::bind(context->callback, 0)); + return; + } + + context->transport->Post(std::bind(context->callback, ETIMEDOUT)); + return; + }); + } + + void DiagnosticFlush(int channel = 1, double timeout_s = 0.2) { + // Read until nothing is left or the timeout hits. + const auto timeout = timeout_s * 1000000000ll; + const auto start = Fdcanusb::GetNow(); + auto end_time = start + timeout; + + while (true) { + const auto response = DiagnosticRead(channel); + const auto now = Fdcanusb::GetNow(); + if (!response.empty()) { + // Every time we get something, bump out timeout further into + // the future. + end_time = now + timeout; + continue; + } + if (now > end_time) { + break; + } + ::usleep(options_.diagnostic_retry_sleep_ns / 1000); + } + } + + + ///////////////////////////////////////// + // Schema version checking + + CanFdFrame MakeSchemaVersionQuery() { + GenericQuery::Format query; + query.values[0].register_number = Register::kRegisterMapVersion; + query.values[0].resolution = kInt32; + + return MakeFrame(GenericQuery(), {}, query); + } + + void VerifySchemaVersion() { + const auto result = ExecuteSingleCommand(MakeSchemaVersionQuery()); + if (!result) { + throw std::runtime_error("No response to schema version query"); + } + CheckRegisterMapVersion(*result); + } + + void AsyncVerifySchemaVersion(CompletionCallback callback) { + auto result = std::make_shared(); + auto t = transport(); + + AsyncStartSingleCommand( + MakeSchemaVersionQuery(), + result.get(), + [result, callback, t](int value) { + CheckRegisterMapVersion(*result); + t->Post(std::bind(callback, value)); + }); + } + + ////////////////////////////////////////////////// + + Optional ExecuteSingleCommand(const CanFdFrame& cmd) { + single_command_replies_.resize(0); + + transport()->BlockingCycle(&cmd, 1, &single_command_replies_); + + return FindResult(single_command_replies_); + } + + void AsyncStartSingleCommand(const CanFdFrame& cmd, + Result* result, + CompletionCallback callback) { + auto t = transport(); + output_frame_ = cmd; + t->Cycle( + &output_frame_, + 1, + &single_command_replies_, + [callback, result, this, t](int) { + auto maybe_result = this->FindResult(single_command_replies_); + if (maybe_result) { *result = *maybe_result; } + + t->Post( + std::bind( + callback, !options_.default_query ? 0 : + !!maybe_result ? 0 : ETIMEDOUT)); + }); + } + + static std::string FinalName(const std::string& name) { + const size_t pos = name.find_last_of("/"); + if (pos != std::string::npos) { return name.substr(pos + 1); } + return name; + } + + /// This may be called to allow users to configure a default + /// transport. It handles "-h" and "--help" by printing and + /// exiting, so is not suitable for cases where any other command + /// line arguments need to be handled. + static void DefaultArgProcess(int argc, char** argv) { + std::vector args; + for (int i = 0; i < argc; i++) { args.push_back(argv[i]); } + + DefaultArgProcess(args); + } + + /// The same as the above function, but accepts its arguments in a + /// std::vector form. + static void DefaultArgProcess(const std::vector& args) { + if (std::find(args.begin() + 1, args.end(), "--help") != args.end() || + std::find(args.begin() + 1, args.end(), "-h") != args.end()) { + std::cout << "Usage: " << FinalName(args[0]) << "\n"; + auto help_strs = + moteus::TransportRegistry::singleton().cmdline_arguments(); + help_strs.insert(help_strs.begin(), + TransportFactory::Argument( + "--help", 0, "Display this usage message")); + + int max_item = 0; + for (const auto& item : help_strs) { + max_item = std::max(max_item, item.name.size()); + } + max_item = std::min(30, max_item); + + for (const auto& help_item : help_strs) { + std::cout + << " " << help_item.name << " " + << std::string(std::max(0, max_item - help_item.name.size()), ' ') + << help_item.help << "\n"; + } + + std::exit(0); + } + + ArgumentProcessHelper(args); + } + + /// Configure the default transport according to the given command + /// line arguments. Return the command line arguments with all + /// processed commands removed. + /// + /// This is an optional call, and is only required if you want to + /// give a user the ability to configure the default transport from + /// the command line. + /// + /// "-h" and "--help" are not handled here in any way. Thus this + /// method can be used in applications that want to perform + /// additional processing on the command line arguments after. + static std::vector + ProcessTransportArgs(const std::vector& args) { + return ArgumentProcessHelper(args).second; + } + + /// If your application wants to support configuring the default + /// transport from the cmdline and also have application level + /// options, this list can be used to populate --help content. + static std::vector cmdline_arguments() { + return TransportRegistry::singleton().cmdline_arguments(); + } + + private: + static TransportFactory::TransportArgPair + ArgumentProcessHelper(const std::vector& args) { + auto& g_transport = *GlobalTransport(); + if (g_transport) { return std::make_pair(g_transport, args); } + + auto result = TransportRegistry::singleton().make(args); + g_transport = result.first; + return result; + } + + static std::shared_ptr* GlobalTransport() { + static std::shared_ptr g_transport; + return &g_transport; + }; + + // A helper context to maintain asynchronous state while performing + // diagnostic channel commands. + struct AsyncDiagnosticCommandContext + : public std::enable_shared_from_this { + std::string* result = nullptr; + CompletionCallback callback; + DiagnosticReplyMode reply_mode = {}; + + Controller* controller = nullptr; + Transport* transport = nullptr; + + CanFdFrame output_frame_; + std::vector replies; + + int empty_replies = 0; + std::string remaining_command; + std::string current_line; + std::ostringstream output; + + void Start() { + DoWrite(); + } + + void Callback(int error) { + if (error != 0) { + transport->Post(std::bind(callback, error)); + return; + } + + if (ProcessReplies()) { + transport->Post(std::bind(callback, 0)); + return; + } + + if (remaining_command.size()) { + DoWrite(); + } else { + DoRead(); + } + } + + void DoWrite() { + DiagnosticWrite::Command write; + write.data = remaining_command.data(); + const auto to_write = std::min(48, remaining_command.size()); + write.size = to_write; + + output_frame_ = controller->DefaultFrame(kNoReply); + WriteCanData write_frame(output_frame_.data, &output_frame_.size); + DiagnosticWrite::Make(&write_frame, write, {}); + + auto s = shared_from_this(); + controller->transport()->Cycle( + &output_frame_, 1, nullptr, + [s, to_write](int v) { + s->remaining_command = s->remaining_command.substr(to_write); + s->Callback(v); + }); + } + + void DoRead() { + if (empty_replies >= 5) { + // We will call this a timeout. + transport->Post(std::bind(callback, ETIMEDOUT)); + return; + } else if (empty_replies >= 2) { + // Sleep before each subsequent read. + ::usleep(controller->options_.diagnostic_retry_sleep_ns / 1000); + } + + DiagnosticRead::Command read; + output_frame_ = controller->DefaultFrame(kReplyRequired); + WriteCanData write_frame(output_frame_.data, &output_frame_.size); + DiagnosticRead::Make(&write_frame, read, {}); + + auto s = shared_from_this(); + + transport->Cycle( + &output_frame_, 1, &replies, + [s](int v) { + s->Callback(v); + }); + } + + bool ProcessReplies() { + for (const auto& reply : replies) { + if (reply.source != controller->options_.id || + reply.destination != controller->options_.source || + reply.can_prefix != controller->options_.can_prefix) { + continue; + } + + const auto parsed = DiagnosticResponse::Parse(reply.data, reply.size); + if (parsed.channel != 1) { continue; } + + if (parsed.size == 0) { + empty_replies++; + } else { + empty_replies = 0; + } + + current_line += std::string( + reinterpret_cast(parsed.data), parsed.size); + } + + size_t first_newline = std::string::npos; + while ((first_newline = current_line.find_first_of("\r\n")) + != std::string::npos) { + const auto this_line = current_line.substr(0, first_newline); + if (reply_mode == kExpectSingleLine) { + *result = this_line; + return true; + } else if (this_line == "OK") { + *result = output.str(); + return true; + } else { + output.write(current_line.data(), first_newline + 1); + current_line = current_line.substr(first_newline + 1); + } + } + replies.clear(); + return false; + } + }; + + struct AsyncDiagnosticWriteContext + : public std::enable_shared_from_this { + std::string message; + int channel = 0; + Controller* controller = nullptr; + Transport* transport = nullptr; + CompletionCallback callback; + + CanFdFrame output_frame_; + + void Start() { + DoWrite(); + } + + void DoWrite() { + DiagnosticWrite::Command write; + write.channel = channel; + write.data = message.data(); + const auto to_write = std::min(48, message.size()); + write.size = to_write; + + output_frame_ = controller->DefaultFrame(kNoReply); + WriteCanData write_frame(output_frame_.data, &output_frame_.size); + DiagnosticWrite::Make(&write_frame, write, {}); + + auto s = shared_from_this(); + + controller->transport()->Cycle( + &output_frame_, 1, nullptr, + [s, to_write](int v) { + s->message = s->message.substr(to_write); + s->Callback(v); + }); + } + + void Callback(int v) { + if (message.empty()) { + transport->Post(std::bind(callback, v)); + } else { + DoWrite(); + } + } + }; + + static void CheckRegisterMapVersion(const Result& result) { + if (result.values.extra[0].register_number != + Register::kRegisterMapVersion) { + throw std::runtime_error("Malformed response to schema version query"); + } + + const auto int_version = static_cast(result.values.extra[0].value); + if (kCurrentRegisterMapVersion != int_version) { + std::ostringstream ostr; + ostr << "Register map version mismatch device is " + << int_version + << " but library requires " + << kCurrentRegisterMapVersion; + + throw std::runtime_error(ostr.str()); + } + } + + Optional FindResult(const std::vector& replies) const { + // Pick off the last reply we got from our target ID. + for (auto it = replies.rbegin(); it != replies.rend(); ++it) { + if (it->source == options_.id && + it->destination == options_.source && + it->can_prefix == options_.can_prefix) { + + Result result; + result.frame = *it; + result.values = Query::Parse(it->data, it->size); + return result; + } + } + + // We didn't get anything. + return {}; + } + + enum ReplyMode { + kNoReply, + kReplyRequired, + }; + + CanFdFrame DefaultFrame(ReplyMode reply_mode = kReplyRequired) { + CanFdFrame result; + result.destination = options_.id; + result.reply_required = (reply_mode == kReplyRequired); + + result.arbitration_id = + (result.destination) | + (result.source << 8) | + (result.reply_required ? 0x8000 : 0x0000) | + (options_.can_prefix << 16); + result.bus = options_.bus; + + return result; + } + + template + CanFdFrame MakeFrame(const CommandType&, + const typename CommandType::Command& cmd, + const typename CommandType::Format& fmt, + const Query::Format* query_format_override = nullptr) { + auto result = DefaultFrame( + query_format_override != nullptr ? kReplyRequired : + options_.default_query ? kReplyRequired : kNoReply); + + WriteCanData write_frame(result.data, &result.size); + result.expected_reply_size = CommandType::Make(&write_frame, cmd, fmt); + + if (query_format_override) { + result.expected_reply_size = + Query::Make(&write_frame, *query_format_override); + } else if (options_.default_query) { + std::memcpy(&result.data[result.size], + &query_frame_.data[0], + query_frame_.size); + result.size += query_frame_.size; + result.expected_reply_size = query_reply_size_; + } + + return result; + } + + const Options options_; + std::shared_ptr transport_; + CanData query_frame_; + uint8_t query_reply_size_ = 0; + CanFdFrame output_frame_; + + // This is a member variable so we can avoid re-allocating it on + // every call. + std::vector single_command_replies_; +}; + + +} // namespace moteus +} // namespace mjbots diff --git a/deps/mjbots/moteus/moteus_multiplex.h b/deps/mjbots/moteus/moteus_multiplex.h new file mode 100644 index 00000000..2f676fce --- /dev/null +++ b/deps/mjbots/moteus/moteus_multiplex.h @@ -0,0 +1,674 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +#ifdef min +#undef min +#endif + +#ifdef max +#undef max +#endif + +namespace mjbots { +namespace moteus { + +/// Each value can be sent or received as one of the following. +enum Resolution : int8_t { + kInt8 = 0, + kInt16 = 1, + kInt32 = 2, + kFloat = 3, + kIgnore, +}; + +/// A vocabulary type for the basic data in a CAN-FD frame. +struct CanData { + uint8_t data[64] = {}; + uint8_t size = 0; +}; + +enum Multiplex : uint16_t { + kWriteBase = 0x00, + kWriteInt8 = 0x00, + kWriteInt16 = 0x04, + kWriteInt32 = 0x08, + kWriteFloat = 0x0c, + + kReadBase = 0x10, + kReadInt8 = 0x10, + kReadInt16 = 0x14, + kReadInt32 = 0x18, + kReadFloat = 0x1c, + + kReplyBase = 0x20, + kReplyInt8 = 0x20, + kReplyInt16 = 0x24, + kReplyInt32 = 0x28, + kReplyFloat = 0x2c, + + kWriteError = 0x30, + kReadError = 0x31, + + // # Tunneled Stream # + kClientToServer = 0x40, + kServerToClient = 0x41, + kClientPollServer = 0x42, + + kNop = 0x50, +}; + +namespace detail { +template +class numeric_limits { + public: +}; + +template <> +class numeric_limits { + public: + static int8_t max() { return 127; } + static int8_t min() { return -128; } +}; + +template <> +class numeric_limits { + public: + static int16_t max() { return 32767; } + static int16_t min() { return -32768; } +}; + +template <> +class numeric_limits { + public: + static int32_t max() { return 2147483647; } + static int32_t min() { return -2147483648; } +}; + +template <> +class numeric_limits { + public: +}; + +template +T max(T lhs, T rhs) { + return (lhs >= rhs) ? lhs : rhs; +} + +template +T min(T lhs, T rhs) { + return (lhs <= rhs) ? lhs : rhs; +} + +} + +namespace { +template +T Saturate(double value, double scale) { + // TODO: Implement without numeric_limits + if (!::isfinite(value)) { + return detail::numeric_limits::min(); + } + + const double scaled = value / scale; + const auto max = detail::numeric_limits::max(); + + const double double_max = static_cast(max); + // We purposefully limit to +- max, rather than to min. The minimum + // value for our two's complement types is reserved for NaN. + if (scaled < -double_max) { return -max; } + if (scaled > double_max) { return max; } + return static_cast(scaled); +} +} + +/// This class can be used to append values to the end of a CAN-FD +/// frame. +class WriteCanData { + public: + WriteCanData(CanData* frame) : data_(&frame->data[0]), size_(&frame->size) {} + WriteCanData(uint8_t* data, uint8_t* size) : data_(data), size_(size) {} + + uint8_t size() const { return *size_; } + + template + void Write(X value_in) { +#ifndef __ORDER_LITTLE_ENDIAN__ +#error "only little endian architectures supported" +#endif + + T value = static_cast(value_in); + if (sizeof(value) + *size_ > 64) { + abort(); + } + + ::memcpy(&data_[*size_], + reinterpret_cast(&value), + sizeof(value)); + *size_ += sizeof(value); + } + + void WriteVaruint(uint32_t value) { + while (true) { + auto this_byte = value & 0x7f; + value >>= 7; + this_byte |= ((value != 0) ? 0x80 : 0x00); + Write(this_byte); + + if (value == 0) { break; } + } + } + + void Write(const char* data, uint16_t size) { + if ((size + *size_) > 64) { + abort(); + } + ::memcpy(&data_[*size_], data, size); + *size_ += size; + } + + void WriteInt(int32_t value, Resolution res) { + switch (res) { + case Resolution::kInt8: { + Write(detail::max( + -127, detail::min(127, value))); + break; + } + case Resolution::kInt16: { + Write(detail::max( + -32767, detail::min(32767, value))); + break; + } + case Resolution::kInt32: { + Write(value); + break; + } + case Resolution::kFloat: { + Write(static_cast(value)); + break; + } + case Resolution::kIgnore: { + abort(); + } + } + } + + void WriteMapped( + double value, + double int8_scale, double int16_scale, double int32_scale, + Resolution res) { + switch (res) { + case Resolution::kInt8: { + Write(Saturate(value, int8_scale)); + break; + } + case Resolution::kInt16: { + Write(Saturate(value, int16_scale)); + break; + } + case Resolution::kInt32: { + Write(Saturate(value, int32_scale)); + break; + } + case Resolution::kFloat: { + Write(static_cast(value)); + break; + } + case Resolution::kIgnore: { + abort(); + } + } + } + + void WritePosition(double value, Resolution res) { + WriteMapped(value, 0.01, 0.0001, 0.00001, res); + } + + void WriteVelocity(double value, Resolution res) { + WriteMapped(value, 0.1, 0.00025, 0.00001, res); + } + + void WriteAccel(double value, Resolution res) { + WriteMapped(value, 0.05, 0.001, 0.00001, res); + } + + void WriteTorque(double value, Resolution res) { + WriteMapped(value, 0.5, 0.01, 0.001, res); + } + + void WritePwm(double value, Resolution res) { + WriteMapped(value, + 1.0 / 127.0, + 1.0 / 32767.0, + 1.0 / 2147483647.0, + res); + } + + void WriteVoltage(double value, Resolution res) { + WriteMapped(value, 0.5, 0.1, 0.001, res); + } + + void WriteTemperature(float value, Resolution res) { + WriteMapped(value, 1.0, 0.1, 0.001, res); + } + + void WriteTime(double value, Resolution res) { + WriteMapped(value, 0.01, 0.001, 0.000001, res); + } + + void WriteCurrent(double value, Resolution res) { + WriteMapped(value, 1.0, 0.1, 0.001, res); + } + + private: + uint8_t* const data_; + uint8_t* const size_; +}; + +/// Read typed values from a CAN frame. +class MultiplexParser { + public: + MultiplexParser(const CanData* frame) + : data_(&frame->data[0]), + size_(frame->size) {} + MultiplexParser(const uint8_t* data, uint8_t size) + : data_(data), + size_(size) {} + + uint16_t ReadVaruint() { + uint16_t result = 0; + uint16_t shift = 0; + + for (int8_t i = 0; i < 5; i++) { + if (remaining() == 0) { return result; } + + const auto this_byte = static_cast(Read()); + result |= (this_byte & 0x7f) << shift; + shift += 7; + + if ((this_byte & 0x80) == 0) { + return result; + } + } + return result; + } + + struct Result { + bool done = true; + uint16_t value = 0; + Resolution resolution = kIgnore; + + Result(bool done_in, uint16_t value_in, Resolution resolution_in) + : done(done_in), value(value_in), resolution(resolution_in) {} + + Result() {} + }; + + Result next() { + if (offset_ >= size_) { + // We are done. + return Result(true, 0, Resolution::kInt8); + } + + if (remaining_) { + remaining_--; + const auto this_register = current_register_++; + + // Do we actually have enough data? + if (offset_ + ResolutionSize(current_resolution_) > size_) { + return Result(true, 0, Resolution::kInt8); + } + + return Result(false, this_register, current_resolution_); + } + + // We need to look for another command. + while (offset_ < size_) { + const auto cmd = data_[offset_++]; + if (cmd == Multiplex::kNop) { continue; } + + // We are guaranteed to still need data. + if (offset_ >= size_) { + // Nope, we are out. + break; + } + + if (cmd >= 0x20 && cmd < 0x30) { + // This is a regular reply of some sort. + const auto id = (cmd >> 2) & 0x03; + current_resolution_ = [id]() { + switch (id) { + case 0: return Resolution::kInt8; + case 1: return Resolution::kInt16; + case 2: return Resolution::kInt32; + case 3: return Resolution::kFloat; + } + // we cannot reach this point + return Resolution::kInt8; + }(); + int8_t count = cmd & 0x03; + if (count == 0) { + count = data_[offset_++]; + + // We still need more data. + if (offset_ >= size_) { + break; + } + } + + if (count == 0) { + // Empty, guess we can ignore. + continue; + } + + current_register_ = ReadVaruint(); + remaining_ = count - 1; + + if (offset_ + ResolutionSize(current_resolution_) > size_) { + return Result(true, 0, Resolution::kInt8); + } + + return Result(false, current_register_++, current_resolution_); + } + + // For anything else, we'll just assume it is an error of some + // sort and stop parsing. + offset_ = size_; + break; + } + return Result(true, 0, Resolution::kInt8); + } + + template + T Read() { + if (offset_ + sizeof(T) > size_) { + abort(); + } + + T result = {}; + ::memcpy(&result, &data_[offset_], sizeof(T)); + offset_ += sizeof(T); + return result; + } + + template + double Nanify(T value) { + if (value == detail::numeric_limits::min()) { + return NaN; + } + return value; + } + + double ReadMapped(Resolution res, + double int8_scale, + double int16_scale, + double int32_scale) { + switch (res) { + case Resolution::kInt8: { + return Nanify(Read()) * int8_scale; + } + case Resolution::kInt16: { + return Nanify(Read()) * int16_scale; + } + case Resolution::kInt32: { + return Nanify(Read()) * int32_scale; + } + case Resolution::kFloat: { + return Read(); + } + default: { + break; + } + } + abort(); + } + + static constexpr int8_t kInt = 0; + static constexpr int8_t kPosition = 1; + static constexpr int8_t kVelocity = 2; + static constexpr int8_t kTorque = 3; + static constexpr int8_t kPwm = 4; + static constexpr int8_t kVoltage = 5; + static constexpr int8_t kTemperature = 6; + static constexpr int8_t kTime = 7; + static constexpr int8_t kCurrent = 8; + static constexpr int8_t kTheta = 9; + static constexpr int8_t kPower = 10; + static constexpr int8_t kAcceleration = 11; + + double ReadConcrete(Resolution res, int8_t concrete_type) { +#ifndef ARDUINO + static constexpr double kMappingValues[] = { +#else + static constexpr float PROGMEM kMappingValues[] = { +#endif + 1.0, 1.0, 1.0, // kInt + 0.01, 0.0001, 0.00001, // kPosition + 0.1, 0.00025, 0.00001, // kVelocity + 0.5, 0.01, 0.001, // kTorque + 1.0 / 127.0, 1.0 / 32767.0, 1.0 / 2147483647.0, // kPwm + 0.5, 0.1, 0.001, // kVoltage + 1.0, 0.1, 0.001, // kTemperature + 0.01, 0.001, 0.000001, // kTime + 1.0, 0.1, 0.001, // kCurrent + 1.0 / 127.0 * M_PI, 1.0 / 32767.0 * M_PI, 1.0 / 2147483647.0 * M_PI, // kTheta + 10.0, 0.05, 0.0001, // kPower + 0.05, 0.001, 0.00001, // kAcceleration + }; + +#ifndef ARDUINO + const double int8_scale = kMappingValues[concrete_type * 3 + 0]; + const double int16_scale = kMappingValues[concrete_type * 3 + 1]; + const double int32_scale = kMappingValues[concrete_type * 3 + 2]; +#else + const float int8_scale = pgm_read_float_near(kMappingValues + concrete_type * 3 + 0); + const float int16_scale = pgm_read_float_near(kMappingValues + concrete_type * 3 + 1); + const float int32_scale = pgm_read_float_near(kMappingValues + concrete_type * 3 + 2); +#endif + + switch (res) { + case Resolution::kInt8: { + return Nanify(Read()) * int8_scale; + } + case Resolution::kInt16: { + return Nanify(Read()) * int16_scale; + } + case Resolution::kInt32: { + return Nanify(Read()) * int32_scale; + } + case Resolution::kFloat: { + return Read(); + } + default: { + break; + } + } + + abort(); + } + + int ReadInt(Resolution res) { + return static_cast(ReadConcrete(res, kInt)); + } + + double ReadPosition(Resolution res) { + return ReadConcrete(res, kPosition); + } + + double ReadVelocity(Resolution res) { + return ReadConcrete(res, kVelocity); + } + + double ReadTorque(Resolution res) { + return ReadConcrete(res, kTorque); + } + + double ReadPwm(Resolution res) { + return ReadConcrete(res, kPwm); + } + + double ReadVoltage(Resolution res) { + return ReadConcrete(res, kVoltage); + } + + double ReadTemperature(Resolution res) { + return ReadConcrete(res, kTemperature); + } + + double ReadTime(Resolution res) { + return ReadConcrete(res, kTime); + } + + double ReadCurrent(Resolution res) { + return ReadConcrete(res, kCurrent); + } + + double ReadPower(Resolution res) { + return ReadConcrete(res, kPower); + } + + void Ignore(Resolution res) { + offset_ += ResolutionSize(res); + } + + void ReadRaw(uint8_t* output, uint16_t size) { + if ((offset_ + size) > size_) { ::abort(); } + ::memcpy(output, &data_[offset_], size); + offset_ += size; + } + + uint16_t remaining() const { + return size_ - offset_; + } + + static int8_t ResolutionSize(Resolution res) { + switch (res) { + case Resolution::kInt8: return 1; + case Resolution::kInt16: return 2; + case Resolution::kInt32: return 4; + case Resolution::kFloat: return 4; + default: { break; } + } + return 1; + } + + private: + const uint8_t* const data_; + const uint8_t size_; + uint16_t offset_ = 0; + + int8_t remaining_ = 0; + Resolution current_resolution_ = Resolution::kIgnore; + uint16_t current_register_ = 0; +}; + +/// Determines how to group registers when encoding them to minimize +/// the required bytes. +class WriteCombiner { + public: + WriteCombiner(WriteCanData* frame, + int8_t base_command, + uint16_t start_register, + const Resolution* resolutions, + uint16_t resolutions_size) + : frame_(frame), + base_command_(base_command), + start_register_(start_register), + resolutions_(resolutions), + resolutions_size_(resolutions_size) {} + + ~WriteCombiner() { + if (offset_ != resolutions_size_) { + ::abort(); + } + } + + uint8_t reply_size() const { return reply_size_; } + + bool MaybeWrite() { + const auto this_offset = offset_; + offset_++; + + if (current_resolution_ == resolutions_[this_offset]) { + // We don't need to write any register operations here, and the + // value should go out only if requested. + return current_resolution_ != Resolution::kIgnore; + } + // We need to do some kind of framing. See how far ahead the new + // resolution goes. + const auto new_resolution = resolutions_[this_offset]; + current_resolution_ = new_resolution; + + // We are now in a new block of ignores. + if (new_resolution == Resolution::kIgnore) { + return false; + } + + int16_t count = 1; + for (uint16_t i = this_offset + 1; + i < resolutions_size_ && resolutions_[i] == new_resolution; + i++) { + count++; + } + + int8_t write_command = base_command_ + [&]() { + switch (new_resolution) { + case Resolution::kInt8: return 0x00; + case Resolution::kInt16: return 0x04; + case Resolution::kInt32: return 0x08; + case Resolution::kFloat: return 0x0c; + case Resolution::kIgnore: { + abort(); + } + } + return 0x00; + }(); + + const auto start_size = frame_->size(); + if (count <= 3) { + // Use the shorthand formulation. + frame_->Write(write_command + count); + } else { + // Nope, the long form. + frame_->Write(write_command); + frame_->Write(count); + } + frame_->WriteVaruint(start_register_ + this_offset); + const auto end_size = frame_->size(); + + reply_size_ += (end_size - start_size); + reply_size_ += count * MultiplexParser::ResolutionSize(new_resolution); + + return true; + } + + private: + WriteCanData* const frame_; + int8_t base_command_ = 0; + uint16_t start_register_ = 0; + const Resolution* const resolutions_; + uint16_t resolutions_size_ = 0; + + Resolution current_resolution_ = Resolution::kIgnore; + uint16_t offset_ = 0; + uint8_t reply_size_ = 0; +}; + +} +} diff --git a/deps/mjbots/moteus/moteus_optional.h b/deps/mjbots/moteus/moteus_optional.h new file mode 100644 index 00000000..8b9e193d --- /dev/null +++ b/deps/mjbots/moteus/moteus_optional.h @@ -0,0 +1,53 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +namespace mjbots { + namespace moteus { + + // A stripped down optional class for C++11 environments. + template + class Optional { + public: + Optional() : dummy_(0), engaged_(false) {} + Optional(const T& t) : val_(t), engaged_(true) {} + + ~Optional() { + if (engaged_) { val_.~T(); } + } + + Optional& operator=(const T& val) { + engaged_ = true; + val_ = val; + return *this; + } + + bool has_value() const { return engaged_; } + + T& operator*() noexcept { return val_; } + T* operator->() noexcept { return &val_; } + const T& operator*() const noexcept { return val_; } + const T* operator->() const noexcept { return &val_; } + + explicit operator bool() const noexcept { return engaged_; } + bool operator!() const noexcept { return !engaged_; } + + private: + union { char dummy_; T val_; }; + bool engaged_; + }; + + } // namespace moteus +} // namespace mjbots diff --git a/deps/mjbots/moteus/moteus_protocol.h b/deps/mjbots/moteus/moteus_protocol.h new file mode 100644 index 00000000..ed62ecc8 --- /dev/null +++ b/deps/mjbots/moteus/moteus_protocol.h @@ -0,0 +1,1289 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// @file +/// +/// This file contains primitives used to encode and decode CAN-FD +/// messages for the mjbots moteus brushless controller. While C++, +/// it is designed to be usable in "minimal-C++" environments like +/// Arduino, in addition to fully standards conforming environments. + +#pragma once + +#include + +#ifndef ARDUINO + +#include +#include +#define NaN std::numeric_limits::quiet_NaN(); + +#else + +#define NaN (0.0 / 0.0) + +#endif + + +#include "moteus_multiplex.h" + +namespace mjbots { +namespace moteus { + +/// This is a single CAN-FD frame, its headers, and other associated +/// metadata, like which bus the message was sent or received from in +/// multi-bus systems. +struct CanFdFrame { + /////////////////////////////////////////// + /// First are the raw data from the bus. + + // Non-zero if this transport supports multiple busses. + int8_t bus = 0; + + uint32_t arbitration_id = 0; + uint8_t data[64] = {}; + uint8_t size = 0; + + enum Toggle { + kDefault, + kForceOff, + kForceOn, + }; + + Toggle brs = kDefault; + Toggle fdcan_frame = kDefault; + + ////////////////////////////////////////// + /// Next are parsed items for moteus frames. These are not required + /// to be set when sending a frame to a transport, but they will be + /// filled in on receipt. Higher level layers than the transport + /// layer may use them to populate the bus-level data. + + // If true, then the ID used is not calculated from destination and + // source, but is instead determined directly from arbitration_id. + bool raw = false; + + int8_t destination = 0; + int8_t source = 0; + uint16_t can_prefix = 0x0000; // A 13 bit CAN prefix + + //////////////////////////////////////// + /// Finally other hinting data. + + // Whether this frame is expected to elicit a response. + bool reply_required = false; + + // If this frame does elicit a response, how large is it expected to + // be. + uint8_t expected_reply_size = 0; +}; + + +/// The expected version associated with register 0x102. If it +/// differs from this, then semantics of one or more registers may +/// have changed. +enum { + kCurrentRegisterMapVersion = 5, +}; + +enum Register : uint16_t { + kMode = 0x000, + kPosition = 0x001, + kVelocity = 0x002, + kTorque = 0x003, + kQCurrent = 0x004, + kDCurrent = 0x005, + kAbsPosition = 0x006, + kPower = 0x007, + + kMotorTemperature = 0x00a, + kTrajectoryComplete = 0x00b, + kHomeState = 0x00c, + kVoltage = 0x00d, + kTemperature = 0x00e, + kFault = 0x00f, + + kPwmPhaseA = 0x010, + kPwmPhaseB = 0x011, + kPwmPhaseC = 0x012, + + kVoltagePhaseA = 0x014, + kVoltagePhaseB = 0x015, + kVoltagePhaseC = 0x016, + + kVFocTheta = 0x018, + kVFocVoltage = 0x019, + kVoltageDqD = 0x01a, + kVoltageDqQ = 0x01b, + + kCommandQCurrent = 0x01c, + kCommandDCurrent = 0x01d, + + kCommandPosition = 0x020, + kCommandVelocity = 0x021, + kCommandFeedforwardTorque = 0x022, + kCommandKpScale = 0x023, + kCommandKdScale = 0x024, + kCommandPositionMaxTorque = 0x025, + kCommandStopPosition = 0x026, + kCommandTimeout = 0x027, + kCommandVelocityLimit = 0x028, + kCommandAccelLimit = 0x029, + kCommandFixedVoltageOverride = 0x02a, + kCommandIlimitScale = 0x02b, + + kPositionKp = 0x030, + kPositionKi = 0x031, + kPositionKd = 0x032, + kPositionFeedforward = 0x033, + kPositionCommand = 0x034, + + kControlPosition = 0x038, + kControlVelocity = 0x039, + kControlTorque = 0x03a, + kControlPositionError = 0x03b, + kControlVelocityError = 0x03c, + kControlTorqueError = 0x03d, + + kCommandStayWithinLowerBound = 0x040, + kCommandStayWithinUpperBound = 0x041, + kCommandStayWithinFeedforwardTorque = 0x042, + kCommandStayWithinKpScale = 0x043, + kCommandStayWithinKdScale = 0x044, + kCommandStayWithinPositionMaxTorque = 0x045, + kCommandStayWithinTimeout = 0x046, + kCommandStayWithinIlimitScale = 0x047, + + kEncoder0Position = 0x050, + kEncoder0Velocity = 0x051, + kEncoder1Position = 0x052, + kEncoder1Velocity = 0x053, + kEncoder2Position = 0x054, + kEncoder2Velocity = 0x055, + + kEncoderValidity = 0x058, + + kAux1GpioCommand = 0x05c, + kAux2GpioCommand = 0x05d, + kAux1GpioStatus = 0x05e, + kAux2GpioStatus = 0x05f, + + kAux1AnalogIn1 = 0x060, + kAux1AnalogIn2 = 0x061, + kAux1AnalogIn3 = 0x062, + kAux1AnalogIn4 = 0x063, + kAux1AnalogIn5 = 0x064, + + kAux2AnalogIn1 = 0x068, + kAux2AnalogIn2 = 0x069, + kAux2AnalogIn3 = 0x06a, + kAux2AnalogIn4 = 0x06b, + kAux2AnalogIn5 = 0x06c, + + kMillisecondCounter = 0x070, + kClockTrim = 0x071, + + kRegisterMapVersion = 0x102, + kSerialNumber = 0x120, + kSerialNumber1 = 0x120, + kSerialNumber2 = 0x121, + kSerialNumber3 = 0x122, + + kRezero = 0x130, + kSetOutputNearest = 0x130, + kSetOutputExact = 0x131, + kRequireReindex = 0x132, + kRecapturePositionVelocity = 0x133, + + kDriverFault1 = 0x140, + kDriverFault2 = 0x141, +}; + +enum class Mode { + kStopped = 0, + kFault = 1, + kEnabling = 2, + kCalibrating = 3, + kCalibrationComplete = 4, + kPwm = 5, + kVoltage = 6, + kVoltageFoc = 7, + kVoltageDq = 8, + kCurrent = 9, + kPosition = 10, + kPositionTimeout = 11, + kZeroVelocity = 12, + kStayWithin = 13, + kMeasureInd = 14, + kBrake = 15, + kNumModes, +}; + +enum class HomeState { + kRelative = 0, + kRotor = 1, + kOutput = 2, +}; + + +//////////////////////////////////////////////////////////////// +// Each command that can be issued is represented by a structure below +// with a few different sub-structs. Possibilities are: +// +// 'Command' items that are serialized in an outgoing message +// 'Format' the corresponding resolution for each item in 'Command' +// 'Result' the deserialized version of a response +// +// Additionally, there are two static methods possible: +// +// 'Make' - take a Command and Format, and serialize it +// 'Parse' - deserialize CAN data into a 'Result' structure + +struct EmptyMode { + struct Command {}; + struct Format {}; + static uint8_t Make(WriteCanData*, const Command&, const Format&) { + return 0; + } +}; + +struct Query { + struct ItemValue { + int16_t register_number = detail::numeric_limits::max(); + double value = 0.0; + }; + +#ifndef ARDUINO + static constexpr int16_t kMaxExtra = 16; +#else + static constexpr int16_t kMaxExtra = 8; +#endif + + struct Result { + Mode mode = Mode::kStopped; + double position = NaN; + double velocity = NaN; + double torque = NaN; + double q_current = NaN; + double d_current = NaN; + double abs_position = NaN; + double power = NaN; + double motor_temperature = NaN; + bool trajectory_complete = false; + HomeState home_state = HomeState::kRelative; + double voltage = NaN; + double temperature = NaN; + int8_t fault = 0; + + int8_t aux1_gpio = 0; + int8_t aux2_gpio = 0; + + // Before gcc-12, initializating non-POD array types can be + // painful if done in the idiomatic way with ={} inline. Instead + // we do it in the constructor. + // + // https://gcc.gnu.org/bugzilla/show_bug.cgi?id=92385 + ItemValue extra[kMaxExtra]; + + Result() : extra() {} + }; + + struct ItemFormat { + int16_t register_number = detail::numeric_limits::max(); + Resolution resolution = moteus::kIgnore; + }; + + struct Format { + Resolution mode = kInt8; + Resolution position = kFloat; + Resolution velocity = kFloat; + Resolution torque = kFloat; + Resolution q_current = kIgnore; + Resolution d_current = kIgnore; + Resolution abs_position = kIgnore; + Resolution power = kIgnore; + Resolution motor_temperature = kIgnore; + Resolution trajectory_complete = kIgnore; + Resolution home_state = kIgnore; + Resolution voltage = kInt8; + Resolution temperature = kInt8; + Resolution fault = kInt8; + + Resolution aux1_gpio = kIgnore; + Resolution aux2_gpio = kIgnore; + + // Any values here must be sorted by register number. + ItemFormat extra[kMaxExtra]; + + // gcc bug 92385 again + Format() : extra() {} + }; + + static uint8_t Make(WriteCanData* frame, const Format& format) { + uint8_t reply_size = 0; + + { + const Resolution kResolutions[] = { + format.mode, + format.position, + format.velocity, + format.torque, + format.q_current, + format.d_current, + format.abs_position, + format.power, + }; + const uint16_t kResolutionsSize = sizeof(kResolutions) / sizeof(*kResolutions); + WriteCombiner combiner( + frame, 0x10, Register::kMode, + kResolutions, kResolutionsSize); + for (uint16_t i = 0; i < kResolutionsSize; i++) { + combiner.MaybeWrite(); + } + reply_size += combiner.reply_size(); + } + + { + const Resolution kResolutions[] = { + format.motor_temperature, + format.trajectory_complete, + format.home_state, + format.voltage, + format.temperature, + format.fault, + }; + const uint16_t kResolutionsSize = sizeof(kResolutions) / sizeof(*kResolutions); + WriteCombiner combiner( + frame, 0x10, Register::kMotorTemperature, + kResolutions, kResolutionsSize); + for (uint16_t i = 0; i < kResolutionsSize; i++) { + combiner.MaybeWrite(); + } + reply_size += combiner.reply_size(); + } + + { + const Resolution kResolutions[] = { + format.aux1_gpio, + format.aux2_gpio, + }; + const uint16_t kResolutionsSize = sizeof(kResolutions) / sizeof(*kResolutions); + WriteCombiner combiner( + frame, 0x10, Register::kAux1GpioStatus, + kResolutions, kResolutionsSize); + for (uint16_t i = 0; i < kResolutionsSize; i++) { + combiner.MaybeWrite(); + } + reply_size += combiner.reply_size(); + } + + { + const int16_t size = [&]() { + for (int16_t i = 0; i < kMaxExtra; i++) { + if (format.extra[i].register_number == + detail::numeric_limits::max()) { + return i; + } + } + return kMaxExtra; + }(); + + if (size == 0) { return reply_size; } + const int16_t min_register_number = format.extra[0].register_number; + const int16_t max_register_number = format.extra[size - 1].register_number; + + const uint16_t required_registers = + max_register_number - min_register_number + 1; + + // An arbitrary limit to constrain the amount of stack we use + // below. + if (required_registers > 512) { ::abort(); } + +#ifndef ARDUINO + std::vector resolutions(required_registers); +#else + Resolution resolutions[required_registers]; +#endif + ::memset(&resolutions[0], 0, sizeof(Resolution) * required_registers); + + for (int16_t this_register = min_register_number, index = 0; + this_register <= max_register_number; + this_register++) { + if (format.extra[index].register_number == this_register) { + resolutions[this_register - min_register_number] = + format.extra[index].resolution; + index++; + } else { + resolutions[this_register - min_register_number] = kIgnore; + } + } + WriteCombiner combiner( + frame, 0x10, min_register_number, + &resolutions[0], required_registers); + for (uint16_t i = 0; i < required_registers; i++) { + combiner.MaybeWrite(); + } + reply_size += combiner.reply_size(); + } + + return reply_size; + } + + static Result Parse(const uint8_t* data, uint8_t size) { + MultiplexParser parser(data, size); + + return Parse(&parser); + } + + static Result Parse(const CanData* frame) { + MultiplexParser parser(frame); + + return Parse(&parser); + } + + static Result Parse(MultiplexParser* parser) { + Result result; + + int16_t extra_index = 0; + + while (true) { + const auto current = parser->next(); + if (current.done) { return result; } + + const auto res = current.resolution; + + switch (static_cast(current.value)) { + case Register::kMode: { + result.mode = static_cast(parser->ReadInt(res)); + break; + } + case Register::kPosition: { + result.position = parser->ReadPosition(res); + break; + } + case Register::kVelocity: { + result.velocity = parser->ReadVelocity(res); + break; + } + case Register::kTorque: { + result.torque = parser->ReadTorque(res); + break; + } + case Register::kQCurrent: { + result.q_current = parser->ReadCurrent(res); + break; + } + case Register::kDCurrent: { + result.d_current = parser->ReadCurrent(res); + break; + } + case Register::kAbsPosition: { + result.abs_position = parser->ReadPosition(res); + break; + } + case Register::kPower: { + result.power = parser->ReadPower(res); + break; + } + case Register::kMotorTemperature: { + result.motor_temperature = parser->ReadTemperature(res); + break; + } + case Register::kTrajectoryComplete: { + result.trajectory_complete = parser->ReadInt(res) != 0; + break; + } + case Register::kHomeState: { + result.home_state = static_cast(parser->ReadInt(res)); + break; + } + case Register::kVoltage: { + result.voltage = parser->ReadVoltage(res); + break; + } + case Register::kTemperature: { + result.temperature = parser->ReadTemperature(res); + break; + } + case Register::kFault: { + result.fault = parser->ReadInt(res); + break; + } + case Register::kAux1GpioStatus: { + result.aux1_gpio = parser->ReadInt(res); + break; + } + case Register::kAux2GpioStatus: { + result.aux2_gpio = parser->ReadInt(res); + break; + } + default: { + if (extra_index < kMaxExtra) { + result.extra[extra_index].register_number = current.value; + result.extra[extra_index].value = + ParseGeneric(parser, current.value, res); + extra_index++; + } else { + // Ignore this value. + parser->ReadInt(res); + } + break; + } + } + } + } + + static double ParseGeneric(MultiplexParser* parser, + int16_t register_number, + Resolution resolution) { + const auto res = resolution; + + using R = Register; + using MP = MultiplexParser; + + struct RegisterDefinition { + uint16_t register_number; + uint8_t block_size; + int8_t concrete; + }; +#ifndef ARDUINO + static constexpr RegisterDefinition kRegisterDefinitions[] = { +#else + static constexpr RegisterDefinition PROGMEM kRegisterDefinitions[] = { +#endif + { R::kMode, 1, MP::kInt, }, + { R::kPosition, 1, MP::kPosition, }, + { R::kVelocity, 1, MP::kVelocity, }, + { R::kTorque, 1, MP::kTorque, }, + { R::kQCurrent, 2, MP::kCurrent, }, + // { R::kDCurrent, 1, MP::kCurrent, }, + { R::kAbsPosition, 1, MP::kPosition, }, + { R::kPower, 1, MP::kPower, }, + { R::kMotorTemperature, 1, MP::kTemperature, }, + { R::kTrajectoryComplete, 2, MP::kInt, }, + // { R::kHomeState, 1, MP::kInt, }, + { R::kVoltage, 1, MP::kVoltage, }, + { R::kTemperature, 1, MP::kTemperature, }, + { R::kFault, 1, MP::kInt, }, + + { R::kPwmPhaseA, 3, MP::kPwm, }, + // { R::kPwmPhaseB, 1, MP::kPwm, }, + // { R::kPwmPhaseC, 1, MP::kPwm, }, + + { R::kVoltagePhaseA, 3, MP::kVoltage, }, + // { R::kVoltagePhaseB, 1, MP::kVoltage, }, + // { R::kVoltagePhaseC, 1, MP::kVoltage, }, + + { R::kVFocTheta, 1, MP::kTheta, }, + { R::kVFocVoltage, 3, MP::kVoltage, }, + // { R::kVoltageDqD, 1, MP::kVoltage, }, + // { R::kVoltageDqQ, 1, MP::kVoltage, }, + + { R::kCommandQCurrent, 2, MP::kCurrent, }, + // { R::kCommandDCurrent, 1, MP::kCurrent, }, + + { R::kCommandPosition, 1, MP::kPosition, }, + { R::kCommandVelocity, 1, MP::kVelocity, }, + { R::kCommandFeedforwardTorque, 1, MP::kTorque, }, + { R::kCommandKpScale, 2, MP::kPwm, }, + // { R::kCommandKdScale, 1, MP::kPwm, }, + { R::kCommandPositionMaxTorque, 1, MP::kTorque, }, + { R::kCommandStopPosition, 1, MP::kPosition, }, + { R::kCommandTimeout, 1, MP::kTime, }, + { R::kCommandVelocityLimit, 1, MP::kVelocity, }, + { R::kCommandAccelLimit, 1, MP::kAcceleration, }, + { R::kCommandFixedVoltageOverride, 1, MP::kVoltage }, + + { R::kPositionKp, 5, MP::kTorque, }, + // { R::kPositionKi, 1, MP::kTorque, }, + // { R::kPositionKd, 1, MP::kTorque, }, + // { R::kPositionFeedforward, 1, MP::kTorque, }, + // { R::kPositionCommand, 1, MP::kTorque, }, + + { R::kControlPosition, 1, MP::kPosition, }, + { R::kControlVelocity, 1, MP::kVelocity, }, + { R::kControlTorque, 1, MP::kTorque, }, + { R::kControlPositionError, 1, MP::kPosition, }, + { R::kControlVelocityError, 1, MP::kVelocity, }, + { R::kControlTorqueError, 1, MP::kTorque, }, + + { R::kCommandStayWithinLowerBound, 2, MP::kPosition, }, + // { R::kCommandStayWithinUpperBound, 1, MP::kPosition, }, + { R::kCommandStayWithinFeedforwardTorque, 1, MP::kTorque, }, + { R::kCommandStayWithinKpScale, 2, MP::kPwm, }, + // { R::kCommandStayWithinKdScale, 1, MP::kPwm, }, + { R::kCommandStayWithinPositionMaxTorque, 1, MP::kTorque, }, + { R::kCommandStayWithinTimeout, 1, MP::kTime, }, + { R::kCommandStayWithinIlimitScale, 1, MP::kPwm }, + + { R::kEncoder0Position, 1, MP::kPosition, }, + { R::kEncoder0Velocity, 1, MP::kVelocity, }, + { R::kEncoder1Position, 1, MP::kPosition, }, + { R::kEncoder1Velocity, 1, MP::kVelocity, }, + { R::kEncoder2Position, 1, MP::kPosition, }, + { R::kEncoder2Velocity, 1, MP::kVelocity, }, + + { R::kEncoderValidity, 1, MP::kInt, }, + + { R::kAux1GpioCommand, 4, MP::kInt, }, + // { R::kAux2GpioCommand, 1, MP::kInt, }, + // { R::kAux1GpioStatus, 1, MP::kInt, }, + // { R::kAux2GpioStatus, 1, MP::kInt, }, + + { R::kAux1AnalogIn1, 5, MP::kPwm, }, + // { R::kAux1AnalogIn2, 1, MP::kPwm, }, + // { R::kAux1AnalogIn3, 1, MP::kPwm, }, + // { R::kAux1AnalogIn4, 1, MP::kPwm, }, + // { R::kAux1AnalogIn5, 1, MP::kPwm, }, + + { R::kAux2AnalogIn1, 5, MP::kPwm, }, + // { R::kAux2AnalogIn2, 1, MP::kPwm, }, + // { R::kAux2AnalogIn3, 1, MP::kPwm, }, + // { R::kAux2AnalogIn4, 1, MP::kPwm, }, + // { R::kAux2AnalogIn5, 1, MP::kPwm, }, + + { R::kMillisecondCounter, 2, MP::kInt, }, + // { R::kClockTrim, 1, MP::kInt, }, + + { R::kRegisterMapVersion, 1, MP::kInt, }, + { R::kSerialNumber1, 3, MP::kInt, }, + // { R::kSerialNumber2, 1, MP::kInt, }, + // { R::kSerialNumber3, 1, MP::kInt, }, + + { R::kSetOutputNearest, 3, MP::kInt, }, + // { R::kSetOutputExact, 1, MP::kInt, }, + // { R::kRequireReindex, 1, MP::kInt, }, + // { R::kRecapturePositionVelocity, 1, MP::kInt, } + + { R::kDriverFault1, 2, MP::kInt, }, + // { R::kDriverFault2, 1, MP::kInt, }, + + }; + for (uint16_t i = 0; + i < sizeof(kRegisterDefinitions) / sizeof (*kRegisterDefinitions); + i ++) { + +#ifndef ARDUINO + const int16_t start_reg = kRegisterDefinitions[i].register_number; + const uint8_t block_size = kRegisterDefinitions[i].block_size; + const int8_t concrete_type = kRegisterDefinitions[i].concrete; +#else + const int16_t start_reg = pgm_read_word_near(&kRegisterDefinitions[i].register_number); + const uint8_t block_size = pgm_read_byte_near(&kRegisterDefinitions[i].block_size); + const int8_t concrete_type = pgm_read_byte_near(&kRegisterDefinitions[i].concrete); +#endif + if (register_number >= start_reg && + register_number < (start_reg + block_size)) { + return parser->ReadConcrete(res, concrete_type); + } + } + return parser->ReadInt(res); + } +}; + +struct GenericQuery { + struct Command {}; + + struct ItemValue { + int16_t register_number = -1; + double value = 0.0; + }; + + // A CAN-FD frame can only have 64 bytes, so we definitely can't + // have more than 64 items in a single one ever. + static constexpr int16_t kMaxItems = 64; + + struct Result { + ItemValue values[kMaxItems] = {}; + }; + + struct ItemFormat { + int16_t register_number = detail::numeric_limits::max(); + Resolution resolution = kIgnore; + }; + + struct Format { + // These values must be sorted by register number. + ItemFormat values[kMaxItems] = {}; + }; + + static int ItemFormatSort(const void* lhs_in, const void* rhs_in) { + const auto* lhs = reinterpret_cast(lhs_in); + const auto* rhs = reinterpret_cast(rhs_in); + + return lhs->register_number - rhs->register_number; + } + + static uint8_t Make(WriteCanData* frame, const Command&, const Format& format) { + const int16_t size = [&]() { + for (int16_t i = 0; i < kMaxItems; i++) { + if (format.values[i].register_number == + detail::numeric_limits::max()) { + return i; + } + } + return kMaxItems; + }(); + + if (size == 0) { return 0; } + const int16_t min_register_number = format.values[0].register_number; + const int16_t max_register_number = format.values[size - 1].register_number; + + const uint16_t required_registers = max_register_number - min_register_number + 1; + + // An arbitrary limit to constrain the amount of stack we use + // below. + if (required_registers > 512) { ::abort(); } + +#ifndef ARDUINO + std::vector resolutions(required_registers); +#else + Resolution resolutions[required_registers]; +#endif + ::memset(&resolutions[0], 0, sizeof(Resolution) * required_registers); + + for (int16_t this_register = min_register_number, index = 0; + this_register <= max_register_number; + this_register++) { + if (format.values[index].register_number == this_register) { + resolutions[this_register - min_register_number] = + format.values[index].resolution; + index++; + } else { + resolutions[this_register - min_register_number] = kIgnore; + } + } + WriteCombiner combiner( + frame, 0x10, min_register_number, + &resolutions[0], required_registers); + for (uint16_t i = 0; i < required_registers; i++) { + combiner.MaybeWrite(); + } + + return combiner.reply_size(); + } + + static Result Parse(const uint8_t* data, uint8_t size) { + MultiplexParser parser(data, size); + + return Parse(&parser); + } + + static Result Parse(const CanData* frame) { + MultiplexParser parser(frame); + + return Parse(&parser); + } + + static Result Parse(MultiplexParser* parser) { + Result result; + + int16_t index = 0; + + while (true) { + if (index >= kMaxItems) { return result; } + + const auto current = parser->next(); + if (current.done) { return result; } + + result.values[index].register_number = current.value; + result.values[index].value = + Query::ParseGeneric(parser, current.value, current.resolution); + + index++; + } + } +}; + +struct PositionMode { + struct Command { + double position = 0.0; + double velocity = 0.0; + double feedforward_torque = 0.0; + double kp_scale = 1.0; + double kd_scale = 1.0; + double maximum_torque = NaN; + double stop_position = NaN; + double watchdog_timeout = NaN; + double velocity_limit = NaN; + double accel_limit = NaN; + double fixed_voltage_override = NaN; + double ilimit_scale = 1.0; + }; + + struct Format { + Resolution position = kFloat; + Resolution velocity = kFloat; + Resolution feedforward_torque = kIgnore; + Resolution kp_scale = kIgnore; + Resolution kd_scale = kIgnore; + Resolution maximum_torque = kIgnore; + Resolution stop_position = kIgnore; + Resolution watchdog_timeout = kIgnore; + Resolution velocity_limit = kIgnore; + Resolution accel_limit = kIgnore; + Resolution fixed_voltage_override = kIgnore; + Resolution ilimit_scale = kIgnore; + }; + + static uint8_t Make(WriteCanData* frame, + const Command& command, + const Format& format) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kPosition); + + // Now we use some heuristics to try and group consecutive registers + // of the same resolution together into larger writes. + const Resolution kResolutions[] = { + format.position, + format.velocity, + format.feedforward_torque, + format.kp_scale, + format.kd_scale, + format.maximum_torque, + format.stop_position, + format.watchdog_timeout, + format.velocity_limit, + format.accel_limit, + format.fixed_voltage_override, + format.ilimit_scale, + }; + WriteCombiner combiner( + frame, 0x00, + Register::kCommandPosition, + kResolutions, + sizeof(kResolutions) / sizeof(*kResolutions)); + + if (combiner.MaybeWrite()) { + frame->WritePosition(command.position, format.position); + } + if (combiner.MaybeWrite()) { + frame->WriteVelocity(command.velocity, format.velocity); + } + if (combiner.MaybeWrite()) { + frame->WriteTorque(command.feedforward_torque, format.feedforward_torque); + } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.kp_scale, format.kp_scale); + } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.kd_scale, format.kd_scale); + } + if (combiner.MaybeWrite()) { + frame->WriteTorque(command.maximum_torque, format.maximum_torque); + } + if (combiner.MaybeWrite()) { + frame->WritePosition(command.stop_position, format.stop_position); + } + if (combiner.MaybeWrite()) { + frame->WriteTime(command.watchdog_timeout, format.watchdog_timeout); + } + if (combiner.MaybeWrite()) { + frame->WriteVelocity(command.velocity_limit, format.velocity_limit); + } + if (combiner.MaybeWrite()) { + frame->WriteAccel(command.accel_limit, format.accel_limit); + } + if (combiner.MaybeWrite()) { + frame->WriteVoltage(command.fixed_voltage_override, + format.fixed_voltage_override); + } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.ilimit_scale, format.ilimit_scale); + } + return 0; + } +}; + +/// The voltage-mode FOC command. +struct VFOCMode { + struct Command { + double theta_rad = 0.0; + double voltage = 0.0; + double theta_rad_rate = 0.0; + }; + + struct Format { + Resolution theta_rad = kFloat; + Resolution voltage = kFloat; + Resolution theta_rad_rate = kFloat; + }; + + static uint8_t Make(WriteCanData* frame, + const Command& command, + const Format& format) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kVoltageFoc); + + const Resolution kResolutions[] = { + format.theta_rad, + format.voltage, + kIgnore, + kIgnore, + kIgnore, + kIgnore, + format.theta_rad_rate, + }; + WriteCombiner combiner( + frame, 0x00, + Register::kVFocTheta, + kResolutions, + sizeof(kResolutions) / sizeof(*kResolutions)); + + if (combiner.MaybeWrite()) { + frame->WritePwm(command.theta_rad / M_PI, format.theta_rad); + } + if (combiner.MaybeWrite()) { + frame->WriteVoltage(command.voltage, format.voltage); + } + if (combiner.MaybeWrite()) { + abort(); + } + if (combiner.MaybeWrite()) { + abort(); + } + if (combiner.MaybeWrite()) { + abort(); + } + if (combiner.MaybeWrite()) { + abort(); + } + if (combiner.MaybeWrite()) { + frame->WriteVelocity(command.theta_rad_rate / M_PI, format.theta_rad_rate); + } + + return 0; + } +}; + +/// DQ phase current command. +struct CurrentMode { + struct Command { + double d_A = 0.0; + double q_A = 0.0; + }; + + struct Format { + Resolution d_A = kFloat; + Resolution q_A = kFloat; + }; + + static uint8_t Make(WriteCanData* frame, + const Command& command, + const Format& format) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kCurrent); + + const Resolution kResolutions[] = { + format.d_A, + format.q_A, + }; + + WriteCombiner combiner( + frame, 0x00, + Register::kCommandQCurrent, + kResolutions, + sizeof(kResolutions) / sizeof(*kResolutions)); + + if (combiner.MaybeWrite()) { + frame->WriteCurrent(command.q_A, format.q_A); + } + if (combiner.MaybeWrite()) { + frame->WriteCurrent(command.d_A, format.d_A); + } + + return 0; + } +}; + +struct StayWithinMode { + struct Command { + double lower_bound = NaN; + double upper_bound = NaN; + double feedforward_torque = 0.0; + double kp_scale = 1.0; + double kd_scale = 1.0; + double maximum_torque = 0.0; + double watchdog_timeout = NaN; + double ilimit_scale = 1.0; + }; + + struct Format { + Resolution lower_bound = kFloat; + Resolution upper_bound = kFloat; + Resolution feedforward_torque = kIgnore; + Resolution kp_scale = kIgnore; + Resolution kd_scale = kIgnore; + Resolution maximum_torque = kIgnore; + Resolution watchdog_timeout = kIgnore; + Resolution ilimit_scale = kIgnore; + }; + + static uint8_t Make(WriteCanData* frame, + const Command& command, + const Format& format) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kStayWithin); + + const Resolution kResolutions[] = { + format.lower_bound, + format.upper_bound, + format.feedforward_torque, + format.kp_scale, + format.kd_scale, + format.maximum_torque, + format.watchdog_timeout, + format.ilimit_scale, + }; + + WriteCombiner combiner( + frame, 0x00, + Register::kCommandStayWithinLowerBound, + kResolutions, + sizeof(kResolutions) / sizeof(*kResolutions)); + + if (combiner.MaybeWrite()) { + frame->WritePosition(command.lower_bound, format.lower_bound); + } + if (combiner.MaybeWrite()) { + frame->WritePosition(command.upper_bound, format.upper_bound); + } + if (combiner.MaybeWrite()) { + frame->WriteTorque(command.feedforward_torque, + format.feedforward_torque); + } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.kp_scale, format.kp_scale); + } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.kd_scale, format.kd_scale); + } + if (combiner.MaybeWrite()) { + frame->WriteTorque(command.maximum_torque, format.maximum_torque); + } + if (combiner.MaybeWrite()) { + frame->WriteTime(command.watchdog_timeout, format.watchdog_timeout); + } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.ilimit_scale, format.ilimit_scale); + } + return 0; + } +}; + +struct BrakeMode { + struct Command {}; + struct Format {}; + + static uint8_t Make(WriteCanData* frame, + const Command&, + const Format&) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kBrake); + return 0; + } +}; + +struct StopMode { + struct Command {}; + struct Format {}; + + static uint8_t Make(WriteCanData* frame, + const Command&, + const Format&) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kStopped); + return 0; + } +}; + +struct GpioWrite { + struct Command { + int8_t aux1 = 0; + int8_t aux2 = 0; + }; + + struct Format { + Resolution aux1 = kInt8; + Resolution aux2 = kInt8; + }; + + static uint8_t Make(WriteCanData* frame, + const Command& command, + const Format& format) { + const Resolution kResolutions[] = { + format.aux1, + format.aux2, + }; + + WriteCombiner combiner( + frame, 0x00, + Register::kAux1GpioCommand, + kResolutions, + sizeof(kResolutions) / sizeof(*kResolutions)); + + if (combiner.MaybeWrite()) { + frame->WriteInt(command.aux1, format.aux1); + } + if (combiner.MaybeWrite()) { + frame->WriteInt(command.aux2, format.aux2); + } + return 0; + } +}; + +struct OutputNearest { + struct Command { + double position = 0.0; + }; + + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command& command, const Format&) { + frame->Write(Multiplex::kWriteFloat | 0x01); + frame->WriteVaruint(Register::kSetOutputNearest); + frame->Write(command.position); + return 0; + } +}; + +struct OutputExact { + struct Command { + double position = 0.0; + }; + + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command& command, const Format&) { + frame->Write(Multiplex::kWriteFloat | 0x01); + frame->WriteVaruint(Register::kSetOutputExact); + frame->Write(command.position); + return 0; + } +}; + +struct RequireReindex { + struct Command {}; + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command&, const Format&) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->WriteVaruint(Register::kRequireReindex); + frame->Write(1); + return 0; + } +}; + +struct RecapturePositionVelocity { + struct Command {}; + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command&, const Format&) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->WriteVaruint(Register::kRecapturePositionVelocity); + frame->Write(1); + return 0; + } +}; + +struct DiagnosticWrite { + struct Command { + int8_t channel = 1; + const char* data = nullptr; + int8_t size = 0; + }; + + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command& command, const Format&) { + frame->Write(Multiplex::kClientToServer); + frame->Write(command.channel); + frame->Write(command.size); + frame->Write(command.data, command.size); + return 0; + } +}; + +struct DiagnosticRead { + struct Command { + int8_t channel = 1; + int8_t max_length = 48; + }; + + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command& command, const Format&) { + frame->Write(Multiplex::kClientPollServer); + frame->Write(command.channel); + frame->Write(command.max_length); + return command.max_length + 3; + } +}; + +struct DiagnosticResponse { + struct Result { + int8_t channel = 1; + uint8_t data[64] = {}; + int8_t size = 0; + }; + + static Result Parse(const uint8_t* data, uint8_t size) { + MultiplexParser parser(data, size); + return Parse(&parser); + } + + static Result Parse(MultiplexParser* parser) { + Result result; + result.channel = -1; + + if (parser->remaining() < 3) { return result; } + + const auto action = parser->Read(); + if (action != Multiplex::kServerToClient) { return result; } + const auto channel = parser->Read(); + + const uint16_t size = parser->ReadVaruint(); + if (parser->remaining() < size) { return result; } + + result.channel = channel; + result.size = size; + parser->ReadRaw(result.data, size); + + return result; + } +}; + +struct ClockTrim { + struct Command { + int32_t trim = 0; + }; + + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command& command, const Format&) { + frame->Write(Multiplex::kWriteInt32 | 0x01); + frame->WriteVaruint(Register::kClockTrim); + frame->Write(command.trim); + return 0; + } +}; + +} +} diff --git a/deps/mjbots/moteus/moteus_tokenizer.h b/deps/mjbots/moteus/moteus_tokenizer.h new file mode 100644 index 00000000..8ee6b76e --- /dev/null +++ b/deps/mjbots/moteus/moteus_tokenizer.h @@ -0,0 +1,62 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +namespace mjbots { + namespace moteus { + namespace detail { + + class Tokenizer { + public: + Tokenizer(const std::string& source, const char* delimiters) + : source_(source), + delimiters_(delimiters), + position_(source_.cbegin()) {} + + std::string next() { + if (position_ == source_.end()) { return std::string(); } + + const auto start = position_; + auto my_next = position_; + bool found = false; + for (; my_next != source_.end(); ++my_next) { + if (std::strchr(delimiters_, *my_next) != nullptr) { + position_ = my_next; + ++position_; + found = true; + break; + } + } + if (!found) { position_ = my_next; } + return std::string(&*start, my_next - start); + } + + std::string remaining() const { + return std::string(&*position_, source_.end() - position_); + } + + private: + const std::string source_; + const char* const delimiters_; + std::string::const_iterator position_; + }; + + + } // namespace detail + } // namespace moteus +} // namespace mjbots diff --git a/deps/mjbots/moteus/moteus_transport.h b/deps/mjbots/moteus/moteus_transport.h new file mode 100644 index 00000000..822c3c9d --- /dev/null +++ b/deps/mjbots/moteus/moteus_transport.h @@ -0,0 +1,1128 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "moteus_protocol.h" +#include "moteus_tokenizer.h" + +#ifdef CANFD_FDF +#define MJBOTS_MOTEUS_ENABLE_SOCKETCAN 1 +#endif + +namespace mjbots { +namespace moteus { + +using CompletionCallback = std::function; + +/// Turn async methods into synchronous ones. +class BlockingCallback { + public: + /// Pass the result of this to a singular async call. + CompletionCallback callback() { + return [&](int v) { + std::unique_lock lock(mutex_); + done_.store(true); + result_.store(v); + cv_.notify_one(); + }; + } + + /// Then call this to perform the blocking. + int Wait() { + cv_.wait(lock_, [&]() { return done_.load(); }); + return result_.load(); + } + + private: + std::atomic done_{false}; + std::atomic result_{0}; + std::recursive_mutex mutex_; + std::condition_variable_any cv_; + std::unique_lock lock_{mutex_}; +}; + +class Transport { + public: + virtual ~Transport() {} + + /// Start sending all the frames in the @p frames / @p size list. + /// + /// Any replies are collated in the @p replies result. + /// + /// Upon completion, invoke @p completed_callback from an arbitrary + /// thread that may or may not be the calling one and may or may not + /// be invoked rentrantly from within this call. + virtual void Cycle(const CanFdFrame* frames, + size_t size, + std::vector* replies, + CompletionCallback completed_callback) = 0; + + /// The same operation as above, but block until it is complete. + /// + /// A default implementation is provided which delegates to the + /// asynchronous version. + virtual void BlockingCycle(const CanFdFrame* frames, + size_t size, + std::vector* replies) { + BlockingCallback cbk; + + this->Cycle(frames, size, replies, cbk.callback()); + + cbk.Wait(); + } + + /// Schedule the given callback to be invoked at a later time. This + /// will either be invoked from an arbitrary thread, or from within + /// another call to "Cycle or BlockingCycle". + virtual void Post(std::function callback) = 0; +}; + +namespace details { +/// This is just a simple RAII class for managing file descriptors. +class FileDescriptor { + public: + FileDescriptor() {} + FileDescriptor(int fd) { fd_ = fd; } + ~FileDescriptor() { + if (fd_ >= 0) { ::close(fd_); } + } + + FileDescriptor& operator=(int fd) { + if (fd_ >= 0) { ::close(fd_); } + fd_ = fd; + return *this; + } + + bool operator==(const FileDescriptor& rhs) const { + return fd_ == rhs.fd_; + } + + operator int() const { + return fd_; + } + + int release() { + const auto result = fd_; + fd_ = -1; + return result; + } + + private: + int fd_ = -1; +}; + +/// A basic event loop implemented using C++11 primitives. +class ThreadedEventLoop { + public: + ThreadedEventLoop() { + thread_ = std::thread(std::bind(&ThreadedEventLoop::CHILD_Run, this)); + } + + ~ThreadedEventLoop() { + { + std::unique_lock lock(mutex_); + done_ = true; + do_something_ = true; + something_cv_.notify_one(); + } + thread_.join(); + } + + // This is purely to catch out of control queues earlier, as + // typically there will just be 1 outstanding event at a time. + static constexpr int kMaxQueueSize = 2; + + void Post(std::function callback) { + std::unique_lock lock(mutex_); + event_queue_.push_back(std::move(callback)); + if (event_queue_.size() > kMaxQueueSize) { + throw std::runtime_error("There should never be more than one!"); + } + do_something_ = true; + something_cv_.notify_one(); + } + + private: + void CHILD_Run() { + std::unique_lock lock(mutex_); + + while (true) { + something_cv_.wait(lock, [&]() { + return do_something_ || !event_queue_.empty(); + }); + do_something_ = false; + + if (done_) { + return; + } + + // Do at most one event. + if (!event_queue_.empty()) { + auto top = event_queue_.front(); + event_queue_.pop_front(); + top(); + } + } + } + + std::thread thread_; + + // The following variables are controlled by 'something_mutex'. + std::recursive_mutex mutex_; + std::condition_variable_any something_cv_; + + bool do_something_ = false; + bool done_ = false; + + std::deque> event_queue_; +}; + +/// A helper base class for transports that want to manage timeout +/// behavior in a similar manner. +class TimeoutTransport : public Transport { + public: + struct Options { + bool disable_brs = false; + + uint32_t min_ok_wait_ns = 1000000; + uint32_t min_rcv_wait_ns = 5000000; + + uint32_t rx_extra_wait_ns = 5000000; + + // Send at most this many frames before waiting for responses. -1 + // means no limit. + int max_pipeline = -1; + + Options() {} + }; + + TimeoutTransport(const Options& options) : t_options_(options) {} + + virtual void Cycle(const CanFdFrame* frames, + size_t size, + std::vector* replies, + CompletionCallback completed_callback) override { + // The event loop should never be empty here, but we make a copy + // just to assert that. + auto copy = std::atomic_load(&UNPROTECTED_event_loop_); + FailIf(!copy, "unexpected null event loop"); + copy->Post( + std::bind(&TimeoutTransport::CHILD_Cycle, + this, frames, size, replies, completed_callback)); + } + + virtual void Post(std::function callback) override { + // We might have an attempt to post an event while we are being + // destroyed. In that case, just ignore it. + auto copy = std::atomic_load(&UNPROTECTED_event_loop_); + if (copy) { + copy->Post(callback); + } + } + + static int64_t GetNow() { + struct timespec ts = {}; + ::clock_gettime(CLOCK_MONOTONIC_RAW, &ts); + return static_cast(ts.tv_sec) * 1000000000ll + + static_cast(ts.tv_nsec); + } + + static void Fail(const std::string& message) { + throw std::runtime_error(message); + } + + static void FailIf(bool terminate, const std::string& message) { + if (terminate) { + Fail(message); + } + } + + static void FailIfErrno(bool terminate) { + if (terminate) { + Fail(::strerror(errno)); + } + } + + + protected: + virtual int CHILD_GetReadFd() const = 0; + virtual void CHILD_SendCanFdFrame(const CanFdFrame&) = 0; + + struct ConsumeCount { + int rcv = 0; + int ok = 0; + }; + + virtual ConsumeCount CHILD_ConsumeData( + std::vector* replies, + int expected_ok_count, + std::vector* expected_reply_count) = 0; + virtual void CHILD_FlushTransmit() = 0; + + void CHILD_Cycle(const CanFdFrame* frames, + size_t size, + std::vector* replies, + CompletionCallback completed_callback) { + if (replies) { replies->clear(); } + CHILD_CheckReplies(replies, kFlush, 0, nullptr); + + const auto advance = t_options_.max_pipeline < 0 ? + size : t_options_.max_pipeline; + + for (size_t start = 0; start < size; start += advance) { + int expected_ok_count = 0; + for (auto& v : expected_reply_count_) { v = 0; } + + for (size_t i = start; i < (start + advance) && i < size; i++) { + expected_ok_count++; + CHILD_SendCanFdFrame(frames[i]); + if (frames[i].reply_required) { + if ((frames[i].destination + 1) > + static_cast(expected_reply_count_.size())) { + expected_reply_count_.resize(frames[i].destination + 1); + } + expected_reply_count_[frames[i].destination]++; + } + } + + CHILD_FlushTransmit(); + + CHILD_CheckReplies(replies, + kWait, + expected_ok_count, + &expected_reply_count_); + } + + Post(std::bind(completed_callback, 0)); + } + + enum ReadDelay { + kWait, + kFlush, + }; + + void CHILD_CheckReplies(std::vector* replies, + ReadDelay read_delay, + int expected_ok_count, + std::vector* expected_reply_count) { + const auto start = GetNow(); + + const auto any_reply_checker = [&]() { + if (!expected_reply_count) { return false; } + for (auto v : *expected_reply_count) { + if (v) { return true; } + } + return false; + }; + auto end_time = + start + + (read_delay == kWait ? + std::max(expected_ok_count != 0 ? t_options_.min_ok_wait_ns : 0, + any_reply_checker() ? t_options_.min_rcv_wait_ns : 0) : + 5000); + + struct pollfd fds[1] = {}; + fds[0].fd = CHILD_GetReadFd(); + fds[0].events = POLLIN; + + int ok_count = 0; + + while (true) { + const auto now = GetNow(); + fds[0].revents = 0; + + struct timespec tmo = {}; + const auto to_sleep_ns = std::max(0, end_time - now); + tmo.tv_sec = to_sleep_ns / 1000000000; + tmo.tv_nsec = to_sleep_ns % 1000000000; + + const int poll_ret = ::ppoll(&fds[0], 1, &tmo, nullptr); + if (poll_ret < 0) { + if (errno == EINTR) { + // Go back and try again. + continue; + } + FailIfErrno(true); + } + if (poll_ret == 0) { return; } + + const auto consume_count = CHILD_ConsumeData( + replies, expected_ok_count, expected_reply_count); + + ok_count += consume_count.ok; + + if (read_delay != kFlush && + !any_reply_checker() && ok_count >= expected_ok_count) { + // Once we have the expected number of CAN replies and OKs, + // return immediately. + return; + } + + if (consume_count.rcv || consume_count.ok) { + const auto finish_time = GetNow(); + end_time = finish_time + t_options_.rx_extra_wait_ns; + } + } + } + + // This is protected, because derived classes need to delete it + // before freeing any file descriptors. The public methods of the + // ThreadedEventLoop require no locking, but the shared_ptr itself + // requires either synchronization or access using the atomic std + // library methods. We'll exclusively use the atomic std library + // methods. + std::shared_ptr UNPROTECTED_event_loop_ = + std::make_shared(); + + private: + const Options t_options_; + + std::vector expected_reply_count_; +}; +} + +class Fdcanusb : public details::TimeoutTransport { + public: + struct Options : details::TimeoutTransport::Options { + Options() {} + }; + + // If @p device is empty, attempt to auto-detect a fdcanusb in the + // system. + Fdcanusb(const std::string& device_in, const Options& options = {}) + : details::TimeoutTransport(options), + options_(options) { + Open(device_in); + } + + // This constructor overload is intended for use in unit tests, + // where the file descriptors will likely be pipes. + Fdcanusb(int read_fd, int write_fd, const Options& options = {}) + : details::TimeoutTransport(options), + options_(options) { + Open(read_fd, write_fd); + } + + virtual ~Fdcanusb() { + std::atomic_store(&UNPROTECTED_event_loop_, {}); + + if (read_fd_ == write_fd_) { + write_fd_.release(); + } + } + + static std::string DetectFdcanusb() { + // For now, we'll only do linux like systems. + { + std::ifstream inf("/dev/fdcanusb"); + if (inf.is_open()) { return "/dev/fdcanusb"; } + } + + { + glob_t glob_data = {}; + const int result = ::glob( + "/dev/serial/by-id/*fdcanusb*", 0, + nullptr, + &glob_data); + + std::string maybe_path; + + if (result == 0 && glob_data.gl_pathc > 0) { + maybe_path = glob_data.gl_pathv[0]; + } + + globfree(&glob_data); + + if (!maybe_path.empty()) { return maybe_path; } + } + + return ""; + } + + private: + void Open(const std::string& device_in) { + std::string device = device_in; + if (device.empty()) { + device = DetectFdcanusb(); + if (device.empty()) { + throw std::runtime_error("Could not detect fdcanusb"); + } + } + + const int fd = ::open(device.c_str(), O_RDWR | O_NOCTTY); + FailIfErrno(fd == -1); + +#ifndef _WIN32 + { + struct serial_struct serial; + FailIfErrno(::ioctl(fd, TIOCGSERIAL, &serial) < 0); + serial.flags |= ASYNC_LOW_LATENCY; + FailIfErrno(::ioctl(fd, TIOCSSERIAL, &serial) < 0); + + struct termios toptions; + FailIfErrno(::tcgetattr(fd, &toptions) < 0); + + // Turn off things that could munge our byte stream to the + // device. + toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + toptions.c_oflag &= ~OPOST; + + FailIfErrno(::tcsetattr(fd, TCSANOW, &toptions) < 0); + FailIfErrno(::tcsetattr(fd, TCSAFLUSH, &toptions) < 0); + } +#else // _WIN32 + { + // Windows is likely broken for many other reasons, but if we do + // fix all the other problems, this will be necessary. + COMMTIMEOUTS new_timeouts = {MAXDWORD, 0, 0, 0, 0}; + SetCommTimeouts(fd, &new_timeouts); + } +#endif + + Open(fd, fd); + } + + void Open(int read_fd, int write_fd) { + read_fd_ = read_fd; + write_fd_ = write_fd; + } + + virtual int CHILD_GetReadFd() const override { + return read_fd_; + } + + virtual ConsumeCount CHILD_ConsumeData( + std::vector* replies, + int /* expected_ok_count */, + std::vector* expected_reply_count) override { + // Read into our line buffer. + const int to_read = sizeof(line_buffer_) - line_buffer_pos_; + const int read_ret = ::read( + read_fd_, &line_buffer_[line_buffer_pos_], to_read); + if (read_ret < 0) { + if (errno == EINTR || errno == EAGAIN) { return {}; } + FailIfErrno(true); + } + line_buffer_pos_ += read_ret; + + const auto consume_count = CHILD_ConsumeLines( + replies, expected_reply_count); + if (line_buffer_pos_ >= sizeof(line_buffer_)) { + // We overran our line buffer. For now, just drop everything + // and start from 0. + line_buffer_pos_ = 0; + } + + return consume_count; + } + + /// Return the number of CAN frames received. + ConsumeCount CHILD_ConsumeLines(std::vector* replies, + std::vector* expected_reply_count) { + const auto start_size = replies ? replies->size() : 0; + ConsumeCount result; + while (CHILD_ConsumeLine(replies, &result.ok, expected_reply_count)) {} + result.rcv = replies ? (replies->size() - start_size) : 0; + return result; + } + + bool CHILD_ConsumeLine(std::vector* replies, int* ok_count, + std::vector* expected_reply_count) { + const auto line_end = [&]() -> int { + for (size_t i = 0; i < line_buffer_pos_; i++) { + if (line_buffer_[i] == '\r' || line_buffer_[i] == '\n') { return i; } + } + return -1; + }(); + if (line_end < 0) { return false; } + + CHILD_ProcessLine(std::string(&line_buffer_[0], line_end), replies, + ok_count, expected_reply_count); + + std::memmove(&line_buffer_[0], &line_buffer_[line_end + 1], + line_buffer_pos_ - line_end - 1); + line_buffer_pos_ -= (line_end + 1); + + return true; + } + + void CHILD_ProcessLine(const std::string& line, + std::vector* replies, + int* ok_count, + std::vector* expected_reply_count) { + if (line == "OK") { + (*ok_count)++; + return; + } + + // The only line we actually do anything with is a "rcv" line. + detail::Tokenizer tokenizer(line, " "); + const auto start = tokenizer.next(); + if (start != "rcv") { return; } + const auto address = tokenizer.next(); + const auto data = tokenizer.next(); + + if (address.empty() || data.empty()) { return; } + + CanFdFrame this_frame; + this_frame.arbitration_id = std::stoul(address, nullptr, 16); + this_frame.destination = this_frame.arbitration_id & 0x7f; + this_frame.source = (this_frame.arbitration_id >> 8) & 0x7f; + this_frame.can_prefix = (this_frame.arbitration_id >> 16); + + this_frame.size = ParseCanData(data, this_frame.data); + + while (true) { + const auto maybe_flags = tokenizer.next(); + if (maybe_flags.empty()) { break; } + if (maybe_flags.size() != 1) { continue; } + for (const char c : maybe_flags) { + if (c == 'b') { this_frame.brs = CanFdFrame::kForceOff; } + if (c == 'B') { this_frame.brs = CanFdFrame::kForceOn; } + if (c == 'f') { this_frame.fdcan_frame = CanFdFrame::kForceOff; } + if (c == 'F') { this_frame.fdcan_frame = CanFdFrame::kForceOn; } + } + } + + if (expected_reply_count) { + if (this_frame.source < + static_cast(expected_reply_count->size())) { + (*expected_reply_count)[this_frame.source] = std::max( + (*expected_reply_count)[this_frame.source] - 1, 0); + } + } + + if (replies) { + replies->emplace_back(std::move(this_frame)); + } + } + + struct Printer { + Printer(char* buf, size_t capacity) : buf_(buf), capacity_(capacity) {}; + + const char* buf() { return buf_; } + size_t size() const { return pos_; } + size_t remaining() const { return capacity_ - pos_ - 1; } + + void operator()(const char* fmt, ...) { + va_list ap; + va_start(ap, fmt); + auto n = ::vsnprintf(&buf_[pos_], remaining(), fmt, ap); + va_end(ap); + if (n < 0) { ::abort(); } + pos_ += n; + }; + + char* const buf_; + size_t pos_ = 0; + const size_t capacity_; + }; + + virtual void CHILD_SendCanFdFrame(const CanFdFrame& frame) override { + char buf[256] = {}; + + Printer p(buf, sizeof(buf)); + + p("can send %04x ", frame.arbitration_id); + + const auto dlc = RoundUpDlc(frame.size); + for (size_t i = 0; i < frame.size; i++) { + p("%02x", static_cast(frame.data[i])); + } + for (size_t i = frame.size; i < dlc; i++) { + p("50"); + } + + if (options_.disable_brs || frame.brs == CanFdFrame::kForceOff) { + p(" b"); + } else if (frame.brs == CanFdFrame::kForceOn) { + p(" B"); + } + if (frame.fdcan_frame == CanFdFrame::kForceOff) { + p(" f"); + } else if (frame.fdcan_frame == CanFdFrame::kForceOn) { + p(" F"); + } + p("\n"); + + if (p.size() > (sizeof(tx_buffer_) - tx_buffer_size_)) { + CHILD_FlushTransmit(); + } + + std::memcpy(&tx_buffer_[tx_buffer_size_], &buf[0], p.size()); + tx_buffer_size_ += p.size(); + } + + virtual void CHILD_FlushTransmit() override { + for (size_t n = 0; n < tx_buffer_size_; ) { + int ret = ::write(write_fd_, &tx_buffer_[n], tx_buffer_size_ - n); + if (ret < 0) { + if (errno == EINTR || errno == EAGAIN) { continue; } + + FailIfErrno(true); + } else { + n += ret; + } + } + tx_buffer_size_ = 0; + } + + static size_t RoundUpDlc(size_t size) { + if (size <= 8) { return size; } + if (size <= 12) { return 12; } + if (size <= 16) { return 16; } + if (size <= 20) { return 20; } + if (size <= 24) { return 24; } + if (size <= 32) { return 32; } + if (size <= 48) { return 48; } + if (size <= 64) { return 64; } + return size; + } + + static int ParseHexNybble(char c) { + if (c >= '0' && c <= '9') { return c - '0'; } + if (c >= 'a' && c <= 'f') { return c - 'a' + 10; } + if (c >= 'A' && c <= 'F') { return c - 'A' + 10; } + return -1; + } + + static int ParseHexByte(const char* value) { + const int high = ParseHexNybble(value[0]); + if (high < 0) { return high; } + const int low = ParseHexNybble(value[1]); + if (low < 0) { return low; } + return (high << 4) | low; + } + + static int ParseCanData(const std::string& data, uint8_t* out) { + size_t to_read = std::min(64 * 2, data.size()); + for (size_t i = 0; i < to_read; i+= 2) { + out[i / 2] = ParseHexByte(&data[i]); + } + return to_read / 2; + } + + // This is set in the parent, then used in the child. + const Options options_; + + // We have these scoped file descriptors first in our member list, + // so they will only be closed after the threaded event loop has + // been destroyed during destruction. + details::FileDescriptor read_fd_; + details::FileDescriptor write_fd_; + + // The following variables are only used in the child. + char line_buffer_[4096] = {}; + size_t line_buffer_pos_ = 0; + + char tx_buffer_[4096] = {}; + size_t tx_buffer_size_ = 0; +}; + + +#ifdef MJBOTS_MOTEUS_ENABLE_SOCKETCAN +class Socketcan : public details::TimeoutTransport { + public: + struct Options : details::TimeoutTransport::Options { + std::string ifname = "can0"; + + Options() {} + }; + + Socketcan(const Options& options) + : details::TimeoutTransport(options), + options_(options) { + socket_ = Open(options_.ifname); + } + + virtual ~Socketcan() { + std::atomic_store(&UNPROTECTED_event_loop_, {}); + } + + private: + static void SetNonblock(int fd) { + int flags = ::fcntl(fd, F_GETFL, 0); + FailIf(flags < 0, "error getting flags"); + flags |= O_NONBLOCK; + FailIf(::fcntl(fd, F_SETFL, flags), "error setting flags"); + } + + static int Open(const std::string& ifname) { + const int fd = ::socket(PF_CAN, SOCK_RAW, CAN_RAW); + FailIf(fd < 0, "error opening CAN socket"); + + SetNonblock(fd); + + struct ifreq ifr = {}; + std::strncpy(&ifr.ifr_name[0], ifname.c_str(), + sizeof(ifr.ifr_name) - 1); + FailIf(::ioctl(fd, SIOCGIFINDEX, &ifr) < 0, + "could not find CAN: " + ifname); + + const int enable_canfd = 1; + FailIf(::setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, + &enable_canfd, sizeof(enable_canfd)) != 0, + "could not set CAN-FD mode"); + + struct sockaddr_can addr = {}; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + FailIf(::bind(fd, + reinterpret_cast(&addr), + sizeof(addr)) < 0, + "could not bind to CAN if"); + + return fd; + } + + virtual int CHILD_GetReadFd() const override { + return socket_; + } + + virtual void CHILD_SendCanFdFrame(const CanFdFrame& frame) override { + struct canfd_frame send_frame = {}; + send_frame.can_id = frame.arbitration_id; + if (send_frame.can_id >= 0x7ff) { + // Set the frame format flag if we need an extended ID. + send_frame.can_id |= (1 << 31); + } + send_frame.len = frame.size; + std::memcpy(send_frame.data, frame.data, frame.size); + + using F = CanFdFrame; + + send_frame.flags = + ((frame.fdcan_frame == F::kDefault || + frame.fdcan_frame == F::kForceOn) ? CANFD_FDF : 0) | + (((frame.brs == F::kDefault && !options_.disable_brs) || + frame.brs == F::kForceOn) ? CANFD_BRS : 0); + + FailIf(::write(socket_, &send_frame, sizeof(send_frame)) < 0, + "error writing CAN"); + } + + virtual ConsumeCount CHILD_ConsumeData( + std::vector* replies, + int /* expected_ok_count */, + std::vector* expected_reply_count) override { + struct canfd_frame recv_frame = {}; + FailIf(::read(socket_, &recv_frame, sizeof(recv_frame)) < 0, + "error reading CAN frame"); + + CanFdFrame this_frame; + this_frame.arbitration_id = recv_frame.can_id & 0x1fffffff; + this_frame.destination = this_frame.arbitration_id & 0x7f; + this_frame.source = (this_frame.arbitration_id >> 8) & 0x7f; + this_frame.can_prefix = (this_frame.arbitration_id >> 16); + + this_frame.brs = (recv_frame.flags & CANFD_BRS) ? + CanFdFrame::kForceOn : CanFdFrame::kForceOff; + this_frame.fdcan_frame = (recv_frame.flags & CANFD_FDF) ? + CanFdFrame::kForceOn : CanFdFrame::kForceOff; + + std::memcpy(this_frame.data, recv_frame.data, recv_frame.len); + this_frame.size = recv_frame.len; + + if (expected_reply_count) { + if (this_frame.source < + static_cast(expected_reply_count->size())) { + (*expected_reply_count)[this_frame.source] = std::max( + (*expected_reply_count)[this_frame.source] - 1, 0); + } + } + + if (replies) { + replies->emplace_back(std::move(this_frame)); + } + + ConsumeCount result; + result.ok = 1; + result.rcv = 1; + return result; + } + + virtual void CHILD_FlushTransmit() override {} + + const Options options_; + details::FileDescriptor socket_; +}; +#endif // MJBOTS_MOTEUS_ENABLE_SOCKETCAN + +/// A factory which can create transports given an optional set of +/// commandline arguments. +class TransportFactory { + public: + virtual ~TransportFactory() {} + + virtual int priority() = 0; + virtual std::string name() = 0; + using TransportArgPair = std::pair, + std::vector>; + virtual TransportArgPair make(const std::vector&) = 0; + + struct Argument { + std::string name; + int nargs = 1; + std::string help; + + bool operator<(const Argument& rhs) const { + if (name < rhs.name) { return true; } + if (name > rhs.name) { return false; } + return help < rhs.help; + } + + Argument(const std::string& name_in, + int nargs_in, + const std::string& help_in) + : name(name_in), + nargs(nargs_in), + help(help_in) {} + }; + + virtual std::vector cmdline_arguments() = 0; + + virtual bool is_args_set(const std::vector&) = 0; +}; + +class FdcanusbFactory : public TransportFactory { + public: + virtual ~FdcanusbFactory() {} + + virtual int priority() override { return 10; } + virtual std::string name() override { return "fdcanusb"; } + + virtual TransportArgPair make(const std::vector& args_in) override { + auto args = args_in; + + Fdcanusb::Options options; + std::string device; + + { + auto it = std::find(args.begin(), args.end(), "--can-disable-brs"); + if (it != args.end()) { + options.disable_brs = true; + args.erase(it); + } + } + + { + auto it = std::find(args.begin(), args.end(), "--fdcanusb"); + if (it != args.end()) { + if ((it + 1) != args.end()) { + device = *(it + 1); + args.erase(it, it + 2); + } else { + throw std::runtime_error("--fdcanusb requires a path"); + } + } + } + + auto result = std::make_shared(device, options); + return TransportArgPair(result, args); + } + + virtual std::vector cmdline_arguments() override { + return { + { "--fdcanusb", 1, "path to fdcanusb device" }, + { "--can-disable-brs", 0, "do not set BRS" }, + }; + } + + virtual bool is_args_set(const std::vector& args) override { + for (const auto& arg : args) { + if (arg == "--fdcanusb") { return true; } + } + return false; + } +}; + +#ifdef MJBOTS_MOTEUS_ENABLE_SOCKETCAN +class SocketcanFactory : public TransportFactory { + public: + virtual ~SocketcanFactory() {} + + virtual int priority() override { return 11; } + virtual std::string name() override { return "socketcan"; } + + virtual TransportArgPair make(const std::vector& args_in) override { + auto args = args_in; + + Socketcan::Options options; + std::string device; + + { + auto it = std::find(args.begin(), args.end(), "--can-disable-brs"); + if (it != args.end()) { + options.disable_brs = true; + args.erase(it); + } + } + + { + auto it = std::find(args.begin(), args.end(), "--socketcan-iface"); + if (it != args.end()) { + if ((it + 1) != args.end()) { + options.ifname = *(it + 1); + args.erase(it, it + 2); + } else { + throw std::runtime_error("--socketcan-iface requires an interface name"); + } + } + } + + auto result = std::make_shared(options); + return TransportArgPair(result, args); + } + + virtual std::vector cmdline_arguments() override { + return { + { "--socketcan-iface", 1, "socketcan iface name" }, + { "--can-disable-brs", 0, "do not set BRS" }, + }; + } + + virtual bool is_args_set(const std::vector& args) override { + for (const auto& arg : args) { + if (arg == "--socketcan-iface") { return true; } + } + return false; + } +}; +#endif // MJBOTS_MOTEUS_ENABLE_SOCKETCAN + +class TransportRegistry { + public: + template + void Register() { + items_.push_back(std::make_shared()); + } + + static TransportRegistry& singleton() { + static TransportRegistry reg; + return reg; + } + + std::vector cmdline_arguments() const { + std::vector result; + std::set uniqifier; + + result.push_back(TransportFactory::Argument( + "--force-transport", 1, + "force the given transport type to be used")); + uniqifier.insert(result.back()); + + for (const auto& item : items_) { + const auto item_args = item->cmdline_arguments(); + for (const auto& arg : item_args) { + if (uniqifier.count(arg) == 0) { + result.push_back(arg); + uniqifier.insert(arg); + } + } + } + + return result; + } + + TransportFactory::TransportArgPair make(const std::vector& args_in) const { + auto args = args_in; + auto to_try = items_; + + std::sort(to_try.begin(), to_try.end(), + [](const std::shared_ptr& lhs, + const std::shared_ptr& rhs) { + return lhs->priority() < rhs->priority(); + }); + + // Is the transport forced? + const auto it = std::find(args.begin(), args.end(), "--force-transport"); + if (it != args.end()) { + if ((it + 1) != args.end()) { + to_try = {}; + const auto name_to_find = *(it + 1); + for (auto item : items_) { + if (item->name() == name_to_find) { to_try.push_back(item); } + } + args.erase(it, it + 2); + } else { + throw std::runtime_error("--force-transport requires an argument"); + } + } else { + std::vector> options_set; + for (auto item : items_) { + if (item->is_args_set(args)) { + options_set.push_back(item); + } + } + + if (!options_set.empty()) { to_try = options_set; } + } + + std::string errors; + for (auto factory : to_try) { + try { + auto maybe_result = factory->make(args); + if (maybe_result.first) { + return maybe_result; + } + } catch (std::runtime_error& re) { + if (!errors.empty()) { errors += ", "; } + errors += factory->name() + ": " + re.what(); + } + } + throw std::runtime_error("Unable to find a default transport: " + errors); + } + + private: + TransportRegistry() { + Register(); +#ifdef MJBOTS_MOTEUS_ENABLE_SOCKETCAN + Register(); +#endif + } + + std::vector> items_; +}; + +} +} diff --git a/esw/can_bridge/can_bridge.cpp b/esw/can_bridge/can_bridge.cpp new file mode 100644 index 00000000..2b6e5a55 --- /dev/null +++ b/esw/can_bridge/can_bridge.cpp @@ -0,0 +1,229 @@ +#include "can_bridge.hpp" + +namespace mrover { + + static auto checkSyscallResult(int result) -> int { + if (result < 0) throw std::system_error{errno, std::generic_category()}; + + return result; + } + + static auto checkErrorCode(boost::system::error_code const& ec) -> void { + if (ec.value() == boost::system::errc::success) return; + + throw std::runtime_error(std::format("Boost failure: {} {}", ec.value(), ec.message())); + } + + static auto nearestFittingFdcanFrameSize(std::size_t size) -> std::uint8_t { + if (size <= 8) return size; + if (size <= 12) return 12; + if (size <= 16) return 16; + if (size <= 20) return 20; + if (size <= 24) return 24; + if (size <= 32) return 32; + if (size <= 48) return 48; + if (size <= 64) return 64; + throw std::runtime_error("Too large!"); + } + + CanBridge::CanBridge() : Node{"can_bridge", rclcpp::NodeOptions{} + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)} { + try { + RCLCPP_INFO_STREAM(get_logger(), "Starting..."); + + declare_parameter("interface", rclcpp::ParameterType::PARAMETER_STRING); + declare_parameter("is_extended_frame", rclcpp::ParameterType::PARAMETER_BOOL); + declare_parameter("bitrate_switch", rclcpp::ParameterType::PARAMETER_BOOL); + + mInterface = get_parameter("interface").as_string(); + mBus = mInterface.back() - '0'; + + // As far as I can tell array of structs in YAML is not even supported by ROS 2 as compared to ROS 1 + // See: https://robotics.stackexchange.com/questions/109909/reading-a-vector-of-structs-as-parameters-in-ros2 + + std::map devices; + get_parameters("devices", devices); + + if (devices.empty()) { + RCLCPP_FATAL_STREAM(get_logger(), "No devices specified. Did you forget to load the correct ROS parameters?"); + rclcpp::shutdown(); + } + + std::set names; + std::ranges::transform(devices | std::views::keys, std::inserter(names, names.end()), [](std::string const& fullName) { + return fullName.substr(0, fullName.find('.')); + }); + + for (auto const& name: names) { + auto bus = static_cast(devices.at(std::format("{}.bus", name)).as_int()); + + if (bus != mBus) continue; + + auto id = static_cast(devices.at(std::format("{}.id", name)).as_int()); + + mDevices.insert({name, + CanFdAddress{ + .bus = bus, + .id = id, + }}); + + mDevicesPubSub.emplace(name, + CanFdPubSub{ + .publisher = create_publisher(std::format("can/{}/in", name), 16), + .subscriber = create_subscription(std::format("can/{}/out", name), 16, [this](msg::CAN::ConstSharedPtr const& msg) { + frameSendRequestCallback(msg); + }), + }); + + RCLCPP_INFO_STREAM(get_logger(), std::format("Added device: {} (bus: {}, id: {})", name, bus, id)); + } + + mCanNetLink = CanNetLink{get_logger().get_child("link"), mInterface}; + + int socketFileDescriptor = setupSocket(); + mStream.emplace(mIoService); + mStream->assign(socketFileDescriptor); + + readFrameAsync(); + + // Since "onInit" needs to return, kick off a self-joining thread to run the IO concurrently + mIoThread = std::jthread{[this] { mIoService.run(); }}; + + RCLCPP_INFO_STREAM(get_logger(), "Started"); + + } catch (std::exception const& exception) { + RCLCPP_FATAL_STREAM(get_logger(), std::format("Failed to start: {}", exception.what())); + rclcpp::shutdown(); + } + } + + auto CanBridge::setupSocket() const -> int { + int socketFd = checkSyscallResult(socket(PF_CAN, SOCK_RAW, CAN_RAW)); + RCLCPP_INFO_STREAM(get_logger(), std::format("Opened CAN socket with file descriptor: {}", socketFd)); + + ifreq ifr{}; + std::strcpy(ifr.ifr_name, mInterface.c_str()); + ioctl(socketFd, SIOCGIFINDEX, &ifr); + + sockaddr_can addr{ + .can_family = AF_CAN, + .can_ifindex = ifr.ifr_ifindex, + }; + checkSyscallResult(bind(socketFd, reinterpret_cast(&addr), sizeof(addr))); + RCLCPP_INFO_STREAM(get_logger(), "Bound CAN socket"); + + int enableCanFd = 1; + checkSyscallResult(setsockopt(socketFd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enableCanFd, sizeof(enableCanFd))); + + return socketFd; + } + + auto CanBridge::readFrameAsync() -> void { // NOLINT(*-no-recursion) + // You would think we would have to read the header first to find the data length (which is not always 64 bytes) and THEN read the data + // However socketcan is nice and just requires we read the max length + // It then puts the actual length in the header + async_read( + mStream.value(), + boost::asio::buffer(&mReadFrame, sizeof(mReadFrame)), + // Supply lambda that is called on completion + [this](boost::system::error_code const& ec, [[maybe_unused]] std::size_t bytes) { // NOLINT(*-no-recursion) + checkErrorCode(ec); + assert(bytes == sizeof(mReadFrame)); + + frameReadCallback(); + + // Ready for the next frame, start listening again + // Note this is recursive, but it is safe because it is async + // i.e. the stack is not growing + readFrameAsync(); + }); + } + + auto CanBridge::frameReadCallback() -> void { // NOLINT(*-no-recursion) + auto [identifier, isErrorFrame, isRemoteTransmissionRequest, isExtendedFrame] = std::bit_cast(mReadFrame.can_id); + auto [destination, source, isReplyRequired] = std::bit_cast(static_cast(identifier)); + + auto sourceDeviceNameIt = mDevices.right.find(CanFdAddress{ + .bus = mBus, + .id = source, + }); + if (sourceDeviceNameIt == mDevices.right.end()) { + RCLCPP_WARN_STREAM(get_logger(), std::format("Received message on interface {} that had an unknown source ID: {}", mInterface, std::uint8_t{source})); + return; + } + + auto destinationDeviceNameIt = mDevices.right.find(CanFdAddress{ + .bus = mBus, + .id = destination, + }); + if (destinationDeviceNameIt == mDevices.right.end()) { + RCLCPP_WARN_STREAM(get_logger(), std::format("Received message on interface {} that had an unknown destination ID: {}", mInterface, std::uint8_t{destination})); + return; + } + + msg::CAN msg; + msg.source = sourceDeviceNameIt->second; + msg.destination = destinationDeviceNameIt->second; + msg.data.assign(mReadFrame.data, mReadFrame.data + mReadFrame.len); + + mDevicesPubSub.at(msg.source).publisher->publish(msg); + } + + auto CanBridge::frameSendRequestCallback(msg::CAN::ConstSharedPtr const& msg) -> void { + auto sourceIt = mDevices.left.find(msg->source); + if (sourceIt == mDevices.left.end()) { + RCLCPP_WARN_STREAM(get_logger(), std::format("Sending message on interface {} that had an unknown source: {}", mInterface, msg->source)); + return; + } + + auto destinationIt = mDevices.left.find(msg->destination); + if (destinationIt == mDevices.left.end()) { + RCLCPP_WARN_STREAM(get_logger(), std::format("Sending message on interface {} that had an unknown destination: {}", mInterface, msg->destination)); + return; + } + + CanFdMessageId messageId{ + .destination = destinationIt->second.id, + .source = sourceIt->second.id, + .replyRequired = static_cast(msg->reply_required), + }; + + // Craft the SocketCAN frame from the ROS message + canfd_frame frame{ + .can_id = std::bit_cast(RawCanFdId{ + .identifier = std::bit_cast(messageId), + .isExtendedFrame = mIsExtendedFrame, + }), + .len = nearestFittingFdcanFrameSize(msg->data.size()), + }; + std::memcpy(frame.data, msg->data.data(), msg->data.size()); + + try { + if (std::size_t written = boost::asio::write(mStream.value(), boost::asio::buffer(std::addressof(frame), sizeof(frame))); + written != sizeof(frame)) { + RCLCPP_FATAL_STREAM(get_logger(), std::format("Failed to write frame to socket! Expected to write {} bytes, but only wrote {} bytes", sizeof(frame), written)); + rclcpp::shutdown(); + } + } catch (boost::system::system_error const& error) { + // check if ran out of buffer space + if (error.code() == boost::asio::error::no_buffer_space) { + RCLCPP_WARN_STREAM_THROTTLE(get_logger(), *get_clock(), 1000, "No buffer space available to send the message. This usually indicates an electrical problem with the bus. CAN will avoid sending out messages if it can not see other devices."); + return; + } + rclcpp::shutdown(); + } + } + + CanBridge::~CanBridge() { + mIoService.stop(); // This causes the io thread to finish + } + +} // namespace mrover + +auto main(int argc, char** argv) -> int { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return EXIT_SUCCESS; +} diff --git a/esw/can_bridge/can_bridge.hpp b/esw/can_bridge/can_bridge.hpp new file mode 100644 index 00000000..6afe5351 --- /dev/null +++ b/esw/can_bridge/can_bridge.hpp @@ -0,0 +1,85 @@ +#pragma once + +#include "pch.hpp" + +#include "can_net_link.hpp" + +// TODO(owen): support multiple buses + +namespace mrover { + + // [0-28]: CAN identifier (11/29bit) + // [29]: Error frame flag (0 = data frame, 1 = error frame) + // [30]: Remote transmission request flag (1 = rtr frame) + // [31]: Frame format flag (0 = standard 11bit, 1 = extended 29bit) + // In the future, if we want to send different types of messages, + // we should have logic for switching bits such as errorFrameFlag. + struct RawCanFdId { + std::uint32_t identifier : 29 {}; + bool isErrorFrame : 1 {}; + bool isRemoteTransmissionRequest : 1 {}; + bool isExtendedFrame : 1 {}; + }; + static_assert(sizeof(RawCanFdId) == sizeof(canid_t)); + + // Our custom message ID format, this is not part of the standard + // Based on the mjbots protocol + // By default CAN is broadcast, but this allows for directed messages + // Devices can set up filters (usually in hardware) to only receive messages meant for them + struct CanFdMessageId { + std::uint8_t destination{}; + std::uint8_t source : 7 {}; + bool replyRequired : 1 {}; + }; + static_assert(sizeof(CanFdMessageId) == 2); + + struct CanFdAddress { + std::uint8_t bus{}; + std::uint8_t id{}; + + // "Spaceship" operator + // See: https://devblogs.microsoft.com/cppblog/simplify-your-code-with-rocket-science-c20s-spaceship-operator/ + auto operator<=>(CanFdAddress const& other) const = default; + }; + + struct CanFdPubSub { + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr subscriber; + }; + + class CanBridge : public rclcpp::Node { + public: + CanBridge(); + + ~CanBridge() override; + + private: + std::string mInterface; + std::uint8_t mBus{}; + bool mIsExtendedFrame{}; + + canfd_frame mReadFrame{}; + CanNetLink mCanNetLink; + std::optional> mStream; + std::jthread mIoThread; + boost::asio::io_service mIoService; + + // boost::bimap< + // boost::bimaps::unordered_set_of, + // boost::bimaps::unordered_set_of{}(address.bus) ^ std::hash{}(address.id); + // })>> + // mDevices; + boost::bimap mDevices; + std::unordered_map mDevicesPubSub; + + [[nodiscard]] auto setupSocket() const -> int; + + void readFrameAsync(); + + void frameReadCallback(); + + void frameSendRequestCallback(msg::CAN::ConstSharedPtr const& msg); + }; + +} // namespace mrover diff --git a/esw/can_bridge/can_net_link.cpp b/esw/can_bridge/can_net_link.cpp new file mode 100644 index 00000000..922d75aa --- /dev/null +++ b/esw/can_bridge/can_net_link.cpp @@ -0,0 +1,97 @@ +#include "can_net_link.hpp" + +namespace mrover { + + CanNetLink::CanNetLink(rclcpp::Logger logger, std::string interface) + : mInterface{std::move(interface)} { + + try { + mSocket = nl_socket_alloc(); + if (mSocket == nullptr) { + throw std::runtime_error("Failed to allocate the network link socket"); + } + + if (int status = nl_connect(mSocket, NETLINK_ROUTE); status < 0) { + throw std::runtime_error("Failed to connect to the network link socket"); + } + + rtnl_link_alloc_cache(mSocket, AF_UNSPEC, &mCache); + if (mCache == nullptr) { + throw std::runtime_error("Failed to allocate the network link cache"); + } + + rtnl_link* link = rtnl_link_get_by_name(mCache, mInterface.c_str()); + if (link == nullptr) { + throw std::runtime_error(std::format("Failed to retrieve the link {} by name", mInterface)); + } + + if (bool is_up = rtnl_link_get_flags(link) & IFF_UP; !is_up) { + throw std::runtime_error("CAN link must be set up manually"); + } + + // TODO: literally none of this works bruh. for now just assume it is already up + // if (mInterface.starts_with("can")) { + // // can_bittiming bt{ + // // .bitrate = bitrate, + // // .brp = bitrate_prescaler, + // // }; + // // if (int result = rtnl_link_can_set_bittiming(link, &bt); result < 0) { + // // throw std::runtime_error("Failed to set CAN link bit timings"); + // // } + // + // // See https://android.googlesource.com/platform/external/libnl/+/bbcb553f0f4636dd40e84d320a576b3de7b95357/lib/route/link.c + // // Specifically the section "@par 2) Changing Link attributes" + // rtnl_link* link_request = rtnl_link_alloc(); + // + // // Trying to send to the socket without this will return error code 22 (invalid argument) + // // By default the MTU is configured for regular CAN frame which are much smaller + // // The effects of these calls will not be realized until "rtnl_link_change" + // // rtnl_link_set_mtu(link_request, sizeof(canfd_frame)); + // if (is_up) { + // ROS_WARN("CAN link is already up"); + // } else { + // rtnl_link_set_flags(link_request, IFF_UP); + // } + // + // if (int result = rtnl_link_change(mSocket, link, link_request, 0); result < 0) { + // rtnl_link_put(link); // TODO(quintin) Use RAII here + // throw std::runtime_error(std::format("Failed to change CAN link: {}", result)); + // } + // + // rtnl_link_put(link); // Free + // + // } else if (mInterface.starts_with("vcan")) { + // if (!is_up) { + // throw std::runtime_error("Virtual CAN link must be set up manually"); + // } + // } + + RCLCPP_INFO_STREAM(logger, "Set up"); + + } catch (std::exception const& exception) { + RCLCPP_FATAL_STREAM(logger, std::format("Exception in setup: {}", exception.what())); + rclcpp::shutdown(); + } + } + + CanNetLink::~CanNetLink() { + if (!mSocket || !mCache) return; + + // TODO: just assume interface is always up + // if (mInterface.starts_with("can")) { + // rtnl_link *link = rtnl_link_get_by_name(mCache, mInterface.c_str()), *link_request = rtnl_link_alloc(); + // + // rtnl_link_unset_flags(link_request, IFF_UP); + // if (int result = rtnl_link_change(mSocket, link, link_request, 0); result < 0) { + // std::cerr << std::format("Failed to change CAN link: {}", result) << std::endl; + // } + // + // rtnl_link_put(link); // Free + // } + + nl_socket_free(mSocket); + mCache = nullptr; + mSocket = nullptr; + } + +} // namespace mrover diff --git a/esw/can_bridge/can_net_link.hpp b/esw/can_bridge/can_net_link.hpp new file mode 100644 index 00000000..6b1901a8 --- /dev/null +++ b/esw/can_bridge/can_net_link.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include "pch.hpp" + +namespace mrover { + + struct CanNetLink { + + CanNetLink() = default; + + CanNetLink(rclcpp::Logger logger, std::string); + + CanNetLink(CanNetLink const&) = delete; + auto operator=(CanNetLink const&) -> CanNetLink& = delete; + + CanNetLink(CanNetLink&& other) noexcept { + *this = std::move(other); + } + + auto operator=(CanNetLink&& other) noexcept -> CanNetLink& { + mInterface = std::move(other.mInterface); + mSocket = std::exchange(other.mSocket, nullptr); + mCache = std::exchange(other.mCache, nullptr); + return *this; + } + + ~CanNetLink(); + + std::string mInterface{}; + nl_cache* mCache{}; + nl_sock* mSocket{}; + }; + +} // namespace mrover diff --git a/esw/can_bridge/debug_can_device.sh b/esw/can_bridge/debug_can_device.sh new file mode 100755 index 00000000..466bfc24 --- /dev/null +++ b/esw/can_bridge/debug_can_device.sh @@ -0,0 +1,29 @@ +#!/bin/bash + + +# For Jetson CAN info: https://docs.nvidia.com/jetson/archives/r35.2.1/DeveloperGuide/text/HR/ControllerAreaNetworkCan.html +# Generic CAN setup for Linux: +# After connecting to CAN, check if CAN device is connected. +# $ ip addr | grep "can" +# Load the SocketCAN kernel modules +# CAN BUS subsystem support module: +# $ sudo modprobe can +# Raw CAN protocol module: +# $ sudo modprobe can_raw +# Verify if kernel modules are loaded properly +# $ lsmod | grep "can" + +sudo apt-get install can-utils + +sudo modprobe can +sudo modprobe can_raw + +if lsmod | grep -q "can"; then + ip link set can0 type can bitrate 500000 loopback on + ip link set can0 up + candump can0 & + cansend can0 123#abcdabcd +fi + + + diff --git a/esw/can_bridge/debug_can_publisher.py b/esw/can_bridge/debug_can_publisher.py new file mode 100644 index 00000000..adbd7b4f --- /dev/null +++ b/esw/can_bridge/debug_can_publisher.py @@ -0,0 +1,23 @@ +#!/usr/bin/env python3 + +import rospy +from mrover.msg import CAN + + +def callback(msg): + print("Received message!") + print("BUS: " + msg.bus) + print("ID: " + hex(msg.id)) + print("DATA: ") + for d in msg.data: + print(hex(d)) + + +def main(): + rospy.init_node("can_commands_listener") + rospy.Subscriber("can_commands", CAN, callback) + rospy.spin() + + +if __name__ == "__main__": + main() diff --git a/esw/can_bridge/debug_ros_can_node.py b/esw/can_bridge/debug_ros_can_node.py new file mode 100755 index 00000000..bf3d6c97 --- /dev/null +++ b/esw/can_bridge/debug_ros_can_node.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python3 + +import rospy +from mrover.msg import CAN + + +def main(): + rospy.init_node("can_debug_requester") + p = rospy.Publisher("can_requests", CAN, queue_size=1) + + can_msg = CAN(bus=1, message_id=0b10101010101, data=[1, 2, 3, 4, 5]) + + while p.get_num_connections() == 0: + rospy.sleep(0.1) + + p.publish(can_msg) + + +if __name__ == "__main__": + main() diff --git a/esw/can_bridge/pch.hpp b/esw/can_bridge/pch.hpp new file mode 100644 index 00000000..d6653555 --- /dev/null +++ b/esw/can_bridge/pch.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include diff --git a/esw/differential_drive_controller.cpp b/esw/differential_drive_controller.cpp index 8c042f56..f5a368e0 100644 --- a/esw/differential_drive_controller.cpp +++ b/esw/differential_drive_controller.cpp @@ -30,13 +30,13 @@ namespace mrover { leftVelocityPub = create_publisher("drive_left_velocity_cmd", 10); rightVelocityPub = create_publisher("drive_right_velocity_cmd", 10); - twistSub = create_subscription("cmd_vel", 10, [this](geometry_msgs::msg::Twist::SharedPtr msg) { + twistSub = create_subscription("cmd_vel", 10, [this](geometry_msgs::msg::Twist::ConstSharedPtr const& msg) { moveDrive(msg); }); } private: - auto moveDrive(geometry_msgs::msg::Twist::SharedPtr const& msg) -> void { + auto moveDrive(geometry_msgs::msg::Twist::ConstSharedPtr const& msg) -> void { // See 13.3.1.4 in "Modern Robotics" for the math // Link: https://hades.mech.northwestern.edu/images/7/7f/MR.pdf diff --git a/perception/tag_detector/pch.hpp b/perception/tag_detector/pch.hpp index 1ea417d0..86786950 100644 --- a/perception/tag_detector/pch.hpp +++ b/perception/tag_detector/pch.hpp @@ -19,6 +19,8 @@ #include #include +#include +#include #include #include #include diff --git a/simulator/urdf.cpp b/simulator/urdf.cpp index d51b6c38..f4c82fdd 100644 --- a/simulator/urdf.cpp +++ b/simulator/urdf.cpp @@ -51,7 +51,7 @@ namespace mrover { if (position.size() != 3) throw std::invalid_argument{"Position must have 3 elements"}; if (orientation.size() != 4) throw std::invalid_argument{"Orientation must have 4 elements"}; btTransform transform{btQuaternion{static_cast(orientation[0]), static_cast(orientation[1]), static_cast(orientation[2]), static_cast(orientation[3])}, btVector3{static_cast(position[0]), static_cast(position[1]), static_cast(position[2])}}; - if (auto [_, was_added] = mUrdfs.try_emplace(name, *this, uri, transform); !was_added) { + if (auto [_, wasAdded] = mUrdfs.try_emplace(name, *this, uri, transform); !wasAdded) { throw std::invalid_argument{std::format("Duplicate object name: {}", name)}; } } @@ -96,7 +96,7 @@ namespace mrover { assert(mesh); std::string const& fileUri = mesh->filename; - auto [it, was_inserted] = simulator.mUriToModel.try_emplace(fileUri, simulator, fileUri); + auto [it, wasInserted] = simulator.mUriToModel.try_emplace(fileUri, simulator, fileUri); Model& model = it->second; model.waitMeshes(); diff --git a/teleoperation/arm_controller/arm_controller.cpp b/teleoperation/arm_controller/arm_controller.cpp index 00460568..4c05e4d1 100644 --- a/teleoperation/arm_controller/arm_controller.cpp +++ b/teleoperation/arm_controller/arm_controller.cpp @@ -5,8 +5,8 @@ namespace mrover { ArmController::ArmController() : Node{"arm_controller"} { mPosPub = create_publisher("arm_position_cmd", 10); - mIkSub = create_subscription("arm_ik", 1, [this](msg::IK::SharedPtr msg) { - ik_callback(msg); + mIkSub = create_subscription("arm_ik", 1, [this](msg::IK::ConstSharedPtr const& msg) { + ikCallback(msg); }); } @@ -15,10 +15,7 @@ namespace mrover { return {q.normalized()}; } - void ArmController::ik_callback(msg::IK::ConstSharedPtr const& ik_target) { - msg::Position positions; - positions.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"}; - positions.positions.resize(positions.names.size()); + void ArmController::ikCallback(msg::IK::ConstSharedPtr const& ik_target) { SE3d targetFrameToArmBaseLink; try { targetFrameToArmBaseLink = SE3Conversions::fromTfTree(mTfBuffer, ik_target->target.header.frame_id, "arm_base_link"); @@ -49,23 +46,27 @@ namespace mrover { double q3 = -thetaC - 0.1608485915; if (std::isfinite(q1) && std::isfinite(q2) && std::isfinite(q3)) { - SE3d joint_b_pos{{0.034346, 0, 0.049024}, SO3d::Identity()}; - SE3d joint_c_pos{{LINK_BC * cos(thetaA), 0, LINK_BC * sin(thetaA)}, yawSo3(-thetaA)}; - SE3d joint_d_pos{{LINK_CD * cos(thetaB), 0, LINK_CD * sin(thetaB)}, yawSo3(-thetaB)}; - SE3d joint_e_pos{{LINK_DE * cos(thetaC), 0, LINK_DE * sin(thetaC)}, yawSo3(-thetaC)}; - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_b_target", "arm_a_link", joint_b_pos, get_clock()->now()); - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_c_target", "arm_b_target", joint_c_pos, get_clock()->now()); - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_d_target", "arm_c_target", joint_d_pos, get_clock()->now()); - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_e_target", "arm_d_target", joint_e_pos, get_clock()->now()); + SE3d jointBPose{{0.034346, 0, 0.049024}, SO3d::Identity()}; + SE3d jointCPose{{LINK_BC * cos(thetaA), 0, LINK_BC * sin(thetaA)}, yawSo3(-thetaA)}; + SE3d jointDPose{{LINK_CD * cos(thetaB), 0, LINK_CD * sin(thetaB)}, yawSo3(-thetaB)}; + SE3d jointEPose{{LINK_DE * cos(thetaC), 0, LINK_DE * sin(thetaC)}, yawSo3(-thetaC)}; + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_b_target", "arm_a_link", jointBPose, get_clock()->now()); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_c_target", "arm_b_target", jointCPose, get_clock()->now()); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_d_target", "arm_c_target", jointDPose, get_clock()->now()); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_e_target", "arm_d_target", jointEPose, get_clock()->now()); if (y >= JOINT_A_MIN && y <= JOINT_A_MAX && q1 >= JOINT_B_MIN && q1 <= JOINT_B_MAX && q2 >= JOINT_C_MIN && q2 <= JOINT_C_MAX && q3 >= JOINT_DE_PITCH_MIN && q3 <= JOINT_DE_PITCH_MAX) { - positions.positions[0] = static_cast(y); - positions.positions[1] = static_cast(q1); - positions.positions[2] = static_cast(q2); - positions.positions[3] = static_cast(q3); + msg::Position positions; + positions.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch"}; + positions.positions = { + static_cast(y), + static_cast(q1), + static_cast(q2), + static_cast(q3), + }; mPosPub->publish(positions); } else { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Can not reach target within arm limits!"); diff --git a/teleoperation/arm_controller/arm_controller.hpp b/teleoperation/arm_controller/arm_controller.hpp index 60668ad4..b7faf914 100644 --- a/teleoperation/arm_controller/arm_controller.hpp +++ b/teleoperation/arm_controller/arm_controller.hpp @@ -32,7 +32,7 @@ namespace mrover { ArmController(); - void ik_callback(msg::IK::ConstSharedPtr const& ik_target); + void ikCallback(msg::IK::ConstSharedPtr const& ik_target); }; } // namespace mrover \ No newline at end of file