diff --git a/example_1/CMakeLists.txt b/example_1/CMakeLists.txt index 24d91c90a..f7bea298c 100644 --- a/example_1/CMakeLists.txt +++ b/example_1/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(ros2_control_demo_hardware) +project(ros2_control_demo_example_1) # Default to C++14 if(NOT CMAKE_CXX_STANDARD) @@ -10,12 +10,19 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + # find dependencies find_package(ament_cmake REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() ## COMPILE @@ -27,22 +34,19 @@ add_library( target_include_directories( ${PROJECT_NAME} PRIVATE - include + hardware/include ) ament_target_dependencies( ${PROJECT_NAME} - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "ROS2_CONTROL_DEMO_HARDWARE_BUILDING_DLL") +target_compile_definitions(${PROJECT_NAME} PRIVATE "ROS2_CONTROL_DEMO_EXAMPLE_1_BUILDING_DLL") -# Export hardware pligins -pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_hardware.xml) +# Export hardware plugins +pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_1.xml) # INSTALL install( @@ -50,9 +54,17 @@ install( DESTINATION lib ) install( - DIRECTORY include/ + DIRECTORY hardware/include/ DESTINATION include ) +install( + DIRECTORY description/launch description/ros2_control description/urdf description/rviz description/meshes + DESTINATION share/${PROJECT_NAME} +) +install( + DIRECTORY bringup/launch bringup/config + DESTINATION share/${PROJECT_NAME} +) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) @@ -66,9 +78,6 @@ ament_export_libraries( ${PROJECT_NAME} ) ament_export_dependencies( - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_package() diff --git a/example_1/README.rst b/example_1/README.rst index cbe3f9971..1ba724026 100644 --- a/example_1/README.rst +++ b/example_1/README.rst @@ -1,98 +1,109 @@ - - -TODO(destogl): This is not adjusted yet!! +**************** +Example 1: RRBot +**************** *RRBot*, or ''Revolute-Revolute Manipulator Robot'', is a simple 3-linkage, 2-joint arm that we will use to demonstrate various features. It is essentially a double inverted pendulum and demonstrates some fun control concepts within a simulator and was originally introduced for Gazebo tutorials. -The *RRBot* URDF files can be found in the ``urdf`` folder of ``rrbot_description`` package. +The *RRBot* URDF files can be found in the ``description/urdf`` folder. + +1. To check that *RRBot* descriptions are working properly use following launch commands + + .. code-block:: shell -1. To check that *RRBot* descriptions are working properly use following launch commands: + ros2 launch ros2_control_demo_example_1 view_robot.launch.py - *RRBot* - ``` - ros2 launch rrbot_description view_robot.launch.py - ``` **NOTE**: Getting the following output in terminal is OK: ``Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist``. This happens because ``joint_state_publisher_gui`` node need some time to start. - The ``joint_state_publisher_gui`` provides a GUI to generate a random configuration for rrbot. It is immediately displayed in ``Rviz``. + The ``joint_state_publisher_gui`` provides a GUI to change the configuration for rrbot. It is immediately displayed in *RViz*. + .. image:: doc/rrbot.png + :width: 400 + :alt: Revolute-Revolute Manipulator Robot -1. To check that *RRBot* descriptions are working properly use following launch commands: - *RRBot* - ``` - ros2 launch rrbot_description view_robot.launch.py - ``` - **NOTE**: Getting the following output in terminal is OK: ``Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist``. - This happens because ``joint_state_publisher_gui`` node need some time to start. - The ``joint_state_publisher_gui`` provides a GUI to generate a random configuration for rrbot. It is immediately displayed in ``Rviz``. +2. To start *RRBot* example open a terminal, source your ROS2-workspace and execute its launch file with + .. code-block:: shell -1. To start *RRBot* example open a terminal, source your ROS2-workspace and execute its launch file with: - ``` - ros2 launch ros2_control_demo_bringup rrbot.launch.py - ``` - The launch file loads and starts the robot hardware, controllers and opens ``RViz``. + ros2 launch ros2_control_demo_example_1 rrbot.launch.py + + The launch file loads and starts the robot hardware, controllers and opens *RViz*. In starting terminal you will see a lot of output from the hardware implementation showing its internal states. This is only of exemplary purposes and should be avoided as much as possible in a hardware interface implementation. - If you can see two orange and one yellow rectangle in in ``RViz`` everything has started properly. + If you can see two orange and one yellow rectangle in in *RViz* everything has started properly. Still, to be sure, let's introspect the control system before moving *RRBot*. -1. Check if the hardware interface loaded properly, by opening another terminal and executing: - ``` - ros2 control list_hardware_interfaces - ``` +3. Check if the hardware interface loaded properly, by opening another terminal and executing + + .. code-block:: shell - You should get:: + ros2 control list_hardware_interfaces + + .. code-block:: shell command interfaces - joint1/position [claimed] - joint2/position [claimed] + joint1/position [available] [claimed] + joint2/position [available] [claimed] state interfaces - joint1/position - joint2/position + joint1/position + joint2/position Marker ``[claimed]`` by command interfaces means that a controller has access to command *RRBot*. -1. Check is controllers are running: - ``` - ros2 control list_controllers - ``` - You should get: - ``` - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - forward_position_controller[forward_command_controller/ForwardCommandController] active - ``` +4. Check is controllers are running by + + .. code-block:: shell + + ros2 control list_controllers + + .. code-block:: shell -1. If you get output from above you can send commands to *Forward Command Controller*, either: + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + forward_position_controller[forward_command_controller/ForwardCommandController] active + +5. If you get output from above you can send commands to *Forward Command Controller*, either: a. Manually using ros2 cli interface: - ``` - ros2 topic pub /position_commands std_msgs/msg/Float64MultiArray "data: - - 0.5 - - 0.5" - ``` - B. Or you can start a demo node which sends two goals every 5 seconds in a loop: - ``` - ros2 launch ros2_control_demo_bringup test_forward_position_controller.launch.py - ``` - You should now see orange and yellow blocks moving in ``RViz``. - Also, you should see changing states in the terminal where launch file is started. - - -Files used for this demos: - - Launch file: [rrbot.launch.py](ros2_control_demo_bringup/launch/rrbot.launch.py) - - Controllers yaml: [rrbot_controllers.yaml](ros2_control_demo_bringup/config/rrbot_controllers.yaml) - - URDF file: [rrbot.urdf.xacro](ros2_control_demo_description/rrbot_description/urdf/rrbot.urdf.xacro) - - Description: [rrbot_description.urdf.xacro](ros2_control_demo_description/rrbot_description/urdf/rrbot_description.urdf.xacro) - - `ros2_control` tag: [rrbot.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot.ros2_control.xacro) - - RViz configuration: [rrbot.rviz](ros2_control_demo_description/rrbot_description/config/rrbot.rviz) - - - Hardware interface plugin: [rrbot_system_position_only.cpp](ros2_control_demo_hardware/src/rrbot_system_position_only.cpp) - - -Controllers from this demo: - - ``Joint State Broadcaster`` ([``ros2_controllers`` repository](https://github.com/ros-controls/ros2_controllers)): [doc](https://ros-controls.github.io/control.ros.org/ros2_controllers/joint_state_broadcaster/doc/userdoc.html) - - ``Forward Command Controller`` ([``ros2_controllers`` repository](https://github.com/ros-controls/ros2_controllers)): [doc](https://ros-controls.github.io/control.ros.org/ros2_controllers/forward_command_controller/doc/userdoc.html) + + .. code-block:: shell + + ros2 topic pub /position_commands std_msgs/msg/Float64MultiArray "data: + - 0.5 + - 0.5" + + B. Or you can start a demo node which sends two goals every 5 seconds in a loop + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_1 test_forward_position_controller.launch.py + + You should now see orange and yellow blocks moving in *RViz*. + Also, you should see changing states in the terminal where launch file is started, e.g. + + .. code-block:: shell + + [RRBotSystemPositionOnlyHardware]: Got command 0.50000 for joint 0! + [RRBotSystemPositionOnlyHardware]: Got command 0.50000 for joint 1! + + +Files used for this demos +######################### + +- Launch file: `rrbot.launch.py `__ +- Controllers yaml: `rrbot_controllers.yaml `__ +- URDF file: `rrbot.urdf.xacro `__ + + + Description: `rrbot_description.urdf.xacro `__ + + ``ros2_control`` tag: `rrbot.ros2_control.xacro `__ + +- RViz configuration: `rrbot.rviz `__ + +- Hardware interface plugin: `rrbot.cpp `__ + + +Controllers from this demo +########################## +- ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ +- ``Forward Command Controller`` (`ros2_controllers repository `__): `doc `__ diff --git a/example_1/bringup/config/rrbot_forward_position_publisher.yaml b/example_1/bringup/config/rrbot_forward_position_publisher.yaml new file mode 100644 index 000000000..d0fd6330a --- /dev/null +++ b/example_1/bringup/config/rrbot_forward_position_publisher.yaml @@ -0,0 +1,11 @@ +publisher_forward_position_controller: + ros__parameters: + + wait_sec_between_publish: 5 + publish_topic: "/forward_position_controller/commands" + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785] + pos2: [0, 0] + pos3: [-0.785, -0.785] + pos4: [0, 0] diff --git a/example_1/bringup/launch/rrbot.launch.py b/example_1/bringup/launch/rrbot.launch.py index 3ce2b90d8..504fd93b2 100644 --- a/example_1/bringup/launch/rrbot.launch.py +++ b/example_1/bringup/launch/rrbot.launch.py @@ -30,7 +30,7 @@ def generate_launch_description(): " ", PathJoinSubstitution( [ - FindPackageShare("rrbot_description"), + FindPackageShare("ros2_control_demo_example_1"), "urdf", "rrbot.urdf.xacro", ] @@ -41,25 +41,19 @@ def generate_launch_description(): robot_controllers = PathJoinSubstitution( [ - FindPackageShare("ros2_control_demo_bringup"), + FindPackageShare("ros2_control_demo_example_1"), "config", "rrbot_controllers.yaml", ] ) rviz_config_file = PathJoinSubstitution( - [FindPackageShare("rrbot_description"), "config", "rrbot.rviz"] + [FindPackageShare("ros2_control_demo_example_1"), "rviz", "rrbot.rviz"] ) control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[robot_description, robot_controllers], - remappings=[ - ( - "/forward_position_controller/commands", - "/position_commands", - ), - ], output="both", ) robot_state_pub_node = Node( diff --git a/ros2_control_demo_bringup/launch/test_forward_position_controller.launch.py b/example_1/bringup/launch/test_forward_position_controller.launch.py similarity index 91% rename from ros2_control_demo_bringup/launch/test_forward_position_controller.launch.py rename to example_1/bringup/launch/test_forward_position_controller.launch.py index 2dbb17b9d..3605c3446 100644 --- a/ros2_control_demo_bringup/launch/test_forward_position_controller.launch.py +++ b/example_1/bringup/launch/test_forward_position_controller.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): position_goals = PathJoinSubstitution( [ - FindPackageShare("ros2_control_demo_bringup"), + FindPackageShare("ros2_control_demo_example_1"), "config", "rrbot_forward_position_publisher.yaml", ] @@ -31,7 +31,7 @@ def generate_launch_description(): return LaunchDescription( [ Node( - package="ros2_control_test_nodes", + package="ros2_controllers_test_nodes", executable="publisher_forward_position_controller", name="publisher_forward_position_controller", parameters=[position_goals], diff --git a/ros2_control_demo_description/rrbot_description/launch/view_robot.launch.py b/example_1/description/launch/view_robot.launch.py similarity index 96% rename from ros2_control_demo_description/rrbot_description/launch/view_robot.launch.py rename to example_1/description/launch/view_robot.launch.py index 7d19ca4c5..5295d4531 100644 --- a/ros2_control_demo_description/rrbot_description/launch/view_robot.launch.py +++ b/example_1/description/launch/view_robot.launch.py @@ -26,7 +26,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "description_package", - default_value="rrbot_description", + default_value="ros2_control_demo_example_1", description="Description package with robot URDF/xacro files. Usually the argument \ is not set, it enables use of a custom description.", ) @@ -69,7 +69,7 @@ def generate_launch_description(): robot_description = {"robot_description": robot_description_content} rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "config", "rrbot.rviz"] + [FindPackageShare(description_package), "rviz", "rrbot.rviz"] ) joint_state_publisher_node = Node( diff --git a/ros2_control_demo_description/rrbot_description/meshes/hokuyo.dae b/example_1/description/meshes/hokuyo.dae similarity index 100% rename from ros2_control_demo_description/rrbot_description/meshes/hokuyo.dae rename to example_1/description/meshes/hokuyo.dae diff --git a/example_1/description/rrbot.ros2_control.xacro b/example_1/description/ros2_control/rrbot.ros2_control.xacro similarity index 91% rename from example_1/description/rrbot.ros2_control.xacro rename to example_1/description/ros2_control/rrbot.ros2_control.xacro index 5bfe39152..6c3b3f7ec 100644 --- a/example_1/description/rrbot.ros2_control.xacro +++ b/example_1/description/ros2_control/rrbot.ros2_control.xacro @@ -5,7 +5,7 @@ - ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + ros2_control_demo_example_1/RRBotSystemPositionOnlyHardware 0 3.0 100 diff --git a/example_1/description/rviz/rrbot.rviz b/example_1/description/rviz/rrbot.rviz new file mode 100644 index 000000000..0efbecc38 --- /dev/null +++ b/example_1/description/rviz/rrbot.rviz @@ -0,0 +1,205 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 1096 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 0.20000000298023224 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fts_broadcaster/wrench + Torque Arrow Scale: 0.20000000298023224 + Torque Color: 204; 204; 51 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 8.443930625915527 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.0044944556429982185 + Y: 1.0785865783691406 + Z: 2.4839563369750977 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.23039916157722473 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.150422096252441 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1379 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000004f0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000048000004f00000010101000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004f0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000048000004f0000000db01000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000784000004f000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 1470 diff --git a/example_1/description/urdf/rrbot.materials.xacro b/example_1/description/urdf/rrbot.materials.xacro new file mode 100644 index 000000000..eb8e0212c --- /dev/null +++ b/example_1/description/urdf/rrbot.materials.xacro @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_1/description/rrbot.urdf.xacro b/example_1/description/urdf/rrbot.urdf.xacro similarity index 71% rename from example_1/description/rrbot.urdf.xacro rename to example_1/description/urdf/rrbot.urdf.xacro index daa838f56..b519fc2e3 100644 --- a/example_1/description/rrbot.urdf.xacro +++ b/example_1/description/urdf/rrbot.urdf.xacro @@ -8,13 +8,13 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + - + - + diff --git a/example_1/description/rrbot_description.urdf.xacro b/example_1/description/urdf/rrbot_description.urdf.xacro similarity index 98% rename from example_1/description/rrbot_description.urdf.xacro rename to example_1/description/urdf/rrbot_description.urdf.xacro index 19a0c59c3..8c0673208 100644 --- a/example_1/description/rrbot_description.urdf.xacro +++ b/example_1/description/urdf/rrbot_description.urdf.xacro @@ -144,7 +144,7 @@ - + diff --git a/example_1/doc/rrbot.png b/example_1/doc/rrbot.png new file mode 100644 index 000000000..9f5980b2c Binary files /dev/null and b/example_1/doc/rrbot.png differ diff --git a/example_1/hardware/rrbot.hpp b/example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp similarity index 79% rename from example_1/hardware/rrbot.hpp rename to example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp index 1ba370fc3..46881e85a 100644 --- a/example_1/hardware/rrbot.hpp +++ b/example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXAMPLE_1__HARDWARE__RRBOT_HPP_ -#define EXAMPLE_1__HARDWARE__RRBOT_HPP_ +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_1__RRBOT_HPP_ +#define ROS2_CONTROL_DEMO_EXAMPLE_1__RRBOT_HPP_ #include #include @@ -26,42 +26,42 @@ #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "ros2_control_demo_hardware/visibility_control.h" +#include "ros2_control_demo_example_1/visibility_control.h" -namespace ros2_control_demo_hardware +namespace ros2_control_demo_example_1 { class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware); - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC std::vector export_state_interfaces() override; - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC std::vector export_command_interfaces() override; - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC hardware_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC hardware_interface::return_type read( const rclcpp::Time & time, const rclcpp::Duration & period) override; - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; @@ -76,6 +76,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa std::vector hw_states_; }; -} // namespace ros2_control_demo_hardware +} // namespace ros2_control_demo_example_1 -#endif // EXAMPLE_1__HARDWARE__RRBOT_HPP_ +#endif // ROS2_CONTROL_DEMO_EXAMPLE_1__RRBOT_HPP_ diff --git a/example_1/hardware/include/ros2_control_demo_example_1/visibility_control.h b/example_1/hardware/include/ros2_control_demo_example_1/visibility_control.h new file mode 100644 index 000000000..91529ae90 --- /dev/null +++ b/example_1/hardware/include/ros2_control_demo_example_1/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2021 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_1__VISIBILITY_CONTROL_H_ +#define ROS2_CONTROL_DEMO_EXAMPLE_1__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ROS2_CONTROL_DEMO_EXAMPLE_1_EXPORT __attribute__((dllexport)) +#define ROS2_CONTROL_DEMO_EXAMPLE_1_IMPORT __attribute__((dllimport)) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_1_EXPORT __declspec(dllexport) +#define ROS2_CONTROL_DEMO_EXAMPLE_1_IMPORT __declspec(dllimport) +#endif +#ifdef ROS2_CONTROL_DEMO_EXAMPLE_1_BUILDING_DLL +#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_1_EXPORT +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_1_IMPORT +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC_TYPE ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_1_LOCAL +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_1_EXPORT __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_1_IMPORT +#if __GNUC__ >= 4 +#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_1_LOCAL __attribute__((visibility("hidden"))) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_1_LOCAL +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC_TYPE +#endif + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_1__VISIBILITY_CONTROL_H_ diff --git a/example_1/hardware/rrbot.cpp b/example_1/hardware/rrbot.cpp index 06576229d..40b110be2 100644 --- a/example_1/hardware/rrbot.cpp +++ b/example_1/hardware/rrbot.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ros2_control_demos_example_1/hardware/rrbot.hpp" +#include "ros2_control_demo_example_1/rrbot.hpp" #include #include @@ -23,7 +23,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" -namespace ros2_control_demo_hardware +namespace ros2_control_demo_example_1 { hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( const hardware_interface::HardwareInfo & info) @@ -228,9 +228,9 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write( return hardware_interface::return_type::OK; } -} // namespace ros2_control_demo_hardware +} // namespace ros2_control_demo_example_1 #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - ros2_control_demo_hardware::RRBotSystemPositionOnlyHardware, hardware_interface::SystemInterface) + ros2_control_demo_example_1::RRBotSystemPositionOnlyHardware, hardware_interface::SystemInterface) diff --git a/example_1/package.xml b/example_1/package.xml index 866c80bab..b42c85c92 100644 --- a/example_1/package.xml +++ b/example_1/package.xml @@ -1,11 +1,14 @@ - ros2_control_demo_hardware + ros2_control_demo_example_1 0.0.0 - Demo package of `ros2_control` Hardware. This package provides example on how to define hardware of your own robot for ROS2 control. The package is used with `ros2_control_demo_bringup` from package `rrbot_description` and `diffbot_description`. + Demo package of `ros2_control` hardware for RRbot. - Denis Štogl + Dr.-Ing. Denis Štogl + Christoph Froehlich + + Dr.-Ing. Denis Štogl Apache-2.0 diff --git a/example_1/ros2_control_demo_example_1.xml b/example_1/ros2_control_demo_example_1.xml new file mode 100644 index 000000000..7a9a82409 --- /dev/null +++ b/example_1/ros2_control_demo_example_1.xml @@ -0,0 +1,9 @@ + + + + The ros2_control RRbot example using a system hardware interface-type. + + + diff --git a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml index 1456dc50a..3453e4d76 100644 --- a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml +++ b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml @@ -1,11 +1,4 @@ - - - The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1. - - diff --git a/ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py b/ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py deleted file mode 100644 index 92aedbbd1..000000000 --- a/ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py +++ /dev/null @@ -1,78 +0,0 @@ -# Copyright 2021 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. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import rclpy -from rclpy.node import Node - -from std_msgs.msg import Float64MultiArray - - -class PublisherForwardPosition(Node): - def __init__(self): - super().__init__("publisher_forward_position_controller") - # Declare all parameters - self.declare_parameter("controller_name", "forward_position_controller") - self.declare_parameter("wait_sec_between_publish", 5) - self.declare_parameter("goal_names", ["pos1", "pos2"]) - - # Read parameters - # controller_name = self.get_parameter("controller_name").value - wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value - goal_names = self.get_parameter("goal_names").value - - # Read all positions from parameters - self.goals = [] - for name in goal_names: - self.declare_parameter(name) - goal = self.get_parameter(name).value - if goal is None or len(goal) == 0: - raise Exception(f'Values for goal "{name}" not set!') - - float_goal = [] - for value in goal: - float_goal.append(float(value)) - self.goals.append(float_goal) - - publish_topic = "/position_commands" - - self.get_logger().info( - f'Publishing {len(goal_names)} goals on topic "{publish_topic}"\ - every {wait_sec_between_publish} s' - ) - - self.publisher_ = self.create_publisher(Float64MultiArray, publish_topic, 1) - self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback) - self.i = 0 - - def timer_callback(self): - msg = Float64MultiArray() - msg.data = self.goals[self.i] - self.get_logger().info(f'Publishing: "{msg.data}"') - self.publisher_.publish(msg) - self.i += 1 - self.i %= len(self.goals) - - -def main(args=None): - rclpy.init(args=args) - - publisher_forward_position = PublisherForwardPosition() - - rclpy.spin(publisher_forward_position) - publisher_forward_position.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main()