diff --git a/CMakeLists.txt b/CMakeLists.txt index 5d7f605..2594cdd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,9 +5,9 @@ project(tf_keyboard_cal) set(CMAKE_CXX_FLAGS "-std=c++11 -Wall ${CMAKE_CXX_FLAGS}") find_package(catkin REQUIRED COMPONENTS - keyboard roscpp tf2 + tf2_msgs tf roslib roslint @@ -16,76 +16,106 @@ find_package(catkin REQUIRED COMPONENTS rviz_visual_tools tf_conversions interactive_markers + rviz + std_msgs + geometry_msgs ) -find_package(Eigen REQUIRED) +find_package(Eigen3 REQUIRED) + +# Eigen 3.2 (Wily) only provides EIGEN3_INCLUDE_DIR, not EIGEN3_INCLUDE_DIRS +if(NOT EIGEN3_INCLUDE_DIRS) + set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) +endif() + find_package(Boost REQUIRED) catkin_package( INCLUDE_DIRS - include + include LIBRARIES - ${PROJECT_NAME}_manual_tf_alignment - ${PROJECT_NAME}_imarker_simple + ${PROJECT_NAME}_gui +# ${PROJECT_NAME}_manual_tf_alignment +# ${PROJECT_NAME}_imarker_simple CATKIN_DEPENDS - keyboard roscpp + # keyboard + message_runtime + roscpp tf2 tf rosparam_shortcuts interactive_markers DEPENDS - Eigen + EIGEN3 ) -########### -## Build ## -########### +# Qt 4 or 5 +if(rviz_QT_VERSION VERSION_LESS "5") + message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui) + include(${QT_USE_FILE}) + macro(qt_wrap_ui) + qt4_wrap_ui(${ARGN}) + endmacro() + macro(qt_wrap_cpp) + qt4_wrap_cpp(${ARGN}) + endmacro() +else() + message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) + set(QT_LIBRARIES Qt5::Widgets) + macro(qt_wrap_ui) + qt5_wrap_ui(${ARGN}) + endmacro() + macro(qt_wrap_cpp) + qt5_wrap_cpp(${ARGN}) + endmacro() +endif() +## Prefer the Qt signals and slots to avoid defining "emit", "slots", +## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. +set(CMAKE_INCLUDE_CURRENT_DIR ON) +set(CMAKE_AUTOMOC ON) +add_definitions(-DQT_NO_KEYWORDS) include_directories( include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIR} - ${EIGEN_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} ) -# Library -add_library(${PROJECT_NAME}_manual_tf_alignment - src/manual_tf_alignment.cpp -) -target_link_libraries(${PROJECT_NAME}_manual_tf_alignment - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} -) +########### +## Build ## +########### -# Library -add_library(${PROJECT_NAME}_imarker_simple - src/imarker_simple.cpp +## specify which header files need to be run through "moc", +## Qt's meta-object compiler. +qt_wrap_cpp(MOC_FILES + src/tf_keyboard_cal_gui.h ) -target_link_libraries(${PROJECT_NAME}_imarker_simple - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + +## specify the list of source files, including the output of +## the previous command which is stored in ``${MOC_FILES}``. +set(SOURCE_FILES + src/tf_keyboard_cal_gui.cpp ) -# Executable +add_library(${PROJECT_NAME}_receiver src/tf_remote_receiver.cpp) +target_link_libraries(${PROJECT_NAME}_receiver ${catkin_LIBRARIES}) + +add_library(${PROJECT_NAME}_gui ${SOURCE_FILES}) +target_link_libraries(${PROJECT_NAME}_gui ${PROJECT_NAME}_receiver ${rviz_DEFAULT_PLUGIN_LIBRARIES} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + +add_library(${PROJECT_NAME}_rviz_publisher + src/rviz_tf_publisher.cpp) +target_link_libraries(${PROJECT_NAME}_rviz_publisher + ${catkin_LIBRARIES}) + add_executable(${PROJECT_NAME} - src/tf_keyboard.cpp -) + src/tf_keyboard.cpp) target_link_libraries(${PROJECT_NAME} - ${PROJECT_NAME}_manual_tf_alignment - ${catkin_LIBRARIES} -) - -# Executable -add_executable(${PROJECT_NAME}_demo_tf_listener - src/demo_tf_listener.cpp -) -# Rename C++ executable without namespace -set_target_properties(${PROJECT_NAME}_demo_tf_listener - PROPERTIES OUTPUT_NAME demo_tf_listener PREFIX "") -# Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME}_demo_tf_listener - ${catkin_LIBRARIES} -) + ${PROJECT_NAME}_rviz_publisher + ${catkin_LIBRARIES}) ############# ## Testing ## @@ -99,26 +129,28 @@ roslint_cpp() ############# # Install libraries -install(TARGETS ${PROJECT_NAME}_manual_tf_alignment ${PROJECT_NAME}_imarker_simple - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install( + TARGETS + ${PROJECT_NAME}_gui + LIBRARY DESTINATION + ${CATKIN_PACKAGE_LIB_DESTINATION} + ) # Install header files -install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +# Install shared resources +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY resources DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +# Install xml plugin config +install(FILES + plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) # Install executables -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_demo_tf_listener +install(TARGETS LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# Install scripts -install(PROGRAMS - scripts/tf_interactive_marker.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# Install launch files -install(DIRECTORY - launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) + ) diff --git a/README.md b/README.old similarity index 96% rename from README.md rename to README.old index fe56fd9..2120bff 100644 --- a/README.md +++ b/README.old @@ -1,3 +1,13 @@ +# NEW BRANCH + +``` +roslaunch tf_keyboard_cal rviz_demo.launch +``` + +In `rviz` add the tf panel by: `Panel -> Add New Panel ->TFKeyboardCalGUI` + + + # Manual TF Calibration Tools Move /tf frames around using your keyboard or interactive markers - a simple calibration-by-eye tool! diff --git a/bug_notes.md b/bug_notes.md new file mode 100644 index 0000000..90c58f6 --- /dev/null +++ b/bug_notes.md @@ -0,0 +1,27 @@ +# Bugs and Issues + +Notes on bugs and issues seen during development. + +## 1. Interactive Marker Reset + +If you select `Enable Transparancy` under the Interactive Marker topic in Rviz BEFORE using the GUI buttons to adjust the marker's position, the marker will reset back to its origin. Once you adjust the marker through the GUI this problem seems to disappear. + +## 2. Interactive Markers Branching from non Interactive Marker + +If a TF is defined without an interactive marker and then a TF with is interactive marker is defined as a child of that, the interactive marker does not update properly when using the keyboard commands. + +There seems to be a disconnect between the values of the imarker and the active tfs list. + +### Fix + +Seems that `active_tf_list_.push_back(new_tf)` was being called before `.imarker_ = true` was called. This appears to have solved the errors. + +## 3. Large Decimals + +On the `manipulate` tab, dragging the interactive marker causes large decimals. Need to display these only to 2 or 3 significant digits. + +## 4. When `has menu` is ticked, tick `imarker` checkbox. + +## 5. Multiple TFs with same name + +Need to not allow user to create multiple TFs with same `to` and `from`, or create loops in TF tree. diff --git a/config/example_imarker_menu.tf b/config/example_imarker_menu.tf new file mode 100644 index 0000000..f621495 --- /dev/null +++ b/config/example_imarker_menu.tf @@ -0,0 +1,15 @@ +# example menu file +# (shebang anywhere in line comments line) +# FORMAT: (supports only 1 sub level, sub menus cannot have sub menus) +# (num sub menus), menu title +# submenu title +# very little error checking done... so check your input. +3, menu item A +sub entry 1 +sub entry 2 +sub entry 3 +0, menu item B +0, menu item C +2, menu item D +sub entry 1 +sub entry 2 \ No newline at end of file diff --git a/config/example_save_file.tf b/config/example_save_file.tf new file mode 100644 index 0000000..2d09f75 --- /dev/null +++ b/config/example_save_file.tf @@ -0,0 +1,11 @@ +#ID FRAME_ID CHILD_FRAME_ID X Y Z ROLL PITCH YAW +# (comment line) +# space delimeter +0 a b 0.50 1.50 0.50 0.00 0.00 0.00 +1 b c 1.00 0.00 1.00 0.00 0.00 0.00 +2 b d 0.50 1.50 1.50 0.00 0.00 0.00 +3 d e 1.00 -3.00 1.00 0.00 0.00 0.00 +4 e f 1.00 3.00 1.00 0.00 0.00 0.00 +5 f g 1.00 -3.00 1.00 0.00 0.00 0.00 +6 g h 1.00 3.00 1.00 0.00 0.00 0.00 +7 h i 1.00 -3.00 1.00 0.00 0.00 0.00 diff --git a/config/tf_keyboard_world_to_thing.yaml b/config/tf_keyboard_world_to_thing.yaml deleted file mode 100644 index 6842d09..0000000 --- a/config/tf_keyboard_world_to_thing.yaml +++ /dev/null @@ -1,11 +0,0 @@ -initial_x: 0.5 -initial_y: 0.5 -initial_z: 0.5 -initial_roll: 0.0 -initial_pitch: 0.0 -initial_yaw: 0.0 -from: world -to: thing -file_package: tf_keyboard_cal -file_name: /config/tf_keyboard_world_to_thing.yaml -topic_name: /keyboard_world_to_thing/keydown diff --git a/include/tf_keyboard_cal/imarker_simple.h b/include/tf_keyboard_cal/imarker_simple.h deleted file mode 100644 index f00c374..0000000 --- a/include/tf_keyboard_cal/imarker_simple.h +++ /dev/null @@ -1,102 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, PickNik LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik LLC nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman - Desc: Use interactive markers in a C++ class via the external python node -*/ - -#ifndef TF_KEYBOARD_CAL_IMARKER_SIMPLE_H -#define TF_KEYBOARD_CAL_IMARKER_SIMPLE_H - -// ROS -#include - -#include - -#include -#include -#include -#include -#include - -namespace tf_keyboard_cal -{ -using visualization_msgs::InteractiveMarkerFeedback; -using visualization_msgs::InteractiveMarkerControl; - -typedef std::function -IMarkerCallback; - -class IMarkerSimple -{ -public: - -/** \brief Constructor */ -IMarkerSimple(); - -geometry_msgs::Pose& getPose(); - -void iMarkerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); - -void sendUpdatedIMarkerPose(); - -void make6DofMarker(const geometry_msgs::Pose &pose); - -private: - -// -------------------------------------------------------- - -// The short name of this class -std::string name_ = "imarker_simple"; - -// A shared node handle -ros::NodeHandle nh_; - -geometry_msgs::Pose latest_pose_; - -// Interactive markers -std::shared_ptr imarker_server_; - -// Interactive markers -// interactive_markers::MenuHandler menu_handler_; -visualization_msgs::InteractiveMarker int_marker_; - -}; // end class - -// Create std pointers for this class -typedef std::shared_ptr IMarkerSimplePtr; -typedef std::shared_ptr IMarkerSimpleConstPtr; - -} // namespace tf_keyboard_cal -#endif // TF_KEYBOARD_CAL_IMARKER_SIMPLE_H diff --git a/include/tf_keyboard_cal/manual_tf_alignment.h b/include/tf_keyboard_cal/manual_tf_alignment.h deleted file mode 100644 index ce5bbe8..0000000 --- a/include/tf_keyboard_cal/manual_tf_alignment.h +++ /dev/null @@ -1,115 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, PickNik LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik LLC nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* - * Author : Andy McEvoy (mcevoy.andy@gmail.com), Dave Coleman - * Desc : Allows manual control of a TF through the keyboard - */ - -#ifndef TF_KEYBOARD_CAL__MANUAL_TF_ALIGNMENT_ -#define TF_KEYBOARD_CAL__MANUAL_TF_ALIGNMENT_ - -#include - -#include - -#include - -#include - -#include - -namespace tf_keyboard_cal -{ - -class ManualTFAlignment -{ -public: - - /* - * \brief - */ - ManualTFAlignment(); - - /* - * \brief - */ - void keyboardCallback(const keyboard::Key::ConstPtr& msg); - - /* - * \brief - */ - void printMenu(); - - /* - * \brief - */ - void publishTF(); - - /* - * \brief - */ - void setPose(Eigen::Vector3d translation, Eigen::Vector3d rotation); - - /* - * \brief - */ - void updateTF(int mode, double delta); - - /* - * \brief - */ - void writeTFToFile(); - - ros::NodeHandle nh_; - - // Name of class - std::string name_ = "manipulation_data"; - - Eigen::Vector3d translation_; - Eigen::Vector3d rotation_; - std::string save_path_; - int mode_; - double delta_; - std::string from_; - std::string to_; - std::string file_package_; - std::string file_name_; - std::string topic_name_; - ros::Subscriber keyboard_sub_; -}; - -} // end namespace - -#endif diff --git a/include/tf_keyboard_cal/rviz_tf_publisher.h b/include/tf_keyboard_cal/rviz_tf_publisher.h new file mode 100644 index 0000000..48b6fe1 --- /dev/null +++ b/include/tf_keyboard_cal/rviz_tf_publisher.h @@ -0,0 +1,74 @@ +/**************************************************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2017, Andy McEvoy + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, this list of + * conditions and the following disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER + * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ****************************************************************************************************/ + +/* + * Author(s) : Andy McEvoy ( mcevoy.andy@gmail.com ) + * Desc : tf publisher for rviz transforms + * Created : 2017 - May - 18 + */ + + +#ifndef RVIZ_TF_PUBLISHER_H_ +#define RVIZ_TF_PUBLISHER_H_ + +#include +#include +#include +#include + +#include + +namespace tf_keyboard_cal +{ + +class RvizTFPublisher +{ + +public: + void publishTFs(); + RvizTFPublisher(); + +private: + + ros::NodeHandle nh_; + + void removeTF(geometry_msgs::TransformStamped remove_tf_msg); + void createTF(geometry_msgs::TransformStamped create_tf_msg); + void updateTF(geometry_msgs::TransformStamped update_tf_msg); + + ros::Subscriber create_tf_sub_; + ros::Subscriber remove_tf_sub_; + ros::Subscriber update_tf_sub_; + + std::vector< geometry_msgs::TransformStamped > active_tfs_; +}; // end class RvizTFPublisher + + +} // end namespace tf_keyboard_cal + +#endif diff --git a/include/tf_keyboard_cal/tf_remote_receiver.h b/include/tf_keyboard_cal/tf_remote_receiver.h new file mode 100644 index 0000000..df21e3d --- /dev/null +++ b/include/tf_keyboard_cal/tf_remote_receiver.h @@ -0,0 +1,85 @@ +/**************************************************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2017, Andy McEvoy + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, this list of + * conditions and the following disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER + * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ****************************************************************************************************/ +/* + * Author(s) : Andy McEvoy ( mcevoy.andy@gmail.com ) + * Desc : Object for wrapping tf control functionality + * Created : 09 - May - 2017 + */ + +#ifndef TF_REMOTE_RECEIVER_H +#define TF_REMOTE_RECEIVER_H + +#include +#include +#include +#include +#include +#include + +namespace tf_keyboard_cal +{ + +class TFRemoteReceiver +{ +public: + // singleton + static TFRemoteReceiver& getInstance() + { + static TFRemoteReceiver instance; + return instance; + } + + void createTF(geometry_msgs::TransformStamped create_tf_msg); + void removeTF(geometry_msgs::TransformStamped remove_tf_msg); + void updateTF(geometry_msgs::TransformStamped update_tf_msg); + void addIMarkerMenuPub(int menu_index, std::string menu_name); + void publishIMarkerMenuSelection(int menu_index); + + std::vector getTFNames(); + +private: + + TFRemoteReceiver(); + + ros::NodeHandle nh_; + ros::Publisher create_tf_pub_; + ros::Publisher remove_tf_pub_; + ros::Publisher update_tf_pub_; + + std::vector< std::string > tf_names_; + + std::vector< std::pair > menu_pubs_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener *tf_listener_; + +}; // end class TFRemoteReceiver + +} // end namespace tf_keyboard_cal + +#endif diff --git a/launch/rviz_demo.launch b/launch/rviz_demo.launch index 1768a98..462f9ea 100644 --- a/launch/rviz_demo.launch +++ b/launch/rviz_demo.launch @@ -1,6 +1,8 @@ - + - + + + diff --git a/launch/rviz_demo.rviz b/launch/rviz_demo.rviz index 3806e6c..0bab9d6 100644 --- a/launch/rviz_demo.rviz +++ b/launch/rviz_demo.rviz @@ -6,8 +6,9 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /InteractiveMarkers1 Splitter Ratio: 0.5 - Tree Height: 353 + Tree Height: 308 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -16,7 +17,7 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 + Splitter Ratio: 0.588679016 - Class: rviz/Views Expanded: - /Current View1 @@ -27,6 +28,8 @@ Panels: Name: Time SyncMode: 0 SyncSource: "" + - Class: tf_keyboard_cal/TFKeyboardCalGui + Name: TFKeyboardCalGui Visualization Manager: Class: "" Displays: @@ -36,7 +39,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.0299999993 Value: Lines Name: Grid Normal Cell Count: 0 @@ -53,7 +56,7 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false - thing: + a: Value: true world: Value: true @@ -64,7 +67,7 @@ Visualization Manager: Show Names: true Tree: world: - thing: + a: {} Update Interval: 0 Value: true @@ -79,18 +82,28 @@ Visualization Manager: Value: true - Alpha: 1 Axes Length: 0.5 - Axes Radius: 0.05 + Axes Radius: 0.0500000007 Class: rviz/Pose Color: 255; 25; 0 Enabled: false - Head Length: 0.3 - Head Radius: 0.1 + Head Length: 0.300000012 + Head Radius: 0.100000001 Name: PoseStamped Shaft Length: 1 - Shaft Radius: 0.05 + Shaft Radius: 0.0500000007 Shape: Axes Topic: /posestamped + Unreliable: false Value: false + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /tf_keyboard_cal_imarkers/update + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -115,38 +128,42 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 2.64484 + Distance: 7.7095623 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.408868 - Y: 0.78209 - Z: 0.395786 + X: 0.408868015 + Y: 0.782090008 + Z: 0.395785987 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.270398 + Near Clip Distance: 0.00999999978 + Pitch: 0.590397716 Target Frame: Value: Orbit (rviz) - Yaw: 4.89858 + Yaw: 5.51858854 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 678 + Height: 1056 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000013c000001f7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000001f7000000de00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001f7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000001f7000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055400000044fc0100000002fb0000000800540069006d00650100000000000005540000025800fffffffb0000000800540069006d0065010000000000000450000000000000000000000412000001f700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016d00000390fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001c3000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002000540046004b006500790062006f00610072006400430061006c00470075006901000001f1000001c7000001c700ffffff000000010000010f000001f7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000001f7000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000044fc0100000002fb0000000800540069006d00650100000000000007800000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000060d0000039000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false + TFKeyboardCalGui: + collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true - Width: 1364 - X: 0 - Y: 27 + Width: 1920 + X: 1920 + Y: 24 diff --git a/launch/tf_im_world_to_thing.launch b/launch/tf_im_world_to_thing.launch deleted file mode 100644 index d03be37..0000000 --- a/launch/tf_im_world_to_thing.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - diff --git a/launch/tf_keyboard_world_to_thing.launch b/launch/tf_keyboard_world_to_thing.launch deleted file mode 100644 index 697cfed..0000000 --- a/launch/tf_keyboard_world_to_thing.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/package.xml b/package.xml index 7b1b94b..c77825d 100644 --- a/package.xml +++ b/package.xml @@ -10,17 +10,19 @@ BSD https://github.com/davetcoleman/tf_keyboard_cal - https://github.com/davetcoleman/tf_keyboard_cal/issues + https://github.com/davetcoleman/tf_keyboard_cal/issues https://github.com/davetcoleman/tf_keyboard_cal/ Dave Coleman Andy McEvoy catkin - keyboard + + rviz rviz_visual_tools roscpp tf2 + tf2_msgs tf roslib rosparam_shortcuts @@ -28,7 +30,12 @@ roslint tf_conversions interactive_markers + message_generation + message_runtime + std_msgs + geometry_msgs + diff --git a/plugin_description.xml b/plugin_description.xml new file mode 100644 index 0000000..62513d4 --- /dev/null +++ b/plugin_description.xml @@ -0,0 +1,9 @@ + + + + A panel widget that allows you to create and control TFs. + + + diff --git a/resources/interactive_marker_screenshot.png b/resources/interactive_marker_screenshot.png deleted file mode 100644 index 5c664d2..0000000 Binary files a/resources/interactive_marker_screenshot.png and /dev/null differ diff --git a/resources/keyboard_screenshot.png b/resources/keyboard_screenshot.png deleted file mode 100644 index 170a4cd..0000000 Binary files a/resources/keyboard_screenshot.png and /dev/null differ diff --git a/scripts/tf_interactive_marker.py b/scripts/tf_interactive_marker.py deleted file mode 100755 index da6772f..0000000 --- a/scripts/tf_interactive_marker.py +++ /dev/null @@ -1,370 +0,0 @@ -#! /usr/bin/env python -# -*- coding: utf-8 -*- -""" -Created on 15/01/15 -@author: Sammy Pfeiffer - -# Software License Agreement (BSD License) -# -# Copyright (c) 2016, PAL Robotics, S.L. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -tf_interactive_marker.py contains a commandline -node that sets up a 6DOF Interactive Marker at /posestamped_im/update -and a PoseStamped topic at /posestamped and a -TF transform broadcaster with the pose of the interactive marker. -It prints on the commandline the current commandline transform and URDF -transform as you move the interactive marker. - -You can stop publishing transforms doing right click -and click on Stop Publish transform. - -As the help says: -Generate an interactive marker to setup your transforms. -Usage: -./tf_interactive_marker.py from_frame to frame position (x,y,z) orientation (r,p,y) or (x,y,z,w) -./tf_interactive_marker.py x y z r p y [deg] -./tf_interactive_marker.py x y z x y z w -For example (they do all the same): -./tf_interactive_marker.py base_footprint new_frame 1.0 0.0 1.0 0.0 0.0 90.0 deg -./tf_interactive_marker.py base_footprint new_frame 1.0 0.0 1.0 0.0 0.0 1.57 -./tf_interactive_marker.py base_footprint new_frame 1.0 0.0 1.0 0.0 0.7071 0.7071 0.0 - -The output looks like: - -Static transform publisher command (with roll pitch yaw): - rosrun tf static_transform_publisher 1.0 0.0 1.0 0.0 -0.0 1.57 base_footprint new_frame 50 - -Static transform publisher command (with quaternion): - rosrun tf static_transform_publisher 1.0 0.0 1.0 0.0 0.7068 0.7074 0.0 base_footprint new_frame 50 - -Roslaunch line of static transform publisher (rpy): - - -URDF format: - - -Example usage video: https://www.youtube.com/watch?v=C9BbFv-C9Zo - -""" - -# based on https://github.com/ros-visualization/visualization_tutorials/blob/indigo-devel/interactive_marker_tutorials/scripts/basic_controls.py -# and https://github.com/ros-visualization/visualization_tutorials/blob/indigo-devel/interactive_marker_tutorials/scripts/menu.py - -import sys -from copy import deepcopy -import rospy -from interactive_markers.interactive_marker_server import InteractiveMarkerServer -from interactive_markers.menu_handler import MenuHandler -from visualization_msgs.msg import InteractiveMarkerControl, Marker, InteractiveMarker, \ - InteractiveMarkerFeedback, InteractiveMarkerUpdate, InteractiveMarkerPose, MenuEntry -from geometry_msgs.msg import Point, Pose, PoseStamped, Vector3, Quaternion -import tf -from tf.transformations import quaternion_from_euler, euler_from_quaternion -from math import radians - - -class InteractiveMarkerPoseStampedPublisher(): - - def __init__(self, from_frame, to_frame, position, orientation): - self.server = InteractiveMarkerServer("posestamped_im") - o = orientation - r, p, y = euler_from_quaternion([o.w, o.x, o.y, o.z]) - rospy.loginfo("Publishing transform and PoseStamped from: " + - from_frame + " to " + to_frame + - " at " + str(position.x) + " " + str(position.y) + " " + str(position.z) + - " and orientation " + str(o.x) + " " + str(o.y) + - " " + str(o.z) + " " + str(o.w) + " or rpy " + - str(r) + " " + str(p) + " " + str(y)) - self.menu_handler = MenuHandler() - self.from_frame = from_frame - self.to_frame = to_frame - # Transform broadcaster - self.tf_br = tf.TransformBroadcaster() - - self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1) - rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name)) - pose = Pose() - pose.position = position - pose.orientation = orientation - self.last_pose = pose - self.print_commands(pose) - self.makeGraspIM(pose) - - self.server.applyChanges() - - def processFeedback(self, feedback): - """ - :type feedback: InteractiveMarkerFeedback - """ - s = "Feedback from marker '" + feedback.marker_name - s += "' / control '" + feedback.control_name + "'" - - mp = "" - if feedback.mouse_point_valid: - mp = " at " + str(feedback.mouse_point.x) - mp += ", " + str(feedback.mouse_point.y) - mp += ", " + str(feedback.mouse_point.z) - mp += " in frame " + feedback.header.frame_id - - if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: - rospy.logdebug(s + ": button click" + mp + ".") - - elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT: - rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".") - if feedback.menu_entry_id == 1: - rospy.logdebug("Start publishing transform...") - if self.tf_br is None: - self.tf_br = tf.TransformBroadcaster() - elif feedback.menu_entry_id == 2: - rospy.logdebug("Stop publishing transform...") - self.tf_br = None - - # When clicking this event triggers! - elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: - rospy.logdebug(s + ": pose changed") - - elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN: - rospy.logdebug(s + ": mouse down" + mp + ".") - - elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: - rospy.logdebug(s + ": mouse up" + mp + ".") - - # TODO: Print here the commands - # tf static transform - self.print_commands(feedback.pose) - - - self.last_pose = deepcopy(feedback.pose) - - self.server.applyChanges() - - def print_commands(self, pose, decimals=4): - p = pose.position - o = pose.orientation - - print "\n---------------------------" - print "Static transform publisher command (with roll pitch yaw):" - common_part = "rosrun tf static_transform_publisher " - pos_part = str(round(p.x, decimals)) + " " + str(round(p.y, decimals)) + " "+ str(round(p.z, decimals)) - r, p, y = euler_from_quaternion([o.w, o.x, o.y, o.z]) - ori_part = str(round(r, decimals)) + " " + str(round(p, decimals)) + " " + str(round(y, decimals)) - static_tf_cmd = common_part + pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50" - print " " + static_tf_cmd - print - print "Static transform publisher command (with quaternion):" - ori_q = str(round(o.x, decimals)) + " " + str(round(o.y, decimals)) + " " + str(round(o.z, decimals)) + " " + str(round(o.w, decimals)) - static_tf_cmd = common_part + pos_part + " " + ori_q + " " + self.from_frame + " " + self.to_frame + " 50" - print " " + static_tf_cmd - print - - print "Roslaunch line of static transform publisher (rpy):" - node_name = "from_" + self.from_frame + "_to_" + self.to_frame + "_static_tf" - roslaunch_part = ' ' - print roslaunch_part - print - - print "URDF format:" # - print ' ' - print "\n---------------------------" - - def makeArrow(self, msg): - marker = Marker() - - # An arrow that's squashed to easier view the orientation on roll - marker.type = Marker.ARROW - marker.scale.x = 0.15 - marker.scale.y = 0.05 - marker.scale.z = 0.05 - marker.color.r = 1.0 - marker.color.g = 0.0 - marker.color.b = 0.0 - marker.color.a = 1.0 - - return marker - - def makeBoxControl(self, msg): - control = InteractiveMarkerControl() - control.always_visible = True - control.markers.append(self.makeArrow(msg)) - msg.controls.append(control) - return control - - def makeGraspIM(self, pose): - """ - :type pose: Pose - """ - int_marker = InteractiveMarker() - int_marker.header.frame_id = self.from_frame - int_marker.pose = pose - int_marker.scale = 0.3 - - int_marker.name = "6dof_eef" - int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame - - # insert a box, well, an arrow - self.makeBoxControl(int_marker) - int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D - - control = InteractiveMarkerControl() - control.orientation.w = 1 - control.orientation.x = 1 - control.orientation.y = 0 - control.orientation.z = 0 - control.name = "rotate_x" - control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS - int_marker.controls.append(control) - - control = InteractiveMarkerControl() - control.orientation.w = 1 - control.orientation.x = 1 - control.orientation.y = 0 - control.orientation.z = 0 - control.name = "move_x" - control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS - int_marker.controls.append(control) - - control = InteractiveMarkerControl() - control.orientation.w = 1 - control.orientation.x = 0 - control.orientation.y = 1 - control.orientation.z = 0 - control.name = "rotate_z" - control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS - int_marker.controls.append(control) - - control = InteractiveMarkerControl() - control.orientation.w = 1 - control.orientation.x = 0 - control.orientation.y = 1 - control.orientation.z = 0 - control.name = "move_z" - control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS - int_marker.controls.append(control) - - control = InteractiveMarkerControl() - control.orientation.w = 1 - control.orientation.x = 0 - control.orientation.y = 0 - control.orientation.z = 1 - control.name = "rotate_y" - control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS - int_marker.controls.append(control) - - control = InteractiveMarkerControl() - control.orientation.w = 1 - control.orientation.x = 0 - control.orientation.y = 0 - control.orientation.z = 1 - control.name = "move_y" - control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS - int_marker.controls.append(control) - - control = InteractiveMarkerControl() - control.orientation.w = 1 - control.orientation.x = 0 - control.orientation.y = 0 - control.orientation.z = 1 - control.name = "move_3d" - control.interaction_mode = InteractiveMarkerControl.MOVE_3D - int_marker.controls.append(control) - - self.menu_handler.insert("Publish transform", - callback=self.processFeedback) - self.menu_handler.insert("Stop publishing transform", - callback=self.processFeedback) - - self.server.insert(int_marker, self.processFeedback) - self.menu_handler.apply(self.server, int_marker.name) - - def run(self): - r = rospy.Rate(20) - while not rospy.is_shutdown(): - if self.tf_br is not None: - pos = self.last_pose.position - ori = self.last_pose.orientation - self.tf_br.sendTransform( - (pos.x, pos.y, pos.z), - (ori.x, ori.y, ori.z, ori.w), - rospy.Time.now(), - self.to_frame, - self.from_frame - ) - ps = PoseStamped() - ps.pose = self.last_pose - ps.header.frame_id = self.from_frame - ps.header.stamp = rospy.Time.now() - self.pub.publish(ps) - r.sleep() - - -def usage(): - print "Generate an interactive marker to setup your transforms." - print "Usage:" - print sys.argv[0] + " from_frame to frame position (x,y,z) orientation (r,p,y) or (x,y,z,w)" - print sys.argv[0] + " x y z r p y [deg]" - print sys.argv[0] + " x y z x y z w" - print "For example (they all do the same):" - print sys.argv[0] + " base_footprint new_frame 1.0 0.0 1.0 0.0 0.0 90.0 deg" - print sys.argv[0] + " base_footprint new_frame 1.0 0.0 1.0 0.0 0.0 1.57" - print sys.argv[0] + " base_footprint new_frame 1.0 0.0 1.0 0.0 0.0 0.7071 0.7071" - print "You can stop publishing transforms in the right click menu of the interactive marker." - -if __name__ == "__main__": - rospy.init_node("tf_interactive_marker", anonymous=True) - cleaned_args = rospy.myargv(sys.argv) - if "-h" in cleaned_args or "--help" in cleaned_args: - usage() - exit() - if len(cleaned_args) not in [9, 10]: - print "Arguments error." - usage() - exit() - print len(cleaned_args) - from_frame = cleaned_args[1] - to_frame = cleaned_args[2] - px, py, pz = [float(item) for item in cleaned_args[3:6]] - position = Point(px, py, pz) - if len(cleaned_args) == 9: - r, p, y = [float(item) for item in cleaned_args[6:]] - quat = quaternion_from_euler(r, p, y) - orientation = Quaternion(quat[1], quat[2], quat[3], quat[0]) - elif cleaned_args[-1] == 'deg': - r, p, y = [radians(float(item)) for item in cleaned_args[6:-1]] - quat = quaternion_from_euler(r, p, y) - orientation = Quaternion(quat[1], quat[2], quat[3], quat[0]) - else: - x, y, z, w = [float(item) for item in cleaned_args[-4:]] - orientation = Quaternion(x, y, z, w) - - ig = InteractiveMarkerPoseStampedPublisher(from_frame, to_frame, - position, orientation) - ig.run() diff --git a/src/demo_tf_listener.cpp b/src/demo_tf_listener.cpp deleted file mode 100644 index 878b922..0000000 --- a/src/demo_tf_listener.cpp +++ /dev/null @@ -1,153 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik LLC nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman - Desc: -*/ - -#ifndef TF_KEYBOARD_CAL_DEMO_TF_LISTENER_H -#define TF_KEYBOARD_CAL_DEMO_TF_LISTENER_H - -// ROS -#include -#include - -// Conversions -#include -#include - -// Visualize -#include - -namespace tf_keyboard_cal -{ -class DemoTFListener -{ -public: - /** - * \brief Constructor - */ - DemoTFListener() : name_("demo_tf_listener"), nh_("~") - { - visual_tools_.reset(new rviz_visual_tools::RvizVisualTools("world", "/demo_tf_listener/markers")); - visual_tools_->deleteAllMarkers(); - - ROS_INFO_STREAM_NAMED(name_, "DemoTFListener Ready."); - } - - void runLoop() - { - // Allow time for listener to load - ros::Duration(1.0).sleep(); - - const bool verbose = false; - - ros::Rate rate(10.0); - while (ros::ok()) - { - Eigen::Affine3d pose; - - getPose("/world", "/thing", pose); - - // Show pose in console - if (verbose) - std::cout << "Position: \n" << pose.translation() << std::endl; - - // Visualize pose - visual_tools_->publishXArrow(pose); - - rate.sleep(); - } - } - - bool getPose(const std::string& from_frame, const std::string& to_frame, Eigen::Affine3d &pose) - { - tf::StampedTransform tf_transform; - try - { - tf_.lookupTransform(from_frame, to_frame, ros::Time(0), tf_transform); - } - catch (tf::TransformException ex) - { - ROS_ERROR("%s", ex.what()); - return false; - } - - // Convert to eigen - tf::transformTFToEigen(tf_transform, pose); - return true; - } - -private: - // -------------------------------------------------------- - // The short name of this class - std::string name_; - - // A shared node handle - ros::NodeHandle nh_; - - // For visualizing things in rviz - rviz_visual_tools::RvizVisualToolsPtr visual_tools_; - - tf::TransformListener tf_; - -}; // end class - -// Create boost pointers for this class -typedef boost::shared_ptr DemoTFListenerPtr; -typedef boost::shared_ptr DemoTFListenerConstPtr; - -} // namespace tf_keyboard_cal -#endif // TF_KEYBOARD_CAL_DEMO_TF_LISTENER_H - -int main(int argc, char** argv) -{ - // Initialize ROS - ros::init(argc, argv, "demo_tf_listener"); - ROS_INFO_STREAM_NAMED("main", "Starting DemoTFListener..."); - - // Allow the action server to recieve and send ros messages - ros::AsyncSpinner spinner(2); - spinner.start(); - - // Initialize main class - tf_keyboard_cal::DemoTFListener server; - server.runLoop(); - - // Shutdown - ROS_INFO_STREAM_NAMED("main", "Shutting down."); - ros::shutdown(); - - return 0; -} diff --git a/src/imarker_simple.cpp b/src/imarker_simple.cpp deleted file mode 100644 index 6326017..0000000 --- a/src/imarker_simple.cpp +++ /dev/null @@ -1,133 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, PickNik LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik LLC nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman - Desc: Use interactive markers in a C++ class via the external python node -*/ - -#include - -namespace tf_keyboard_cal -{ - IMarkerSimple::IMarkerSimple() - : nh_("~") - { - // Create Marker Server - const std::string imarker_topic = nh_.getNamespace() + "/imarker"; - imarker_server_.reset(new interactive_markers::InteractiveMarkerServer(imarker_topic, "", false)); - - // Initialize Pose - latest_pose_.orientation.w = 0; - - ros::Duration(2.0).sleep(); - - // Create imarker - make6DofMarker(latest_pose_); - - // Apply - imarker_server_->applyChanges(); - } - - geometry_msgs::Pose& IMarkerSimple::getPose() - { - return latest_pose_; - } - - void IMarkerSimple::iMarkerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) - { - // Ignore if not pose update - if (feedback->event_type != visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE) - return; - - latest_pose_ = feedback->pose; - } - - void IMarkerSimple::sendUpdatedIMarkerPose() - { - imarker_server_->setPose(int_marker_.name, latest_pose_); - imarker_server_->applyChanges(); - } - - void IMarkerSimple::make6DofMarker(const geometry_msgs::Pose &pose) - { - ROS_INFO_STREAM_NAMED(name_, "Making 6dof interactive marker named " << name_); - - int_marker_.header.frame_id = "world"; - int_marker_.pose = pose; - int_marker_.scale = 0.2; - - int_marker_.name = name_; - - // int_marker_.controls[0].interaction_mode = InteractiveMarkerControl::MENU; - - InteractiveMarkerControl control; - control.orientation.w = 1; - control.orientation.x = 1; - control.orientation.y = 0; - control.orientation.z = 0; - control.name = "rotate_x"; - control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; - int_marker_.controls.push_back(control); - control.name = "move_x"; - control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; - int_marker_.controls.push_back(control); - - control.orientation.w = 1; - control.orientation.x = 0; - control.orientation.y = 1; - control.orientation.z = 0; - control.name = "rotate_z"; - control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; - int_marker_.controls.push_back(control); - control.name = "move_z"; - control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; - int_marker_.controls.push_back(control); - - control.orientation.w = 1; - control.orientation.x = 0; - control.orientation.y = 0; - control.orientation.z = 1; - control.name = "rotate_y"; - control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; - int_marker_.controls.push_back(control); - control.name = "move_y"; - control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; - int_marker_.controls.push_back(control); - - imarker_server_->insert(int_marker_); - imarker_server_->setCallback(int_marker_.name, boost::bind(&IMarkerSimple::iMarkerCallback, this, _1)); - // menu_handler_.apply(*imarker_server_, int_marker_.name); - } - -} // namespace tf_keyboard_cal diff --git a/src/manual_tf_alignment.cpp b/src/manual_tf_alignment.cpp deleted file mode 100644 index c9eee6d..0000000 --- a/src/manual_tf_alignment.cpp +++ /dev/null @@ -1,279 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, PickNik LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik LLC nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* - * Author : Andy McEvoy, Dave Coleman - * Desc : Allows manual control of a TF through the keyboard - */ - - -#include - -// Parameter loading -#include - -#include -#include -#include - -#include - -namespace tf_keyboard_cal -{ - -ManualTFAlignment::ManualTFAlignment() - : nh_("~") -{ - // set defaults - mode_ = 1; - delta_ = 0.010; - - // initial camera transform - double x, y, z, roll, pitch, yaw; - - // Get settings from rosparam - std::size_t error = 0; - error += !rosparam_shortcuts::get(name_, nh_, "initial_x", x); - error += !rosparam_shortcuts::get(name_, nh_, "initial_y", y); - error += !rosparam_shortcuts::get(name_, nh_, "initial_z", z); - error += !rosparam_shortcuts::get(name_, nh_, "initial_roll", roll); - error += !rosparam_shortcuts::get(name_, nh_, "initial_pitch", pitch); - error += !rosparam_shortcuts::get(name_, nh_, "initial_yaw", yaw); - error += !rosparam_shortcuts::get(name_, nh_, "file_package", file_package_); - error += !rosparam_shortcuts::get(name_, nh_, "file_name", file_name_); - error += !rosparam_shortcuts::get(name_, nh_, "topic_name", topic_name_); - error += !rosparam_shortcuts::get(name_, nh_, "from", from_); - error += !rosparam_shortcuts::get(name_, nh_, "to", to_); - rosparam_shortcuts::shutdownIfError(name_, error); - - setPose(Eigen::Vector3d(x, y, z), Eigen::Vector3d(roll, pitch, yaw)); - - // default, save in tf_keyboard_cal/data - std::string package_path = ros::package::getPath(file_package_); - save_path_ = package_path + file_name_; - - // listen to keyboard topic - keyboard_sub_ = nh_.subscribe(topic_name_, 100, - &ManualTFAlignment::keyboardCallback, this); - - // Echo info - ROS_INFO_STREAM_NAMED("manualTF","Listening to topic : " << topic_name_); - ROS_INFO_STREAM_NAMED("manualTF","Transform from : " << from_); - ROS_INFO_STREAM_NAMED("manualTF","Transform to : " << to_); - ROS_INFO_STREAM_NAMED("manualTF","Config File : " << save_path_); - ROS_INFO_STREAM_NAMED("manualTF","Initial transform : " << x << ", " << y << ", " << z << ", " << roll << ", " << pitch << ", " << yaw ); -} - -void ManualTFAlignment::keyboardCallback(const keyboard::Key::ConstPtr& msg) -{ - int entry = msg->code; - const double fine = 0.001; - const double coarse = 0.01; - const double very_coarse = 0.1; - - //std::cout << "key: " << entry << std::endl; - - switch(entry) - { - case 112: // - writeTFToFile(); - break; - case 117: // (very coarse delta) - std::cout << "Delta = very coarse (0.1)" << std::endl; - delta_ = very_coarse; - break; - case 105: // (coarse delta) - std::cout << "Delta = coarse (0.01)" << std::endl; - delta_ = coarse; - break; - case 111: // (fine delta) - std::cout << "Delta = fine (0.001)" << std::endl; - delta_ = fine; - break; - - // X axis - case 113: // up - updateTF(1, delta_); - break; - case 97: // down - updateTF(1, -delta_); - break; - - // y axis - case 119: // up - updateTF(2, delta_); - break; - case 115: // down - updateTF(2, -delta_); - break; - - // z axis - case 101: // up - updateTF(3, delta_); - break; - case 100: // down - updateTF(3, -delta_); - break; - - // roll - case 114: // up - updateTF(4, delta_); - break; - case 102: // down - updateTF(4, -delta_); - break; - - // pitch - case 116: // up - updateTF(5, delta_); - break; - case 103: // down - updateTF(5, -delta_); - break; - - // yaw - case 121: // up - updateTF(6, delta_); - break; - case 104: // down - updateTF(6, -delta_); - break; - - default: - // don't do anything - break; - } -} - -void ManualTFAlignment::printMenu() -{ - std::cout << "Manual alignment of camera to world CS:" << std::endl; - std::cout << "=======================================" << std::endl; - std::cout << "MOVE: X Y Z R P YAW " << std::endl; - std::cout << "------------------------" << std::endl; - std::cout << "up q w e r t y " << std::endl; - std::cout << "down a s d f g h " << std::endl; - std::cout << std::endl; - std::cout << "Fast: u " << std::endl; - std::cout << "Med: i " << std::endl; - std::cout << "Slow: o " << std::endl; - std::cout << "Save: p " << std::endl; -} - -void ManualTFAlignment::publishTF() -{ - static tf::TransformBroadcaster br; - tf::Transform transform; - tf::Quaternion q; - - // set camera pose translation - transform.setOrigin( tf::Vector3( translation_[0], - translation_[1], - translation_[2]) ); - - // set camera pose rotation - q.setRPY(rotation_[0], rotation_[1], rotation_[2]); - transform.setRotation(q); - - // publish - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), from_ , to_)); -} - -void ManualTFAlignment::setPose(Eigen::Vector3d translation, Eigen::Vector3d rotation) -{ - translation_ = translation; - rotation_ = rotation; -} - -void ManualTFAlignment::updateTF(int mode, double delta) -{ - ROS_DEBUG_STREAM_NAMED("tf_alignment","mode = " << mode << ", delta = " << delta); - - switch(mode) - { - case 1: - translation_ += Eigen::Vector3d(delta, 0, 0); - break; - case 2: - translation_ += Eigen::Vector3d(0, delta, 0); - break; - case 3: - translation_ += Eigen::Vector3d(0, 0, delta); - break; - case 4: - rotation_ += Eigen::Vector3d(delta, 0, 0); - break; - case 5: - rotation_ += Eigen::Vector3d(0, delta, 0); - break; - case 6: - rotation_ += Eigen::Vector3d(0, 0, delta); - break; - default: - // don't do anything - break; - } -} - - -void ManualTFAlignment::writeTFToFile() -{ - std::ofstream file (save_path_.c_str()); //, std::ios::app); - ROS_INFO_STREAM_NAMED("tf_align.write","Writing transformation to file " << save_path_); - - if (!file.is_open()) - ROS_ERROR_STREAM_NAMED("tf_align.write","Output file could not be opened: " << save_path_); - else - { - ROS_INFO_STREAM_NAMED("tf_align.write","Initial transform : " << translation_[0] << ", " << translation_[1] - << ", " << translation_[2] << ", " << rotation_[0] << ", " << rotation_[1] - << ", " << rotation_[2] ); - - file << "initial_x: " << translation_[0] << std::endl; - file << "initial_y: " << translation_[1] << std::endl; - file << "initial_z: " << translation_[2] << std::endl; - file << "initial_roll: " << rotation_[0] << std::endl; - file << "initial_pitch: " << rotation_[1] << std::endl; - file << "initial_yaw: " << rotation_[2] << std::endl; - file << "from: " << from_ << std::endl; - file << "to: " << to_ << std::endl; - file << "file_package: " << file_package_ << std::endl; - file << "file_name: " << file_name_ << std::endl; - file << "topic_name: " << topic_name_ << std::endl; - } - file.close(); - -} - -} diff --git a/src/rviz_tf_publisher.cpp b/src/rviz_tf_publisher.cpp new file mode 100644 index 0000000..52edd34 --- /dev/null +++ b/src/rviz_tf_publisher.cpp @@ -0,0 +1,93 @@ +/**************************************************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2017, Andy McEvoy + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, this list of + * conditions and the following disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER + * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ****************************************************************************************************/ + +/* + * Author(s) : Andy McEvoy ( mcevoy.andy@gmail.com ) + * Desc : implementation of rviz_tf_publisher.h. See *.h file for documentation. + * Created : 2017 - May - 18 + */ + +#include + +namespace tf_keyboard_cal +{ + +RvizTFPublisher::RvizTFPublisher() + : nh_("~") +{ + create_tf_sub_ = nh_.subscribe("/rviz_tf_create", 10, &RvizTFPublisher::createTF, this); + remove_tf_sub_ = nh_.subscribe("/rviz_tf_remove", 10, &RvizTFPublisher::removeTF, this); + update_tf_sub_ = nh_.subscribe("/rviz_tf_update", 10, &RvizTFPublisher::updateTF, this); +} + +void RvizTFPublisher::createTF(geometry_msgs::TransformStamped create_tf_msg) +{ + active_tfs_.push_back(create_tf_msg); +} + +void RvizTFPublisher::removeTF(geometry_msgs::TransformStamped remove_tf_msg) +{ + for (std::size_t i = 0; i < active_tfs_.size(); i++) + { + if (remove_tf_msg.child_frame_id.compare(active_tfs_[i].child_frame_id) == 0 && + remove_tf_msg.header.frame_id.compare(active_tfs_[i].header.frame_id) == 0) + { + active_tfs_.erase(active_tfs_.begin() + i); + break; + } + } +} + +void RvizTFPublisher::updateTF(geometry_msgs::TransformStamped update_tf_msg) +{ + ROS_DEBUG_STREAM_NAMED("updateTF","from: " << update_tf_msg.header.frame_id << ", to: " << update_tf_msg.child_frame_id); + for (std::size_t i = 0; i < active_tfs_.size(); i++) + { + if (update_tf_msg.child_frame_id.compare(active_tfs_[i].child_frame_id) == 0 && + update_tf_msg.header.frame_id.compare(active_tfs_[i].header.frame_id) == 0) + { + ROS_DEBUG_STREAM_NAMED("updateTF","i = " << i); + active_tfs_[i].transform = update_tf_msg.transform; + } + } +} + +void RvizTFPublisher::publishTFs() +{ + ROS_DEBUG_STREAM_THROTTLE_NAMED(1,"publishTFs","publishing TFs: " << active_tfs_.size()); + static tf::TransformBroadcaster br; + + for (std::size_t i = 0; i < active_tfs_.size(); i++) + { + tf::StampedTransform tf; + active_tfs_[i].header.stamp = ros::Time::now(); + tf::transformStampedMsgToTF(active_tfs_[i], tf); + br.sendTransform(tf); + } +} +} // end namespace tf_keyboard_cal diff --git a/src/tf_keyboard.cpp b/src/tf_keyboard.cpp index 6e71897..ed26c52 100644 --- a/src/tf_keyboard.cpp +++ b/src/tf_keyboard.cpp @@ -36,25 +36,23 @@ Desc : Tweak a TF transform using a keyboard */ -#include +// #include +#include int main(int argc, char** argv) { ros::init(argc, argv, "tf_keyboard"); ROS_INFO_STREAM_NAMED("tf_keyboard","Starting keyboard control"); - ros::AsyncSpinner spinner(2); + ros::AsyncSpinner spinner(4); spinner.start(); - tf_keyboard_cal::ManualTFAlignment tf_align; - tf_align.printMenu(); + tf_keyboard_cal::RvizTFPublisher tf_pub; ros::Rate rate(30.0); // hz while ( ros::ok() ) { - // publish transform to camera - tf_align.publishTF(); - + tf_pub.publishTFs(); rate.sleep(); } diff --git a/src/tf_keyboard_cal_gui.cpp b/src/tf_keyboard_cal_gui.cpp new file mode 100644 index 0000000..2767eb7 --- /dev/null +++ b/src/tf_keyboard_cal_gui.cpp @@ -0,0 +1,941 @@ +/**************************************************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2017, Andy McEvoy + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, this list of + * conditions and the following disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER + * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ****************************************************************************************************/ + +/* + * Author(s) : Andy McEvoy ( mcevoy.andy@gmail.com ) + * Desc : Rviz display panel for dynamically manipulating TFs + * Created : 26 - April - 2017 + */ +#include + +// TODO: Why doesn't example put this in the include dir? +// Looks like it doesn't work if *.h is in include dir? +#include "tf_keyboard_cal_gui.h" + +namespace tf_keyboard_cal +{ + +std::vector< tf_data > active_tf_list_; + +boost::shared_ptr imarker_server_; +interactive_markers::MenuHandler imarker_menu_handler_; + +TFKeyboardCalGui::TFKeyboardCalGui(QWidget* parent) : rviz::Panel(parent) +{ + tab_widget_ = new QTabWidget; + connect(tab_widget_, SIGNAL(tabBarClicked(int)), this, SLOT(updateTabData(int))); + + new_create_tab_ = new createTFTab(); + tab_widget_->addTab(new_create_tab_, tr("Add / Remove")); + + new_manipulate_tab_ = new manipulateTFTab(); + tab_widget_->addTab(new_manipulate_tab_, tr("Manipulate")); + + new_save_load_tab_ = new saveLoadTFTab(); + tab_widget_->addTab(new_save_load_tab_, tr("Save / Load")); + + QVBoxLayout *main_layout = new QVBoxLayout; + main_layout->addWidget(tab_widget_); + setLayout(main_layout); + + imarker_server_.reset(new interactive_markers::InteractiveMarkerServer("tf_keyboard_cal_imarkers", "", false)); + ros::Duration(0.25).sleep(); + + updateTabData(0); +} + +void TFKeyboardCalGui::updateTabData(int index) +{ + ROS_DEBUG_STREAM_NAMED("updateTabData","index = " << index); + new_manipulate_tab_->updateTFList(); + new_create_tab_->updateFromList(); + + if (index == 1) // manipulate tab selected. + { + new_manipulate_tab_->setFocus(); + } +} + +createTFTab::createTFTab(QWidget *parent) : QWidget(parent) +{ + menu_handler_set_ = false; + + // TF controls + QLabel *from_label = new QLabel(tr("from:")); + QLabel *to_label = new QLabel(tr("to:")); + + from_ = new QComboBox; + from_->setEditable(true); + from_->addItem(tr("Select existing or add new TF")); + connect(from_, SIGNAL(editTextChanged(const QString &)), this, SLOT(fromTextChanged(const QString &))); + + to_ = new QLineEdit; + to_->setPlaceholderText("to TF"); + connect(to_, SIGNAL(textChanged(const QString &)), this, SLOT(toTextChanged(const QString &))); + + add_imarker_ = new QCheckBox("i marker?", this); + add_imarker_->setCheckState(Qt::Unchecked); + + add_imarker_menu_ = new QCheckBox("menus?", this); + add_imarker_menu_->setCheckState(Qt::Unchecked); + + create_tf_btn_ = new QPushButton(this); + create_tf_btn_->setText("Create TF"); + connect(create_tf_btn_, SIGNAL(clicked()), this, SLOT(createNewTF())); + + remove_tf_btn_ = new QPushButton(this); + remove_tf_btn_->setText("Remove TF"); + connect(remove_tf_btn_, SIGNAL(clicked()), this, SLOT(removeTF())); + + active_tfs_ = new QComboBox; + active_tfs_->addItem(tr("Select TF to delete")); + + // Layout + QGroupBox *add_section = new QGroupBox(tr("Add TF")); + + QHBoxLayout *from_row = new QHBoxLayout; + from_row->addWidget(from_label); + from_row->addWidget(from_); + + QHBoxLayout *to_row = new QHBoxLayout; + to_row->addWidget(to_label); + to_row->addWidget(to_); + + QHBoxLayout *create_row = new QHBoxLayout; + create_row->addWidget(add_imarker_); + create_row->addWidget(add_imarker_menu_); + create_row->addWidget(create_tf_btn_); + + QHBoxLayout *remove_row = new QHBoxLayout; + remove_row->addWidget(active_tfs_); + remove_row->addWidget(remove_tf_btn_); + + QVBoxLayout *add_controls = new QVBoxLayout; + add_controls->addLayout(from_row); + add_controls->addLayout(to_row); + add_controls->addLayout(create_row); + add_section->setLayout(add_controls); + + QGroupBox *remove_section = new QGroupBox(tr("Remove TF")); + + QVBoxLayout *remove_controls = new QVBoxLayout; + remove_controls->addLayout(remove_row); + remove_section->setLayout(remove_controls); + + QVBoxLayout *main_layout = new QVBoxLayout; + main_layout->addWidget(add_section); + main_layout->addWidget(remove_section); + setLayout(main_layout); + + id_ = 0; + + this->setFocus(); + + remote_receiver_ = &TFRemoteReceiver::getInstance(); +} + +void createTFTab::createNewTF() +{ + ROS_DEBUG_STREAM_NAMED("createNewTF","create new TF button pressed."); + ROS_DEBUG_STREAM_NAMED("createNewTF","from:to = " << from_tf_name_ << ":" << to_tf_name_); + + // create new tf + tf_data new_tf; + new_tf.id_ = id_++; + new_tf.from_ = from_tf_name_; + new_tf.to_ = to_tf_name_; + for (std::size_t i = 0; i < 6; i++) + { + new_tf.values_[i] = 0.0; + } + std::string text = std::to_string(new_tf.id_) + ": " + new_tf.from_ + "-" + new_tf.to_; + new_tf.name_ = QString::fromStdString(text); + + // interactive marker + new_tf.imarker_ = false; + if (add_imarker_->isChecked()) + { + new_tf.imarker_ = true; + ROS_DEBUG_STREAM_NAMED("createNewTF","imarker = " << new_tf.imarker_); + createNewIMarker(new_tf, add_imarker_menu_->isChecked()); + } + active_tf_list_.push_back(new_tf); + + // repopulate dropdown box + active_tfs_->clear(); + for (std::size_t i = 0; i < active_tf_list_.size(); i++) + { + active_tfs_->addItem(active_tf_list_[i].name_); + } + + // publish new tf + remote_receiver_->createTF(new_tf.getTFMsg()); + + updateFromList(); +} + +void createTFTab::createNewIMarker(tf_data new_tf, bool has_menu) +{ + ROS_DEBUG_STREAM_NAMED("createNewIMarker","creating interactive marker..."); + + // create a box to visualize the marker + visualization_msgs::Marker marker; + marker.type = visualization_msgs::Marker::CUBE; + marker.scale.x = 0.1; + marker.scale.y = 0.1; + marker.scale.z = 0.1; + marker.color.r = 0.5; + marker.color.g = 0.5; + marker.color.b = 0.5; + marker.color.a = 1.0; + + visualization_msgs::InteractiveMarker int_marker; + int_marker.header.frame_id = new_tf.from_; + int_marker.scale = 0.25; + int_marker.name = new_tf.name_.toStdString(); + + visualization_msgs::InteractiveMarkerControl box_control; + box_control.always_visible = true; + box_control.markers.push_back(marker); + box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D; + int_marker.controls.push_back(box_control); + + // create the handles to control individual dofs + visualization_msgs::InteractiveMarkerControl control; + control.orientation.w = 1; + control.orientation.x = 1; + control.orientation.y = 0; + control.orientation.z = 0; + control.name = "rotate_x"; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back(control); + control.name = "move_x"; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(control); + + control.orientation.w = 1; + control.orientation.x = 0; + control.orientation.y = 1; + control.orientation.z = 0; + control.name = "rotate_y"; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back(control); + control.name = "move_y"; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(control); + + control.orientation.w = 1; + control.orientation.x = 0; + control.orientation.y = 0; + control.orientation.z = 1; + control.name = "rotate_z"; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back(control); + control.name = "move_z"; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(control); + + imarker_server_->insert(int_marker); + imarker_server_->setCallback(int_marker.name, boost::bind( &createTFTab::processIMarkerFeedback, this, _1) ); + + if (has_menu && !menu_handler_set_) + { + ROS_DEBUG_STREAM_NAMED("createNewIMarker","set menu"); + + QString filters("rviz tf files (*.tf);;All files (*.*)"); + QString default_filter("rviz tf files (*.tf)"); + + std::string pkg_path = ros::package::getPath("tf_keyboard_cal"); + + QString directory = + QFileDialog::getOpenFileName(0, "Load TF Menu File", QString::fromStdString(pkg_path), filters, &default_filter); + + std::string full_load_path; + full_load_path = directory.toStdString(); + ROS_DEBUG_STREAM_NAMED("load","load_file = " << full_load_path); + + std::ifstream in_file(full_load_path); + std::string line; + int menu_idx = 1; + + // std::string menu_name(name); + + if (in_file.is_open()) + { + while (std::getline(in_file, line)) + { + // skip comments + boost::trim(line); + if (line.find("#") != std::string::npos) + { + continue; + } + + int result; + int num_sub_menus; + char menu_name[256]; + std::string menu_str; + std::string sub_menu_str; + + interactive_markers::MenuHandler::EntryHandle sub_menu_handle; + + result = sscanf(line.c_str(), "%d, %[^\t\n\r]s", &num_sub_menus, menu_name); + + if (result == 2) + { + + if (num_sub_menus == 0) + { + //ROS_DEBUG_STREAM_NAMED("createNewIMarker","create main item: " << menu_idx << ", " << menu_name); + imarker_menu_handler_.insert(menu_name, boost::bind( &createTFTab::processIMarkerFeedback, this, _1)); + menu_str = menu_name; + remote_receiver_->addIMarkerMenuPub(menu_idx, menu_str); + menu_idx++; + } + else + { + //ROS_DEBUG_STREAM_NAMED("createNewIMarker","create sub menu: " << menu_idx << ", " << menu_name); + sub_menu_handle = imarker_menu_handler_.insert(menu_name); + menu_str = menu_name; + menu_idx++; + } + } + else + { + ROS_WARN_STREAM_NAMED("createNewIMarker","skipping mal formed line: " << line); + } + + for (int i = 0; i < num_sub_menus; i++) + { + std::getline(in_file, line); + result = sscanf(line.c_str(), "%[^\t\n\r]s", menu_name); + + if (result == 1) + { + //ROS_DEBUG_STREAM_NAMED("createNewIMarker","create sub menu: " << menu_idx << ", " << menu_name); + imarker_menu_handler_.insert(sub_menu_handle, menu_name, boost::bind( &createTFTab::processIMarkerFeedback, this, _1)); + sub_menu_str = menu_name; + remote_receiver_->addIMarkerMenuPub(menu_idx, menu_str + "_" + sub_menu_str); + menu_idx++; + } + } + } + } + menu_handler_set_ = true; + } + + if (has_menu) + imarker_menu_handler_.apply(*imarker_server_, int_marker.name); + + imarker_server_->applyChanges(); +} + +void createTFTab::processIMarkerFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) +{ + ROS_DEBUG_STREAM_NAMED("processIMarkerFeedback","Feedback from " << feedback->marker_name); + + QString imarker_name = QString::fromStdString(feedback->marker_name); + + for (std::size_t i = 0; i < active_tf_list_.size(); i++) + { + if (active_tf_list_[i].name_ == imarker_name) + { + ROS_DEBUG_STREAM_NAMED("processIMarkerFeedback","update index = " << i); + manipulateTFTab::updateTFValues(i, feedback->pose); + remote_receiver_->updateTF(active_tf_list_[i].getTFMsg()); + break; + } + } + + if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT) + { + remote_receiver_->publishIMarkerMenuSelection(feedback->menu_entry_id); + } +} + +void createTFTab::updateFromList() +{ + // give tf a chance to update + ros::Duration(0.25).sleep(); + + // update from list + from_->clear(); + from_->addItem(tr("Select existing or add new TF")); + std::vector names = remote_receiver_->getTFNames(); + for (std::size_t i = 0; i < names.size(); i++) + { + from_->addItem(tr(names[i].c_str())); + } +} + +geometry_msgs::TransformStamped tf_data::getTFMsg() +{ + geometry_msgs::TransformStamped msg; + msg.header.frame_id = from_; + msg.header.stamp = ros::Time::now(); + msg.child_frame_id = to_; + msg.transform.translation.x = values_[0]; + msg.transform.translation.y = values_[1]; + msg.transform.translation.z = values_[2]; + + tf::Quaternion q; + double deg_to_rad = 3.14159265 / 180.0; + q.setRPY(values_[3] * deg_to_rad, values_[4] * deg_to_rad, values_[5] * deg_to_rad); + + msg.transform.rotation.x = q[0]; + msg.transform.rotation.y = q[1]; + msg.transform.rotation.z = q[2]; + msg.transform.rotation.w = q[3]; + + return msg; +} + +void createTFTab::removeTF() +{ + // find tf and remove from list + std::string tf_id = active_tfs_->currentText().toStdString(); + ROS_DEBUG_STREAM_NAMED("removeTF","remove TF: " << active_tfs_->currentIndex() << ", " << tf_id); + for (std::size_t i = 0; i < active_tf_list_.size(); i++) + { + if (active_tf_list_[i].name_ == active_tfs_->currentText()) + { + ROS_DEBUG_STREAM_NAMED("removeTF","remove index = " << i); + remote_receiver_->removeTF(active_tf_list_[i].getTFMsg()); + active_tf_list_.erase(active_tf_list_.begin() + i); + break; + } + } + + // repopulate dropdown lists + active_tfs_->clear(); + for (std::size_t i = 0; i < active_tf_list_.size(); i++) + { + active_tfs_->addItem(active_tf_list_[i].name_); + } + + // update from list + from_->clear(); + from_->addItem(tr("Select existing or add new TF")); + std::list names; + for (std::size_t i = 0; i < active_tf_list_.size(); i++) + { + names.push_back(active_tf_list_[i].from_); + names.push_back(active_tf_list_[i].to_); + } + + names.sort(); + names.unique(); + + std::list::iterator it; + for (it = names.begin(); it != names.end(); ++it) + { + from_->addItem(tr( (*it).c_str() )); + } + +} + +void createTFTab::fromTextChanged(QString text) +{ + from_tf_name_ = text.toStdString(); +} + +void createTFTab::toTextChanged(QString text) +{ + to_tf_name_ = text.toStdString(); +} + +manipulateTFTab::manipulateTFTab(QWidget *parent) : QWidget(parent) +{ + QLabel *tf_label = new QLabel(tr("tf:")); + active_tfs_ = new QComboBox; + active_tfs_->addItem(tr("Select TF")); + connect(active_tfs_, SIGNAL(activated(int)), this, SLOT(setQLineValues(int))); + + QLabel *xyz_delta_label = new QLabel(tr("xyz delta (m):")); + xyz_delta_box_ = new QLineEdit; + xyz_delta_box_->setText("0.1"); + xyz_delta_ = 0.1; + connect(xyz_delta_box_, SIGNAL(textChanged(const QString &)), this, SLOT(setXYZDelta(const QString &))); + + QLabel *rpy_delta_label = new QLabel(tr("rpy delta (deg):")); + rpy_delta_box_ = new QLineEdit; + rpy_delta_box_->setText("5.0"); + rpy_delta_ = 5.0; + connect(rpy_delta_box_, SIGNAL(textChanged(const QString &)), this, SLOT(setRPYDelta(const QString &))); + + QGroupBox *tf_ctrl_section = new QGroupBox(tr("Manipulation Data")); + + QHBoxLayout *tf_row = new QHBoxLayout; + tf_row->addWidget(tf_label); + tf_row->addWidget(active_tfs_); + + QHBoxLayout *xyz_delta_row = new QHBoxLayout; + xyz_delta_row->addWidget(xyz_delta_label); + xyz_delta_row->addWidget(xyz_delta_box_); + + QHBoxLayout *rpy_delta_row = new QHBoxLayout; + rpy_delta_row->addWidget(rpy_delta_label); + rpy_delta_row->addWidget(rpy_delta_box_); + + QVBoxLayout *tf_controls = new QVBoxLayout; + tf_controls->addLayout(tf_row); + tf_controls->addLayout(xyz_delta_row); + tf_controls->addLayout(rpy_delta_row); + tf_ctrl_section->setLayout(tf_controls); + + // Set up xyr rpy increment controls + QGroupBox *tf_increment_section = new QGroupBox(tr("Increment TF")); + QGridLayout *grid_layout = new QGridLayout(); + + std::vector labels = { "x (m):", "y (m):", "z (m):", "r (deg):", "p (deg):", "y (deg):"}; + for (std::size_t i = 0; i < labels.size(); i++) + { + QLabel *label = new QLabel(tr(labels[i].c_str())); + + QPushButton *minus = new QPushButton; + minus->setText("-"); + minus->setProperty("sign", -1.0); + minus->setProperty("dof", (int)i); + connect(minus, SIGNAL(clicked()), this, SLOT(incrementDOF())); + + QLineEdit *value = new QLineEdit; + value->setText("0.0"); + value->setProperty("dof", (int)i); + dof_qline_edits_.push_back(value); + connect(dof_qline_edits_[i], SIGNAL(textEdited(const QString &)), this, SLOT(editTFTextValue(const QString &))); + + QPushButton *plus = new QPushButton; + plus->setText("+"); + plus->setProperty("sign", 1.0); + plus->setProperty("dof", (int)i); + connect(plus, SIGNAL(clicked()), this, SLOT(incrementDOF())); + + grid_layout->addWidget(label, i, 1); + grid_layout->addWidget(minus, i, 2); + grid_layout->addWidget(value, i, 3); + grid_layout->addWidget(plus, i, 4); + } + + tf_increment_section->setLayout(grid_layout); + + // set main layout + QVBoxLayout *main_layout = new QVBoxLayout; + main_layout->addWidget(tf_ctrl_section); + main_layout->addWidget(tf_increment_section); + setLayout(main_layout); + + remote_receiver_ = &TFRemoteReceiver::getInstance(); +} + +void manipulateTFTab::keyPressEvent(QKeyEvent *event) +{ + + double xyz_delta = 0.01; + double rpy_delta = 1.0; // degrees + + switch (event->key()) + { + case Qt::Key_A: + ROS_DEBUG_STREAM_NAMED("keyPressEvent","x+"); + incrementDOF(0, 1.0); + break; + case Qt::Key_Q: + ROS_DEBUG_STREAM_NAMED("keyPressEvent","x-"); + incrementDOF(0, -1.0); + break; + case Qt::Key_W: + ROS_DEBUG_STREAM_NAMED("keyPressEvent","y+"); + incrementDOF(1, 1.0); + break; + case Qt::Key_S: + ROS_DEBUG_STREAM_NAMED("keyPressEvent","y-"); + incrementDOF(1, -1.0); + break; + case Qt::Key_E: + ROS_DEBUG_STREAM_NAMED("keyPressEvent","z+"); + incrementDOF(2, 1.0); + break; + case Qt::Key_D: + ROS_DEBUG_STREAM_NAMED("keyPressEvent","z-"); + incrementDOF(2, -1.0); + break; + case Qt::Key_R: + ROS_DEBUG_STREAM_NAMED("keyPressEvent","roll+"); + incrementDOF(3, 1.0); + break; + case Qt::Key_F: + ROS_DEBUG_STREAM_NAMED("keyPressEvent","roll-"); + incrementDOF(3, -1.0); + break; + case Qt::Key_T: + ROS_DEBUG_STREAM_NAMED("keyPressEvent","pitch+"); + incrementDOF(4, 1.0); + break; + case Qt::Key_G: + ROS_DEBUG_STREAM_NAMED("keyPressEvent","pitch-"); + incrementDOF(4, -1.0); + break; + case Qt::Key_Y: + ROS_DEBUG_STREAM_NAMED("keyPressEvent","yaw+"); + incrementDOF(5, 1.0); + break; + case Qt::Key_H: + ROS_DEBUG_STREAM_NAMED("keyPressEvent","yaw-"); + incrementDOF(5, -1.0); + break; + case Qt::Key_U: + ROS_DEBUG_STREAM_NAMED("keyPressEvent","fast"); + xyz_delta = 0.1; + rpy_delta = 5.0; // degrees + xyz_delta_box_->setText(QString::number(xyz_delta)); + rpy_delta_box_->setText(QString::number(rpy_delta)); + setXYZDelta(xyz_delta); + setRPYDelta(rpy_delta); + break; + case Qt::Key_I: + ROS_DEBUG_STREAM_NAMED("keyPressEvent","medium"); + xyz_delta = 0.01; + rpy_delta = 1.0; // degrees + xyz_delta_box_->setText(QString::number(xyz_delta)); + rpy_delta_box_->setText(QString::number(rpy_delta)); + setXYZDelta(xyz_delta); + setRPYDelta(rpy_delta); + break; + case Qt::Key_O: + ROS_DEBUG_STREAM_NAMED("keyPressEvent","slow"); + xyz_delta = 0.001; + rpy_delta = 0.5; // degrees + xyz_delta_box_->setText(QString::number(xyz_delta)); + rpy_delta_box_->setText(QString::number(rpy_delta)); + setXYZDelta(xyz_delta); + setRPYDelta(rpy_delta); + break; + default: + ROS_DEBUG_STREAM_NAMED("keyPressEvent","undefined key pressed"); + break; + } +} + +void manipulateTFTab::setQLineValues(int item_id) +{ + for (std::size_t i = 0; i < active_tf_list_.size(); i++) + { + if (active_tf_list_[i].name_ == active_tfs_->currentText()) + { + ROS_DEBUG_STREAM_NAMED("setQLineValues","name_ = " << active_tf_list_[i].name_.toStdString()); + for (std::size_t j = 0; j < 6; j++) + { + dof_qline_edits_[j]->setText(QString::number(active_tf_list_[i].values_[j])); + } + break; + } + } +} + +void manipulateTFTab::updateTFList() +{ + active_tfs_->clear(); + for (std::size_t i = 0; i < active_tf_list_.size(); i++) + { + active_tfs_->addItem(active_tf_list_[i].name_); + } + + setQLineValues(active_tfs_->currentIndex()); +} + +void manipulateTFTab::editTFTextValue(QString text) +{ + double value = text.toDouble(); + int dof = (sender()->property("dof")).toInt(); + + updateTFValues(dof, value); +} + +void manipulateTFTab::incrementDOF() +{ + int dof = (sender()->property("dof")).toInt(); + double sign = (sender()->property("sign")).toDouble(); + + incrementDOF(dof, sign); +} + +void manipulateTFTab::incrementDOF(int dof, double sign) +{ + double value = dof_qline_edits_[dof]->text().toDouble(); + + if (dof == 0 || dof == 1 || dof == 2) + value += (double)sign * xyz_delta_; + else + value += (double)sign * rpy_delta_; + + dof_qline_edits_[dof]->setText(QString::number(value)); + updateTFValues(dof, value); +} + +void manipulateTFTab::updateTFValues(int idx, geometry_msgs::Pose pose) +{ + // TODO: update euler angles, change pose -> transformstamped, call remote_receiver_, update gui text + active_tf_list_[idx].values_[0] = pose.position.x; + active_tf_list_[idx].values_[1] = pose.position.y; + active_tf_list_[idx].values_[2] = pose.position.z; + + double rad_to_deg = 180.0 / 3.14159265; + + Eigen::Quaterniond eigen_quaternion; + tf::quaternionMsgToEigen(pose.orientation, eigen_quaternion); + Eigen::Vector3d euler_angles = eigen_quaternion.toRotationMatrix().eulerAngles(2, 1, 0); + ROS_DEBUG_STREAM_NAMED("updateTFValues","euler_angles = " << euler_angles.transpose()); + active_tf_list_[idx].values_[3] = euler_angles[0] * rad_to_deg; + active_tf_list_[idx].values_[4] = euler_angles[1] * rad_to_deg; + active_tf_list_[idx].values_[5] = euler_angles[2] * rad_to_deg; +} + +void manipulateTFTab::updateTFValues(int dof, double value) +{ + for (std::size_t i = 0; i < active_tf_list_.size(); i++) + { + if (active_tf_list_[i].name_ == active_tfs_->currentText()) + { + ROS_DEBUG_STREAM_NAMED("updateTFValues","name_ = " << active_tf_list_[i].name_.toStdString()); + active_tf_list_[i].values_[dof] = value; + for (std::size_t j = 0; j < 6; j++) + { + ROS_DEBUG_STREAM_NAMED("updateTFValues", j << " = " << active_tf_list_[i].values_[j]); + } + + geometry_msgs::TransformStamped tf_msg = active_tf_list_[i].getTFMsg(); + ROS_DEBUG_STREAM_NAMED("updateTFValues","imarker_ = " << active_tf_list_[i].imarker_); + if (active_tf_list_[i].imarker_) + { + ROS_DEBUG_STREAM_NAMED("updateTFValues","update imarker pose..."); + geometry_msgs::Pose imarker_pose; + imarker_pose.position.x = tf_msg.transform.translation.x; + imarker_pose.position.y = tf_msg.transform.translation.y; + imarker_pose.position.z = tf_msg.transform.translation.z; + imarker_pose.orientation = tf_msg.transform.rotation; + imarker_server_->setPose(active_tf_list_[i].name_.toStdString(), imarker_pose); + imarker_server_->applyChanges(); + } + + remote_receiver_->updateTF(tf_msg); + break; + } + } +} + +void manipulateTFTab::setXYZDelta(QString text) +{ + double xyz_delta = text.toDouble(); + + ROS_DEBUG_STREAM_NAMED("set_xyz_delta","text = " << text.toStdString() << ", value = " << xyz_delta); + + setXYZDelta(xyz_delta); +} + +void manipulateTFTab::setXYZDelta(double xyz_delta) +{ + xyz_delta_ = xyz_delta; + + if (std::abs(xyz_delta_) > MAX_XYZ_DELTA) + { + ROS_WARN_STREAM_NAMED("setXYZDelta","Tried to set XYZ delta outside of limits. (+/-" << MAX_XYZ_DELTA << ")"); + xyz_delta_ > 0 ? xyz_delta_ = MAX_XYZ_DELTA : xyz_delta_ = -1.0 * MAX_XYZ_DELTA; + QString value = QString::number(xyz_delta_); + xyz_delta_box_->setText(value); + } + + ROS_DEBUG_STREAM_NAMED("set_xyz_delta","setting xyz_delta_ = " << xyz_delta_); + +} + +void manipulateTFTab::setRPYDelta(QString text) +{ + double rpy_delta = text.toDouble(); + + ROS_DEBUG_STREAM_NAMED("set_rpy_delta","text = " << text.toStdString() << ", value = " << rpy_delta); + + setRPYDelta(rpy_delta); +} + +void manipulateTFTab::setRPYDelta(double rpy_delta) +{ + rpy_delta_ = rpy_delta; + + if (std::abs(rpy_delta_) > MAX_RPY_DELTA) + { + ROS_WARN_STREAM_NAMED("setRPYDelta","Tried to set RPY delta outside of limits. (+/-" << MAX_RPY_DELTA << ")"); + rpy_delta_ > 0 ? rpy_delta_ = MAX_RPY_DELTA : rpy_delta_ = -1.0 * MAX_RPY_DELTA; + QString value = QString::number(rpy_delta_); + rpy_delta_box_->setText(value); + } + + ROS_DEBUG_STREAM_NAMED("set_rpy_delta","setting rpy_delta_ = " << rpy_delta_); +} + +saveLoadTFTab::saveLoadTFTab(QWidget *parent) : QWidget(parent) +{ + load_btn_ = new QPushButton(tr("Load TFs from File"), this); + connect(load_btn_ , SIGNAL(clicked()), this, SLOT(load())); + + save_btn_ = new QPushButton(tr("Save TFs to File"), this); + connect(save_btn_ , SIGNAL(clicked()), this, SLOT(save())); + + QGroupBox *file_section = new QGroupBox(tr("Save/Load TF Files")); + + QVBoxLayout *file_layout = new QVBoxLayout; + file_layout->addWidget(load_btn_); + file_layout->addWidget(save_btn_); + + file_section->setLayout(file_layout); + + QVBoxLayout *main_layout = new QVBoxLayout; + main_layout->addWidget(file_section); + setLayout(main_layout); + + remote_receiver_ = &TFRemoteReceiver::getInstance(); +} + +void saveLoadTFTab::load() +{ + QString filters("rviz tf files (*.tf);;All files (*.*)"); + QString default_filter("rviz tf files (*.tf)"); + + QString directory = + QFileDialog::getOpenFileName(0, "Load TF File", QDir::currentPath(), filters, &default_filter); + + full_load_path_ = directory.toStdString(); + ROS_DEBUG_STREAM_NAMED("load","load_file = " << full_load_path_); + + std::ifstream in_file(full_load_path_); + std::string line; + + int id; + int result; + char from[256]; + char to[256]; + float x, y, z, roll, pitch, yaw; + + if (in_file.is_open()) + { + while (std::getline(in_file, line)) + { + result = sscanf(line.c_str(), "%d %s %s %f %f %f %f %f %f", &id, from, to, &x, &y, &z, &roll, &pitch, &yaw); + + if (result == 9) + { + ROS_DEBUG_STREAM_NAMED("load",id); + ROS_DEBUG_STREAM_NAMED("load",from); + ROS_DEBUG_STREAM_NAMED("load",to); + ROS_DEBUG_STREAM_NAMED("load",x); + ROS_DEBUG_STREAM_NAMED("load",y); + ROS_DEBUG_STREAM_NAMED("load",z); + ROS_DEBUG_STREAM_NAMED("load",roll); + ROS_DEBUG_STREAM_NAMED("load",pitch); + ROS_DEBUG_STREAM_NAMED("load",yaw); + + // create new tf + tf_data new_tf; + new_tf.id_ = id; + std::string frame_id(from); + new_tf.from_ = frame_id; + std::string child_frame_id(to); + new_tf.to_ = child_frame_id; + new_tf.values_[0] = x; + new_tf.values_[1] = y; + new_tf.values_[2] = z; + new_tf.values_[3] = roll; + new_tf.values_[4] = pitch; + new_tf.values_[5] = yaw; + + std::string text = std::to_string(new_tf.id_) + ": " + new_tf.from_ + "-" + new_tf.to_; + new_tf.name_ = QString::fromStdString(text); + + active_tf_list_.push_back(new_tf); + remote_receiver_->createTF(new_tf.getTFMsg()); + } + else + { + ROS_INFO_STREAM_NAMED("load","skipping line: " << line); + } + } + } + else + { + ROS_ERROR_STREAM_NAMED("load","Unable to open file: " << full_load_path_); + } + in_file.close(); +} + +void saveLoadTFTab::save() +{ + QString filters("rviz tf files (*.tf);;All files (*.*)"); + QString default_filter("rviz tf files (*.tf)"); + + QString directory = + QFileDialog::getSaveFileName(0, "Save TF File", QDir::currentPath(), filters, &default_filter); + + full_save_path_ = directory.toStdString(); + + // check if user specified file extension + std::size_t found = full_save_path_.find("."); + if (found == std::string::npos) + full_save_path_ += ".tf"; + + ROS_DEBUG_STREAM_NAMED("save","save_file = " << full_save_path_); + + std::ofstream out_file; + out_file.open(full_save_path_); + + std::string header = "#ID FRAME_ID CHILD_FRAME_ID X Y Z ROLL PITCH YAW\n# (comment line)\n# space delimeter"; + + if (out_file.is_open()) + { + out_file << header << std::endl; + for (std::size_t i = 0; i < active_tf_list_.size(); i++) + { + out_file << active_tf_list_[i].id_ << " "; + out_file << active_tf_list_[i].from_ << " "; + out_file << active_tf_list_[i].to_; + for (std::size_t j = 0; j < 6; j++) + { + out_file << " " << active_tf_list_[i].values_[j]; + } + out_file << std::endl; + } + out_file.close(); + } + else + { + ROS_ERROR_STREAM_NAMED("save","Unable to open file: " << full_save_path_); + } +} + +} // end namespace tf_keyboard_cal + +#include +PLUGINLIB_EXPORT_CLASS(tf_keyboard_cal::TFKeyboardCalGui, rviz::Panel) diff --git a/src/tf_keyboard_cal_gui.h b/src/tf_keyboard_cal_gui.h new file mode 100644 index 0000000..a0af796 --- /dev/null +++ b/src/tf_keyboard_cal_gui.h @@ -0,0 +1,236 @@ +/**************************************************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2017, Andy McEvoy + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, this list of + * conditions and the following disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER + * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ****************************************************************************************************/ + +/* + * Author(s) : Andy McEvoy ( mcevoy.andy@gmail.com ) + * Desc : Rviz display panel for dynamically manipulating TFs + * Created : 25 - April - 2017 + */ + +#ifndef TF_KEYBOARD_CAL_GUI_H +#define TF_KEYBOARD_CAL_GUI_H + +#ifndef Q_MOC_RUN +#include +#include +#include +#endif + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include + +namespace tf_keyboard_cal +{ +struct tf_data{ + std::size_t id_; + std::string from_; + std::string to_; + bool imarker_; + QString name_; + double values_[6]; + + geometry_msgs::TransformStamped getTFMsg(); +}; + +/** + * Tab for creating, saving, loading, removing and deleting TFs + */ +class createTFTab : public QWidget +{ + Q_OBJECT + +public: + explicit createTFTab(QWidget *parent = 0); + void updateFromList(); + +protected Q_SLOTS: + void createNewTF(); + void removeTF(); + + void fromTextChanged(QString text); + void toTextChanged(QString text); + +private: + void processIMarkerFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + void createNewIMarker(tf_data new_tf, bool has_menu); + + bool menu_handler_set_; + + std::string from_tf_name_; + std::string to_tf_name_; + + std::size_t id_; + + QComboBox *from_; + QLineEdit *to_; + + QCheckBox *add_imarker_; + QCheckBox *add_imarker_menu_; + + QPushButton *create_tf_btn_; + QPushButton *remove_tf_btn_; + + QComboBox *active_tfs_; + + TFRemoteReceiver *remote_receiver_; +}; + +/** + * Tab for manipulating TFs + */ +class manipulateTFTab : public QWidget +{ + Q_OBJECT + +public: + explicit manipulateTFTab(QWidget *parent = 0); + void updateTFList(); + static void updateTFValues(int list_index, geometry_msgs::Pose pose); + +protected Q_SLOTS: + void incrementDOF(); + void incrementDOF(int dof, double sign); + + void editTFTextValue(QString text); + + void setXYZDelta(QString text); + void setRPYDelta(QString text); + + void setXYZDelta(double xyz_delta); + void setRPYDelta(double rpy_delta); + + void setQLineValues(int item_id); + +protected: + + + void keyPressEvent(QKeyEvent *); + // TODO: Don't think I need release... + // void keyReleaseEvent(QKeyEvent *); + +private: + + void updateTFValues(int dof, double value); + + // can't overload 'set + + static constexpr double MAX_XYZ_DELTA = 100.0; + static constexpr double MAX_RPY_DELTA = 360.0; + + double xyz_delta_; + double rpy_delta_; + + QComboBox *active_tfs_; + + // TODO: on select, update values + + QLineEdit *xyz_delta_box_; + QLineEdit *rpy_delta_box_; + + std::vector dof_qline_edits_; + + TFRemoteReceiver *remote_receiver_; +}; + +/** + * Tab for saving and loading TFs + */ +class saveLoadTFTab : public QWidget +{ + Q_OBJECT + +public: + explicit saveLoadTFTab(QWidget *parent = 0); + +protected Q_SLOTS: + void load(); + void save(); + +private: + QPushButton *load_btn_; + QPushButton *save_btn_; + + std::string full_save_path_; + std::string full_load_path_; + + TFRemoteReceiver *remote_receiver_; +}; + +/** + * Main class + */ +class TFKeyboardCalGui : public rviz::Panel +{ + Q_OBJECT +public: + explicit TFKeyboardCalGui(QWidget *parent = 0); + +protected Q_SLOTS: + void updateTabData(int); + +private: + QTabWidget *tab_widget_; + + createTFTab *new_create_tab_; + manipulateTFTab *new_manipulate_tab_; + saveLoadTFTab *new_save_load_tab_; +}; + +} // end namespace tf_keyboard_cal + +#endif diff --git a/src/tf_remote_receiver.cpp b/src/tf_remote_receiver.cpp new file mode 100644 index 0000000..87f7a64 --- /dev/null +++ b/src/tf_remote_receiver.cpp @@ -0,0 +1,106 @@ +/**************************************************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2017, Andy McEvoy + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, this list of + * conditions and the following disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER + * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ****************************************************************************************************/ +/* + * Author(s) : Andy McEvoy ( mcevoy.andy@gmail.com ) + * Desc : implementation of TFRemoteReceiver. See H file for documentation. + * Created : 09 - May - 2017 + */ + +#include + +namespace tf_keyboard_cal +{ + +TFRemoteReceiver::TFRemoteReceiver() + : nh_("~") +{ + std::cout << "\033[1;36m" << "Remote receiver initialized" << "\033[0m" << std::endl; + + create_tf_pub_ = nh_.advertise("/rviz_tf_create", 10); + remove_tf_pub_ = nh_.advertise("/rviz_tf_remove", 10); + update_tf_pub_ = nh_.advertise("/rviz_tf_update", 10); + + tf_listener_ = new tf2_ros::TransformListener(tf_buffer_); +} + +void TFRemoteReceiver::createTF(geometry_msgs::TransformStamped create_tf_msg) +{ + create_tf_pub_.publish(create_tf_msg); +} + +void TFRemoteReceiver::removeTF(geometry_msgs::TransformStamped remove_tf_msg) +{ + remove_tf_pub_.publish(remove_tf_msg); +} + +void TFRemoteReceiver::updateTF(geometry_msgs::TransformStamped update_tf_msg) +{ + update_tf_pub_.publish(update_tf_msg); +} + +std::vector TFRemoteReceiver::getTFNames() +{ + tf_buffer_._getFrameStrings(tf_names_); + + return tf_names_; +} + +void TFRemoteReceiver::addIMarkerMenuPub(int menu_index, std::string menu_name) +{ + // replace ' ' with '_' + for (std::size_t i = 0; i < menu_name.size(); i++) + { + if (menu_name[i] == ' ') + menu_name[i] = '_'; + } + + + std::string new_topic_name = "/imarker/" + menu_name; + ros::Publisher new_pub = nh_.advertise(new_topic_name, 1); + + std::pair new_menu_pub(menu_index, new_pub); + menu_pubs_.push_back(new_menu_pub); +} + +void TFRemoteReceiver::publishIMarkerMenuSelection(int menu_index) +{ + std_msgs::Bool msg; + msg.data = true; + + for (std::size_t i = 0; i < menu_pubs_.size(); i++) + { + if (menu_pubs_[i].first == menu_index) + { + menu_pubs_[i].second.publish(msg); + break; + } + } +} + + +} // end namespace tf_keyboard_cal