diff --git a/flexiv_bringup/config/rizon_controllers.yaml b/flexiv_bringup/config/rizon_controllers.yaml index 41ce947..e043ee8 100644 --- a/flexiv_bringup/config/rizon_controllers.yaml +++ b/flexiv_bringup/config/rizon_controllers.yaml @@ -5,6 +5,9 @@ controller_manager: rizon_arm_controller: type: joint_trajectory_controller/JointTrajectoryController + streaming_position_controller: + type: streaming_position_controller/StreamingPositionController + joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster @@ -35,3 +38,14 @@ rizon_arm_controller: constraints: stopped_velocity_tolerance: 0.01 goal_time: 0.0 + +streaming_position_controller: + ros__parameters: + joints: + - $(var robot_sn)_joint1 + - $(var robot_sn)_joint2 + - $(var robot_sn)_joint3 + - $(var robot_sn)_joint4 + - $(var robot_sn)_joint5 + - $(var robot_sn)_joint6 + - $(var robot_sn)_joint7 diff --git a/flexiv_bringup/launch/rizon.launch.py b/flexiv_bringup/launch/rizon.launch.py index 52b227d..fe4697e 100644 --- a/flexiv_bringup/launch/rizon.launch.py +++ b/flexiv_bringup/launch/rizon.launch.py @@ -111,7 +111,7 @@ def generate_launch_description(): DeclareLaunchArgument( robot_controller_param_name, default_value="rizon_arm_controller", - description="Robot controller to start. Available: rizon_arm_controller", + description="Robot controller to start. Available: rizon_arm_controller, streaming_position_controller", ) ) diff --git a/flexiv_controllers/streaming_position_controller/CMakeLists.txt b/flexiv_controllers/streaming_position_controller/CMakeLists.txt new file mode 100644 index 0000000..558fae2 --- /dev/null +++ b/flexiv_controllers/streaming_position_controller/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.5) +project(streaming_position_controller LANGUAGES CXX) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(sensor_msgs REQUIRED) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + sensor_msgs +) + +add_library(${PROJECT_NAME} SHARED + src/streaming_position_controller.cpp +) + +target_include_directories(${PROJECT_NAME} PRIVATE include) + +ament_target_dependencies(${PROJECT_NAME} PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +pluginlib_export_plugin_description_file(controller_interface streaming_position_controller.xml) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + +ament_package() diff --git a/flexiv_controllers/streaming_position_controller/include/streaming_position_controller/streaming_position_controller.hpp b/flexiv_controllers/streaming_position_controller/include/streaming_position_controller/streaming_position_controller.hpp new file mode 100644 index 0000000..5d1ff95 --- /dev/null +++ b/flexiv_controllers/streaming_position_controller/include/streaming_position_controller/streaming_position_controller.hpp @@ -0,0 +1,57 @@ +/** + * @file streaming_position_controller.hpp + * @brief Minimal ros2_control controller that accepts sensor_msgs/JointState + * on a topic and writes positions directly to the hardware command + * interface. + */ + +#ifndef STREAMING_POSITION_CONTROLLER__STREAMING_POSITION_CONTROLLER_HPP_ +#define STREAMING_POSITION_CONTROLLER__STREAMING_POSITION_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" +#include "sensor_msgs/msg/joint_state.hpp" + +namespace streaming_position_controller { + +class StreamingPositionController : public controller_interface::ControllerInterface +{ +public: + StreamingPositionController(); + ~StreamingPositionController() override = default; + + controller_interface::CallbackReturn on_init() override; + controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override; + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override; + controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + std::vector joint_names_; + std::vector command_interface_types_; + std::unordered_map name_to_index_; + + realtime_tools::RealtimeThreadSafeBox rt_command_; + sensor_msgs::msg::JointState last_command_; + + std::vector pos_state_idx_; + std::vector current_positions_; + bool positions_initialized_; + + rclcpp::Subscription::SharedPtr command_subscriber_; +}; + +} // namespace streaming_position_controller + +#endif // STREAMING_POSITION_CONTROLLER__STREAMING_POSITION_CONTROLLER_HPP_ diff --git a/flexiv_controllers/streaming_position_controller/package.xml b/flexiv_controllers/streaming_position_controller/package.xml new file mode 100644 index 0000000..8735033 --- /dev/null +++ b/flexiv_controllers/streaming_position_controller/package.xml @@ -0,0 +1,27 @@ + + + + streaming_position_controller + 1.0.0 + + Lightweight ros2_control controller that accepts sensor_msgs/JointState + on a topic and forwards joint positions to the hardware command interface + with configurable velocity clamping to prevent torque limit faults. + + Flexiv + Apache License 2.0 + + ament_cmake + + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + sensor_msgs + + + ament_cmake + + diff --git a/flexiv_controllers/streaming_position_controller/src/streaming_position_controller.cpp b/flexiv_controllers/streaming_position_controller/src/streaming_position_controller.cpp new file mode 100644 index 0000000..0e5bee4 --- /dev/null +++ b/flexiv_controllers/streaming_position_controller/src/streaming_position_controller.cpp @@ -0,0 +1,232 @@ +/** + * @file streaming_position_controller.cpp + * @brief Minimal ros2_control controller: subscribes to JointState on a topic + * and writes target positions directly to the hardware command interface. + * + * Parameters: + * joints (string[]) -- joint names to command + * + * Subscribes to: + * ~/joint_commands (sensor_msgs/JointState) + * + */ + +#include "streaming_position_controller/streaming_position_controller.hpp" + +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" + +namespace streaming_position_controller { + +StreamingPositionController::StreamingPositionController() +: controller_interface::ControllerInterface(), + positions_initialized_(false), + command_subscriber_(nullptr) +{ +} + +controller_interface::CallbackReturn StreamingPositionController::on_init() +{ + try + { + auto_declare>("joints", std::vector()); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "on_init failed: %s", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn StreamingPositionController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + joint_names_ = get_node()->get_parameter("joints").as_string_array(); + if (joint_names_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } + + command_interface_types_.clear(); + name_to_index_.clear(); + for (std::size_t i = 0; i < joint_names_.size(); ++i) + { + command_interface_types_.push_back( + joint_names_[i] + "/" + hardware_interface::HW_IF_POSITION); + name_to_index_[joint_names_[i]] = i; + } + + command_subscriber_ = get_node()->create_subscription( + "~/joint_commands", rclcpp::SystemDefaultsQoS(), + [this](const sensor_msgs::msg::JointState::SharedPtr msg) + { + if (msg->name.size() != msg->position.size()) + { + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *(get_node()->get_clock()), + 1000, "JointState name/position size mismatch -- dropping"); + return; + } + for (const auto & p : msg->position) + { + if (!std::isfinite(p)) + { + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *(get_node()->get_clock()), + 1000, "Non-finite position received -- dropping"); + return; + } + } + rt_command_.set(*msg); + }); + + RCLCPP_INFO(get_node()->get_logger(), + "Configured with %zu joints (direct passthrough, no smoothing)", + joint_names_.size()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +StreamingPositionController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration cfg; + cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cfg.names = command_interface_types_; + return cfg; +} + +controller_interface::InterfaceConfiguration +StreamingPositionController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration cfg; + cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & joint : joint_names_) + { + cfg.names.push_back(joint + "/" + hardware_interface::HW_IF_POSITION); + } + return cfg; +} + +controller_interface::CallbackReturn StreamingPositionController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + std::vector> ordered; + if (!controller_interface::get_ordered_interfaces( + command_interfaces_, command_interface_types_, std::string(""), ordered) || + ordered.size() != joint_names_.size()) + { + RCLCPP_ERROR(get_node()->get_logger(), + "Expected %zu command interfaces, got %zu", + joint_names_.size(), ordered.size()); + return controller_interface::CallbackReturn::ERROR; + } + + const std::size_t n = joint_names_.size(); + + pos_state_idx_.assign(n, SIZE_MAX); + for (std::size_t si = 0; si < state_interfaces_.size(); ++si) + { + const auto & iface = state_interfaces_[si]; + auto jt = name_to_index_.find(iface.get_prefix_name()); + if (jt == name_to_index_.end()) { continue; } + if (iface.get_interface_name() == hardware_interface::HW_IF_POSITION) + { + pos_state_idx_[jt->second] = si; + } + } + + for (std::size_t i = 0; i < n; ++i) + { + if (pos_state_idx_[i] == SIZE_MAX) + { + RCLCPP_ERROR(get_node()->get_logger(), + "Position state interface not found for joint '%s'", + joint_names_[i].c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + + current_positions_.resize(n); + for (std::size_t i = 0; i < n; ++i) + { + auto val_opt = state_interfaces_[pos_state_idx_[i]].get_optional(); + double val = val_opt.has_value() ? val_opt.value() : 0.0; + current_positions_[i] = std::isfinite(val) ? val : 0.0; + } + positions_initialized_ = true; + + for (std::size_t i = 0; i < n; ++i) + { + (void)command_interfaces_[i].set_value(current_positions_[i]); + } + + sensor_msgs::msg::JointState empty; + rt_command_.try_set(empty); + + RCLCPP_INFO(get_node()->get_logger(), "Activated -- holding current position"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn StreamingPositionController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + positions_initialized_ = false; + sensor_msgs::msg::JointState empty; + rt_command_.try_set(empty); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type StreamingPositionController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + if (!positions_initialized_) + { + return controller_interface::return_type::OK; + } + + auto cmd_opt = rt_command_.try_get(); + if (cmd_opt.has_value()) + { + last_command_ = cmd_opt.value(); + } + + if (last_command_.name.empty()) + { + for (std::size_t i = 0; i < command_interfaces_.size(); ++i) + { + (void)command_interfaces_[i].set_value(current_positions_[i]); + } + return controller_interface::return_type::OK; + } + + for (std::size_t j = 0; j < last_command_.name.size(); ++j) + { + auto it = name_to_index_.find(last_command_.name[j]); + if (it != name_to_index_.end() && j < last_command_.position.size()) + { + current_positions_[it->second] = last_command_.position[j]; + } + } + + for (std::size_t i = 0; i < command_interfaces_.size(); ++i) + { + (void)command_interfaces_[i].set_value(current_positions_[i]); + } + + return controller_interface::return_type::OK; +} + +} // namespace streaming_position_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + streaming_position_controller::StreamingPositionController, + controller_interface::ControllerInterface) diff --git a/flexiv_controllers/streaming_position_controller/streaming_position_controller.xml b/flexiv_controllers/streaming_position_controller/streaming_position_controller.xml new file mode 100644 index 0000000..e857141 --- /dev/null +++ b/flexiv_controllers/streaming_position_controller/streaming_position_controller.xml @@ -0,0 +1,10 @@ + + + + Accepts sensor_msgs/JointState on a topic and forwards joint positions + to the hardware position command interface with velocity clamping. + + +