diff --git a/README.md b/README.md index f65a5ab..bde5da5 100644 --- a/README.md +++ b/README.md @@ -12,16 +12,31 @@ Для управления используется универсальная плата-контроллер, которая может быть использована в вариантах исполнения двигателей разных диаметров (на данный момент 50 мм и 70 мм) со сходными характеристиками обмоток. Контроллер управляется через CAN-интерфейс. -Для удобства изготовления двигателя разработан станок для намотки катушек индуктивности статора. Исходные файлы для производства станка размещены в репозитории [gitlab.com/robossembler/cnc/motor-wire-winder](https://gitlab.com/robossembler/cnc/motor-wire-winder). - ## Описание директорий - `brd` - KiCAD проект печатной платы контроллера -- `firmware` - исходный код программы для микроконтроллера -- `rbs_servo_hardware` - аппаратный интерфейс для ros2_control +- `firmware` - исходный код программы для микроконтроллера, инструкция по сборке и загрузке прошивки в `firmware/embed`, тесты для проверки встроенного ПО в `firmware/test` +- `ros2_environment` - пакеты для управления мотором из ROS 2 с помощью ros2_control - `src` - 3D-модели вариантов исполнения мотора: 50 мм, 70 мм, металлическая версия двигателя (не интегрирована с контроллером) - `tools` - вспомогательное оборудование для тестирования, испытаний +## Краткая инструкция по изготовлению + +### Статор + +Для удобства изготовления статоров разработан станок для намотки катушек индуктивности. Исходные файлы для производства станка и инструкции размещены в репозитории [gitlab.com/robossembler/cnc/motor-wire-winder](https://gitlab.com/robossembler/cnc/motor-wire-winder). + +### Сборка + +1. Вставить 28 магнитов в ротор +2. Установить подшипник в статор +3. Установить проставку в статор между подшипником и платой +4. Припаять плату к обмоткам (схема обмоток приведена ниже) и установить ее в статор +5. Накрыть плату крышкой +6. Установить фиксирующий шплинт +7. Надеть на сборку статора ротор +8. Подключить разъем программирования XP3 и прошить с помощью ST-Link-совместимого программатора + ## Фото прототипов Первый прототип изготовленного печатного мотора диаметром 50мм. @@ -30,4 +45,4 @@ ## Схема намотки для двигателя 70мм -![coil winder schema](img/coil_winder_schema.jpg) +![coil winder schema](img/coil_winder_schema.jpg) \ No newline at end of file diff --git a/firmware/embed/README.md b/firmware/embed/README.md new file mode 100644 index 0000000..61796e3 --- /dev/null +++ b/firmware/embed/README.md @@ -0,0 +1,20 @@ +# Встроенное ПО для сервипривода на STM32F446RE + +## Для разработки + +- [Установить platformio](#introduction) +```bash +pip install -U platformio +``` +- [Скомпилировать проект](#build_project) +```bash +platformio run --environment robotroller_reborn +``` +- [Загрузить прошивку](#upload_project) +```bash +platformio run --target upload --environment robotroller_reborn +``` +- [Открыть монитор UART](#monitor_port) +```bash +platformio device monitor +``` diff --git a/firmware/embed/include/hal_conf_extra.h b/firmware/embed/include/hal_conf_extra.h index cc08e41..3ee6087 100644 --- a/firmware/embed/include/hal_conf_extra.h +++ b/firmware/embed/include/hal_conf_extra.h @@ -26,7 +26,8 @@ #define CAN2_RX PB12 #define CAN1_TX PB9 #define CAN1_RX PB8 - +#define GM6208_RESISTANCE 31 +#define OWN_RESISTANCE 26 #pragma endregion #if !defined(HAL_CAN_MODULE_ENABLED) diff --git a/firmware/embed/platformio.ini b/firmware/embed/platformio.ini index 4460efc..854186d 100644 --- a/firmware/embed/platformio.ini +++ b/firmware/embed/platformio.ini @@ -21,7 +21,7 @@ monitor_parity = N build_flags = -DSTM32F446xx -D HAL_CAN_MODULE_ENABLED - ; -D HAL_TIM_MODULE_ENABLED + -D SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH lib_deps = askuric/Simple FOC@^2.3.2 pazi88/STM32_CAN@^1.1.0 diff --git a/firmware/embed/src/main.cpp b/firmware/embed/src/main.cpp index 53bc8cd..d446a0d 100644 --- a/firmware/embed/src/main.cpp +++ b/firmware/embed/src/main.cpp @@ -68,33 +68,21 @@ void setup_foc(MagneticSensorAS5045 *sensor, BLDCMotor *motor, DRV8313Driver *dr motor->initFOC(); } -uint8_t Counter; void send_data() { // TODO(vanyabeat) add can functionability for ROS packets - if (Counter >= 255) { - Counter = 0; - } CAN_TX_msg.id = 1; - snprintf(reinterpret_cast(CAN_TX_msg.buf), sizeof(CAN_TX_msg.buf), "CNT:%d", Counter); - CAN_TX_msg.len = 8; + CAN_TX_msg.buf[0] = 'A'; + CAN_TX_msg.len = 5; Can.write(CAN_TX_msg); - // ++Counter; - // Serial.print("Sent: \n"); digitalWrite(PC11, !digitalRead(PC11)); } void read_can_message() { - Serial.print("Received: "); - Serial.print(CAN_inMsg.id); - Serial.print(" "); - Serial.print(CAN_inMsg.len); - Serial.print(" "); - CAN_inMsg.buf[7] = 0; - auto ptr = reinterpret_cast(CAN_inMsg.buf); - Serial.println(ptr); - doMotor(ptr); - Serial.print("\n"); + float angle; + memcpy(&angle, &CAN_inMsg.buf[1], sizeof(angle)); + motor.target = angle; + digitalWrite(PC10, !digitalRead(PC10)); } void run_foc(BLDCMotor *motor, Commander *commander) { @@ -115,15 +103,18 @@ void setup() { Can.setBaudRate(1000000); TIM_TypeDef *Instance = TIM2; HardwareTimer *SendTimer = new HardwareTimer(Instance); - SendTimer->setOverflow(1, HERTZ_FORMAT); // 50 Hz + SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz SendTimer->attachInterrupt(send_data); SendTimer->resume(); setup_foc(&sensor, &motor, &driver, ¤t_sense, &command, doMotor); _delay(1000); } +float current_angle = 0; void loop() { run_foc(&motor, &command); + current_angle = sensor.getAngle(); + memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle)); while (Can.read(CAN_inMsg)) { read_can_message(); } diff --git a/firmware/test/bring_up_can.sh b/firmware/test/bring_up_can.sh new file mode 100755 index 0000000..7ff94f6 --- /dev/null +++ b/firmware/test/bring_up_can.sh @@ -0,0 +1,4 @@ +#!/bin/bash + +ip link set can0 type can bitrate 1000000 +ip link set up can0 diff --git a/ros2_environment/README.md b/ros2_environment/README.md new file mode 100644 index 0000000..1690d6e --- /dev/null +++ b/ros2_environment/README.md @@ -0,0 +1,24 @@ +# ROS 2 пакеты для управления двигателем + +## Пререквизиты + +- ROS 2 Humble + +## Сборка и запуск ROS2 пакета + +```bash +colcon build --symlink-install +``` + +```bash +source install/setup.bash +``` + +```bash +ros2 launch rbs_servo_test startup.launch.py +``` + +Посмотреть состояние топиков: +```bash +ros2 topic echo /joint_states +``` diff --git a/ros2_environment/src/bldc_motor_control/CMakeLists.txt b/ros2_environment/src/bldc_motor_control/CMakeLists.txt new file mode 100644 index 0000000..4a6f143 --- /dev/null +++ b/ros2_environment/src/bldc_motor_control/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.8) +project(bldc_motor_control) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_executable(motor_control_node src/motor_control_node.cpp) +ament_target_dependencies(motor_control_node rclcpp std_msgs) + +install(TARGETS + motor_control_node + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/ros2_environment/src/bldc_motor_control/package.xml b/ros2_environment/src/bldc_motor_control/package.xml new file mode 100644 index 0000000..83c0e73 --- /dev/null +++ b/ros2_environment/src/bldc_motor_control/package.xml @@ -0,0 +1,19 @@ + + + + bldc_motor_control + 0.0.1 + TODO: Package description + vanyabeat + Apache License 2.0 + + ament_cmake + + ament_cmake + rclcpp + std_msgs + can-utils + + ament_cmake + + diff --git a/ros2_environment/src/bldc_motor_control/src/motor_control_node.cpp b/ros2_environment/src/bldc_motor_control/src/motor_control_node.cpp new file mode 100644 index 0000000..028fcad --- /dev/null +++ b/ros2_environment/src/bldc_motor_control/src/motor_control_node.cpp @@ -0,0 +1,93 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using std::placeholders::_1; + +class BLDCMotorControlNode : public rclcpp::Node +{ +public: + BLDCMotorControlNode() : Node("bldc_motor_control_node") + { + this->declare_parameter("can_interface", "can0"); + + this->get_parameter("can_interface", can_interface_); + + if ((can_socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + RCLCPP_ERROR(this->get_logger(), "Failed to create CAN socket"); + throw std::runtime_error("Failed to create CAN socket"); + } + + struct ifreq ifr; + strncpy(ifr.ifr_name, can_interface_.c_str(), IFNAMSIZ - 1); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; + if (ioctl(can_socket_, SIOCGIFINDEX, &ifr) < 0) { + RCLCPP_ERROR(this->get_logger(), "Failed to get interface index"); + throw std::runtime_error("Failed to get interface index"); + } + + struct sockaddr_can addr; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + if (bind(can_socket_, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + RCLCPP_ERROR(this->get_logger(), "Failed to bind CAN socket"); + throw std::runtime_error("Failed to bind CAN socket"); + } + + subscription_ = this->create_subscription( + "motor_angle", 10, std::bind(&BLDCMotorControlNode::listener_callback, this, _1)); + } + + ~BLDCMotorControlNode() + { + close(can_socket_); + } + +private: + void listener_callback(const std_msgs::msg::Float32::SharedPtr msg) + { + float angle = msg->data; + send_motor_command(angle); + } + + void send_motor_command(float angle) + { + struct can_frame frame; + frame.can_id = 0x2; // Replace with your desired CAN ID + std::stringstream ss; + // Serialize speed to string with 4 decimal places + ss << std::fixed << std::setprecision(3); + ss << angle; + std::string angle_string = ss.str(); + frame.can_dlc = angle_string.size(); // Data length code + std::copy(angle_string.begin(), angle_string.end(), frame.data); + // frame.data[speed_string.size() + 1] = '\0' +; // You'll need to serialize speed to fit CAN frame + + if (write(can_socket_, &frame, sizeof(frame)) != sizeof(frame)) { + RCLCPP_ERROR(this->get_logger(), "Failed to send CAN frame"); + } else { + RCLCPP_INFO(this->get_logger(), "Sent CAN frame: angle=%f rad", angle); + } + } + + std::string can_interface_; + int can_socket_; + rclcpp::Subscription::SharedPtr subscription_; +}; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/rbs_servo_hardware/CMakeLists.txt b/ros2_environment/src/rbs_servo_hardware/CMakeLists.txt similarity index 100% rename from rbs_servo_hardware/CMakeLists.txt rename to ros2_environment/src/rbs_servo_hardware/CMakeLists.txt diff --git a/rbs_servo_hardware/README.md b/ros2_environment/src/rbs_servo_hardware/README.md similarity index 100% rename from rbs_servo_hardware/README.md rename to ros2_environment/src/rbs_servo_hardware/README.md diff --git a/rbs_servo_hardware/include/rbs_servo_hardware/rbs_servo_actuator.hpp b/ros2_environment/src/rbs_servo_hardware/include/rbs_servo_hardware/rbs_servo_actuator.hpp similarity index 91% rename from rbs_servo_hardware/include/rbs_servo_hardware/rbs_servo_actuator.hpp rename to ros2_environment/src/rbs_servo_hardware/include/rbs_servo_hardware/rbs_servo_actuator.hpp index 9c00557..8bca87b 100644 --- a/rbs_servo_hardware/include/rbs_servo_hardware/rbs_servo_actuator.hpp +++ b/ros2_environment/src/rbs_servo_hardware/include/rbs_servo_hardware/rbs_servo_actuator.hpp @@ -56,8 +56,8 @@ public: private: // Parameters for the rbs_servo_hardware simulation - double hw_start_sec_; - double hw_stop_sec_; + // double hw_start_sec_; + // double hw_stop_sec_; // Store the command for the robot double hw_joint_command_; @@ -65,10 +65,13 @@ private: double hw_joint_state_; // Fake "mechanical connection" between actuator and sensor using sockets - struct sockaddr_in address_; - int socket_port_; - int sockoptval_ = 1; - int sock_; + // struct sockaddr_in address_; + // int socket_port_; + // int sockoptval_ = 1; + // int sock_; + + std::string can_interface_ = "can0"; + int can_socket_; }; } // namespace rbs_servo_hardware diff --git a/rbs_servo_hardware/include/rbs_servo_hardware/visibility_control.h b/ros2_environment/src/rbs_servo_hardware/include/rbs_servo_hardware/visibility_control.h similarity index 100% rename from rbs_servo_hardware/include/rbs_servo_hardware/visibility_control.h rename to ros2_environment/src/rbs_servo_hardware/include/rbs_servo_hardware/visibility_control.h diff --git a/rbs_servo_hardware/package.xml b/ros2_environment/src/rbs_servo_hardware/package.xml similarity index 100% rename from rbs_servo_hardware/package.xml rename to ros2_environment/src/rbs_servo_hardware/package.xml diff --git a/rbs_servo_hardware/rbs_servo_hardware.xml b/ros2_environment/src/rbs_servo_hardware/rbs_servo_hardware.xml similarity index 66% rename from rbs_servo_hardware/rbs_servo_hardware.xml rename to ros2_environment/src/rbs_servo_hardware/rbs_servo_hardware.xml index 850b89e..bbb8a75 100644 --- a/rbs_servo_hardware/rbs_servo_hardware.xml +++ b/ros2_environment/src/rbs_servo_hardware/rbs_servo_hardware.xml @@ -1,6 +1,6 @@ - Hardware interface for rbs_servo_hardware diff --git a/rbs_servo_hardware/src/rbs_servo_actuator.cpp b/ros2_environment/src/rbs_servo_hardware/src/rbs_servo_actuator.cpp similarity index 57% rename from rbs_servo_hardware/src/rbs_servo_actuator.cpp rename to ros2_environment/src/rbs_servo_hardware/src/rbs_servo_actuator.cpp index ed074cc..cb48ae7 100644 --- a/rbs_servo_hardware/src/rbs_servo_actuator.cpp +++ b/ros2_environment/src/rbs_servo_hardware/src/rbs_servo_actuator.cpp @@ -1,5 +1,5 @@ #include "rbs_servo_hardware/rbs_servo_actuator.hpp" -// Libs for fake connection +// Libs for fake connection // WARN: Delete it in the future #include // System libs @@ -9,10 +9,16 @@ #include #include // ROS specific libs -#include #include "hardware_interface/actuator_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +#include +#include namespace rbs_servo_hardware { hardware_interface::CallbackReturn @@ -21,16 +27,6 @@ RbsServoActuator::on_init(const hardware_interface::HardwareInfo &info) { hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; } - // START: This part here is for exemplary purposes - Please do not copy to - // your production code - hw_start_sec_ = std::stod( - info_.hardware_parameters["example_param_hw_start_duration_sec"]); - hw_stop_sec_ = std::stod( - info_.hardware_parameters["example_param_hw_stop_duration_sec"]); - socket_port_ = - std::stoi(info_.hardware_parameters["example_param_socket_port"]); - // END: This part here is for exemplary purposes - Please do not copy to your - // production code hw_joint_command_ = std::numeric_limits::quiet_NaN(); @@ -43,54 +39,67 @@ RbsServoActuator::on_init(const hardware_interface::HardwareInfo &info) { return hardware_interface::CallbackReturn::ERROR; } - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) { + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"), "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), - hardware_interface::HW_IF_VELOCITY); + hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } else if (joint.state_interfaces[0].name != - hardware_interface::HW_IF_VELOCITY) { + hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"), "Joint '%s' have %s state interface found. '%s' expected.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), - hardware_interface::HW_IF_VELOCITY); + hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } - // START: This part here is for exemplary purposes - Please do not copy to - // your production code Initialize objects for fake mechanical connection - sock_ = socket(AF_INET, SOCK_STREAM, 0); - if (sock_ < 0) { - RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"), - "Creating socket failed."); + if ((can_socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"), + "Failed to create CAN socket"); return hardware_interface::CallbackReturn::ERROR; } - auto server = gethostbyname("localhost"); - - address_.sin_family = AF_INET; - bcopy(reinterpret_cast(server->h_addr), - reinterpret_cast(&address_.sin_addr.s_addr), server->h_length); - address_.sin_port = htons(socket_port_); - - RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), - "Trying to connect to port %d.", socket_port_); - if (connect(sock_, (struct sockaddr *)&address_, sizeof(address_)) < 0) { - RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"), - "Connection over socket failed."); + struct ifreq ifr; + strncpy(ifr.ifr_name, can_interface_.c_str(), IFNAMSIZ - 1); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; + if (ioctl(can_socket_, SIOCGIFINDEX, &ifr) < 0) { + RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"), + "Failed to get interface index"); return hardware_interface::CallbackReturn::ERROR; } + + struct sockaddr_can addr; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + if (bind(can_socket_, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"), "Failed to bind CAN socket"); + return hardware_interface::CallbackReturn::ERROR; + } + + // auto server = gethostbyname("localhost"); + + // address_.sin_family = AF_INET; + // bcopy(reinterpret_cast(server->h_addr), + // reinterpret_cast(&address_.sin_addr.s_addr), + // server->h_length); + // address_.sin_port = htons(socket_port_); + + // RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), + // "Trying to connect to port %d.", socket_port_); + // if (connect(sock_, (struct sockaddr *)&address_, sizeof(address_)) < 0) { + // RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"), + // "Connection over socket failed."); + // return hardware_interface::CallbackReturn::ERROR; + // } RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Connected to socket"); - // END: This part here is for exemplary purposes - Please do not copy to your - // production code return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RbsServoActuator::on_shutdown( const rclcpp_lifecycle::State & /*previous_state*/) { - shutdown(sock_, SHUT_RDWR); // shutdown socket + // shutdown(sock_, SHUT_RDWR); // shutdown socket return hardware_interface::CallbackReturn::SUCCESS; } @@ -100,7 +109,7 @@ RbsServoActuator::export_state_interfaces() { std::vector state_interfaces; state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, + info_.joints[0].name, hardware_interface::HW_IF_POSITION, &hw_joint_state_)); return state_interfaces; @@ -111,7 +120,7 @@ RbsServoActuator::export_command_interfaces() { std::vector command_interfaces; command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, + info_.joints[0].name, hardware_interface::HW_IF_POSITION, &hw_joint_command_)); return command_interfaces; @@ -124,11 +133,12 @@ hardware_interface::CallbackReturn RbsServoActuator::on_activate( RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Activating ...please wait..."); - for (int i = 0; i < hw_start_sec_; i++) { - rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "%.1f seconds left...", - hw_start_sec_ - i); - } + // for (int i = 0; i < hw_start_sec_; i++) { + // rclcpp::sleep_for(std::chrono::seconds(1)); + // RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "%.1f seconds + // left...", + // hw_start_sec_ - i); + // } // END: This part here is for exemplary purposes - Please do not copy to your // production code @@ -151,48 +161,59 @@ hardware_interface::CallbackReturn RbsServoActuator::on_deactivate( RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Deactivating ...please wait..."); - for (int i = 0; i < hw_stop_sec_; i++) { - rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "%.1f seconds left...", - hw_stop_sec_ - i); - } + // for (int i = 0; i < hw_stop_sec_; i++) { + // rclcpp::sleep_for(std::chrono::seconds(1)); + // RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "%.1f seconds + // left...", + // hw_stop_sec_ - i); + // } RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Successfully deactivated!"); // END: This part here is for exemplary purposes - Please do not copy to your // production code - + close(can_socket_); return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::return_type RbsServoActuator::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + + struct can_frame frame; + if (::read(can_socket_, &frame, sizeof(frame)) != sizeof(frame)) { + RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"), "Failed to read CAN frame"); + } else { + float angle; + std::memcpy(&angle, &frame.data[1], sizeof(float)); + RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Received CAN frame: angle=%f rad", angle); + hw_joint_state_ = static_cast(angle); + } return hardware_interface::return_type::OK; - // TODO: Implementation of reading process - // see write() below for example of command - // interface } -hardware_interface::return_type -rbs_servo_hardware::RbsServoActuator::write(const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { +hardware_interface::return_type rbs_servo_hardware::RbsServoActuator::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // START: This part here is for exemplary purposes - Please do not copy to // your production code RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Writing command: %f", hw_joint_command_); - // Simulate sending commands to the hardware - std::ostringstream data; - data << hw_joint_command_; - RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), - "Sending data command: %s", data.str().c_str()); - send(sock_, data.str().c_str(), strlen(data.str().c_str()), 0); + struct can_frame frame; + frame.can_id = 0x2; // Replace with your desired CAN ID + + float angle = static_cast(hw_joint_command_); + frame.data[0] = 'A'; + std::memcpy(&frame.data[1], &angle, sizeof(float)); + frame.can_dlc = 5; + // frame.data[speed_string.size() + 1] = '\0' + ; // You'll need to serialize speed to fit CAN frame - RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), - "Joints successfully written!"); - // END: This part here is for exemplary purposes - Please do not copy to your - // production code + if (::write(can_socket_, &frame, sizeof(frame)) != sizeof(frame)) { + RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"), "Failed to send CAN frame"); + } else { + RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Sent CAN frame: angle=%f rad", angle); + } return hardware_interface::return_type::OK; } diff --git a/ros2_environment/src/rbs_servo_test/CMakeLists.txt b/ros2_environment/src/rbs_servo_test/CMakeLists.txt new file mode 100644 index 0000000..4e0ebb3 --- /dev/null +++ b/ros2_environment/src/rbs_servo_test/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.8) +project(rbs_servo_test) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + +# find dependencies +find_package(ament_cmake REQUIRED) + +install(DIRECTORY + launch + config + urdf +DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/ros2_environment/src/rbs_servo_test/LICENSE b/ros2_environment/src/rbs_servo_test/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/ros2_environment/src/rbs_servo_test/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/ros2_environment/src/rbs_servo_test/config/ros2_controllers.yaml b/ros2_environment/src/rbs_servo_test/config/ros2_controllers.yaml new file mode 100644 index 0000000..95b2950 --- /dev/null +++ b/ros2_environment/src/rbs_servo_test/config/ros2_controllers.yaml @@ -0,0 +1,16 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + +forward_position_controller: + ros__parameters: + joints: + - joint1 + interface_name: position diff --git a/ros2_environment/src/rbs_servo_test/launch/startup.launch.py b/ros2_environment/src/rbs_servo_test/launch/startup.launch.py new file mode 100644 index 0000000..ed6c9f7 --- /dev/null +++ b/ros2_environment/src/rbs_servo_test/launch/startup.launch.py @@ -0,0 +1,120 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler, OpaqueFunction +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node, node +from launch_ros.substitutions import FindPackageShare + + +def launch_setup(context, *args, **kwargs): + # Initialize Arguments + robot_controller = LaunchConfiguration("robot_controller") + gui = LaunchConfiguration("gui") + filepath = PathJoinSubstitution( + [ + FindPackageShare("rbs_servo_test"), + "urdf", + "simple_robot.urdf" + ] + ).perform(context) + + with open(filepath, "r") as file: + robot_description_content = file.read() + + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("rbs_servo_test"), + "config", + "ros2_controllers.yaml", + ] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("rbs_servo_test"), "config", "driver.rviz"] + ) + + control_node = Node( + package="controller_manager", + # prefix=['gdbserver localhost:1234'], + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + # arguments=["-d", rviz_config_file], + condition=IfCondition(gui), + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[robot_controller, "--controller-manager", "/controller_manager"], + ) + + # Delay rviz start after `joint_state_broadcaster` + delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[rviz_node], + ) + ) + + # Delay start of robot_controller after `joint_state_broadcaster` + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[robot_controller_spawner], + ) + ) + + nodes = [ + control_node, + robot_state_pub_node, + joint_state_broadcaster_spawner, + delay_rviz_after_joint_state_broadcaster_spawner, + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + ] + return nodes + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="false", + description="Start RViz2 automatically with this launch file.", + ) + + ) + + declared_arguments.append( + DeclareLaunchArgument( + "robot_controller", + default_value="forward_position_controller", + description=".......", + ) + ) + + + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/ros2_environment/src/rbs_servo_test/package.xml b/ros2_environment/src/rbs_servo_test/package.xml new file mode 100644 index 0000000..792deb4 --- /dev/null +++ b/ros2_environment/src/rbs_servo_test/package.xml @@ -0,0 +1,18 @@ + + + + rbs_servo_test + 0.0.0 + TODO: Package description + bill-finger + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros2_environment/src/rbs_servo_test/src/send_float64.py b/ros2_environment/src/rbs_servo_test/src/send_float64.py new file mode 100755 index 0000000..9ea4b5c --- /dev/null +++ b/ros2_environment/src/rbs_servo_test/src/send_float64.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension + + +class Float64Publisher(Node): + + def __init__(self): + super().__init__('float64_publisher') + self.publisher_ = self.create_publisher(Float64MultiArray, '/forward_position_controller/commands', 10) + # timer_period = 60000 # seconds + # self.timer = self.create_timer(timer_period, self.publish_float64_array) + self.get_logger().info('Float64Publisher node started.') + + def publish_float64_array(self): + msg = Float64MultiArray() + + # Specify layout + dim = MultiArrayDimension() + dim.label = 'example_dim' + dim.size = 10 + dim.stride = 10 + + layout = MultiArrayLayout() + layout.dim.append(dim) + layout.data_offset = 0 + + msg.layout = layout + + # Specify data + msg.data = [50.0] # Example float64 data + + self.publisher_.publish(msg) + self.get_logger().info('Publishing: "%s"' % msg.data) + + +def main(args=None): + rclpy.init(args=args) + publisher = Float64Publisher() + publisher.publish_float64_array(); + rclpy.spin_once(publisher) + publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ros2_environment/src/rbs_servo_test/urdf/simple_robot.urdf b/ros2_environment/src/rbs_servo_test/urdf/simple_robot.urdf new file mode 100644 index 0000000..5e9c1bc --- /dev/null +++ b/ros2_environment/src/rbs_servo_test/urdf/simple_robot.urdf @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + rbs_servo_hardware/RbsServoActuator + + + + + + + + + + + + + +