From d9a7a8a8599559c72f12e45befa39f92d83c0eb2 Mon Sep 17 00:00:00 2001 From: Mun Seng <101093998+munseng-flexiv@users.noreply.github.com> Date: Tue, 18 Jul 2023 15:54:12 +0800 Subject: [PATCH] Release/Flexiv ROS 2 Humble 0.8 (#19) --- ...nary-build.yml => humble-binary-build.yml} | 36 +-- README.md | 69 +++-- .../config/sine_sweep_impedance_config.yaml | 9 +- flexiv_bringup/launch/rizon.launch.py | 121 +++++--- flexiv_bringup/launch/rizon_moveit.launch.py | 124 ++++---- .../external_tcp_wrench_broadcaster.hpp | 36 +-- .../joint_impedance_controller.hpp | 27 +- .../tcp_pose_state_broadcaster.hpp | 30 +- .../src/external_tcp_wrench_broadcaster.cpp | 95 +++--- .../src/joint_impedance_controller.cpp | 94 +++--- .../src/tcp_pose_state_broadcaster.cpp | 65 ++-- .../config/initial_positions.yaml | 15 +- ...rials.xacro => flexiv_arm.materials.xacro} | 4 - flexiv_description/urdf/rizon.urdf.xacro | 5 +- .../urdf/rizon10.ros2_control.xacro | 56 ++-- .../urdf/rizon4.ros2_control.xacro | 56 ++-- flexiv_description/urdf/rizon_macro.xacro | 26 +- flexiv_hardware/CMakeLists.txt | 3 + .../flexiv_hardware_interface.hpp | 34 +- flexiv_hardware/package.xml | 1 + .../src/flexiv_hardware_interface.cpp | 292 +++++++----------- flexiv_moveit_config/config/kinematics.yaml | 11 +- flexiv_moveit_config/package.xml | 2 +- .../publisher_joint_trajectory_controller.py | 14 +- .../sine_sweep_impedance_controller.py | 6 +- 25 files changed, 584 insertions(+), 647 deletions(-) rename .github/workflows/{foxy-binary-build.yml => humble-binary-build.yml} (57%) rename flexiv_description/urdf/{flexiv_arm_materials.xacro => flexiv_arm.materials.xacro} (87%) diff --git a/.github/workflows/foxy-binary-build.yml b/.github/workflows/humble-binary-build.yml similarity index 57% rename from .github/workflows/foxy-binary-build.yml rename to .github/workflows/humble-binary-build.yml index d95d90c..d2c6b9c 100644 --- a/.github/workflows/foxy-binary-build.yml +++ b/.github/workflows/humble-binary-build.yml @@ -1,25 +1,23 @@ -name: Foxy Binary Build +name: Humble Binary Build on: push: branches: - main - - foxy pull_request: branches: - main - - foxy jobs: - foxy_binary: - name: Foxy binary job - runs-on: ubuntu-20.04 + humble_binary: + name: Humble binary job + runs-on: ubuntu-22.04 container: - image: osrf/ros:foxy-desktop + image: osrf/ros:humble-desktop strategy: fail-fast: false env: - ROS_DISTRO: foxy + ROS_DISTRO: humble steps: - name: Checkout uses: actions/checkout@v3 @@ -27,17 +25,17 @@ jobs: run: | sudo apt-get update && sudo apt-get install -y \ libeigen3-dev \ - ros-foxy-xacro \ - ros-foxy-tinyxml2-vendor \ - ros-foxy-ros2-control \ - ros-foxy-realtime-tools \ - ros-foxy-control-toolbox \ - ros-foxy-moveit \ - ros-foxy-ros2-controllers \ - ros-foxy-test-msgs \ - ros-foxy-joint-state-publisher \ - ros-foxy-joint-state-publisher-gui \ - ros-foxy-robot-state-publisher + ros-humble-xacro \ + ros-humble-tinyxml2-vendor \ + ros-humble-ros2-control \ + ros-humble-realtime-tools \ + ros-humble-control-toolbox \ + ros-humble-moveit \ + ros-humble-ros2-controllers \ + ros-humble-test-msgs \ + ros-humble-joint-state-publisher \ + ros-humble-joint-state-publisher-gui \ + ros-humble-robot-state-publisher - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 diff --git a/README.md b/README.md index 8c06bd5..7655de6 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,8 @@ # Flexiv ROS 2 -[![Foxy Source Build](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/foxy-source-build.yml/badge.svg)](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/foxy-source-build.yml) -[![Foxy Binary Build](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/foxy-binary-build.yml/badge.svg?branch=foxy)](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/foxy-binary-build.yml) -[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) +[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) [![docs](https://img.shields.io/badge/docs-sphinx-yellow)](https://rdk.flexiv.com/manual/ros2_packages.html) -For ROS 2 users to easily work with [RDK](https://github.com/flexivrobotics/flexiv_rdk), the APIs of RDK are wrapped into ROS packages in `flexiv_ros2`. Key functionalities like real-time joint torque and position control are supported, and the integration with `ros2_control` framework and MoveIt 2 is also implemented. +For ROS 2 users to easily work with [RDK](https://github.com/flexivrobotics/flexiv_rdk), the APIs of RDK are wrapped into ROS packages in `flexiv_ros2`. Key functionalities like real-time joint torque and position control are supported, and the integration with `ros2_control` framework and MoveIt! 2 is also implemented. ## References @@ -12,15 +10,24 @@ For ROS 2 users to easily work with [RDK](https://github.com/flexivrobotics/flex ## Compatibility -| **Supported OS** | **Supported ROS 2 distribution** | -| ---------------------- | ---------------------------------------------------- | -| Ubuntu 20.04 | [Foxy Fitzroy](https://docs.ros.org/en/foxy/index.html) | +| **Supported OS** | **Supported ROS 2 distribution** | +| ---------------- | ------------------------------------------------------------- | +| Ubuntu 20.04 | [Foxy Fitzroy](https://docs.ros.org/en/foxy/index.html) | +| Ubuntu 22.04 | [Humble Hawksbill](https://docs.ros.org/en/humble/index.html) | + +### Release Status + +| **ROS 2 Distro** | Foxy | Humble | +| ------------------ | --------------------------------------------------------------- | ----------------------------------------------------- | +| **Branch** | [foxy](https://github.com/flexivrobotics/flexiv_ros2/tree/foxy) | [main](https://github.com/flexivrobotics/flexiv_ros2) | +| **Release Status** | [![Foxy Binary Build](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/foxy-binary-build.yml/badge.svg?branch=foxy)](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/foxy-binary-build.yml) | [![Humble Binary Build](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/humble-binary-build.yml/badge.svg?branch=main)](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/humble-binary-build.yml) | ## Getting Started -This project was developed for ROS 2 Foxy on Ubuntu 20.04. Other versions of Ubuntu and ROS 2 may work, but are not officially supported. +This project was developed for ROS 2 Foxy (Ubuntu 20.04) and Humble (Ubuntu 22.04). Other versions of Ubuntu and ROS 2 may work, but are not officially supported. + +1. Install [ROS 2 Humble via Debian Packages](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) -1. Install [ROS 2 Foxy via Debian Packages](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html) 2. Install `colcon` and additional ROS packages: ```bash @@ -28,19 +35,20 @@ This project was developed for ROS 2 Foxy on Ubuntu 20.04. Other versions of Ubu python3-colcon-common-extensions \ python3-rosdep2 \ libeigen3-dev \ - ros-foxy-xacro \ - ros-foxy-tinyxml2-vendor \ - ros-foxy-ros2-control \ - ros-foxy-realtime-tools \ - ros-foxy-control-toolbox \ - ros-foxy-moveit \ - ros-foxy-ros2-controllers \ - ros-foxy-test-msgs \ - ros-foxy-joint-state-publisher \ - ros-foxy-joint-state-publisher-gui \ - ros-foxy-robot-state-publisher \ - ros-foxy-rviz2 + ros-humble-xacro \ + ros-humble-tinyxml2-vendor \ + ros-humble-ros2-control \ + ros-humble-realtime-tools \ + ros-humble-control-toolbox \ + ros-humble-moveit \ + ros-humble-ros2-controllers \ + ros-humble-test-msgs \ + ros-humble-joint-state-publisher \ + ros-humble-joint-state-publisher-gui \ + ros-humble-robot-state-publisher \ + ros-humble-rviz2 ``` + 3. Setup workspace: ```bash @@ -50,18 +58,20 @@ This project was developed for ROS 2 Foxy on Ubuntu 20.04. Other versions of Ubu cd flexiv_ros2/ git submodule update --init --recursive ``` + 4. Install dependencies: ```bash cd ~/flexiv_ros2_ws rosdep update - rosdep install --from-paths src --ignore-src --rosdistro foxy -r -y + rosdep install --from-paths src --ignore-src --rosdistro humble -r -y ``` + 5. Build and source the workspace: ```bash cd ~/flexiv_ros2_ws - source /opt/ros/foxy/setup.bash + source /opt/ros/humble/setup.bash colcon build --symlink-install source install/setup.bash ``` @@ -69,7 +79,7 @@ This project was developed for ROS 2 Foxy on Ubuntu 20.04. Other versions of Ubu **NOTE**: Remember to source the setup file and the workspace whenever a new terminal is opened: ```bash -source /opt/ros/foxy/setup.bash +source /opt/ros/humble/setup.bash source ~/flexiv_ros2_ws/install/setup.bash ``` @@ -107,6 +117,7 @@ The main launch file to start the robot driver is the `rizon.launch.py` - it loa ```bash ros2 launch flexiv_bringup rizon.launch.py robot_ip:=dont-care local_ip:=dont-care use_fake_hardware:=true ``` + 2. Publish commands to controllers - To send the goal position to the controller by using the node from `flexiv_test_nodes`, start the following command in a new terminal: @@ -152,10 +163,10 @@ ros2 launch flexiv_bringup rizon_moveit.launch.py robot_ip:=dont-care local_ip:= The robot driver (`rizon.launch.py`) publishes the following feedback states to the respective ROS topics: -* `/joint_states`: Measured joint states of the robot: joint position, velocity and torque. [[`sensor_msgs/JointState.msg`](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/JointState.html)] -* `/external_wrench_in_tcp_broadcaster/external_wrench_in_tcp`: Estimated external wrench applied on TCP and expressed in TCP frame $^{TCP}F_{ext}$ in force $[N]$ and moment $[Nm]$. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)] -* `/external_wrench_in_base_broadcaster/external_wrench_in_base`: Estimated external wrench applied on TCP and expressed in base frame $^{0}F_{ext}$ in force $[N]$ and moment $[Nm]$. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)] -* `/force_torque_sensor_broadcaster/wrench`: Force-torque (FT) sensor raw reading in flange frame: $^{flange}F_{raw}$ in force $[N]$ and moment $[Nm]$. The value is 0 if no FT sensor is installed. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)] -* `/tcp_pose_broadcaster/tcp_pose`: Measured TCP pose expressed in base frame $^{0}T_{TCP}$ in position $[m]$ and quaternion. [[`geometry_msgs/PoseStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)] +- `/joint_states`: Measured joint states of the robot: joint position, velocity and torque. [[`sensor_msgs/JointState.msg`](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/JointState.html)] +- `/external_wrench_in_tcp_broadcaster/external_wrench_in_tcp`: Estimated external wrench applied on TCP and expressed in TCP frame $^{TCP}F_{ext}$ in force $[N]$ and moment $[Nm]$. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)] +- `/external_wrench_in_base_broadcaster/external_wrench_in_base`: Estimated external wrench applied on TCP and expressed in base frame $^{0}F_{ext}$ in force $[N]$ and moment $[Nm]$. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)] +- `/force_torque_sensor_broadcaster/wrench`: Force-torque (FT) sensor raw reading in flange frame: $^{flange}F_{raw}$ in force $[N]$ and moment $[Nm]$. The value is 0 if no FT sensor is installed. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)] +- `/tcp_pose_broadcaster/tcp_pose`: Measured TCP pose expressed in base frame $^{0}T_{TCP}$ in position $[m]$ and quaternion. [[`geometry_msgs/PoseStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)] **NOTE**: The topic names of the broadcasters are specified in `flexiv_bringup/config/rizon_controllers.yaml` diff --git a/flexiv_bringup/config/sine_sweep_impedance_config.yaml b/flexiv_bringup/config/sine_sweep_impedance_config.yaml index 6aa79bd..df5b4c3 100644 --- a/flexiv_bringup/config/sine_sweep_impedance_config.yaml +++ b/flexiv_bringup/config/sine_sweep_impedance_config.yaml @@ -3,11 +3,4 @@ sine_sweep_impedance_controller: controller_name: "joint_impedance_controller" wait_sec_between_publish: 0.001 - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - joint7 + speed_scaling: 1.0 diff --git a/flexiv_bringup/launch/rizon.launch.py b/flexiv_bringup/launch/rizon.launch.py index 567bc8d..8cfb98a 100644 --- a/flexiv_bringup/launch/rizon.launch.py +++ b/flexiv_bringup/launch/rizon.launch.py @@ -1,11 +1,10 @@ -import os - -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, Shutdown +from launch.actions import DeclareLaunchArgument, RegisterEventHandler from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare def generate_launch_description(): @@ -87,14 +86,14 @@ def generate_launch_description(): robot_controller = LaunchConfiguration("robot_controller") # Get URDF via xacro - flexiv_urdf_xacro = os.path.join( - get_package_share_directory("flexiv_description"), "urdf", "rizon.urdf.xacro" + flexiv_urdf_xacro = PathJoinSubstitution( + [FindPackageShare("flexiv_description"), "urdf", "rizon.urdf.xacro"] ) # Get URDF via xacro robot_description_content = Command( [ - FindExecutable(name="xacro"), + PathJoinSubstitution([FindExecutable(name="xacro")]), " ", flexiv_urdf_xacro, " ", @@ -119,9 +118,10 @@ def generate_launch_description(): ) robot_description = {"robot_description": robot_description_content} - # RViZ - rviz_base = os.path.join(get_package_share_directory("flexiv_description"), "rviz") - rviz_config_file = os.path.join(rviz_base, "view_rizon.rviz") + # RViZ + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("flexiv_description"), "rviz", "view_rizon.rviz"] + ) rviz_node = Node( package="rviz2", @@ -133,22 +133,16 @@ def generate_launch_description(): ) # Robot controllers - robot_controllers = [ - get_package_share_directory("flexiv_bringup"), - "/config/", - controllers_file, - ] + robot_controllers = PathJoinSubstitution( + [FindPackageShare("flexiv_bringup"), "config", controllers_file] + ) # Controller Manager ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[robot_description, robot_controllers], - output={ - "stdout": "screen", - "stderr": "screen", - }, - on_exit=Shutdown(), + output="both", ) # Robot state publisher @@ -163,32 +157,71 @@ def generate_launch_description(): # Run robot controller robot_controller_spawner = Node( package="controller_manager", - executable="spawner.py", - arguments=[robot_controller, "-c", "/controller_manager"], - ) - - # Load broadcasters - load_controllers = [] - for controller in [ - "joint_state_broadcaster", - "force_torque_sensor_broadcaster", - "external_wrench_in_base_broadcaster", - "external_wrench_in_tcp_broadcaster", - "tcp_pose_state_broadcaster", - ]: - load_controllers += [ - ExecuteProcess( - cmd=["ros2 run controller_manager spawner.py {}".format(controller)], - shell=True, - output="screen", - ) - ] + executable="spawner", + arguments=[robot_controller, "--controller-manager", "/controller_manager"], + ) + + # Run joint state broadcaster + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + # Run force torque sensor broadcaster + force_torque_sensor_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["force_torque_sensor_broadcaster", "--controller-manager", "/controller_manager"], + ) + + # Run external wrench in base broadcaster + external_wrench_in_base_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["external_wrench_in_base_broadcaster", "--controller-manager", "/controller_manager"], + ) + + # Run external wrench in tcp broadcaster + external_wrench_in_tcp_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["external_wrench_in_tcp_broadcaster", "--controller-manager", "/controller_manager"], + ) + + # Run tcp pose state broadcaster + tcp_pose_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["tcp_pose_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + # Delay rviz start after `joint_state_broadcaster` + delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[rviz_node], + ) + ) + + # Delay start of robot_controller after `joint_state_broadcaster` + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[robot_controller_spawner], + ) + ) nodes = [ ros2_control_node, robot_state_publisher_node, - rviz_node, - robot_controller_spawner, + joint_state_broadcaster_spawner, + force_torque_sensor_broadcaster_spawner, + external_wrench_in_base_broadcaster_spawner, + external_wrench_in_tcp_broadcaster_spawner, + tcp_pose_state_broadcaster_spawner, + delay_rviz_after_joint_state_broadcaster_spawner, + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, ] - return LaunchDescription(declared_arguments + nodes + load_controllers) + return LaunchDescription(declared_arguments + nodes) diff --git a/flexiv_bringup/launch/rizon_moveit.launch.py b/flexiv_bringup/launch/rizon_moveit.launch.py index 51a0e69..12e12c9 100644 --- a/flexiv_bringup/launch/rizon_moveit.launch.py +++ b/flexiv_bringup/launch/rizon_moveit.launch.py @@ -1,15 +1,13 @@ -# This launch file is based on -# https://github.com/ros-planning/moveit_resources/blob/galactic/panda_moveit_config/launch/demo.launch.py - import os - import yaml from ament_index_python.packages import get_package_share_directory + from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, Shutdown +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare def load_yaml(package_name, file_path): @@ -49,6 +47,14 @@ def generate_launch_description(): description="IP address of the workstation PC (local).", ) ) + + declared_arguments.append( + DeclareLaunchArgument( + "start_rviz", + default_value="true", + description="start RViz automatically with the launch file", + ) + ) declared_arguments.append( DeclareLaunchArgument( @@ -66,30 +72,31 @@ def generate_launch_description(): Used only if 'use_fake_hardware' parameter is true.", ) ) - + declared_arguments.append( DeclareLaunchArgument( - "db", - default_value="false", - description="Database flag", + "warehouse_sqlite_path", + default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"), + description="Path to the sqlite database used by the warehouse_ros package.", ) ) rizon_type = LaunchConfiguration("rizon_type") robot_ip = LaunchConfiguration("robot_ip") local_ip = LaunchConfiguration("local_ip") + start_rviz = LaunchConfiguration("start_rviz") use_fake_hardware = LaunchConfiguration("use_fake_hardware") fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") - db_config = LaunchConfiguration("db") + warehouse_sqlite_path = LaunchConfiguration("warehouse_sqlite_path") # Get URDF via xacro - flexiv_urdf_xacro = os.path.join( - get_package_share_directory("flexiv_description"), "urdf", "rizon.urdf.xacro" + flexiv_urdf_xacro = PathJoinSubstitution( + [FindPackageShare("flexiv_description"), "urdf", "rizon.urdf.xacro"] ) robot_description_content = Command( [ - FindExecutable(name="xacro"), + PathJoinSubstitution([FindExecutable(name="xacro")]), " ", flexiv_urdf_xacro, " ", @@ -115,12 +122,13 @@ def generate_launch_description(): robot_description = {"robot_description": robot_description_content} # MoveIt configuration - flexiv_srdf_xacro = os.path.join( - get_package_share_directory("flexiv_moveit_config"), "srdf", "rizon.srdf.xacro" + flexiv_srdf_xacro = PathJoinSubstitution( + [FindPackageShare("flexiv_moveit_config"), "srdf", "rizon.srdf.xacro"] ) + robot_description_semantic_content = Command( [ - FindExecutable(name="xacro"), + PathJoinSubstitution([FindExecutable(name="xacro")]), " ", flexiv_srdf_xacro, " ", @@ -133,7 +141,9 @@ def generate_launch_description(): "robot_description_semantic": robot_description_semantic_content } - kinematics_yaml = load_yaml("flexiv_moveit_config", "config/kinematics.yaml") + robot_description_kinematics = PathJoinSubstitution( + [FindPackageShare("flexiv_moveit_config"), "config", "kinematics.yaml"] + ) # Planning Configuration ompl_planning_pipeline_config = { @@ -180,6 +190,11 @@ def generate_launch_description(): ) } + warehouse_ros_config = { + "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection", + "warehouse_host": warehouse_sqlite_path, + } + # Start the actual move_group node/action server move_group_node = Node( package="moveit_ros_move_group", @@ -188,20 +203,20 @@ def generate_launch_description(): parameters=[ robot_description, robot_description_semantic, - kinematics_yaml, + robot_description_kinematics, ompl_planning_pipeline_config, trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, joint_limits_yaml, + warehouse_ros_config, ], ) # RViz with MoveIt configuration - rviz_base = os.path.join( - get_package_share_directory("flexiv_moveit_config"), "rviz" + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("flexiv_moveit_config"), "rviz", "moveit.rviz"] ) - rviz_config_file = os.path.join(rviz_base, "moveit.rviz") rviz_node = Node( package="rviz2", @@ -213,9 +228,11 @@ def generate_launch_description(): robot_description, robot_description_semantic, ompl_planning_pipeline_config, - kinematics_yaml, + robot_description_kinematics, joint_limits_yaml, + warehouse_ros_config, ], + condition=IfCondition(start_rviz), ) # Publish TF @@ -226,54 +243,41 @@ def generate_launch_description(): output="both", parameters=[robot_description], ) - - ros2_controllers_path = os.path.join( - get_package_share_directory("flexiv_bringup"), - "config", - "rizon_controllers.yaml", + + # Robot controllers + robot_controllers = PathJoinSubstitution( + [FindPackageShare("flexiv_bringup"), "config", "rizon_controllers.yaml"] ) + # Run controller manager ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, ros2_controllers_path], - output={ - "stdout": "screen", - "stderr": "screen", - }, - on_exit=Shutdown(), + parameters=[robot_description, robot_controllers], + output="both", ) - - # Load controllers - load_controllers = [] - for controller in ["rizon_arm_controller", "joint_state_broadcaster"]: - load_controllers += [ - ExecuteProcess( - cmd=["ros2 run controller_manager spawner.py {}".format(controller)], - shell=True, - output="screen", - ) - ] - - # Warehouse mongodb server - mongodb_server_node = Node( - package="warehouse_ros_mongo", - executable="mongo_wrapper_ros.py", - parameters=[ - {"warehouse_port": 33829}, - {"warehouse_host": "localhost"}, - {"warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection"}, - ], - output="screen", - condition=IfCondition(db_config), + + # Run robot controller + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["rizon_arm_controller", "--controller-manager", "/controller_manager"], + ) + + # Run joint state broadcaster + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) nodes = [ move_group_node, robot_state_publisher_node, - rviz_node, ros2_control_node, - mongodb_server_node, + joint_state_broadcaster_spawner, + robot_controller_spawner, + rviz_node, ] - return LaunchDescription(declared_arguments + nodes + load_controllers) + return LaunchDescription(declared_arguments + nodes) diff --git a/flexiv_controllers/include/flexiv_controllers/external_tcp_wrench_broadcaster.hpp b/flexiv_controllers/include/flexiv_controllers/external_tcp_wrench_broadcaster.hpp index 95e53d9..909f08c 100644 --- a/flexiv_controllers/include/flexiv_controllers/external_tcp_wrench_broadcaster.hpp +++ b/flexiv_controllers/include/flexiv_controllers/external_tcp_wrench_broadcaster.hpp @@ -21,34 +21,27 @@ #include "semantic_components/force_torque_sensor.hpp" namespace flexiv_controllers { -using CallbackReturn - = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -class ExternalTcpWrenchBroadcaster -: public controller_interface::ControllerInterface +class ExternalTcpWrenchBroadcaster : public controller_interface::ControllerInterface { public: ExternalTcpWrenchBroadcaster(); - controller_interface::InterfaceConfiguration - command_interface_configuration() const override; + controller_interface::InterfaceConfiguration command_interface_configuration() const override; - controller_interface::InterfaceConfiguration - state_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time& time, const rclcpp::Duration& period) override; - controller_interface::return_type init( - const std::string& controller_name) override; + CallbackReturn on_init() override; - CallbackReturn on_configure( - const rclcpp_lifecycle::State& previous_state) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; - CallbackReturn on_activate( - const rclcpp_lifecycle::State& previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; - CallbackReturn on_deactivate( - const rclcpp_lifecycle::State& previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; protected: std::string sensor_name_; @@ -56,13 +49,10 @@ class ExternalTcpWrenchBroadcaster std::string frame_id_; std::string topic_name_; - std::unique_ptr - force_torque_sensor_; + std::unique_ptr force_torque_sensor_; - using StatePublisher - = realtime_tools::RealtimePublisher; - rclcpp::Publisher::SharedPtr - sensor_state_publisher_; + using StatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr sensor_state_publisher_; std::unique_ptr realtime_publisher_; }; diff --git a/flexiv_controllers/include/flexiv_controllers/joint_impedance_controller.hpp b/flexiv_controllers/include/flexiv_controllers/joint_impedance_controller.hpp index 50d6489..72992af 100644 --- a/flexiv_controllers/include/flexiv_controllers/joint_impedance_controller.hpp +++ b/flexiv_controllers/include/flexiv_controllers/joint_impedance_controller.hpp @@ -24,34 +24,27 @@ namespace flexiv_controllers { using CmdType = flexiv_msgs::msg::JointPosVel; -using CallbackReturn - = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -class JointImpedanceController -: public controller_interface::ControllerInterface +class JointImpedanceController : public controller_interface::ControllerInterface { public: JointImpedanceController(); - controller_interface::InterfaceConfiguration - command_interface_configuration() const override; + controller_interface::InterfaceConfiguration command_interface_configuration() const override; - controller_interface::InterfaceConfiguration - state_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time& time, const rclcpp::Duration& period) override; - controller_interface::return_type init( - const std::string& controller_name) override; + CallbackReturn on_init() override; - CallbackReturn on_configure( - const rclcpp_lifecycle::State& previous_state) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; - CallbackReturn on_activate( - const rclcpp_lifecycle::State& previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; - CallbackReturn on_deactivate( - const rclcpp_lifecycle::State& previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; protected: std::vector joint_names_; diff --git a/flexiv_controllers/include/flexiv_controllers/tcp_pose_state_broadcaster.hpp b/flexiv_controllers/include/flexiv_controllers/tcp_pose_state_broadcaster.hpp index 736058e..422ef24 100644 --- a/flexiv_controllers/include/flexiv_controllers/tcp_pose_state_broadcaster.hpp +++ b/flexiv_controllers/include/flexiv_controllers/tcp_pose_state_broadcaster.hpp @@ -21,33 +21,27 @@ #include "flexiv_controllers/cartesian_pose_sensor.hpp" namespace flexiv_controllers { -using CallbackReturn - = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; class TcpPoseStateBroadcaster : public controller_interface::ControllerInterface { public: TcpPoseStateBroadcaster(); - controller_interface::InterfaceConfiguration - command_interface_configuration() const override; + controller_interface::InterfaceConfiguration command_interface_configuration() const override; - controller_interface::InterfaceConfiguration - state_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time& time, const rclcpp::Duration& period) override; - controller_interface::return_type init( - const std::string& controller_name) override; + CallbackReturn on_init() override; - CallbackReturn on_configure( - const rclcpp_lifecycle::State& previous_state) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; - CallbackReturn on_activate( - const rclcpp_lifecycle::State& previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; - CallbackReturn on_deactivate( - const rclcpp_lifecycle::State& previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; protected: std::string sensor_name_; @@ -56,10 +50,8 @@ class TcpPoseStateBroadcaster : public controller_interface::ControllerInterface std::unique_ptr cartesian_pose_sensor_; - using StatePublisher - = realtime_tools::RealtimePublisher; - rclcpp::Publisher::SharedPtr - sensor_state_publisher_; + using StatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr sensor_state_publisher_; std::unique_ptr realtime_publisher_; }; diff --git a/flexiv_controllers/src/external_tcp_wrench_broadcaster.cpp b/flexiv_controllers/src/external_tcp_wrench_broadcaster.cpp index 1401d62..433dc26 100644 --- a/flexiv_controllers/src/external_tcp_wrench_broadcaster.cpp +++ b/flexiv_controllers/src/external_tcp_wrench_broadcaster.cpp @@ -22,8 +22,7 @@ controller_interface::InterfaceConfiguration ExternalTcpWrenchBroadcaster::command_interface_configuration() const { controller_interface::InterfaceConfiguration command_interfaces_config; - command_interfaces_config.type - = controller_interface::interface_configuration_type::NONE; + command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; return command_interfaces_config; } @@ -31,21 +30,13 @@ controller_interface::InterfaceConfiguration ExternalTcpWrenchBroadcaster::state_interface_configuration() const { controller_interface::InterfaceConfiguration state_interfaces_config; - state_interfaces_config.type - = controller_interface::interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names - = force_torque_sensor_->get_state_interface_names(); + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = force_torque_sensor_->get_state_interface_names(); return state_interfaces_config; } -controller_interface::return_type ExternalTcpWrenchBroadcaster::init( - const std::string& controller_name) +CallbackReturn ExternalTcpWrenchBroadcaster::on_init() { - auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) { - return ret; - } - try { auto_declare("sensor_name", ""); auto_declare("interface_names.force.x", ""); @@ -57,36 +48,29 @@ controller_interface::return_type ExternalTcpWrenchBroadcaster::init( auto_declare("frame_id", ""); auto_declare("topic_name", ""); } catch (const std::exception& e) { - fprintf(stderr, - "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::return_type::ERROR; + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; } - return controller_interface::return_type::OK; + return CallbackReturn::SUCCESS; } CallbackReturn ExternalTcpWrenchBroadcaster::on_configure( const rclcpp_lifecycle::State& /*previous_state*/) { - sensor_name_ = node_->get_parameter("sensor_name").as_string(); - interface_names_[0] - = node_->get_parameter("interface_names.force.x").as_string(); - interface_names_[1] - = node_->get_parameter("interface_names.force.y").as_string(); - interface_names_[2] - = node_->get_parameter("interface_names.force.z").as_string(); - interface_names_[3] - = node_->get_parameter("interface_names.torque.x").as_string(); - interface_names_[4] - = node_->get_parameter("interface_names.torque.y").as_string(); - interface_names_[5] - = node_->get_parameter("interface_names.torque.z").as_string(); + sensor_name_ = get_node()->get_parameter("sensor_name").as_string(); + interface_names_[0] = get_node()->get_parameter("interface_names.force.x").as_string(); + interface_names_[1] = get_node()->get_parameter("interface_names.force.y").as_string(); + interface_names_[2] = get_node()->get_parameter("interface_names.force.z").as_string(); + interface_names_[3] = get_node()->get_parameter("interface_names.torque.x").as_string(); + interface_names_[4] = get_node()->get_parameter("interface_names.torque.y").as_string(); + interface_names_[5] = get_node()->get_parameter("interface_names.torque.z").as_string(); const bool no_interface_names_defined = std::count(interface_names_.begin(), interface_names_.end(), "") == 6; if (sensor_name_.empty() && no_interface_names_defined) { - RCLCPP_ERROR(node_->get_logger(), + RCLCPP_ERROR(get_node()->get_logger(), "'sensor_name' or at least one " "'interface_names.[force|torque].[x|y|z]' parameter has to be " "specified."); @@ -94,47 +78,40 @@ CallbackReturn ExternalTcpWrenchBroadcaster::on_configure( } if (!sensor_name_.empty() && !no_interface_names_defined) { - RCLCPP_ERROR(node_->get_logger(), + RCLCPP_ERROR(get_node()->get_logger(), "both 'sensor_name' and " "'interface_names.[force|torque].[x|y|z]' parameters can not be " "specified together."); return CallbackReturn::ERROR; } - frame_id_ = node_->get_parameter("frame_id").as_string(); + frame_id_ = get_node()->get_parameter("frame_id").as_string(); if (frame_id_.empty()) { - RCLCPP_ERROR( - node_->get_logger(), "'frame_id' parameter has to be provided."); + RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); return CallbackReturn::ERROR; } if (!sensor_name_.empty()) { - force_torque_sensor_ - = std::make_unique( - semantic_components::ForceTorqueSensor(sensor_name_)); + force_torque_sensor_ = std::make_unique( + semantic_components::ForceTorqueSensor(sensor_name_)); } else { - force_torque_sensor_ - = std::make_unique( - semantic_components::ForceTorqueSensor(interface_names_[0], - interface_names_[1], interface_names_[2], - interface_names_[3], interface_names_[4], - interface_names_[5])); + force_torque_sensor_ = std::make_unique( + semantic_components::ForceTorqueSensor(interface_names_[0], interface_names_[1], + interface_names_[2], interface_names_[3], interface_names_[4], + interface_names_[5])); } - topic_name_ = node_->get_parameter("topic_name").as_string(); + topic_name_ = get_node()->get_parameter("topic_name").as_string(); if (topic_name_.empty()) { - RCLCPP_ERROR( - node_->get_logger(), "'topic_name' parameter has to be provided."); + RCLCPP_ERROR(get_node()->get_logger(), "'topic_name' parameter has to be provided."); return CallbackReturn::ERROR; } try { // register ft sensor data publisher - sensor_state_publisher_ - = node_->create_publisher( - "~/" + topic_name_, rclcpp::SystemDefaultsQoS()); - realtime_publisher_ - = std::make_unique(sensor_state_publisher_); + sensor_state_publisher_ = get_node()->create_publisher( + "~/" + topic_name_, rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = std::make_unique(sensor_state_publisher_); } catch (const std::exception& e) { fprintf(stderr, "Exception thrown during publisher creation at configure stage " @@ -147,16 +124,16 @@ CallbackReturn ExternalTcpWrenchBroadcaster::on_configure( realtime_publisher_->msg_.header.frame_id = frame_id_; realtime_publisher_->unlock(); - RCLCPP_DEBUG(node_->get_logger(), "configure successful"); + RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } -controller_interface::return_type ExternalTcpWrenchBroadcaster::update() +controller_interface::return_type ExternalTcpWrenchBroadcaster::update( + const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { if (realtime_publisher_ && realtime_publisher_->trylock()) { - realtime_publisher_->msg_.header.stamp = node_->now(); - force_torque_sensor_->get_values_as_message( - realtime_publisher_->msg_.wrench); + realtime_publisher_->msg_.header.stamp = get_node()->now(); + force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench); realtime_publisher_->unlockAndPublish(); } @@ -181,5 +158,5 @@ CallbackReturn ExternalTcpWrenchBroadcaster::on_deactivate( #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(flexiv_controllers::ExternalTcpWrenchBroadcaster, - controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS( + flexiv_controllers::ExternalTcpWrenchBroadcaster, controller_interface::ControllerInterface) diff --git a/flexiv_controllers/src/joint_impedance_controller.cpp b/flexiv_controllers/src/joint_impedance_controller.cpp index b6da36b..0e1335b 100644 --- a/flexiv_controllers/src/joint_impedance_controller.cpp +++ b/flexiv_controllers/src/joint_impedance_controller.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -33,12 +34,10 @@ controller_interface::InterfaceConfiguration JointImpedanceController::command_interface_configuration() const { controller_interface::InterfaceConfiguration config; - config.type - = controller_interface::interface_configuration_type::INDIVIDUAL; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; config.names.reserve(joint_names_.size()); for (const auto& joint_name : joint_names_) { - config.names.push_back( - joint_name + "/" + hardware_interface::HW_IF_EFFORT); + config.names.push_back(joint_name + "/" + hardware_interface::HW_IF_EFFORT); } return config; } @@ -47,14 +46,11 @@ controller_interface::InterfaceConfiguration JointImpedanceController::state_interface_configuration() const { controller_interface::InterfaceConfiguration config; - config.type - = controller_interface::interface_configuration_type::INDIVIDUAL; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; config.names.reserve(joint_names_.size() * 2); for (const auto& joint_name : joint_names_) { - config.names.push_back( - joint_name + "/" + hardware_interface::HW_IF_POSITION); - config.names.push_back( - joint_name + "/" + hardware_interface::HW_IF_VELOCITY); + config.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); + config.names.push_back(joint_name + "/" + hardware_interface::HW_IF_VELOCITY); } return config; } @@ -63,8 +59,7 @@ JointImpedanceController::state_interface_configuration() const // in the same order as in joint_names template bool get_ordered_interfaces(std::vector& unordered_interfaces, - const std::vector& joint_names, - const std::string& interface_type, + const std::vector& joint_names, const std::string& interface_type, std::vector>& ordered_interfaces) { for (const auto& joint_name : joint_names) { @@ -79,42 +74,35 @@ bool get_ordered_interfaces(std::vector& unordered_interfaces, return joint_names.size() == ordered_interfaces.size(); } -controller_interface::return_type JointImpedanceController::init( - const std::string& controller_name) +CallbackReturn JointImpedanceController::on_init() { - auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) { - return ret; - } try { // definition of the parameters that need to be queried from the // controller configuration file with default values - auto_declare>( - "joints", std::vector()); + auto_declare>("joints", std::vector()); auto_declare>("k_p", std::vector()); auto_declare>("k_d", std::vector()); } catch (const std::exception& e) { - fprintf(stderr, - "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::return_type::ERROR; + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; } - return controller_interface::return_type::OK; + return CallbackReturn::SUCCESS; } CallbackReturn JointImpedanceController::on_configure( const rclcpp_lifecycle::State& /*previous_state*/) { // getting the names of the joints - joint_names_ = node_->get_parameter("joints").as_string_array(); + joint_names_ = get_node()->get_parameter("joints").as_string_array(); if (joint_names_.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter not set"); return CallbackReturn::FAILURE; } // getting the impedance parameters - k_p_ = node_->get_parameter("k_p").as_double_array(); - k_d_ = node_->get_parameter("k_d").as_double_array(); + k_p_ = get_node()->get_parameter("k_p").as_double_array(); + k_d_ = get_node()->get_parameter("k_d").as_double_array(); if (k_p_.empty()) { k_p_.resize(joint_names_.size(), 50.0); @@ -124,38 +112,34 @@ CallbackReturn JointImpedanceController::on_configure( } if (k_p_.size() != static_cast(num_joints)) { - RCLCPP_ERROR(get_node()->get_logger(), - "k_p should be of size %d but is of size %d", num_joints, - k_p_.size()); + RCLCPP_ERROR(get_node()->get_logger(), "k_p should be of size %d but is of size %ld", + num_joints, k_p_.size()); return CallbackReturn::FAILURE; } if (k_d_.size() != static_cast(num_joints)) { - RCLCPP_ERROR(get_node()->get_logger(), - "k_d should be of size %d but is of size %d", num_joints, - k_d_.size()); + RCLCPP_ERROR(get_node()->get_logger(), "k_d should be of size %d but is of size %ld", + num_joints, k_d_.size()); return CallbackReturn::FAILURE; } for (auto i = 0ul; i < k_p_.size(); i++) { if (k_p_[i] < 0 || k_d_[i] < 0) { - RCLCPP_ERROR( - get_node()->get_logger(), "Wrong Impedance parameters!"); + RCLCPP_ERROR(get_node()->get_logger(), "Wrong Impedance parameters!"); return CallbackReturn::FAILURE; } } joints_command_subscriber_ - = get_node()->create_subscription("~/commands", - rclcpp::SystemDefaultsQoS(), [this](const CmdType::SharedPtr msg) { - rt_command_ptr_.writeFromNonRT(msg); - }); + = get_node()->create_subscription("~/commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); RCLCPP_INFO(get_node()->get_logger(), "Configure successful"); return CallbackReturn::SUCCESS; } -controller_interface::return_type JointImpedanceController::update() +controller_interface::return_type JointImpedanceController::update( + const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { auto joint_commands = rt_command_ptr_.readFromRT(); @@ -166,8 +150,9 @@ controller_interface::return_type JointImpedanceController::update() // check command size if correct if ((*joint_commands)->positions.size() != joint_names_.size()) { - RCLCPP_ERROR_THROTTLE(get_node()->get_logger(), *node_->get_clock(), - 1000, "command size (%zu) does not match number of joints"); + RCLCPP_ERROR_THROTTLE(get_node()->get_logger(), *(get_node()->get_clock()), 1000, + "command size (%zu) does not match number of joints (%zu)", + (*joint_commands)->positions.size(), joint_names_.size()); return controller_interface::return_type::ERROR; } @@ -196,32 +181,33 @@ CallbackReturn JointImpedanceController::on_activate( // check if we have all resources defined in the "points" parameter // also verify that we *only* have the resources defined in the "points" // parameter - std::vector> - ordered_interfaces; - if (!get_ordered_interfaces(command_interfaces_, joint_names_, - hardware_interface::HW_IF_EFFORT, ordered_interfaces) + std::vector> ordered_interfaces; + if (!controller_interface::get_ordered_interfaces( + command_interfaces_, joint_names_, hardware_interface::HW_IF_EFFORT, ordered_interfaces) || command_interfaces_.size() != ordered_interfaces.size()) { - RCLCPP_ERROR(node_->get_logger(), - "Expected %zu position command interfaces, got %zu", + RCLCPP_ERROR(get_node()->get_logger(), "Expected %zu position command interfaces, got %zu", joint_names_.size(), ordered_interfaces.size()); return CallbackReturn::ERROR; } + // reset command buffer if a command came through callback when controller was inactive + rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + + RCLCPP_INFO(get_node()->get_logger(), "Activate successful"); + return CallbackReturn::SUCCESS; } CallbackReturn JointImpedanceController::on_deactivate( const rclcpp_lifecycle::State& /*previous_state*/) { - // the effort on all joints is set to 0 - for (auto index = 0ul; index < joint_names_.size(); ++index) { - command_interfaces_[index].set_value(0.0); - } + // reset command buffer + rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); return CallbackReturn::SUCCESS; } } /* namespace flexiv_controllers */ #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(flexiv_controllers::JointImpedanceController, - controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS( + flexiv_controllers::JointImpedanceController, controller_interface::ControllerInterface) diff --git a/flexiv_controllers/src/tcp_pose_state_broadcaster.cpp b/flexiv_controllers/src/tcp_pose_state_broadcaster.cpp index fa67bd5..1224d4c 100644 --- a/flexiv_controllers/src/tcp_pose_state_broadcaster.cpp +++ b/flexiv_controllers/src/tcp_pose_state_broadcaster.cpp @@ -22,8 +22,7 @@ controller_interface::InterfaceConfiguration TcpPoseStateBroadcaster::command_interface_configuration() const { controller_interface::InterfaceConfiguration command_interfaces_config; - command_interfaces_config.type - = controller_interface::interface_configuration_type::NONE; + command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; return command_interfaces_config; } @@ -31,72 +30,58 @@ controller_interface::InterfaceConfiguration TcpPoseStateBroadcaster::state_interface_configuration() const { controller_interface::InterfaceConfiguration state_interfaces_config; - state_interfaces_config.type - = controller_interface::interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names - = cartesian_pose_sensor_->get_state_interface_names(); + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = cartesian_pose_sensor_->get_state_interface_names(); return state_interfaces_config; } -controller_interface::return_type TcpPoseStateBroadcaster::init( - const std::string& controller_name) +CallbackReturn TcpPoseStateBroadcaster::on_init() { - auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) { - return ret; - } - try { auto_declare("sensor_name", ""); auto_declare("frame_id", ""); auto_declare("topic_name", ""); } catch (const std::exception& e) { - fprintf(stderr, - "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::return_type::ERROR; + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; } - return controller_interface::return_type::OK; + return CallbackReturn::SUCCESS; } CallbackReturn TcpPoseStateBroadcaster::on_configure( const rclcpp_lifecycle::State& /*previous_state*/) { - sensor_name_ = node_->get_parameter("sensor_name").as_string(); + sensor_name_ = get_node()->get_parameter("sensor_name").as_string(); if (sensor_name_.empty()) { - RCLCPP_ERROR(node_->get_logger(), - "'sensor_name' parameter has to be specified."); + RCLCPP_ERROR(get_node()->get_logger(), "'sensor_name' parameter has to be specified."); return CallbackReturn::ERROR; } - frame_id_ = node_->get_parameter("frame_id").as_string(); + frame_id_ = get_node()->get_parameter("frame_id").as_string(); if (frame_id_.empty()) { - RCLCPP_ERROR( - node_->get_logger(), "'frame_id' parameter has to be specified."); + RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be specified."); return CallbackReturn::ERROR; } if (!sensor_name_.empty()) { - cartesian_pose_sensor_ = std::make_unique( - CartesianPoseSensor(sensor_name_)); + cartesian_pose_sensor_ + = std::make_unique(CartesianPoseSensor(sensor_name_)); } - topic_name_ = node_->get_parameter("topic_name").as_string(); + topic_name_ = get_node()->get_parameter("topic_name").as_string(); if (topic_name_.empty()) { - RCLCPP_ERROR( - node_->get_logger(), "'topic_name' parameter has to be specified."); + RCLCPP_ERROR(get_node()->get_logger(), "'topic_name' parameter has to be specified."); return CallbackReturn::ERROR; } try { // register ft sensor data publisher - sensor_state_publisher_ - = node_->create_publisher( - "~/" + topic_name_, rclcpp::SystemDefaultsQoS()); - realtime_publisher_ - = std::make_unique(sensor_state_publisher_); + sensor_state_publisher_ = get_node()->create_publisher( + "~/" + topic_name_, rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = std::make_unique(sensor_state_publisher_); } catch (const std::exception& e) { fprintf(stderr, "Exception thrown during publisher creation at configure stage " @@ -109,16 +94,16 @@ CallbackReturn TcpPoseStateBroadcaster::on_configure( realtime_publisher_->msg_.header.frame_id = frame_id_; realtime_publisher_->unlock(); - RCLCPP_DEBUG(node_->get_logger(), "configure successful"); + RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } -controller_interface::return_type TcpPoseStateBroadcaster::update() +controller_interface::return_type TcpPoseStateBroadcaster::update( + const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { if (realtime_publisher_ && realtime_publisher_->trylock()) { - realtime_publisher_->msg_.header.stamp = node_->now(); - cartesian_pose_sensor_->get_values_as_message( - realtime_publisher_->msg_.pose); + realtime_publisher_->msg_.header.stamp = get_node()->now(); + cartesian_pose_sensor_->get_values_as_message(realtime_publisher_->msg_.pose); realtime_publisher_->unlockAndPublish(); } @@ -143,5 +128,5 @@ CallbackReturn TcpPoseStateBroadcaster::on_deactivate( #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(flexiv_controllers::TcpPoseStateBroadcaster, - controller_interface::ControllerInterface) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS( + flexiv_controllers::TcpPoseStateBroadcaster, controller_interface::ControllerInterface) \ No newline at end of file diff --git a/flexiv_description/config/initial_positions.yaml b/flexiv_description/config/initial_positions.yaml index 42425d8..4c162a3 100644 --- a/flexiv_description/config/initial_positions.yaml +++ b/flexiv_description/config/initial_positions.yaml @@ -1,8 +1,7 @@ -initial_positions: - joint1: 0.0 - joint2: -0.69813 - joint3: 0.0 - joint4: 1.570796 - joint5: 0.0 - joint6: 0.69813 - joint7: 0.0 +joint1: 0.0 +joint2: -0.69813 +joint3: 0.0 +joint4: 1.570796 +joint5: 0.0 +joint6: 0.69813 +joint7: 0.0 diff --git a/flexiv_description/urdf/flexiv_arm_materials.xacro b/flexiv_description/urdf/flexiv_arm.materials.xacro similarity index 87% rename from flexiv_description/urdf/flexiv_arm_materials.xacro rename to flexiv_description/urdf/flexiv_arm.materials.xacro index 2da52d8..4501bb6 100644 --- a/flexiv_description/urdf/flexiv_arm_materials.xacro +++ b/flexiv_description/urdf/flexiv_arm.materials.xacro @@ -1,8 +1,6 @@ - - @@ -16,6 +14,4 @@ - - diff --git a/flexiv_description/urdf/rizon.urdf.xacro b/flexiv_description/urdf/rizon.urdf.xacro index d833580..307bfa6 100644 --- a/flexiv_description/urdf/rizon.urdf.xacro +++ b/flexiv_description/urdf/rizon.urdf.xacro @@ -18,6 +18,9 @@ + + + + initial_positions="${xacro.load_yaml(initial_positions_file)}"> diff --git a/flexiv_description/urdf/rizon10.ros2_control.xacro b/flexiv_description/urdf/rizon10.ros2_control.xacro index 387aeec..d168392 100644 --- a/flexiv_description/urdf/rizon10.ros2_control.xacro +++ b/flexiv_description/urdf/rizon10.ros2_control.xacro @@ -1,14 +1,20 @@ - - + - fake_components/GenericSystem - ${fake_sensor_commands} + mock_components/GenericSystem + ${fake_sensor_commands} 0.0 @@ -28,10 +34,12 @@ 1.7453 - + + + ${initial_positions['joint1']} + - ${initial_positions['joint1']} @@ -43,10 +51,12 @@ 1.7453 - + + + ${initial_positions['joint2']} + - ${initial_positions['joint2']} @@ -58,10 +68,12 @@ 2.0944 - + + + ${initial_positions['joint3']} + - ${initial_positions['joint3']} @@ -73,10 +85,12 @@ 2.0944 - + + + ${initial_positions['joint4']} + - ${initial_positions['joint4']} @@ -88,10 +102,12 @@ 3.8397 - + + + ${initial_positions['joint5']} + - ${initial_positions['joint5']} @@ -103,10 +119,12 @@ 3.8397 - + + + ${initial_positions['joint6']} + - ${initial_positions['joint6']} @@ -118,10 +136,12 @@ 3.8397 - + + + ${initial_positions['joint7']} + - ${initial_positions['joint7']} diff --git a/flexiv_description/urdf/rizon4.ros2_control.xacro b/flexiv_description/urdf/rizon4.ros2_control.xacro index c98d8fd..fa24f3a 100644 --- a/flexiv_description/urdf/rizon4.ros2_control.xacro +++ b/flexiv_description/urdf/rizon4.ros2_control.xacro @@ -1,14 +1,20 @@ - - + - fake_components/GenericSystem - ${fake_sensor_commands} + mock_components/GenericSystem + ${fake_sensor_commands} 0.0 @@ -28,10 +34,12 @@ 2.0944 - + + + ${initial_positions['joint1']} + - ${initial_positions['joint1']} @@ -43,10 +51,12 @@ 2.0944 - + + + ${initial_positions['joint2']} + - ${initial_positions['joint2']} @@ -58,10 +68,12 @@ 2.4435 - + + + ${initial_positions['joint3']} + - ${initial_positions['joint3']} @@ -73,10 +85,12 @@ 2.4435 - + + + ${initial_positions['joint4']} + - ${initial_positions['joint4']} @@ -88,10 +102,12 @@ 4.8869 - + + + ${initial_positions['joint5']} + - ${initial_positions['joint5']} @@ -103,10 +119,12 @@ 4.8869 - + + + ${initial_positions['joint6']} + - ${initial_positions['joint6']} @@ -118,10 +136,12 @@ 4.8869 - + + + ${initial_positions['joint7']} + - ${initial_positions['joint7']} diff --git a/flexiv_description/urdf/rizon_macro.xacro b/flexiv_description/urdf/rizon_macro.xacro index ca4f000..c2dbf84 100644 --- a/flexiv_description/urdf/rizon_macro.xacro +++ b/flexiv_description/urdf/rizon_macro.xacro @@ -8,35 +8,37 @@ local_ip use_fake_hardware:=false fake_sensor_commands:=false - initial_positions_file"> + initial_positions:=${dict(joint1=0.0,joint2=-0.69813,joint3=0.0,joint4=1.570796,joint5=0.0,joint6=0.69813,joint7=0.0)} + "> - + + - + - - + - diff --git a/flexiv_hardware/CMakeLists.txt b/flexiv_hardware/CMakeLists.txt index db4cdd3..9d51615 100644 --- a/flexiv_hardware/CMakeLists.txt +++ b/flexiv_hardware/CMakeLists.txt @@ -13,6 +13,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) @@ -64,6 +65,7 @@ ament_target_dependencies( hardware_interface pluginlib rclcpp + rclcpp_lifecycle ) pluginlib_export_plugin_description_file(hardware_interface flexiv_hardware_interface_plugin.xml) @@ -97,6 +99,7 @@ ament_export_dependencies( hardware_interface pluginlib rclcpp + rclcpp_lifecycle ) ament_package() diff --git a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp index 0b4916b..2b6e36c 100644 --- a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp +++ b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp @@ -1,7 +1,7 @@ /** * @file flexiv_hardware_interface.hpp * @brief Hardware interface to Flexiv robots for ROS 2 control. Adapted from - * ros2_control_demos/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_multi_interface.hpp + * ros2_control_demos/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -19,21 +19,18 @@ #include #include #include +#include // ros2_control hardware_interface #include -#include #include #include -#include #include "flexiv_hardware/visibility_control.h" // Flexiv #include "flexiv/Robot.hpp" -using hardware_interface::return_type; - namespace flexiv_hardware { enum StoppingInterface @@ -44,45 +41,46 @@ enum StoppingInterface STOP_EFFORT }; -class FlexivHardwareInterface -: public hardware_interface::BaseInterface +class FlexivHardwareInterface : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(FlexivHardwareInterface) FLEXIV_HARDWARE_PUBLIC - return_type configure( + hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo& info) override; FLEXIV_HARDWARE_PUBLIC - std::vector - export_state_interfaces() override; + std::vector export_state_interfaces() override; FLEXIV_HARDWARE_PUBLIC - std::vector - export_command_interfaces() override; + std::vector export_command_interfaces() override; FLEXIV_HARDWARE_PUBLIC - return_type prepare_command_mode_switch( + hardware_interface::return_type prepare_command_mode_switch( const std::vector& start_interfaces, const std::vector& stop_interfaces) override; FLEXIV_HARDWARE_PUBLIC - return_type perform_command_mode_switch( + hardware_interface::return_type perform_command_mode_switch( const std::vector& start_interfaces, const std::vector& stop_interfaces) override; FLEXIV_HARDWARE_PUBLIC - return_type start() override; + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State& previous_state) override; FLEXIV_HARDWARE_PUBLIC - return_type stop() override; + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State& previous_state) override; FLEXIV_HARDWARE_PUBLIC - return_type read() override; + hardware_interface::return_type read( + const rclcpp::Time& time, const rclcpp::Duration& period) override; FLEXIV_HARDWARE_PUBLIC - return_type write() override; + hardware_interface::return_type write( + const rclcpp::Time& time, const rclcpp::Duration& period) override; static const size_t n_joints = 7; diff --git a/flexiv_hardware/package.xml b/flexiv_hardware/package.xml index d2522b2..f584cd4 100644 --- a/flexiv_hardware/package.xml +++ b/flexiv_hardware/package.xml @@ -11,6 +11,7 @@ ament_cmake rclcpp + rclcpp_lifecycle hardware_interface pluginlib diff --git a/flexiv_hardware/src/flexiv_hardware_interface.cpp b/flexiv_hardware/src/flexiv_hardware_interface.cpp index 50d640f..1b197cc 100644 --- a/flexiv_hardware/src/flexiv_hardware_interface.cpp +++ b/flexiv_hardware/src/flexiv_hardware_interface.cpp @@ -1,7 +1,7 @@ /** * @file flexiv_hardware_interface.cpp * @brief Hardware interface to Flexiv robots for ROS 2 control. Adapted from - * ros2_control_demos/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp + * ros2_control_demos/example_3/hardware/rrbot_system_multi_interface.cpp * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -20,19 +20,19 @@ namespace flexiv_hardware { -return_type FlexivHardwareInterface::configure( +hardware_interface::CallbackReturn FlexivHardwareInterface::on_init( const hardware_interface::HardwareInfo& info) { - if (configure_default(info) != return_type::OK) { - return return_type::ERROR; + if (hardware_interface::SystemInterface::on_init(info) + != hardware_interface::CallbackReturn::SUCCESS) { + return hardware_interface::CallbackReturn::ERROR; } hw_states_joint_positions_.resize( info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_states_joint_velocities_.resize( info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_states_joint_efforts_.resize( - info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_states_joint_efforts_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_commands_joint_positions_.resize( info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_commands_joint_velocities_.resize( @@ -40,20 +40,16 @@ return_type FlexivHardwareInterface::configure( hw_commands_joint_efforts_.resize( info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_states_force_torque_sensor_.resize( - info_.sensors[0].state_interfaces.size(), - std::numeric_limits::quiet_NaN()); + info_.sensors[0].state_interfaces.size(), std::numeric_limits::quiet_NaN()); hw_states_external_wrench_in_base_.resize( - info_.sensors[1].state_interfaces.size(), - std::numeric_limits::quiet_NaN()); + info_.sensors[1].state_interfaces.size(), std::numeric_limits::quiet_NaN()); hw_states_external_wrench_in_tcp_.resize( - info_.sensors[2].state_interfaces.size(), - std::numeric_limits::quiet_NaN()); + info_.sensors[2].state_interfaces.size(), std::numeric_limits::quiet_NaN()); hw_states_tcp_pose_.resize(7, std::numeric_limits::quiet_NaN()); internal_commands_joint_positions_.resize( info_.joints.size(), std::numeric_limits::quiet_NaN()); - stop_modes_ = {StoppingInterface::NONE, StoppingInterface::NONE, - StoppingInterface::NONE, StoppingInterface::NONE, - StoppingInterface::NONE, StoppingInterface::NONE, + stop_modes_ = {StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE, + StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE}; start_modes_ = {}; position_controller_running_ = false; @@ -62,78 +58,63 @@ return_type FlexivHardwareInterface::configure( controllers_initialized_ = false; if (info_.joints.size() != n_joints) { - RCLCPP_FATAL(getLogger(), "Got %d joints. Expected %d.", - info_.joints.size(), n_joints); - return return_type::ERROR; + RCLCPP_FATAL(getLogger(), "Got %ld joints. Expected %ld.", info_.joints.size(), n_joints); + return hardware_interface::CallbackReturn::ERROR; } for (const hardware_interface::ComponentInfo& joint : info_.joints) { if (joint.command_interfaces.size() != 3) { - RCLCPP_FATAL(getLogger(), - "Joint '%s' has %d command interfaces found. 3 expected.", + RCLCPP_FATAL(getLogger(), "Joint '%s' has %ld command interfaces found. 3 expected.", joint.name.c_str(), joint.command_interfaces.size()); - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } - if (joint.command_interfaces[0].name - != hardware_interface::HW_IF_POSITION) { - RCLCPP_FATAL(getLogger(), - "Joint '%s' has '%s' command interface. Expected '%s'", + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + RCLCPP_FATAL(getLogger(), "Joint '%s' has '%s' command interface. Expected '%s'", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } - if (joint.command_interfaces[1].name - != hardware_interface::HW_IF_VELOCITY) { - RCLCPP_FATAL(getLogger(), - "Joint '%s' has '%s' command interface. Expected '%s'", + if (joint.command_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { + RCLCPP_FATAL(getLogger(), "Joint '%s' has '%s' command interface. Expected '%s'", joint.name.c_str(), joint.command_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } - if (joint.command_interfaces[2].name - != hardware_interface::HW_IF_EFFORT) { - RCLCPP_FATAL(getLogger(), - "Joint '%s' has '%s' command interface. Expected '%s'", + if (joint.command_interfaces[2].name != hardware_interface::HW_IF_EFFORT) { + RCLCPP_FATAL(getLogger(), "Joint '%s' has '%s' command interface. Expected '%s'", joint.name.c_str(), joint.command_interfaces[2].name.c_str(), hardware_interface::HW_IF_EFFORT); - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 3) { - RCLCPP_FATAL(getLogger(), - "Joint '%s' has %d state interfaces found. 3 expected.", + RCLCPP_FATAL(getLogger(), "Joint '%s' has %ld state interfaces found. 3 expected.", joint.name.c_str(), joint.state_interfaces.size()); - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } - if (joint.state_interfaces[0].name - != hardware_interface::HW_IF_POSITION) { - RCLCPP_FATAL(getLogger(), - "Joint '%s' has '%s' state interface. Expected '%s'", + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + RCLCPP_FATAL(getLogger(), "Joint '%s' has '%s' state interface. Expected '%s'", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } - if (joint.state_interfaces[1].name - != hardware_interface::HW_IF_VELOCITY) { - RCLCPP_FATAL(getLogger(), - "Joint '%s' has '%s' state interface. Expected '%s'", + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { + RCLCPP_FATAL(getLogger(), "Joint '%s' has '%s' state interface. Expected '%s'", joint.name.c_str(), joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } - if (joint.state_interfaces[2].name - != hardware_interface::HW_IF_EFFORT) { - RCLCPP_FATAL(getLogger(), - "Joint '%s' has '%s' state interface. Expected '%s'", + if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) { + RCLCPP_FATAL(getLogger(), "Joint '%s' has '%s' state interface. Expected '%s'", joint.name.c_str(), joint.state_interfaces[2].name.c_str(), hardware_interface::HW_IF_EFFORT); - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } } @@ -142,7 +123,7 @@ return_type FlexivHardwareInterface::configure( robot_ip = info_.hardware_parameters["robot_ip"]; } catch (const std::out_of_range& ex) { RCLCPP_FATAL(getLogger(), "Parameter 'robot_ip' not set"); - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } std::string local_ip; @@ -150,25 +131,23 @@ return_type FlexivHardwareInterface::configure( local_ip = info_.hardware_parameters["local_ip"]; } catch (const std::out_of_range& ex) { RCLCPP_FATAL(getLogger(), "Parameter 'local_ip' not set"); - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } try { - RCLCPP_INFO(getLogger(), - "Connecting to robot at \"%s\" from \"%s\" ...", robot_ip.c_str(), + RCLCPP_INFO(getLogger(), "Connecting to robot at \"%s\" from \"%s\" ...", robot_ip.c_str(), local_ip.c_str()); robot_ = std::make_unique(robot_ip, local_ip); } catch (const flexiv::Exception& e) { RCLCPP_FATAL(getLogger(), "Could not connect to robot"); RCLCPP_FATAL(getLogger(), e.what()); - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } clock_ = rclcpp::Clock(); - status_ = hardware_interface::status::CONFIGURED; RCLCPP_INFO(getLogger(), "Successfully connected to robot"); - return return_type::OK; + return hardware_interface::CallbackReturn::SUCCESS; } rclcpp::Logger FlexivHardwareInterface::getLogger() @@ -176,47 +155,35 @@ rclcpp::Logger FlexivHardwareInterface::getLogger() return rclcpp::get_logger("FlexivHardwareInterface"); } -std::vector -FlexivHardwareInterface::export_state_interfaces() +std::vector FlexivHardwareInterface::export_state_interfaces() { RCLCPP_INFO(getLogger(), "export_state_interfaces"); std::vector state_interfaces; for (std::size_t i = 0; i < info_.joints.size(); i++) { + state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name, + hardware_interface::HW_IF_POSITION, &hw_states_joint_positions_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name, + hardware_interface::HW_IF_VELOCITY, &hw_states_joint_velocities_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, - &hw_states_joint_positions_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, - &hw_states_joint_velocities_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_EFFORT, - &hw_states_joint_efforts_[i])); + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_states_joint_efforts_[i])); } for (std::size_t i = 0; i < info_.sensors.size(); i++) { const auto& sensor = info_.sensors[i]; for (std::size_t j = 0; j < sensor.state_interfaces.size(); j++) { if (i == 0) { - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor.name, - sensor.state_interfaces[j].name, - &hw_states_force_torque_sensor_[j])); + state_interfaces.emplace_back(hardware_interface::StateInterface(sensor.name, + sensor.state_interfaces[j].name, &hw_states_force_torque_sensor_[j])); } else if (i == 1) { - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor.name, - sensor.state_interfaces[j].name, - &hw_states_external_wrench_in_base_[j])); + state_interfaces.emplace_back(hardware_interface::StateInterface(sensor.name, + sensor.state_interfaces[j].name, &hw_states_external_wrench_in_base_[j])); } else if (i == 2) { - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor.name, - sensor.state_interfaces[j].name, - &hw_states_external_wrench_in_tcp_[j])); + state_interfaces.emplace_back(hardware_interface::StateInterface(sensor.name, + sensor.state_interfaces[j].name, &hw_states_external_wrench_in_tcp_[j])); } else if (i == 3) { - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor.name, - sensor.state_interfaces[j].name, - &hw_states_tcp_pose_[j])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + sensor.name, sensor.state_interfaces[j].name, &hw_states_tcp_pose_[j])); } } } @@ -231,21 +198,19 @@ FlexivHardwareInterface::export_command_interfaces() std::vector command_interfaces; for (size_t i = 0; i < info_.joints.size(); i++) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, - &hw_commands_joint_positions_[i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, - &hw_commands_joint_velocities_[i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_EFFORT, - &hw_commands_joint_efforts_[i])); + command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, + hardware_interface::HW_IF_POSITION, &hw_commands_joint_positions_[i])); + command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, + hardware_interface::HW_IF_VELOCITY, &hw_commands_joint_velocities_[i])); + command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, + hardware_interface::HW_IF_EFFORT, &hw_commands_joint_efforts_[i])); } return command_interfaces; } -return_type FlexivHardwareInterface::start() +hardware_interface::CallbackReturn FlexivHardwareInterface::on_activate( + const rclcpp_lifecycle::State& /*previous_state*/) { RCLCPP_INFO(getLogger(), "Starting... please wait..."); @@ -256,7 +221,7 @@ return_type FlexivHardwareInterface::start() } catch (const flexiv::Exception& e) { RCLCPP_FATAL(getLogger(), "Could not enable robot."); RCLCPP_FATAL(getLogger(), e.what()); - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } // Wait for the robot to become operational @@ -266,13 +231,13 @@ return_type FlexivHardwareInterface::start() RCLCPP_INFO(getLogger(), "Robot is now operational"); last_timestamp_ = clock_.now(); - status_ = hardware_interface::status::STARTED; RCLCPP_INFO(getLogger(), "System successfully started!"); - return return_type::OK; + return hardware_interface::CallbackReturn::SUCCESS; } -return_type FlexivHardwareInterface::stop() +hardware_interface::CallbackReturn FlexivHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State& /*previous_state*/) { RCLCPP_INFO(getLogger(), "Stopping... please wait..."); @@ -281,19 +246,14 @@ return_type FlexivHardwareInterface::stop() robot_->stop(); robot_->disconnect(); - status_ = hardware_interface::status::STOPPED; RCLCPP_INFO(getLogger(), "System successfully stopped!"); - return return_type::OK; + return hardware_interface::CallbackReturn::SUCCESS; } -return_type FlexivHardwareInterface::read() +hardware_interface::return_type FlexivHardwareInterface::read( + const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - if (status_ != hardware_interface::status::STARTED) { - RCLCPP_FATAL(getLogger(), "Hardware not started!"); - return return_type::ERROR; - } - flexiv::RobotStates robot_states; if (robot_->isOperational() && robot_->getMode() != flexiv::Mode::IDLE) { robot_->getRobotStates(robot_states); @@ -317,16 +277,12 @@ return_type FlexivHardwareInterface::read() hw_states_tcp_pose_[6] = robot_states.tcpPose[3]; } - return return_type::OK; + return hardware_interface::return_type::OK; } -return_type FlexivHardwareInterface::write() +hardware_interface::return_type FlexivHardwareInterface::write( + const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - if (status_ != hardware_interface::status::STARTED) { - RCLCPP_FATAL(getLogger(), "Hardware not started!"); - return return_type::ERROR; - } - current_timestamp_ = clock_.now(); rclcpp::Duration duration = current_timestamp_ - last_timestamp_; last_timestamp_ = current_timestamp_; @@ -342,36 +298,33 @@ return_type FlexivHardwareInterface::write() for (std::size_t i = 0; i < n_joints; i++) { if (hw_commands_joint_positions_[i] != hw_commands_joint_positions_[i]) isNanPos = true; - if (hw_commands_joint_velocities_[i] - != hw_commands_joint_velocities_[i]) + if (hw_commands_joint_velocities_[i] != hw_commands_joint_velocities_[i]) isNanVel = true; if (hw_commands_joint_efforts_[i] != hw_commands_joint_efforts_[i]) isNanEff = true; } - if (position_controller_running_ - && robot_->getMode() == flexiv::Mode::RT_JOINT_POSITION && !isNanPos) { + if (position_controller_running_ && robot_->getMode() == flexiv::Mode::RT_JOINT_POSITION + && !isNanPos) { robot_->streamJointPosition( hw_commands_joint_positions_, targetVelocity, targetAcceleration); - } else if (velocity_controller_running_ - && robot_->getMode() == flexiv::Mode::RT_JOINT_POSITION + } else if (velocity_controller_running_ && robot_->getMode() == flexiv::Mode::RT_JOINT_POSITION && !isNanVel) { for (std::size_t i = 0; i < n_joints; i++) { internal_commands_joint_positions_[i] += hw_commands_joint_velocities_[i] * duration.seconds(); } - robot_->streamJointPosition(internal_commands_joint_positions_, - hw_commands_joint_velocities_, targetAcceleration); - } else if (torque_controller_running_ - && robot_->getMode() == flexiv::Mode::RT_JOINT_TORQUE + robot_->streamJointPosition( + internal_commands_joint_positions_, hw_commands_joint_velocities_, targetAcceleration); + } else if (torque_controller_running_ && robot_->getMode() == flexiv::Mode::RT_JOINT_TORQUE && !isNanEff) { robot_->streamJointTorque(hw_commands_joint_efforts_, true, true); } - return return_type::OK; + return hardware_interface::return_type::OK; } -return_type FlexivHardwareInterface::prepare_command_mode_switch( +hardware_interface::return_type FlexivHardwareInterface::prepare_command_mode_switch( const std::vector& start_interfaces, const std::vector& stop_interfaces) { @@ -381,51 +334,37 @@ return_type FlexivHardwareInterface::prepare_command_mode_switch( // Starting interfaces for (const auto& key : start_interfaces) { for (std::size_t i = 0; i < info_.joints.size(); i++) { - if (key - == info_.joints[i].name + "/" - + hardware_interface::HW_IF_POSITION) { + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { start_modes_.push_back(hardware_interface::HW_IF_POSITION); } - if (key - == info_.joints[i].name + "/" - + hardware_interface::HW_IF_VELOCITY) { + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); } - if (key - == info_.joints[i].name + "/" - + hardware_interface::HW_IF_EFFORT) { + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_EFFORT) { start_modes_.push_back(hardware_interface::HW_IF_EFFORT); } } } // All joints must be given new command mode at the same time - if (start_modes_.size() != 0 - && start_modes_.size() != info_.joints.size()) { - return return_type::ERROR; + if (start_modes_.size() != 0 && start_modes_.size() != info_.joints.size()) { + return hardware_interface::return_type::ERROR; } // All joints must have the same command mode if (start_modes_.size() != 0 - && !std::equal(start_modes_.begin() + 1, start_modes_.end(), - start_modes_.begin())) { - return return_type::ERROR; + && !std::equal(start_modes_.begin() + 1, start_modes_.end(), start_modes_.begin())) { + return hardware_interface::return_type::ERROR; } // Stop motion on all relevant joints that are stopping for (const auto& key : stop_interfaces) { for (std::size_t i = 0; i < info_.joints.size(); i++) { - if (key - == info_.joints[i].name + "/" - + hardware_interface::HW_IF_POSITION) { + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { stop_modes_.push_back(StoppingInterface::STOP_POSITION); } - if (key - == info_.joints[i].name + "/" - + hardware_interface::HW_IF_VELOCITY) { + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { stop_modes_.push_back(StoppingInterface::STOP_VELOCITY); } - if (key - == info_.joints[i].name + "/" - + hardware_interface::HW_IF_EFFORT) { + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_EFFORT) { stop_modes_.push_back(StoppingInterface::STOP_EFFORT); } } @@ -433,49 +372,44 @@ return_type FlexivHardwareInterface::prepare_command_mode_switch( // stop all interfaces at the same time if (stop_modes_.size() != 0 && (stop_modes_.size() != info_.joints.size() - || !std::equal(stop_modes_.begin() + 1, stop_modes_.end(), - stop_modes_.begin()))) { - return return_type::ERROR; + || !std::equal(stop_modes_.begin() + 1, stop_modes_.end(), stop_modes_.begin()))) { + return hardware_interface::return_type::ERROR; } controllers_initialized_ = true; - return return_type::OK; + return hardware_interface::return_type::OK; } -return_type FlexivHardwareInterface::perform_command_mode_switch( - const std::vector& start_interfaces, - const std::vector& stop_interfaces) +hardware_interface::return_type FlexivHardwareInterface::perform_command_mode_switch( + const std::vector& /*start_interfaces*/, + const std::vector& /*stop_interfaces*/) { if (stop_modes_.size() != 0 - && std::find(stop_modes_.begin(), stop_modes_.end(), - StoppingInterface::STOP_POSITION) + && std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_POSITION) != stop_modes_.end()) { position_controller_running_ = false; robot_->stop(); } else if (stop_modes_.size() != 0 - && std::find(stop_modes_.begin(), stop_modes_.end(), - StoppingInterface::STOP_VELOCITY) + && std::find( + stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_VELOCITY) != stop_modes_.end()) { velocity_controller_running_ = false; robot_->stop(); } else if (stop_modes_.size() != 0 - && std::find(stop_modes_.begin(), stop_modes_.end(), - StoppingInterface::STOP_EFFORT) + && std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_EFFORT) != stop_modes_.end()) { torque_controller_running_ = false; robot_->stop(); } if (start_modes_.size() != 0 - && std::find(start_modes_.begin(), start_modes_.end(), - hardware_interface::HW_IF_POSITION) + && std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_POSITION) != start_modes_.end()) { velocity_controller_running_ = false; torque_controller_running_ = false; // Hold joints before user commands arrives - std::fill(hw_commands_joint_positions_.begin(), - hw_commands_joint_positions_.end(), + std::fill(hw_commands_joint_positions_.begin(), hw_commands_joint_positions_.end(), std::numeric_limits::quiet_NaN()); // Set to joint position mode @@ -483,15 +417,14 @@ return_type FlexivHardwareInterface::perform_command_mode_switch( position_controller_running_ = true; } else if (start_modes_.size() != 0 - && std::find(start_modes_.begin(), start_modes_.end(), - hardware_interface::HW_IF_VELOCITY) + && std::find( + start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_VELOCITY) != start_modes_.end()) { position_controller_running_ = false; torque_controller_running_ = false; // Hold joints before user commands arrives - std::fill(hw_commands_joint_velocities_.begin(), - hw_commands_joint_velocities_.end(), + std::fill(hw_commands_joint_velocities_.begin(), hw_commands_joint_velocities_.end(), std::numeric_limits::quiet_NaN()); // Set to joint position mode @@ -499,16 +432,15 @@ return_type FlexivHardwareInterface::perform_command_mode_switch( velocity_controller_running_ = true; } else if (start_modes_.size() != 0 - && std::find(start_modes_.begin(), start_modes_.end(), - hardware_interface::HW_IF_EFFORT) + && std::find( + start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_EFFORT) != start_modes_.end()) { position_controller_running_ = false; velocity_controller_running_ = false; // Hold joints when starting joint torque controller before user // commands arrives - std::fill(hw_commands_joint_efforts_.begin(), - hw_commands_joint_efforts_.end(), + std::fill(hw_commands_joint_efforts_.begin(), hw_commands_joint_efforts_.end(), std::numeric_limits::quiet_NaN()); // Set to joint torque mode @@ -520,12 +452,12 @@ return_type FlexivHardwareInterface::perform_command_mode_switch( start_modes_.clear(); stop_modes_.clear(); - return return_type::OK; + return hardware_interface::return_type::OK; } } /* namespace flexiv_hardware */ #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(flexiv_hardware::FlexivHardwareInterface, - hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS( + flexiv_hardware::FlexivHardwareInterface, hardware_interface::SystemInterface) diff --git a/flexiv_moveit_config/config/kinematics.yaml b/flexiv_moveit_config/config/kinematics.yaml index aa7997a..7511a3e 100644 --- a/flexiv_moveit_config/config/kinematics.yaml +++ b/flexiv_moveit_config/config/kinematics.yaml @@ -1,4 +1,7 @@ -rizon_arm: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.05 +/**: + ros__parameters: + robot_description_kinematics: + rizon_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.05 diff --git a/flexiv_moveit_config/package.xml b/flexiv_moveit_config/package.xml index 3645f34..d118bc9 100644 --- a/flexiv_moveit_config/package.xml +++ b/flexiv_moveit_config/package.xml @@ -21,7 +21,7 @@ rviz2 flexiv_description urdf - warehouse_ros_mongo + warehouse_ros_sqlite xacro diff --git a/flexiv_test_nodes/flexiv_test_nodes/publisher_joint_trajectory_controller.py b/flexiv_test_nodes/flexiv_test_nodes/publisher_joint_trajectory_controller.py index f4582c2..240b451 100644 --- a/flexiv_test_nodes/flexiv_test_nodes/publisher_joint_trajectory_controller.py +++ b/flexiv_test_nodes/flexiv_test_nodes/publisher_joint_trajectory_controller.py @@ -1,4 +1,4 @@ -# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -17,6 +17,7 @@ from rclpy.node import Node from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from rcl_interfaces.msg import ParameterDescriptor class PublisherJointTrajectory(Node): @@ -26,9 +27,9 @@ def __init__(self): self.declare_parameter("controller_name", "joint_trajectory_controller") self.declare_parameter("wait_sec_between_publish", 6) self.declare_parameter("goal_names", ["pos1", "pos2"]) - self.declare_parameter("joints") + self.declare_parameter("joints", [""]) self.declare_parameter("check_starting_point", False) - self.declare_parameter("starting_point_limits") + self.declare_parameter("starting_point_limits", False) # Read parameters controller_name = self.get_parameter("controller_name").value @@ -56,17 +57,14 @@ def __init__(self): JointState, "joint_states", self.joint_state_callback, 10 ) # initialize starting point status - if not self.check_starting_point: - self.starting_point_ok = True - else: - self.starting_point_ok = False + self.starting_point_ok = not self.check_starting_point self.joint_state_msg_received = False # Read all positions from parameters self.goals = [] for name in goal_names: - self.declare_parameter(name) + self.declare_parameter(name, descriptor=ParameterDescriptor(dynamic_typing=True)) goal = self.get_parameter(name).value if goal is None or len(goal) == 0: raise Exception(f'Values for goal "{name}" not set!') diff --git a/flexiv_test_nodes/flexiv_test_nodes/sine_sweep_impedance_controller.py b/flexiv_test_nodes/flexiv_test_nodes/sine_sweep_impedance_controller.py index 7f229d1..1af64c7 100644 --- a/flexiv_test_nodes/flexiv_test_nodes/sine_sweep_impedance_controller.py +++ b/flexiv_test_nodes/flexiv_test_nodes/sine_sweep_impedance_controller.py @@ -19,12 +19,12 @@ def __init__(self): # Declare all parameters self.declare_parameter("controller_name", "joint_impedance_controller") self.declare_parameter("wait_sec_between_publish", 0.001) - self.declare_parameter("joints") + self.declare_parameter("speed_scaling", 1.0) # Read parameters controller_name = self.get_parameter("controller_name").value wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value - self.joints = self.get_parameter("joints").value + self.speed_scaling = self.get_parameter("speed_scaling").value publish_topic = "/" + controller_name + "/" + "commands" @@ -44,7 +44,7 @@ def timer_callback(self): target_pos = self.init_pos.copy() for i in range(7): target_pos[i] = self.init_pos[i] + SWING_AMP * math.sin( - 2 * math.pi * SWING_FREQ * self.loop_time + 2 * math.pi * SWING_FREQ * self.speed_scaling * self.loop_time ) msg = JointPosVel() msg.positions = target_pos