diff --git a/.gitignore b/.gitignore index 7411f32..64716e2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,6 @@ .env __pycache__ .vscode/ +cursor/ watod-config.local.sh diff --git a/autonomy/interfacing/aggregator/CMakeLists.txt b/autonomy/interfacing/aggregator/CMakeLists.txt new file mode 100644 index 0000000..bc85e07 --- /dev/null +++ b/autonomy/interfacing/aggregator/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.10) +project(aggregator) + +# Set compiler to use C++ 17 standard +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Search for dependencies required for building this package +find_package(ament_cmake REQUIRED) # ROS2 build tool +find_package(rclcpp REQUIRED) # ROS2 C++ package +find_package(sample_msgs REQUIRED) # Custom package containing ROS2 messages + +# Compiles source files into a library +# A library is not executed, instead other executables can link +# against it to access defined methods and classes. +# We build a library so that the methods defined can be used by +# both the unit test and ROS2 node executables. +add_library(aggregator_lib + src/aggregator_core.cpp) +# Indicate to compiler where to search for header files +target_include_directories(aggregator_lib + PUBLIC include) +# Add ROS2 dependencies required by package +ament_target_dependencies(aggregator_lib rclcpp sample_msgs) + +# By default tests are built. This can be overridden by modifying the +# colcon build command to include the -DBUILD_TESTING=OFF flag. +option(BUILD_TESTING "Build tests" ON) +if(BUILD_TESTING) + # Search for dependencies required for building tests + linting + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_common REQUIRED) + + # Remove the default C++ and copyright linter + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_copyright) + + # Reinstall cpplint ignoring copyright errors + ament_cpplint(FILTERS "-legal/copyright") + + ament_lint_auto_find_test_dependencies() + + # Create test executable from source files + ament_add_gtest(aggregator_test test/aggregator_test.cpp) + # Link to the previously built library to access Aggregator classes and methods + target_link_libraries(aggregator_test aggregator_lib) + + # Copy executable to installation location + install(TARGETS + aggregator_test + DESTINATION lib/${PROJECT_NAME}) +endif() + +# Create ROS2 node executable from source files +add_executable(aggregator_node src/aggregator_node.cpp) +# Link to the previously built library to access Aggregator classes and methods +target_link_libraries(aggregator_node aggregator_lib) + +# Copy executable to installation location +install(TARGETS + aggregator_node + DESTINATION lib/${PROJECT_NAME}) + +# Copy launch files to installation location +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/autonomy/interfacing/aggregator/include/aggregator_core.hpp b/autonomy/interfacing/aggregator/include/aggregator_core.hpp new file mode 100644 index 0000000..9dcea65 --- /dev/null +++ b/autonomy/interfacing/aggregator/include/aggregator_core.hpp @@ -0,0 +1,71 @@ +#ifndef AGGREGATOR_CORE_HPP_ +#define AGGREGATOR_CORE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "sample_msgs/msg/filtered_array.hpp" +#include "sample_msgs/msg/unfiltered.hpp" + +namespace samples { + +/** + * Implementation of the internal logic used by the Aggregator Node to + * calculate the operating frequency of topics. + */ +class AggregatorCore { +public: + /** + * Aggregator constructor. + * + * @param timestamp the Unix timestamp https://en.wikipedia.org/wiki/Unix_time + */ + explicit AggregatorCore(int64_t timestamp); + + /** + * Calculates the operating frequency on the "unfiltered" topic. Handles + * invalid timestamps and division by zero by returning zero. + * + * @returns frequency of messages on "unfiltered" topic + */ + double raw_frequency() const; + /** + * Calculates the operating frequency on the "filtered" topic. Handles + * invalid timestamps and division by zero by returning zero. + * + * @returns frequency of messages on "filtered" topic + */ + double filtered_frequency() const; + + /** + * Used to keep track of latest timestamp and number of messages received + * over the "unfiltered" topic. Should be called before raw_frequency(). + * + * @param msg + */ + void add_raw_msg(const sample_msgs::msg::Unfiltered::SharedPtr msg); + /** + * Used to keep track of latest timestamp and number of messages received + * over the "filtered" topic. Should be called before filtered_frequency(). + * + * @param msg + */ + void add_filtered_msg(const sample_msgs::msg::FilteredArray::SharedPtr msg); + +private: + // Number of message received on "unfiltered" and "filtered" topics. + int raw_msg_count_; + int filtered_msg_count_; + + // Unix timestamp used to determine the amount of time that has passed + // since the beginning of the program. + int64_t start_; + + // Unix timestamps for last time a message was received on the "unfiltered" + // and "filtered" topics. + int64_t latest_raw_time_; + int64_t latest_filtered_time_; +}; + +} // namespace samples + +#endif // AGGREGATOR_CORE_HPP_ diff --git a/autonomy/interfacing/aggregator/include/aggregator_node.hpp b/autonomy/interfacing/aggregator/include/aggregator_node.hpp new file mode 100644 index 0000000..a273a2e --- /dev/null +++ b/autonomy/interfacing/aggregator/include/aggregator_node.hpp @@ -0,0 +1,56 @@ +#ifndef AGGREGATOR_NODE_HPP_ +#define AGGREGATOR_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "sample_msgs/msg/filtered_array.hpp" +#include "sample_msgs/msg/unfiltered.hpp" + +#include "aggregator_core.hpp" + +/** + * Implementation of a ROS2 node that listens to the "unfiltered" and "filtered" + * topics and echoes the operating frequency of the topic to the console. + */ +class AggregatorNode : public rclcpp::Node { +public: + // Configure pubsub nodes to keep last 20 messages. + // https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html + static constexpr int ADVERTISING_FREQ = 20; + + /** + * Aggregator node constructor. + */ + AggregatorNode(); + +private: + /** + * A ROS2 subscription node callback used to unpack raw data from the + * "unfiltered" topic and echo the operating frequency of the topic to the + * console. + * + * @param msg a raw message from the "unfiltered" topic + */ + void unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg); + + /** + * A ROS2 subscription node callback used to unpack processed data from the + * "filtered" topic and echo the operating frequency of the topic to the + * console. + * + * @param msg a processed message from the "filtered" topic + */ + void filtered_callback(const sample_msgs::msg::FilteredArray::SharedPtr msg); + + // ROS2 subscriber listening to the unfiltered topic. + rclcpp::Subscription::SharedPtr raw_sub_; + + // ROS2 subscriber listening to the filtered topic. + rclcpp::Subscription::SharedPtr + filtered_sub_; + + // Object containing methods to determine the operating frequency on topics. + samples::AggregatorCore aggregator_; +}; + +#endif // AGGREGATOR_NODE_HPP_ diff --git a/autonomy/interfacing/aggregator/launch/aggregator.launch.py b/autonomy/interfacing/aggregator/launch/aggregator.launch.py new file mode 100755 index 0000000..d0486d5 --- /dev/null +++ b/autonomy/interfacing/aggregator/launch/aggregator.launch.py @@ -0,0 +1,14 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + """Launch aggregator node.""" + aggregator_node = Node( + package='aggregator', + executable='aggregator_node', + ) + + return LaunchDescription([ + aggregator_node + ]) diff --git a/autonomy/interfacing/aggregator/package.xml b/autonomy/interfacing/aggregator/package.xml new file mode 100644 index 0000000..13366d4 --- /dev/null +++ b/autonomy/interfacing/aggregator/package.xml @@ -0,0 +1,23 @@ + + + aggregator + 0.0.0 + A sample ROS package for pubsub communication + + Cheran Mahalingam + Apache2.0 + + + ament_cmake + rclcpp + sample_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + + ament_cmake + + \ No newline at end of file diff --git a/autonomy/interfacing/aggregator/src/aggregator_core.cpp b/autonomy/interfacing/aggregator/src/aggregator_core.cpp new file mode 100644 index 0000000..58f9e6a --- /dev/null +++ b/autonomy/interfacing/aggregator/src/aggregator_core.cpp @@ -0,0 +1,42 @@ +#include + +#include "aggregator_core.hpp" + +namespace samples { + +AggregatorCore::AggregatorCore(int64_t timestamp) + : raw_msg_count_(0), filtered_msg_count_(0), start_(timestamp), + latest_raw_time_(-1), latest_filtered_time_(-1) {} + +double AggregatorCore::raw_frequency() const { + if (latest_raw_time_ <= start_) { + return 0.0; + } + return static_cast(raw_msg_count_) / (latest_raw_time_ - start_); +} + +double AggregatorCore::filtered_frequency() const { + if (latest_filtered_time_ <= start_) { + return 0.0; + } + return static_cast(filtered_msg_count_) / + (latest_filtered_time_ - start_); +} + +void AggregatorCore::add_raw_msg( + const sample_msgs::msg::Unfiltered::SharedPtr msg) { + latest_raw_time_ = + std::max(static_cast(msg->timestamp), latest_raw_time_); + raw_msg_count_++; +} + +void AggregatorCore::add_filtered_msg( + const sample_msgs::msg::FilteredArray::SharedPtr msg) { + for (auto filtered_msg : msg->packets) { + latest_filtered_time_ = std::max( + static_cast(filtered_msg.timestamp), latest_filtered_time_); + } + filtered_msg_count_++; +} + +} // namespace samples diff --git a/autonomy/interfacing/aggregator/src/aggregator_node.cpp b/autonomy/interfacing/aggregator/src/aggregator_node.cpp new file mode 100644 index 0000000..8bf5b87 --- /dev/null +++ b/autonomy/interfacing/aggregator/src/aggregator_node.cpp @@ -0,0 +1,41 @@ +#include +#include + +#include "aggregator_node.hpp" + +AggregatorNode::AggregatorNode() + : Node("aggregator"), + aggregator_(samples::AggregatorCore( + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count())) { + raw_sub_ = this->create_subscription( + "/unfiltered_topic", ADVERTISING_FREQ, + std::bind(&AggregatorNode::unfiltered_callback, this, + std::placeholders::_1)); + filtered_sub_ = this->create_subscription( + "/filtered_topic", ADVERTISING_FREQ, + std::bind(&AggregatorNode::filtered_callback, this, + std::placeholders::_1)); +} + +void AggregatorNode::unfiltered_callback( + const sample_msgs::msg::Unfiltered::SharedPtr msg) { + aggregator_.add_raw_msg(msg); + RCLCPP_INFO(this->get_logger(), "Raw Frequency(msg/s): %f", + aggregator_.raw_frequency() * 1000); +} + +void AggregatorNode::filtered_callback( + const sample_msgs::msg::FilteredArray::SharedPtr msg) { + aggregator_.add_filtered_msg(msg); + RCLCPP_INFO(this->get_logger(), "Filtered Frequency(msg/s): %f", + aggregator_.filtered_frequency() * 1000); +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/autonomy/interfacing/aggregator/test/aggregator_test.cpp b/autonomy/interfacing/aggregator/test/aggregator_test.cpp new file mode 100644 index 0000000..3e5c2db --- /dev/null +++ b/autonomy/interfacing/aggregator/test/aggregator_test.cpp @@ -0,0 +1,73 @@ +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "gtest/gtest.h" + +#include "aggregator_core.hpp" + +TEST(AggregatorTest, RawDivisionByZero) { + samples::AggregatorCore aggregator(0); + auto msg = std::make_shared(); + msg->timestamp = 0; + + aggregator.add_raw_msg(msg); + EXPECT_DOUBLE_EQ(0.0, aggregator.raw_frequency()); +} + +TEST(AggregatorTest, FilteredDivisionByZero) { + samples::AggregatorCore aggregator(1); + auto filtered_packet = std::make_shared(); + std::vector msgs; + auto msg = sample_msgs::msg::Filtered(); + msg.timestamp = 1; + msgs.push_back(msg); + filtered_packet->packets = msgs; + + aggregator.add_filtered_msg(filtered_packet); + EXPECT_DOUBLE_EQ(0.0, aggregator.filtered_frequency()); +} + +TEST(AggregatorTest, RawFrequencyAddSingleMessage) { + samples::AggregatorCore aggregator(0); + auto msg = std::make_shared(); + + msg->timestamp = 2; + aggregator.add_raw_msg(msg); + EXPECT_DOUBLE_EQ(0.5, aggregator.raw_frequency()); +} + +TEST(AggregatorTest, RawFrequencyAddMultipleMessages) { + samples::AggregatorCore aggregator(0); + auto msg = std::make_shared(); + + msg->timestamp = 2; + aggregator.add_raw_msg(msg); + EXPECT_DOUBLE_EQ(0.5, aggregator.raw_frequency()); + + msg->timestamp = 1; + aggregator.add_raw_msg(msg); + EXPECT_DOUBLE_EQ(1.0, aggregator.raw_frequency()); + + msg->timestamp = 5; + aggregator.add_raw_msg(msg); + EXPECT_DOUBLE_EQ(0.6, aggregator.raw_frequency()); +} + +TEST(AggregatorTest, FilteredUnorderedTimestamps) { + samples::AggregatorCore aggregator(0); + auto filtered_packet = std::make_shared(); + std::vector msgs; + auto msg = sample_msgs::msg::Filtered(); + + msg.timestamp = 5; + msgs.push_back(msg); + msg.timestamp = 2; + msgs.push_back(msg); + msg.timestamp = 3; + msgs.push_back(msg); + filtered_packet->packets = msgs; + + aggregator.add_filtered_msg(filtered_packet); + EXPECT_DOUBLE_EQ(0.2, aggregator.filtered_frequency()); +} diff --git a/autonomy/interfacing/can/CMakeLists.txt b/autonomy/interfacing/can/CMakeLists.txt new file mode 100644 index 0000000..7ec9e9f --- /dev/null +++ b/autonomy/interfacing/can/CMakeLists.txt @@ -0,0 +1,80 @@ +cmake_minimum_required(VERSION 3.10) +project(can) + +# Set compiler to C++ 17 Standard +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Find dependencies +find_package(ament_index_cpp REQUIRED) # ROS tool to find shared directories of installed packages +find_package(ament_cmake REQUIRED) # ROS2 build tool +find_package(rclcpp REQUIRED) # ROS2 C++ package +find_package(std_msgs REQUIRED) # ROS2 message package + +# Compiles source files into a library +# A library is not executed, instead other executables can link +# against it to access defined methods and classes. +# We build a library so that the methods defined can be used by +# both the unit test and ROS2 node executables. +add_library(can_lib + src/can_core.cpp +) + +# Indicate to compiler where to search for header files +target_include_directories(can_lib + PUBLIC + include +) + +# Add ROS2 dependencies required by package +ament_target_dependencies(can_lib + ament_index_cpp + rclcpp + std_msgs +) + +# Link system libraries for CAN support +# Note: Using system CAN libraries instead of libsocketcan for broader compatibility +target_link_libraries(can_lib) + +# Create the can_node executable +add_executable(can_node src/can_node.cpp src/can_core.cpp) +ament_target_dependencies(can_node rclcpp std_msgs) +# Link to the previously built libraries +target_link_libraries(can_node can_lib) + +# Copy executable to installation location +install(TARGETS + can_node + DESTINATION lib/${PROJECT_NAME}) + +# Install launch and config files +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}) + +# Install scripts with execute permissions +install(PROGRAMS + scripts/setup_can.sh + DESTINATION share/${PROJECT_NAME}/scripts) + +# Testing and Development +find_package(ament_lint_auto REQUIRED) +ament_lint_auto_find_test_dependencies() + +# Add test executables (comment out if not needed in production) +# Test controller that publishes to /test_controller topic +add_executable(test_controller_node test/test_controller.cpp) +ament_target_dependencies(test_controller_node rclcpp std_msgs) + +install(TARGETS + test_controller_node + DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/autonomy/interfacing/can/README.md b/autonomy/interfacing/can/README.md new file mode 100644 index 0000000..0a739fc --- /dev/null +++ b/autonomy/interfacing/can/README.md @@ -0,0 +1,104 @@ +## CAN Interfacing Package Documentation + +The documentation for the CAN interfacing package follows a standard WATOnomous package level scheme. As shown below: +``` +can/ +├── CMakeLists.txt +├── README.md +├── package.xml +├── config/ +│ └── params.yaml +├── include/ +│ ├── can_core.hpp +│ └── can_node.hpp +├── launch/ +│ └── can.launch.py +├── scripts/ +│ └── setup_can.sh +├── src/ +│ ├── can_core.cpp +│ └── can_node.cpp +└── test/ + └── test_can.cpp +``` + +### Package Level Overview + +#### Purpose +This ROS 2 package serves as a communication bridge between a high-level controller and an embedded hand system via CAN bus. The package enables: +- Bidirectional communication between ROS 2 nodes and embedded systems +- Protocol translation between ROS 2 messages and CAN Bus frames +- Real-time feedback loop for hand control +- Integration of embedded hand systems within the WATonomous ROS2 ecosystem + +The package implements the following workflow: +1. Receive ROS 2 movement messages from the controller/behaviour node +2. Convert and send these messages as CAN frames through a USB CAN transceiver (accessible within a Docker container) +3. Receive CAN frames from the embedded system containing hand odometry +4. Convert and publish arm odometry as ROS2 messages for feedback to the controller + +#### Inputs & Outputs +- Data flow, including message types, service calls, or file interactions + +#### Key Features + +**Architecture Design Pattern: Core vs Node Separation** + +This package follows a clean architecture pattern that separates concerns between hardware abstraction and ROS integration: + +- **CanCore** (`can_core.hpp/.cpp`): + - Pure C++ class handling low-level CAN bus operations + - Hardware abstraction layer for CAN communication + - Reusable component independent of ROS + - Responsibilities: socket management, frame transmission/reception, interface setup + - Could be used in non-ROS applications or embedded systems + +- **CanNode** (`can_node.hpp/.cpp`): + - ROS 2 Node class managing ROS ecosystem integration + - Message translation between ROS and CAN protocols + - ROS-specific functionality: publishers, subscribers, parameters, timers + - Responsibilities: ROS message handling, topic management, service calls + +This separation provides: +- **Single Responsibility**: Each class has one clear purpose +- **Testability**: CanCore can be unit tested without ROS overhead +- **Reusability**: CanCore can be used in other projects +- **Maintainability**: CAN protocol changes only affect CanCore, ROS changes only affect CanNode + +#### Usage + +**Building the Package** +Build the package using the standard ROS 2 build process within the interfacing container. + +**Running the CAN Node** +1. Start the interfacing container +2. Access the container: `sudo ./watod -t interfacing` +3. Source the environment: `source ./watod_ros_entrypoint.sh` +4. Launch the CAN node: `ros2 launch can can.launch.py` + +**Note**: The launch file includes a test controller node for development/testing. To disable it for production use, comment out the test controller section in `launch/can.launch.py` (lines marked with "TEST CONTROLLER NODE" comments). + +#### Configuration +- Relevant parameters, environment variables, or dependencies + +#### Dependencies +- List of required packages, libraries, or tools + - `rclcpp`: ROS 2 C++ client library + - `can_msgs`: Custom message definitions for CAN frames + - `socketcan`: Linux socket CAN interface + - `ament_cmake`: Build system for ROS 2 packages + +#### Config +- Configuration files, environment variables, or command-line arguments + - `config/params.yaml`: Contains parameters for CAN interface setup, such as: + - `can_interface`: Name of the CAN interface (e.g., `can0`) + - `baud_rate`: CAN bus speed (e.g., `500000`) + - `frame_timeout`: Timeout for receiving frames in milliseconds + +### Package Architecture + + +### Infrastructure Documentation +1. [Documentation Structure of Repo](docs/README.md) +2. [Project Infrastructure Development Docs](https://github.com/WATonomous/wato_monorepo/tree/main/docs/dev/) +3. [ROS Structure Docs](src/samples/README.md) \ No newline at end of file diff --git a/autonomy/interfacing/can/config/params.yaml b/autonomy/interfacing/can/config/params.yaml new file mode 100644 index 0000000..4415c42 --- /dev/null +++ b/autonomy/interfacing/can/config/params.yaml @@ -0,0 +1,23 @@ +# Parameters for the CAN node +can_node: + ros__parameters: + # CAN Interface Configuration + can_interface: "can0" # CAN interface name (can0, can1, etc.) + device_path: "/dev/canable" # Serial device path for SLCAN + bustype: "slcan" # Bus type to create a CAN interface: "socketcan" or "slcan" + bitrate: 1000000 # CAN bitrate in bps (125000, 250000, 500000, 1000000) + + # Communication Settings + publish_rate_hz: 50 # Rate for checking incoming CAN messages + receive_timeout_ms: 10000 # Timeout for receiving CAN messages + + profile_type: "trapezoidal" # "trapezoidal" or "triangular" + max_acc: 1.0 # m/s^2 + + # Topics to subscribe to (message types will be auto-detected) + topics: + - "/test_controller" + - "/cmd_arm_joint" + - "/cmd_arm_ee" + - "/cmd_hand_joint" + - "/cmd_hand_ee" \ No newline at end of file diff --git a/autonomy/interfacing/can/include/can_core.hpp b/autonomy/interfacing/can/include/can_core.hpp new file mode 100644 index 0000000..36bb194 --- /dev/null +++ b/autonomy/interfacing/can/include/can_core.hpp @@ -0,0 +1,61 @@ +#ifndef CAN_CORE_HPP +#define CAN_CORE_HPP + +#include "rclcpp/rclcpp.hpp" +#include +#include +#include + +namespace autonomy +{ + +struct CanMessage { + uint32_t id; // CAN message ID + std::vector data; // Message data (up to 8 bytes for classic CAN) + uint8_t dlc; // Data Length Code + bool is_extended_id; // Extended frame format flag + bool is_remote_frame; // Remote transmission request flag + uint64_t timestamp_us; // Timestamp in microseconds +}; + +struct CanConfig { + std::string interface_name; // CAN interface name (e.g., "can0") + std::string device_path; // Device path for SLCAN (e.g., "/dev/ttyACM0") + std::string bustype; // Bus type: "socketcan" or "slcan" + uint32_t bitrate; // Bitrate in bps for arbitration phase + uint32_t data_bitrate; // Data bitrate in bps for CAN-FD data phase + uint32_t receive_timeout_ms; // Receive timeout in milliseconds +}; + +class CanCore { +public: + CanCore(const rclcpp::Logger& logger); + + // CAN Interface Management + bool initialize(const CanConfig& config); + bool shutdown(); + bool isInitialized() const; + + // Message Transmission + bool sendMessage(const CanMessage& message); + + // Message Reception + bool receiveMessage(CanMessage& message); + +private: + rclcpp::Logger logger_; + + // Internal state + int socket_fd_; + bool initialized_; + bool connected_; + CanConfig config_; + + // Internal helper methods + bool setupSocketCan(); + bool setupSlcan(); +}; + +} + +#endif // CAN_CORE_HPP diff --git a/autonomy/interfacing/can/include/can_node.hpp b/autonomy/interfacing/can/include/can_node.hpp new file mode 100644 index 0000000..577b121 --- /dev/null +++ b/autonomy/interfacing/can/include/can_node.hpp @@ -0,0 +1,40 @@ +#ifndef CAN_NODE_HPP +#define CAN_NODE_HPP + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/generic_subscription.hpp" +#include "std_msgs/msg/string.hpp" +#include "can_core.hpp" +#include +#include +#include + +struct TopicConfig { + // name of the topic, and the message type + std::string name; + std::string type; +}; + +class CanNode : public rclcpp::Node { +public: + CanNode(); + +private: + autonomy::CanCore can_; + std::vector topic_configs_; + std::unordered_map> subscribers_; + rclcpp::TimerBase::SharedPtr receive_timer_; // Timer to periodically check for CAN messages + + // Methods + void loadTopicConfigurations(); + void createSubscribers(); + std::string discoverTopicType(const std::string& topic_name); + void topicCallback(std::shared_ptr msg, const std::string& topic_name, const std::string& topic_type); + void receiveCanMessages(); // Method to be called by the timer + + // Helper methods + uint32_t generateCanId(const std::string& topic_name); + std::vector createCanMessages(const std::string& topic_name, std::shared_ptr ros_msg, uint32_t can_id = 0); +}; + +#endif // CAN_NODE_HPP diff --git a/autonomy/interfacing/can/include/mit_motor_control.hpp b/autonomy/interfacing/can/include/mit_motor_control.hpp new file mode 100644 index 0000000..48f5193 --- /dev/null +++ b/autonomy/interfacing/can/include/mit_motor_control.hpp @@ -0,0 +1,6 @@ +#include "can_node.hpp" + +// MIT Motor Control Functions +void moveMIT(double velocity, double force, double position, int id); +void moveProfile(const std::string& profile_type, double max_acc); + diff --git a/autonomy/interfacing/can/launch/can.launch.py b/autonomy/interfacing/can/launch/can.launch.py new file mode 100644 index 0000000..ed775e1 --- /dev/null +++ b/autonomy/interfacing/can/launch/can.launch.py @@ -0,0 +1,61 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # Get the path to the config file + config_file = os.path.join( + get_package_share_directory('can'), + 'config', + 'params.yaml' + ) + + # Declare launch arguments + can_interface_arg = DeclareLaunchArgument( + 'can_interface', + default_value='can0', + description='Name of the CAN interface to use (e.g., can0)' + ) + + publish_rate_arg = DeclareLaunchArgument( + 'publish_rate_hz', + default_value='50', + description='Rate in Hz at which to check for CAN messages' + ) + + # Create the CAN node + can_node = Node( + package='can', + executable='can_node', + name='can_node', + parameters=[config_file, { + 'can_interface': LaunchConfiguration('can_interface'), + 'publish_rate_hz': LaunchConfiguration('publish_rate_hz') + }], + output='screen' + ) + + # ============================================================================ + # TEST CONTROLLER NODE (for development/testing only) + # Comment out the following section if you want to disable the test controller + # ============================================================================ + test_controller_node = Node( + package='can', + executable='test_controller_node', + name='test_controller', + output='screen' + ) + # ============================================================================ + # End of test controller section + # ============================================================================ + + # Return the launch description + return LaunchDescription([ + can_interface_arg, + publish_rate_arg, + can_node, + test_controller_node # Comment out this line to disable test controller + ]) diff --git a/autonomy/interfacing/can/package.xml b/autonomy/interfacing/can/package.xml new file mode 100644 index 0000000..3cd227d --- /dev/null +++ b/autonomy/interfacing/can/package.xml @@ -0,0 +1,21 @@ + + + + can + 0.0.0 + ROS 2 C++ package for CAN bus communication + Gavin Tranquilino + Apache2.0 + + ament_cmake + + rclcpp + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/autonomy/interfacing/can/scripts/setup_can.sh b/autonomy/interfacing/can/scripts/setup_can.sh new file mode 100755 index 0000000..6629d12 --- /dev/null +++ b/autonomy/interfacing/can/scripts/setup_can.sh @@ -0,0 +1,45 @@ +#!/bin/bash +# Purpose: Sets up a CAN interface using slcand (daemon for serial CAN interfaces) +# This script configures a CAN interface by: +# 1. Starting the slcand daemon to create a network interface from a serial device +# 2. Bringing up the CAN interface +# 3. Setting the transmission queue length +# +# The script takes three parameters all found in the autonomy/interfacing/can/config/params.yaml: +# - device_path: Serial device path (e.g., /dev/ttyACM0) +# - interface_name: Name for the CAN interface (e.g., can0) +# - bitrate_code: Bitrate configuration code (e.g., -s6 for 500k, -s8 for 1M) + +set -e + +# These parameters are found in the autonomy/interfacing/can/config/params.yaml +DEVICE_PATH="$1" +INTERFACE_NAME="$2" +BITRATE_CODE="$3" # e.g., -s6 for 500k, -s8 for 1M + +if [ -z "$DEVICE_PATH" ] || [ -z "$INTERFACE_NAME" ] || [ -z "$BITRATE_CODE" ]; then + echo "Usage: $0 " + echo "Example: $0 /dev/ttyACM0 can0 -s6" + exit 1 +fi + +echo "Attempting to set up CAN interface $INTERFACE_NAME on $DEVICE_PATH with bitrate code $BITRATE_CODE..." + +# Check if slcand is already running for this device/interface to avoid errors +if pgrep -f "slcand.*$DEVICE_PATH.*$INTERFACE_NAME"; then + echo "slcand appears to be already running for $DEVICE_PATH and $INTERFACE_NAME. Assuming interface is (or will be) up." +else + echo "Starting slcand: slcand -o -c $BITRATE_CODE $DEVICE_PATH $INTERFACE_NAME" + slcand -o -c "$BITRATE_CODE" "$DEVICE_PATH" "$INTERFACE_NAME" + # Add a small delay to allow slcand to establish the interface + sleep 1 +fi + +echo "Bringing up interface: ifconfig $INTERFACE_NAME up" +ifconfig "$INTERFACE_NAME" up + +echo "Setting txqueuelen: ifconfig $INTERFACE_NAME txqueuelen 1000" # Transmisson queue length +ifconfig "$INTERFACE_NAME" txqueuelen 1000 + +echo "CAN interface $INTERFACE_NAME setup complete." +exit 0 \ No newline at end of file diff --git a/autonomy/interfacing/can/src/can_core.cpp b/autonomy/interfacing/can/src/can_core.cpp new file mode 100644 index 0000000..a3946b7 --- /dev/null +++ b/autonomy/interfacing/can/src/can_core.cpp @@ -0,0 +1,296 @@ +#include "can_core.hpp" +#include // Core socket functions for SocketCAN [socket(), bind(), send(), recv()] +#include // Definitions for can frames and CAN FD +#include // CAN RAW +#include +#include +#include +#include // For fcntl +#include +#include // For std::system to call the external script for slcand +#include // checking if script files exist + +#include "ament_index_cpp/get_package_share_directory.hpp" // to load the bash scripts +#include "ament_index_cpp/get_package_prefix.hpp" // defines PackageNotFoundError + +namespace autonomy +{ + +CanCore::CanCore(const rclcpp::Logger& logger) + : logger_(logger), socket_fd_(-1), initialized_(false), connected_(false) +{ + RCLCPP_INFO(logger_, "CAN Core initialized"); +} + +bool CanCore::initialize(const CanConfig& config) +{ + RCLCPP_INFO(logger_, "Initializing CAN interface: %s", config.interface_name.c_str()); + + config_ = config; + + if (config.bustype == "socketcan") { + return setupSocketCan(); + } else if (config.bustype == "slcan") { + return setupSlcan(); + } else { + RCLCPP_ERROR(logger_, "Unsupported bus type: %s", config.bustype.c_str()); + return false; + } +} + +bool CanCore::shutdown() +{ + RCLCPP_INFO(logger_, "Shutting down CAN interface"); + + if (socket_fd_ >= 0) { + close(socket_fd_); + socket_fd_ = -1; + } + + initialized_ = false; + connected_ = false; + + return true; +} + +bool CanCore::isInitialized() const +{ + return initialized_; +} + +bool CanCore::sendMessage(const CanMessage& message) +{ + if (!initialized_ || socket_fd_ < 0) { + RCLCPP_ERROR(logger_, "CAN interface not initialized or socket not valid. Cannot send message."); + return false; + } + + // Below are just some debug logs to show the message being sent + // RCLCPP_INFO(logger_, "Sending CAN frame:"); + // RCLCPP_INFO(logger_, " ID: 0x%X (%s)", message.id, message.is_extended_id ? "Extended" : "Standard"); + // RCLCPP_INFO(logger_, " Data Length: %zu bytes", message.data.size()); + + // // Log the actual data bytes + // std::string data_hex = ""; + // for (size_t i = 0; i < message.data.size(); ++i) { + // char hex_byte[8]; + // snprintf(hex_byte, sizeof(hex_byte), "%02X", message.data[i]); + // data_hex += hex_byte; + // if (i < message.data.size() - 1) data_hex += " "; + // } + // RCLCPP_INFO(logger_, " Data: [%s]", data_hex.c_str()); + + ssize_t bytes_written = -1; + size_t expected_frame_size = 0; + + // Classic CAN frame only + struct can_frame frame; + std::memset(&frame, 0, sizeof(frame)); + + frame.can_id = message.id; + if (message.is_extended_id) { + frame.can_id |= CAN_EFF_FLAG; + } + if (message.is_remote_frame) { + frame.can_id |= CAN_RTR_FLAG; + } + + if (message.data.size() > CAN_MAX_DLEN) { + frame.can_dlc = CAN_MAX_DLEN; + } else { + frame.can_dlc = static_cast<__u8>(message.data.size()); + } + + if (!message.is_remote_frame) { + std::memcpy(frame.data, message.data.data(), frame.can_dlc); + } + + expected_frame_size = sizeof(struct can_frame); + bytes_written = write(socket_fd_, &frame, expected_frame_size); + + if (bytes_written < 0) { + RCLCPP_ERROR(logger_, "Failed to write CAN frame to socket: %s", strerror(errno)); + return false; + } + + if (static_cast(bytes_written) < expected_frame_size) { + RCLCPP_WARN(logger_, "Could not send full CAN frame. Sent %zd bytes, expected %zu bytes.", + bytes_written, expected_frame_size); + return false; + } + + // RCLCPP_INFO(logger_, "CAN frame sent successfully! (%zd bytes written)", bytes_written); + return true; +} + +bool CanCore::receiveMessage(CanMessage& message) +{ + if (!initialized_ || socket_fd_ < 0) { + RCLCPP_ERROR(logger_, "CAN interface not initialized or socket not valid. Cannot receive message."); + return false; + } + + struct can_frame frame; + ssize_t bytes_read = read(socket_fd_, &frame, sizeof(struct can_frame)); + + if (bytes_read < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK) { + // No data available right now (non-blocking mode) + return false; + } + RCLCPP_ERROR(logger_, "Failed to read CAN frame from socket: %s", strerror(errno)); + return false; + } + + if (bytes_read == 0) { + // According to read(2), a return value of 0 indicates end-of-file. + // For a socket, this can mean the peer has performed an orderly shutdown. + // For raw CAN sockets, this is less typical than EAGAIN for no data. + // RCLCPP_WARN(logger_, "Read 0 bytes from CAN socket. Peer might have closed connection or no data."); + return false; + } + + if (static_cast(bytes_read) < sizeof(struct can_frame)) { + RCLCPP_WARN(logger_, "Incomplete CAN frame received. Read %zd bytes, expected %zu bytes.", + bytes_read, sizeof(struct can_frame)); + return false; + } + + message.is_extended_id = (frame.can_id & CAN_EFF_FLAG) ? true : false; + message.is_remote_frame = (frame.can_id & CAN_RTR_FLAG) ? true : false; + + if (message.is_extended_id) { + message.id = frame.can_id & CAN_EFF_MASK; + } else { + message.id = frame.can_id & CAN_SFF_MASK; + } + + message.dlc = frame.can_dlc; + message.data.resize(frame.can_dlc); + if (!message.is_remote_frame) { + std::memcpy(message.data.data(), frame.data, frame.can_dlc); + } else { + // For RTR frames, data field is irrelevant but dlc indicates requested data length + message.data.clear(); + } + + // This is to log received message details for debugging + RCLCPP_INFO(logger_, "CAN frame received: ID=0x%X, Extended=%d, RTR=%d", + message.id, message.is_extended_id, message.is_remote_frame); + + return true; +} + +bool CanCore::setupSocketCan() +{ + RCLCPP_INFO(logger_, "Setting up SocketCAN interface: %s", config_.interface_name.c_str()); + + // Create socket + socket_fd_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (socket_fd_ < 0) { + RCLCPP_ERROR(logger_, "Failed to create SocketCAN socket: %s", strerror(errno)); + return false; + } + + // Set socket to non-blocking + int flags = fcntl(socket_fd_, F_GETFL, 0); + if (flags == -1) { + RCLCPP_ERROR(logger_, "Failed to get socket flags: %s", strerror(errno)); + close(socket_fd_); + socket_fd_ = -1; + return false; + } + if (fcntl(socket_fd_, F_SETFL, flags | O_NONBLOCK) == -1) { + RCLCPP_ERROR(logger_, "Failed to set socket to non-blocking: %s", strerror(errno)); + close(socket_fd_); + socket_fd_ = -1; + return false; + } + RCLCPP_INFO(logger_, "Socket set to non-blocking mode."); + + // Get interface index + struct ifreq ifr; + std::strncpy(ifr.ifr_name, config_.interface_name.c_str(), IFNAMSIZ - 1); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; // Ensure null termination + if (ioctl(socket_fd_, SIOCGIFINDEX, &ifr) < 0) { + RCLCPP_ERROR(logger_, "Failed to get interface index for %s: %s", config_.interface_name.c_str(), strerror(errno)); + close(socket_fd_); + socket_fd_ = -1; + return false; + } + + // Bind socket to the CAN interface + struct sockaddr_can addr; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (bind(socket_fd_, (struct sockaddr*)&addr, sizeof(addr)) < 0) { + RCLCPP_ERROR(logger_, "Failed to bind SocketCAN socket to %s: %s", config_.interface_name.c_str(), strerror(errno)); + close(socket_fd_); + socket_fd_ = -1; + return false; + } + + initialized_ = true; + connected_ = true; + + RCLCPP_INFO(logger_, "SocketCAN interface %s setup completed successfully (Classic CAN mode).", config_.interface_name.c_str()); + return true; +} + +bool CanCore::setupSlcan() +{ + RCLCPP_INFO(logger_, "Setting up SLCAN interface '%s' via external script.", config_.interface_name.c_str()); + RCLCPP_INFO(logger_, " Device path for script: %s", config_.device_path.c_str()); + RCLCPP_INFO(logger_, " Bitrate for script: %u", config_.bitrate); + + // Run the slcand daemon to initialize the CAN bus + std::string package_share_directory; + try { + package_share_directory = ament_index_cpp::get_package_share_directory("can"); // the package we're in is can + } catch (const ament_index_cpp::PackageNotFoundError& e) { + RCLCPP_ERROR(logger_, "Package not found for script path: %s", e.what()); + return false; + } + std::string script_path = package_share_directory + "/scripts/setup_can.sh"; + + // Check if the script actually exists at this script_path + struct stat buffer; + if (stat(script_path.c_str(), &buffer) != 0) { + RCLCPP_ERROR(logger_, "Setup script not found"); + return false; + } + + std::string bitrate_code; + + // Map config_.bitrate to the slcan code (e.g., 500000 -> "-s6") + if (config_.bitrate == 10000) bitrate_code = "-s0"; + else if (config_.bitrate == 20000) bitrate_code = "-s1"; + else if (config_.bitrate == 50000) bitrate_code = "-s2"; + else if (config_.bitrate == 100000) bitrate_code = "-s3"; + else if (config_.bitrate == 125000) bitrate_code = "-s4"; + else if (config_.bitrate == 250000) bitrate_code = "-s5"; + else if (config_.bitrate == 500000) bitrate_code = "-s6"; + else if (config_.bitrate == 750000) bitrate_code = "-s7"; + else if (config_.bitrate == 1000000) bitrate_code = "-s8"; + else { + RCLCPP_ERROR(logger_, "Unsupported bitrate for slcan script: %u", config_.bitrate); + return false; + } + + std::string command = script_path + " " + config_.device_path + " " + config_.interface_name + " " + bitrate_code; + + RCLCPP_INFO(logger_, "Executing CAN setup script: %s", command.c_str()); + int result = std::system(command.c_str()); + + if (result != 0) { + RCLCPP_ERROR(logger_, "CAN setup script failed with exit code %d. Check script output and permissions.", result); + return false; + } + RCLCPP_INFO(logger_, "CAN setup script executed successfully."); + + // by now, a new CAN interface should be created based on the name given in the can/config/params.yml + RCLCPP_INFO(logger_, "Proceeding to configure '%s' as a SocketCAN interface.", config_.interface_name.c_str()); + return setupSocketCan(); +} +} diff --git a/autonomy/interfacing/can/src/can_node.cpp b/autonomy/interfacing/can/src/can_node.cpp new file mode 100644 index 0000000..1fbf116 --- /dev/null +++ b/autonomy/interfacing/can/src/can_node.cpp @@ -0,0 +1,346 @@ +#include "can_node.hpp" +#include "mit_motor_control.hpp" +#include +#include +#include +#include +#include // Required for std::memcpy + +CanNode::CanNode() : Node("can_node"), can_(this->get_logger()) { + RCLCPP_INFO(this->get_logger(), "CAN Node has been initialized"); + + // Load parameters from config/params.yaml + this->declare_parameter("can_interface", "can0"); + this->declare_parameter("device_path", "/dev/canable"); + this->declare_parameter("bustype", "slcan"); + this->declare_parameter("bitrate", 500000); + this->declare_parameter("receive_poll_interval_ms", 10); // Parameter for polling interval + + // Get parameter values + std::string can_interface = this->get_parameter("can_interface").as_string(); + std::string device_path = this->get_parameter("device_path").as_string(); + std::string bustype = this->get_parameter("bustype").as_string(); + int bitrate = this->get_parameter("bitrate").as_int(); + long receive_poll_interval_ms = this->get_parameter("receive_poll_interval_ms").as_int(); + + RCLCPP_INFO(this->get_logger(), "Loaded parameters: interface=%s, bustype=%s, bitrate=%d, poll_interval_ms=%ld", + can_interface.c_str(), bustype.c_str(), bitrate, receive_poll_interval_ms); + + // Configure CanCore + autonomy::CanConfig config; + config.interface_name = can_interface; + config.device_path = device_path; + config.bustype = bustype; + config.bitrate = bitrate; + config.receive_timeout_ms = 10000; // This specific timeout in CanConfig might be for other uses or can be reviewed. + + // Initialize the CAN interface + if (can_.initialize(config)) { + RCLCPP_INFO(this->get_logger(), "CAN Core interface initialized successfully"); + + // Setup a timer to periodically call receiveCanMessages + receive_timer_ = this->create_wall_timer( + std::chrono::milliseconds(receive_poll_interval_ms), + std::bind(&CanNode::receiveCanMessages, this)); + RCLCPP_INFO(this->get_logger(), "CAN message receive timer started with %ld ms interval.", receive_poll_interval_ms); + + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize CAN Core"); + } + + // Load topic configurations and create subscribers + loadTopicConfigurations(); + createSubscribers(); +} + +void CanNode::loadTopicConfigurations() { + try { + // Declare and get the topics parameter as a string array + this->declare_parameter("topics", std::vector{"/test_controller"}); + auto topic_names = this->get_parameter("topics").as_string_array(); + + for (const auto& topic_name : topic_names) { + std::string topic_type = discoverTopicType(topic_name); // Discover the message type for a given topic + + if (!topic_type.empty()) { + TopicConfig config; + config.name = topic_name; + config.type = topic_type; + topic_configs_.push_back(config); + + RCLCPP_INFO(this->get_logger(), "Loaded topic config: %s (%s)", + topic_name.c_str(), topic_type.c_str()); + } else { + RCLCPP_WARN(this->get_logger(), "Could not discover message type for topic: %s", + topic_name.c_str()); + } + } + + if (topic_configs_.empty()) { + RCLCPP_WARN(this->get_logger(), "No valid topics found in configuration - CAN node will not subscribe to any topics"); + } + + } catch (const std::exception& e) { + RCLCPP_ERROR(this->get_logger(), "Error loading topic configurations: %s", e.what()); + } +} + +std::string CanNode::discoverTopicType(const std::string& topic_name) { + auto topic_names_and_types = this->get_topic_names_and_types(); // get topic information from ROS graph + + for (const auto& topic_info : topic_names_and_types) { + if (topic_info.first == topic_name) { + if (!topic_info.second.empty()) { + return topic_info.second[0]; + } + } + } + + // If topic is not found, wait a bit and try again (topic might not be published yet) + RCLCPP_INFO(this->get_logger(), "Topic '%s' not found, waiting for it to become available...", topic_name.c_str()); + + // Wait up to 10 seconds for the topic to appear + for (int i = 0; i < 100; ++i) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + topic_names_and_types = this->get_topic_names_and_types(); + + for (const auto& topic_info : topic_names_and_types) { + if (topic_info.first == topic_name) { + if (!topic_info.second.empty()) { + RCLCPP_INFO(this->get_logger(), "Found topic '%s' with type '%s'", + topic_name.c_str(), topic_info.second[0].c_str()); + return topic_info.second[0]; + } + } + } + } + + RCLCPP_ERROR(this->get_logger(), "Timeout waiting for topic '%s' to become available", topic_name.c_str()); + return ""; +} + +void CanNode::createSubscribers() { + for (const auto& topic_config : topic_configs_) { + if (topic_config.name == "/test_controller") { + subscribers_[topic_config.name] = this->create_generic_subscription( + topic_config.name, + topic_config.type, + 10, + [this](std::shared_ptr msg) { + this->handleControllerTopic(msg, "/test_controller"); + } + ); + } else if (topic_config.name == "/cmd_arm_joint") { + subscribers_[topic_config.name] = this->create_generic_subscription( + topic_config.name, + topic_config.type, + 10, + [this](std::shared_ptr msg) { + this->handleArmJointTopic(msg, "/cmd_arm_joint"); + } + ); + } else if (topic_config.name == "/cmd_hand_joint") { + subscribers_[topic_config.name] = this->create_generic_subscription( + topic_config.name, + topic_config.type, + 10, + [this](std::shared_ptr msg) { + this->handleHandJointTopic(msg, "/cmd_hand_joint"); + } + ); + } else if (topic_config.name == "/cmd_arm_ee") { + subscribers_[topic_config.name] = this->create_generic_subscription( + topic_config.name, + topic_config.type, + 10, + [this](std::shared_ptr msg) { + this->handleArmEeTopic(msg, "/cmd_arm_ee"); + } + ); + } else if (topic_config.name == "/cmd_hand_ee") { + subscribers_[topic_config.name] = this->create_generic_subscription( + topic_config.name, + topic_config.type, + 10, + [this](std::shared_ptr msg) { + this->handleHandEeTopic(msg, "/cmd_hand_ee"); + } + ); + } else { + subscribers_[topic_config.name] = this->create_generic_subscription( + topic_config.name, + topic_config.type, + 10, + [this, topic_name = topic_config.name](std::shared_ptr msg) { + this->handleGenericTopic(msg, topic_name); + } + ); + } + + RCLCPP_INFO(this->get_logger(), "Created generic subscriber for topic: %s (type: %s)", + topic_config.name.c_str(), topic_config.type.c_str()); + } +} + +// dummy handler for testing +void CanNode::handleArmJointTopic(std::shared_ptr msg, const std::string& topic_name) { + // Custom CAN ID range for controller messages + // uint32_t can_id = 0x100; // set based on robot message + // std::vector can_messages = createCanMessages(topic_name, msg, can_id); + // sendCanMessages(can_messages, topic_name); +} + +void CanNode::handleHandJointTopic(std::shared_ptr msg, const std::string& topic_name) {} +void CanNode::handleHandEeTopic(std::shared_ptr msg, const std::string& topic_name) {} +void CanNode::handleArmEeTopic(std::shared_ptr msg, const std::string& topic_name) {} + + +void CanNode::handleGenericTopic(std::shared_ptr msg, const std::string& topic_name) { + // Original generic handling + std::vector can_messages = createCanMessages(topic_name, msg); + sendCanMessages(can_messages, topic_name); +} + +// Helper method to reduce code duplication +void CanNode::sendCanMessages(const std::vector& can_messages, const std::string& topic_name) { + int successful_sends = 0; + for (const auto& can_message : can_messages) { + if (can_.sendMessage(can_message)) { + successful_sends++; + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to send CAN message for topic '%s' (ID 0x%X)", topic_name.c_str(), can_message.id); + } + } + + if (can_messages.size() > 1) { + RCLCPP_INFO(this->get_logger(), "Successfully sent %d/%zu CAN frames for topic '%s'", + successful_sends, can_messages.size(), topic_name.c_str()); + } +} + + +void CanNode::receiveCanMessages() { + autonomy::CanMessage received_msg; + // Attempt to receive a message. CanCore::receiveMessage is non-blocking. + if (can_.receiveMessage(received_msg)) { + // Print the received message details to the console + std::stringstream ss; + ss << "Received CAN Message: ID=0x" << std::hex << received_msg.id + << std::dec << ", DLC=" << static_cast(received_msg.dlc) + << ", Extended=" << received_msg.is_extended_id + << ", RTR=" << received_msg.is_remote_frame + << ", Data=[ "; + for (size_t i = 0; i < received_msg.data.size(); ++i) { + ss << "0x" << std::hex << static_cast(received_msg.data[i]) << (i < received_msg.data.size() - 1 ? " " : ""); + } + ss << std::dec << " ]"; // Switch back to decimal for any further logging if needed + RCLCPP_INFO(this->get_logger(), "%s", ss.str().c_str()); + } + // If receiveMessage returns false, it means no message was available at that moment (non-blocking read) + // or an error occurred (which CanCore should log). No action needed here for no-message case. +} + + +// Not sure if this is needed, but it's here for now +uint32_t CanNode::generateCanId(const std::string& topic_name) { + // Generate a CAN ID from topic name using hash + // Use the first 29 bits for extended CAN ID (CAN 2.0B) + std::hash hasher; + uint32_t hash = static_cast(hasher(topic_name)); + + // Mask to 29 bits for extended CAN ID (0x1FFFFFFF) + // For CAN-FD, we can use the full extended ID range + return hash & 0x1FFFFFFF; +} + +std::vector CanNode::createCanMessages(const std::string& topic_name, std::shared_ptr ros_msg, uint32_t can_id) { + std::vector messages_to_send; + + bool is_extended = true; // Use extended CAN ID (29 bits) + bool is_rtr = false; // Remote Transmission Request + + auto now_chrono = std::chrono::high_resolution_clock::now(); + uint64_t timestamp = std::chrono::duration_cast( + now_chrono.time_since_epoch()).count(); + + const uint8_t* ros_msg_buffer = ros_msg->get_rcl_serialized_message().buffer; + size_t ros_msg_size = ros_msg->size(); + + // Determine max data payload per frame + // For Classic CAN, it's 8 bytes maximum + // But reserve 1 byte for sequence number if fragmentation is needed. + const size_t max_data_bytes_per_classic_frame = 8; + + size_t max_payload_per_frame = max_data_bytes_per_classic_frame; + size_t data_chunk_size = max_payload_per_frame; + bool needs_fragmentation = ros_msg_size > max_payload_per_frame; + + if (needs_fragmentation) { + // Reserve 1 byte for sequence number if fragmenting + data_chunk_size = max_payload_per_frame - 1; + if (data_chunk_size == 0 && max_payload_per_frame > 0) { // Should not happen with CAN_MAX_DLEN >= 1 + RCLCPP_ERROR(this->get_logger(), "Calculated data_chunk_size is 0, this should not happen. Max payload: %zu", max_payload_per_frame); + return messages_to_send; // Return empty, indicates error + } + } + + if (!needs_fragmentation) { + autonomy::CanMessage can_msg; + can_msg.id = can_id; + can_msg.is_extended_id = is_extended; + can_msg.is_remote_frame = is_rtr; + can_msg.timestamp_us = timestamp; + + can_msg.data.resize(ros_msg_size); + std::memcpy(can_msg.data.data(), ros_msg_buffer, ros_msg_size); + + RCLCPP_DEBUG(this->get_logger(), "Packaged %zu bytes from topic '%s' into single CAN frame with ID 0x%X (Classic CAN)", + ros_msg_size, topic_name.c_str(), can_msg.id); + messages_to_send.push_back(can_msg); + } else { + RCLCPP_INFO(this->get_logger(), "Message from topic '%s' (%zu bytes) is too large for a single CAN frame (max %zu bytes). Fragmenting.", + topic_name.c_str(), ros_msg_size, max_payload_per_frame); + + size_t bytes_sent = 0; + uint8_t sequence_number = 0; + + while (bytes_sent < ros_msg_size) { + autonomy::CanMessage can_fragment; + can_fragment.id = can_id; // All fragments share the same ID for now + can_fragment.is_extended_id = is_extended; + can_fragment.is_remote_frame = is_rtr; + can_fragment.timestamp_us = timestamp; // Could also update timestamp per fragment + + size_t current_fragment_payload_size = std::min(data_chunk_size, ros_msg_size - bytes_sent); + + can_fragment.data.resize(1 + current_fragment_payload_size); // 1 byte for sequence number + can_fragment.data[0] = sequence_number; + std::memcpy(can_fragment.data.data() + 1, ros_msg_buffer + bytes_sent, current_fragment_payload_size); + + messages_to_send.push_back(can_fragment); + + // This is just logging that fragment creation was successful + // RCLCPP_INFO(this->get_logger(), "Created fragment %u for topic '%s' (ID 0x%X), size %zu (payload %zu)", + // sequence_number, topic_name.c_str(), can_fragment.id, can_fragment.data.size(), current_fragment_payload_size); + + bytes_sent += current_fragment_payload_size; + sequence_number++; + + if (sequence_number == 0 && bytes_sent < ros_msg_size) { // Rollover, too many fragments + RCLCPP_ERROR(this->get_logger(), "Too many fragments for message from topic '%s'. Max 256 fragments supported with 1-byte sequence.", topic_name.c_str()); + messages_to_send.clear(); // Indicate error by returning no messages + return messages_to_send; + } + } + // RCLCPP_INFO(this->get_logger(), "Fragmented message from topic '%s' into %zu frames.", topic_name.c_str(), messages_to_send.size()); + } + + return messages_to_send; +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/autonomy/interfacing/can/src/mit_motor_control.cpp b/autonomy/interfacing/can/src/mit_motor_control.cpp new file mode 100644 index 0000000..58193f1 --- /dev/null +++ b/autonomy/interfacing/can/src/mit_motor_control.cpp @@ -0,0 +1,70 @@ +void CanNode::moveMIT(double velocity, double force, double position, int id) { + // Create CAN message for MIT (Massachusetts Institute of Technology) control mode + // This typically involves velocity, force, and position parameters + + autonomy::CanMessage can_msg; + can_msg.id = id; // MIT control command ID + can_msg.is_extended_id = true; + can_msg.is_remote_frame = false; + can_msg.dlc = 8; // 8 bytes for the control parameters + + // Pack the control parameters into the CAN message data + // Assuming IEEE 754 double precision format (8 bytes total) + // We'll use 2 bytes for velocity, 2 for force, 2 for position, 2 reserved + uint16_t vel_scaled = static_cast(velocity * 1000.0); // Scale for precision + uint16_t force_scaled = static_cast(force * 1000.0); + uint16_t pos_scaled = static_cast(position * 1000.0); + + can_msg.data[0] = (vel_scaled >> 8) & 0xFF; // Velocity high byte + can_msg.data[1] = vel_scaled & 0xFF; // Velocity low byte + can_msg.data[2] = (force_scaled >> 8) & 0xFF; // Force high byte + can_msg.data[3] = force_scaled & 0xFF; // Force low byte + can_msg.data[4] = (pos_scaled >> 8) & 0xFF; // Position high byte + can_msg.data[5] = pos_scaled & 0xFF; // Position low byte + can_msg.data[6] = 0x00; // Reserved + can_msg.data[7] = 0x00; // Reserved + + // Send the CAN message + if (can_.sendMessage(can_msg)) { + RCLCPP_INFO(this->get_logger(), "MIT control message sent - V:%.3f, F:%.3f, P:%.3f", + velocity, force, position); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to send MIT control message"); + } +} + +void CanNode::moveProfile(const std::string& profile_type, double max_acc) { + // Create CAN message for profile-based motion control + // Profile types could be: "trapezoidal", "s-curve", "polynomial", etc. + + autonomy::CanMessage can_msg; + can_msg.id = 0x201; // Profile control command ID + can_msg.is_extended_id = true; + can_msg.is_remote_frame = false; + can_msg.dlc = 8; // 8 bytes for profile parameters + + // Encode profile type (first 4 bytes) + std::hash hasher; + uint32_t profile_hash = static_cast(hasher(profile_type)); + + can_msg.data[0] = (profile_hash >> 24) & 0xFF; // Profile type hash high byte + can_msg.data[1] = (profile_hash >> 16) & 0xFF; // Profile type hash mid-high byte + can_msg.data[2] = (profile_hash >> 8) & 0xFF; // Profile type hash mid-low byte + can_msg.data[3] = profile_hash & 0xFF; // Profile type hash low byte + + // Encode maximum acceleration (last 4 bytes) + uint32_t acc_scaled = static_cast(max_acc * 1000.0); // Scale for precision + + can_msg.data[4] = (acc_scaled >> 24) & 0xFF; // Max acc high byte + can_msg.data[5] = (acc_scaled >> 16) & 0xFF; // Max acc mid-high byte + can_msg.data[6] = (acc_scaled >> 8) & 0xFF; // Max acc mid-low byte + can_msg.data[7] = acc_scaled & 0xFF; // Max acc low byte + + // Send the CAN message + if (can_.sendMessage(can_msg)) { + RCLCPP_INFO(this->get_logger(), "Profile control message sent - Type:%s, MaxAcc:%.3f", + profile_type.c_str(), max_acc); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to send profile control message"); + } +} \ No newline at end of file diff --git a/modules_future/docker-compose.interfacing.yaml b/autonomy/interfacing/can/test/test_can.cpp similarity index 100% rename from modules_future/docker-compose.interfacing.yaml rename to autonomy/interfacing/can/test/test_can.cpp diff --git a/autonomy/interfacing/can/test/test_controller.cpp b/autonomy/interfacing/can/test/test_controller.cpp new file mode 100644 index 0000000..cf25538 --- /dev/null +++ b/autonomy/interfacing/can/test/test_controller.cpp @@ -0,0 +1,59 @@ +#include +#include +#include + +class TestControllerNode : public rclcpp::Node +{ +public: + TestControllerNode() : Node("test_controller") + { + // Create publisher for test controller messages + publisher_ = this->create_publisher("/test_controller", 10); + + // Create timer to publish messages at 1 Hz + timer_ = this->create_wall_timer( + std::chrono::seconds(1), + std::bind(&TestControllerNode::publish_test_data, this) + ); + + RCLCPP_INFO(this->get_logger(), "Test Controller Node started - publishing to /test_controller at 1 Hz"); + } + +private: + void publish_test_data() + { + auto message = std_msgs::msg::String(); + + // Create a readable test message with timestamp + auto now = std::chrono::system_clock::now(); + auto time_t = std::chrono::system_clock::to_time_t(now); + + message.data = "Hello from WATonomous! Timestamp: " + std::to_string(time_t) + " Status: Active"; + + // Publish the message + publisher_->publish(message); + + RCLCPP_INFO(this->get_logger(), "Published: %s", message.data.c_str()); + } + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + RCLCPP_INFO(node->get_logger(), "Starting Test Controller..."); + + try { + rclcpp::spin(node); + } catch (const std::exception& e) { + RCLCPP_ERROR(node->get_logger(), "Exception in test controller: %s", e.what()); + } + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/autonomy/interfacing/test.cpp b/autonomy/interfacing/test.cpp new file mode 100644 index 0000000..e69de29 diff --git a/autonomy/wato_msgs/common_msgs/README.md b/autonomy/wato_msgs/common_msgs/README.md new file mode 100644 index 0000000..73ea65d --- /dev/null +++ b/autonomy/wato_msgs/common_msgs/README.md @@ -0,0 +1,64 @@ +# Common Messages (common_msgs) + +## File Tree/Package Structure +This message package follows standard ROS2 package scheme: +``` +common_msgs/ +├── CMakeLists.txt +├── package.xml +├── README.md +└── msg/ + ├── ArmPose.msg + ├── HandPose.msg + └── JointState.msg +``` + +## Purpose +The `common_msgs` package serves as a global package for message definitions that are commonly used across various components and packages within the humanoid project. + +## Inputs & Outputs +This package primarily defines message structures. Therefore: +- **Outputs**: Provides `.msg` definitions that other ROS 2 packages can import and use. +- **Inputs**: Does not process data or subscribe to topics; it only defines data structures. + +## Key Features +- **Message Definitions**: Contains a collection of `.msg` files, each defining a specific data structure. These messages are designed to be generic and applicable in multiple contexts. + +### Message Definitions +- `ArmPose.msg`: Describes the pose of an arm, including its major joints and optionally the hand. +- `HandPose.msg`: Details the pose of a hand, including individual finger joint states. +- `JointState.msg`: A custom message (similar message structure to [sensor_msgs/JointState](https://docs.ros.org/en/humble/p/sensor_msgs/msg/JointState.html)) to represent a single joint. + +## Usage +To use the messages defined in this package within another ROS 2 package: + +1. **Copy the package into your Docker image's source workspace**: Add the following line to your Dockerfile (typically `root/docker/MODULE_NAME/MODULE_NAME.Dockerfile`), typically in the section where you copy your source code: + ```dockerfile + # # Copy in source code + # # COPY autonomy/wato_msgs/sample_msgs sample_msgs + COPY autonomy/wato_msgs/common_msgs common_msgs + ``` + +2. **Add Dependency**: Ensure that `common_msgs` is listed as a dependency in the `package.xml` of your consuming package: + ```xml + common_msgs + ``` +3. **Include in CMakeLists.txt** (for C++ packages): + ```cmake + find_package(common_msgs REQUIRED) + ament_target_dependencies(your_target_name common_msgs) + ``` +4. **Import in Python scripts**: + ```python + from common_msgs.msg import YourMessageName + ``` +5. **Include in C++ code**: + ```cpp + #include "common_msgs/msg/your_message_name.hpp" + ``` + +### Testing +Currently, this package primarily contains message definitions, so testing focuses on ensuring they can be correctly compiled and used by dependent packages. + +## Configuration +- **Parameters**: No configurable parameters are defined within this package itself. diff --git a/docker/interfacing/interfacing.Dockerfile b/docker/interfacing/interfacing.Dockerfile new file mode 100644 index 0000000..0b74b4f --- /dev/null +++ b/docker/interfacing/interfacing.Dockerfile @@ -0,0 +1,53 @@ +ARG BASE_IMAGE=ghcr.io/watonomous/robot_base/base:humble-ubuntu22.04 + +################################ Source ################################ +FROM ${BASE_IMAGE} AS source + +WORKDIR ${AMENT_WS}/src + +# Copy in source code +COPY autonomy/interfacing/can can +COPY autonomy/wato_msgs/sample_msgs sample_msgs + +# Scan for rosdeps +RUN apt-get -qq update && rosdep update && \ + rosdep install --from-paths . --ignore-src -r -s \ + | grep 'apt-get install' \ + | awk '{print $3}' \ + | sort > /tmp/colcon_install_list + +################################# Dependencies ################################ +FROM ${BASE_IMAGE} AS dependencies + +# Install Rosdep requirements +COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list +RUN apt-fast install -qq -y --no-install-recommends \ + $(cat /tmp/colcon_install_list) can-utils net-tools iproute2 + +# Copy in source code from source stage +WORKDIR ${AMENT_WS} +COPY --from=source ${AMENT_WS}/src src + +# Dependency Cleanup +WORKDIR / +RUN apt-get -qq autoremove -y && apt-get -qq autoclean && apt-get -qq clean && \ + rm -rf /root/* /root/.ros /tmp/* /var/lib/apt/lists/* /usr/share/doc/* + +################################ Build ################################ +FROM dependencies AS build + +# Build ROS2 packages +WORKDIR ${AMENT_WS} +RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ + colcon build \ + --cmake-args -DCMAKE_BUILD_TYPE=Release --install-base ${WATONOMOUS_INSTALL} + +# Source and Build Artifact Cleanup +RUN rm -rf src/* build/* devel/* install/* log/* + +# pass through the udev-created symlinks to the container. +ENV UDEV=1 + +# Entrypoint will run before any CMD on launch. Sources ~/opt//setup.bash and ~/ament_ws/install/setup.bash +COPY docker/wato_ros_entrypoint.sh ${AMENT_WS}/wato_ros_entrypoint.sh +ENTRYPOINT ["./wato_ros_entrypoint.sh"] diff --git a/modules/docker-compose.interfacing.yaml b/modules/docker-compose.interfacing.yaml new file mode 100644 index 0000000..082cf3a --- /dev/null +++ b/modules/docker-compose.interfacing.yaml @@ -0,0 +1,21 @@ +version: "3.8" + +services: + interfacing: + build: &interfacing + context: .. + dockerfile: docker/interfacing/interfacing.Dockerfile + cache_from: + - "${INTERFACING_IMAGE:?}:${TAG}" + - "${INTERFACING_IMAGE:?}:main" + image: "${INTERFACING_IMAGE:?}:${TAG}" + profiles: [deploy, develop] + command: /bin/bash -c "ros2 launch can can.launch.py" + privileged: false + devices: + - /dev/canable:/dev/canable + network_mode: host # Allow the container to access the host's network interfaces + cap_add: # Grant the capability to configure network interfaces + - NET_ADMIN + volumes: + - ${MONO_DIR}/autonomy/interfacing:/root/ament_ws/src/interfacing \ No newline at end of file diff --git a/watod-config.sh b/watod-config.sh index d7ac13e..2a97203 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -17,7 +17,7 @@ ## - simulation : starts simulation ## - samples : starts sample ROS2 pubsub nodes -ACTIVE_MODULES="" +ACTIVE_MODULES="interfacing" ################################# MODE OF OPERATION ################################# ## Possible modes of operation when running watod. diff --git a/watod_scripts/watod-setup-docker-env.sh b/watod_scripts/watod-setup-docker-env.sh index 0db3992..cc76050 100644 --- a/watod_scripts/watod-setup-docker-env.sh +++ b/watod_scripts/watod-setup-docker-env.sh @@ -54,6 +54,7 @@ SAMPLES_TRANSFORMER_IMAGE=${SAMPLES_TRANSFORMER_IMAGE:-"$REGISTRY_URL/samples/sa # Images INFRASTRUCTURE_FOXGLOVE_IMAGE=${INFRASTRUCTURE_FOXGLOVE_IMAGE:-"$REGISTRY_URL/infrastructure/foxglove"} +INTERFACING_IMAGE=${INTERFACING_IMAGE:-"$REGISTRY_URL/interfacing"} ## --------------------------- Ports ------------------------------ @@ -87,3 +88,4 @@ echo "SAMPLES_TRANSFORMER_IMAGE=$SAMPLES_TRANSFORMER_IMAGE" >> "$MODULES_DIR/.en # Images echo "INFRASTRUCTURE_FOXGLOVE_IMAGE=$INFRASTRUCTURE_FOXGLOVE_IMAGE" >> "$MODULES_DIR/.env" +echo "INTERFACING_IMAGE=$INTERFACING_IMAGE" >> "$MODULES_DIR/.env"