Call SDK API directly in xarm_controller

1. [RobotHW] use xarm_ros_driver in xarm_controller 2. [parameter] `robot1_ip` => `robot_ip_1` 3. [parameter] `robot2_ip` => `robot_ip_2`
locusrobotics · Jun 15, 2022 · e5a1e3f · e5a1e3f
1 parent b148a99
commit e5a1e3f
Show file tree

Hide file tree

Showing 38 changed files with 671 additions and 446 deletions.
diff --git a/ReadMe.md b/ReadMe.md
@@ -172,14 +172,14 @@ __Reminder 3： All following instructions will base on xArm6，please use prope
 
         ```bash
         $ cd ~/dev_ws/
-        # 'robot1_ip': IP address of left arm
-        # 'robot2_ip': IP address of right arm
+        # 'robot_ip_1': IP address of left arm
+        # 'robot_ip_2': IP address of right arm
         # set 'add_gripper=true' to attach xArm gripper model
         # 'add_gripper_1': can separately decide whether to attach gripper for left arm，default for same value with 'add_gripper'
         # 'add_gripper_2': can separately decide whether to attach gripper for right arm，default for same value with 'add_gripper'
         # 'dof_1': can separately configure the model DOF of left arm，default to be the same DOF specified in filename.
         # 'dof_2': can separately configure the model DOF of right arm，default to be the same DOF specified in filename.
-        $ ros2 launch xarm_moveit_config dual_xarm6_moveit_realmove.launch.py robot1_ip:=192.168.1.117 robot2_ip:=192.168.1.203 [add_gripper:=true]
+        $ ros2 launch xarm_moveit_config dual_xarm6_moveit_realmove.launch.py robot_ip_1:=192.168.1.117 robot_ip_2:=192.168.1.203 [add_gripper:=true]
         ```
 
 - ### 5.7 xarm_planner

diff --git a/ReadMe_cn.md b/ReadMe_cn.md
@@ -167,14 +167,14 @@ __注意3： 以下启动说明以6轴为例，5轴和7轴的用法只需找到
 
         ```bash
         $ cd ~/dev_ws/
-        # robot1_ip表示左臂控制的IP地址
-        # robot2_ip表示右臂控制的IP地址
+        # robot_ip_1表示左臂控制的IP地址
+        # robot_ip_2表示右臂控制的IP地址
         # add_gripper为true时会加载xarm夹爪的模型
         # add_gripper_1参数可以单独指定左臂是否加载夹爪的模型，默认为add_gripper的值
         # add_gripper_2参数可以单独指定右臂是否加载夹爪的模型，默认为add_gripper的值
         # dof_1参数可以单独指定左臂轴数，默认为dof的值（这里为6，不同启动脚本不一样）
         # dof_2参数可以单独指定右臂轴数，默认为dof的值（这里为6，不同启动脚本不一样）
-        $ ros2 launch xarm_moveit_config dual_xarm6_moveit_realmove.launch.py robot1_ip:=192.168.1.117 robot2_ip:=192.168.1.203 [add_gripper:=true]
+        $ ros2 launch xarm_moveit_config dual_xarm6_moveit_realmove.launch.py robot_ip_1_1:=192.168.1.117 robot_ip_2:=192.168.1.203 [add_gripper:=true]
         ```
 
 - ### 5.7 xarm_planner

diff --git a/xarm_api/CMakeLists.txt b/xarm_api/CMakeLists.txt
@@ -3,7 +3,7 @@ project(xarm_api)
 
 # Default to C99
 if(NOT CMAKE_C_STANDARD)
-  set(CMAKE_C_STANDARD 14)
+  set(CMAKE_C_STANDARD 11)
 endif()
 
 # Default to C++14
@@ -100,6 +100,7 @@ add_executable(test_xarm_states
 )
 ament_target_dependencies(test_xarm_states ${dependencies})
 
+ament_export_libraries(xarm_ros_driver)
 ament_export_libraries(xarm_ros_client)
 ament_export_include_directories(include)
 

diff --git a/xarm_api/include/xarm_api/xarm_driver.h b/xarm_api/include/xarm_api/xarm_driver.h
@@ -32,6 +32,7 @@ namespace xarm_api
         void pub_cgpio_state(xarm_msgs::msg::CIOState &cio_msg);
 
         bool is_connected(void);
+        std::string controller_error_interpreter(int err=-1);
 
         rclcpp::Logger get_logger() { return node_->get_logger(); }
 

