From b5ac45961ca70396d318dc20ac4d33267f7dd32d Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Wed, 10 Jan 2024 19:30:13 +0100 Subject: [PATCH] Remove unused repos The `cartesian_controller_examples` is now contained in the simulation package. The `joint_to_cartesian_controller` won't be ported to ROS2. That should be implemented with the new ROS2-control *chaining* mechanisms instead. --- cartesian_controller_examples/CMakeLists.txt | 208 -------------- cartesian_controller_examples/COLCON_IGNORE | 0 .../config/example_controllers.yaml | 154 ----------- .../config/example_hw_config.yaml | 14 - .../etc/examples.rviz | 255 ------------------ .../launch/examples.launch | 63 ----- cartesian_controller_examples/package.xml | 55 ---- .../urdf/robot.urdf.xacro | 254 ----------------- joint_to_cartesian_controller/CMakeLists.txt | 224 --------------- joint_to_cartesian_controller/COLCON_IGNORE | 0 joint_to_cartesian_controller/README.md | 78 ------ .../JointControllerAdapter.h | 95 ------- .../joint_to_cartesian_controller.h | 111 -------- .../joint_to_cartesian_controller_plugin.xml | 11 - joint_to_cartesian_controller/package.xml | 73 ----- .../src/JointControllerAdapter.cpp | 128 --------- .../src/joint_to_cartesian_controller.cpp | 211 --------------- 17 files changed, 1934 deletions(-) delete mode 100644 cartesian_controller_examples/CMakeLists.txt delete mode 100644 cartesian_controller_examples/COLCON_IGNORE delete mode 100644 cartesian_controller_examples/config/example_controllers.yaml delete mode 100644 cartesian_controller_examples/config/example_hw_config.yaml delete mode 100644 cartesian_controller_examples/etc/examples.rviz delete mode 100644 cartesian_controller_examples/launch/examples.launch delete mode 100644 cartesian_controller_examples/package.xml delete mode 100644 cartesian_controller_examples/urdf/robot.urdf.xacro delete mode 100644 joint_to_cartesian_controller/CMakeLists.txt delete mode 100644 joint_to_cartesian_controller/COLCON_IGNORE delete mode 100644 joint_to_cartesian_controller/README.md delete mode 100644 joint_to_cartesian_controller/include/joint_to_cartesian_controller/JointControllerAdapter.h delete mode 100644 joint_to_cartesian_controller/include/joint_to_cartesian_controller/joint_to_cartesian_controller.h delete mode 100644 joint_to_cartesian_controller/joint_to_cartesian_controller_plugin.xml delete mode 100644 joint_to_cartesian_controller/package.xml delete mode 100644 joint_to_cartesian_controller/src/JointControllerAdapter.cpp delete mode 100644 joint_to_cartesian_controller/src/joint_to_cartesian_controller.cpp diff --git a/cartesian_controller_examples/CMakeLists.txt b/cartesian_controller_examples/CMakeLists.txt deleted file mode 100644 index b2064f7b..00000000 --- a/cartesian_controller_examples/CMakeLists.txt +++ /dev/null @@ -1,208 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(cartesian_controller_examples) - -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++17) - -set(ADDITIONAL_COMPILE_OPTIONS -Wall -Wextra -Wpedantic -Wno-unused-parameter) -add_compile_options(${ADDITIONAL_COMPILE_OPTIONS}) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a run_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES cartesian_controller_examples -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include -# ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/cartesian_controller_examples.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/cartesian_controller_examples_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -# We build our integration tests on the example.launch, -# so make sure each component is installed. -install(DIRECTORY - config - etc - launch - urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_cartesian_controller_examples.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/cartesian_controller_examples/COLCON_IGNORE b/cartesian_controller_examples/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/cartesian_controller_examples/config/example_controllers.yaml b/cartesian_controller_examples/config/example_controllers.yaml deleted file mode 100644 index c126e37e..00000000 --- a/cartesian_controller_examples/config/example_controllers.yaml +++ /dev/null @@ -1,154 +0,0 @@ -my_cartesian_motion_controller: - type: "position_controllers/CartesianMotionController" - end_effector_link: "tool0" - robot_base_link: "base_link" - target_frame_topic: "target_frame" - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - pd_gains: - trans_x: {p: 10.0} - trans_y: {p: 10.0} - trans_z: {p: 10.0} - rot_x: {p: 1.0} - rot_y: {p: 1.0} - rot_z: {p: 1.0} - -my_cartesian_force_controller: - type: "position_controllers/CartesianForceController" - end_effector_link: "tool0" - robot_base_link: "base_link" - ft_sensor_ref_link: "sensor_link" - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - pd_gains: - trans_x: {p: 0.05} - trans_y: {p: 0.05} - trans_z: {p: 0.05} - rot_x: {p: 0.01} - rot_y: {p: 0.01} - rot_z: {p: 0.01} - -my_cartesian_compliance_controller: - type: "position_controllers/CartesianComplianceController" - end_effector_link: "tool0" # All links below must come before this link - robot_base_link: "base_link" - ft_sensor_ref_link: "sensor_link" - compliance_ref_link: "tool0" - target_frame_topic: "target_frame" - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - stiffness: # w.r.t. compliance_ref_link - trans_x: 500 - trans_y: 500 - trans_z: 500 - rot_x: 100 - rot_y: 100 - rot_z: 100 - - pd_gains: - trans_x: {p: 0.05} - trans_y: {p: 0.05} - trans_z: {p: 0.05} - rot_x: {p: 0.01} - rot_y: {p: 0.01} - rot_z: {p: 0.01} - -my_motion_control_handle: - type: "cartesian_controllers/MotionControlHandle" - end_effector_link: "tool0" - robot_base_link: "base_link" - target_frame_topic: "/my_cartesian_motion_controller/target_frame" - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - -my_joint_to_cartesian_controller: - type: "cartesian_controllers/JointControllerAdapter" - end_effector_link: "tool0" - robot_base_link: "base_link" - target_frame_topic: "/my_cartesian_motion_controller/target_frame" - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - joint_limits: - joint1: - has_velocity_limits: true - max_velocity: 3.15 - has_acceleration_limits: true - max_acceleration: 10.0 - joint2: - has_velocity_limits: true - max_velocity: 3.15 - has_acceleration_limits: true - max_acceleration: 10.0 - joint3: - has_velocity_limits: true - max_velocity: 3.15 - has_acceleration_limits: true - max_acceleration: 10.0 - joint4: - has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: true - max_acceleration: 10.0 - joint5: - has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: true - max_acceleration: 10.0 - joint6: - has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: true - max_acceleration: 10.0 - -my_joint_to_cartesian_controller/joint_trajectory_controller: - type: "position_controllers/JointTrajectoryController" - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - -joint_trajectory_controller: - type: "position_controllers/JointTrajectoryController" - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 diff --git a/cartesian_controller_examples/config/example_hw_config.yaml b/cartesian_controller_examples/config/example_hw_config.yaml deleted file mode 100644 index 517c0aad..00000000 --- a/cartesian_controller_examples/config/example_hw_config.yaml +++ /dev/null @@ -1,14 +0,0 @@ -generic_hw_control_loop: - loop_hz: 100 - cycle_time_error_threshold: 0.03 - -# Settings for ros_control hardware interface -hardware_interface: - sim_control_mode: 0 - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 diff --git a/cartesian_controller_examples/etc/examples.rviz b/cartesian_controller_examples/etc/examples.rviz deleted file mode 100644 index 41e7dc67..00000000 --- a/cartesian_controller_examples/etc/examples.rviz +++ /dev/null @@ -1,255 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /TF1/Frames1 - - /RobotModel1/Links1 - Splitter Ratio: 0.5114820003509521 - Tree Height: 455 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/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 - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - base_link: - Value: true - link1: - Value: true - link2: - Value: true - link3: - Value: true - link4: - Value: true - link5: - Value: true - link6: - Value: true - sensor_link: - Value: false - tool0: - Value: true - world: - Value: true - Marker Alpha: 1 - Marker Scale: 0.30000001192092896 - Name: TF - Show Arrows: false - Show Axes: true - Show Names: false - Tree: - world: - base_link: - link1: - link2: - link3: - link4: - link5: - link6: - sensor_link: - {} - tool0: - {} - Update Interval: 0 - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - 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 - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_link: - Alpha: 1 - Show Axes: false - Show Trail: false - tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Alpha: 1 - Axes Length: 0.07000000029802322 - Axes Radius: 0.009999999776482582 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: Pose - Queue Size: 10 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Axes - Topic: /target_frame - Unreliable: false - Value: true - - Class: rviz/InteractiveMarkers - Enable Transparency: false - Enabled: true - Name: InteractiveMarkers - Show Axes: true - Show Descriptions: false - Show Visual Aids: false - Update Topic: /my_motion_control_handle/update - Value: true - Enabled: true - Global Options: - Background Color: 116; 116; 116 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.649895668029785 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: -0.003465852700173855 - Y: -0.3048836588859558 - Z: 0.1462906450033188 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.29479700326919556 - Target Frame: - Yaw: 0.8704105615615845 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 752 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000252fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000252000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002dcfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002dc000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005700000003efc0100000002fb0000000800540069006d0065010000000000000570000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004140000025200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1392 - X: 319 - Y: 135 diff --git a/cartesian_controller_examples/launch/examples.launch b/cartesian_controller_examples/launch/examples.launch deleted file mode 100644 index c93953eb..00000000 --- a/cartesian_controller_examples/launch/examples.launch +++ /dev/null @@ -1,63 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/cartesian_controller_examples/package.xml b/cartesian_controller_examples/package.xml deleted file mode 100644 index 52a9cc5f..00000000 --- a/cartesian_controller_examples/package.xml +++ /dev/null @@ -1,55 +0,0 @@ - - - cartesian_controller_examples - 0.0.0 - The cartesian_controller_examples package - - - - scherzin - - - - - BSD-3-Clause - - - - - - https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers - - - - - - Stefan Scherzinger - - - - - - - - - - - xacro - rviz - joint_trajectory_controller - joint_state_controller - ros_control_boilerplate - controller_manager - robot_state_publisher - - - - catkin - - - - - - - - diff --git a/cartesian_controller_examples/urdf/robot.urdf.xacro b/cartesian_controller_examples/urdf/robot.urdf.xacro deleted file mode 100644 index f8704f9e..00000000 --- a/cartesian_controller_examples/urdf/robot.urdf.xacro +++ /dev/null @@ -1,254 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/joint_to_cartesian_controller/CMakeLists.txt b/joint_to_cartesian_controller/CMakeLists.txt deleted file mode 100644 index a5375766..00000000 --- a/joint_to_cartesian_controller/CMakeLists.txt +++ /dev/null @@ -1,224 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(joint_to_cartesian_controller) - -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++17) - -set(ADDITIONAL_COMPILE_OPTIONS -Wall -Wextra -Wpedantic -Wno-unused-parameter) -add_compile_options(${ADDITIONAL_COMPILE_OPTIONS}) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - cartesian_controller_base - controller_interface - controller_manager - geometry_msgs - hardware_interface - joint_limits_interface - kdl_parser - pluginlib - roscpp - sensor_msgs - urdf -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a run_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - joint_to_cartesian_controller - CATKIN_DEPENDS - cartesian_controller_base - controller_interface - controller_manager - geometry_msgs - hardware_interface - joint_limits_interface - kdl_parser - pluginlib - roscpp - sensor_msgs - urdf - -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -add_library(${PROJECT_NAME} - src/joint_to_cartesian_controller.cpp - src/JointControllerAdapter.cpp -) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/joint_to_cartesian_controller_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} -) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE -) - -## Mark other files for installation (e.g. launch and bag files, etc.) -install(FILES - joint_to_cartesian_controller_plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_joint_to_cartesian_controller.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/joint_to_cartesian_controller/COLCON_IGNORE b/joint_to_cartesian_controller/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/joint_to_cartesian_controller/README.md b/joint_to_cartesian_controller/README.md deleted file mode 100644 index 111a8f4b..00000000 --- a/joint_to_cartesian_controller/README.md +++ /dev/null @@ -1,78 +0,0 @@ -## Joint to Cartesian Controller -This adapter-controller basically transforms the joint trajectory commands from -a connected joint-based controller to target pose commands for Cartesian -controllers. It only works for controllers from the *position_controllers* family, e.g. *position_controllers/JointTrajectoryController*. - -## Example ## -Below is an example entry for a controller specific configuration using the ur10 robot. -```yaml -my_joint_to_cartesian_controller: - type: "cartesian_controllers/JointControllerAdapter" - end_effector_link: "tool0" - robot_base_link: "base_link" - target_frame_topic: "target_frame" - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - - joint_limits: - shoulder_pan_joint: - has_velocity_limits: true - max_velocity: 3.15 - has_acceleration_limits: true - max_acceleration: 10.0 - shoulder_lift_joint: - has_velocity_limits: true - max_velocity: 3.15 - has_acceleration_limits: true - max_acceleration: 10.0 - elbow_joint: - has_velocity_limits: true - max_velocity: 3.15 - has_acceleration_limits: true - max_acceleration: 10.0 - wrist_1_joint: - has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: true - max_acceleration: 10.0 - wrist_2_joint: - has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: true - max_acceleration: 10.0 - wrist_3_joint: - has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: true - max_acceleration: 10.0 -``` -Joint limits are necessary, because this adapter implements an internal hardware interface and a controller manager to manage connected controllers. -To adapt a joint-based controller to this adapter, you must specify and load the controller to the adapters namespace: -```yaml -my_joint_to_cartesian_controller/joint_trajectory_controller: - type: "position_controllers/JointTrajectoryController" - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint -``` -And in the launch file for the adapter: -```xml - - -``` -And for the connected joint-based controller: -```xml - - - -``` -Note the usage of the proper namespace! It is useful to start this joint-based controller on loading, so that the adapter starts publishing valid poses upon activation. diff --git a/joint_to_cartesian_controller/include/joint_to_cartesian_controller/JointControllerAdapter.h b/joint_to_cartesian_controller/include/joint_to_cartesian_controller/JointControllerAdapter.h deleted file mode 100644 index 7176a544..00000000 --- a/joint_to_cartesian_controller/include/joint_to_cartesian_controller/JointControllerAdapter.h +++ /dev/null @@ -1,95 +0,0 @@ -//////////////////////////////////////////////////////////////////////////////// -// Copyright 2019 FZI Research Center for Information Technology -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from this -// software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -//////////////////////////////////////////////////////////////////////////////// - -//----------------------------------------------------------------------------- -/*!\file JointControllerAdapter.h - * - * \author Stefan Scherzinger - * \date 2017/08/16 - * - */ -//----------------------------------------------------------------------------- - -#ifndef JOINT_CONTROLLER_ADAPTER_H_INCLUDED -#define JOINT_CONTROLLER_ADAPTER_H_INCLUDED - -// ROS -#include -#include - -// ROS control -#include -#include -#include -#include - -// KDL -#include - -// Other -#include - -namespace joint_to_cartesian_controller -{ -/** - * @brief A controller adapter in form of a ROS-control hardware interface - */ -class JointControllerAdapter : public hardware_interface::RobotHW -{ -public: - JointControllerAdapter(); - ~JointControllerAdapter(); - - bool init(const std::vector & handles, - ros::NodeHandle & nh); - - void write(KDL::JntArray & positions); - -private: - //! Number of actuated joints - size_t m_number_joints; - - //! Actuated joints in order from base to tip - std::vector m_joint_names; - - hardware_interface::JointStateInterface m_state_interface; - hardware_interface::PositionJointInterface m_pos_interface; - - joint_limits_interface::PositionJointSoftLimitsInterface m_limits_interface; - - std::vector m_joint_handles; - std::vector m_limits_handles; - - std::vector m_cmd; -}; - -} // namespace joint_to_cartesian_controller - -#endif diff --git a/joint_to_cartesian_controller/include/joint_to_cartesian_controller/joint_to_cartesian_controller.h b/joint_to_cartesian_controller/include/joint_to_cartesian_controller/joint_to_cartesian_controller.h deleted file mode 100644 index 48db2285..00000000 --- a/joint_to_cartesian_controller/include/joint_to_cartesian_controller/joint_to_cartesian_controller.h +++ /dev/null @@ -1,111 +0,0 @@ -//////////////////////////////////////////////////////////////////////////////// -// Copyright 2019 FZI Research Center for Information Technology -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from this -// software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -//////////////////////////////////////////////////////////////////////////////// - -//----------------------------------------------------------------------------- -/*!\file joint_to_cartesian_controller.h - * - * \author Stefan Scherzinger - * \date 2017/08/15 - * - */ -//----------------------------------------------------------------------------- - -#ifndef JOINT_TO_CARTESIAN_CONTROLLER_H_INCLUDED -#define JOINT_TO_CARTESIAN_CONTROLLER_H_INCLUDED - -// Project -#include - -// ROS -#include - -// ros_controls -#include -#include -#include - -// KDL -#include -#include - -namespace joint_to_cartesian_controller -{ -/** - * @brief A controller to turn joint trajectories into a moving Cartesian target pose - * - * Use this controller, if you have (for some reason) only joint-based - * trajectories and want to use Cartesian controllers from the - * cartesian_controller package. - * - * This controller handles an internal controller manager, which can load - * standard ros_controllers. The control commands from these controllers are - * turned into Cartesian poses with forward kinematics, and can be used by - * the Cartesian_controllers. An application of this controller is to - * provide an easy interface to the rqt_joint_trajectory_controller plugin and - * MoveIt!. - * - * Note, however, that transforming joint motion into Cartesian motion for - * target following loses explicit control over the joints and collision checking. - */ -class JointToCartesianController -: public controller_interface::Controller -{ -public: - JointToCartesianController(); - - bool init(hardware_interface::JointStateInterface * hw, ros::NodeHandle & nh); - - void starting(const ros::Time & time); - - void stopping(const ros::Time & time); - - void update(const ros::Time & time, const ros::Duration & period); - -private: - std::string m_end_effector_link; - std::string m_robot_base_link; - std::string m_target_frame_topic; - KDL::JntArray m_positions; - KDL::JntArray m_velocities; - std::vector m_joint_names; - ros::Publisher m_pose_publisher; - - JointControllerAdapter m_controller_adapter; - - std::vector m_joint_handles; - - std::shared_ptr m_fk_solver; - - std::shared_ptr m_controller_manager; -}; - -} // namespace joint_to_cartesian_controller - -#endif diff --git a/joint_to_cartesian_controller/joint_to_cartesian_controller_plugin.xml b/joint_to_cartesian_controller/joint_to_cartesian_controller_plugin.xml deleted file mode 100644 index cefa98d4..00000000 --- a/joint_to_cartesian_controller/joint_to_cartesian_controller_plugin.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - Convert joint commands into Cartesian end-effector poses. - - - - diff --git a/joint_to_cartesian_controller/package.xml b/joint_to_cartesian_controller/package.xml deleted file mode 100644 index 070bd5d1..00000000 --- a/joint_to_cartesian_controller/package.xml +++ /dev/null @@ -1,73 +0,0 @@ - - - joint_to_cartesian_controller - 0.0.0 - The joint_to_cartesian_controller package - - - - scherzin - - - - - BSD-3-Clause - - - - - - https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers - - - - - - Stefan Scherzinger - - - - - - - - - - - - - - catkin - - cartesian_controller_base - controller_interface - controller_manager - geometry_msgs - hardware_interface - joint_limits_interface - kdl_parser - pluginlib - roscpp - sensor_msgs - urdf - - cartesian_controller_base - controller_interface - controller_manager - geometry_msgs - hardware_interface - joint_limits_interface - kdl_parser - pluginlib - - roscpp - sensor_msgs - urdf - - - - - - - - diff --git a/joint_to_cartesian_controller/src/JointControllerAdapter.cpp b/joint_to_cartesian_controller/src/JointControllerAdapter.cpp deleted file mode 100644 index 05ed6b5e..00000000 --- a/joint_to_cartesian_controller/src/JointControllerAdapter.cpp +++ /dev/null @@ -1,128 +0,0 @@ -//////////////////////////////////////////////////////////////////////////////// -// Copyright 2019 FZI Research Center for Information Technology -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from this -// software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -//////////////////////////////////////////////////////////////////////////////// - -//----------------------------------------------------------------------------- -/*!\file JointControllerAdapter.cpp - * - * \author Stefan Scherzinger - * \date 2017/08/16 - * - */ -//----------------------------------------------------------------------------- - -// Project -#include - -// ROS control -#include - -// Other -#include - -namespace joint_to_cartesian_controller -{ -JointControllerAdapter::JointControllerAdapter() {} - -bool JointControllerAdapter::init( - const std::vector & state_handles, ros::NodeHandle & nh) -{ - for (size_t i = 0; i < state_handles.size(); ++i) - { - m_joint_names.push_back(state_handles[i].getName()); - } - m_number_joints = m_joint_names.size(); - m_cmd.resize(m_number_joints); - - // Start where you are - for (size_t i = 0; i < m_number_joints; ++i) - { - m_cmd[i] = state_handles[i].getPosition(); - } - - // Register external state_handles - for (size_t i = 0; i < m_number_joints; ++i) - { - m_state_interface.registerHandle(state_handles[i]); - } - registerInterface(&m_state_interface); - - // Initialize and register handles to the actuators - for (size_t i = 0; i < m_number_joints; ++i) - { - m_joint_handles.push_back( - hardware_interface::JointHandle(m_state_interface.getHandle(m_joint_names[i]), &m_cmd[i])); - - m_pos_interface.registerHandle(m_joint_handles[i]); - } - registerInterface(&m_pos_interface); - - // Read joint limits from param server - joint_limits_interface::JointLimits limits; - for (size_t i = 0; i < m_number_joints; ++i) - { - joint_limits_interface::getJointLimits(m_joint_names[i], nh, limits); - } - - joint_limits_interface::SoftJointLimits soft_limits; - - // Initialize and register handles to the joint limits - for (size_t i = 0; i < m_number_joints; ++i) - { - m_limits_handles.push_back(joint_limits_interface::PositionJointSoftLimitsHandle( - m_joint_handles[i], limits, - soft_limits // deliberately empty - )); - } - - for (size_t i = 0; i < m_number_joints; ++i) - { - m_limits_interface.registerHandle(m_limits_handles[i]); - } - - return true; -} - -JointControllerAdapter::~JointControllerAdapter() {} - -void JointControllerAdapter::write(KDL::JntArray & positions) -{ - if (static_cast(positions.data.size()) != m_cmd.size()) - { - throw std::runtime_error("Joint number mismatch!"); - } - - // Fill positions for forward kinematics - for (size_t i = 0; i < m_cmd.size(); ++i) - { - positions(i) = m_cmd[i]; - } -} - -} // namespace joint_to_cartesian_controller diff --git a/joint_to_cartesian_controller/src/joint_to_cartesian_controller.cpp b/joint_to_cartesian_controller/src/joint_to_cartesian_controller.cpp deleted file mode 100644 index a7f05d87..00000000 --- a/joint_to_cartesian_controller/src/joint_to_cartesian_controller.cpp +++ /dev/null @@ -1,211 +0,0 @@ -//////////////////////////////////////////////////////////////////////////////// -// Copyright 2019 FZI Research Center for Information Technology -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from this -// software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -//////////////////////////////////////////////////////////////////////////////// - -//----------------------------------------------------------------------------- -/*!\file joint_to_cartesian_controller.cpp - * - * \author Stefan Scherzinger - * \date 2017/08/15 - * - */ -//----------------------------------------------------------------------------- - -// Pluginlib -#include - -// Project -#include -#include - -// KDL -#include -#include - -// URDF -#include - -// Other -#include - -namespace cartesian_controllers -{ -/** - * @brief Connect joint-based controllers and transform their commands to Cartesian target poses - * - * This controller handles an internal controller manager, which can load - * standard ros_controllers. The control commands from these controllers are - * turned into Cartesian poses with forward kinematics, and can be used by - * the Cartesian_controllers. An application of this controller is to - * provide an easy interface to the rqt_joint_trajectory_controller plugin and - * MoveIt!. - */ -typedef joint_to_cartesian_controller::JointToCartesianController JointControllerAdapter; -} // namespace cartesian_controllers - -PLUGINLIB_EXPORT_CLASS(cartesian_controllers::JointControllerAdapter, - controller_interface::ControllerBase) - -namespace joint_to_cartesian_controller -{ -JointToCartesianController::JointToCartesianController() {} - -bool JointToCartesianController::init(hardware_interface::JointStateInterface * hw, - ros::NodeHandle & nh) -{ - std::string robot_description; - urdf::Model robot_model; - KDL::Tree robot_tree; - KDL::Chain robot_chain; - - // Get controller specific configuration - if (!nh.getParam("/robot_description", robot_description)) - { - ROS_ERROR("Failed to load '/robot_description' from parameter server"); - return false; - } - if (!nh.getParam("robot_base_link", m_robot_base_link)) - { - ROS_ERROR_STREAM("Failed to load " << nh.getNamespace() + "/robot_base_link" - << " from parameter server"); - return false; - } - if (!nh.getParam("end_effector_link", m_end_effector_link)) - { - ROS_ERROR_STREAM("Failed to load " << nh.getNamespace() + "/end_effector_link" - << " from parameter server"); - return false; - } - if (!nh.getParam("target_frame_topic", m_target_frame_topic)) - { - m_target_frame_topic = "target_frame"; - ROS_WARN_STREAM("Failed to load " - << nh.getNamespace() + "/target_frame_topic" - << " from parameter server. " - << "Will default to: " << nh.getNamespace() + m_target_frame_topic); - } - - // Publishers - m_pose_publisher = nh.advertise(m_target_frame_topic, 10); - - // Build a kinematic chain of the robot - if (!robot_model.initString(robot_description)) - { - ROS_ERROR("Failed to parse urdf model from 'robot_description'"); - return false; - } - if (!kdl_parser::treeFromUrdfModel(robot_model, robot_tree)) - { - const std::string error = - "" - "Failed to parse KDL tree from urdf model"; - ROS_ERROR_STREAM(error); - throw std::runtime_error(error); - } - if (!robot_tree.getChain(m_robot_base_link, m_end_effector_link, robot_chain)) - { - const std::string error = - "" - "Failed to parse robot chain from urdf model. " - "Are you sure that both your 'robot_base_link' and 'end_effector_link' exist?"; - ROS_ERROR_STREAM(error); - throw std::runtime_error(error); - } - - // Get names of controllable joints from the parameter server - if (!nh.getParam("joints", m_joint_names)) - { - const std::string error = - "" - "Failed to load " + - nh.getNamespace() + "/joints" + " from parameter server"; - ROS_ERROR_STREAM(error); - throw std::runtime_error(error); - } - - // Get the joint handles to use in the control loop - for (size_t i = 0; i < m_joint_names.size(); ++i) - { - m_joint_handles.push_back(hw->getHandle(m_joint_names[i])); - } - - // Adjust joint buffers - m_positions.data = ctrl::VectorND::Zero(m_joint_handles.size()); - m_velocities.data = ctrl::VectorND::Zero(m_joint_handles.size()); - - // Initialize controller adapter and according manager - m_controller_adapter.init(m_joint_handles, nh); - m_controller_manager.reset(new controller_manager::ControllerManager(&m_controller_adapter, nh)); - - // Initialize forward kinematics solver - m_fk_solver.reset(new KDL::ChainFkSolverPos_recursive(robot_chain)); - - return true; -} - -void JointToCartesianController::starting(const ros::Time & time) -{ - // Get current joint positions from hardware - for (size_t i = 0; i < m_joint_handles.size(); ++i) - { - m_positions(i) = m_joint_handles[i].getPosition(); - } -} - -void JointToCartesianController::stopping(const ros::Time & time) {} - -void JointToCartesianController::update(const ros::Time & time, const ros::Duration & period) -{ - // Note: The connected joint-based controller gets the feedback directly from - // the joint state handles of this joint_to_cartesian_controller. So, - // there's no need for a read() function. - - // Update connected joint controller - m_controller_manager->update(time, period); - - // Get commanded positions - m_controller_adapter.write(m_positions); - - // Solve forward kinematics - KDL::Frame frame; - m_fk_solver->JntToCart(m_positions, frame); - - // Publish end-effector pose - geometry_msgs::PoseStamped target_pose = geometry_msgs::PoseStamped(); - target_pose.header.stamp = ros::Time::now(); - target_pose.header.frame_id = m_robot_base_link; - target_pose.pose.position.x = frame.p.x(); - target_pose.pose.position.y = frame.p.y(); - target_pose.pose.position.z = frame.p.z(); - frame.M.GetQuaternion(target_pose.pose.orientation.x, target_pose.pose.orientation.y, - target_pose.pose.orientation.z, target_pose.pose.orientation.w); - m_pose_publisher.publish(target_pose); -} - -} // namespace joint_to_cartesian_controller