├── pi_hexapod_driver ├── pi_hexapod_gui │ ├── src │ │ └── pi_hexapod_gui │ │ │ └── __init__.py │ ├── setup.py │ ├── plugin.xml │ ├── package.xml │ ├── README.md │ ├── CMakeLists.txt │ ├── rqt │ │ └── rqt_reference_and_real_positions.perspective │ └── resource │ │ └── pi_hexapod_gui.ui ├── pi_hexapod_control │ ├── doc │ │ └── src │ │ │ └── architecture.png │ ├── lib │ │ ├── libpi_pi_gcs2-3.11.0.a │ │ └── libpi_pi_gcs2.so.3.11.0 │ ├── hardware_interface_plugin.xml │ ├── launch │ │ ├── rqt_gui.launch │ │ ├── interactive_marker.launch │ │ └── hexapod_common_control.launch │ ├── include │ │ └── pi_hexapod_control │ │ │ ├── types.h │ │ │ ├── visual_joint_generator.h │ │ │ ├── hardware_interface.h │ │ │ ├── interactive_marker_node.h │ │ │ ├── log.h │ │ │ └── pi_driver.h │ ├── cfg │ │ ├── hexapod_controllers.yaml │ │ └── default_hexapod.rviz │ ├── package.xml │ ├── src │ │ ├── hardware_interface_node.cpp │ │ ├── visual_joint_generator.cpp │ │ ├── pi_driver.cpp │ │ ├── hardware_interface.cpp │ │ └── interactive_marker_node.cpp │ ├── README.md │ └── CMakeLists.txt ├── pi_hexapod_description │ ├── meshes │ │ └── H-811 │ │ │ ├── platform.stl │ │ │ ├── baseplate.stl │ │ │ ├── H_811.I2_platform.stl │ │ │ ├── H-811.D2_LowerStrut.stl │ │ │ ├── H-811.D2_UpperStrut.stl │ │ │ ├── H-811.I2_LowerStrut.stl │ │ │ └── H-811.I2_UpperStrut.stl │ ├── README.md │ ├── urdf │ │ └── pi_hexapod.urdf.xacro │ ├── package.xml │ ├── CMakeLists.txt │ └── cfg │ │ └── H-811.i2.yaml └── pi_hexapod_msgs │ ├── action │ └── Referencing.action │ ├── README.md │ ├── package.xml │ └── CMakeLists.txt ├── README.md └── LICENSE /pi_hexapod_driver/pi_hexapod_gui/src/pi_hexapod_gui/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/doc/src/architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PI-PhysikInstrumente/PI_ROS_Driver/HEAD/pi_hexapod_driver/pi_hexapod_control/doc/src/architecture.png -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/lib/libpi_pi_gcs2-3.11.0.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PI-PhysikInstrumente/PI_ROS_Driver/HEAD/pi_hexapod_driver/pi_hexapod_control/lib/libpi_pi_gcs2-3.11.0.a -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/lib/libpi_pi_gcs2.so.3.11.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PI-PhysikInstrumente/PI_ROS_Driver/HEAD/pi_hexapod_driver/pi_hexapod_control/lib/libpi_pi_gcs2.so.3.11.0 -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_description/meshes/H-811/platform.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PI-PhysikInstrumente/PI_ROS_Driver/HEAD/pi_hexapod_driver/pi_hexapod_description/meshes/H-811/platform.stl -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_description/meshes/H-811/baseplate.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PI-PhysikInstrumente/PI_ROS_Driver/HEAD/pi_hexapod_driver/pi_hexapod_description/meshes/H-811/baseplate.stl -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_description/meshes/H-811/H_811.I2_platform.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PI-PhysikInstrumente/PI_ROS_Driver/HEAD/pi_hexapod_driver/pi_hexapod_description/meshes/H-811/H_811.I2_platform.stl -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_description/meshes/H-811/H-811.D2_LowerStrut.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PI-PhysikInstrumente/PI_ROS_Driver/HEAD/pi_hexapod_driver/pi_hexapod_description/meshes/H-811/H-811.D2_LowerStrut.stl -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_description/meshes/H-811/H-811.D2_UpperStrut.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PI-PhysikInstrumente/PI_ROS_Driver/HEAD/pi_hexapod_driver/pi_hexapod_description/meshes/H-811/H-811.D2_UpperStrut.stl -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_description/meshes/H-811/H-811.I2_LowerStrut.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PI-PhysikInstrumente/PI_ROS_Driver/HEAD/pi_hexapod_driver/pi_hexapod_description/meshes/H-811/H-811.I2_LowerStrut.stl -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_description/meshes/H-811/H-811.I2_UpperStrut.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PI-PhysikInstrumente/PI_ROS_Driver/HEAD/pi_hexapod_driver/pi_hexapod_description/meshes/H-811/H-811.I2_UpperStrut.stl -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_msgs/action/Referencing.action: -------------------------------------------------------------------------------- 1 | # This file describes a ROS Action to trigger the referencing movement of a PI Hexapod. 2 | 3 | # goal 4 | --- 5 | # result 6 | bool success 7 | --- 8 | # feedback 9 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_msgs/README.md: -------------------------------------------------------------------------------- 1 | ## pi_hexapod_msgs 2 | 3 | This package is a ROS catkin package to generate messages to be used with other pi_hexapod packages. 4 | 5 | ### License 6 | 7 | Please consider the license information provided with this repository. 8 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_gui/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | # fetch values from package.xml 5 | setup_args = generate_distutils_setup( 6 | packages=["pi_hexapod_gui"], package_dir={"": "src"} 7 | ) 8 | setup(**setup_args) 9 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_description/README.md: -------------------------------------------------------------------------------- 1 | ## pi_hexapod_description 2 | 3 | This package contains a macro to create robot description of Physik Instrumente (PI) hexapods. It also contains meshes and configuration files describing specific hexapod models. 4 | 5 | ### License 6 | 7 | Please consider the license information provided with this repository. 8 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/hardware_interface_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | PI Hexapod ROS Driver as plugin 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_gui/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A GUI plugin to operate PI Hexapods with a comfortable user interface. 5 | 6 | 7 | 8 | system-help 9 | User interface to operate PI Hexapods. 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_description/urdf/pi_hexapod.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/launch/rqt_gui.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pi_hexapod_description 4 | 1.0.5 5 | URDF descriptions for Physik Instrumente (PI) Hexapods 6 | 7 | "Physik Instrumente (PI) GmbH u. Co. KG" 8 | 9 | Apache-2.0 10 | 11 | Christian Eichmann 12 | Philip Keller 13 | 14 | catkin 15 | urdf 16 | xacro 17 | urdf 18 | xacro 19 | urdf 20 | xacro 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pi_hexapod_msgs 4 | 1.0.5 5 | Messages, Services and Actions for Physik Instrumente (PI) Hexapods 6 | 7 | "Physik Instrumente (PI) GmbH u. Co. KG" 8 | 9 | Apache-2.0 10 | 11 | Christian Eichmann 12 | Philip Keller 13 | 14 | catkin 15 | actionlib 16 | actionlib_msgs 17 | message_generation 18 | actionlib 19 | actionlib_msgs 20 | message_generation 21 | actionlib 22 | actionlib_msgs 23 | message_runtime 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_gui/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pi_hexapod_gui 4 | 1.0.5 5 | PI Hexapod Control GUI as plugin for rqt 6 | 7 | "Physik Instrumente (PI) GmbH u. Co. KG" 8 | 9 | Apache-2.0 10 | 11 | Christian Eichmann 12 | Philip Keller 13 | 14 | catkin 15 | rospy 16 | rqt_gui 17 | rqt_gui_py 18 | rospy 19 | rqt_gui 20 | rqt_gui_py 21 | rospy 22 | rqt_gui 23 | rqt_gui_py 24 | pi_hexapod_msgs 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/include/pi_hexapod_control/types.h: -------------------------------------------------------------------------------- 1 | // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- 2 | 3 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 4 | // This file is part of the PI-Hexapod Driver. 5 | // 6 | // 7 | // © Copyright 2020 Physik Instrumente (PI) GmbH & Co. KG, Karlsruhe, Germany 8 | // © Copyright 2020 FZI Forschungszentrum Informatik, Karlsruhe, Germany 9 | // -- END LICENSE BLOCK ------------------------------------------------ 10 | 11 | //---------------------------------------------------------------------- 12 | /*!\file 13 | * 14 | * This file contains type declarations used with the PiDriver. 15 | * 16 | * \author Christian Eichmann 17 | * \author Philip Keller 18 | * \date 2020-04-06 19 | * 20 | */ 21 | //---------------------------------------------------------------------- 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | namespace pi_hexapod_control { 29 | using vector6d_t = std::array; //!< An array of 6 double values. 30 | using vector36d_t = std::array; //!< An array of 36 double values. 31 | } // namespace pi_hexapod_control 32 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/launch/interactive_marker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_gui/README.md: -------------------------------------------------------------------------------- 1 | ## pi_hexapod_gui 2 | 3 | This package contains a the PI Hexapod Control GUI. Implemented as a rqt plugin it provides easy access to various control modes of hexapods with ROS controllers. 4 | 5 | A brief description of the relevant directories and their content: 6 | - **resource**: Here is the ui-file located. It has been created with QT-Creator and defines the layout, the components and the general appearance of the plugin. 7 | - **rqt**: This folder contains a pre-configured rqt perspective (gui_mainboard.perspective) known as "PI Hexapod Mainboard". 8 | - **src**: The implementation of the functionality connected to the rqt plugin called PI Hexapod Control GUI is located in this folder. 9 | 10 | ##### First use: 11 | - After building and sourcing your workspace the plugin (PiGuiPlugin) should be listed in `$rqt --list-plugins`. If not, try `$rqt --force-discover`. 12 | - Now either start `$rqt` and add the plugin via the "Plugins" menu on the top. Or start the PI Hexapod Control GUI plugin as a standalone via `$rqt --standalone pi_hexapod_gui`. With the second option you do not have the possibility to add other plugins to your perspective. 13 | 14 | ##### Using the PI Hexapod Mainboard: 15 | - This preconfigured rqt-perspective can be started via the "Perspectives" menu on the top of rqt. Select "Import" and navigate to the corresponding file. 16 | - The RViz-View in the upper left corner uses the default configuration. 17 | You can open the configuration provided with the pi\_hexapod\_driver by using the `File` -> `Open Config` or pressing Ctrl+O after clicking inside the RViz View. 18 | The configuration file is located in `pi_hexapod_control/cfg/default_hexapod.rviz`. 19 | --- 20 | 21 | ### License 22 | 23 | Please consider the license information provided with this repository. 24 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_gui/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pi_hexapod_gui) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | rqt_gui 13 | rqt_gui_py 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | catkin_python_setup() 24 | 25 | ################################### 26 | ## catkin specific configuration ## 27 | ################################### 28 | ## The catkin_package macro generates cmake config files for your package 29 | ## Declare things to be passed to dependent projects 30 | ## INCLUDE_DIRS: uncomment this if your package contains header files 31 | ## LIBRARIES: libraries you create in this project that dependent projects also need 32 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 33 | ## DEPENDS: system dependencies of this project that dependent projects also need 34 | catkin_package( 35 | # INCLUDE_DIRS include 36 | # LIBRARIES pi_hexapod_gui 37 | # CATKIN_DEPENDS rospy rqt_gui rqt_gui_py 38 | # DEPENDS system_lib 39 | ) 40 | 41 | ############ 42 | ## Install ## 43 | ############# 44 | 45 | ## Mark other files for installation (e.g. launch and bag files, etc.) 46 | install(PROGRAMS 47 | src/${PROJECT_NAME}/hexapod_gui_plugin.py 48 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 49 | ) 50 | 51 | install(DIRECTORY 52 | resource 53 | rqt 54 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 55 | ) 56 | 57 | install(FILES 58 | plugin.xml 59 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 60 | ) 61 | 62 | ############# 63 | ## Testing ## 64 | ############# 65 | 66 | ## Add gtest based cpp test target and link libraries 67 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_pi_hexapod_gui.cpp) 68 | # if(TARGET ${PROJECT_NAME}-test) 69 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 70 | # endif() 71 | 72 | ## Add folders to be run by python nosetests 73 | # catkin_add_nosetests(test) 74 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/cfg/hexapod_controllers.yaml: -------------------------------------------------------------------------------- 1 | # This file contains the configuration for different ROS Controllers 2 | # to be used with PI Hexapods. 3 | # It also serves as example on how to configure Controllers to be used 4 | # with this package. 5 | 6 | hardware_interface: 7 | controller_rate: &controller_rate 100 8 | # actuated 6D-Pose of Hexapod platform 9 | joints: &robot_joints 10 | - cart_x 11 | - cart_y 12 | - cart_z 13 | - ang_u 14 | - ang_v 15 | - ang_w 16 | # read-only, calculated joints to visualize actuators 17 | visual_joints: &visual_joints 18 | - axis0_base_anchor_joint_x 19 | - axis0_base_anchor_joint_y 20 | - axis0_base_anchor_joint_z 21 | - axis0_platform_anchor_joint_x 22 | - axis0_platform_anchor_joint_y 23 | - axis0_platform_anchor_joint_z 24 | - axis1_base_anchor_joint_x 25 | - axis1_base_anchor_joint_y 26 | - axis1_base_anchor_joint_z 27 | - axis1_platform_anchor_joint_x 28 | - axis1_platform_anchor_joint_y 29 | - axis1_platform_anchor_joint_z 30 | - axis2_base_anchor_joint_x 31 | - axis2_base_anchor_joint_y 32 | - axis2_base_anchor_joint_z 33 | - axis2_platform_anchor_joint_x 34 | - axis2_platform_anchor_joint_y 35 | - axis2_platform_anchor_joint_z 36 | - axis3_base_anchor_joint_x 37 | - axis3_base_anchor_joint_y 38 | - axis3_base_anchor_joint_z 39 | - axis3_platform_anchor_joint_x 40 | - axis3_platform_anchor_joint_y 41 | - axis3_platform_anchor_joint_z 42 | - axis4_base_anchor_joint_x 43 | - axis4_base_anchor_joint_y 44 | - axis4_base_anchor_joint_z 45 | - axis4_platform_anchor_joint_x 46 | - axis4_platform_anchor_joint_y 47 | - axis4_platform_anchor_joint_z 48 | - axis5_base_anchor_joint_x 49 | - axis5_base_anchor_joint_y 50 | - axis5_base_anchor_joint_z 51 | - axis5_platform_anchor_joint_x 52 | - axis5_platform_anchor_joint_y 53 | - axis5_platform_anchor_joint_z 54 | 55 | joint_state_controller: 56 | type: joint_state_controller/JointStateController 57 | publish_rate: *controller_rate 58 | 59 | joint_group_pos_controller: 60 | type: position_controllers/JointGroupPositionController 61 | joints: *robot_joints 62 | 63 | joint_trajectory_controller: 64 | type: position_controllers/JointTrajectoryController 65 | joints: *robot_joints 66 | 67 | cart_x_controller: 68 | type: position_controllers/JointPositionController 69 | joint: cart_x 70 | 71 | cart_y_controller: 72 | type: position_controllers/JointPositionController 73 | joint: cart_y 74 | 75 | cart_z_controller: 76 | type: position_controllers/JointPositionController 77 | joint: cart_z 78 | 79 | ang_u_controller: 80 | type: position_controllers/JointPositionController 81 | joint: ang_u 82 | 83 | ang_v_controller: 84 | type: position_controllers/JointPositionController 85 | joint: ang_v 86 | 87 | ang_w_controller: 88 | type: position_controllers/JointPositionController 89 | joint: ang_w 90 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pi_hexapod_msgs) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | actionlib 12 | actionlib_msgs 13 | message_generation 14 | ) 15 | 16 | 17 | ################################################ 18 | ## Declare ROS messages, services and actions ## 19 | ################################################ 20 | 21 | ## To declare and build messages, services or actions from within this 22 | ## package, follow these steps: 23 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 24 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 25 | ## * In the file package.xml: 26 | ## * add a build_depend tag for "message_generation" 27 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 28 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 29 | ## but can be declared for certainty nonetheless: 30 | ## * add a exec_depend tag for "message_runtime" 31 | ## * In this file (CMakeLists.txt): 32 | ## * add "message_generation" and every package in MSG_DEP_SET to 33 | ## find_package(catkin REQUIRED COMPONENTS ...) 34 | ## * add "message_runtime" and every package in MSG_DEP_SET to 35 | ## catkin_package(CATKIN_DEPENDS ...) 36 | ## * uncomment the add_*_files sections below as needed 37 | ## and list every .msg/.srv/.action file to be processed 38 | ## * uncomment the generate_messages entry below 39 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 40 | 41 | ## Generate messages in the 'msg' folder 42 | # add_message_files( 43 | # FILES 44 | # Message1.msg 45 | # Message2.msg 46 | # ) 47 | 48 | ## Generate services in the 'srv' folder 49 | # add_service_files( 50 | # FILES 51 | # Service1.srv 52 | # Service2.srv 53 | # ) 54 | 55 | ## Generate actions in the 'action' folder 56 | add_action_files( 57 | DIRECTORY action 58 | FILES Referencing.action 59 | ) 60 | 61 | ## Generate added messages and services with any dependencies listed here 62 | generate_messages( 63 | DEPENDENCIES 64 | actionlib_msgs 65 | ) 66 | 67 | ################################### 68 | ## catkin specific configuration ## 69 | ################################### 70 | ## The catkin_package macro generates cmake config files for your package 71 | ## Declare things to be passed to dependent projects 72 | ## INCLUDE_DIRS: uncomment this if your package contains header files 73 | ## LIBRARIES: libraries you create in this project that dependent projects also need 74 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 75 | ## DEPENDS: system dependencies of this project that dependent projects also need 76 | catkin_package( 77 | # INCLUDE_DIRS include 78 | # LIBRARIES 79 | CATKIN_DEPENDS 80 | actionlib_msgs 81 | message_runtime 82 | # DEPENDS system_lib 83 | ) 84 | 85 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/include/pi_hexapod_control/visual_joint_generator.h: -------------------------------------------------------------------------------- 1 | // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- 2 | 3 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 4 | // This file is part of the PI-Hexapod Driver. 5 | // 6 | // 7 | // © Copyright 2020 Physik Instrumente (PI) GmbH & Co. KG, Karlsruhe, Germany 8 | // © Copyright 2020 FZI Forschungszentrum Informatik, Karlsruhe, Germany 9 | // -- END LICENSE BLOCK ------------------------------------------------ 10 | 11 | //---------------------------------------------------------------------- 12 | /*!\file 13 | * 14 | * This file contains the VisualJointGenerator to calculate joint values for the visual joint of 15 | * PI Hexapods. 16 | * 17 | * \author Christian Eichmann 18 | * \author Philip Keller 19 | * \date 2020-04-06 20 | * 21 | */ 22 | //---------------------------------------------------------------------- 23 | #ifndef PI_HEXAPOD_CONTROL_VISUAL_JOINT_GENERATOR_H_INCLUDED 24 | #define PI_HEXAPOD_CONTROL_VISUAL_JOINT_GENERATOR_H_INCLUDED 25 | 26 | #include "Eigen/Dense" 27 | #include "Eigen/Geometry" 28 | #include "pi_hexapod_control/types.h" 29 | #include 30 | 31 | namespace pi_hexapod_control { 32 | 33 | /*! 34 | * \brief An helper class for calculating the anchor joints. 35 | */ 36 | class VisualJointGenerator 37 | { 38 | public: 39 | //! \brief Constructor 40 | VisualJointGenerator(); 41 | 42 | /*! 43 | * \brief Initializes the VisualJointGenerator, reads ROS params 44 | * 45 | * \param nh Root level ROS node handle 46 | * \param priv_nh ROS node handle for the node's namespace 47 | * 48 | * \returns indicates success 49 | */ 50 | bool init(ros::NodeHandle& nh, ros::NodeHandle& priv_nh); 51 | 52 | /*! 53 | * \brief calculates visual link angles for a given 6D platform pose 54 | * 55 | * \param current_pos current 6D pose of the platform 56 | * \param visual_link_pos array in which the calculated angles shall be stored 57 | * \return indicates success 58 | */ 59 | bool calculateVisualLinks(const vector6d_t& current_pos, vector36d_t& visual_link_pos); 60 | 61 | private: 62 | /*! 63 | * \brief Reads parameter of type double, writes log message on error 64 | * 65 | * \param priv_nh ROS node handle for the node's namespace 66 | * \param param_name name of parameter 67 | * \param value reference to store read parameter 68 | * \return indicates success 69 | */ 70 | bool getNamedParamDouble(ros::NodeHandle& priv_nh, const std::string& param_name, double& value); 71 | 72 | // allocate memory for all Eigen objects used for calculating visual links to save time 73 | std::array base_vector_; 74 | std::array platform_vector_; 75 | 76 | Eigen::Vector3d base_axis_; 77 | Eigen::Vector3d base_euler_; 78 | Eigen::Vector3d current_platform_vector_; 79 | Eigen::Vector3d platform_axis_; 80 | Eigen::Vector3d platform_euler_; 81 | Eigen::Vector3d platform_translation_; 82 | Eigen::Vector3d z_axis_; 83 | 84 | Eigen::Quaternion platform_rotation_; 85 | 86 | Eigen::Matrix3d rot_matrix_; 87 | }; 88 | 89 | } // namespace pi_hexapod_control 90 | #endif // ifndef PI_HEXAPOD_CONTROL_VISUAL_JOINT_GENERATOR_H_INCLUDED 91 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pi_hexapod_control 4 | 1.0.5 5 | Drivers for Physik Instrumente (PI) Hexapods 6 | 7 | "Physik Instrumente (PI) GmbH u. Co. KG" 8 | 9 | Apache-2.0 10 | 11 | Christian Eichmann 12 | Philip Keller 13 | 14 | catkin 15 | actionlib 16 | control_msgs 17 | controller_manager 18 | geometry_msgs 19 | hardware_interface 20 | libusb-1.0-dev 21 | interactive_markers 22 | pi_hexapod_msgs 23 | pluginlib 24 | position_controllers 25 | realtime_tools 26 | roscpp 27 | std_msgs 28 | std_srvs 29 | tf2_eigen 30 | zlib 31 | actionlib 32 | control_msgs 33 | controller_manager 34 | geometry_msgs 35 | hardware_interface 36 | interactive_markers 37 | libusb-1.0-dev 38 | pi_hexapod_msgs 39 | pluginlib 40 | realtime_tools 41 | roscpp 42 | std_msgs 43 | std_srvs 44 | tf2_eigen 45 | zlib 46 | actionlib 47 | controller_interface 48 | controller_manager 49 | geometry_msgs 50 | hardware_interface 51 | libusb-1.0-dev 52 | interactive_markers 53 | joint_state_controller 54 | joint_trajectory_controller 55 | message_runtime 56 | pi_hexapod_description 57 | pi_hexapod_gui 58 | pi_hexapod_msgs 59 | pluginlib 60 | position_controllers 61 | realtime_tools 62 | robot_state_publisher 63 | roscpp 64 | rqt_gui 65 | rviz 66 | std_msgs 67 | std_srvs 68 | xacro 69 | zlib 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_gui/rqt/rqt_reference_and_real_positions.perspective: -------------------------------------------------------------------------------- 1 | { 2 | "keys": {}, 3 | "groups": { 4 | "pluginmanager": { 5 | "keys": { 6 | "running-plugins": { 7 | "type": "repr", 8 | "repr": "{u'rqt_plot/Plot': [1]}" 9 | } 10 | }, 11 | "groups": { 12 | "plugin__rqt_plot__Plot__1": { 13 | "keys": {}, 14 | "groups": { 15 | "dock_widget__DataPlotWidget": { 16 | "keys": { 17 | "dockable": { 18 | "type": "repr", 19 | "repr": "True" 20 | }, 21 | "parent": { 22 | "type": "repr", 23 | "repr": "None" 24 | }, 25 | "dock_widget_title": { 26 | "type": "repr", 27 | "repr": "u'MatPlot'" 28 | } 29 | }, 30 | "groups": {} 31 | }, 32 | "plugin": { 33 | "keys": { 34 | "autoscroll": { 35 | "type": "repr", 36 | "repr": "True" 37 | }, 38 | "plot_type": { 39 | "type": "repr", 40 | "repr": "1" 41 | }, 42 | "topics": { 43 | "type": "repr", 44 | "repr": "[u'/joint_group_pos_controller/command/data[1]', u'/joint_states/position[41]', u'/joint_group_pos_controller/command/data[2]', u'/joint_group_pos_controller/command/data[3]', u'/joint_states/position[1]', u'/joint_states/position[0]', u'/joint_states/position[39]', u'/joint_states/position[40]', u'/joint_group_pos_controller/command/data[0]', u'/joint_states/position[2]', u'/joint_group_pos_controller/command/data[4]', u'/joint_group_pos_controller/command/data[5]']" 45 | }, 46 | "y_limits": { 47 | "type": "repr", 48 | "repr": "[-0.3665208882, 0.3665208882]" 49 | }, 50 | "x_limits": { 51 | "type": "repr", 52 | "repr": "[1022.6178402900696, 1042.6178402900696]" 53 | } 54 | }, 55 | "groups": {} 56 | } 57 | } 58 | } 59 | } 60 | }, 61 | "mainwindow": { 62 | "keys": { 63 | "geometry": { 64 | "type": "repr(QByteArray.hex)", 65 | "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb00020000000003c0000000120000077f00000424000003c2000000120000077d0000042200000001000000000780')", 66 | "pretty-print": " $ } \" " 67 | }, 68 | "state": { 69 | "type": "repr(QByteArray.hex)", 70 | "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd0000000100000003000003bc000003e7fc0100000001fb00000042007200710074005f0070006c006f0074005f005f0050006c006f0074005f005f0031005f005f00440061007400610050006c006f00740057006900640067006500740100000000000003bc0000011300ffffff000003bc0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", 71 | "pretty-print": " Brqt_plot__Plot__1__DataPlotWidget 6MinimizedDockWidgetsToolbar " 72 | } 73 | }, 74 | "groups": { 75 | "toolbar_areas": { 76 | "keys": { 77 | "MinimizedDockWidgetsToolbar": { 78 | "type": "repr", 79 | "repr": "8" 80 | } 81 | }, 82 | "groups": {} 83 | } 84 | } 85 | } 86 | } 87 | } -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/src/hardware_interface_node.cpp: -------------------------------------------------------------------------------- 1 | // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- 2 | 3 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 4 | // This file is part of the PI-Hexapod Driver. 5 | // 6 | // 7 | // © Copyright 2020 Physik Instrumente (PI) GmbH & Co. KG, Karlsruhe, Germany 8 | // © Copyright 2020 FZI Forschungszentrum Informatik, Karlsruhe, Germany 9 | // -- END LICENSE BLOCK ------------------------------------------------ 10 | 11 | //---------------------------------------------------------------------- 12 | /*!\file 13 | * 14 | * This file contains the Controller Manager for PI Hexapods. 15 | * 16 | * \author Christian Eichmann 17 | * \author Philip Keller 18 | * \date 2020-04-06 19 | * 20 | */ 21 | //---------------------------------------------------------------------- 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | //! global pointer to hardware interface; used in signal handler 31 | std::unique_ptr g_hw_interface; 32 | 33 | /*! 34 | * \brief Costum signal handler, stopping and disconnecting the hexapod on interupt 35 | * 36 | * \param signum signal 37 | */ 38 | void signalHandler(int signum) 39 | { 40 | // trying to halt Hexapod, if unsuccessful, try sending stop 41 | if (!g_hw_interface->haltHexapod()) 42 | { 43 | if (!g_hw_interface->stopHexapod()) 44 | { 45 | ROS_FATAL("Failed to stop Hexapod, hardware may still be running!"); 46 | } 47 | } 48 | 49 | g_hw_interface->disconnectHexapod(); 50 | 51 | ros::shutdown(); 52 | } 53 | 54 | /*! 55 | * \brief main function of this node 56 | * 57 | * Sets up ROS, starts async spinners for callback handling, 58 | creates hardware_interface and acts als controller manager main loop 59 | * 60 | * \param argc 61 | * \param argv 62 | * \return int 63 | */ 64 | int main(int argc, char** argv) 65 | { 66 | // Set up ROS. 67 | ros::init(argc, argv, "pi_hardware_interface", ros::init_options::NoSigintHandler); 68 | ros::AsyncSpinner spinner(2); 69 | spinner.start(); 70 | 71 | ros::NodeHandle nh; 72 | ros::NodeHandle nh_priv("~"); 73 | 74 | // register signal SIGINT and signal handler 75 | signal(SIGINT, signalHandler); 76 | 77 | // get controller_rate from parameter server 78 | double controller_rate = nh.param("hardware_interface/controller_rate", 100.0); 79 | if (controller_rate <= 0) 80 | { 81 | ROS_WARN_STREAM("Invalid controller_rate of " << controller_rate << ", using 100 instead."); 82 | controller_rate = 100.0; 83 | } 84 | 85 | // Setup controller manager and hardware interface 86 | g_hw_interface.reset(new pi_hexapod_control::HardwareInterface); 87 | if (!g_hw_interface->init(nh, nh_priv)) 88 | { 89 | ROS_ERROR_STREAM("Initialization of hexapod failed."); 90 | exit(1); 91 | } 92 | ROS_DEBUG_STREAM("Initialized hardware interface."); 93 | controller_manager::ControllerManager cm(g_hw_interface.get(), nh); 94 | 95 | ros::Rate rate(controller_rate); 96 | ros::Time prev_time = ros::Time::now(); 97 | 98 | while (!ros::isShuttingDown()) 99 | { 100 | const ros::Time time = ros::Time::now(); 101 | const ros::Duration period = time - prev_time; 102 | 103 | g_hw_interface->read(time, period); 104 | cm.update(time, period); 105 | g_hw_interface->write(time, period); 106 | 107 | prev_time = time; 108 | rate.sleep(); 109 | } 110 | 111 | spinner.stop(); 112 | 113 | g_hw_interface.reset(); 114 | 115 | ROS_INFO_STREAM_NAMED("hardware_interface", "Shutting down."); 116 | return 0; 117 | } 118 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/launch/hexapod_common_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 59 | 60 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/src/visual_joint_generator.cpp: -------------------------------------------------------------------------------- 1 | // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- 2 | 3 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 4 | // This file is part of the PI-Hexapod Driver. 5 | // 6 | // 7 | // © Copyright 2020 Physik Instrumente (PI) GmbH & Co. KG, Karlsruhe, Germany 8 | // © Copyright 2020 FZI Forschungszentrum Informatik, Karlsruhe, Germany 9 | // -- END LICENSE BLOCK ------------------------------------------------ 10 | 11 | //---------------------------------------------------------------------- 12 | /*!\file 13 | * 14 | * This file contains the implementation of VisualJointGenerator. 15 | * 16 | * \author Christian Eichmann 17 | * \author Philip Keller 18 | * \date 2020-04-29 19 | * 20 | */ 21 | //---------------------------------------------------------------------- 22 | 23 | #include "pi_hexapod_control/visual_joint_generator.h" 24 | #include 25 | 26 | namespace pi_hexapod_control { 27 | 28 | VisualJointGenerator::VisualJointGenerator() 29 | : z_axis_(0.0, 0.0, 1.0){}; 30 | 31 | bool VisualJointGenerator::getNamedParamDouble(ros::NodeHandle& priv_nh, 32 | const std::string& param_name, 33 | double& value) 34 | { 35 | if (priv_nh.getParam(param_name, value)) 36 | { 37 | return true; 38 | } 39 | 40 | ROS_ERROR_STREAM("Cannot find required parameter " << priv_nh.resolveName(param_name) 41 | << " on the parameter server."); 42 | return false; 43 | } 44 | 45 | bool VisualJointGenerator::init(ros::NodeHandle& nh, ros::NodeHandle& priv_nh) 46 | { 47 | const int x{0}, y{1}, z{2}; 48 | 49 | bool ret_val = true; 50 | 51 | // for every axis, create base and platform vector from parameter 52 | for (size_t axis_id = 0; axis_id < 6; axis_id++) 53 | { 54 | const std::string axis_string = "axis_" + std::to_string(axis_id); 55 | ret_val = 56 | ret_val && 57 | getNamedParamDouble(priv_nh, axis_string + "/base_anchor/x", base_vector_[axis_id][x]) && 58 | getNamedParamDouble(priv_nh, axis_string + "/base_anchor/y", base_vector_[axis_id][y]) && 59 | getNamedParamDouble(priv_nh, axis_string + "/base_anchor/z", base_vector_[axis_id][z]) && 60 | getNamedParamDouble( 61 | priv_nh, axis_string + "/platform_anchor/x", platform_vector_[axis_id][x]) && 62 | getNamedParamDouble( 63 | priv_nh, axis_string + "/platform_anchor/y", platform_vector_[axis_id][y]) && 64 | getNamedParamDouble( 65 | priv_nh, axis_string + "/platform_anchor/z", platform_vector_[axis_id][z]); 66 | } 67 | 68 | // return true, if all parameters were read successful 69 | return ret_val; 70 | }; 71 | 72 | 73 | bool VisualJointGenerator::calculateVisualLinks(const vector6d_t& current_pos, 74 | vector36d_t& visual_link_pos) 75 | { 76 | // current platform position as translation vector 77 | platform_translation_ << current_pos[0], current_pos[1], current_pos[2]; 78 | 79 | // current platform rotation from RPY 80 | platform_rotation_ = Eigen::AngleAxisd(current_pos[3], Eigen::Vector3d::UnitX()) * 81 | Eigen::AngleAxisd(current_pos[4], Eigen::Vector3d::UnitY()) * 82 | Eigen::AngleAxisd(current_pos[5], Eigen::Vector3d::UnitZ()); 83 | 84 | for (size_t axis_id = 0; axis_id < 6; axis_id++) 85 | { 86 | // calculate platform anchor position in relation to zero_frame 87 | // dependent on current translation and rotation 88 | current_platform_vector_ = 89 | platform_translation_ + platform_rotation_ * platform_vector_[axis_id]; 90 | 91 | // calculate vector from base anchor point to current platform anchor point 92 | base_axis_ = current_platform_vector_ - base_vector_[axis_id]; 93 | // calculate euler angles between base_axis_ and z_axis_ (0, 0, 1) 94 | base_euler_ = Eigen::Quaternion::FromTwoVectors(z_axis_, base_axis_) 95 | .toRotationMatrix() 96 | .eulerAngles(0, 1, 2); 97 | // Use calculated angles as joint states for visual links 98 | visual_link_pos[6 * axis_id + 0] = base_euler_[0]; 99 | visual_link_pos[6 * axis_id + 1] = base_euler_[1]; 100 | visual_link_pos[6 * axis_id + 2] = base_euler_[2]; 101 | 102 | // calculate vector from current platform anchor point to base anchor point 103 | platform_axis_ = base_vector_[axis_id] - current_platform_vector_; 104 | // calculate euler angles between z_axis_ (0, 0, 1) and platform_axis_ 105 | rot_matrix_ = 106 | Eigen::Quaternion::FromTwoVectors(z_axis_, platform_axis_).toRotationMatrix(); 107 | platform_euler_ = (platform_rotation_.inverse() * rot_matrix_).eulerAngles(0, 1, 2); 108 | // Use calculated angles as joint states for visual links 109 | visual_link_pos[6 * axis_id + 3] = platform_euler_[0]; 110 | visual_link_pos[6 * axis_id + 4] = platform_euler_[1]; 111 | visual_link_pos[6 * axis_id + 5] = 0.0; // axis rotation in Z is fixed 112 | } 113 | return true; 114 | }; 115 | 116 | 117 | } // namespace pi_hexapod_control 118 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/include/pi_hexapod_control/hardware_interface.h: -------------------------------------------------------------------------------- 1 | // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- 2 | 3 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 4 | // This file is part of the PI-Hexapod Driver. 5 | // 6 | // 7 | // © Copyright 2020 Physik Instrumente (PI) GmbH & Co. KG, Karlsruhe, Germany 8 | // © Copyright 2020 FZI Forschungszentrum Informatik, Karlsruhe, Germany 9 | // -- END LICENSE BLOCK ------------------------------------------------ 10 | 11 | //---------------------------------------------------------------------- 12 | /*!\file 13 | * 14 | * This file contains the HardwareInterface (hardware_interface::RobotHW) class for PI Hexapods. 15 | * 16 | * \author Christian Eichmann 17 | * \author Philip Keller 18 | * \date 2020-04-06 19 | * 20 | */ 21 | //---------------------------------------------------------------------- 22 | #ifndef PI_HEXAPOD_CONTROL_HARDWARE_INTERFACE_H_INCLUDED 23 | #define PI_HEXAPOD_CONTROL_HARDWARE_INTERFACE_H_INCLUDED 24 | 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include "pi_hexapod_control/pi_driver.h" 41 | 42 | namespace pi_hexapod_control { 43 | 44 | /*! 45 | * \brief Hardware Interface to control a Physik Instrumente (PI) Hexapod 46 | */ 47 | class HardwareInterface : public hardware_interface::RobotHW 48 | { 49 | public: 50 | /*! 51 | * \brief Creates a new HardwareInterface object. 52 | */ 53 | HardwareInterface(); 54 | virtual ~HardwareInterface() = default; 55 | 56 | /*! 57 | * \brief Initializes HardwareInterface, reads ROS params 58 | * 59 | * \param root_nh Root level ROS node handle 60 | * \param robot_hw_nh ROS node handle for the robot namespace 61 | * 62 | * \returns 63 | */ 64 | virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override; 65 | 66 | /*! 67 | * \brief Reads joint values from Hexapod 68 | * 69 | * \param time Current time 70 | * \param period Duration of current control loop iteration 71 | */ 72 | virtual void read(const ros::Time& time, const ros::Duration& period) override; 73 | 74 | /*! 75 | * \brief Sends command joint values to Hexapod 76 | * 77 | * \param time Current time 78 | * \param period Duration of current control loop iteration 79 | */ 80 | virtual void write(const ros::Time& time, const ros::Duration& period) override; 81 | 82 | /*! 83 | * \brief Checks whether a proposed controller switch is feasable 84 | * 85 | * \param start_list List of controllers to start 86 | * \param stop_list List of controllers to stop 87 | * 88 | * \returns 89 | */ 90 | virtual bool 91 | prepareSwitch(const std::list& start_list, 92 | const std::list& stop_list) override; 93 | /*! 94 | * \brief Switches Controllers 95 | * 96 | * \param start_list List of controllers to start 97 | * \param stop_list List of controllers to stop 98 | */ 99 | virtual void doSwitch(const std::list& start_list, 100 | const std::list& stop_list) override; 101 | 102 | /*! 103 | * \brief Sends HALT command to Hexapod Controller. 104 | * 105 | * \returns True on successful request 106 | */ 107 | bool haltHexapod(); 108 | 109 | /*! 110 | * \brief Sends STOP command to Hexapod Controller. 111 | * 112 | * \returns True on successful request 113 | */ 114 | bool stopHexapod(); 115 | 116 | /*! 117 | * \brief Disconnects the Hexapod 118 | */ 119 | void disconnectHexapod(); 120 | 121 | /*! 122 | * \brief Callback to enable/disable active control of the Hexapod via ROS Service 123 | * 124 | * \param req SetBool request with the desired control mode 125 | * \param res True on successful request 126 | * 127 | * \returns True when callback finished 128 | */ 129 | bool enableServiceCallback(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res); 130 | 131 | /*! 132 | * \brief Callback to "halt" the Hexapod via ROS Service 133 | * 134 | * \param req Trigger request 135 | * \param res True on successful request 136 | * 137 | * \returns True when callback finished 138 | */ 139 | bool haltServiceCallback(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res); 140 | 141 | /*! 142 | * \brief Callback to "stop" the Hexapod via ROS Service 143 | * 144 | * \param req Trigger request 145 | * \param res True on successful request 146 | * 147 | * \returns True when callback finished 148 | */ 149 | bool stopServiceCallback(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res); 150 | 151 | protected: 152 | /*! 153 | * \brief Callback to reference Hexapod via ROS Action 154 | * 155 | * \param goal An action goal message 156 | */ 157 | void referencingActionCallback(const pi_hexapod_msgs::ReferencingGoalConstPtr& goal); 158 | 159 | /*! 160 | * \brief Publishes the ID and Message belonging to the current error of the Driver 161 | * 162 | */ 163 | void publishDriverStatus(); 164 | 165 | private: 166 | std::unique_ptr pi_driver_; 167 | 168 | std::unique_ptr > 169 | referencing_action_server_; 170 | ros::ServiceServer enable_srv_; 171 | ros::ServiceServer halt_srv_; 172 | ros::ServiceServer stop_srv_; 173 | 174 | ros::Publisher pub_error_id_; 175 | ros::Publisher pub_error_msg_; 176 | 177 | hardware_interface::JointStateInterface js_interface_; 178 | hardware_interface::PositionJointInterface pj_interface_; 179 | 180 | std::vector joint_names_; 181 | vector6d_t joint_position_command_; 182 | vector6d_t joint_positions_; 183 | vector6d_t joint_velocities_; 184 | vector6d_t joint_efforts_; 185 | 186 | std::vector visual_joint_names_; 187 | vector36d_t visual_joint_positions_; 188 | vector36d_t visual_joint_velocities_; 189 | vector36d_t visual_joint_efforts_; 190 | 191 | 192 | volatile bool control_mode_enabled_; 193 | volatile bool position_controller_running_; 194 | bool referencing_running_; 195 | bool prefer_stop_over_halt_; 196 | int latest_error_id_; 197 | 198 | std::string hexapod_ip_; 199 | std::string tf_prefix_; 200 | 201 | VisualJointGenerator visual_joint_generator_; 202 | }; 203 | 204 | } // namespace pi_hexapod_control 205 | 206 | #endif // ifndef PI_HEXAPOD_CONTROL_HARDWARE_INTERFACE_H_INCLUDED 207 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/include/pi_hexapod_control/interactive_marker_node.h: -------------------------------------------------------------------------------- 1 | // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- 2 | 3 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 4 | // This file is part of the PI-Hexapod Driver. 5 | // 6 | // 7 | // © Copyright 2020 Physik Instrumente (PI) GmbH & Co. KG, Karlsruhe, Germany 8 | // © Copyright 2020 FZI Forschungszentrum Informatik, Karlsruhe, Germany 9 | // -- END LICENSE BLOCK ------------------------------------------------ 10 | 11 | //---------------------------------------------------------------------- 12 | /*!\file 13 | * 14 | * This file contains the InteractiveMarkerNode which provides an interactive marker server 15 | * to control PI Hexapods. 16 | * 17 | * \author Christian Eichmann 18 | * \author Philip Keller 19 | * \date 2020-05-06 20 | * 21 | */ 22 | //---------------------------------------------------------------------- 23 | #ifndef PI_HEXAPOD_CONTROL_INTERACTIVE_MARKER_NODE_H_INCLUDED 24 | #define PI_HEXAPOD_CONTROL_INTERACTIVE_MARKER_NODE_H_INCLUDED 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace pi_hexapod_control { 37 | 38 | /** 39 | * \brief Node providing an Interactive Marker to control a hexapod. 40 | * 41 | * Provides an Interactive Marker to control a PI hexapod using RViz. 42 | * Uses a JointGroupPositionController to send commands. 43 | * Requires a hexapod configuration (YAML) file. 44 | */ 45 | class InteractiveMarkerNode 46 | { 47 | public: 48 | InteractiveMarkerNode(); 49 | ~InteractiveMarkerNode() = default; 50 | 51 | /** 52 | * \brief Callback to receive joint state messages 53 | * 54 | * \param msg a joint state message 55 | */ 56 | void jointStateCallback(const sensor_msgs::JointState::ConstPtr& msg); 57 | 58 | /** 59 | * \brief initializes the Node. 60 | * 61 | * Reads configuration from the parameter server and sets up the marker server, publishers 62 | * and callbacks. 63 | * 64 | * \param nh Root NodeHandle 65 | * \param private_nh NodeHandle in the nodes namespace 66 | * 67 | * \return indicates success 68 | */ 69 | bool init(ros::NodeHandle& nh, ros::NodeHandle& private_nh); 70 | 71 | /** 72 | * \brief Handles marker updates from translations 73 | * 74 | * \param feedback new marker attributes 75 | * 76 | */ 77 | void 78 | translationMarkerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 79 | 80 | /** 81 | * \brief Handles marker updates from rotations 82 | * 83 | * \param axis_id ID of the axis which was rotated 84 | * \param feedback new marker attributes 85 | */ 86 | void 87 | rotationMarkerCallback(const int32_t axis_id, 88 | const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 89 | 90 | /** 91 | * \brief Sends commands to Hexapod. 92 | * 93 | * Sends marker position to topic and commands to the Controller. 94 | * Timer rate can be set as parameter. 95 | * 96 | * \param timer_event 97 | */ 98 | void timerCallback(const ros::TimerEvent& timer_event); 99 | 100 | /** 101 | * \brief Service callback to toggle command publication 102 | * 103 | * \param req 104 | * \param res 105 | * \return indicates success of the call itself, not of the operation 106 | */ 107 | bool activateCommandPublishingCallback(std_srvs::SetBool::Request& req, 108 | std_srvs::SetBool::Response& res); 109 | 110 | /** 111 | * \brief Service callback to set marker position to zero 112 | * 113 | * \param req 114 | * \param res 115 | * \return indicates success of the call itself, not of the operation 116 | * 117 | */ 118 | bool setToZeroCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); 119 | 120 | /** 121 | * \brief Service callback to set marker position to current position based on joint states 122 | * 123 | * \param req 124 | * \param res 125 | * \return indicates success of the call itself, not of the operation 126 | */ 127 | bool setToCurrentCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); 128 | 129 | /** 130 | * \brief Sends current marker position as command to Hexapod Controller 131 | * 132 | * \param req 133 | * \param res 134 | * \return indicates success of the call itself, not of the operation 135 | */ 136 | bool sendSingleCommandCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); 137 | 138 | /** 139 | * \brief Set internal command publication state and update menu 140 | * 141 | * \param new_state true for command publication 142 | * \return indicates success of the call itself, not of the operation 143 | */ 144 | void setPublishingCommands(const bool new_state); 145 | 146 | /** 147 | * \brief Set marker position to zero 148 | * 149 | * \param translation new replacements in X, Y and Z direction 150 | * \param angles new rotations around X, Y and Z axis 151 | */ 152 | void setToPosition(const Eigen::Array3d& translation, const Eigen::Array3d& angles); 153 | 154 | private: 155 | const int32_t ROLL_AXIS_ID = 0; 156 | const int32_t PITCH_AXIS_ID = 1; 157 | const int32_t YAW_AXIS_ID = 2; 158 | 159 | const std::string TRANSLATION_MARKER = "translation_marker"; 160 | const std::string VISUALIZATION_MARKER = "visualization_marker"; 161 | 162 | // initialize with double curly braces due to bug in some GCC versions for xenial (e.g. 5.4.0) 163 | const std::array MAKER_NAME_FROM_AXIS = { 164 | {"roll_marker", "pitch_marker", "yaw_maker"}}; 165 | 166 | void updatePoseMsg(); 167 | void publishCommand(); 168 | 169 | void initMenu(); 170 | void setToZeroMenuCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 171 | void 172 | setToCurrentMenuCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 173 | 174 | void menuPublishCommandsHandleCallback( 175 | const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 176 | 177 | interactive_markers::MenuHandler menu_handler_; 178 | 179 | interactive_markers::MenuHandler::EntryHandle menu_publish_commands_handle_; 180 | 181 | ros::Timer timer_; 182 | std::shared_ptr server_; 183 | ros::Publisher pose_publisher_; 184 | ros::Publisher command_publisher_; 185 | ros::ServiceServer activate_command_publishing_server_; 186 | ros::ServiceServer set_to_zero_server_; 187 | ros::ServiceServer set_to_current_server_; 188 | ros::ServiceServer send_single_command_server_; 189 | ros::Subscriber joint_state_subscriber_; 190 | 191 | 192 | double rate_; 193 | double control_scale_; 194 | bool joint_state_received_; 195 | bool is_publishing_commands_; 196 | std::string base_frame_; 197 | std::string topic_ns_; 198 | geometry_msgs::PoseStamped pose_msg_; 199 | 200 | Eigen::Array3d received_joint_state_translation_; 201 | Eigen::Array3d received_joint_state_angles_; 202 | 203 | Eigen::Array3d translation_; 204 | Eigen::Array3d angles_; 205 | 206 | Eigen::Array3d lower_translation_limits_; 207 | Eigen::Array3d upper_translation_limits_; 208 | Eigen::Array3d lower_angle_limits_; 209 | Eigen::Array3d upper_angle_limits_; 210 | }; 211 | 212 | } // namespace pi_hexapod_control 213 | 214 | #endif // ifndef PI_HEXAPOD_CONTROL_INTERACTIVE_MARKER_NODE_H_INCLUDED 215 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/include/pi_hexapod_control/log.h: -------------------------------------------------------------------------------- 1 | // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- 2 | 3 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 4 | // This file is part of the PI-Hexapod Driver. 5 | // 6 | // 7 | // © Copyright 2020 Physik Instrumente (PI) GmbH & Co. KG, Karlsruhe, Germany 8 | // © Copyright 2020 FZI Forschungszentrum Informatik, Karlsruhe, Germany 9 | // -- END LICENSE BLOCK ------------------------------------------------ 10 | 11 | //---------------------------------------------------------------------- 12 | /*!\file 13 | * 14 | * This file defines Log Makros for the PiDriver. 15 | * Swapping this definitions makes it possible to change the drivers logging easily. 16 | * 17 | * \author Christian Eichmann 18 | * \author Philip Keller 19 | * \date 2020-04-06 20 | * 21 | */ 22 | //---------------------------------------------------------------------- 23 | 24 | #include 25 | 26 | //@cond DOXYGEN_IGNORE 27 | 28 | #define LOG_DEBUG ROS_DEBUG 29 | #define LOG_DEBUG_STREAM ROS_DEBUG_STREAM 30 | #define LOG_DEBUG_NAMED ROS_DEBUG_NAMED 31 | #define LOG_DEBUG_STREAM_NAMED ROS_DEBUG_STREAM_NAMED 32 | #define LOG_DEBUG_COND ROS_DEBUG_COND 33 | #define LOG_DEBUG_STREAM_COND ROS_DEBUG_STREAM_COND 34 | #define LOG_DEBUG_COND_NAMED ROS_DEBUG_COND_NAMED 35 | #define LOG_DEBUG_STREAM_COND_NAMED ROS_DEBUG_STREAM_COND_NAMED 36 | #define LOG_DEBUG_ONCE ROS_DEBUG_ONCE 37 | #define LOG_DEBUG_STREAM_ONCE ROS_DEBUG_STREAM_ONCE 38 | #define LOG_DEBUG_ONCE_NAMED ROS_DEBUG_ONCE_NAMED 39 | #define LOG_DEBUG_STREAM_ONCE_NAMED ROS_DEBUG_STREAM_ONCE_NAMED 40 | #define LOG_DEBUG_THROTTLE ROS_DEBUG_THROTTLE 41 | #define LOG_DEBUG_STREAM_THROTTLE ROS_DEBUG_STREAM_THROTTLE 42 | #define LOG_DEBUG_THROTTLE_NAMED ROS_DEBUG_THROTTLE_NAMED 43 | #define LOG_DEBUG_STREAM_THROTTLE_NAMED ROS_DEBUG_STREAM_THROTTLE_NAMED 44 | #define LOG_DEBUG_DELAYED_THROTTLE ROS_DEBUG_DELAYED_THROTTLE 45 | #define LOG_DEBUG_STREAM_DELAYED_THROTTLE ROS_DEBUG_STREAM_DELAYED_THROTTLE 46 | #define LOG_DEBUG_DELAYED_THROTTLE_NAMED ROS_DEBUG_DELAYED_THROTTLE_NAMED 47 | #define LOG_DEBUG_STREAM_DELAYED_THROTTLE_NAMED ROS_DEBUG_STREAM_DELAYED_THROTTLE_NAMED 48 | #define LOG_DEBUG_FILTER ROS_DEBUG_FILTER 49 | #define LOG_DEBUG_STREAM_FILTER ROS_DEBUG_STREAM_FILTER 50 | #define LOG_DEBUG_FILTER_NAMED ROS_DEBUG_FILTER_NAMED 51 | #define LOG_DEBUG_STREAM_FILTER_NAMED ROS_DEBUG_STREAM_FILTER_NAMED 52 | 53 | #define LOG_INFO ROS_INFO 54 | #define LOG_INFO_STREAM ROS_INFO_STREAM 55 | #define LOG_INFO_NAMED ROS_INFO_NAMED 56 | #define LOG_INFO_STREAM_NAMED ROS_INFO_STREAM_NAMED 57 | #define LOG_INFO_COND ROS_INFO_COND 58 | #define LOG_INFO_STREAM_COND ROS_INFO_STREAM_COND 59 | #define LOG_INFO_COND_NAMED ROS_INFO_COND_NAMED 60 | #define LOG_INFO_STREAM_COND_NAMED ROS_INFO_STREAM_COND_NAMED 61 | #define LOG_INFO_ONCE ROS_INFO_ONCE 62 | #define LOG_INFO_STREAM_ONCE ROS_INFO_STREAM_ONCE 63 | #define LOG_INFO_ONCE_NAMED ROS_INFO_ONCE_NAMED 64 | #define LOG_INFO_STREAM_ONCE_NAMED ROS_INFO_STREAM_ONCE_NAMED 65 | #define LOG_INFO_THROTTLE ROS_INFO_THROTTLE 66 | #define LOG_INFO_STREAM_THROTTLE ROS_INFO_STREAM_THROTTLE 67 | #define LOG_INFO_THROTTLE_NAMED ROS_INFO_THROTTLE_NAMED 68 | #define LOG_INFO_STREAM_THROTTLE_NAMED ROS_INFO_STREAM_THROTTLE_NAMED 69 | #define LOG_INFO_DELAYED_THROTTLE ROS_INFO_DELAYED_THROTTLE 70 | #define LOG_INFO_STREAM_DELAYED_THROTTLE ROS_INFO_STREAM_DELAYED_THROTTLE 71 | #define LOG_INFO_DELAYED_THROTTLE_NAMED ROS_INFO_DELAYED_THROTTLE_NAMED 72 | #define LOG_INFO_STREAM_DELAYED_THROTTLE_NAMED ROS_INFO_STREAM_DELAYED_THROTTLE_NAMED 73 | #define LOG_INFO_FILTER ROS_INFO_FILTER 74 | #define LOG_INFO_STREAM_FILTER ROS_INFO_STREAM_FILTER 75 | #define LOG_INFO_FILTER_NAMED ROS_INFO_FILTER_NAMED 76 | #define LOG_INFO_STREAM_FILTER_NAMED ROS_INFO_STREAM_FILTER_NAMED 77 | 78 | #define LOG_WARN ROS_WARN 79 | #define LOG_WARN_STREAM ROS_WARN_STREAM 80 | #define LOG_WARN_NAMED ROS_WARN_NAMED 81 | #define LOG_WARN_STREAM_NAMED ROS_WARN_STREAM_NAMED 82 | #define LOG_WARN_COND ROS_WARN_COND 83 | #define LOG_WARN_STREAM_COND ROS_WARN_STREAM_COND 84 | #define LOG_WARN_COND_NAMED ROS_WARN_COND_NAMED 85 | #define LOG_WARN_STREAM_COND_NAMED ROS_WARN_STREAM_COND_NAMED 86 | #define LOG_WARN_ONCE ROS_WARN_ONCE 87 | #define LOG_WARN_STREAM_ONCE ROS_WARN_STREAM_ONCE 88 | #define LOG_WARN_ONCE_NAMED ROS_WARN_ONCE_NAMED 89 | #define LOG_WARN_STREAM_ONCE_NAMED ROS_WARN_STREAM_ONCE_NAMED 90 | #define LOG_WARN_THROTTLE ROS_WARN_THROTTLE 91 | #define LOG_WARN_STREAM_THROTTLE ROS_WARN_STREAM_THROTTLE 92 | #define LOG_WARN_THROTTLE_NAMED ROS_WARN_THROTTLE_NAMED 93 | #define LOG_WARN_STREAM_THROTTLE_NAMED ROS_WARN_STREAM_THROTTLE_NAMED 94 | #define LOG_WARN_DELAYED_THROTTLE ROS_WARN_DELAYED_THROTTLE 95 | #define LOG_WARN_STREAM_DELAYED_THROTTLE ROS_WARN_STREAM_DELAYED_THROTTLE 96 | #define LOG_WARN_DELAYED_THROTTLE_NAMED ROS_WARN_DELAYED_THROTTLE_NAMED 97 | #define LOG_WARN_STREAM_DELAYED_THROTTLE_NAMED ROS_WARN_STREAM_DELAYED_THROTTLE_NAMED 98 | #define LOG_WARN_FILTER ROS_WARN_FILTER 99 | #define LOG_WARN_STREAM_FILTER ROS_WARN_STREAM_FILTER 100 | #define LOG_WARN_FILTER_NAMED ROS_WARN_FILTER_NAMED 101 | #define LOG_WARN_STREAM_FILTER_NAMED ROS_WARN_STREAM_FILTER_NAMED 102 | 103 | #define LOG_ERROR ROS_ERROR 104 | #define LOG_ERROR_STREAM ROS_ERROR_STREAM 105 | #define LOG_ERROR_NAMED ROS_ERROR_NAMED 106 | #define LOG_ERROR_STREAM_NAMED ROS_ERROR_STREAM_NAMED 107 | #define LOG_ERROR_COND ROS_ERROR_COND 108 | #define LOG_ERROR_STREAM_COND ROS_ERROR_STREAM_COND 109 | #define LOG_ERROR_COND_NAMED ROS_ERROR_COND_NAMED 110 | #define LOG_ERROR_STREAM_COND_NAMED ROS_ERROR_STREAM_COND_NAMED 111 | #define LOG_ERROR_ONCE ROS_ERROR_ONCE 112 | #define LOG_ERROR_STREAM_ONCE ROS_ERROR_STREAM_ONCE 113 | #define LOG_ERROR_ONCE_NAMED ROS_ERROR_ONCE_NAMED 114 | #define LOG_ERROR_STREAM_ONCE_NAMED ROS_ERROR_STREAM_ONCE_NAMED 115 | #define LOG_ERROR_THROTTLE ROS_ERROR_THROTTLE 116 | #define LOG_ERROR_STREAM_THROTTLE ROS_ERROR_STREAM_THROTTLE 117 | #define LOG_ERROR_THROTTLE_NAMED ROS_ERROR_THROTTLE_NAMED 118 | #define LOG_ERROR_STREAM_THROTTLE_NAMED ROS_ERROR_STREAM_THROTTLE_NAMED 119 | #define LOG_ERROR_DELAYED_THROTTLE ROS_ERROR_DELAYED_THROTTLE 120 | #define LOG_ERROR_STREAM_DELAYED_THROTTLE ROS_ERROR_STREAM_DELAYED_THROTTLE 121 | #define LOG_ERROR_DELAYED_THROTTLE_NAMED ROS_ERROR_DELAYED_THROTTLE_NAMED 122 | #define LOG_ERROR_STREAM_DELAYED_THROTTLE_NAMED ROS_ERROR_STREAM_DELAYED_THROTTLE_NAMED 123 | #define LOG_ERROR_FILTER ROS_ERROR_FILTER 124 | #define LOG_ERROR_STREAM_FILTER ROS_ERROR_STREAM_FILTER 125 | #define LOG_ERROR_FILTER_NAMED ROS_ERROR_FILTER_NAMED 126 | #define LOG_ERROR_STREAM_FILTER_NAMED ROS_ERROR_STREAM_FILTER_NAMED 127 | 128 | #define LOG_FATAL ROS_FATAL 129 | #define LOG_FATAL_STREAM ROS_FATAL_STREAM 130 | #define LOG_FATAL_NAMED ROS_FATAL_NAMED 131 | #define LOG_FATAL_STREAM_NAMED ROS_FATAL_STREAM_NAMED 132 | #define LOG_FATAL_COND ROS_FATAL_COND 133 | #define LOG_FATAL_STREAM_COND ROS_FATAL_STREAM_COND 134 | #define LOG_FATAL_COND_NAMED ROS_FATAL_COND_NAMED 135 | #define LOG_FATAL_STREAM_COND_NAMED ROS_FATAL_STREAM_COND_NAMED 136 | #define LOG_FATAL_ONCE ROS_FATAL_ONCE 137 | #define LOG_FATAL_STREAM_ONCE ROS_FATAL_STREAM_ONCE 138 | #define LOG_FATAL_ONCE_NAMED ROS_FATAL_ONCE_NAMED 139 | #define LOG_FATAL_STREAM_ONCE_NAMED ROS_FATAL_STREAM_ONCE_NAMED 140 | #define LOG_FATAL_THROTTLE ROS_FATAL_THROTTLE 141 | #define LOG_FATAL_STREAM_THROTTLE ROS_FATAL_STREAM_THROTTLE 142 | #define LOG_FATAL_THROTTLE_NAMED ROS_FATAL_THROTTLE_NAMED 143 | #define LOG_FATAL_STREAM_THROTTLE_NAMED ROS_FATAL_STREAM_THROTTLE_NAMED 144 | #define LOG_FATAL_DELAYED_THROTTLE ROS_FATAL_DELAYED_THROTTLE 145 | #define LOG_FATAL_STREAM_DELAYED_THROTTLE ROS_FATAL_STREAM_DELAYED_THROTTLE 146 | #define LOG_FATAL_DELAYED_THROTTLE_NAMED ROS_FATAL_DELAYED_THROTTLE_NAMED 147 | #define LOG_FATAL_STREAM_DELAYED_THROTTLE_NAMED ROS_FATAL_STREAM_DELAYED_THROTTLE_NAMED 148 | #define LOG_FATAL_FILTER ROS_FATAL_FILTER 149 | #define LOG_FATAL_STREAM_FILTER ROS_FATAL_STREAM_FILTER 150 | #define LOG_FATAL_FILTER_NAMED ROS_FATAL_FILTER_NAMED 151 | #define LOG_FATAL_STREAM_FILTER_NAMED ROS_FATAL_STREAM_FILTER_NAMED 152 | 153 | //@endcond 154 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pi_hexapod_description) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | urdf 12 | xacro 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a exec_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | ## Generate services in the 'srv' folder 56 | # add_service_files( 57 | # FILES 58 | # Service1.srv 59 | # Service2.srv 60 | # ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | # generate_messages( 71 | # DEPENDENCIES 72 | # std_msgs # Or other packages containing msgs 73 | # ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if your package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | # INCLUDE_DIRS include 106 | # LIBRARIES pi_hexapod_description 107 | # CATKIN_DEPENDS urdf xacro 108 | # DEPENDS system_lib 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | ## Specify additional locations of header files 116 | ## Your package locations should be listed before other locations 117 | #include_directories( 118 | # include 119 | # ${catkin_INCLUDE_DIRS} 120 | #) 121 | 122 | ## Declare a C++ library 123 | # add_library(${PROJECT_NAME} 124 | # src/${PROJECT_NAME}/pi_hexapod_description.cpp 125 | # ) 126 | 127 | ## Add cmake target dependencies of the library 128 | ## as an example, code may need to be generated before libraries 129 | ## either from message generation or dynamic reconfigure 130 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 131 | 132 | ## Declare a C++ executable 133 | ## With catkin_make all packages are built within a single CMake context 134 | ## The recommended prefix ensures that target names across packages don't collide 135 | # add_executable(${PROJECT_NAME}_node src/pi_hexapod_description_node.cpp) 136 | 137 | ## Rename C++ executable without prefix 138 | ## The above recommended prefix causes long target names, the following renames the 139 | ## target back to the shorter version for ease of user use 140 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 141 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 142 | 143 | ## Add cmake target dependencies of the executable 144 | ## same as for the library above 145 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 146 | 147 | ## Specify libraries to link a library or executable target against 148 | # target_link_libraries(${PROJECT_NAME}_node 149 | # ${catkin_LIBRARIES} 150 | # ) 151 | 152 | ############# 153 | ## Install ## 154 | ############# 155 | 156 | install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 157 | install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 158 | install(DIRECTORY cfg DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 159 | 160 | # all install targets should use catkin DESTINATION variables 161 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 162 | 163 | ## Mark executable scripts (Python etc.) for installation 164 | ## in contrast to setup.py, you can choose the destination 165 | # install(PROGRAMS 166 | # scripts/my_python_script 167 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark executables for installation 171 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 172 | # install(TARGETS ${PROJECT_NAME}_node 173 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | # ) 175 | 176 | ## Mark libraries for installation 177 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 178 | # install(TARGETS ${PROJECT_NAME} 179 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 180 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 181 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 182 | # ) 183 | 184 | ## Mark cpp header files for installation 185 | # install(DIRECTORY include/${PROJECT_NAME}/ 186 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 187 | # FILES_MATCHING PATTERN "*.h" 188 | # PATTERN ".svn" EXCLUDE 189 | # ) 190 | 191 | ## Mark other files for installation (e.g. launch and bag files, etc.) 192 | # install(FILES 193 | # # myfile1 194 | # # myfile2 195 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 196 | # ) 197 | 198 | ############# 199 | ## Testing ## 200 | ############# 201 | 202 | ## Add gtest based cpp test target and link libraries 203 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_pi_hexapod_description.cpp) 204 | # if(TARGET ${PROJECT_NAME}-test) 205 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 206 | # endif() 207 | 208 | ## Add folders to be run by python nosetests 209 | # catkin_add_nosetests(test) 210 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PI_ROS_Driver 2 | This package contains the ROS Driver for Hexapods from Physik Intrumente (PI). 3 | 4 | This includes a controller manager as standalone node as well as launch files and configurations 5 | as easy starting point for your ROS application. 6 | 7 | The package is using ROS control and builds on the known interfaces and paradigms. 8 | 9 | ## License 10 | Please consider the license information provided with this repository. 11 | 12 | ## Setup 13 | Due to the library used for connecting to the PI Hexapod, this Driver can only be used on x86 14 | architectures. 15 | 16 | A number of general ROS tutorials can be found at the [ROS wiki](http://wiki.ros.org/ROS/Tutorials). 17 | To install ROS Melodic for Ubuntu 18.04 follow the instructions given at 18 | [Melodic for Ubuntu](http://wiki.ros.org/melodic/Installation/Ubuntu). 19 | For other ROS Distributions or target platforms, 20 | see [ROS Wiki](http://wiki.ros.org/ROS/Installation). 21 | 22 | To create a workspace, refer to 23 | [this tutorial](http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment) 24 | or follow these abbreviated instructions for ROS Melodic: 25 | ``` 26 | source /opt/ros/melodic/setup.bash 27 | mkdir -p ~/catkin_ws/src 28 | cd ~/catkin_ws/ 29 | catkin_make 30 | source devel/setup.bash 31 | ``` 32 | 33 | Clone this repository, install dependencies and build the workspace: 34 | ``` 35 | cd src 36 | # clone this repository: git clone ... 37 | cd ~/catkin_ws/ 38 | rosdep install --from-paths src --ignore-src -r -y 39 | catkin_make 40 | ``` 41 | 42 | Now, the driver is ready to be used. For example, run the following command to start a simulated 43 | hexapod and an interactive marker to control it: 44 | ``` 45 | roslaunch pi_hexapod_control interactive_marker.launch sim:=true 46 | ``` 47 | 48 | ## Usage 49 | 50 | ### Launch files 51 | A number of launch files are included to serve as example on how to configure and use the Driver. 52 | 53 | `hexapod_common_control.launch` provides an elaborate example on how to connect to a hexapod. 54 | It can be used as standalone configuration to interact with hardware or a simulated hexapod. 55 | Also, it can be easily included in other launch files, allowing for easy configuration due to using 56 | smart default arguments. 57 | For example, `interactive_marker.launch` and `rqt_gui.launch` build onto 58 | `hexapod_common_control.launch` resulting in easy to understand configurations. 59 | It is advised to not use the interactive marker and the RQT gui at the same time. 60 | 61 | 62 | `interactive_marker.launch` provides an interactive marker server to control a hexapod via RViz. 63 | For more information on its functionality, see \ref interactive_marker "Interactive Marker Node". 64 | 65 | `rqt_gui.launch` provides an different interface to control a hexapod. 66 | A precofigured RQT perspective with a costum control interface is launched. 67 | If you expirience problems launching this RQT perspective, have a lock at the README provided with 68 | the `pi_hexapod_gui` package for more details. 69 | 70 | ### Controller Manager Node 71 | \anchor controller_manager_node 72 | This node provides a controller manager and hardware interface to interact with a PI Hexapod. 73 | The driver can connect to PI Hexapod Hardware via TCP/IP or serial connection. 74 | Also, the hexapod can be simulated without requiring a connection to hardware. 75 | 76 | #### Parameters: 77 | 78 | Exactly one of `using_tcp_ip_connection` or `using_rs232_connection` needs to be set to true, 79 | determining the connection type. 80 | 81 | If TCP/IP is choosen, the string `hexapod_ip` needs to be set. 82 | Additionally, the integer `hexapod_port` can be set, its default is 50000. 83 | 84 | If serial connection is choosen, the integers `rs232_port_nr` and `baudrate` need to be specified. 85 | 86 | `is_sim` can be set to true to use a simulated hexapod instead of a connection to a real one. 87 | Its default value is false. 88 | 89 | `auto_referencing` allows to start referencing the hexapod on startup without the need to call the 90 | referencing action. 91 | 92 | `prefer_stop_over_halt` allows to define the hardware behavior in case of a unexpected shutdown. 93 | Setting this boolean to true results in the more strict STP command word being used to stop the 94 | Hexapod instead of a HLT command. The parameters default value is false. 95 | 96 | `hardware_interface/controller_rate`: positive double providing the main control loop rate in Hz, 97 | default=1000 98 | 99 | `hardware_interface/joints` and `hardware_interface/visual_joints` need to be set. 100 | The `hardware_interface/` parameters are set via the provided `cfg/hexapod_controllers.yaml`. 101 | 102 | #### Actions: 103 | `referencing` of type `pi_hexapod_msgs::ReferencingAction`: 104 | Allows to manually start the referencing of the hexapods axes. 105 | 106 | ### Interactive Marker Node 107 | \anchor interactive_marker 108 | This nodes provides an interactive marker server allowing the user to control the hexapod from RViz. 109 | 110 | #### Parameters: 111 | Direct node parameter: 112 | `rate` (default=10): Command publishing rate 113 | `autostart_publishing_commands` (default=false): interactive marker starts sending commands to 114 | JointGroupPosController directly after startup, if set to true 115 | `topic_ns` (default="pi\_interactive\_marker"): namespace in which to current marker pose is published 116 | 117 | Parameter found in Hexapod configuration YAML: 118 | `interactive_marker/control_scale` (default=1): scales the size of control elements 119 | `interactive_marker/base_frame` (default="zero_link"): Frame name of the zero position frame of the 120 | hexapod 121 | 122 | `x/lower_limit`, `x/upper_limit`, `y/lower_limit`, `y/upper_limit`, `z/lower_limit`, 123 | `z/upper_limit`, `u/lower_limit`, `u/upper_limit`, `v/lower_limit`, `v/upper_limit`, 124 | `w/lower_limit`, `w/upper_limit`: defining movement limits 125 | 126 | `interactive_marker/scale/x`, `interactive_marker/scale/y`, `interactive_marker/scale/z`, 127 | `interactive_marker/position/x`, `interactive_marker/position/y`, `interactive_marker/position/z`, 128 | `interactive_marker/orientation/w`, `interactive_marker/orientation/x`, 129 | `interactive_marker/orientation/y`, `interactive_marker/orientation/z`, 130 | `interactive_marker/color/r`, `interactive_marker/color/g`, `interactive_marker/color/b`, 131 | `interactive_marker/color/a`: defining the optics of the interactive marker 132 | 133 | #### Publisher: 134 | `pose` of type `geometry_msgs::PoseStamped`: 135 | Publishes the current pose of the interactive marker. 136 | 137 | `command` of `std_msgs::Float64MultiArray`: 138 | Publishes JointGroupPositionController command, if set to active via startup parameter, 139 | service call or context menu in RViz. 140 | 141 | #### Subcriber: 142 | `joint_states` of type `sensor_msgs::JointState`: Requires current joint states of the hexapod. 143 | 144 | #### Services: 145 | Services can also be activated via the context menu in RViz accessed via right clicking the 146 | interactive marker. 147 | 148 | `activate_command_publishing` of type `std_srvs::SetBool`: 149 | De-/activates command publishing. 150 | 151 | `set_to_zero` of type `std_srvs::Empty`: 152 | Resets marker position to zero. 153 | 154 | `set_to_current` of `type std_srvs::Empty`: 155 | Sets marker to current hexapod position. 156 | 157 | `send_single_command` of type `std_srvs::Empty`: 158 | Send a single command message to the `command` topic. 159 | 160 | ## Documentation 161 | This package is documented with Doxygen (`apt install doxygen`). 162 | Create the documentation by executing `doxygen Doxyfile` in the package root directory. 163 | HTML documentation will be generated in the `doc/html` directory and can be easily accessed by 164 | opening `doc/html/index.html` in a web browser. 165 | 166 | ## Software Architecture 167 | The packages builds onto ROS Control and provides an abstraction of PI Hexapods by implementing 168 | `hardware_interface::RobotHW` and a Controller Manager as standalone node. 169 | `JointStateInterface` and `PositionJointInterface` are provided to be used with common ROS 170 | Controllers or to develop costum ones. 171 | 172 | The user can interact with the Hexapod through Controllers and tooling building onto them. 173 | There are also direct interaction options with the controller, see 174 | \ref controller_manager_node "Controller Manager Node" for details. 175 | 176 | A sketch of the software architecture and possible User interfaces is provided as following: 177 | ![](doc/src/architecture.png) 178 | 179 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/README.md: -------------------------------------------------------------------------------- 1 | # PI Hexapod Control 2 | This package contains the ROS Driver for Hexapods from Physik Intrumente (PI). 3 | 4 | This includes a controller manager as standalone node as well as launch files and configurations 5 | as easy starting point for your ROS application. 6 | 7 | The package is using ROS control and builds on the known interfaces and paradigms. 8 | 9 | ## License 10 | Please consider the license information provided with this repository. 11 | 12 | ## Setup 13 | Due to the library used for connecting to the PI Hexapod, this Driver can only be used on x86-64 14 | architectures. 15 | 16 | A number of general ROS tutorials can be found at the [ROS wiki](http://wiki.ros.org/ROS/Tutorials). 17 | To install ROS Melodic for Ubuntu 18.04 follow the instructions given at 18 | [Melodic for Ubuntu](http://wiki.ros.org/melodic/Installation/Ubuntu). 19 | For other ROS Distributions or target platforms, 20 | see [ROS Wiki](http://wiki.ros.org/ROS/Installation). 21 | 22 | To create a workspace, refer to 23 | [this tutorial](http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment) 24 | or follow these abbreviated instructions for ROS Melodic: 25 | ``` 26 | source /opt/ros/melodic/setup.bash 27 | mkdir -p ~/catkin_ws/src 28 | cd ~/catkin_ws/ 29 | catkin_make 30 | source devel/setup.bash 31 | ``` 32 | 33 | Clone this repository, install dependencies and build the workspace: 34 | ``` 35 | cd src 36 | # clone this repository: git clone ... 37 | cd ~/catkin_ws/ 38 | rosdep install --from-paths src --ignore-src -r -y 39 | catkin_make 40 | source devel/setup.bash 41 | ``` 42 | 43 | Now, the driver is ready to be used. For example, run the following command to start a simulated 44 | hexapod and an interactive marker to control it: 45 | ``` 46 | roslaunch pi_hexapod_control interactive_marker.launch sim:=true 47 | ``` 48 | 49 | ## PI GCS2 Library 50 | As default, pi\_hexapod\_control is build against `libpi_pi_gcs2-3.11.0.a` provided in the 51 | `lib` directory of this package. 52 | To build against a shared version of this library, set the `PI_GCS2_PATH` cmake option. 53 | ``` 54 | catkin_make --cmake-args -DPI_GCS2_PATH="/usr/local/PI/lib64/libpi_pi_gcs2.so.3.11.0" 55 | ``` 56 | 57 | ## Usage 58 | 59 | ### Launch files 60 | A number of launch files are included to serve as example on how to configure and use the Driver. 61 | 62 | `hexapod_common_control.launch` provides an elaborate example on how to connect to a hexapod. 63 | It can be used as standalone configuration to interact with hardware or a simulated hexapod. 64 | Also, it can be easily included in other launch files, allowing for easy configuration due to using 65 | smart default arguments. 66 | For example, `interactive_marker.launch` and `rqt_gui.launch` build onto 67 | `hexapod_common_control.launch` resulting in easy to understand configurations. 68 | It is advised to not use the interactive marker and the RQT gui at the same time. 69 | 70 | 71 | `interactive_marker.launch` provides an interactive marker server to control a hexapod via RViz. 72 | For more information on its functionality, see \ref interactive_marker "Interactive Marker Node". 73 | 74 | `rqt_gui.launch` provides an different interface to control a hexapod. 75 | A precofigured RQT perspective with a costum control interface is launched. 76 | If you expirience problems launching this RQT perspective, have a lock at the README provided with 77 | the `pi_hexapod_gui` package for more details. 78 | 79 | ### Controller Manager Node 80 | \anchor controller_manager_node 81 | This node provides a controller manager and hardware interface to interact with a PI Hexapod. 82 | The driver can connect to PI Hexapod Hardware via TCP/IP or serial connection. 83 | Also, the hexapod can be simulated without requiring a connection to hardware. 84 | 85 | #### Parameters: 86 | 87 | Exactly one of `using_tcp_ip_connection` or `using_rs232_connection` needs to be set to true, 88 | determining the connection type. 89 | 90 | If TCP/IP is choosen, the string `hexapod_ip` needs to be set. 91 | Additionally, the integer `hexapod_port` can be set, its default is 50000. 92 | 93 | If serial connection is choosen, the integers `rs232_port_nr` and `baudrate` need to be specified. 94 | 95 | `is_sim` can be set to true to use a simulated hexapod instead of a connection to a real one. 96 | Its default value is false. 97 | 98 | `auto_referencing` allows to start referencing the hexapod on startup without the need to call the 99 | referencing action. 100 | 101 | `prefer_stop_over_halt` allows to define the hardware behavior in case of a unexpected shutdown. 102 | Setting this boolean to true results in the more strict STP command word being used to stop the 103 | Hexapod instead of a HLT command. The parameters default value is false. 104 | 105 | `hardware_interface/controller_rate`: positive double providing the main control loop rate in Hz, 106 | default=1000 107 | 108 | `hardware_interface/joints` and `hardware_interface/visual_joints` need to be set. 109 | The `hardware_interface/` parameters are set via the provided `cfg/hexapod_controllers.yaml`. 110 | 111 | #### Actions: 112 | `referencing` of type `pi_hexapod_msgs::ReferencingAction`: 113 | Allows to manually start the referencing of the hexapods axes. 114 | 115 | ### Interactive Marker Node 116 | \anchor interactive_marker 117 | This nodes provides an interactive marker server allowing the user to control the hexapod from RViz. 118 | 119 | #### Parameters: 120 | Direct node parameter: 121 | `rate` (default=10): Command publishing rate 122 | `autostart_publishing_commands` (default=false): interactive marker starts sending commands to 123 | JointGroupPosController directly after startup, if set to true 124 | `topic_ns` (default="pi\_interactive\_marker"): namespace in which to current marker pose is published 125 | 126 | Parameter found in Hexapod configuration YAML: 127 | `interactive_marker/control_scale` (default=1): scales the size of control elements 128 | `interactive_marker/base_frame` (default="zero_link"): Frame name of the zero position frame of the 129 | hexapod 130 | 131 | `x/lower_limit`, `x/upper_limit`, `y/lower_limit`, `y/upper_limit`, `z/lower_limit`, 132 | `z/upper_limit`, `u/lower_limit`, `u/upper_limit`, `v/lower_limit`, `v/upper_limit`, 133 | `w/lower_limit`, `w/upper_limit`: defining movement limits 134 | 135 | `interactive_marker/scale/x`, `interactive_marker/scale/y`, `interactive_marker/scale/z`, 136 | `interactive_marker/position/x`, `interactive_marker/position/y`, `interactive_marker/position/z`, 137 | `interactive_marker/orientation/w`, `interactive_marker/orientation/x`, 138 | `interactive_marker/orientation/y`, `interactive_marker/orientation/z`, 139 | `interactive_marker/color/r`, `interactive_marker/color/g`, `interactive_marker/color/b`, 140 | `interactive_marker/color/a`: defining the optics of the interactive marker 141 | 142 | #### Publisher: 143 | `pose` of type `geometry_msgs::PoseStamped`: 144 | Publishes the current pose of the interactive marker. 145 | 146 | `command` of `std_msgs::Float64MultiArray`: 147 | Publishes JointGroupPositionController command, if set to active via startup parameter, 148 | service call or context menu in RViz. 149 | 150 | #### Subcriber: 151 | `joint_states` of type `sensor_msgs::JointState`: Requires current joint states of the hexapod. 152 | 153 | #### Services: 154 | Services can also be activated via the context menu in RViz accessed via right clicking the 155 | interactive marker. 156 | 157 | `activate_command_publishing` of type `std_srvs::SetBool`: 158 | De-/activates command publishing. 159 | 160 | `set_to_zero` of type `std_srvs::Empty`: 161 | Resets marker position to zero. 162 | 163 | `set_to_current` of `type std_srvs::Empty`: 164 | Sets marker to current hexapod position. 165 | 166 | `send_single_command` of type `std_srvs::Empty`: 167 | Send a single command message to the `command` topic. 168 | 169 | ## Documentation 170 | This package is documented with Doxygen (`apt install doxygen`). 171 | Create the documentation by executing `doxygen Doxyfile` in the package root directory. 172 | HTML documentation will be generated in the `doc/html` directory and can be easily accessed by 173 | opening `doc/html/index.html` in a web browser. 174 | 175 | ## Software Architecture 176 | The packages builds onto ROS Control and provides an abstraction of PI Hexapods by implementing 177 | `hardware_interface::RobotHW` and a Controller Manager as standalone node. 178 | `JointStateInterface` and `PositionJointInterface` are provided to be used with common ROS 179 | Controllers or to develop costum ones. 180 | 181 | The user can interact with the Hexapod through Controllers and tooling building onto them. 182 | There are also direct interaction options with the controller, see 183 | \ref controller_manager_node "Controller Manager Node" for details. 184 | 185 | A sketch of the software architecture and possible User interfaces is provided as following: 186 | ![](doc/src/architecture.png) 187 | 188 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/include/pi_hexapod_control/pi_driver.h: -------------------------------------------------------------------------------- 1 | // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- 2 | 3 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 4 | // This file is part of the PI-Hexapod Driver. 5 | // 6 | // 7 | // © Copyright 2020 Physik Instrumente (PI) GmbH & Co. KG, Karlsruhe, Germany 8 | // © Copyright 2020 FZI Forschungszentrum Informatik, Karlsruhe, Germany 9 | // -- END LICENSE BLOCK ------------------------------------------------ 10 | 11 | //---------------------------------------------------------------------- 12 | /*!\file 13 | * 14 | * This file contains the abstract PiDriver class and its derived classes for handling both 15 | * hardware and simulation. It also contains the implementation for the SimulationPiDriver. 16 | * 17 | * \author Christian Eichmann 18 | * \author Philip Keller 19 | * \date 2020-04-06 20 | * 21 | */ 22 | //---------------------------------------------------------------------- 23 | #ifndef PI_HEXAPOD_CONTROL_PI_DRIVER_H_INCLUDED 24 | #define PI_HEXAPOD_CONTROL_PI_DRIVER_H_INCLUDED 25 | 26 | #include "pi_hexapod_control/pi_errors.h" 27 | #include "pi_hexapod_control/types.h" 28 | 29 | namespace pi_hexapod_control { 30 | 31 | /*! 32 | * \brief An abstract class representing a Hexapod Driver. 33 | */ 34 | class PiDriver 35 | { 36 | public: 37 | /*! 38 | * \brief References the hexapod. 39 | * 40 | * \returns True if referenciation successful. 41 | */ 42 | virtual bool reference() = 0; 43 | 44 | /*! 45 | * \brief Requests the controller data. 46 | * 47 | * \param values Current joint positions 48 | * 49 | * \returns True on successful request. 50 | */ 51 | virtual bool requestControllerData(vector6d_t& values) = 0; 52 | 53 | /*! 54 | * \brief Writes a joint command together with a keepalive signal onto the socket being sent to 55 | * the robot. 56 | * 57 | * \param values Desired joint positions 58 | * 59 | * \returns True on successful write. 60 | */ 61 | virtual bool writeControllerCommand(const vector6d_t& values) = 0; 62 | 63 | /*! 64 | * \brief Connects to hexapod. 65 | * 66 | * \returns True if connection successful. 67 | */ 68 | virtual bool connect() = 0; 69 | 70 | /*! 71 | * \brief Disconnects from hexapod. 72 | */ 73 | virtual void disconnect() = 0; 74 | 75 | /*! 76 | * \brief Updates the referenced-status. 77 | * 78 | * \return indicates success 79 | */ 80 | virtual bool updateReferencedStatus() = 0; 81 | 82 | /*! 83 | * \brief Halts the hexapod. 84 | * 85 | * \returns True if request successful. 86 | */ 87 | virtual bool haltHexapod() = 0; 88 | 89 | /*! 90 | * \brief Stops the hexapod. 91 | * 92 | * \returns True if request successful. 93 | */ 94 | virtual bool stopHexapod() = 0; 95 | 96 | /*! 97 | * \brief Provides the current error ID. 98 | * 99 | * \returns The PI error ID. 100 | */ 101 | int getErrorID() { return error_id_; }; 102 | 103 | /*! 104 | * \brief Provides the current error message as string. 105 | * 106 | * \returns The translated PI error message. 107 | */ 108 | std::string getErrorMessage(); 109 | 110 | /*! 111 | * \brief Prints the error to the default log. 112 | * 113 | * \returns void 114 | */ 115 | virtual void updateError() = 0; 116 | 117 | /*! 118 | * \brief Returns the current referenced-status. 119 | * 120 | * \return indicates success 121 | */ 122 | bool isReferenced() { return is_referenced_; }; 123 | 124 | 125 | protected: 126 | bool is_referenced_; //!< idicates whether the hexapod is already referenced or not 127 | int error_id_ = 0; //!< id of the last received error message 128 | }; 129 | 130 | /*! 131 | * \brief This is the main class for interfacing the driver. 132 | * 133 | * It sets up all the necessary socket connections and handles the data exchange with the robot. 134 | * Use this classes methods to access and write data. 135 | */ 136 | class HardwarePiDriver : public PiDriver 137 | { 138 | public: 139 | /*! 140 | * \brief Constructs a new PiDriver object. 141 | * 142 | * \param communication_timeout_ms timeout in ms 143 | * \param hexapod_ip IP-address under which the hexapod is reachable. 144 | * \param hexapod_port Port-address under which the hexapod is reachable. 145 | */ 146 | HardwarePiDriver(const uint32_t communication_timeout_ms, 147 | const std::string& hexapod_ip, 148 | const uint32_t hexapod_port = 50000); 149 | 150 | /*! 151 | * \brief Constructs a new PiDriver object. 152 | * 153 | * \param communication_timeout_ms timeout in ms 154 | * \param rs232_port_nr number of chosen RS232 port. 155 | * \param baudrate Baudrate for communication with Hexapod Controller. 156 | */ 157 | HardwarePiDriver(const uint32_t communication_timeout_ms, 158 | const int32_t rs232_port_nr, 159 | const int32_t baudrate); 160 | 161 | /*! 162 | * \brief Destructor, disconnects if required. 163 | * 164 | * The Destructor disconnects the Driver from the Hexapod controller, if there is a 165 | * open connection. 166 | */ 167 | ~HardwarePiDriver(); 168 | 169 | /*! 170 | * \brief References the hexapod. 171 | * 172 | * \returns True if referenciation successful. 173 | */ 174 | bool reference(); 175 | 176 | /*! 177 | * \brief Connects to hexapod. 178 | * 179 | * \returns True if connection successful. 180 | */ 181 | bool connect(); 182 | 183 | /*! 184 | * \brief Disconnects from hexapod. 185 | */ 186 | void disconnect(); 187 | 188 | /*! 189 | * \brief Requests the controller data. 190 | * 191 | * \param values Current joint positions 192 | * 193 | * \returns True on successful request. 194 | */ 195 | bool requestControllerData(vector6d_t& values); 196 | 197 | /*! 198 | * \brief Writes a joint command together with a keepalive signal onto the socket being sent to 199 | * the robot. 200 | * 201 | * \param values Desired joint positions 202 | * 203 | * \returns True on successful write. 204 | */ 205 | bool writeControllerCommand(const vector6d_t& values); 206 | 207 | /*! 208 | * \brief Halts the hexapod. 209 | * 210 | * \returns True if request successful. 211 | */ 212 | bool haltHexapod(); 213 | 214 | /*! 215 | * \brief Stops the hexapod. 216 | * 217 | * \returns True if request successful. 218 | */ 219 | bool stopHexapod(); 220 | 221 | /*! 222 | * \brief Updates the referenced-status. 223 | * 224 | * \returns True on successful status update call 225 | */ 226 | bool updateReferencedStatus(); 227 | 228 | /*! 229 | * \brief Prints the error to the default log. 230 | * 231 | * \returns void 232 | */ 233 | void updateError(); 234 | 235 | private: 236 | const bool using_tcp_ip_connection_; 237 | std::string hexapod_ip_; 238 | uint32_t hexapod_port_; 239 | 240 | const bool using_rs232_connection_; 241 | const bool error_checking_ = false; 242 | int32_t rs232_port_nr_; 243 | int32_t baudrate_; 244 | 245 | bool is_connected_; 246 | 247 | int pi_id_; 248 | uint32_t communication_timeout_ms_; 249 | 250 | const char axis_[12] = "X Y Z U V W"; 251 | double joint_pos_[6] = {0, 0, 0, 0, 0, 0}; 252 | }; 253 | 254 | /*! 255 | * \brief Driver with simulated Hexapod hardware. 256 | * 257 | * Creates a mock-up driver. No data is read from or writen to hardware. 258 | * Send commands are stored and used as current position on next cycle. 259 | * Connections and referencing never fail. 260 | */ 261 | class SimulatedPiDriver : public PiDriver 262 | { 263 | public: 264 | //! Constructs new Simulated Driver 265 | SimulatedPiDriver() { mock_values_.fill(0.0); }; 266 | 267 | /*! 268 | * \brief Mock of referencing. 269 | * 270 | * \returns True 271 | */ 272 | bool reference() 273 | { 274 | is_referenced_ = true; 275 | return true; 276 | }; 277 | 278 | /*! 279 | * \brief Gets dummy controller data. 280 | * 281 | * \param values Current joint positions 282 | * 283 | * \returns True 284 | */ 285 | bool requestControllerData(vector6d_t& values) 286 | { 287 | values = mock_values_; 288 | return true; 289 | }; 290 | 291 | /*! 292 | * \brief Stores a joint command to use as position on next read. 293 | * 294 | * \param values Desired joint positions 295 | * 296 | * \returns True 297 | */ 298 | bool writeControllerCommand(const vector6d_t& values) 299 | { 300 | mock_values_ = values; 301 | error_id_ = 0; 302 | return true; 303 | }; 304 | 305 | /*! 306 | * \brief Mock of connecting to hexapod. 307 | * 308 | * \returns True 309 | */ 310 | bool connect() { return true; }; 311 | 312 | /*! 313 | * \brief Mock of disconnecting from hexapod. 314 | */ 315 | void disconnect(){}; 316 | 317 | /*! 318 | * \brief Halts the hexapod. 319 | * 320 | * \returns True 321 | */ 322 | bool haltHexapod() 323 | { 324 | error_id_ = 10; 325 | return true; 326 | }; 327 | 328 | /*! 329 | * \brief Stops the hexapod. 330 | * 331 | * \returns True 332 | */ 333 | bool stopHexapod() 334 | { 335 | error_id_ = 10; 336 | return true; 337 | }; 338 | 339 | /*! 340 | * \brief Mock of referenced-status update. 341 | * 342 | * \return True 343 | */ 344 | bool updateReferencedStatus() { return true; }; 345 | 346 | /*! 347 | * \brief Mock of driver error update. 348 | * 349 | */ 350 | void updateError() { return; }; 351 | 352 | private: 353 | vector6d_t mock_values_; 354 | }; 355 | 356 | } // namespace pi_hexapod_control 357 | #endif // ifndef PI_HEXAPOD_CONTROL_PI_DRIVER_H_INCLUDED 358 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/src/pi_driver.cpp: -------------------------------------------------------------------------------- 1 | // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- 2 | 3 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 4 | // This file is part of the PI-Hexapod Driver. 5 | // 6 | // 7 | // © Copyright 2020 Physik Instrumente (PI) GmbH & Co. KG, Karlsruhe, Germany 8 | // © Copyright 2020 FZI Forschungszentrum Informatik, Karlsruhe, Germany 9 | // -- END LICENSE BLOCK ------------------------------------------------ 10 | 11 | //---------------------------------------------------------------------- 12 | /*!\file 13 | * 14 | * This file contains the Implementation of HardwarePiDriver. 15 | * 16 | * \author Christian Eichmann 17 | * \author Philip Keller 18 | * \date 2020-04-06 19 | * 20 | */ 21 | //---------------------------------------------------------------------- 22 | 23 | #include "pi_hexapod_control/pi_driver.h" 24 | #include "pi_hexapod_control/log.h" 25 | #include "pi_hexapod_control/pi_errors.h" 26 | #include 27 | #include 28 | 29 | namespace pi_hexapod_control { 30 | 31 | std::string PiDriver::getErrorMessage() 32 | { 33 | auto error_obj = g_PI_ERRORS_UMAP.find(error_id_); 34 | if (error_obj != g_PI_ERRORS_UMAP.end()) 35 | { 36 | // handle common error with elaborate log message 37 | if (error_id_ == PI_UNEXPECTED_RESPONSE) 38 | { 39 | LOG_ERROR_STREAM("# PI-Hexapod-Error (" 40 | << error_id_ << "): Maybe the Communication Timeout is too small."); 41 | } 42 | 43 | // return error message string 44 | return error_obj->second; 45 | } 46 | 47 | LOG_ERROR_STREAM("Unknown Error with ID: " << error_id_); 48 | return "Unknown error"; 49 | } 50 | 51 | HardwarePiDriver::HardwarePiDriver(const uint32_t communication_timeout_ms, 52 | const std::string& hexapod_ip, 53 | const uint32_t hexapod_port) 54 | : using_tcp_ip_connection_(true) 55 | , using_rs232_connection_(false) 56 | , hexapod_ip_(hexapod_ip) 57 | , hexapod_port_(hexapod_port) 58 | , is_connected_(false) 59 | , communication_timeout_ms_(communication_timeout_ms) 60 | { 61 | LOG_DEBUG("Initializing TCP-PiDriver."); 62 | LOG_DEBUG("Initialization done."); 63 | } 64 | 65 | HardwarePiDriver::HardwarePiDriver(const uint32_t communication_timeout_ms, 66 | const int32_t rs232_port_nr, 67 | const int32_t baudrate) 68 | : using_tcp_ip_connection_(false) 69 | , using_rs232_connection_(true) 70 | , rs232_port_nr_(rs232_port_nr) 71 | , baudrate_(baudrate) 72 | , is_connected_(false) 73 | , communication_timeout_ms_(communication_timeout_ms) 74 | { 75 | LOG_DEBUG("Initializing Serial-PiDriver."); 76 | LOG_DEBUG("Initialization done."); 77 | } 78 | 79 | HardwarePiDriver::~HardwarePiDriver() 80 | { 81 | LOG_DEBUG("Destroying PiDriver."); 82 | if (is_connected_) 83 | { 84 | disconnect(); 85 | } 86 | } 87 | 88 | bool HardwarePiDriver::connect() 89 | { 90 | // setup connection 91 | if (using_tcp_ip_connection_) 92 | { 93 | LOG_INFO_STREAM("Calling PI_ConnectTCPIP with " << const_cast(hexapod_ip_.c_str()) 94 | << " and " << hexapod_port_ << "."); 95 | pi_id_ = PI_ConnectTCPIP(const_cast(hexapod_ip_.c_str()), hexapod_port_); 96 | LOG_INFO_STREAM("PI_ConnectTCPIP returned " << pi_id_ << "."); 97 | } 98 | else if (using_rs232_connection_) 99 | { 100 | LOG_INFO_STREAM("Calling PI_ConnectRS232 with " << rs232_port_nr_ << " and " << baudrate_ 101 | << "."); 102 | pi_id_ = PI_ConnectRS232(rs232_port_nr_, baudrate_); 103 | LOG_INFO_STREAM("PI_ConnectRS232 returned " << pi_id_ << "."); 104 | } 105 | else 106 | { 107 | LOG_ERROR("Communication protocol not specified."); 108 | return false; 109 | } 110 | 111 | // pi_id_ below 0 indicates failure 112 | if (pi_id_ < 0) 113 | { 114 | LOG_ERROR_STREAM("Invalid Hexapod ID: " << pi_id_); 115 | LOG_ERROR_STREAM_COND(using_tcp_ip_connection_, 116 | "Connecting with the Hexapod failed with TCP/IP " 117 | << hexapod_ip_.c_str() << ":" << hexapod_port_); 118 | LOG_ERROR_STREAM_COND(using_rs232_connection_, 119 | "Connecting with the Hexapod failed with RS232 Port " 120 | << rs232_port_nr_ << " and baudrate " << baudrate_); 121 | updateError(); 122 | return false; 123 | } 124 | 125 | // configure timeout and error checking behavior 126 | PI_SetTimeout(pi_id_, communication_timeout_ms_); 127 | PI_EnableReconnect(pi_id_, FALSE); 128 | PI_SetNrTimeoutsBeforeClose(pi_id_, 1); 129 | BOOL prev_error_checking = PI_SetErrorCheck(pi_id_, error_checking_ ? TRUE : FALSE); 130 | LOG_INFO_STREAM("Hexapod-ErrorCheck is set to " << std::boolalpha << error_checking_ 131 | << ", it was: " << (prev_error_checking == TRUE)); 132 | 133 | is_connected_ = true; 134 | return true; 135 | } 136 | 137 | bool HardwarePiDriver::requestControllerData(vector6d_t& values) 138 | { 139 | // request current position 140 | if (PI_qPOS(pi_id_, axis_, joint_pos_) == FALSE) 141 | { 142 | LOG_ERROR("Requesting Data from the Hexapod failed."); 143 | updateError(); 144 | return false; 145 | } 146 | 147 | // transform values from millimeter to meter 148 | values[0] = joint_pos_[0] / 1000; 149 | values[1] = joint_pos_[1] / 1000; 150 | values[2] = joint_pos_[2] / 1000; 151 | // transform values from degress to radians 152 | values[3] = joint_pos_[3] * M_PI / 180; 153 | values[4] = joint_pos_[4] * M_PI / 180; 154 | values[5] = joint_pos_[5] * M_PI / 180; 155 | return true; 156 | } 157 | 158 | bool HardwarePiDriver::writeControllerCommand(const vector6d_t& values) 159 | { 160 | if (!isReferenced()) 161 | { 162 | LOG_WARN_ONCE("Hexapod is not referenced. Writing commands not available in this state."); 163 | return false; 164 | } 165 | 166 | vector6d_t values_in_mm_and_degree; 167 | // transform values from meter to millimeter 168 | values_in_mm_and_degree[0] = values[0] * 1000; 169 | values_in_mm_and_degree[1] = values[1] * 1000; 170 | values_in_mm_and_degree[2] = values[2] * 1000; 171 | // transform values from radians to degrees 172 | values_in_mm_and_degree[3] = values[3] / M_PI * 180; 173 | values_in_mm_and_degree[4] = values[4] / M_PI * 180; 174 | values_in_mm_and_degree[5] = values[5] / M_PI * 180; 175 | 176 | // command move to absolute position 177 | if (PI_MOV(pi_id_, axis_, values_in_mm_and_degree.data()) == FALSE) 178 | { 179 | LOG_ERROR("Writing Command to the Hexapod failed."); 180 | updateError(); 181 | return false; 182 | } 183 | 184 | if (!error_checking_) 185 | { 186 | updateError(); 187 | } 188 | 189 | return true; 190 | } 191 | 192 | void HardwarePiDriver::disconnect() 193 | { 194 | PI_CloseConnection(pi_id_); 195 | LOG_INFO("Disconnected from Hexapod Controller."); 196 | is_connected_ = false; 197 | } 198 | 199 | bool HardwarePiDriver::updateReferencedStatus() 200 | { 201 | int referenced; 202 | char x_axis[2] = "X"; 203 | if (!is_connected_) 204 | { 205 | LOG_ERROR("Hexapod-Reference request failed, because there is no connection."); 206 | updateError(); 207 | return false; 208 | } 209 | 210 | // query referencing status 211 | if (PI_qFRF(pi_id_, axis_, &referenced) == FALSE) 212 | { 213 | LOG_ERROR("Hexapod-Reference request failed."); 214 | updateError(); 215 | return false; 216 | } 217 | 218 | is_referenced_ = (referenced == TRUE); 219 | LOG_INFO_STREAM("Hexapod-Reference request successful. Status: " << std::boolalpha 220 | << is_referenced_); 221 | 222 | return true; 223 | } 224 | 225 | bool HardwarePiDriver::reference() 226 | { 227 | updateReferencedStatus(); 228 | 229 | if (isReferenced()) 230 | { 231 | LOG_INFO("Hexapod is already referenced."); 232 | return true; 233 | } 234 | 235 | // reference the axis using the refence switch 236 | int flag; 237 | char x_axis[2] = "X"; 238 | LOG_INFO("Referencing hexapod."); 239 | if (PI_FRF(pi_id_, x_axis) == FALSE) 240 | { 241 | LOG_ERROR("Hexapod Axis-Referencing failed."); 242 | updateError(); 243 | return false; 244 | } 245 | 246 | // Wait until the reference move is done. 247 | flag = 0; 248 | while (flag == 0) 249 | { 250 | if (PI_IsControllerReady(pi_id_, &flag) == FALSE) 251 | { 252 | LOG_ERROR("Hexapod Axis-Referencing failed to finish."); 253 | updateError(); 254 | return false; 255 | } 256 | } 257 | 258 | updateReferencedStatus(); 259 | 260 | LOG_INFO("Hexapod Axis Referencing completed."); 261 | return true; 262 | } 263 | 264 | bool HardwarePiDriver::haltHexapod() 265 | { 266 | BOOL response = PI_HLT(pi_id_, axis_); 267 | if (response == TRUE) 268 | { 269 | LOG_INFO("Halted the Hexapod."); 270 | updateError(); 271 | } 272 | else 273 | { 274 | LOG_ERROR("Failed to halt the Hexapod."); 275 | updateError(); 276 | } 277 | return (response == TRUE); 278 | } 279 | 280 | bool HardwarePiDriver::stopHexapod() 281 | { 282 | BOOL response = PI_STP(pi_id_); 283 | if (response == TRUE) 284 | { 285 | LOG_INFO("Stopped the Hexapod."); 286 | updateError(); 287 | } 288 | else 289 | { 290 | LOG_ERROR("Failed to stop the Hexapod."); 291 | updateError(); 292 | } 293 | return (response == TRUE); 294 | } 295 | 296 | void HardwarePiDriver::updateError() 297 | { 298 | error_id_ = PI_GetError(pi_id_); 299 | } 300 | 301 | } // namespace pi_hexapod_control 302 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pi_hexapod_control) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | option(PI_GCS2_PATH "Optional: path to the shared version of pi_pi_gcs2.s.o" OFF) 8 | 9 | ## Find catkin macros and libraries 10 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 11 | ## is used, also find other catkin packages 12 | find_package(catkin REQUIRED COMPONENTS 13 | actionlib 14 | control_msgs 15 | controller_manager 16 | geometry_msgs 17 | hardware_interface 18 | pi_hexapod_msgs 19 | interactive_markers 20 | pluginlib 21 | position_controllers 22 | realtime_tools 23 | roscpp 24 | std_msgs 25 | std_srvs 26 | tf2_eigen 27 | ) 28 | 29 | ## System dependencies are found with CMake's conventions 30 | # find_package(Boost REQUIRED COMPONENTS system) 31 | 32 | 33 | ## Uncomment this if the package has a setup.py. This macro ensures 34 | ## modules and global scripts declared therein get installed 35 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 36 | # catkin_python_setup() 37 | 38 | ################################################ 39 | ## Declare ROS messages, services and actions ## 40 | ################################################ 41 | 42 | ## To declare and build messages, services or actions from within this 43 | ## package, follow these steps: 44 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 45 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 46 | ## * In the file package.xml: 47 | ## * add a build_depend tag for "message_generation" 48 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 49 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 50 | ## but can be declared for certainty nonetheless: 51 | ## * add a exec_depend tag for "message_runtime" 52 | ## * In this file (CMakeLists.txt): 53 | ## * add "message_generation" and every package in MSG_DEP_SET to 54 | ## find_package(catkin REQUIRED COMPONENTS ...) 55 | ## * add "message_runtime" and every package in MSG_DEP_SET to 56 | ## catkin_package(CATKIN_DEPENDS ...) 57 | ## * uncomment the add_*_files sections below as needed 58 | ## and list every .msg/.srv/.action file to be processed 59 | ## * uncomment the generate_messages entry below 60 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 61 | 62 | ## Generate messages in the 'msg' folder 63 | # add_message_files( 64 | # FILES 65 | # Message1.msg 66 | # Message2.msg 67 | # ) 68 | 69 | ## Generate services in the 'srv' folder 70 | # add_service_files( 71 | # FILES 72 | # Service1.srv 73 | # Service2.srv 74 | # ) 75 | 76 | ## Generate actions in the 'action' folder 77 | # add_action_files( 78 | # DIRECTORY action 79 | # FILES 80 | # Action1.action 81 | # Action2.action 82 | # ) 83 | 84 | ## Generate added messages and services with any dependencies listed here 85 | # generate_messages( 86 | # DEPENDENCIES 87 | # actionlib_msgs 88 | # ) 89 | 90 | ################################################ 91 | ## Declare ROS dynamic reconfigure parameters ## 92 | ################################################ 93 | 94 | ## To declare and build dynamic reconfigure parameters within this 95 | ## package, follow these steps: 96 | ## * In the file package.xml: 97 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 98 | ## * In this file (CMakeLists.txt): 99 | ## * add "dynamic_reconfigure" to 100 | ## find_package(catkin REQUIRED COMPONENTS ...) 101 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 102 | ## and list every .cfg file to be processed 103 | 104 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 105 | # generate_dynamic_reconfigure_options( 106 | # cfg/DynReconf1.cfg 107 | # cfg/DynReconf2.cfg 108 | # ) 109 | 110 | ################################### 111 | ## catkin specific configuration ## 112 | ################################### 113 | ## The catkin_package macro generates cmake config files for your package 114 | ## Declare things to be passed to dependent projects 115 | ## INCLUDE_DIRS: uncomment this if your package contains header files 116 | ## LIBRARIES: libraries you create in this project that dependent projects also need 117 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 118 | ## DEPENDS: system dependencies of this project that dependent projects also need 119 | catkin_package( 120 | INCLUDE_DIRS include 121 | LIBRARIES pi_hexapod_control 122 | CATKIN_DEPENDS 123 | actionlib 124 | control_msgs 125 | geometry_msgs 126 | hardware_interface 127 | pi_hexapod_msgs 128 | roscpp 129 | std_msgs 130 | std_srvs 131 | tf2_eigen 132 | # DEPENDS system_lib 133 | ) 134 | 135 | ########### 136 | ## Build ## 137 | ########### 138 | 139 | ## Specify additional locations of header files 140 | ## Your package locations should be listed before other locations 141 | include_directories( 142 | include 143 | ${catkin_INCLUDE_DIRS} 144 | ) 145 | 146 | # PiDriver as library 147 | add_library(pi_hexapod_driver 148 | src/pi_errors.cpp 149 | src/pi_driver.cpp 150 | ) 151 | if(NOT PI_GCS2_PATH) 152 | # PI_GCS2_PATH is 0, OFF, NO, FALSE, N, IGNORE, NOTFOUND, an empty string 153 | target_link_libraries(pi_hexapod_driver 154 | ${CMAKE_CURRENT_SOURCE_DIR}/lib/libpi_pi_gcs2-3.11.0.a 155 | ) 156 | else() 157 | # PI_GCS2_PATH is set 158 | add_library(pi_pi_gcs2 STATIC IMPORTED) 159 | set_target_properties(pi_pi_gcs2 PROPERTIES 160 | IMPORTED_LOCATION "${PI_GCS2_PATH}") 161 | target_link_libraries(pi_hexapod_driver 162 | pi_pi_gcs2 163 | ) 164 | endif() 165 | target_link_libraries(pi_hexapod_driver 166 | z 167 | usb-1.0 168 | ${catkin_LIBRARIES} 169 | ) 170 | 171 | add_dependencies(pi_hexapod_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 172 | 173 | # HardwareInterface as library 174 | add_library(pi_hexapod_control_plugin 175 | src/hardware_interface.cpp 176 | src/visual_joint_generator.cpp 177 | ) 178 | target_link_libraries(pi_hexapod_control_plugin 179 | pi_hexapod_driver 180 | ${catkin_LIBRARIES} 181 | ) 182 | add_dependencies(pi_hexapod_control_plugin ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 183 | 184 | # Control Node as executable 185 | add_executable(pi_hexapod_control_node 186 | src/hardware_interface.cpp 187 | src/hardware_interface_node.cpp 188 | src/visual_joint_generator.cpp 189 | ) 190 | target_link_libraries(pi_hexapod_control_node 191 | ${catkin_LIBRARIES} 192 | pi_hexapod_driver 193 | ) 194 | add_dependencies(pi_hexapod_control_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 195 | 196 | # Interactive Marker Node as executable 197 | add_executable(interactive_marker_node src/interactive_marker_node.cpp) 198 | target_link_libraries(interactive_marker_node 199 | ${catkin_LIBRARIES} 200 | ) 201 | 202 | ## Add cmake target dependencies of the library 203 | ## as an example, code may need to be generated before libraries 204 | ## either from message generation or dynamic reconfigure 205 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 206 | 207 | ## Rename C++ executable without prefix 208 | ## The above recommended prefix causes long target names, the following renames the 209 | ## target back to the shorter version for ease of user use 210 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 211 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 212 | 213 | ## Add cmake target dependencies of the executable 214 | ## same as for the library above 215 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 216 | 217 | ############# 218 | ## Install ## 219 | ############# 220 | 221 | # all install targets should use catkin DESTINATION variables 222 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 223 | 224 | ## Mark executable scripts (Python etc.) for installation 225 | ## in contrast to setup.py, you can choose the destination 226 | # install(PROGRAMS 227 | # scripts/my_python_script 228 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 229 | # ) 230 | 231 | ## Mark libraries for installation 232 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 233 | install(TARGETS pi_hexapod_driver 234 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 235 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 236 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 237 | ) 238 | 239 | ## Mark executables for installation 240 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 241 | install(TARGETS pi_hexapod_control_node interactive_marker_node 242 | RUNTIME DESTINATION 243 | ${CATKIN_PACKAGE_BIN_DESTINATION} 244 | ) 245 | 246 | ## Mark cpp header files for installation 247 | install(DIRECTORY include/${PROJECT_NAME}/ 248 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 249 | FILES_MATCHING PATTERN "*.h" 250 | PATTERN ".svn" EXCLUDE 251 | ) 252 | 253 | install(DIRECTORY cfg launch 254 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 255 | 256 | install(FILES hardware_interface_plugin.xml 257 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 258 | ) 259 | 260 | ## Mark other files for installation (e.g. launch and bag files, etc.) 261 | # install(FILES 262 | # # myfile1 263 | # # myfile2 264 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 265 | # ) 266 | 267 | ############# 268 | ## Testing ## 269 | ############# 270 | 271 | ## Add gtest based cpp test target and link libraries 272 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_pi_hexapod_control.cpp) 273 | # if(TARGET ${PROJECT_NAME}-test) 274 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 275 | # endif() 276 | 277 | ## Add folders to be run by python nosetests 278 | # catkin_add_nosetests(test) 279 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [2020] [Physik Instrumente (PI) GmbH & Co. KG] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/cfg/default_hexapod.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1/Frames1 10 | - /TF1/Tree1 11 | Splitter Ratio: 0.5 12 | Tree Height: 726 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | - /Current View1/Focal Point1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: true 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 10 56 | Reference Frame: 57 | Value: true 58 | - Class: rviz/TF 59 | Enabled: false 60 | Frame Timeout: 15 61 | Frames: 62 | All Enabled: true 63 | Marker Scale: 0.10000000149011612 64 | Name: TF 65 | Show Arrows: true 66 | Show Axes: true 67 | Show Names: true 68 | Tree: 69 | {} 70 | Update Interval: 0 71 | Value: false 72 | - Alpha: 1 73 | Class: rviz/RobotModel 74 | Collision Enabled: false 75 | Enabled: true 76 | Links: 77 | All Links Enabled: true 78 | Expand Joint Details: false 79 | Expand Link Details: false 80 | Expand Tree: false 81 | Link Tree Style: Links in Alphabetic Order 82 | axis0_base_anchor_link_1: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | axis0_base_anchor_link_2: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | axis0_base_anchor_link_3: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | Value: true 95 | axis0_platform_anchor_link_1: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | axis0_platform_anchor_link_2: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | axis0_platform_anchor_link_3: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | axis1_base_anchor_link_1: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | axis1_base_anchor_link_2: 113 | Alpha: 1 114 | Show Axes: false 115 | Show Trail: false 116 | axis1_base_anchor_link_3: 117 | Alpha: 1 118 | Show Axes: false 119 | Show Trail: false 120 | Value: true 121 | axis1_platform_anchor_link_1: 122 | Alpha: 1 123 | Show Axes: false 124 | Show Trail: false 125 | axis1_platform_anchor_link_2: 126 | Alpha: 1 127 | Show Axes: false 128 | Show Trail: false 129 | axis1_platform_anchor_link_3: 130 | Alpha: 1 131 | Show Axes: false 132 | Show Trail: false 133 | Value: true 134 | axis2_base_anchor_link_1: 135 | Alpha: 1 136 | Show Axes: false 137 | Show Trail: false 138 | axis2_base_anchor_link_2: 139 | Alpha: 1 140 | Show Axes: false 141 | Show Trail: false 142 | axis2_base_anchor_link_3: 143 | Alpha: 1 144 | Show Axes: false 145 | Show Trail: false 146 | Value: true 147 | axis2_platform_anchor_link_1: 148 | Alpha: 1 149 | Show Axes: false 150 | Show Trail: false 151 | axis2_platform_anchor_link_2: 152 | Alpha: 1 153 | Show Axes: false 154 | Show Trail: false 155 | axis2_platform_anchor_link_3: 156 | Alpha: 1 157 | Show Axes: false 158 | Show Trail: false 159 | Value: true 160 | axis3_base_anchor_link_1: 161 | Alpha: 1 162 | Show Axes: false 163 | Show Trail: false 164 | axis3_base_anchor_link_2: 165 | Alpha: 1 166 | Show Axes: false 167 | Show Trail: false 168 | axis3_base_anchor_link_3: 169 | Alpha: 1 170 | Show Axes: false 171 | Show Trail: false 172 | Value: true 173 | axis3_platform_anchor_link_1: 174 | Alpha: 1 175 | Show Axes: false 176 | Show Trail: false 177 | axis3_platform_anchor_link_2: 178 | Alpha: 1 179 | Show Axes: false 180 | Show Trail: false 181 | axis3_platform_anchor_link_3: 182 | Alpha: 1 183 | Show Axes: false 184 | Show Trail: false 185 | Value: true 186 | axis4_base_anchor_link_1: 187 | Alpha: 1 188 | Show Axes: false 189 | Show Trail: false 190 | axis4_base_anchor_link_2: 191 | Alpha: 1 192 | Show Axes: false 193 | Show Trail: false 194 | axis4_base_anchor_link_3: 195 | Alpha: 1 196 | Show Axes: false 197 | Show Trail: false 198 | Value: true 199 | axis4_platform_anchor_link_1: 200 | Alpha: 1 201 | Show Axes: false 202 | Show Trail: false 203 | axis4_platform_anchor_link_2: 204 | Alpha: 1 205 | Show Axes: false 206 | Show Trail: false 207 | axis4_platform_anchor_link_3: 208 | Alpha: 1 209 | Show Axes: false 210 | Show Trail: false 211 | Value: true 212 | axis5_base_anchor_link_1: 213 | Alpha: 1 214 | Show Axes: false 215 | Show Trail: false 216 | axis5_base_anchor_link_2: 217 | Alpha: 1 218 | Show Axes: false 219 | Show Trail: false 220 | axis5_base_anchor_link_3: 221 | Alpha: 1 222 | Show Axes: false 223 | Show Trail: false 224 | Value: true 225 | axis5_platform_anchor_link_1: 226 | Alpha: 1 227 | Show Axes: false 228 | Show Trail: false 229 | axis5_platform_anchor_link_2: 230 | Alpha: 1 231 | Show Axes: false 232 | Show Trail: false 233 | axis5_platform_anchor_link_3: 234 | Alpha: 1 235 | Show Axes: false 236 | Show Trail: false 237 | Value: true 238 | base_link: 239 | Alpha: 1 240 | Show Axes: false 241 | Show Trail: false 242 | Value: true 243 | platform_link: 244 | Alpha: 1 245 | Show Axes: false 246 | Show Trail: false 247 | Value: true 248 | u_link: 249 | Alpha: 1 250 | Show Axes: false 251 | Show Trail: false 252 | v_link: 253 | Alpha: 1 254 | Show Axes: false 255 | Show Trail: false 256 | w_link: 257 | Alpha: 1 258 | Show Axes: false 259 | Show Trail: false 260 | x_link: 261 | Alpha: 1 262 | Show Axes: false 263 | Show Trail: false 264 | y_link: 265 | Alpha: 1 266 | Show Axes: false 267 | Show Trail: false 268 | z_link: 269 | Alpha: 1 270 | Show Axes: false 271 | Show Trail: false 272 | zero_link: 273 | Alpha: 1 274 | Show Axes: false 275 | Show Trail: false 276 | Name: RobotModel 277 | Robot Description: robot_description 278 | TF Prefix: "" 279 | Update Interval: 0 280 | Value: true 281 | Visual Enabled: true 282 | - Class: rviz/InteractiveMarkers 283 | Enable Transparency: true 284 | Enabled: true 285 | Name: InteractiveMarkers 286 | Show Axes: false 287 | Show Descriptions: true 288 | Show Visual Aids: false 289 | Update Topic: /pi_interactive_marker/update 290 | Value: true 291 | Enabled: true 292 | Global Options: 293 | Background Color: 128; 128; 128 294 | Default Light: true 295 | Fixed Frame: base_link 296 | Frame Rate: 30 297 | Name: root 298 | Tools: 299 | - Class: rviz/Interact 300 | Hide Inactive Objects: true 301 | - Class: rviz/MoveCamera 302 | - Class: rviz/Select 303 | - Class: rviz/FocusCamera 304 | - Class: rviz/Measure 305 | - Class: rviz/SetInitialPose 306 | Theta std deviation: 0.2617993950843811 307 | Topic: /initialpose 308 | X std deviation: 0.5 309 | Y std deviation: 0.5 310 | - Class: rviz/SetGoal 311 | Topic: /move_base_simple/goal 312 | - Class: rviz/PublishPoint 313 | Single click: true 314 | Topic: /clicked_point 315 | Value: true 316 | Views: 317 | Current: 318 | Class: rviz/Orbit 319 | Distance: 1.211824655532837 320 | Enable Stereo Rendering: 321 | Stereo Eye Separation: 0.05999999865889549 322 | Stereo Focal Distance: 1 323 | Swap Stereo Eyes: false 324 | Value: false 325 | Focal Point: 326 | X: 0 327 | Y: 0 328 | Z: 0.10000000149011612 329 | Focal Shape Fixed Size: true 330 | Focal Shape Size: 0.05000000074505806 331 | Invert Z Axis: false 332 | Name: Current View 333 | Near Clip Distance: 0.009999999776482582 334 | Pitch: 0.6510100960731506 335 | Target Frame: 336 | Value: Orbit (rviz) 337 | Yaw: 0.5719151496887207 338 | Saved: ~ 339 | Window Geometry: 340 | Displays: 341 | collapsed: true 342 | Height: 1023 343 | Hide Left Dock: true 344 | Hide Right Dock: true 345 | QMainWindow State: 000000ff00000000fd00000004000000000000025600000361fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000361000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001a8000001a60000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000354000000410000000000000000000000010000010f00000361fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000361000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003bc0000003efc0100000002fb0000000800540069006d00650100000000000003bc000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003bc0000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 346 | Selection: 347 | collapsed: false 348 | Time: 349 | collapsed: false 350 | Tool Properties: 351 | collapsed: false 352 | Views: 353 | collapsed: true 354 | Width: 956 355 | X: 960 356 | Y: 27 357 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/src/hardware_interface.cpp: -------------------------------------------------------------------------------- 1 | // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- 2 | 3 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 4 | // This file is part of the PI-Hexapod Driver. 5 | // 6 | // 7 | // © Copyright 2020 Physik Instrumente (PI) GmbH & Co. KG, Karlsruhe, Germany 8 | // © Copyright 2020 FZI Forschungszentrum Informatik, Karlsruhe, Germany 9 | // -- END LICENSE BLOCK ------------------------------------------------ 10 | 11 | //---------------------------------------------------------------------- 12 | /*!\file 13 | * 14 | * This file contains the implementation for the HardwareInterface (hardware_interface::RobotHW) 15 | * for PI Hexapods. 16 | * 17 | * \author Christian Eichmann 18 | * \author Philip Keller 19 | * \date 2020-04-06 20 | * 21 | */ 22 | //---------------------------------------------------------------------- 23 | #include "pi_hexapod_control/hardware_interface.h" 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace pi_hexapod_control { 31 | HardwareInterface::HardwareInterface() 32 | : joint_names_(6) 33 | , visual_joint_names_(36) 34 | , position_controller_running_(false) 35 | , referencing_running_(false) 36 | , prefer_stop_over_halt_(false) 37 | , control_mode_enabled_(true) 38 | , latest_error_id_(0) 39 | { 40 | joint_position_command_.fill(0.0); 41 | joint_positions_.fill(0.0); 42 | joint_velocities_.fill(0.0); 43 | joint_efforts_.fill(0.0); 44 | 45 | visual_joint_positions_.fill(0.0); 46 | visual_joint_velocities_.fill(0.0); 47 | visual_joint_efforts_.fill(0.0); 48 | } 49 | 50 | bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) 51 | { 52 | if (!visual_joint_generator_.init(root_nh, robot_hw_nh)) 53 | { 54 | ROS_ERROR_STREAM( 55 | "Hexapod Visualization will fail: Relevant parameters were not configured properly."); 56 | } 57 | 58 | /* 59 | * Get relevant parameter from the parameter server 60 | */ 61 | bool using_tcp_ip_connection = robot_hw_nh.param("using_tcp_ip_connection", false); 62 | bool using_rs232_connection = robot_hw_nh.param("using_rs232_connection", false); 63 | std::string hexapod_ip; 64 | int32_t rs232_port_nr, baudrate; 65 | 66 | // check whether or not the connection type is clearly specified 67 | if (using_tcp_ip_connection == using_rs232_connection) 68 | { 69 | ROS_ERROR_COND( 70 | (!using_tcp_ip_connection && !using_rs232_connection), 71 | "Either using_tcp_ip_connection or using_rs232_connection parameter needs to be set."); 72 | ROS_ERROR_COND( 73 | (using_tcp_ip_connection && using_rs232_connection), 74 | "Only one of using_tcp_ip_connection and using_rs232_connection parameter can be set."); 75 | return false; 76 | } 77 | 78 | // if using TCP/IP check if IP is specified 79 | if (using_tcp_ip_connection) 80 | { 81 | if (!robot_hw_nh.getParam("hexapod_ip", hexapod_ip)) 82 | { 83 | ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("hexapod_ip") 84 | << " not given."); 85 | return false; 86 | } 87 | } 88 | 89 | // if using RS232 check if port number and baut rate are specified 90 | if (using_rs232_connection) 91 | { 92 | if (!robot_hw_nh.getParam("rs232_port_nr", rs232_port_nr)) 93 | { 94 | ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("rs232_port_nr") 95 | << " not given."); 96 | return false; 97 | } 98 | if (!robot_hw_nh.getParam("baudrate", baudrate)) 99 | { 100 | ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("baudrate") 101 | << " not given."); 102 | return false; 103 | } 104 | } 105 | 106 | // Get joint names 107 | if (!root_nh.getParam("hardware_interface/joints", joint_names_)) 108 | { 109 | ROS_ERROR_STREAM("Cannot find required parameter " 110 | << root_nh.resolveName("hardware_interface/joints") 111 | << " on the parameter server."); 112 | return false; 113 | } 114 | 115 | // get communication_timeout_ms from parameter server 116 | int32_t communication_timeout_ms = robot_hw_nh.param("communication_timeout_ms", 7000); 117 | if (communication_timeout_ms <= 0) 118 | { 119 | ROS_WARN_STREAM("Invalid communication_timeout_ms of " << communication_timeout_ms 120 | << " ms, using 7000 ms instead."); 121 | communication_timeout_ms = 7000; 122 | } 123 | ROS_INFO_STREAM("Parameter communication_timeout_ms is set to: " << communication_timeout_ms 124 | << " ms."); 125 | 126 | 127 | // additional parameter 128 | uint32_t hexapod_port = robot_hw_nh.param("hexapod_port", 50000); 129 | bool is_auto_referencing = robot_hw_nh.param("auto_referencing", false); 130 | bool is_sim = robot_hw_nh.param("is_sim", false); 131 | prefer_stop_over_halt_ = robot_hw_nh.param("prefer_stop_over_halt", false); 132 | 133 | ROS_INFO("Initializing hexapod driver"); 134 | ROS_DEBUG_COND( 135 | prefer_stop_over_halt_, 136 | "Prefering STOP over HALT. All calls to haltHexapod() will send STP instead of HLT commands"); 137 | 138 | /* 139 | * Setup Pi Driver 140 | */ 141 | 142 | if (is_sim) 143 | { 144 | ROS_INFO("Simulation mode active, no real Hexapod is used."); 145 | pi_driver_.reset(new SimulatedPiDriver()); 146 | } 147 | else 148 | { 149 | ROS_INFO("Real Hexapod is used, this is no simulation."); 150 | if (using_tcp_ip_connection) 151 | { 152 | pi_driver_.reset(new HardwarePiDriver(communication_timeout_ms, hexapod_ip, hexapod_port)); 153 | } 154 | else if (using_rs232_connection) 155 | { 156 | pi_driver_.reset(new HardwarePiDriver(communication_timeout_ms, rs232_port_nr, baudrate)); 157 | } 158 | else 159 | { 160 | ROS_ERROR("Pi Driver Constructor unclear."); 161 | return false; 162 | } 163 | } 164 | if (!root_nh.getParam("hardware_interface/visual_joints", visual_joint_names_)) 165 | { 166 | ROS_ERROR_STREAM("Cannot find required parameter " 167 | << root_nh.resolveName("hardware_interface/visual_joints") 168 | << " on the parameter server."); 169 | return false; 170 | } 171 | 172 | // Setup Driver Error Publishers (latch = true) 173 | pub_error_id_ = robot_hw_nh.advertise("error_id", 10, true); 174 | pub_error_msg_ = robot_hw_nh.advertise("error_msg", 10, true); 175 | 176 | // Create ros_control interfaces 177 | for (std::size_t i = 0; i < joint_names_.size(); ++i) 178 | { 179 | ROS_DEBUG_STREAM("Registering handles for joint " << joint_names_[i]); 180 | 181 | js_interface_.registerHandle(hardware_interface::JointStateHandle( 182 | joint_names_[i], &joint_positions_[i], &joint_velocities_[i], &joint_efforts_[i])); 183 | 184 | pj_interface_.registerHandle(hardware_interface::JointHandle( 185 | js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i])); 186 | } 187 | 188 | for (std::size_t i = 0; i < visual_joint_names_.size(); ++i) 189 | { 190 | ROS_DEBUG_STREAM("Registering only a state handles for joint " << visual_joint_names_[i]); 191 | 192 | js_interface_.registerHandle(hardware_interface::JointStateHandle(visual_joint_names_[i], 193 | &visual_joint_positions_[i], 194 | &visual_joint_velocities_[i], 195 | &visual_joint_efforts_[i])); 196 | } 197 | 198 | registerInterface(&js_interface_); 199 | registerInterface(&pj_interface_); 200 | 201 | // Open connection to hexapod 202 | if (!pi_driver_->connect()) 203 | { 204 | ROS_ERROR("Unable to connect to Hexapod controller, hardware_interface can't be initialized."); 205 | return false; 206 | } 207 | 208 | // Reference hexapod automatically, if required 209 | if (is_auto_referencing) 210 | { 211 | ROS_INFO("Automatic referencing, stand clear of Hexapod hardware!"); 212 | referencing_running_ = true; 213 | bool reference_successful = pi_driver_->reference(); 214 | referencing_running_ = false; 215 | if (!reference_successful) 216 | { 217 | ROS_ERROR("Referencing failed, Hardware Interface was not initialized."); 218 | return false; 219 | } 220 | } 221 | 222 | // Setup referencing Action Server 223 | referencing_action_server_.reset( 224 | new actionlib::SimpleActionServer( 225 | robot_hw_nh, 226 | "referencing", 227 | boost::bind(&HardwareInterface::referencingActionCallback, this, _1), 228 | false)); 229 | referencing_action_server_->start(); 230 | 231 | // Setup enable-control, HALT and STOP Services 232 | enable_srv_ = 233 | robot_hw_nh.advertiseService("enable_control", &HardwareInterface::enableServiceCallback, this); 234 | halt_srv_ = 235 | robot_hw_nh.advertiseService("halt_hexapod", &HardwareInterface::haltServiceCallback, this); 236 | stop_srv_ = 237 | robot_hw_nh.advertiseService("stop_hexapod", &HardwareInterface::stopServiceCallback, this); 238 | 239 | ROS_INFO("Loaded pi_hexapod_control hardware_interface"); 240 | return true; 241 | } 242 | 243 | void HardwareInterface::publishDriverStatus() 244 | { 245 | int current_id = pi_driver_->getErrorID(); 246 | std::string current_msg; 247 | if (latest_error_id_ == current_id) 248 | { 249 | // No need to publish the same error again. 250 | return; 251 | } 252 | 253 | current_msg = pi_driver_->getErrorMessage(); 254 | if (current_id == PI_CNTR_NO_ERROR) 255 | { 256 | ROS_INFO_STREAM("DriverError(" << latest_error_id_ << ") has been resolved."); 257 | } 258 | else 259 | { 260 | ROS_ERROR_STREAM("DriverError(" << current_id << "): " << current_msg); 261 | } 262 | latest_error_id_ = current_id; 263 | 264 | std_msgs::Int32 error_id_msg; 265 | std_msgs::String error_msg_msg; 266 | error_id_msg.data = current_id; 267 | error_msg_msg.data = current_msg; 268 | pub_error_id_.publish(error_id_msg); 269 | pub_error_msg_.publish(error_msg_msg); 270 | } 271 | 272 | void HardwareInterface::referencingActionCallback( 273 | const pi_hexapod_msgs::ReferencingGoalConstPtr& goal) 274 | { 275 | pi_hexapod_msgs::ReferencingResult result; 276 | 277 | // Abort if a position controller is running 278 | if (position_controller_running_) 279 | { 280 | ROS_WARN("Cannot reference because of running position controller"); 281 | result.success = false; 282 | referencing_action_server_->setAborted(result); 283 | return; 284 | } 285 | 286 | ROS_INFO("Referencing, stand clear of Hexapod hardware!"); 287 | referencing_running_ = true; 288 | bool reference_successful = pi_driver_->reference(); 289 | referencing_running_ = false; 290 | if (!reference_successful) 291 | { 292 | ROS_ERROR("Referencing failed, Hardware Interface was not initialized."); 293 | result.success = false; 294 | referencing_action_server_->setAborted(result); 295 | return; 296 | } 297 | ROS_INFO("Referencing successfull"); 298 | result.success = true; 299 | referencing_action_server_->setSucceeded(result); 300 | publishDriverStatus(); 301 | } 302 | 303 | bool HardwareInterface::enableServiceCallback(std_srvs::SetBoolRequest& req, 304 | std_srvs::SetBoolResponse& res) 305 | { 306 | ROS_INFO_STREAM("Control-Mode set to: " << std::boolalpha << (bool)req.data); 307 | control_mode_enabled_ = (bool)req.data; 308 | res.success = true; 309 | return true; 310 | } 311 | 312 | bool HardwareInterface::stopServiceCallback(std_srvs::TriggerRequest& req, 313 | std_srvs::TriggerResponse& res) 314 | { 315 | control_mode_enabled_ = false; 316 | 317 | if (stopHexapod()) 318 | { 319 | res.success = true; 320 | } 321 | else 322 | { 323 | res.success = false; 324 | } 325 | 326 | publishDriverStatus(); 327 | return true; 328 | } 329 | 330 | bool HardwareInterface::haltServiceCallback(std_srvs::TriggerRequest& req, 331 | std_srvs::TriggerResponse& res) 332 | { 333 | control_mode_enabled_ = false; 334 | 335 | if (haltHexapod()) 336 | { 337 | res.success = true; 338 | } 339 | else 340 | { 341 | res.success = false; 342 | } 343 | 344 | publishDriverStatus(); 345 | return true; 346 | } 347 | 348 | void HardwareInterface::read(const ros::Time& time, const ros::Duration& period) 349 | { 350 | pi_driver_->requestControllerData(joint_positions_); 351 | visual_joint_generator_.calculateVisualLinks(joint_positions_, visual_joint_positions_); 352 | } 353 | 354 | void HardwareInterface::write(const ros::Time& time, const ros::Duration& period) 355 | { 356 | if (control_mode_enabled_) 357 | { 358 | pi_driver_->writeControllerCommand(joint_position_command_); 359 | publishDriverStatus(); 360 | } 361 | } 362 | 363 | bool HardwareInterface::prepareSwitch( 364 | const std::list& start_list, 365 | const std::list& stop_list) 366 | { 367 | bool ret_val = true; 368 | if (!start_list.empty()) 369 | { 370 | for (auto& controller : start_list) 371 | { 372 | // check if controller conflicts with referencing 373 | if (!pi_driver_->isReferenced() || referencing_running_) 374 | { 375 | for (auto& resource : controller.claimed_resources) 376 | { 377 | if (resource.hardware_interface == "hardware_interface::PositionJointInterface") 378 | { 379 | ROS_ERROR_STREAM("Controller " << controller.name 380 | << " cannot be started while the hexapod is unreferenced" 381 | << " or currently referencing."); 382 | ret_val = false; 383 | } 384 | } 385 | } 386 | } 387 | } 388 | return ret_val; 389 | } 390 | 391 | void HardwareInterface::doSwitch(const std::list& start_list, 392 | const std::list& stop_list) 393 | { 394 | for (auto& controller : stop_list) 395 | { 396 | for (auto& resource : controller.claimed_resources) 397 | { 398 | if (resource.hardware_interface == "hardware_interface::PositionJointInterface") 399 | { 400 | position_controller_running_ = false; 401 | } 402 | } 403 | } 404 | 405 | for (auto& controller : start_list) 406 | { 407 | for (auto& resource : controller.claimed_resources) 408 | { 409 | if (resource.hardware_interface == "hardware_interface::PositionJointInterface") 410 | { 411 | position_controller_running_ = true; 412 | } 413 | } 414 | } 415 | } 416 | 417 | bool HardwareInterface::haltHexapod() 418 | { 419 | return (prefer_stop_over_halt_ ? pi_driver_->stopHexapod() : pi_driver_->haltHexapod()); 420 | } 421 | 422 | bool HardwareInterface::stopHexapod() 423 | { 424 | return pi_driver_->stopHexapod(); 425 | } 426 | 427 | void HardwareInterface::disconnectHexapod() 428 | { 429 | pi_driver_->disconnect(); 430 | } 431 | 432 | 433 | } // namespace pi_hexapod_control 434 | 435 | PLUGINLIB_EXPORT_CLASS(pi_hexapod_control::HardwareInterface, hardware_interface::RobotHW) 436 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_gui/resource/pi_hexapod_gui.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | Form 4 | 5 | 6 | 7 | 0 8 | 0 9 | 622 10 | 505 11 | 12 | 13 | 14 | Form 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | W 24 | 25 | 26 | 27 | 28 | 29 | 30 | Qt::Horizontal 31 | 32 | 33 | QSlider::TicksBelow 34 | 35 | 36 | 1 37 | 38 | 39 | 40 | 41 | 42 | 43 | Step [deg] 44 | 45 | 46 | 47 | 48 | 49 | 50 | Qt::Horizontal 51 | 52 | 53 | QSlider::TicksBelow 54 | 55 | 56 | 1 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 0 68 | 0 69 | 70 | 71 | 72 | mm 73 | 74 | 75 | 76 | 77 | 78 | 79 | Qt::Horizontal 80 | 81 | 82 | QSlider::TicksBelow 83 | 84 | 85 | 1 86 | 87 | 88 | 89 | 90 | 91 | 92 | Translations [mm] 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 75 101 | 16777215 102 | 103 | 104 | 105 | 0.0 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 75 114 | 16777215 115 | 116 | 117 | 118 | 0.0 119 | 120 | 121 | 122 | 123 | 124 | 125 | Qt::Horizontal 126 | 127 | 128 | QSlider::TicksBelow 129 | 130 | 131 | 1 132 | 133 | 134 | 135 | 136 | 137 | 138 | Rotations [deg] 139 | 140 | 141 | 142 | 143 | 144 | 145 | V 146 | 147 | 148 | 149 | 150 | 151 | 152 | Qt::Horizontal 153 | 154 | 155 | 156 | 40 157 | 20 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | Reference 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 75 174 | 16777215 175 | 176 | 177 | 178 | 0.0 179 | 180 | 181 | 182 | 183 | 184 | 185 | Step [mm] 186 | 187 | 188 | 189 | 190 | 191 | 192 | Z 193 | 194 | 195 | 196 | 197 | 198 | 199 | Sine-Test-Motion 200 | 201 | 202 | 203 | 204 | 205 | 206 | Return-Home 207 | 208 | 209 | 210 | 211 | 212 | 213 | -20 214 | 215 | 216 | 20 217 | 218 | 219 | Qt::Horizontal 220 | 221 | 222 | QSlider::TicksBelow 223 | 224 | 225 | 1 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 0 234 | 0 235 | 236 | 237 | 238 | STOP 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 75 247 | 16777215 248 | 249 | 250 | 251 | 0.0 252 | 253 | 254 | 255 | 256 | 257 | 258 | U 259 | 260 | 261 | 262 | 263 | 264 | 265 | Y 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 15 274 | 16777215 275 | 276 | 277 | 278 | X 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 75 287 | 16777215 288 | 289 | 290 | 291 | 0.0 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 0 300 | 0 301 | 302 | 303 | 304 | deg 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 0 313 | 0 314 | 315 | 316 | 317 | 318 | 30 319 | 16777215 320 | 321 | 322 | 323 | deg 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 0 332 | 0 333 | 334 | 335 | 336 | Halt 337 | 338 | 339 | 340 | 341 | 342 | 343 | Qt::Horizontal 344 | 345 | 346 | QSlider::TicksBelow 347 | 348 | 349 | 1 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 0 358 | 0 359 | 360 | 361 | 362 | deg 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 0 378 | 0 379 | 380 | 381 | 382 | mm 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 75 391 | 16777215 392 | 393 | 394 | 395 | 0.0 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 0 404 | 0 405 | 406 | 407 | 408 | mm 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 0 417 | 0 418 | 419 | 420 | 421 | Enable Control 422 | 423 | 424 | 425 | 426 | 427 | 428 | Referenced: 429 | 430 | 431 | 432 | 433 | 434 | 435 | Error Status: 436 | 437 | 438 | 439 | 440 | 441 | 442 | False 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 24 451 | 75 452 | true 453 | 454 | 455 | 456 | false 457 | 458 | 459 | false 460 | 461 | 462 | 463 | 464 | 465 | 466 | No error 467 | 468 | 469 | Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop 470 | 471 | 472 | 473 | 474 | 475 | 476 | 477 | 478 | 479 | Controller Type: 480 | 481 | 482 | 483 | 484 | 485 | 486 | 487 | 75 488 | true 489 | 490 | 491 | 492 | PI Hexapod Control GUI 493 | 494 | 495 | 496 | 497 | 498 | 499 | 500 | 501 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_description/cfg/H-811.i2.yaml: -------------------------------------------------------------------------------- 1 | # This configuration file describes the kinematics of the H-811.i2 PI Hexapod. 2 | # 3 | # Comments with a line number relate to the corresponding entry in the hexapod-specific datafile 4 | # Values are converted to meters and radians, as it is the standard in ROS. 5 | # Axis can be configured individually. 6 | 7 | base_height: # line 13 (convert mm in m) 8 | 0.088326097 9 | vel_max_xyz: 10 | 0.005 11 | vel_max_uvw: 12 | 0.625 13 | effort_limit: 14 | 300 15 | 16 | 17 | base: 18 | visual: 19 | mesh: 20 | "package://pi_hexapod_description/meshes/H-811/baseplate.stl" 21 | scale: # to convert from mm to m 22 | 0.001 23 | x: # M811_CAD.ini -> [H-811.I2] -> BaseplateOffset_X (conver mm in m) - 0.129 24 | -0.128 25 | y: # M811_CAD.ini -> [H-811.I2] -> BaseplateOffset_Y (conver mm in m) - 0.068 26 | -0.068 27 | z: # M811_CAD.ini -> [H-811.I2] -> BaseplateOffset_Z (conver mm in m) - 0.0085 28 | -0.017 29 | roll: 0.0 30 | pitch: 0.0 31 | yaw: 0.0 32 | collision: 33 | x: 0.0 34 | y: 0.0 35 | z: -0.0085 36 | roll: 0.0 37 | pitch: 0.0 38 | yaw: 0.0 39 | radius: 0.075 40 | length: 0.019 41 | 42 | platform: 43 | outer_radius: # line 117 44 | 0.05 45 | inner_radius: # line 118 46 | 0.025 47 | height: # line 119 48 | 0.009 49 | visual: 50 | mesh: 51 | "package://pi_hexapod_description/meshes/H-811/H_811.I2_platform_with_logo.dae" 52 | scale: # to convert from mm to m 53 | 0.001 54 | x: # M811_CAD.ini -> [H-811.I2] -> PlatformOffset_X (conver mm in m) 55 | 0.0 56 | y: # M811_CAD.ini -> [H-811.I2] -> PlatformOffset_Y (conver mm in m) 57 | 0.0 58 | z: # M811_CAD.ini -> [H-811.I2] -> PlatformOffset_Z (conver mm in m) 59 | 0.0045 60 | roll: 0.0 61 | pitch: 0.0 62 | yaw: 0.0 63 | collision: 64 | x: 0.0 65 | y: 0.0 66 | z: 0.0045 67 | roll: 0.0 68 | pitch: 0.0 69 | yaw: 0.0 70 | radius: 0.055 71 | length: 0.01 72 | 73 | # Configuration of the six Degrees of Freedom 74 | x: 75 | lower_limit: # line 24 (convert mm in m) 76 | -0.0170001 77 | upper_limit: # line 23 (convert mm in m) 78 | 0.0170001 79 | y: 80 | lower_limit: # line 27 (convert mm in m) 81 | -0.0160001 82 | upper_limit: # line 26 (convert mm in m) 83 | 0.0160001 84 | z: 85 | lower_limit: # line 28 (convert mm in m) 86 | -0.0065001 87 | upper_limit: # line 27 (convert mm in m) 88 | 0.0065001 89 | u: 90 | lower_limit: # line 30 (convert deg in rad) 91 | -0.1745346705 92 | upper_limit: # line 29 (convert deg in rad) 93 | 0.1745346705 94 | v: 95 | lower_limit: # line 32 (convert deg in rad) 96 | -0.1745346705 97 | upper_limit: # line 31 (convert deg in rad) 98 | 0.1745346705 99 | w: 100 | lower_limit: # line 34 (convert deg in rad) 101 | -0.3665208882 102 | upper_limit: # line 33 (convert deg in rad) 103 | 0.3665208882 104 | 105 | # Configuration of the six Hardware Axis 106 | axis_0: 107 | base_anchor: 108 | x: # line 75 (convert mm in m) 109 | 0.055773843 110 | y: # line 76 (convert mm in m) 111 | 0.012801635 112 | z: # line 77 (convert mm in m) 113 | -0.088326097 114 | visual: 115 | mesh: 116 | "package://pi_hexapod_description/meshes/H-811/H-811.I2_LowerStrut.stl" 117 | scale: # to convert from mm to m 118 | 0.001 119 | x: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_X (conver mm in m) + 0.0038 120 | 0.0 121 | y: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_Y (conver mm in m) 122 | 0.0 123 | z: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_Z (conver mm in m) + 0.01 124 | 0.037 125 | roll: 1.57079632679 126 | pitch: 0.0 127 | yaw: # pi/2 - cos^-1( (75-38) / root((75-38)^2+(76-39)^2+(77-40)^2) ) 128 | 0.31415924388 129 | collision: 130 | x: 0.0 131 | y: 0.0 132 | z: 0.037 133 | roll: 0.0 134 | pitch: 0.0 135 | yaw: 0.0 136 | radius: 0.014 137 | length: 0.08 138 | platform_anchor: 139 | x: # line 94 (convert mm in m) 140 | 0.029355221 141 | y: # line 95 (convert mm in m) 142 | 0.025682309 143 | z: # line 96 (convert mm in m) 144 | 0.0 145 | visual: 146 | mesh: 147 | "package://pi_hexapod_description/meshes/H-811/H-811.I2_UpperStrut.stl" 148 | scale: # to convert from mm to m 149 | 0.001 150 | x: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_X (conver mm in m) 151 | 0.0 152 | y: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_Y (conver mm in m) 153 | 0.0 154 | z: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_Z (conver mm in m) + 0.01 155 | 0.039 156 | roll: 1.57079632679 157 | pitch: 0.0 158 | yaw: # -cos^-1(...) 159 | -0.837758058361 160 | collision: 161 | x: 0.0 162 | y: 0.0 163 | z: 0.039 164 | roll: 0.0 165 | pitch: 0.0 166 | yaw: 0.0 167 | radius: 0.014 168 | length: 0.08 169 | 170 | 171 | axis_1: 172 | base_anchor: 173 | x: # line 78 (convert mm in m) 174 | -0.016800381 175 | y: # line 79 (convert mm in m) 176 | 0.054702383 177 | z: # line 80 (convert mm in m) 178 | -0.088326097 179 | visual: 180 | mesh: 181 | "package://pi_hexapod_description/meshes/H-811/H-811.I2_LowerStrut.stl" 182 | scale: # to convert from mm to m 183 | 0.001 184 | x: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_X (conver mm in m) + 0.0038 185 | 0.0 186 | y: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_Y (conver mm in m) 187 | 0.0 188 | z: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_Z (conver mm in m) + 0.01 189 | 0.037 190 | roll: 1.57079632679 191 | pitch: 0.0 192 | yaw: # pi/2 + cos^-1(...) 193 | 1.78023590486 194 | collision: 195 | x: 0.0 196 | y: 0.0 197 | z: 0.037 198 | roll: 0.0 199 | pitch: 0.0 200 | yaw: 0.0 201 | radius: 0.014 202 | length: 0.08 203 | platform_anchor: 204 | x: # line 97 (convert mm in m) 205 | 0.007563921 206 | y: # line 98 (convert mm in m) 207 | 0.038263522 208 | z: # line 99 (convert mm in m) 209 | 0.0 210 | visual: 211 | mesh: 212 | "package://pi_hexapod_description/meshes/H-811/H-811.I2_UpperStrut.stl" 213 | scale: # to convert from mm to m 214 | 0.001 215 | x: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_X (conver mm in m) 216 | 0.0 217 | y: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_Y (conver mm in m) 218 | 0.0 219 | z: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_Z (conver mm in m) + 0.01 220 | 0.039 221 | roll: 1.57079632679 222 | pitch: 0.0 223 | yaw: # pi/2 + cos^-1(...) 224 | 1.78023582661 225 | collision: 226 | x: 0.0 227 | y: 0.0 228 | z: 0.039 229 | roll: 0.0 230 | pitch: 0.0 231 | yaw: 0.0 232 | radius: 0.014 233 | length: 0.08 234 | 235 | 236 | axis_2: 237 | base_anchor: 238 | x: # line 81 (convert mm in m) 239 | -0.038973463 240 | y: # line 82 (convert mm in m) 241 | 0.041900747 242 | z: # line 83 (convert mm in m) 243 | -0.088326097 244 | visual: 245 | mesh: 246 | "package://pi_hexapod_description/meshes/H-811/H-811.I2_LowerStrut.stl" 247 | scale: # to convert from mm to m 248 | 0.001 249 | x: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_X (conver mm in m) + 0.0038 250 | 0.0 251 | y: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_Y (conver mm in m) 252 | 0.0 253 | z: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_Z (conver mm in m) + 0.01 254 | 0.037 255 | roll: 1.57079632679 256 | pitch: 0.0 257 | yaw: # pi/2 + cos^-1(...) 258 | 2.40855433163 259 | collision: 260 | x: 0.0 261 | y: 0.0 262 | z: 0.037 263 | roll: 0.0 264 | pitch: 0.0 265 | yaw: 0.0 266 | radius: 0.014 267 | length: 0.08 268 | platform_anchor: 269 | x: # line 100 (convert mm in m) 270 | -0.036919143 271 | y: # line 101 (convert mm in m) 272 | 0.012581213 273 | z: # line 102 (convert mm in m) 274 | 0.0 275 | visual: 276 | mesh: 277 | "package://pi_hexapod_description/meshes/H-811/H-811.I2_UpperStrut.stl" 278 | scale: # to convert from mm to m 279 | 0.001 280 | x: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_X (conver mm in m) 281 | 0.0 282 | y: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_Y (conver mm in m) 283 | 0.0 284 | z: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_Z (conver mm in m) + 0.01 285 | 0.039 286 | roll: 1.57079632679 287 | pitch: 0.0 288 | yaw: # pi/2 - cos^-1(...) 289 | 0.314159219161 290 | collision: 291 | x: 0.0 292 | y: 0.0 293 | z: 0.039 294 | roll: 0.0 295 | pitch: 0.0 296 | yaw: 0.0 297 | radius: 0.014 298 | length: 0.08 299 | 300 | 301 | axis_3: 302 | base_anchor: 303 | x: # line 84 (convert mm in m) 304 | -0.038973463 305 | y: # line 85 (convert mm in m) 306 | -0.041900748 307 | z: # line 86 (convert mm in m) 308 | -0.088326097 309 | visual: 310 | mesh: 311 | "package://pi_hexapod_description/meshes/H-811/H-811.I2_LowerStrut.stl" 312 | scale: # to convert from mm to m 313 | 0.001 314 | x: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_X (conver mm in m) + 0.0038 315 | 0.0 316 | y: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_Y (conver mm in m) 317 | 0.0 318 | z: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_Z (conver mm in m) + 0.01 319 | 0.037 320 | roll: 1.57079632679 321 | pitch: 0.0 322 | yaw: # pi + cos^-1(...) 323 | 3.97935071195 324 | collision: 325 | x: 0.0 326 | y: 0.0 327 | z: 0.037 328 | roll: 0.0 329 | pitch: 0.0 330 | yaw: 0.0 331 | radius: 0.014 332 | length: 0.08 333 | platform_anchor: 334 | x: # line 103 (convert mm in m) 335 | -0.036919143 336 | y: # line 104 (convert mm in m) 337 | -0.012581213 338 | z: # line 105 (convert mm in m) 339 | 0.0 340 | visual: 341 | mesh: 342 | "package://pi_hexapod_description/meshes/H-811/H-811.I2_UpperStrut.stl" 343 | scale: # to convert from mm to m 344 | 0.001 345 | x: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_X (conver mm in m) 346 | 0.0 347 | y: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_Y (conver mm in m) 348 | 0.0 349 | z: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_Z (conver mm in m) + 0.01 350 | 0.039 351 | roll: 1.57079632679 352 | pitch: 0.0 353 | yaw: # pi/2 + cos^-1(...) 354 | 2.82743343443 355 | collision: 356 | x: 0.0 357 | y: 0.0 358 | z: 0.039 359 | roll: 0.0 360 | pitch: 0.0 361 | yaw: 0.0 362 | radius: 0.014 363 | length: 0.08 364 | 365 | 366 | axis_4: 367 | base_anchor: 368 | x: # line 87 (convert mm in m) 369 | -0.016800381 370 | y: # line 88 (convert mm in m) 371 | -0.054702383 372 | z: # line 89 (convert mm in m) 373 | -0.088326097 374 | visual: 375 | mesh: 376 | "package://pi_hexapod_description/meshes/H-811/H-811.I2_LowerStrut.stl" 377 | scale: # to convert from mm to m 378 | 0.001 379 | x: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_X (conver mm in m) + 0.0038 380 | 0.0 381 | y: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_Y (conver mm in m) 382 | 0.0 383 | z: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_Z (conver mm in m) + 0.01 384 | 0.037 385 | roll: 1.57079632679 386 | pitch: 0.0 387 | yaw: # 3*pi/2 - cos^-1(...) 388 | 4.50294948057 389 | collision: 390 | x: 0.0 391 | y: 0.0 392 | z: 0.037 393 | roll: 0.0 394 | pitch: 0.0 395 | yaw: 0.0 396 | radius: 0.014 397 | length: 0.08 398 | platform_anchor: 399 | x: # line 106 (convert mm in m) 400 | 0.007563922 401 | y: # line 107 (convert mm in m) 402 | -0.038263522 403 | z: # line 108 (convert mm in m) 404 | 0.0 405 | visual: 406 | mesh: 407 | "package://pi_hexapod_description/meshes/H-811/H-811.I2_UpperStrut.stl" 408 | scale: # to convert from mm to m 409 | 0.001 410 | x: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_X (conver mm in m) 411 | 0.0 412 | y: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_Y (conver mm in m) 413 | 0.0 414 | z: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_Z (conver mm in m) + 0.01 415 | 0.039 416 | roll: 1.57079632679 417 | pitch: 0.0 418 | yaw: # pi/2 - cos^-1(...) 419 | 1.36135684362 420 | collision: 421 | x: 0.0 422 | y: 0.0 423 | z: 0.039 424 | roll: 0.0 425 | pitch: 0.0 426 | yaw: 0.0 427 | radius: 0.014 428 | length: 0.08 429 | 430 | 431 | axis_5: 432 | base_anchor: 433 | x: # line 90 (convert mm in m) 434 | 0.055773843 435 | y: # line 91 (convert mm in m) 436 | -0.012801635 437 | z: # line 92 (convert mm in m) 438 | -0.088326097 439 | visual: 440 | mesh: 441 | "package://pi_hexapod_description/meshes/H-811/H-811.I2_LowerStrut.stl" 442 | scale: # to convert from mm to m 443 | 0.001 444 | x: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_X (conver mm in m) + 0.0038 445 | 0.0 446 | y: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_Y (conver mm in m) 447 | 0.0 448 | z: # M811_CAD.ini -> [H-811.I2] -> LowerStrutOffset_Z (conver mm in m) + 0.01 449 | 0.037 450 | roll: 1.57079632679 451 | pitch: 0.0 452 | yaw: # -pi/2 + cos^-1(...) 453 | -0.314159243882 454 | collision: 455 | x: 0.0 456 | y: 0.0 457 | z: 0.037 458 | roll: 0.0 459 | pitch: 0.0 460 | yaw: 0.0 461 | radius: 0.014 462 | length: 0.08 463 | platform_anchor: 464 | x: # line 109 (convert mm in m) 465 | 0.029355222 466 | y: # line 110 (convert mm in m) 467 | -0.025682309 468 | z: # line 111 (convert mm in m) 469 | 0.0 470 | visual: 471 | mesh: 472 | "package://pi_hexapod_description/meshes/H-811/H-811.I2_UpperStrut.stl" 473 | scale: # to convert from mm to m 474 | 0.001 475 | x: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_X (conver mm in m) 476 | 0.0 477 | y: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_Y (conver mm in m) 478 | 0.0 479 | z: # M811_CAD.ini -> [H-811.I2] -> UpperStrutOffset_Z (conver mm in m) + 0.01 480 | 0.039 481 | roll: 1.57079632679 482 | pitch: 0.0 483 | yaw: # cos^-1(...) 484 | 0.837757998909 485 | collision: 486 | x: 0.0 487 | y: 0.0 488 | z: 0.039 489 | roll: 0.0 490 | pitch: 0.0 491 | yaw: 0.0 492 | radius: 0.014 493 | length: 0.08 494 | 495 | interactive_marker: 496 | base_frame: "zero_link" 497 | control_scale: 0.5 498 | scale: 499 | x: 0.1 500 | y: 0.1 501 | z: 0.0095 502 | position: 503 | x: 0.0 504 | y: 0.0 505 | z: 0.0048 506 | orientation: 507 | w: 1.0 508 | x: 0.0 509 | y: 0.0 510 | z: 0.0 511 | color: 512 | r: 0.0 513 | g: 0.0 514 | b: 1.0 515 | a: 0.8 516 | -------------------------------------------------------------------------------- /pi_hexapod_driver/pi_hexapod_control/src/interactive_marker_node.cpp: -------------------------------------------------------------------------------- 1 | // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- 2 | 3 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 4 | // This file is part of the PI-Hexapod Driver. 5 | // 6 | // 7 | // © Copyright 2020 Physik Instrumente (PI) GmbH & Co. KG, Karlsruhe, Germany 8 | // © Copyright 2020 FZI Forschungszentrum Informatik, Karlsruhe, Germany 9 | // -- END LICENSE BLOCK ------------------------------------------------ 10 | 11 | //---------------------------------------------------------------------- 12 | /*!\file 13 | * 14 | * This file contains the implementation of InteractiveMarkerNode and a main() to be run as Node. 15 | * 16 | * \author Christian Eichmann 17 | * \author Philip Keller 18 | * \date 2020-05-06 19 | * 20 | */ 21 | //---------------------------------------------------------------------- 22 | 23 | #include "Eigen/Geometry" 24 | #include 25 | 26 | namespace pi_hexapod_control { 27 | 28 | InteractiveMarkerNode::InteractiveMarkerNode() 29 | : joint_state_received_(false){}; 30 | 31 | void InteractiveMarkerNode::jointStateCallback(const sensor_msgs::JointState::ConstPtr& msg) 32 | { 33 | joint_state_received_ = true; 34 | for (size_t i = 0; i < msg->name.size(); i++) 35 | { 36 | if (msg->name[i] == "ang_u") 37 | { 38 | received_joint_state_angles_[0] = msg->position[i]; 39 | } 40 | else if (msg->name[i] == "ang_v") 41 | { 42 | received_joint_state_angles_[1] = msg->position[i]; 43 | } 44 | else if (msg->name[i] == "ang_w") 45 | { 46 | received_joint_state_angles_[2] = msg->position[i]; 47 | } 48 | else if (msg->name[i] == "cart_x") 49 | { 50 | received_joint_state_translation_[0] = msg->position[i]; 51 | } 52 | else if (msg->name[i] == "cart_y") 53 | { 54 | received_joint_state_translation_[1] = msg->position[i]; 55 | } 56 | else if (msg->name[i] == "cart_z") 57 | { 58 | received_joint_state_translation_[2] = msg->position[i]; 59 | } 60 | } 61 | } 62 | 63 | bool InteractiveMarkerNode::activateCommandPublishingCallback(std_srvs::SetBool::Request& req, 64 | std_srvs::SetBool::Response& res) 65 | { 66 | setPublishingCommands(req.data); 67 | res.success = true; 68 | return true; 69 | } 70 | 71 | bool InteractiveMarkerNode::setToZeroCallback(std_srvs::Empty::Request& req, 72 | std_srvs::Empty::Response& res) 73 | { 74 | Eigen::Array3d zeros = Eigen::Array3d::Zero(); 75 | setToPosition(zeros, zeros); 76 | return true; 77 | } 78 | 79 | bool InteractiveMarkerNode::setToCurrentCallback(std_srvs::Empty::Request& req, 80 | std_srvs::Empty::Response& res) 81 | { 82 | if (joint_state_received_) 83 | { 84 | setToPosition(received_joint_state_translation_, received_joint_state_angles_); 85 | } 86 | else 87 | { 88 | ROS_ERROR("Can't set marker to current position. Interactive Marker Server did not receive " 89 | "joint_states."); 90 | } 91 | return true; 92 | } 93 | 94 | bool InteractiveMarkerNode::sendSingleCommandCallback(std_srvs::Empty::Request& req, 95 | std_srvs::Empty::Response& res) 96 | { 97 | publishCommand(); 98 | return true; 99 | } 100 | 101 | void InteractiveMarkerNode::setToPosition(const Eigen::Array3d& translation, 102 | const Eigen::Array3d& angles) 103 | { 104 | ROS_INFO("Setting marker to position."); 105 | 106 | // set rotation axis controller 107 | for (size_t i = 0; i < 3; i++) 108 | { 109 | Eigen::Vector3d axis; 110 | axis[i] = 1; 111 | Eigen::AngleAxis aa(angles[i], axis); 112 | Eigen::Quaternion quaternion(aa); 113 | geometry_msgs::Pose axis_pose; 114 | axis_pose.orientation.w = quaternion.w(); 115 | axis_pose.orientation.x = quaternion.x(); 116 | axis_pose.orientation.y = quaternion.y(); 117 | axis_pose.orientation.z = quaternion.z(); 118 | server_->setPose(MAKER_NAME_FROM_AXIS[i], axis_pose); 119 | // server_->applyChanges() is called from updatePoseMsg() 120 | } 121 | 122 | // set marker position 123 | translation_ = translation; 124 | angles_ = angles; 125 | updatePoseMsg(); 126 | } 127 | 128 | void InteractiveMarkerNode::menuPublishCommandsHandleCallback( 129 | const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) 130 | { 131 | interactive_markers::MenuHandler::CheckState state; 132 | menu_handler_.getCheckState(menu_publish_commands_handle_, state); 133 | 134 | // if previously unchecked, activate now and vice versa 135 | setPublishingCommands(state == interactive_markers::MenuHandler::UNCHECKED); 136 | } 137 | 138 | void InteractiveMarkerNode::setPublishingCommands(const bool new_state) 139 | { 140 | is_publishing_commands_ = new_state; 141 | ROS_INFO("Command publication %s.", (is_publishing_commands_ ? "activated" : "deactivated")); 142 | 143 | if (is_publishing_commands_) 144 | { 145 | menu_handler_.setCheckState(menu_publish_commands_handle_, 146 | interactive_markers::MenuHandler::CHECKED); 147 | } 148 | else 149 | { 150 | menu_handler_.setCheckState(menu_publish_commands_handle_, 151 | interactive_markers::MenuHandler::UNCHECKED); 152 | } 153 | menu_handler_.reApply(*server_.get()); 154 | server_->applyChanges(); 155 | } 156 | 157 | 158 | void InteractiveMarkerNode::translationMarkerCallback( 159 | const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) 160 | { 161 | Eigen::Array3d vector( 162 | feedback->pose.position.x, feedback->pose.position.y, feedback->pose.position.z); 163 | 164 | translation_ = lower_translation_limits_.max(upper_translation_limits_.min(vector)); 165 | 166 | updatePoseMsg(); 167 | } 168 | 169 | void InteractiveMarkerNode::rotationMarkerCallback( 170 | const int32_t axis_id, const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) 171 | { 172 | geometry_msgs::Pose pose = feedback->pose; 173 | 174 | const Eigen::Quaternion feedback_quaternion( 175 | pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); 176 | const Eigen::AngleAxis feedback_angle_axis(feedback_quaternion); 177 | 178 | // determine algebraic sign of angle from rotation axis 179 | const double angle = feedback_angle_axis.angle() * feedback_angle_axis.axis()[axis_id]; 180 | angles_[axis_id] = 181 | std::max(lower_angle_limits_[axis_id], std::min(upper_angle_limits_[axis_id], angle)); 182 | 183 | const Eigen::AngleAxis limited_angle_axis( 184 | angles_[axis_id] * feedback_angle_axis.axis()[axis_id], feedback_angle_axis.axis()); 185 | const Eigen::Quaternion limited_quaternion(limited_angle_axis); 186 | pose.orientation.w = limited_quaternion.w(); 187 | pose.orientation.x = limited_quaternion.x(); 188 | pose.orientation.y = limited_quaternion.y(); 189 | pose.orientation.z = limited_quaternion.z(); 190 | 191 | server_->setPose(MAKER_NAME_FROM_AXIS[axis_id], pose); 192 | updatePoseMsg(); 193 | } 194 | 195 | void InteractiveMarkerNode::updatePoseMsg() 196 | { 197 | const Eigen::Quaternion q = 198 | Eigen::AngleAxisd(angles_[ROLL_AXIS_ID], Eigen::Vector3d::UnitX()) * 199 | Eigen::AngleAxisd(angles_[PITCH_AXIS_ID], Eigen::Vector3d::UnitY()) * 200 | Eigen::AngleAxisd(angles_[YAW_AXIS_ID], Eigen::Vector3d::UnitZ()); 201 | 202 | pose_msg_.pose.position.x = translation_[0]; 203 | pose_msg_.pose.position.y = translation_[1]; 204 | pose_msg_.pose.position.z = translation_[2]; 205 | pose_msg_.pose.orientation.w = q.w(); 206 | pose_msg_.pose.orientation.x = q.x(); 207 | pose_msg_.pose.orientation.y = q.y(); 208 | pose_msg_.pose.orientation.z = q.z(); 209 | 210 | server_->setPose(TRANSLATION_MARKER, pose_msg_.pose); 211 | server_->setPose(VISUALIZATION_MARKER, pose_msg_.pose); 212 | server_->applyChanges(); 213 | } 214 | 215 | void InteractiveMarkerNode::initMenu() 216 | { 217 | menu_handler_.insert( 218 | "Set to zero", 219 | std::bind(&InteractiveMarkerNode::setToZeroMenuCallback, this, std::placeholders::_1)); 220 | 221 | menu_handler_.insert( 222 | "Set to current", 223 | std::bind(&InteractiveMarkerNode::setToCurrentMenuCallback, this, std::placeholders::_1)); 224 | 225 | menu_handler_.insert("Send marker as command", 226 | std::bind(&InteractiveMarkerNode::publishCommand, this)); 227 | 228 | 229 | menu_publish_commands_handle_ = menu_handler_.insert( 230 | "Publish commands", 231 | std::bind( 232 | &InteractiveMarkerNode::menuPublishCommandsHandleCallback, this, std::placeholders::_1)); 233 | 234 | if (is_publishing_commands_) 235 | { 236 | menu_handler_.setCheckState(menu_publish_commands_handle_, 237 | interactive_markers::MenuHandler::CHECKED); 238 | } 239 | else 240 | { 241 | menu_handler_.setCheckState(menu_publish_commands_handle_, 242 | interactive_markers::MenuHandler::UNCHECKED); 243 | } 244 | } 245 | 246 | void InteractiveMarkerNode::setToZeroMenuCallback( 247 | const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) 248 | { 249 | Eigen::Array3d zeros = Eigen::Array3d::Zero(); 250 | setToPosition(zeros, zeros); 251 | } 252 | 253 | void InteractiveMarkerNode::setToCurrentMenuCallback( 254 | const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) 255 | { 256 | if (joint_state_received_) 257 | { 258 | setToPosition(received_joint_state_translation_, received_joint_state_angles_); 259 | } 260 | else 261 | { 262 | ROS_ERROR("Can't set marker to current position. Interactive Marker Server did not receive " 263 | "joint_states."); 264 | } 265 | } 266 | 267 | 268 | bool InteractiveMarkerNode::init(ros::NodeHandle& nh, ros::NodeHandle& private_nh) 269 | { 270 | private_nh.param("rate", rate_, 10.0); 271 | private_nh.param("interactive_marker/control_scale", control_scale_, 1.0); 272 | private_nh.param("autostart_publishing_commands", is_publishing_commands_, false); 273 | private_nh.param("topic_ns", topic_ns_, "pi_interactive_marker"); 274 | private_nh.param("interactive_marker/base_frame", base_frame_, "zero_link"); 275 | 276 | const double neg_max = -1 * std::numeric_limits::max(); 277 | const double pos_max = std::numeric_limits::max(); 278 | private_nh.param("x/lower_limit", lower_translation_limits_[0], neg_max); 279 | private_nh.param("x/upper_limit", upper_translation_limits_[0], pos_max); 280 | private_nh.param("y/lower_limit", lower_translation_limits_[1], neg_max); 281 | private_nh.param("y/upper_limit", upper_translation_limits_[1], pos_max); 282 | private_nh.param("z/lower_limit", lower_translation_limits_[2], neg_max); 283 | private_nh.param("z/upper_limit", upper_translation_limits_[2], pos_max); 284 | private_nh.param("u/lower_limit", lower_angle_limits_[ROLL_AXIS_ID], neg_max); 285 | private_nh.param("u/upper_limit", upper_angle_limits_[ROLL_AXIS_ID], pos_max); 286 | private_nh.param("v/lower_limit", lower_angle_limits_[PITCH_AXIS_ID], neg_max); 287 | private_nh.param("v/upper_limit", upper_angle_limits_[PITCH_AXIS_ID], pos_max); 288 | private_nh.param("w/lower_limit", lower_angle_limits_[YAW_AXIS_ID], neg_max); 289 | private_nh.param("w/upper_limit", upper_angle_limits_[YAW_AXIS_ID], pos_max); 290 | 291 | // Initialize the pose to 0, so it is a valid pose even before the first callback 292 | pose_msg_.pose.position.x = 0; 293 | pose_msg_.pose.position.y = 0; 294 | pose_msg_.pose.position.z = 0; 295 | pose_msg_.pose.orientation.x = 0; 296 | pose_msg_.pose.orientation.y = 0; 297 | pose_msg_.pose.orientation.z = 0; 298 | pose_msg_.pose.orientation.w = 1; 299 | 300 | // create Marker Server but do not spin up a thread to handle callbacks 301 | server_.reset(new interactive_markers::InteractiveMarkerServer(topic_ns_, "", false)); 302 | 303 | // Initialize right click menu and set its callback handlers 304 | initMenu(); 305 | 306 | /* ******************* * 307 | * VISUALIZATION MAKER * 308 | * ******************* */ 309 | visualization_msgs::InteractiveMarker visualization_marker; 310 | visualization_marker.header.frame_id = base_frame_; 311 | visualization_marker.name = VISUALIZATION_MARKER; 312 | visualization_marker.scale = control_scale_; 313 | 314 | // Create visualized cylinder 315 | visualization_msgs::InteractiveMarkerControl cylinder_control; 316 | cylinder_control.always_visible = true; 317 | cylinder_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MENU; 318 | visualization_msgs::Marker platform_marker; 319 | platform_marker.type = visualization_msgs::Marker::CYLINDER; 320 | private_nh.param("interactive_marker/scale/x", platform_marker.scale.x, 0.2); 321 | private_nh.param("interactive_marker/scale/y", platform_marker.scale.y, 0.2); 322 | private_nh.param("interactive_marker/scale/z", platform_marker.scale.z, 0.1); 323 | private_nh.param("interactive_marker/position/x", platform_marker.pose.position.x, 0.0); 324 | private_nh.param("interactive_marker/position/y", platform_marker.pose.position.y, 0.0); 325 | private_nh.param("interactive_marker/position/z", platform_marker.pose.position.z, 0.0); 326 | private_nh.param( 327 | "interactive_marker/orientation/w", platform_marker.pose.orientation.w, 1.0); 328 | private_nh.param( 329 | "interactive_marker/orientation/x", platform_marker.pose.orientation.x, 0.0); 330 | private_nh.param( 331 | "interactive_marker/orientation/y", platform_marker.pose.orientation.y, 0.0); 332 | private_nh.param( 333 | "interactive_marker/orientation/z", platform_marker.pose.orientation.z, 0.0); 334 | private_nh.param("interactive_marker/color/r", platform_marker.color.r, 1.0); 335 | private_nh.param("interactive_marker/color/g", platform_marker.color.g, 0.0); 336 | private_nh.param("interactive_marker/color/b", platform_marker.color.b, 0.0); 337 | private_nh.param("interactive_marker/color/a", platform_marker.color.a, 0.5); 338 | cylinder_control.markers.push_back(platform_marker); 339 | 340 | // Add cube to form X-Axis Arrow 341 | visualization_msgs::Marker x_axis_marker(platform_marker); 342 | x_axis_marker.color.r = 1.0; 343 | x_axis_marker.color.b = 0.0; 344 | x_axis_marker.pose.position.x = platform_marker.scale.x * 0.25 + 0.005; 345 | x_axis_marker.scale.x = 0.005; 346 | x_axis_marker.scale.y = 0.005; 347 | x_axis_marker.scale.z = platform_marker.scale.x * 0.5 + 0.01; 348 | x_axis_marker.pose.orientation.w = 0.7071068; 349 | x_axis_marker.pose.orientation.y = 0.7071068; 350 | cylinder_control.markers.push_back(x_axis_marker); 351 | 352 | // Add cube to form Y-Axis Arrow 353 | visualization_msgs::Marker y_axis_marker(platform_marker); 354 | y_axis_marker.color.g = 1.0; 355 | y_axis_marker.color.b = 0.0; 356 | y_axis_marker.pose.position.x = 0; 357 | y_axis_marker.pose.position.y = platform_marker.scale.y * 0.25 + 0.005; 358 | y_axis_marker.scale.x = 0.005; 359 | y_axis_marker.scale.y = 0.005; 360 | y_axis_marker.scale.z = platform_marker.scale.y * 0.5 + 0.01; 361 | y_axis_marker.pose.orientation.w = 0.7071068; 362 | y_axis_marker.pose.orientation.x = 0.7071068; 363 | cylinder_control.markers.push_back(y_axis_marker); 364 | 365 | visualization_marker.controls.push_back(cylinder_control); 366 | server_->insert(visualization_marker); 367 | menu_handler_.apply(*server_.get(), visualization_marker.name); 368 | 369 | /* ***************** * 370 | * TRANSLATION MAKER * 371 | * ***************** */ 372 | visualization_msgs::InteractiveMarker translation_marker; 373 | translation_marker.header.frame_id = base_frame_; 374 | translation_marker.name = TRANSLATION_MARKER; 375 | translation_marker.scale = control_scale_; 376 | 377 | // Add three different interaction controls to Marker 378 | visualization_msgs::InteractiveMarkerControl control; 379 | 380 | control.orientation.w = 0.7071068; 381 | control.orientation.x = 0.7071068; 382 | control.orientation.y = 0; 383 | control.orientation.z = 0; 384 | control.name = "move_x"; 385 | control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED; 386 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 387 | translation_marker.controls.push_back(control); 388 | 389 | control.orientation.w = 0.7071068; 390 | control.orientation.x = 0; 391 | control.orientation.y = 0; 392 | control.orientation.z = 0.7071068; 393 | control.name = "move_y"; 394 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 395 | translation_marker.controls.push_back(control); 396 | 397 | control.orientation.w = 0.7071068; 398 | control.orientation.x = 0; 399 | control.orientation.y = 0.7071068; 400 | control.orientation.z = 0; 401 | control.name = "move_z"; 402 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 403 | translation_marker.controls.push_back(control); 404 | 405 | server_->insert(translation_marker); 406 | menu_handler_.apply(*server_.get(), translation_marker.name); 407 | server_->setCallback( 408 | translation_marker.name, 409 | std::bind(&InteractiveMarkerNode::translationMarkerCallback, this, std::placeholders::_1)); 410 | 411 | /* *********** * 412 | * ROLL MARKER * 413 | * *********** */ 414 | visualization_msgs::InteractiveMarker roll_marker; 415 | roll_marker.header.frame_id = base_frame_; 416 | roll_marker.name = MAKER_NAME_FROM_AXIS[ROLL_AXIS_ID]; 417 | roll_marker.scale = control_scale_; 418 | control.orientation.w = 0.7071068; 419 | control.orientation.x = 0.7071068; 420 | control.orientation.y = 0; 421 | control.orientation.z = 0; 422 | control.name = "rotate_u"; 423 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 424 | roll_marker.controls.push_back(control); 425 | server_->insert(roll_marker); 426 | menu_handler_.apply(*server_.get(), roll_marker.name); 427 | server_->setCallback( 428 | roll_marker.name, 429 | std::bind( 430 | &InteractiveMarkerNode::rotationMarkerCallback, this, ROLL_AXIS_ID, std::placeholders::_1)); 431 | 432 | /* ************ * 433 | * PITCH MARKER * 434 | * ************ */ 435 | visualization_msgs::InteractiveMarker pitch_marker; 436 | pitch_marker.header.frame_id = base_frame_; 437 | pitch_marker.name = MAKER_NAME_FROM_AXIS[PITCH_AXIS_ID]; 438 | pitch_marker.scale = control_scale_; 439 | control.orientation.w = 0.7071068; 440 | control.orientation.x = 0; 441 | control.orientation.y = 0; 442 | control.orientation.z = 0.7071068; 443 | control.name = "rotate_v"; 444 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 445 | pitch_marker.controls.push_back(control); 446 | server_->insert(pitch_marker); 447 | menu_handler_.apply(*server_.get(), pitch_marker.name); 448 | server_->setCallback( 449 | pitch_marker.name, 450 | std::bind( 451 | &InteractiveMarkerNode::rotationMarkerCallback, this, PITCH_AXIS_ID, std::placeholders::_1)); 452 | 453 | /* ********** * 454 | * YAW MARKER * 455 | * ********** */ 456 | visualization_msgs::InteractiveMarker yaw_marker; 457 | yaw_marker.header.frame_id = base_frame_; 458 | yaw_marker.name = MAKER_NAME_FROM_AXIS[YAW_AXIS_ID]; 459 | yaw_marker.scale = control_scale_; 460 | control.orientation.w = 0.7071068; 461 | control.orientation.x = 0; 462 | control.orientation.y = 0.7071068; 463 | control.orientation.z = 0; 464 | control.name = "rotate_w"; 465 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 466 | yaw_marker.controls.push_back(control); 467 | server_->insert(yaw_marker); 468 | menu_handler_.apply(*server_.get(), yaw_marker.name); 469 | server_->setCallback( 470 | yaw_marker.name, 471 | std::bind( 472 | &InteractiveMarkerNode::rotationMarkerCallback, this, YAW_AXIS_ID, std::placeholders::_1)); 473 | 474 | // Apply changes to server 475 | server_->applyChanges(); 476 | 477 | // Setup publisher to publish pose in relation to the base frame 478 | pose_publisher_ = private_nh.advertise("pose", 10); 479 | // Setup publisher to publish command to joint_group_position_controller 480 | command_publisher_ = private_nh.advertise("command", 10); 481 | // Setup joint state subscriber 482 | joint_state_subscriber_ = private_nh.subscribe( 483 | "joint_states", 100, &InteractiveMarkerNode::jointStateCallback, this); 484 | 485 | activate_command_publishing_server_ = private_nh.advertiseService( 486 | "activate_command_publishing", &InteractiveMarkerNode::activateCommandPublishingCallback, this); 487 | set_to_zero_server_ = 488 | private_nh.advertiseService("set_to_zero", &InteractiveMarkerNode::setToZeroCallback, this); 489 | set_to_current_server_ = private_nh.advertiseService( 490 | "set_to_current", &InteractiveMarkerNode::setToCurrentCallback, this); 491 | send_single_command_server_ = private_nh.advertiseService( 492 | "send_single_command", &InteractiveMarkerNode::sendSingleCommandCallback, this); 493 | 494 | // Setup the timer which is used to send out messages with a given rate 495 | ros::Duration period(1.0 / rate_); 496 | timer_ = nh.createTimer( 497 | period, std::bind(&InteractiveMarkerNode::timerCallback, this, std::placeholders::_1)); 498 | 499 | return true; 500 | } 501 | 502 | void InteractiveMarkerNode::timerCallback(const ros::TimerEvent& timer_event) 503 | { 504 | pose_msg_.header.frame_id = base_frame_; 505 | pose_msg_.header.stamp = ros::Time::now(); 506 | pose_publisher_.publish(pose_msg_); 507 | 508 | if (is_publishing_commands_) 509 | { 510 | publishCommand(); 511 | } 512 | } 513 | 514 | void InteractiveMarkerNode::publishCommand() 515 | { 516 | std_msgs::Float64MultiArray command; 517 | command.data.push_back(translation_[0]); 518 | command.data.push_back(translation_[1]); 519 | command.data.push_back(translation_[2]); 520 | command.data.push_back(angles_[0]); 521 | command.data.push_back(angles_[1]); 522 | command.data.push_back(angles_[2]); 523 | command_publisher_.publish(command); 524 | } 525 | 526 | } // namespace pi_hexapod_control 527 | 528 | /*! 529 | * \brief Launches a InteractiveMarkerNode as ROS Node. 530 | * 531 | * \param argc 532 | * \param argv 533 | * \return int 534 | */ 535 | int main(int argc, char** argv) 536 | { 537 | ros::init(argc, argv, "interactive_marker_tf"); 538 | ros::NodeHandle nh; 539 | ros::NodeHandle pnh("~"); 540 | 541 | pi_hexapod_control::InteractiveMarkerNode node = pi_hexapod_control::InteractiveMarkerNode(); 542 | 543 | if (!node.init(nh, pnh)) 544 | { 545 | ROS_ERROR("Setup failed, shutting down"); 546 | return -1; 547 | } 548 | 549 | ROS_INFO("Start spinning."); 550 | ros::spin(); 551 | ROS_INFO("Stopped spinning, shutting down."); 552 | 553 | return 0; 554 | } 555 | --------------------------------------------------------------------------------