diff --git a/src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt b/src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt index e6d5942..310a3b6 100644 --- a/src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt +++ b/src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(realtime_tools REQUIRED) include_directories(include) -add_library(${PROJECT_NAME} +add_library(robotiq_2f_controllers SHARED src/robotiq_forward_controller.cpp ) ament_target_dependencies(${PROJECT_NAME} diff --git a/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_forward_controller.hpp b/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_forward_controller.hpp index 4c04aaa..858dddf 100644 --- a/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_forward_controller.hpp +++ b/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_forward_controller.hpp @@ -68,6 +68,7 @@ public: protected: std::string joint_name_; std::vector command_interface_types_; + std::vector state_interface_types_; // communication interface bool new_msg_ = false; diff --git a/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp b/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp index 1ea1258..9ddc007 100644 --- a/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp +++ b/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp @@ -11,23 +11,43 @@ controller_interface::CallbackReturn RobotiqForwardController::on_init() { joint_name_ = auto_declare("joint", joint_name_); command_interface_types_ = - auto_declare>("command_interfaces", command_interface_types_); + auto_declare>("command_interfaces", command_interface_types_); + state_interface_types_ = + auto_declare>("state_interfaces", state_interface_types_); + + + if (joint_name_.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "Missing 'joint_name' parameter."); + return CallbackReturn::ERROR; + } + else{ + RCLCPP_INFO(get_node()->get_logger(), "Joint name: %s.", joint_name_.c_str()); + } return CallbackReturn::SUCCESS; } -controller_interface::InterfaceConfiguration RobotiqForwardController::command_interface_configuration() - const +controller_interface::InterfaceConfiguration RobotiqForwardController::command_interface_configuration() const { - controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}}; + RCLCPP_DEBUG(get_node()->get_logger(), "Starting configuration of command interfaces."); - conf.names.reserve(command_interface_types_.size()); - for (const auto & interface_type : command_interface_types_) - { - conf.names.push_back(joint_name_ + "/" + interface_type); + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + for (const auto& interface_map_entry : command_interface_map_) { + const std::string& interface_type = interface_map_entry.first; + config.names.push_back(joint_name_ + "/" + interface_type); } + RCLCPP_DEBUG(get_node()->get_logger(), "Completed configuration of command interfaces."); - return conf; + return config; +} + + +controller_interface::InterfaceConfiguration RobotiqForwardController::state_interface_configuration() const { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::NONE; // Or whatever is appropriate + return config; } controller_interface::CallbackReturn RobotiqForwardController::on_configure(const rclcpp_lifecycle::State &) @@ -47,18 +67,23 @@ controller_interface::CallbackReturn RobotiqForwardController::on_configure(cons controller_interface::CallbackReturn RobotiqForwardController::on_activate(const rclcpp_lifecycle::State &) { - // clear out vectors in case of restart - joint_position_command_interface_.clear(); - max_velocity_command_interface_.clear(); - max_effort_command_interface_.clear(); + RCLCPP_INFO(get_node()->get_logger(), "Available command interfaces:"); + for (const auto& interface : command_interfaces_) { + RCLCPP_INFO(get_node()->get_logger(), "Interface Name: %s, Interface Type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str()); + } - // assign command interfaces - for (auto & interface : command_interfaces_) - { - command_interface_map_[interface.get_interface_name()]->push_back(interface); - } + // clear out vectors in case of restart + joint_position_command_interface_.clear(); + max_velocity_command_interface_.clear(); + max_effort_command_interface_.clear(); - return CallbackReturn::SUCCESS; + // assign command interfaces + for (auto & interface : command_interfaces_) + { + command_interface_map_[interface.get_interface_name()]->push_back(interface); + } + + return CallbackReturn::SUCCESS; } controller_interface::return_type RobotiqForwardController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) @@ -71,6 +96,11 @@ controller_interface::return_type RobotiqForwardController::update(const rclcpp: desired_max_effort_ = command_msg_->max_effort; } // Set the hardware interface commands + if (joint_position_command_interface_.empty()) { + RCLCPP_FATAL(get_node()->get_logger(), "Command interface is not available."); + return controller_interface::return_type::ERROR; + } + joint_position_command_interface_[0].get().set_value(desired_position_); max_velocity_command_interface_[0].get().set_value(desired_max_velocity_); max_effort_command_interface_[0].get().set_value(desired_max_effort_); @@ -100,4 +130,8 @@ controller_interface::CallbackReturn RobotiqForwardController::on_shutdown(const { return CallbackReturn::SUCCESS; } -} \ No newline at end of file +} + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(robotiq_2f_controllers::RobotiqForwardController, controller_interface::ControllerInterface) + diff --git a/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller_old.cpp b/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller_old.cpp deleted file mode 100644 index b86f395..0000000 --- a/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller_old.cpp +++ /dev/null @@ -1,82 +0,0 @@ -#include "robotiq_2f_controllers/robotiq_forward_controller.hpp" -#include - -namespace robotiq_2f_controllers -{ -RobotiqForwardController::RobotiqForwardController() -{ - -} - -controller_interface::InterfaceConfiguration RobotiqForwardController::command_interface_configuration() const -{ - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - config.names.emplace_back(joint_name_ + hardware_interface::HW_IF_POSITION); - config.names.emplace_back(joint_name_ + "/set_gripper_max_effort"); - config.names.emplace_back(joint_name_ + "/set_gripper_max_velocity"); - return config; -} - -controller_interface::InterfaceConfiguration RobotiqForwardController::state_interface_configuration() const -{ - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::NONE; - return config; -} - -void RobotiqForwardController::command_callback(const robotiq_2f_msgs::msg::ForwardCommand::SharedPtr msg) -{ - desired_position_ = msg->position; - desired_max_velocity_ = msg->max_velocity; - desired_max_effort_ = msg->max_effort; -} - -controller_interface::return_type RobotiqForwardController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) -{ - // Set the hardware interface commands - position_command_.set_value(desired_position_); - max_effort_command_.set_value(desired_max_effort_); - max_velocity_command_.set_value(desired_max_velocity_); - - return controller_interface::return_type::OK; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqForwardController::on_init() -{ - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqForwardController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) -{ - joint_name_ = get_node()->declare_parameter("joint_name", "finger_joint"); - - auto command_interfaces = get_node()->get_node_base_interface(); - auto position_handle = robot_hardware.get_handle(joint_name_ + hardware_interface::HW_IF_POSITION); - - - position_command_ = hardware_interface::LoanedCommandInterface(command_interfaces_[0].); - max_effort_command_ = hardware_interface::LoanedCommandInterface(command_interfaces_[0].); - max_velocity_command_ = hardware_interface::LoanedCommandInterface(command_interfaces_(joint_name_ + "/set_gripper_max_velocity")); - - - command_subscriber_ = get_node()->create_subscription( - "~/command", 10, std::bind(&RobotiqForwardController::command_callback, this, std::placeholders::_1)); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqForwardController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) -{ - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqForwardController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) -{ - for (size_t i = 0; i < command_interfaces_.size(); ++i) { - command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); - } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} -} // namespace robotiq_2f_controllers - -PLUGINLIB_EXPORT_CLASS(robotiq_2f_controllers::RobotiqForwardController, controller_interface::ControllerInterface) diff --git a/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml b/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml index b38c13c..987a43d 100644 --- a/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml +++ b/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml @@ -1,7 +1,7 @@ robotiq_2f_gripper: # Physical limits - min_position: 0.0 # Meters (fully closed) - max_position: 0.14 # Meters (fully open) + min_position: 0.0 # radiant (fully open) + max_position: 0.695 # radiant (fully open) min_speed: 0.02 # Meters per second max_speed: 0.15 # Meters per second min_force: 20.0 # Newtons diff --git a/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml b/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml index 0e58ad7..62b551f 100644 --- a/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml +++ b/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml @@ -8,7 +8,7 @@ controller_manager: fake_gripper_controller: type: joint_trajectory_controller/JointTrajectoryController forward_gripper_position_controller: - type: position_controllers/JointGroupPositionController + type: robotiq_2f_controllers/RobotiqForwardController robotiq_activation_controller: type: robotiq_controllers/RobotiqActivationController @@ -21,8 +21,7 @@ robotiq_gripper_controller: forward_gripper_position_controller: ros__parameters: - joints: - - finger_joint + joint: finger_joint fake_gripper_controller: ros__parameters: diff --git a/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py b/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py index d685726..4a11ebd 100644 --- a/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py +++ b/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py @@ -122,9 +122,9 @@ def generate_launch_description(): controller_spawner_inactive_names = ["forward_gripper_position_controller", "fake_gripper_controller"] else: - controller_spawner_names = ["fake_gripper_controller"] - controller_spawner_inactive_names = ["forward_gripper_position_controller", - "robotiq_gripper_controller"] + controller_spawner_names = ["forward_gripper_position_controller"] + controller_spawner_inactive_names = ["robotiq_gripper_controller", + "fake_gripper_controller"] controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [ controller_spawner(name, active=False) for name in controller_spawner_inactive_names diff --git a/src/robotiq_2f/robotiq_2f_description/package.xml b/src/robotiq_2f/robotiq_2f_description/package.xml index 59e3d99..565f15f 100644 --- a/src/robotiq_2f/robotiq_2f_description/package.xml +++ b/src/robotiq_2f/robotiq_2f_description/package.xml @@ -17,9 +17,12 @@ urdf xacro + robotiq_2f_msgs + ros2_control ros2_controllers + robotiq_2f_controllers gripper_controllers joint_state_broadcaster diff --git a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp index d1d6944..048a871 100644 --- a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp +++ b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp @@ -311,9 +311,9 @@ std::vector RobotiqGripperHardwareInterfac gripper_force_ = gripper_force_multiplier; command_interfaces.emplace_back( - hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_velocity", &gripper_speed_)); + hardware_interface::CommandInterface(info_.joints[0].name, "max_velocity", &gripper_speed_)); command_interfaces.emplace_back( - hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_effort", &gripper_force_)); + hardware_interface::CommandInterface(info_.joints[0].name, "max_effort", &gripper_force_)); command_interfaces.emplace_back( hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_)); command_interfaces.emplace_back(