Support Omnicore controllers (#56)

* Generate robot controller description from ros2_control xacro Signed-off-by: Yadunund <[email protected]> * Wrap description as string parameter Signed-off-by: Yadunund <[email protected]> * Remove choices arg for launch parameters Signed-off-by: Yadunund <[email protected]> * Cleanup moveit_config Signed-off-by: Yadunund <[email protected]> * Remove newline Signed-off-by: Yadunund <[email protected]> --------- Signed-off-by: Yadunund <[email protected]>
PickNikRobotics · Jan 27, 2024 · 854abc1 · 854abc1
1 parent 6935658
commit 854abc1
Show file tree

Hide file tree

Showing 6 changed files with 118 additions and 30 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:macro name="abb_ros2_control" params="name prefix use_fake_hardware:=true rws_ip rws_port">
+  <xacro:macro name="abb_ros2_control" params="name prefix use_fake_hardware:=true rws_ip rws_port configure_via_rws:=true">
     <ros2_control name="${name}" type="system">
       <hardware>
         <!-- ros2_control simulation -->
@@ -13,6 +13,11 @@
         <!-- physical hardware or RobotStudio simulation -->
         <xacro:unless value="${use_fake_hardware}">
           <plugin>abb_hardware_interface/ABBSystemHardware</plugin>
+          <!-- If false, the robot description required for EGM connection-->
+          <!-- will be generated as per joint information listed below. -->
+          <!-- If true, the robot description will be generated by retrieving -->
+          <!-- information from the ABB controller via RWS. -->
+          <param name="configure_via_rws">${configure_via_rws}</param>
           <param name="rws_port">${rws_port}</param>
           <param name="rws_ip">${rws_ip}</param>
           <!-- The following parameter is used for non-MultiMove only -->
@@ -24,8 +29,8 @@
       </hardware>
       <joint name="${prefix}joint_1">
         <command_interface name="position">
-          <param name="min">{-2*pi}</param>
-          <param name="max">{2*pi}</param>
+          <param name="min">${-2*pi}</param>
+          <param name="max">${2*pi}</param>
         </command_interface>
         <command_interface name="velocity"/>
         <state_interface name="position">
@@ -46,8 +51,8 @@
       </joint>
       <joint name="${prefix}joint_3">
         <command_interface name="position">
-          <param name="min">{-2*pi}</param>
-          <param name="max">{2*pi}</param>
+          <param name="min">${-2*pi}</param>
+          <param name="max">${2*pi}</param>
         </command_interface>
         <command_interface name="velocity"/>
         <state_interface name="position">
@@ -68,8 +73,8 @@
       </joint>
       <joint name="${prefix}joint_5">
         <command_interface name="position">
-          <param name="min">{-2*pi}</param>
-          <param name="max">{2*pi}</param>
+          <param name="min">${-2*pi}</param>
+          <param name="max">${2*pi}</param>
         </command_interface>
         <command_interface name="velocity"/>
         <state_interface name="position">

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
@@ -6,6 +6,8 @@
   <xacro:property name="rws_ip" value="$(arg rws_ip)"/>
   <xacro:arg name="rws_port" default="80"/>
   <xacro:property name="rws_port" value="$(arg rws_port)"/>
+  <xacro:arg name="configure_via_rws" default="true"/>
+  <xacro:property name="configure_via_rws" value="$(arg configure_via_rws)"/>
   <!-- Robot description -->
   <xacro:include filename="$(find abb_irb1200_support)/urdf/irb1200_5_90_macro.xacro"/>
   <xacro:abb_irb1200_5_90 prefix=""/>
@@ -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}"/>
 </robot>