From 854abc1aec5d1730a141d579c7dd71d02e67c78c Mon Sep 17 00:00:00 2001 From: Yadu Date: Sat, 27 Jan 2024 13:23:14 +0800 Subject: [PATCH] Support Omnicore controllers (#56) * Generate robot controller description from ros2_control xacro Signed-off-by: Yadunund * Wrap description as string parameter Signed-off-by: Yadunund * Remove choices arg for launch parameters Signed-off-by: Yadunund * Cleanup moveit_config Signed-off-by: Yadunund * Remove newline Signed-off-by: Yadunund --------- Signed-off-by: Yadunund --- .pre-commit-config.yaml | 2 +- abb_bringup/launch/abb_control.launch.py | 17 ++- abb_bringup/launch/abb_moveit.launch.py | 4 - .../src/abb_hardware_interface.cpp | 101 +++++++++++++++--- .../urdf/irb1200.ros2_control.xacro | 19 ++-- .../urdf/irb1200_5_90.xacro | 5 +- 6 files changed, 118 insertions(+), 30 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 94efcca..b29531a 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -43,7 +43,7 @@ repos: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format-12 + entry: clang-format language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ["-fallback-style=none", "-i"] diff --git a/abb_bringup/launch/abb_control.launch.py b/abb_bringup/launch/abb_control.launch.py index e192122..4e28219 100644 --- a/abb_bringup/launch/abb_control.launch.py +++ b/abb_bringup/launch/abb_control.launch.py @@ -8,6 +8,7 @@ PathJoinSubstitution, ) from launch_ros.actions import Node +from launch_ros.descriptions import ParameterValue from launch_ros.substitutions import FindPackageShare @@ -83,6 +84,14 @@ def generate_launch_description(): Used only if 'use_fake_hardware' parameter is false.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "configure_via_rws", + default_value="true", + description="If false, the robot description will be generate from joint information \ + in the ros2_control xacro. Used only if 'use_fake_hardware' parameter is false.", + ) + ) declared_arguments.append( DeclareLaunchArgument( "fake_sensor_commands", @@ -115,6 +124,7 @@ def generate_launch_description(): fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") rws_ip = LaunchConfiguration("rws_ip") rws_port = LaunchConfiguration("rws_port") + configure_via_rws = LaunchConfiguration("configure_via_rws") initial_joint_controller = LaunchConfiguration("initial_joint_controller") launch_rviz = LaunchConfiguration("launch_rviz") @@ -141,9 +151,14 @@ def generate_launch_description(): "rws_port:=", rws_port, " ", + "configure_via_rws:=", + configure_via_rws, + " ", ] ) - robot_description = {"robot_description": robot_description_content} + robot_description = { + "robot_description": ParameterValue(robot_description_content, value_type=str) + } robot_controllers = PathJoinSubstitution( [FindPackageShare(runtime_config_package), "config", controllers_file] diff --git a/abb_bringup/launch/abb_moveit.launch.py b/abb_bringup/launch/abb_moveit.launch.py index 8cfb7d6..02325bb 100644 --- a/abb_bringup/launch/abb_moveit.launch.py +++ b/abb_bringup/launch/abb_moveit.launch.py @@ -148,28 +148,24 @@ def generate_launch_description(): DeclareLaunchArgument( "robot_xacro_file", description="Xacro describing the robot.", - choices=["irb1200_5_90.xacro"], ) ) declared_arguments.append( DeclareLaunchArgument( "support_package", description="Name of the support package", - choices=["abb_irb1200_support"], ) ) declared_arguments.append( DeclareLaunchArgument( "moveit_config_package", description="Name of the support package", - choices=["abb_irb1200_5_90_moveit_config"], ) ) declared_arguments.append( DeclareLaunchArgument( "moveit_config_file", description="Name of the SRDF file", - choices=["abb_irb1200_5_90.srdf.xacro"], ) ) diff --git a/abb_hardware_interface/src/abb_hardware_interface.cpp b/abb_hardware_interface/src/abb_hardware_interface.cpp index 43ef979..8644662 100644 --- a/abb_hardware_interface/src/abb_hardware_interface.cpp +++ b/abb_hardware_interface/src/abb_hardware_interface.cpp @@ -30,22 +30,7 @@ CallbackReturn ABBSystemHardware::on_init(const hardware_interface::HardwareInfo return CallbackReturn::ERROR; } - const auto rws_port = stoi(info_.hardware_parameters["rws_port"]); - const auto rws_ip = info_.hardware_parameters["rws_ip"]; - - if (rws_ip == "None") - { - RCLCPP_FATAL(LOGGER, "RWS IP not specified"); - return CallbackReturn::ERROR; - } - - // Get robot controller description from RWS - abb::robot::RWSManager rws_manager(rws_ip, rws_port, "Default User", "robotics"); - const auto robot_controller_description_ = - abb::robot::utilities::establishRWSConnection(rws_manager, "IRB1200", true); - RCLCPP_INFO_STREAM(LOGGER, "Robot controller description:\n" - << abb::robot::summaryText(robot_controller_description_)); - + // Validate interfaces configured in ros2_control xacro. for (const hardware_interface::ComponentInfo& joint : info_.joints) { if (joint.command_interfaces.size() != 2) @@ -91,6 +76,90 @@ CallbackReturn ABBSystemHardware::on_init(const hardware_interface::HardwareInfo } } + // By default, construct the robot_controller_description_ by connecting to RWS. + // If configure_via_rws is set to false, configure the robot_controller_description_ + // relying on joint information in the ros2_control xacro. + const auto configure_it = info_.hardware_parameters.find("configure_via_rws"); + const bool configure_via_rws = configure_it == info_.hardware_parameters.end() ? true : + configure_it->second == "false" || configure_it->second == "False" ? false : + true; + + if (configure_via_rws) + { + RCLCPP_INFO_STREAM(LOGGER, "Generating robot controller description from RWS."); + const auto rws_port = stoi(info_.hardware_parameters["rws_port"]); + const auto rws_ip = info_.hardware_parameters["rws_ip"]; + + if (rws_ip == "None") + { + RCLCPP_FATAL(LOGGER, "RWS IP not specified"); + return CallbackReturn::ERROR; + } + + // Get robot controller description from RWS + abb::robot::RWSManager rws_manager(rws_ip, rws_port, "Default User", "robotics"); + robot_controller_description_ = abb::robot::utilities::establishRWSConnection(rws_manager, "IRB1200", true); + } + else + { + RCLCPP_INFO_STREAM(LOGGER, "Generating robot controller description from HardwareInfo."); + + // Add header. + auto header{ robot_controller_description_.mutable_header() }; + // Omnicore controllers have RobotWare version >=7.0.0. + header->mutable_robot_ware_version()->set_major_number(7); + header->mutable_robot_ware_version()->set_minor_number(3); + header->mutable_robot_ware_version()->set_patch_number(2); + + // Add system indicators. + auto system_indicators{ robot_controller_description_.mutable_system_indicators() }; + system_indicators->mutable_options()->set_egm(true); + + // Add single mechanical units group. + auto mug{ robot_controller_description_.add_mechanical_units_groups() }; + mug->set_name(""); + + // Add single robot to mechanical units group. + auto robot{ mug->mutable_robot() }; + robot->set_type(abb::robot::MechanicalUnit_Type_TCP_ROBOT); + robot->set_axes_total(info_.joints.size()); + robot->set_mode(abb::robot::MechanicalUnit_Mode_ACTIVATED); + + // Add joints to robot. + for (std::size_t i = 0; i < info_.joints.size(); ++i) + { + const hardware_interface::ComponentInfo& joint = info_.joints[i]; + // We assume it's a revolute joint unless explicitly specified. + // Check if a "type" key is present in joint.parameters with value other than "revolute" + // as per sdformat conventions http://sdformat.org/spec?elem=joint. + const auto type_it = joint.parameters.find("type"); + const bool is_revolute = type_it != joint.parameters.end() && type_it->second != "revolute" ? false : true; + + // Get the range of the joint from its command interfaces. + for (const hardware_interface::InterfaceInfo& joint_info : joint.command_interfaces) + { + if (joint_info.name == hardware_interface::HW_IF_POSITION) + { + const double min = std::stod(joint_info.min); + const double max = std::stod(joint_info.max); + + abb::robot::StandardizedJoint* p_joint = robot->add_standardized_joints(); + p_joint->set_standardized_name(joint.name); + p_joint->set_rotating_move(is_revolute); + p_joint->set_lower_joint_bound(min); + p_joint->set_upper_joint_bound(max); + + RCLCPP_INFO(LOGGER, "Configured component %s of type %s with range [%.3f, %.3f]", joint.name.c_str(), + joint.type.c_str(), min, max); + break; + } + } + } + } + + RCLCPP_INFO_STREAM(LOGGER, "Robot controller description:\n" + << abb::robot::summaryText(robot_controller_description_)); + // Configure EGM RCLCPP_INFO(LOGGER, "Configuring EGM interface..."); diff --git a/robot_specific_config/abb_irb1200_support/urdf/irb1200.ros2_control.xacro b/robot_specific_config/abb_irb1200_support/urdf/irb1200.ros2_control.xacro index d9f9938..bee4767 100644 --- a/robot_specific_config/abb_irb1200_support/urdf/irb1200.ros2_control.xacro +++ b/robot_specific_config/abb_irb1200_support/urdf/irb1200.ros2_control.xacro @@ -1,7 +1,7 @@ - + @@ -13,6 +13,11 @@ abb_hardware_interface/ABBSystemHardware + + + + + ${configure_via_rws} ${rws_port} ${rws_ip} @@ -24,8 +29,8 @@ - {-2*pi} - {2*pi} + ${-2*pi} + ${2*pi} @@ -46,8 +51,8 @@ - {-2*pi} - {2*pi} + ${-2*pi} + ${2*pi} @@ -68,8 +73,8 @@ - {-2*pi} - {2*pi} + ${-2*pi} + ${2*pi} diff --git a/robot_specific_config/abb_irb1200_support/urdf/irb1200_5_90.xacro b/robot_specific_config/abb_irb1200_support/urdf/irb1200_5_90.xacro index c174a2d..4f11cb5 100644 --- a/robot_specific_config/abb_irb1200_support/urdf/irb1200_5_90.xacro +++ b/robot_specific_config/abb_irb1200_support/urdf/irb1200_5_90.xacro @@ -6,6 +6,8 @@ + + @@ -17,5 +19,6 @@ prefix="" use_fake_hardware="${use_fake_hardware}" rws_ip="${rws_ip}" - rws_port="${rws_port}"/> + rws_port="${rws_port}" + configure_via_rws="${configure_via_rws}"/>