diff --git a/xarm_api/include/xarm_api/xarm_msgs.h b/xarm_api/include/xarm_api/xarm_msgs.h
@@ -41,75 +41,4 @@
 #include <xarm_msgs/srv/traj_play.hpp>
 #include <xarm_msgs/srv/vacuum_gripper_ctrl.hpp>
 
-inline std::string uf_controller_error_interpreter(int err)
-{
-    switch(err)
-    {
-        case 0:
-            return "Everything OK";
-        case 1:
-            return "Hardware Emergency STOP effective";
-        case 2:
-            return "Emergency IO of Control Box is triggered";
-        case 3:
-            return "Emergency Stop of Three-state Switch triggered";
-        case 11 ... 17:
-            return std::string("Servo Motor Error of Joint ") + std::to_string(err-10); 
-        case 19:
-            return "End Module Communication Error";
-        case 21:
-            return "Kinematic Error";
-        case 22:
-            return "Self-collision Error";
-        case 23:
-            return "Joint Angle Exceed Limit";
-        case 24:
-            return "Speed Exceeds Limit";
-        case 25:
-            return "Planning Error";
-        case 26:
-            return "System Real Time Error";
-        case 27:
-            return "Command Reply Error";
-        case 29:
-            return "Other Errors, please contact technical support";
-        case 30:
-            return "Feedback Speed Exceeds limit";
-        case 31:
-            return "Collision Caused Abnormal Joint Current";
-        case 32:
-            return "Circle Calculation Error";
-        case 33:
-            return "Controller GPIO Error";
-        case 34:
-            return "Trajectory Recording Timeout";
-        case 35:
-            return "Exceed Safety Boundary";
-        case 36:
-            return "Number of Delayed Command Exceed Limit";
-        case 37:
-            return "Abnormal Motion in Manual Mode";
-        case 38: 
-            return "Abnormal Joint Angle";
-        case 39:
-            return "Abnormal Communication Between Master and Slave IC of Power Board";
-        case 50:
-            return "Tool Force/Torque Sensor Error";
-        case 51:
-            return "Tool Force Torque Sensor Mode Setting Error";
-        case 52:
-            return "Tool Force Torque Sensor Zero Setting Error";
-        case 53:
-            return "Tool Force Torque Sensor Overload";
-        case 110:
-            return "Robot Arm Base Board Communication Error";
-        case 111:
-            return "Control Box External RS485 Device Communication Error";
-
-        default:
-            return "Abnormal Error Code, please contact support!";
-
-    }
-}
-
 #endif // __XARM_MSGS_H
diff --git a/xarm_api/launch/_robot_driver.launch.py b/xarm_api/launch/_robot_driver.launch.py
@@ -7,8 +7,6 @@
 # Author: Vinman <[email protected]> <[email protected]>
 
 import os
-import yaml
-from tempfile import NamedTemporaryFile
 from ament_index_python import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import OpaqueFunction, DeclareLaunchArgument, IncludeLaunchDescription
@@ -17,52 +15,7 @@
 from launch_ros.substitutions import FindPackageShare
 from launch import LaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-
-def merge_dict(dict1, dict2):
-    for k, v in dict1.items():
-        try:
-            if k not in dict2:
-                continue
-            if isinstance(v, dict):
-                merge_dict(v, dict2[k])
-            else:
-                dict1[k] = dict2[k]
-        except Exception as e:
-            pass
-
-
-def load_yaml(path):
-    if os.path.exists(path):
-        try:
-            with open(path, 'r') as file:
-                return yaml.safe_load(file)
-        except Exception as e:
-            print('load {} error, {}'.format(path, e))
-    return {}
-
-
-def generate_xarm_params(xarm_default_params_path, xarm_user_params_path=None, ros_namespace=''):
-    if not os.path.exists(xarm_user_params_path):
-        xarm_user_params_path = None
-    if ros_namespace or (xarm_user_params_path is not None and xarm_default_params_path != xarm_user_params_path):
-        ros2_control_params_yaml = load_yaml(xarm_default_params_path)
-        ros2_control_user_params_yaml = load_yaml(xarm_user_params_path)
-        # change xarm_driver to ufactory_driver
-        if 'xarm_driver' in ros2_control_params_yaml and 'ufactory_driver' not in ros2_control_params_yaml:
-            ros2_control_params_yaml['ufactory_driver'] = ros2_control_params_yaml.pop('xarm_driver')
-        if 'xarm_driver' in ros2_control_user_params_yaml and 'ufactory_driver' not in ros2_control_user_params_yaml:
-            ros2_control_user_params_yaml['ufactory_driver'] = ros2_control_user_params_yaml.pop('xarm_driver')
-        merge_dict(ros2_control_params_yaml, ros2_control_user_params_yaml)
-        if ros_namespace:
-            xarm_params_yaml = {
-                ros_namespace: ros2_control_params_yaml
-            }
-        else:
-            xarm_params_yaml = ros2_control_params_yaml
-        with NamedTemporaryFile(mode='w', prefix='launch_params_', delete=False) as h:
-            yaml.dump(xarm_params_yaml, h, default_flow_style=False)
-            return h.name
-    return xarm_default_params_path
+from launch.launch_description_sources import load_python_launch_file_as_module
 
 
 def launch_setup(context, *args, **kwargs):
@@ -110,10 +63,12 @@ def launch_setup(context, *args, **kwargs):
     show_rviz = LaunchConfiguration('show_rviz', default=False)
     robot_type = LaunchConfiguration('robot_type', default='xarm')
 
-    xarm_params = generate_xarm_params(
+    mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_api'), 'launch', 'lib', 'robot_api_lib.py'))
+    generate_robot_api_params = getattr(mod, 'generate_robot_api_params')
+    robot_params = generate_robot_api_params(
         os.path.join(get_package_share_directory('xarm_api'), 'config', 'xarm_params.yaml'),
         os.path.join(get_package_share_directory('xarm_api'), 'config', 'xarm_user_params.yaml'),
-        LaunchConfiguration('ros_namespace', default='').perform(context)
+        LaunchConfiguration('ros_namespace', default='').perform(context), node_name='ufactory_driver'
     )
 
     # robot driver node
@@ -125,13 +80,13 @@ def launch_setup(context, *args, **kwargs):
         output='screen',
         emulate_tty=True,
         parameters=[
-            xarm_params,
+            robot_params,
             {
                 'robot_ip': robot_ip,
                 'report_type': report_type,
                 'dof': dof,
                 'add_gripper': add_gripper if robot_type.perform(context) == 'xarm' else False,
-                'hw_ns': hw_ns.perform(context).strip('/'),
+                'hw_ns': '{}{}'.format(prefix.perform(context).strip('/'), hw_ns.perform(context).strip('/')),
                 'prefix': prefix.perform(context).strip('/'),
                 'baud_checkset': baud_checkset,
                 'default_gripper_baud': default_gripper_baud,

diff --git a/xarm_api/launch/lib/robot_api_lib.py b/xarm_api/launch/lib/robot_api_lib.py
@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2022, UFACTORY, Inc.
+# All rights reserved.
+#
+# Author: Vinman <[email protected]> <[email protected]>
+
+import os
+import yaml
+from tempfile import NamedTemporaryFile
+
+
+def merge_dict(dict1, dict2):
+    for k, v in dict1.items():
+        try:
+            if k not in dict2:
+                continue
+            if isinstance(v, dict):
+                merge_dict(v, dict2[k])
+            else:
+                dict1[k] = dict2[k]
+        except Exception as e:
+            pass
+
+
+def load_yaml(path):
+    if os.path.exists(path):
+        try:
+            with open(path, 'r') as file:
+                return yaml.safe_load(file)
+        except Exception as e:
+            print('load {} error, {}'.format(path, e))
+    return {}
+
+
+def generate_robot_api_params(robot_default_params_path, robot_user_params_path=None, ros_namespace='', node_name='ufactory_driver'):
+    if not os.path.exists(robot_user_params_path):
+        robot_user_params_path = None
+    if ros_namespace or (robot_user_params_path is not None and robot_default_params_path != robot_user_params_path):
+        ros2_control_params_yaml = load_yaml(robot_default_params_path)
+        ros2_control_user_params_yaml = load_yaml(robot_user_params_path)
+        # change xarm_driver to ufactory_driver
+        if 'xarm_driver' in ros2_control_params_yaml and node_name not in ros2_control_params_yaml:
+            ros2_control_params_yaml[node_name] = ros2_control_params_yaml.pop('xarm_driver')
+        if 'xarm_driver' in ros2_control_user_params_yaml and node_name not in ros2_control_user_params_yaml:
+            ros2_control_user_params_yaml[node_name] = ros2_control_user_params_yaml.pop('xarm_driver')
+        merge_dict(ros2_control_params_yaml, ros2_control_user_params_yaml)
+        if ros_namespace:
+            xarm_params_yaml = {
+                ros_namespace: ros2_control_params_yaml
+            }
+        else:
+            xarm_params_yaml = ros2_control_params_yaml
+        with NamedTemporaryFile(mode='w', prefix='launch_params_', delete=False) as h:
+            yaml.dump(xarm_params_yaml, h, default_flow_style=False)
+            return h.name
+    return robot_default_params_path
+