From 097134322514eb7f245dd81b8c7d741eb134a720 Mon Sep 17 00:00:00 2001 From: 7929f1cfe06ad78d8b8c60a417e1b4a8de37050c Date: Sat, 6 Jun 2020 21:17:01 +0800 Subject: [PATCH 01/26] Add .gitignore --- .gitignore | 60 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 60 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..808fe85 --- /dev/null +++ b/.gitignore @@ -0,0 +1,60 @@ +*~ +~$ +.swp$ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/.*Action\.msg$ +msg/.*ActionFeedback\.msg$ +msg/.*ActionGoal\.msg$ +msg/.*ActionResult\.msg$ +msg/.*Feedback\.msg$ +msg/.*Goal\.msg$ +msg/.*Result\.msg$ +msg/_.*\.py$ + +\.pcd$ +.pyc$ + +# Generated by dynamic reconfigure +\.cfgc$ +/cfg/cpp/ +/cfg/.*\.py$ + +# Ignore generated docs +.dox$ +.wikidoc$ + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_.*\.py$ +\.pcd$ +.pyc$ +qtcreator-* +*.user + +*~$ + +# Emacs +.#* + +# VSCode +.vscode + +# Vim +*.swp + +# Continous Integration +.moveit_ci + +*.pyc + +#gdb +*.gdb From f673c702dc500340cf72aa5933b035d697b982d3 Mon Sep 17 00:00:00 2001 From: 7929f1cfe06ad78d8b8c60a417e1b4a8de37050c Date: Sat, 6 Jun 2020 21:20:02 +0800 Subject: [PATCH 02/26] Add moveit_calibration_gui package.xml and CMakeLists.txt files --- moveit_calibration_gui/CMakeLists.txt | 79 +++++++++++++++++++++++++++ moveit_calibration_gui/package.xml | 41 ++++++++++++++ 2 files changed, 120 insertions(+) create mode 100644 moveit_calibration_gui/CMakeLists.txt create mode 100644 moveit_calibration_gui/package.xml diff --git a/moveit_calibration_gui/CMakeLists.txt b/moveit_calibration_gui/CMakeLists.txt new file mode 100644 index 0000000..6e05815 --- /dev/null +++ b/moveit_calibration_gui/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.1.3) +project(moveit_calibration_gui) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_EXTENSIONS OFF) + +if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +find_package(catkin REQUIRED COMPONENTS + class_loader + geometric_shapes + moveit_calibration_plugins + moveit_ros_planning_interface + moveit_ros_visualization + moveit_visual_tools + image_geometry + pluginlib + rosconsole + roscpp + rviz + tf2_eigen +) + +find_package(Eigen3 REQUIRED) + +# Qt Stuff +if(rviz_QT_VERSION VERSION_LESS "5") + find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui) + include(${QT_USE_FILE}) + macro(qt_wrap_ui) + qt4_wrap_ui(${ARGN}) + endmacro() +else() + find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) + set(QT_LIBRARIES Qt5::Widgets) + macro(qt_wrap_ui) + qt5_wrap_ui(${ARGN}) + endmacro() +endif() + +set(CMAKE_INCLUDE_CURRENT_DIR ON) +set(CMAKE_AUTOMOC ON) +add_definitions(-DQT_NO_KEYWORDS) + +catkin_package( + LIBRARIES + moveit_handeye_calibration_rviz_plugin_core + INCLUDE_DIRS + handeye_calibration_rviz_plugin/include + CATKIN_DEPENDS + moveit_ros_planning_interface + moveit_visual_tools + moveit_calibration_plugins + roscpp + rviz + DEPENDS + EIGEN3 +) + +include_directories( + handeye_calibration_rviz_plugin/include + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS}) + +include_directories(SYSTEM + ${EIGEN3_INCLUDE_DIRS} + ${QT_INCLUDE_DIR}) + +add_subdirectory(handeye_calibration_rviz_plugin) + +install(FILES + handeye_calibration_rviz_plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +if (CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) +endif() diff --git a/moveit_calibration_gui/package.xml b/moveit_calibration_gui/package.xml new file mode 100644 index 0000000..dba61fe --- /dev/null +++ b/moveit_calibration_gui/package.xml @@ -0,0 +1,41 @@ + + moveit_calibration_gui + 0.1.0 + MoveIt calibration gui + + Yu Yan + + Yu Yan + + BSD + + http://moveit.ros.org + https://github.com/ros-planning/moveit_calibration/issues + https://github.com/ros-planning/moveit_calibration + + catkin + pkg-config + + class_loader + eigen + qtbase5-dev + image_geometry + + geometric_shapes + moveit_ros_visualization + moveit_calibration_plugins + moveit_ros_planning_interface + moveit_visual_tools + pluginlib + rosconsole + roscpp + rviz + tf2_eigen + + rostest + + + + + + From fcaa1e196f89803124d7fce9636bd04beb1397c8 Mon Sep 17 00:00:00 2001 From: 7929f1cfe06ad78d8b8c60a417e1b4a8de37050c Date: Sat, 6 Jun 2020 21:20:34 +0800 Subject: [PATCH 03/26] Add handeye calibration rviz plugin export files --- .../CMakeLists.txt | 35 ++++++++++++++++ .../src/plugin_init.cpp | 40 +++++++++++++++++++ ...ye_calibration_rviz_plugin_description.xml | 7 ++++ 3 files changed, 82 insertions(+) create mode 100644 moveit_calibration_gui/handeye_calibration_rviz_plugin/CMakeLists.txt create mode 100644 moveit_calibration_gui/handeye_calibration_rviz_plugin/src/plugin_init.cpp create mode 100644 moveit_calibration_gui/handeye_calibration_rviz_plugin_description.xml diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/CMakeLists.txt b/moveit_calibration_gui/handeye_calibration_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000..52819fe --- /dev/null +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/CMakeLists.txt @@ -0,0 +1,35 @@ +find_package(OpenCV REQUIRED) + +set(HEADERS + include/moveit/handeye_calibration_rviz_plugin/handeye_calibration_gui.h + include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h + include/moveit/handeye_calibration_rviz_plugin/handeye_context_widget.h + include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h +) + +#catkin_lint: ignore_once missing_directory +include_directories(${CMAKE_CURRENT_BINARY_DIR}) + +# Plugin Source +set(SOURCE_FILES + src/handeye_calibration_gui.cpp + src/handeye_target_widget.cpp + src/handeye_context_widget.cpp + src/handeye_control_widget.cpp +) + +set(MOVEIT_LIB_NAME moveit_handeye_calibration_rviz_plugin) +add_library(${MOVEIT_LIB_NAME}_core ${SOURCE_FILES} ${HEADERS}) +set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(${MOVEIT_LIB_NAME}_core + ${catkin_LIBRARIES} ${OpenCV_LIBS} ${rviz_DEFAULT_PLUGIN_LIBRARIES} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(${MOVEIT_LIB_NAME} src/plugin_init.cpp) +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) + +install(TARGETS ${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/plugin_init.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/plugin_init.cpp new file mode 100644 index 0000000..9287316 --- /dev/null +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/plugin_init.cpp @@ -0,0 +1,40 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Intel Corporation. + * 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 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: Yu Yan */ + +#include +#include + +CLASS_LOADER_REGISTER_CLASS(moveit_rviz_plugin::HandEyeCalibrationGui, rviz::Panel) \ No newline at end of file diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin_description.xml b/moveit_calibration_gui/handeye_calibration_rviz_plugin_description.xml new file mode 100644 index 0000000..cb96254 --- /dev/null +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin_description.xml @@ -0,0 +1,7 @@ + + + + Gui widget for the hand-eye calibration. + + + \ No newline at end of file From fefb29d3389694b2a689ba3b36d99a196a0fb8e1 Mon Sep 17 00:00:00 2001 From: 7929f1cfe06ad78d8b8c60a417e1b4a8de37050c Date: Sat, 6 Jun 2020 21:21:05 +0800 Subject: [PATCH 04/26] Add handeye calibration rviz panel --- .../handeye_calibration_gui.h | 85 ++++++++++++++ .../src/handeye_calibration_gui.cpp | 109 ++++++++++++++++++ 2 files changed, 194 insertions(+) create mode 100644 moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_calibration_gui.h create mode 100644 moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_gui.cpp diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_calibration_gui.h b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_calibration_gui.h new file mode 100644 index 0000000..9029e33 --- /dev/null +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_calibration_gui.h @@ -0,0 +1,85 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Intel Corporation. + * 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 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: Yu Yan */ + +#ifndef MOVEIT_HANDEYE_CALIBRATION_RVIZ_PLUGIN_HANDEYE_CALIBRATION_GUI_ +#define MOVEIT_HANDEYE_CALIBRATION_RVIZ_PLUGIN_HANDEYE_CALIBRATION_GUI_ + +// qt + +// ros +#include + +// local +#include +#include +#include + +#ifndef Q_MOC_RUN +#include +#include +#endif + +namespace moveit_rviz_plugin +{ +class HandEyeCalibrationGui : public rviz::Panel +{ + Q_OBJECT +public: + explicit HandEyeCalibrationGui(QWidget* parent = 0); + ~HandEyeCalibrationGui() override; + + virtual void load(const rviz::Config& config); + virtual void save(rviz::Config config) const; + +private: + // ****************************************************************************************** + // Qt Components + // ****************************************************************************************** + + TargetTabWidget* tab_target_; + ContextTabWidget* tab_context_; + ControlTabWidget* tab_control_; + + // ****************************************************************************************** + // Ros Components + // ****************************************************************************************** + + rviz_visual_tools::TFVisualToolsPtr tf_tools_; +}; + +} // namedist moveit_rviz_plugin + +#endif \ No newline at end of file diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_gui.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_gui.cpp new file mode 100644 index 0000000..9ab1344 --- /dev/null +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_gui.cpp @@ -0,0 +1,109 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Intel Corporation. + * 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 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: Yu Yan */ + +#include + +#include +#include + +namespace moveit_rviz_plugin +{ +HandEyeCalibrationGui::HandEyeCalibrationGui(QWidget* parent) : rviz::Panel(parent) +{ + setMinimumSize(695, 460); + // Basic widget container + QVBoxLayout* layout = new QVBoxLayout(); + setLayout(layout); + + // Description + QLabel* description = new QLabel(this); + description->setText(QString("Configure the position and orientation of your 3D sensors to work with Moveit!")); + description->setWordWrap(true); + description->setMinimumWidth(1); + layout->addWidget(description); + + // Tab menu ------------------------------------------------------------ + QTabWidget* tabs = new QTabWidget(this); + tab_target_ = new TargetTabWidget(); + + tf_tools_.reset(new rviz_visual_tools::TFVisualTools(250)); + + tab_context_ = new ContextTabWidget(); + tab_context_->setTFTool(tf_tools_); + connect(tab_target_, SIGNAL(cameraInfoChanged(sensor_msgs::CameraInfo)), tab_context_, + SLOT(setCameraInfo(sensor_msgs::CameraInfo))); + connect(tab_target_, SIGNAL(opticalFrameChanged(const std::string&)), tab_context_, + SLOT(setOpticalFrame(const std::string&))); + + tab_control_ = new ControlTabWidget(); + tab_control_->setTFTool(tf_tools_); + connect(tab_context_, SIGNAL(sensorMountTypeChanged(int)), tab_control_, SLOT(UpdateSensorMountType(int))); + connect(tab_context_, SIGNAL(frameNameChanged(std::map)), tab_control_, + SLOT(updateFrameNames(std::map))); + connect(tab_control_, SIGNAL(sensorPoseUpdate(double, double, double, double, double, double)), tab_context_, + SLOT(updateCameraPose(double, double, double, double, double, double))); + + tabs->addTab(tab_target_, "Target"); + tabs->addTab(tab_context_, "Context"); + tabs->addTab(tab_control_, "Calibrate"); + layout->addWidget(tabs); + + ROS_INFO_STREAM("handeye calibration gui created."); +} + +HandEyeCalibrationGui::~HandEyeCalibrationGui() = default; + +void HandEyeCalibrationGui::save(rviz::Config config) const +{ + tab_target_->saveWidget(config); + tab_context_->saveWidget(config); + tab_control_->saveWidget(config); + rviz::Panel::save(config); +} + +// Load all configuration data for this panel from the given Config object. +void HandEyeCalibrationGui::load(const rviz::Config& config) +{ + rviz::Panel::load(config); + + tab_target_->loadWidget(config); + tab_context_->loadWidget(config); + tab_control_->loadWidget(config); + + ROS_INFO_STREAM("handeye calibration gui loaded."); +} + +} // namespace moveit_rviz_plugin \ No newline at end of file From cdba87617781aeef98b2ddd08224818c059c3bf8 Mon Sep 17 00:00:00 2001 From: 7929f1cfe06ad78d8b8c60a417e1b4a8de37050c Date: Sat, 6 Jun 2020 21:21:24 +0800 Subject: [PATCH 05/26] Add handeye calibration target detection widget --- .../handeye_target_widget.h | 205 +++++++ .../src/handeye_target_widget.cpp | 538 ++++++++++++++++++ 2 files changed, 743 insertions(+) create mode 100644 moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h create mode 100644 moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h new file mode 100644 index 0000000..094080b --- /dev/null +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h @@ -0,0 +1,205 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Intel Corporation. + * 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 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: Yu Yan */ + +#ifndef MOVEIT_HANDEYE_CALIBRATION_RVIZ_PLUGIN_HANDEYE_TARGET_WIDGET_ +#define MOVEIT_HANDEYE_CALIBRATION_RVIZ_PLUGIN_HANDEYE_TARGET_WIDGET_ + +// qt +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// opencv +#include +#include + +// ros +#include +#include +#include + +#include +#include + +#include +#include +#include + +#ifndef Q_MOC_RUN +#include +#include +#endif + +Q_DECLARE_METATYPE(sensor_msgs::CameraInfo); +Q_DECLARE_METATYPE(std::string); + +namespace moveit_rviz_plugin +{ +// ************************************************** +// Custom QComboBox for image and camera_info topic +// ************************************************** +class RosTopicComboBox : public QComboBox +{ + Q_OBJECT +public: + explicit RosTopicComboBox(QWidget* parent = Q_NULLPTR) : QComboBox(parent) + { + } + ~RosTopicComboBox() = default; + + void addMsgsFilterType(QString msgs_type); + + bool hasTopic(const QString& topic_name); + + bool getFilteredTopics(); + +protected: + void mousePressEvent(QMouseEvent* event); + + QSet message_types_; + QSet image_topics_; +}; + +class TargetTabWidget : public QWidget +{ + Q_OBJECT +public: + explicit TargetTabWidget(QWidget* parent = Q_NULLPTR); + ~TargetTabWidget() + { + target_.reset(); + target_plugins_loader_.reset(); + camera_info_.reset(); + } + + void loadWidget(const rviz::Config& config); + void saveWidget(rviz::Config& config); + + bool loadTargetPlugin(); + + bool createTargetInstance(const std::string& plugin_name); + + void fillDictionaryIds(std::string id = ""); + + void imageCallback(const sensor_msgs::ImageConstPtr& msg); + + void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& msg); + +private Q_SLOTS: + + // Called when the current item of target_type_ changed + void targetTypeComboboxChanged(const QString& text); + + // Called when the create_target_btn clicked + void createTargetImageBtnClicked(bool clicked); + + // Called when the save_target_btn clicked + void saveTargetImageBtnClicked(bool clicked); + + // Called when the intrinsic or real params of the target changed + void targetParamsSet(const QString& text = ""); + + // Called when the item of image_topic_field_ combobox is selected + void imageTopicComboboxChanged(const QString& topic); + + // Called when the item of camera_info_topic_field_ combobox is selected + void cameraInfoComboBoxChanged(const QString& topic); + +Q_SIGNALS: + + void cameraInfoChanged(sensor_msgs::CameraInfo msg); + + void opticalFrameChanged(const std::string& frame_id); + +private: + // ************************************************************** + // Qt components + // ************************************************************** + + // Target intrinsic params + QComboBox* target_type_; + QComboBox* dictionary_id_; + std::map target_params_; + + // Target 3D pose recognition + RosTopicComboBox* image_topic_; + RosTopicComboBox* camera_info_topic_; + std::map ros_topics_; + std::map target_real_dims_; + + // Target Image display, create and save + QLabel* target_display_label_; + QPushButton* create_target_btn_; + QPushButton* save_target_btn_; + + // ************************************************************** + // Variables + // ************************************************************** + + cv::Mat target_image_; + + std::string optical_frame_; + + sensor_msgs::CameraInfoPtr camera_info_; + + // ************************************************************** + // Ros components + // ************************************************************** + ros::NodeHandle nh_; + std::unique_ptr > target_plugins_loader_; + pluginlib::UniquePtr target_; + image_transport::ImageTransport it_; + image_transport::Subscriber image_sub_; + image_transport::Publisher image_pub_; + ros::Subscriber camerainfo_sub_; + // tf broadcaster + tf2_ros::TransformBroadcaster tf_pub_; +}; + +} // namedist moveit_rviz_plugin + +#endif \ No newline at end of file diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp new file mode 100644 index 0000000..4888081 --- /dev/null +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp @@ -0,0 +1,538 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Intel Corporation. + * 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 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: Yu Yan */ + +#include + +namespace moveit_rviz_plugin +{ +const std::string LOGNAME = "handeye_target_widget"; + +void RosTopicComboBox::addMsgsFilterType(QString msgs_type) +{ + message_types_.insert(msgs_type); +} + +bool RosTopicComboBox::hasTopic(const QString& topic_name) +{ + getFilteredTopics(); + return image_topics_.contains(topic_name); +} + +bool RosTopicComboBox::getFilteredTopics() +{ + // Get all topic names + ros::master::V_TopicInfo ros_topic_vec; + if (ros::master::getTopics(ros_topic_vec)) + { + image_topics_.clear(); + // Filter out the topic names with specific topic type + for (const ros::master::TopicInfo& topic_info : ros_topic_vec) + { + if (message_types_.contains(QString(topic_info.datatype.c_str()))) + { + image_topics_.insert(QString(topic_info.name.c_str())); + } + } + } + + clear(); + addItem(QString("")); + for (const QString& topic : image_topics_) + { + addItem(topic); + } + + return !image_topics_.isEmpty(); +} + +void RosTopicComboBox::mousePressEvent(QMouseEvent* event) +{ + getFilteredTopics(); + showPopup(); +} + +TargetTabWidget::TargetTabWidget(QWidget* parent) + : QWidget(parent), nh_("~"), it_(nh_), target_plugins_loader_(nullptr), target_(nullptr) +{ + // Target setting tab area ----------------------------------------------- + QHBoxLayout* layout = new QHBoxLayout(); + this->setLayout(layout); + QVBoxLayout* layout_left = new QVBoxLayout(); + layout->addLayout(layout_left); + + // Target creation area + QGroupBox* group_left_top = new QGroupBox("Target_Intrinsic_Params", this); + layout_left->addWidget(group_left_top); + QFormLayout* layout_left_top = new QFormLayout(); + group_left_top->setLayout(layout_left_top); + + target_type_ = new QComboBox(); + connect(target_type_, SIGNAL(activated(const QString&)), this, SLOT(targetTypeComboboxChanged(const QString&))); + layout_left_top->addRow("Target Type", target_type_); + + dictionary_id_ = new QComboBox(); + layout_left_top->addRow("Dictionary", dictionary_id_); + + target_params_.clear(); + target_params_.insert(std::make_pair("markers_x", new QLineEdit())); + target_params_.insert(std::make_pair("markers_y", new QLineEdit())); + target_params_.insert(std::make_pair("marker_size", new QLineEdit())); + target_params_.insert(std::make_pair("marker_dist", new QLineEdit())); + target_params_.insert(std::make_pair("marker_border", new QLineEdit())); + + target_params_["markers_x"]->setText(QString("4")); + target_params_["markers_x"]->setValidator(new QIntValidator(1, 50)); + layout_left_top->addRow("Num_Markers_X", target_params_["markers_x"]); + + target_params_["markers_y"]->setText(QString("5")); + target_params_["markers_y"]->setValidator(new QIntValidator(1, 50)); + layout_left_top->addRow("Num_Markers_Y", target_params_["markers_y"]); + + target_params_["marker_size"]->setText(QString("200")); + target_params_["marker_size"]->setValidator(new QIntValidator(100, 1000)); + layout_left_top->addRow("Marker_Size_(pixels)", target_params_["marker_size"]); + + target_params_["marker_dist"]->setText(QString("20")); + target_params_["marker_dist"]->setValidator(new QIntValidator(10, 200)); + layout_left_top->addRow("Marker_Dist_(pixels)", target_params_["marker_dist"]); + + target_params_["marker_border"]->setText(QString("1")); + target_params_["marker_border"]->setValidator(new QIntValidator(1, 4)); + layout_left_top->addRow("Marker_Border_(bits)", target_params_["marker_border"]); + + // Target 3D pose recognition area + QGroupBox* group_left_bottom = new QGroupBox("Target_Pose_Recognition", this); + layout_left->addWidget(group_left_bottom); + QFormLayout* layout_left_bottom = new QFormLayout(); + group_left_bottom->setLayout(layout_left_bottom); + + ros_topics_.insert(std::make_pair("image_topic", new RosTopicComboBox(this))); + ros_topics_["image_topic"]->addMsgsFilterType("sensor_msgs/Image"); + layout_left_bottom->addRow("Image Topic", ros_topics_["image_topic"]); + connect(ros_topics_["image_topic"], SIGNAL(activated(const QString&)), this, + SLOT(imageTopicComboboxChanged(const QString&))); + + ros_topics_.insert(std::make_pair("camera_info_topic", new RosTopicComboBox(this))); + ros_topics_["camera_info_topic"]->addMsgsFilterType("sensor_msgs/CameraInfo"); + layout_left_bottom->addRow("CameraInfo Topic", ros_topics_["camera_info_topic"]); + connect(ros_topics_["camera_info_topic"], SIGNAL(activated(const QString&)), this, + SLOT(cameraInfoComboBoxChanged(const QString&))); + + target_real_dims_.insert(std::make_pair("marker_size_real", new QLineEdit())); + target_real_dims_.insert(std::make_pair("marker_dist_real", new QLineEdit())); + + target_real_dims_["marker_size_real"]->setText("0.0256"); + target_real_dims_["marker_size_real"]->setValidator(new QDoubleValidator(0, 2, 4)); + layout_left_bottom->addRow("Marker Size (m)", target_real_dims_["marker_size_real"]); + + target_real_dims_["marker_dist_real"]->setText("0.0066"); + target_real_dims_["marker_dist_real"]->setValidator(new QDoubleValidator(0, 2, 4)); + layout_left_bottom->addRow("Marker Dist (m)", target_real_dims_["marker_dist_real"]); + + // Target image dislay, create and save area + QGroupBox* group_right = new QGroupBox("Target_Create_Save", this); + group_right->setMinimumWidth(330); + layout->addWidget(group_right); + QVBoxLayout* layout_right = new QVBoxLayout(); + group_right->setLayout(layout_right); + + target_display_label_ = new QLabel(); + target_display_label_->setAlignment(Qt::AlignHCenter); + layout_right->addWidget(target_display_label_); + + QPushButton* create_target_btn = new QPushButton("Create Target"); + layout_right->addWidget(create_target_btn); + connect(create_target_btn, SIGNAL(clicked(bool)), this, SLOT(createTargetImageBtnClicked(bool))); + + QPushButton* save_target_btn = new QPushButton("Save Target"); + layout_right->addWidget(save_target_btn); + connect(save_target_btn, SIGNAL(clicked(bool)), this, SLOT(saveTargetImageBtnClicked(bool))); + + // Load target availible plugins + if (loadTargetPlugin() && createTargetInstance(target_type_->currentText().toStdString())) + fillDictionaryIds(); + + // Initialize image publisher + image_pub_ = it_.advertise("/handeye_calibration/target_detection", 1); + + // Initialize camera info dada + camera_info_.reset(new sensor_msgs::CameraInfo()); + + // Register custom types + qRegisterMetaType(); + qRegisterMetaType(); +} + +void TargetTabWidget::loadWidget(const rviz::Config& config) +{ + if (target_type_->count() > 0) + { + QString type; + if (config.mapGetString("target_type", &type) && target_type_->findText(type, Qt::MatchCaseSensitive) != -1) + target_type_->setCurrentText(type); + + createTargetInstance(target_type_->currentText().toStdString()); + + QString dict_name; + config.mapGetString("dictionary", &dict_name); + fillDictionaryIds(dict_name.toStdString()); + } + + int param_int; + for (const std::pair& param : target_params_) + if (config.mapGetInt(param.first.c_str(), ¶m_int)) + param.second->setText(std::to_string(param_int).c_str()); + + for (const std::pair& topic : ros_topics_) + { + QString topic_name; + if (config.mapGetString(topic.first.c_str(), &topic_name)) + { + if (topic.second->hasTopic(topic_name)) + { + topic.second->setCurrentText(topic_name); + try + { + if (!topic.first.compare("image_topic")) + { + image_sub_.shutdown(); + image_sub_ = it_.subscribe(topic_name.toStdString(), 1, &TargetTabWidget::imageCallback, this); + } + + if (!topic.first.compare("camera_info_topic")) + { + camerainfo_sub_.shutdown(); + camerainfo_sub_ = nh_.subscribe(topic_name.toStdString(), 1, &TargetTabWidget::cameraInfoCallback, this); + } + } + catch (const image_transport::TransportLoadException& e) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Subscribe to " << topic_name.toStdString() << " fail: " << e.what()); + } + } + } + } + + for (const std::pair& param : target_real_dims_) + { + float param_double; + if (config.mapGetFloat(param.first.c_str(), ¶m_double)) + param.second->setText(std::to_string(param_double).c_str()); + } +} + +void TargetTabWidget::saveWidget(rviz::Config& config) +{ + config.mapSetValue("target_type", target_type_->currentText()); + config.mapSetValue("dictionary", dictionary_id_->currentText()); + for (const std::pair& param : target_params_) + config.mapSetValue(param.first.c_str(), param.second->text().toInt()); + + for (const std::pair& topic : ros_topics_) + config.mapSetValue(topic.first.c_str(), topic.second->currentText()); + + for (const std::pair& param : target_real_dims_) + config.mapSetValue(param.first.c_str(), param.second->text().toDouble()); +} + +bool TargetTabWidget::loadTargetPlugin() +{ + if (!target_plugins_loader_) + { + try + { + target_plugins_loader_.reset(new pluginlib::ClassLoader( + "moveit_calibration_plugins", "moveit_handeye_calibration::HandEyeTargetBase")); + } + catch (pluginlib::PluginlibException& ex) + { + QMessageBox::warning(this, tr("Exception while creating handeye target plugin loader "), tr(ex.what())); + return false; + } + } + + // Get target classes + const std::vector& classes = target_plugins_loader_->getDeclaredClasses(); + + target_type_->clear(); + if (classes.empty()) + { + QMessageBox::warning(this, tr("Missing target plugins"), "No MoveIt handeye calibration target plugin found."); + return false; + } + + for (const std::string& it : classes) + target_type_->addItem(tr(it.c_str())); + + return true; +} + +bool TargetTabWidget::createTargetInstance(const std::string& plugin_name) +{ + if (plugin_name.empty()) + return false; + + try + { + target_ = target_plugins_loader_->createUniqueInstance(plugin_name); + target_->initialize(target_params_["markers_x"]->text().toInt(), target_params_["markers_y"]->text().toInt(), + target_params_["marker_size"]->text().toInt(), target_params_["marker_dist"]->text().toInt(), + target_params_["marker_border"]->text().toInt(), dictionary_id_->currentText().toStdString(), + target_real_dims_["marker_size_real"]->text().toDouble(), + target_real_dims_["marker_dist_real"]->text().toDouble()); + } + catch (pluginlib::PluginlibException& ex) + { + QMessageBox::warning(this, tr("Exception while loading a handeye target plugin"), tr(ex.what())); + target_ = nullptr; + return false; + } + + return true; +} + +void TargetTabWidget::fillDictionaryIds(std::string id) +{ + if (target_) + { + // Get dictionary ids + const std::vector& ids = target_->getDictionaryIds(); + dictionary_id_->clear(); + for (const std::string& id : ids) + { + dictionary_id_->addItem(tr(id.c_str())); + } + + // Check if 'id' exists in the list + if (!id.empty()) + { + auto it = std::find(ids.begin(), ids.end(), id); + if (it != ids.end()) + dictionary_id_->setCurrentText(QString(id.c_str())); + } + } +} + +void TargetTabWidget::imageCallback(const sensor_msgs::ImageConstPtr& msg) +{ + targetParamsSet(); + + // Depth image format `16UC1` cannot be converted to `MONO8` + if (msg->encoding == "16UC1") + return; + + std::string frame_id = msg->header.frame_id; + if (!frame_id.empty()) + { + if (optical_frame_.compare(frame_id)) + { + optical_frame_ = frame_id; + Q_EMIT opticalFrameChanged(optical_frame_); + } + } + else + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Image msg has empty frame_id."); + return; + } + + if (msg->data.empty()) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Image msg has empty data."); + return; + } + + cv_bridge::CvImagePtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8); + + sensor_msgs::ImagePtr pub_msg; + if (target_ && target_->detectTargetPose(cv_ptr->image)) + { + pub_msg = cv_bridge::CvImage(std_msgs::Header(), "rgb8", cv_ptr->image).toImageMsg(); + + geometry_msgs::TransformStamped tf2_msg = target_->getTransformStamped(optical_frame_); + tf_pub_.sendTransform(tf2_msg); + } + else + { + pub_msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", cv_ptr->image).toImageMsg(); + } + image_pub_.publish(pub_msg); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "cv_bridge exception: " << e.what()); + } + catch (cv::Exception& e) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "cv exception: " << e.what()); + } +} + +void TargetTabWidget::cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& msg) +{ + if (target_) + { + if (msg->height > 0 && msg->width > 0 && !msg->K.empty() && !msg->D.empty()) + { + if (msg->K != camera_info_->K || msg->P != camera_info_->P) + { + ROS_DEBUG("Received camera info."); + camera_info_->header = msg->header; + camera_info_->height = msg->height; + camera_info_->width = msg->width; + camera_info_->distortion_model = msg->distortion_model; + camera_info_->D = msg->D; + camera_info_->K = msg->K; + camera_info_->R = msg->R; + camera_info_->P = msg->P; + target_->setCameraIntrinsicParams(camera_info_); + Q_EMIT cameraInfoChanged(*camera_info_); + } + } + } +} + +void TargetTabWidget::targetTypeComboboxChanged(const QString& text) +{ + if (!text.isEmpty()) + { + if (createTargetInstance(text.toStdString())) + { + fillDictionaryIds(); + } + } +} + +void TargetTabWidget::createTargetImageBtnClicked(bool clicked) +{ + if (target_) + { + targetParamsSet(); + target_->createTargetImage(target_image_); + } + else + QMessageBox::warning(this, tr("Fail to create a target image."), "No available target plugin."); + + if (!target_image_.empty()) + { + // Show target image + QImage qimage(target_image_.data, target_image_.cols, target_image_.rows, QImage::Format_Grayscale8); + if (target_image_.cols > target_image_.rows) + qimage = qimage.scaledToWidth(320, Qt::SmoothTransformation); + else + qimage = qimage.scaledToHeight(260, Qt::SmoothTransformation); + target_display_label_->setPixmap(QPixmap::fromImage(qimage)); + } +} + +void TargetTabWidget::targetParamsSet(const QString& text) +{ + if (target_) + { + target_->setTargetIntrinsicParams( + target_params_["markers_x"]->text().toInt(), target_params_["markers_y"]->text().toInt(), + target_params_["marker_size"]->text().toInt(), target_params_["marker_dist"]->text().toInt(), + target_params_["marker_border"]->text().toInt(), dictionary_id_->currentText().toStdString()); + target_->setTargetDimension(target_real_dims_["marker_size_real"]->text().toDouble(), + target_real_dims_["marker_dist_real"]->text().toDouble()); + } +} + +void TargetTabWidget::saveTargetImageBtnClicked(bool clicked) +{ + if (target_image_.empty()) + { + QMessageBox::warning(this, tr("Unable to save image"), tr("Please create a target at first.")); + return; + } + + QString fileName = + QFileDialog::getSaveFileName(this, tr("Save Target Image"), "", tr("Target Image (*.png);;All Files (*)")); + + if (fileName.isEmpty()) + return; + + if (!fileName.endsWith(".png")) + fileName += ".png"; + + QFile file(fileName); + if (!file.open(QIODevice::WriteOnly)) + { + QMessageBox::warning(this, tr("Unable to open file"), file.errorString()); + return; + } + + if (!cv::imwrite(cv::String(fileName.toStdString()), target_image_)) + ROS_ERROR_STREAM_NAMED(LOGNAME, "Error OpenCV saving image."); +} + +void TargetTabWidget::imageTopicComboboxChanged(const QString& topic) +{ + image_sub_.shutdown(); + if (!topic.isNull() and !topic.isEmpty()) + { + try + { + image_sub_ = it_.subscribe(topic.toStdString(), 1, &TargetTabWidget::imageCallback, this); + } + catch (image_transport::TransportLoadException& e) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Subscribe to image topic: " << topic.toStdString() << " failed. " << e.what()); + } + } +} + +void TargetTabWidget::cameraInfoComboBoxChanged(const QString& topic) +{ + camerainfo_sub_.shutdown(); + if (!topic.isNull() and !topic.isEmpty()) + { + try + { + camerainfo_sub_ = nh_.subscribe(topic.toStdString(), 1, &TargetTabWidget::cameraInfoCallback, this); + } + catch (ros::Exception& e) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Subscribe to camera info topic: " << topic.toStdString() << " failed. " + << e.what()); + } + } +} + +} // namedist moveit_rviz_plugin \ No newline at end of file From 4b4a11cd57c58a62eaf262345f7684568d8f2c98 Mon Sep 17 00:00:00 2001 From: 7929f1cfe06ad78d8b8c60a417e1b4a8de37050c Date: Sat, 6 Jun 2020 21:21:46 +0800 Subject: [PATCH 06/26] Add handeye calibration context configuration widget --- .../handeye_context_widget.h | 249 ++++++++ .../src/handeye_context_widget.cpp | 553 ++++++++++++++++++ 2 files changed, 802 insertions(+) create mode 100644 moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_context_widget.h create mode 100644 moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_context_widget.cpp diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_context_widget.h b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_context_widget.h new file mode 100644 index 0000000..f05e112 --- /dev/null +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_context_widget.h @@ -0,0 +1,249 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Intel Corporation. + * 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 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: Yu Yan */ + +#ifndef MOVEIT_HANDEYE_CALIBRATION_RVIZ_PLUGIN_HANDEYE_CONTEXT_WIDGET_ +#define MOVEIT_HANDEYE_CALIBRATION_RVIZ_PLUGIN_HANDEYE_CONTEXT_WIDGET_ + +// qt +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ros +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef Q_MOC_RUN +#include +#include +#endif + +namespace rvt = rviz_visual_tools; +namespace mhc = moveit_handeye_calibration; + +namespace moveit_rviz_plugin +{ +enum FRAME_SOURCE +{ + ROBOT_FRAME = 0, + CAMERA_FRAME = 1, + ENVIRONMENT_FRAME = 2 +}; + +// ************************************************** +// Custom QComboBox for frame name +// ************************************************** +class TFFrameNameComboBox : public QComboBox +{ + Q_OBJECT +public: + TFFrameNameComboBox(FRAME_SOURCE source = ROBOT_FRAME, QWidget* parent = 0) : QComboBox(parent), frame_source_(source) + { + robot_model_loader_.reset(new robot_model_loader::RobotModelLoader("robot_description")); + frame_manager_.reset(new rviz::FrameManager()); + } + + ~TFFrameNameComboBox() + { + robot_model_loader_.reset(); + } + + bool hasFrame(const std::string& frame_name); + +protected: + void mousePressEvent(QMouseEvent* event); + +private: + FRAME_SOURCE frame_source_; + std::unique_ptr frame_manager_; + robot_model_loader::RobotModelLoaderConstPtr robot_model_loader_; +}; + +// ************************************************** +// Custom slider class +// ************************************************** +class SliderWidget : public QWidget +{ + Q_OBJECT + +public: + SliderWidget(QWidget* parent, std::string name, double min, double max); + + ~SliderWidget() override = default; + + double getValue(); + + void setValue(double value); + + QLabel* label_; + QSlider* slider_; + QLineEdit* edit_; + +private Q_SLOTS: + + // Called when the slider is changed + void changeValue(int value); + + // Called when the edit box is changed + void changeSlider(); + +Q_SIGNALS: + + // Indicate value when slider widget changed + void valueChanged(double value); + +private: + // Max & min position + double max_position_; + double min_position_; +}; + +class ContextTabWidget : public QWidget +{ + Q_OBJECT +public: + explicit ContextTabWidget(QWidget* parent = Q_NULLPTR); + ~ContextTabWidget() + { + camera_info_.reset(); + visual_tools_.reset(); + tf_tools_.reset(); + } + + void loadWidget(const rviz::Config& config); + void saveWidget(rviz::Config& config); + void setTFTool(rviz_visual_tools::TFVisualToolsPtr& tf_pub); + + void updateAllMarkers(); + + void updateFOVPose(); + + static shape_msgs::Mesh getCameraFOVMesh(const sensor_msgs::CameraInfo& camera_info, double maxdist); + + visualization_msgs::Marker getCameraFOVMarker(const Eigen::Isometry3d& pose, const shape_msgs::Mesh& mesh, + rvt::colors color, double alpha, std::string frame_id); + + visualization_msgs::Marker getCameraFOVMarker(const geometry_msgs::Pose& pose, const shape_msgs::Mesh& mesh, + rvt::colors color, double alpha, std::string frame_id); + + void setCameraPose(double tx, double ty, double tz, double rx, double ry, double rz); + +public Q_SLOTS: + + void setCameraInfo(sensor_msgs::CameraInfo camera_info); + + void setOpticalFrame(const std::string& frame_id); + + void updateCameraPose(double tx, double ty, double tz, double rx, double ry, double rz); + +private Q_SLOTS: + + // Called when the sensor_mount_type_ changed + void updateSensorMountType(int index); + + // Called when the TFFrameNameComboBox changed + void updateFrameName(int index); + + // Called when the slider of initial camera pose guess changed + void updateCameraMarkerPose(double value); + + // Called when the fov_on_off_ button toggled + void fovOnOffBtnToggled(bool checked); + +Q_SIGNALS: + + void sensorMountTypeChanged(int index); + + void frameNameChanged(std::map names); + +private: + // ************************************************************** + // Qt components + // ************************************************************** + + // Calibration algorithm, sensor mount type area + QComboBox* sensor_mount_type_; + + // Frame selection area + std::map frames_; + + // FOV setting area + QRadioButton* fov_on_off_; + SliderWidget* fov_alpha_; + + // Initial camera pose + std::map guess_pose_; + + // ************************************************************** + // Variables + // ************************************************************** + + sensor_msgs::CameraInfoPtr camera_info_; + + // Transform from camera to robot base or end-effector + Eigen::Isometry3d camera_pose_; + + std::string optical_frame_; + + // Transform from camera to fov + Eigen::Isometry3d fov_pose_; + + // ************************************************************** + // Ros components + // ************************************************************** + + moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; + rviz_visual_tools::TFVisualToolsPtr tf_tools_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; +}; + +} // namespace moveit_rviz_plugin + +#endif \ No newline at end of file diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_context_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_context_widget.cpp new file mode 100644 index 0000000..e0081f3 --- /dev/null +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_context_widget.cpp @@ -0,0 +1,553 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Intel Corporation. + * 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 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: Yu Yan */ + +#include +#include + +namespace moveit_rviz_plugin +{ +const std::string LOGNAME = "handeye_context_widget"; + +void TFFrameNameComboBox::mousePressEvent(QMouseEvent* event) +{ + std::vector names; + frame_manager_->update(); + frame_manager_->getTF2BufferPtr()->_getFrameStrings(names); + + clear(); + addItem(QString("")); + if (robot_model_loader_->getModel()) // Ensure that robot is brought up + { + const std::vector& robot_links = robot_model_loader_->getModel()->getLinkModelNames(); + for (const std::string& name : names) + { + auto it = std::find(robot_links.begin(), robot_links.end(), name); + size_t index = name.find("camera"); + + if (frame_source_ == ROBOT_FRAME) + if (it != robot_links.end()) + addItem(QString(name.c_str())); + + if (frame_source_ == CAMERA_FRAME) + if (index != std::string::npos) + addItem(QString(name.c_str())); + + if (frame_source_ == ENVIRONMENT_FRAME) + if (it == robot_links.end() && index == std::string::npos) + addItem(QString(name.c_str())); + } + } + showPopup(); +} + +bool TFFrameNameComboBox::hasFrame(const std::string& frame_name) +{ + std::vector names; + frame_manager_->update(); + frame_manager_->getTF2BufferPtr()->_getFrameStrings(names); + + auto it = std::find(names.begin(), names.end(), frame_name); + return it != names.end(); +} + +SliderWidget::SliderWidget(QWidget* parent, std::string name, double min, double max) + : QWidget(parent), min_position_(min), max_position_(max) +{ + QHBoxLayout* row = new QHBoxLayout(this); + row->setContentsMargins(0, 10, 0, 10); + + // QLabel init + label_ = new QLabel(QString(name.c_str()), this); + label_->setContentsMargins(0, 0, 0, 0); + row->addWidget(label_); + + // QSlider init + slider_ = new QSlider(Qt::Horizontal, this); + slider_->setSingleStep(100); + slider_->setPageStep(100); + slider_->setTickInterval(1000); + slider_->setContentsMargins(0, 0, 0, 0); + row->addWidget(slider_); + + slider_->setMaximum(max_position_ * 10000); + slider_->setMinimum(min_position_ * 10000); + + connect(slider_, SIGNAL(valueChanged(int)), this, SLOT(changeValue(int))); + + // QLineEdit init + edit_ = new QLineEdit(this); + edit_->setMinimumWidth(62); + edit_->setContentsMargins(0, 0, 0, 0); + connect(edit_, SIGNAL(editingFinished()), this, SLOT(changeSlider())); + row->addWidget(edit_); + + this->setLayout(row); +} + +double SliderWidget::getValue() +{ + return edit_->text().toDouble(); +} + +void SliderWidget::setValue(double value) +{ + if (min_position_ > value || value > max_position_) + { + value = (min_position_ > value) ? min_position_ : max_position_; + } + edit_->setText(QString("%1").arg(value, 0, 'f', 4)); + slider_->setSliderPosition(value * 10000); +} + +void SliderWidget::changeValue(int value) +{ + const double double_value = double(value) / 10000; + + // Set textbox + edit_->setText(QString("%1").arg(double_value, 0, 'f', 4)); + + // Send event to parent widget + Q_EMIT valueChanged(double_value); +} + +void SliderWidget::changeSlider() +{ + // Get joint value + double value = edit_->text().toDouble(); + + setValue(value); + + // Send event to parent widget + Q_EMIT valueChanged(value); +} + +ContextTabWidget::ContextTabWidget(QWidget* parent) : QWidget(parent), tf_listener_(tf_buffer_) +{ + // Context setting tab ---------------------------------------------------- + QHBoxLayout* layout = new QHBoxLayout(); + this->setLayout(layout); + QVBoxLayout* layout_left = new QVBoxLayout(); + layout->addLayout(layout_left); + QVBoxLayout* layout_right = new QVBoxLayout(); + layout->addLayout(layout_right); + + // Sensor mount type area and fov area + QGroupBox* group_left_top = new QGroupBox("General Setting", this); + layout_left->addWidget(group_left_top); + QFormLayout* layout_left_top = new QFormLayout(); + group_left_top->setLayout(layout_left_top); + + sensor_mount_type_ = new QComboBox(); + sensor_mount_type_->addItem("Eye-to-Hand"); + sensor_mount_type_->addItem("Eye-in-hand"); + layout_left_top->addRow("Sensor Mount Type", sensor_mount_type_); + connect(sensor_mount_type_, SIGNAL(activated(int)), this, SLOT(updateSensorMountType(int))); + + // Frame name selection area + QGroupBox* frame_group = new QGroupBox("Frames Selection", this); + layout_left->addWidget(frame_group); + QFormLayout* frame_layout = new QFormLayout(); + frame_group->setLayout(frame_layout); + + frames_.insert(std::make_pair("sensor", new TFFrameNameComboBox(CAMERA_FRAME))); + frame_layout->addRow("Sensor Frame:", frames_["sensor"]); + + frames_.insert(std::make_pair("object", new TFFrameNameComboBox(ENVIRONMENT_FRAME))); + frame_layout->addRow("Object Frame:", frames_["object"]); + + frames_.insert(std::make_pair("eef", new TFFrameNameComboBox(ROBOT_FRAME))); + frame_layout->addRow("End-Effector Frame:", frames_["eef"]); + + frames_.insert(std::make_pair("base", new TFFrameNameComboBox(ROBOT_FRAME))); + frame_layout->addRow("Robot Base Frame:", frames_["base"]); + + for (std::pair& frame : frames_) + connect(frame.second, SIGNAL(activated(int)), this, SLOT(updateFrameName(int))); + + // FOV area + QGroupBox* fov_group = new QGroupBox("FOV", this); + layout_left->addWidget(fov_group); + QFormLayout* fov_layout = new QFormLayout(); + fov_group->setLayout(fov_layout); + + fov_alpha_ = new SliderWidget(this, "Transparency", 0, 1); + fov_alpha_->setValue(0.5); + fov_layout->addRow(fov_alpha_); + connect(fov_alpha_, SIGNAL(valueChanged(double)), this, SLOT(updateCameraMarkerPose(double))); + + fov_on_off_ = new QRadioButton(); + fov_on_off_->setChecked(true); + fov_layout->addRow("ON/OFF", fov_on_off_); + connect(fov_on_off_, SIGNAL(toggled(bool)), this, SLOT(fovOnOffBtnToggled(bool))); + + // Camera Pose initial guess area + QGroupBox* pose_group = new QGroupBox("Camera Pose Inital Guess", this); + pose_group->setMinimumWidth(300); + layout_right->addWidget(pose_group); + QFormLayout* pose_layout = new QFormLayout(); + pose_group->setLayout(pose_layout); + + guess_pose_.insert(std::make_pair("Tx", new SliderWidget(this, "TranslX", -2.0, 2.0))); + pose_layout->addRow(guess_pose_["Tx"]); + + guess_pose_.insert(std::make_pair("Ty", new SliderWidget(this, "TranslY", -2.0, 2.0))); + pose_layout->addRow(guess_pose_["Ty"]); + + guess_pose_.insert(std::make_pair("Tz", new SliderWidget(this, "TranslZ", -2.0, 2.0))); + pose_layout->addRow(guess_pose_["Tz"]); + + guess_pose_.insert(std::make_pair("Rx", new SliderWidget(this, "RotateX", -M_PI, M_PI))); + pose_layout->addRow(guess_pose_["Rx"]); + + guess_pose_.insert(std::make_pair("Ry", new SliderWidget(this, "RotateY", -M_PI, M_PI))); + pose_layout->addRow(guess_pose_["Ry"]); + + guess_pose_.insert(std::make_pair("Rz", new SliderWidget(this, "RotateZ", -M_PI, M_PI))); + pose_layout->addRow(guess_pose_["Rz"]); + + for (std::pair& dim : guess_pose_) + { + dim.second->setValue(0); + connect(dim.second, SIGNAL(valueChanged(double)), this, SLOT(updateCameraMarkerPose(double))); + } + + // Variable Initialization + camera_pose_ = Eigen::Isometry3d::Identity(); + fov_pose_ = Eigen::Quaterniond(0.5, -0.5, 0.5, -0.5); + fov_pose_.translate(Eigen::Vector3d(0.0149, 0.0325, 0.0125)); + + camera_info_.reset(new sensor_msgs::CameraInfo()); + + visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools("world")); + visual_tools_->enableFrameLocking(true); + visual_tools_->setAlpha(1.0); + visual_tools_->setLifetime(0.0); + visual_tools_->trigger(); +} + +void ContextTabWidget::loadWidget(const rviz::Config& config) +{ + int index; + if (config.mapGetInt("sensor_mount_type", &index)) + sensor_mount_type_->setCurrentIndex(index); + + Q_EMIT sensorMountTypeChanged(index); + + for (std::pair& frame : frames_) + { + QString frame_name; + if (config.mapGetString(frame.first.c_str(), &frame_name)) + { + frame.second->clear(); + if (!frame_name.isEmpty() && frame.second->hasFrame(frame_name.toStdString())) + frame.second->addItem(frame_name); + } + } + + float alpha; + if (config.mapGetFloat("fov_transparent", &alpha)) + fov_alpha_->setValue(alpha); + + bool fov_enabled; + if (config.mapGetBool("fov_on_off", &fov_enabled)) + fov_on_off_->setChecked(fov_enabled); + + for (std::pair& dim : guess_pose_) + { + float value; + if (config.mapGetFloat(dim.first.c_str(), &value)) + dim.second->setValue(value); + } + updateAllMarkers(); + + std::map names; + for (std::pair& frame : frames_) + names.insert(std::make_pair(frame.first, frame.second->currentText().toStdString())); + + Q_EMIT frameNameChanged(names); +} + +void ContextTabWidget::saveWidget(rviz::Config& config) +{ + config.mapSetValue("sensor_mount_type", sensor_mount_type_->currentIndex()); + + for (std::pair& frame : frames_) + config.mapSetValue(frame.first.c_str(), frame.second->currentText()); + + config.mapSetValue("fov_transparent", fov_alpha_->getValue()); + config.mapSetValue("fov_on_off", fov_on_off_->isChecked()); + + for (std::pair& dim : guess_pose_) + config.mapSetValue(dim.first.c_str(), dim.second->getValue()); +} + +void ContextTabWidget::setTFTool(rviz_visual_tools::TFVisualToolsPtr& tf_pub) +{ + tf_tools_ = tf_pub; +} + +void ContextTabWidget::updateAllMarkers() +{ + if (visual_tools_ && tf_tools_) + { + visual_tools_->deleteAllMarkers(); + tf_tools_->clearAllTransforms(); + + QString from_frame(""); + mhc::SensorMountType setup = static_cast(sensor_mount_type_->currentIndex()); + + switch (setup) + { + case mhc::EYE_TO_HAND: + from_frame = frames_["base"]->currentText(); + break; + case mhc::EYE_IN_HAND: + from_frame = frames_["eef"]->currentText(); + break; + default: + ROS_ERROR_STREAM_NAMED(LOGNAME, "Error sensor mount type."); + break; + } + + if (!from_frame.isEmpty()) + { + for (std::pair frame : frames_) + { + // Publish selected frame axis + const std::string& frame_id = frame.second->currentText().toStdString(); + if (!frame_id.empty()) + { + visual_tools_->setBaseFrame(frame_id); + visual_tools_->setAlpha(1.0); + visual_tools_->publishAxisLabeled(Eigen::Isometry3d::Identity(), frame_id); + } + } + + // Publish camera and fov marker + QString to_frame = frames_["sensor"]->currentText(); + if (!to_frame.isEmpty()) + { + // // Get camera pose guess + setCameraPose(guess_pose_["Tx"]->getValue(), guess_pose_["Ty"]->getValue(), guess_pose_["Tz"]->getValue(), + guess_pose_["Rx"]->getValue(), guess_pose_["Ry"]->getValue(), guess_pose_["Rz"]->getValue()); + + // Publish new transform from robot base or end-effector to sensor frame + tf_tools_->publishTransform(camera_pose_, from_frame.toStdString(), to_frame.toStdString()); + + // Publish new FOV marker + shape_msgs::Mesh mesh = getCameraFOVMesh(*camera_info_, 1.5); + if (fov_on_off_->isChecked()) + { + visual_tools_->setBaseFrame(to_frame.toStdString()); + visual_tools_->setAlpha(fov_alpha_->getValue()); + visual_tools_->publishMesh(fov_pose_, mesh, rvt::YELLOW, 1.0, "fov", 1); + } + } + } + + visual_tools_->trigger(); + } + else + ROS_ERROR("Visual or TF tool is NULL."); +} + +void ContextTabWidget::updateFOVPose() +{ + QString sensor_frame = frames_["sensor"]->currentText(); + geometry_msgs::TransformStamped tf_msg; + if (!optical_frame_.empty() && !sensor_frame.isEmpty()) + { + try + { + // Get FOV pose W.R.T sensor frame + tf_msg = tf_buffer_.lookupTransform(sensor_frame.toStdString(), optical_frame_, ros::Time(0)); + fov_pose_ = tf2::transformToEigen(tf_msg); + ROS_DEBUG_STREAM_NAMED(LOGNAME, "FOV pose from '" << sensor_frame.toStdString() << "' to '" << optical_frame_ + << "' is:" + << "\nTranslation:\n" + << fov_pose_.translation() << "\nRotation:\n" + << fov_pose_.rotation()); + } + catch (tf2::TransformException& e) + { + ROS_WARN_STREAM("TF exception: " << e.what()); + } + } +} + +shape_msgs::Mesh ContextTabWidget::getCameraFOVMesh(const sensor_msgs::CameraInfo& camera_info, double max_dist) +{ + shape_msgs::Mesh mesh; + image_geometry::PinholeCameraModel camera_model; + camera_model.fromCameraInfo(camera_info); + double delta_x = camera_model.getDeltaX(camera_info.width / 2, max_dist); + double delta_y = camera_model.getDeltaY(camera_info.height / 2, max_dist); + + std::vector x_cords = { -delta_x, delta_x }; + std::vector y_cords = { -delta_y, delta_y }; + + // Get corners + mesh.vertices.clear(); + // Add the first corner at origin of the optical frame + mesh.vertices.push_back(geometry_msgs::Point()); + + // Add the four corners at bottom + for (const double& x_it : x_cords) + for (const double& y_it : y_cords) + { + geometry_msgs::Point vertex; + // Check in case camera info is not valid + if (std::isfinite(x_it) && std::isfinite(y_it) && std::isfinite(max_dist)) + { + vertex.x = x_it; + vertex.y = y_it; + vertex.z = max_dist; + } + mesh.vertices.push_back(vertex); + } + + // Get surface triangles + mesh.triangles.resize(4); + mesh.triangles[0].vertex_indices = { 0, 1, 2 }; + mesh.triangles[1].vertex_indices = { 0, 2, 4 }; + mesh.triangles[2].vertex_indices = { 0, 4, 3 }; + mesh.triangles[3].vertex_indices = { 0, 3, 1 }; + return mesh; +} + +visualization_msgs::Marker ContextTabWidget::getCameraFOVMarker(const Eigen::Isometry3d& pose, + const shape_msgs::Mesh& mesh, rvt::colors color, + double alpha, std::string frame_id) +{ + return getCameraFOVMarker(rvt::RvizVisualTools::convertPose(pose), mesh, color, alpha, frame_id); +} + +visualization_msgs::Marker ContextTabWidget::getCameraFOVMarker(const geometry_msgs::Pose& pose, + const shape_msgs::Mesh& mesh, rvt::colors color, + double alpha, std::string frame_id) +{ + visualization_msgs::Marker marker; + marker.header.frame_id = frame_id; + marker.ns = "camera_fov"; + marker.id = 0; + marker.type = visualization_msgs::Marker::TRIANGLE_LIST; + marker.action = visualization_msgs::Marker::ADD; + marker.lifetime = ros::Duration(0.0); + visual_tools_->setAlpha(alpha); + marker.color = visual_tools_->getColor(color); + marker.pose = pose; + marker.scale.x = 1.0; + marker.scale.y = 1.0; + marker.scale.z = 1.0; + + marker.points.clear(); + for (const shape_msgs::MeshTriangle& triangle : mesh.triangles) + for (const uint32_t& index : triangle.vertex_indices) + marker.points.push_back(mesh.vertices[index]); + + return marker; +} + +void ContextTabWidget::setCameraPose(double tx, double ty, double tz, double rx, double ry, double rz) +{ + camera_pose_.setIdentity(); + camera_pose_ = visual_tools_->convertFromXYZRPY(tx, ty, tz, rx, ry, rz, rviz_visual_tools::XYZ); +} + +void ContextTabWidget::setCameraInfo(sensor_msgs::CameraInfo camera_info) +{ + camera_info_->header = camera_info.header; + camera_info_->height = camera_info.height; + camera_info_->width = camera_info.width; + camera_info_->distortion_model = camera_info.distortion_model; + camera_info_->D = camera_info.D; + camera_info_->K = camera_info.K; + camera_info_->R = camera_info.R; + camera_info_->P = camera_info.P; + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Camera info changed: " << *camera_info_); +} + +void ContextTabWidget::setOpticalFrame(const std::string& frame_id) +{ + optical_frame_ = frame_id; + updateFOVPose(); +} + +void ContextTabWidget::updateCameraPose(double tx, double ty, double tz, double rx, double ry, double rz) +{ + // setCameraPose(tx, ty, tz, rx, ry, rz); + guess_pose_["Tx"]->setValue(tx); + guess_pose_["Ty"]->setValue(ty); + guess_pose_["Tz"]->setValue(tz); + guess_pose_["Rx"]->setValue(rx); + guess_pose_["Ry"]->setValue(ry); + guess_pose_["Rz"]->setValue(rz); + updateCameraMarkerPose(0); +} + +void ContextTabWidget::updateSensorMountType(int index) +{ + for (std::pair dim : guess_pose_) + dim.second->setValue(0); + + updateAllMarkers(); + + Q_EMIT sensorMountTypeChanged(index); +} + +void ContextTabWidget::updateFrameName(int index) +{ + updateAllMarkers(); + updateFOVPose(); + + std::map names; + for (std::pair& frame : frames_) + names.insert(std::make_pair(frame.first, frame.second->currentText().toStdString())); + + Q_EMIT frameNameChanged(names); +} + +void ContextTabWidget::updateCameraMarkerPose(double value) +{ + updateAllMarkers(); +} + +void ContextTabWidget::fovOnOffBtnToggled(bool checked) +{ + updateAllMarkers(); +} + +} // namgespace moveit_rviz_plugin \ No newline at end of file From 5ddf1fa8e0a92f1d5ffcd2204f259f43120f15a1 Mon Sep 17 00:00:00 2001 From: 7929f1cfe06ad78d8b8c60a417e1b4a8de37050c Date: Sat, 6 Jun 2020 21:22:24 +0800 Subject: [PATCH 07/26] Add handeye calibration control operation widget --- .../handeye_control_widget.h | 242 +++++ .../src/handeye_control_widget.cpp | 828 ++++++++++++++++++ 2 files changed, 1070 insertions(+) create mode 100644 moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h create mode 100644 moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h new file mode 100644 index 0000000..fc6794e --- /dev/null +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h @@ -0,0 +1,242 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Intel Corporation. + * 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 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 THEsensorPoseUpdate + * 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: Yu Yan */ + +#ifndef MOVEIT_HANDEYE_CALIBRATION_RVIZ_PLUGIN_HANDEYE_CALIBRATE_WIDGET_ +#define MOVEIT_HANDEYE_CALIBRATION_RVIZ_PLUGIN_HANDEYE_CALIBRATE_WIDGET_ + +// qt +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ros +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef Q_MOC_RUN +#include +#include +#endif + +#include + +namespace mhc = moveit_handeye_calibration; + +namespace moveit_rviz_plugin +{ +class ProgressBarWidget : public QWidget +{ + Q_OBJECT + +public: + ProgressBarWidget(QWidget* parent, int min = 0, int max = 0, int value = 0); + + ~ProgressBarWidget() override = default; + + void setMax(int value); + void setMin(int value); + void setValue(int value); + int getValue(); + + QLabel* name_label_; + QLabel* value_label_; + QLabel* max_label_; + QProgressBar* bar_; +}; + +class ControlTabWidget : public QWidget +{ + Q_OBJECT +public: + explicit ControlTabWidget(QWidget* parent = Q_NULLPTR); + ~ControlTabWidget() + { + tf_tools_.reset(); + tf_buffer_.reset(); + solver_.reset(); + solver_plugins_loader_.reset(); + move_group_.reset(); + planning_scene_monitor_.reset(); + } + + void loadWidget(const rviz::Config& config); + void saveWidget(rviz::Config& config); + + void setTFTool(rviz_visual_tools::TFVisualToolsPtr& tf_pub); + + void addPoseSampleToTreeView(const geometry_msgs::TransformStamped& cTo, const geometry_msgs::TransformStamped& bTe, + int id); + + bool loadSolverPlugin(std::vector& plugins); + + bool createSolverInstance(const std::string& plugin_name); + + void fillSolverTypes(const std::vector& plugins); + + std::string parseSolverName(const std::string& solver_name, char delimiter); + + bool takeTranformSamples(); + + bool solveCameraRobotPose(); + + bool frameNamesEmpty(); + + bool checkJointStates(); + + void computePlan(); + + void computeExecution(); + +Q_SIGNALS: + + void sensorPoseUpdate(double x, double y, double z, double rx, double ry, double rz); + +public Q_SLOTS: + + void UpdateSensorMountType(int index); + + void updateFrameNames(std::map names); + +private Q_SLOTS: + + void takeSampleBtnClicked(bool clicked); + + void resetSampleBtnClicked(bool clicked); + + void saveCameraPoseBtnClicked(bool clicked); + + void planningGroupNameChanged(const QString& text); + + void saveJointStateBtnClicked(bool clicked); + + void loadJointStateBtnClicked(bool clicked); + + void autoPlanBtnClicked(bool clicked); + + void autoExecuteBtnClicked(bool clicked); + + void autoSkipBtnClicked(bool clicked); + + void planFinished(); + + void executeFinished(); + +private: + // ************************************************************** + // Qt components + // ************************************************************** + + QTreeView* sample_tree_view_; + QStandardItemModel* tree_view_model_; + + QComboBox* calibration_solver_; + + // Load & save pose samples and joint goals + QPushButton* save_joint_state_btn_; + QPushButton* load_joint_state_btn_; + QPushButton* save_camera_pose_btn_; + + // Manual calibration + QPushButton* take_sample_btn_; + QPushButton* reset_sample_btn_; + + // Auto calibration + QComboBox* group_name_; + QPushButton* auto_plan_btn_; + QPushButton* auto_execute_btn_; + QPushButton* auto_skip_btn_; + + // Progress of finished joint states for auto calibration + ProgressBarWidget* auto_progress_; + + QFutureWatcher* plan_watcher_; + QFutureWatcher* execution_watcher_; + + // ************************************************************** + // Variables + // ************************************************************** + + mhc::SensorMountType sensor_mount_type_; + std::map frame_names_; + // Transform samples + std::vector effector_wrt_world_; + std::vector object_wrt_sensor_; + std::string from_frame_tag_; + Eigen::Isometry3d camera_robot_pose_; + std::vector> joint_states_; + std::vector joint_names_; + bool auto_started_; + bool planning_res_; + + // ************************************************************** + // Ros components + // ************************************************************** + + ros::NodeHandle nh_; + // ros::CallbackQueue callback_queue_; + // ros::AsyncSpinner spinner_; + std::shared_ptr tf_buffer_; + tf2_ros::TransformListener tf_listener_; + rviz_visual_tools::TFVisualToolsPtr tf_tools_; + std::unique_ptr> solver_plugins_loader_; + pluginlib::UniquePtr solver_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + moveit::planning_interface::MoveGroupInterfacePtr move_group_; + moveit::planning_interface::MoveGroupInterface::PlanPtr current_plan_; +}; + +} // namespace moveit_rviz_plugin +#endif \ No newline at end of file diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp new file mode 100644 index 0000000..21f60ff --- /dev/null +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp @@ -0,0 +1,828 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Intel Corporation. + * 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 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: Yu Yan */ + +#include + +namespace moveit_rviz_plugin +{ +const std::string LOGNAME = "handeye_control_widget"; + +ProgressBarWidget::ProgressBarWidget(QWidget* parent, int min, int max, int value) : QWidget(parent) +{ + QHBoxLayout* row = new QHBoxLayout(this); + row->setContentsMargins(0, 10, 0, 10); + + // QLabel init + name_label_ = new QLabel("joint_state:", this); + name_label_->setContentsMargins(0, 0, 0, 0); + row->addWidget(name_label_); + + value_label_ = new QLabel(QString::number(value), this); + value_label_->setContentsMargins(0, 0, 0, 0); + row->addWidget(value_label_); + + // QProgressBar init + bar_ = new QProgressBar(this); + bar_->setTextVisible(true); + bar_->setMinimum(min); + bar_->setMaximum(max); + bar_->setValue(value); + bar_->setContentsMargins(0, 0, 0, 0); + row->addWidget(bar_); + + max_label_ = new QLabel(QString::number(max), this); + max_label_->setContentsMargins(0, 0, 0, 0); + row->addWidget(max_label_); + + this->setLayout(row); +} + +void ProgressBarWidget::setMax(int value) +{ + bar_->setMaximum(value); + max_label_->setText(QString::number(value)); +} + +void ProgressBarWidget::setMin(int value) +{ + bar_->setMinimum(value); +} + +void ProgressBarWidget::setValue(int value) +{ + bar_->setValue(value); + value_label_->setText(QString::number(value)); +} + +int ProgressBarWidget::getValue() +{ + return bar_->value(); +} + +ControlTabWidget::ControlTabWidget(QWidget* parent) + : QWidget(parent) + , tf_buffer_(new tf2_ros::Buffer()) + , tf_listener_(*tf_buffer_) + , sensor_mount_type_(mhc::EYE_TO_HAND) + , solver_plugins_loader_(nullptr) + , solver_(nullptr) + , move_group_(nullptr) + , camera_robot_pose_(Eigen::Isometry3d::Identity()) + , auto_started_(false) + , planning_res_(false) +// spinner_(0, &callback_queue_) +{ + QVBoxLayout* layout = new QVBoxLayout(); + this->setLayout(layout); + + QHBoxLayout* calib_layout = new QHBoxLayout(); + layout->addLayout(calib_layout); + + // Calibration progress + auto_progress_ = new ProgressBarWidget(this); + layout->addWidget(auto_progress_); + + // Pose sample tree view area + QGroupBox* sample_group = new QGroupBox("Pose_sample"); + sample_group->setMinimumWidth(280); + calib_layout->addWidget(sample_group); + QVBoxLayout* sample_layout = new QVBoxLayout(); + sample_group->setLayout(sample_layout); + + sample_tree_view_ = new QTreeView(this); + sample_tree_view_->setAutoScroll(true); + sample_tree_view_->setAlternatingRowColors(true); + tree_view_model_ = new QStandardItemModel(sample_tree_view_); + sample_tree_view_->setModel(tree_view_model_); + sample_tree_view_->setHeaderHidden(true); + sample_tree_view_->setIndentation(10); + sample_layout->addWidget(sample_tree_view_); + + // Setting area + QVBoxLayout* layout_right = new QVBoxLayout(); + calib_layout->addLayout(layout_right); + + QGroupBox* setting_group = new QGroupBox("Setting"); + layout_right->addWidget(setting_group); + QFormLayout* setting_layout = new QFormLayout(); + setting_group->setLayout(setting_layout); + + calibration_solver_ = new QComboBox(); + setting_layout->addRow("AX=XB Solver", calibration_solver_); + + group_name_ = new QComboBox(); + connect(group_name_, SIGNAL(activated(const QString&)), this, SLOT(planningGroupNameChanged(const QString&))); + setting_layout->addRow("Planning Group", group_name_); + + load_joint_state_btn_ = new QPushButton("Load Joint States"); + connect(load_joint_state_btn_, SIGNAL(clicked(bool)), this, SLOT(loadJointStateBtnClicked(bool))); + setting_layout->addRow(load_joint_state_btn_); + + save_joint_state_btn_ = new QPushButton("Save Joint states"); + connect(save_joint_state_btn_, SIGNAL(clicked(bool)), this, SLOT(saveJointStateBtnClicked(bool))); + setting_layout->addRow(save_joint_state_btn_); + + save_camera_pose_btn_ = new QPushButton("Save Camera Pose"); + connect(save_camera_pose_btn_, SIGNAL(clicked(bool)), this, SLOT(saveCameraPoseBtnClicked(bool))); + setting_layout->addRow(save_camera_pose_btn_); + + // Manual calibration area + QGroupBox* manual_cal_group = new QGroupBox("Manual Calibration"); + layout_right->addWidget(manual_cal_group); + QHBoxLayout* control_cal_layout = new QHBoxLayout(); + manual_cal_group->setLayout(control_cal_layout); + + take_sample_btn_ = new QPushButton("Take Sample"); + take_sample_btn_->setMinimumHeight(35); + connect(take_sample_btn_, SIGNAL(clicked(bool)), this, SLOT(takeSampleBtnClicked(bool))); + control_cal_layout->addWidget(take_sample_btn_); + + reset_sample_btn_ = new QPushButton("Reset Sample"); + reset_sample_btn_->setMinimumHeight(35); + connect(reset_sample_btn_, SIGNAL(clicked(bool)), this, SLOT(resetSampleBtnClicked(bool))); + control_cal_layout->addWidget(reset_sample_btn_); + + // Auto calibration area + QGroupBox* auto_cal_group = new QGroupBox("Auto Calibration"); + layout_right->addWidget(auto_cal_group); + QVBoxLayout* auto_cal_layout = new QVBoxLayout(); + auto_cal_group->setLayout(auto_cal_layout); + + QHBoxLayout* auto_btns_layout = new QHBoxLayout(); + auto_cal_layout->addLayout(auto_btns_layout); + auto_plan_btn_ = new QPushButton("Plan"); + auto_plan_btn_->setMinimumHeight(35); + auto_plan_btn_->setToolTip("Start or resume auto calibration process"); + connect(auto_plan_btn_, SIGNAL(clicked(bool)), this, SLOT(autoPlanBtnClicked(bool))); + auto_btns_layout->addWidget(auto_plan_btn_); + + auto_execute_btn_ = new QPushButton("Execute"); + auto_execute_btn_->setMinimumHeight(35); + auto_execute_btn_->setToolTip("Pause the auto calibration process"); + connect(auto_execute_btn_, SIGNAL(clicked(bool)), this, SLOT(autoExecuteBtnClicked(bool))); + auto_btns_layout->addWidget(auto_execute_btn_); + + auto_skip_btn_ = new QPushButton("Skip"); + auto_skip_btn_->setMinimumHeight(35); + auto_skip_btn_->setToolTip("Skip the current robot state target"); + connect(auto_skip_btn_, SIGNAL(clicked(bool)), this, SLOT(autoSkipBtnClicked(bool))); + auto_btns_layout->addWidget(auto_skip_btn_); + + // Initialize handeye solver plugins + std::vector plugins; + if (loadSolverPlugin(plugins)) + fillSolverTypes(plugins); + + // Fill in available planning group names + planning_scene_monitor_.reset( + new planning_scene_monitor::PlanningSceneMonitor("robot_description", tf_buffer_, "planning_scene_monitor")); + if (planning_scene_monitor_) + { + planning_scene_monitor_->startSceneMonitor("move_group/monitored_planning_scene"); + std::string service_name = planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE; + if (planning_scene_monitor_->requestPlanningSceneState(service_name)) + { + const robot_model::RobotModelConstPtr& kmodel = planning_scene_monitor_->getRobotModel(); + for (const std::string& group_name : kmodel->getJointModelGroupNames()) + group_name_->addItem(group_name.c_str()); + if (!group_name_->currentText().isEmpty()) + try + { + moveit::planning_interface::MoveGroupInterface::Options opt(group_name_->currentText().toStdString()); + opt.node_handle_ = nh_; + move_group_.reset( + new moveit::planning_interface::MoveGroupInterface(opt, tf_buffer_, ros::WallDuration(30, 0))); + } + catch (std::exception& ex) + { + ROS_ERROR_NAMED(LOGNAME, "%s", ex.what()); + } + } + } + + // Set plan and execution watcher + plan_watcher_ = new QFutureWatcher(this); + connect(plan_watcher_, &QFutureWatcher::finished, this, &ControlTabWidget::planFinished); + + execution_watcher_ = new QFutureWatcher(this); + connect(execution_watcher_, &QFutureWatcher::finished, this, &ControlTabWidget::executeFinished); +} + +void ControlTabWidget::loadWidget(const rviz::Config& config) +{ + QString group_name; + config.mapGetString("group", &group_name); + if (!group_name.isEmpty() && planning_scene_monitor_) + { + const std::vector groups = planning_scene_monitor_->getRobotModel()->getJointModelGroupNames(); + std::vector::const_iterator it = std::find(groups.begin(), groups.end(), group_name.toStdString()); + if (it != groups.end()) + { + group_name_->setCurrentText(group_name); + Q_EMIT group_name_->activated(group_name); + } + } +} + +void ControlTabWidget::saveWidget(rviz::Config& config) +{ + config.mapSetValue("solver", calibration_solver_->currentText()); + config.mapSetValue("group", group_name_->currentText()); +} + +bool ControlTabWidget::loadSolverPlugin(std::vector& plugins) +{ + if (!solver_plugins_loader_) + { + try + { + solver_plugins_loader_.reset(new pluginlib::ClassLoader( + "moveit_calibration_plugins", "moveit_handeye_calibration::HandEyeSolverBase")); + } + catch (pluginlib::PluginlibException& ex) + { + QMessageBox::warning(this, tr("Exception while creating handeye solver plugin loader "), tr(ex.what())); + return false; + } + } + + // Get available plugins + plugins = solver_plugins_loader_->getDeclaredClasses(); + return !plugins.empty(); +} + +bool ControlTabWidget::createSolverInstance(const std::string& plugin_name) +{ + try + { + solver_ = solver_plugins_loader_->createUniqueInstance(plugin_name); + solver_->initialize(); + } + catch (pluginlib::PluginlibException& ex) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Exception while loading handeye solver plugin: " << plugin_name << ex.what()); + solver_ = nullptr; + } + + return solver_ != nullptr; +} + +void ControlTabWidget::fillSolverTypes(const std::vector& plugins) +{ + for (const std::string& plugin : plugins) + if (!plugin.empty() && createSolverInstance(plugin)) + { + const std::vector& solvers = solver_->getSolverNames(); + for (const std::string& solver : solvers) + { + std::string solver_name = plugin + "/" + solver; // solver name format is "plugin_name/solver_name" + calibration_solver_->addItem(tr(solver_name.c_str())); + } + } +} + +std::string ControlTabWidget::parseSolverName(const std::string& solver_name, char delimiter) +{ + std::vector tokens; + std::string token; + std::istringstream tokenStream(solver_name); + while (std::getline(tokenStream, token, delimiter)) + { + tokens.push_back(token); + } + return tokens.back(); +} + +bool ControlTabWidget::takeTranformSamples() +{ + // Store the pair of two tf transforms and calculate camera_robot pose + try + { + geometry_msgs::TransformStamped cTo; + geometry_msgs::TransformStamped bTe; + + // Get the transform of the object w.r.t the camera + cTo = tf_buffer_->lookupTransform(frame_names_["sensor"], frame_names_["object"], ros::Time(0)); + + // Get the transform of the end-effector w.r.t the robot base + bTe = tf_buffer_->lookupTransform(frame_names_["base"], frame_names_["eef"], ros::Time(0)); + + // save the pose samples + effector_wrt_world_.push_back(tf2::transformToEigen(bTe)); + object_wrt_sensor_.push_back(tf2::transformToEigen(cTo)); + + ControlTabWidget::addPoseSampleToTreeView(cTo, bTe, effector_wrt_world_.size()); + } + catch (tf2::TransformException& e) + { + ROS_WARN("TF exception: %s", e.what()); + return false; + } + + return true; +} + +bool ControlTabWidget::solveCameraRobotPose() +{ + if (solver_ && !calibration_solver_->currentText().isEmpty()) + { + bool res = solver_->solve(effector_wrt_world_, object_wrt_sensor_, sensor_mount_type_, + parseSolverName(calibration_solver_->currentText().toStdString(), '/')); + if (res) + { + camera_robot_pose_ = solver_->getCameraRobotPose(); + + // Update camera pose guess in context tab + Eigen::Vector3d t = camera_robot_pose_.translation(); + Eigen::Vector3d r = camera_robot_pose_.rotation().eulerAngles(0, 1, 2); + Q_EMIT sensorPoseUpdate(t[0], t[1], t[2], r[0], r[1], r[2]); + + // Publish camera pose tf + std::string& from_frame = frame_names_[from_frame_tag_]; + std::string& to_frame = frame_names_["sensor"]; + if (!from_frame.empty() && !to_frame.empty()) + { + tf_tools_->clearAllTransforms(); + return tf_tools_->publishTransform(camera_robot_pose_, from_frame, to_frame); + } + else + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Invalid key used for reading the frame names."); + return false; + } + } + } + else + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "No available handeye calibration solver instance."); + return false; + } +} + +bool ControlTabWidget::frameNamesEmpty() +{ + // All of four frame names needed for getting the pair of two tf transforms + if (frame_names_["sensor"].empty() || frame_names_["object"].empty() || frame_names_["base"].empty() || + frame_names_["eef"].empty()) + { + QMessageBox::warning(this, tr("Empty Frame Name"), tr("At least one of the four frame names is empty.")); + return true; + } + return false; +} + +bool ControlTabWidget::checkJointStates() +{ + if (joint_names_.empty() || joint_states_.empty()) + return false; + + for (const std::vector& state : joint_states_) + if (state.size() != joint_names_.size()) + return false; + + return true; +} + +void ControlTabWidget::setTFTool(rviz_visual_tools::TFVisualToolsPtr& tf_pub) +{ + tf_tools_ = tf_pub; +} + +void ControlTabWidget::addPoseSampleToTreeView(const geometry_msgs::TransformStamped& cTo, + const geometry_msgs::TransformStamped& bTe, int id) +{ + std::string item_name = "Sample " + std::to_string(id); + QStandardItem* parent = new QStandardItem(QString(item_name.c_str())); + tree_view_model_->appendRow(parent); + + std::ostringstream ss; + + QStandardItem* child_1 = new QStandardItem("bTe"); + ss << bTe.transform; + child_1->appendRow(new QStandardItem(ss.str().c_str())); + parent->appendRow(child_1); + + QStandardItem* child_2 = new QStandardItem("cTo"); + ss.str(""); + ss << cTo.transform; + child_2->appendRow(new QStandardItem(ss.str().c_str())); + parent->appendRow(child_2); +} + +void ControlTabWidget::UpdateSensorMountType(int index) +{ + if (0 <= index && index <= 1) + { + sensor_mount_type_ = static_cast(index); + switch (sensor_mount_type_) + { + case mhc::EYE_TO_HAND: + from_frame_tag_ = "base"; + break; + case mhc::EYE_IN_HAND: + from_frame_tag_ = "eef"; + break; + default: + ROS_ERROR_STREAM_NAMED(LOGNAME, "Error sensor mount type."); + break; + } + } +} + +void ControlTabWidget::updateFrameNames(std::map names) +{ + frame_names_ = names; + ROS_DEBUG("Frame names changed:"); + for (const std::pair& name : frame_names_) + ROS_DEBUG_STREAM(name.first << " : " << name.second); +} + +void ControlTabWidget::takeSampleBtnClicked(bool clicked) +{ + if (frameNamesEmpty() || !takeTranformSamples()) + return; + + if (effector_wrt_world_.size() == object_wrt_sensor_.size() && effector_wrt_world_.size() > 4) + if (!solveCameraRobotPose()) + return; + + // Save the joint values of current robot state + if (planning_scene_monitor_) + { + planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now(), 0.1); + const planning_scene_monitor::LockedPlanningSceneRO& ps = + planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); + if (ps) + { + const robot_state::RobotState& state = ps->getCurrentState(); + const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(group_name_->currentText().toStdString()); + const std::vector& names = jmg->getActiveJointModelNames(); + if (joint_names_.size() != names.size() || joint_names_ != names) + { + joint_names_.clear(); + joint_states_.clear(); + } + std::vector state_joint_values; + state.copyJointGroupPositions(jmg, state_joint_values); + if (names.size() == state_joint_values.size()) + { + joint_names_ = names; + joint_states_.push_back(state_joint_values); + auto_progress_->setMax(joint_states_.size()); + } + } + } +} + +void ControlTabWidget::resetSampleBtnClicked(bool clicked) +{ + // Clear recorded transforms + effector_wrt_world_.clear(); + object_wrt_sensor_.clear(); + tree_view_model_->clear(); + + // Clear recorded joint states + joint_states_.clear(); + auto_progress_->setMax(0); + auto_progress_->setValue(0); +} + +void ControlTabWidget::saveCameraPoseBtnClicked(bool clicked) +{ + std::string& from_frame = frame_names_[from_frame_tag_]; + std::string& to_frame = frame_names_["sensor"]; + + if (from_frame.empty() || to_frame.empty()) + { + QMessageBox::warning(this, tr("Empty Frame Name"), tr("Make sure you have set correct frame names.")); + return; + } + + QString file_name = + QFileDialog::getSaveFileName(this, tr("Save Camera Robot Pose"), "", tr("Target File (*.launch);;All Files (*)")); + + if (file_name.isEmpty()) + return; + + if (!file_name.endsWith(".launch")) + file_name += ".launch"; + + QFile file(file_name); + if (!file.open(QIODevice::WriteOnly)) + { + QMessageBox::warning(this, tr("Unable to open file"), file.errorString()); + return; + } + + QTextStream out(&file); + + Eigen::Vector3d t = camera_robot_pose_.translation(); + Eigen::Vector3d r = camera_robot_pose_.rotation().eulerAngles(0, 1, 2); + std::stringstream ss; + ss << "\n"; + ss << "\n"; + ss << ""; + out << ss.str().c_str(); +} + +void ControlTabWidget::planningGroupNameChanged(const QString& text) +{ + if (!text.isEmpty()) + { + if (move_group_ && move_group_->getName() == text.toStdString()) + return; + + try + { + moveit::planning_interface::MoveGroupInterface::Options opt(group_name_->currentText().toStdString()); + opt.node_handle_ = nh_; + move_group_.reset(new moveit::planning_interface::MoveGroupInterface(opt, tf_buffer_, ros::WallDuration(30, 0))); + + // Clear the joint values aligning with other group + joint_states_.clear(); + auto_progress_->setMax(0); + } + catch (const std::exception& e) + { + ROS_ERROR_NAMED(LOGNAME, "%s", e.what()); + } + } + else + { + QMessageBox::warning(this, tr("Invalid Group Name"), "Group name is empty"); + } +} + +void ControlTabWidget::saveJointStateBtnClicked(bool clicked) +{ + if (!checkJointStates()) + { + QMessageBox::warning(this, tr("Error"), tr("No joint states or joint state dosen't match joint names.")); + return; + } + + QString file_name = + QFileDialog::getSaveFileName(this, tr("Save Joint States"), "", tr("Target File (*.yaml);;All Files (*)")); + + if (file_name.isEmpty()) + return; + + if (!file_name.endsWith(".yaml")) + file_name += ".yaml"; + + QFile file(file_name); + if (!file.open(QIODevice::WriteOnly)) + { + QMessageBox::warning(this, tr("Unable to open file"), file.errorString()); + return; + } + + YAML::Emitter emitter; + emitter << YAML::BeginMap; + + // Joint Names + emitter << YAML::Key << "joint_names"; + emitter << YAML::Value << YAML::BeginSeq; + for (size_t i = 0; i < joint_names_.size(); ++i) + emitter << YAML::Value << joint_names_[i]; + emitter << YAML::EndSeq; + + // Joint Values + emitter << YAML::Key << "joint_values"; + emitter << YAML::Value << YAML::BeginSeq; + for (size_t i = 0; i < joint_states_.size(); ++i) + { + emitter << YAML::BeginSeq; + for (size_t j = 0; j < joint_states_[i].size(); ++j) + emitter << YAML::Value << joint_states_[i][j]; + emitter << YAML::EndSeq; + } + emitter << YAML::EndSeq; + + emitter << YAML::EndMap; + + QTextStream out(&file); + out << emitter.c_str(); +} + +void ControlTabWidget::loadJointStateBtnClicked(bool clicked) +{ + QString file_name = + QFileDialog::getOpenFileName(this, tr("Load Joint States"), "", tr("Target File (*.yaml);;All Files (*)")); + + if (file_name.isEmpty() || !file_name.endsWith(".yaml")) + return; + + QFile file(file_name); + if (!file.open(QIODevice::ReadOnly)) + { + QMessageBox::warning(this, tr("Unable to open file"), file.errorString()); + return; + } + + // Begin parsing + try + { + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Load joint states from file: " << file_name.toStdString().c_str()); + YAML::Node doc = YAML::LoadFile(file_name.toStdString()); + if (!doc.IsMap()) + return; + + // Read joint names + const YAML::Node& names = doc["joint_names"]; + if (!names.IsNull() && names.IsSequence()) + { + joint_names_.clear(); + for (YAML::const_iterator it = names.begin(); it != names.end(); ++it) + joint_names_.push_back(it->as()); + } + else + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Can't find 'joint_names' in the openned file."); + return; + } + + // Read joint values + const YAML::Node& values = doc["joint_values"]; + if (!values.IsNull() && values.IsSequence()) + { + joint_states_.clear(); + for (YAML::const_iterator state_it = values.begin(); state_it != values.end(); ++state_it) + { + std::vector jv; + if (!state_it->IsNull() && state_it->IsSequence()) + for (YAML::const_iterator joint_it = state_it->begin(); joint_it != state_it->end(); ++joint_it) + jv.push_back(joint_it->as()); + if (jv.size() == joint_names_.size()) + joint_states_.push_back(jv); + } + } + else + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Can't find 'joint_values' in the openned file."); + return; + } + } + catch (YAML::ParserException& e) // Catch errors + { + ROS_ERROR_STREAM_NAMED(LOGNAME, e.what()); + return; + } + + if (joint_states_.size() > 0) + { + auto_progress_->setMax(joint_states_.size()); + auto_progress_->setValue(0); + } + ROS_INFO_STREAM_NAMED(LOGNAME, "Loaded and parsed: " << file_name.toStdString()); +} + +void ControlTabWidget::autoPlanBtnClicked(bool clicked) +{ + auto_plan_btn_->setEnabled(false); + plan_watcher_->setFuture(QtConcurrent::run(this, &ControlTabWidget::computePlan)); +} + +void ControlTabWidget::computePlan() +{ + planning_res_ = true; + int max = auto_progress_->bar_->maximum(); + + if (max != joint_states_.size() || auto_progress_->getValue() == max) + { + planning_res_ = false; + return; + } + + if (!checkJointStates()) + { + planning_res_ = false; + return; + } + + if (!planning_scene_monitor_) + { + planning_res_ = false; + return; + } + + if (!move_group_ || move_group_->getActiveJoints() != joint_names_) + { + planning_res_ = false; + return; + } + + // Get current joint state as start state + robot_state::RobotStatePtr start_state = move_group_->getCurrentState(); + planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now(), 0.1); + const planning_scene_monitor::LockedPlanningSceneRO& ps = + planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); + if (ps) + start_state.reset(new robot_state::RobotState(ps->getCurrentState())); + + // Plan motion to the recorded joint state target + if (auto_progress_->getValue() < joint_states_.size()) + { + move_group_->setStartState(*start_state); + move_group_->setJointValueTarget(joint_states_[auto_progress_->getValue()]); + move_group_->setMaxVelocityScalingFactor(0.5); + move_group_->setMaxAccelerationScalingFactor(0.5); + current_plan_.reset(new moveit::planning_interface::MoveGroupInterface::Plan()); + planning_res_ = (move_group_->plan(*current_plan_) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + + if (planning_res_) + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Planning succeed."); + else + ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning failed."); + } +} + +void ControlTabWidget::autoExecuteBtnClicked(bool clicked) +{ + if (plan_watcher_->isRunning()) + { + plan_watcher_->waitForFinished(); + } + + auto_execute_btn_->setEnabled(false); + execution_watcher_->setFuture(QtConcurrent::run(this, &ControlTabWidget::computeExecution)); +} + +void ControlTabWidget::computeExecution() +{ + if (move_group_ && current_plan_) + planning_res_ = (move_group_->execute(*current_plan_) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + + if (planning_res_) + { + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Execution succeed."); + } + else + ROS_ERROR_STREAM_NAMED(LOGNAME, "Execution failed."); +} + +void ControlTabWidget::planFinished() +{ + auto_plan_btn_->setEnabled(true); + if (!planning_res_) + QMessageBox::warning(this, tr("Error"), + tr("Please check if move_group is started or there are recorded joint states.")); + ROS_DEBUG_NAMED(LOGNAME, "Plan finished"); +} + +void ControlTabWidget::executeFinished() +{ + auto_execute_btn_->setEnabled(true); + if (planning_res_) + { + auto_progress_->setValue(auto_progress_->getValue() + 1); + if (!frameNamesEmpty()) + takeTranformSamples(); + + if (effector_wrt_world_.size() == object_wrt_sensor_.size() && effector_wrt_world_.size() > 4) + solveCameraRobotPose(); + } + ROS_DEBUG_NAMED(LOGNAME, "Execution finished"); +} + +void ControlTabWidget::autoSkipBtnClicked(bool clicked) +{ + auto_progress_->setValue(auto_progress_->getValue() + 1); +} + +} // namespace moveit_rviz_plugin \ No newline at end of file From a7c473622336255defca8e4448cd24dfc2d33df0 Mon Sep 17 00:00:00 2001 From: 7929f1cfe06ad78d8b8c60a417e1b4a8de37050c Date: Sat, 6 Jun 2020 23:24:50 +0800 Subject: [PATCH 08/26] Enable this repo build with moveit in CI --- .travis.yml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index ca02c1d..484d700 100644 --- a/.travis.yml +++ b/.travis.yml @@ -3,7 +3,7 @@ sudo: required dist: xenial services: - docker -language: generic +language: cpp cache: ccache compiler: gcc @@ -18,7 +18,8 @@ env: - ROS_REPO=ros matrix: - TEST=catkin_lint - - ROS_DISTRO=melodic + - UPSTREAM_WORKSPACE=https://raw.githubusercontent.com/ros-planning/moveit/master/moveit.rosinstall + TEST_BLACKLIST=moveit_ros_perception before_script: - git clone -q --depth=1 https://github.com/ros-planning/moveit_ci.git .moveit_ci From c698077934ee06f5cd531bf71d0ad1d720f512c2 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Sun, 7 Jun 2020 21:44:05 -0600 Subject: [PATCH 09/26] some suggested changes to GUI text --- .../src/handeye_context_widget.cpp | 26 +++++++++---------- .../src/handeye_control_widget.cpp | 22 ++++++++-------- .../src/handeye_target_widget.cpp | 22 ++++++++-------- 3 files changed, 35 insertions(+), 35 deletions(-) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_context_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_context_widget.cpp index e0081f3..e5ab1d9 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_context_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_context_widget.cpp @@ -171,9 +171,9 @@ ContextTabWidget::ContextTabWidget(QWidget* parent) : QWidget(parent), tf_listen group_left_top->setLayout(layout_left_top); sensor_mount_type_ = new QComboBox(); - sensor_mount_type_->addItem("Eye-to-Hand"); + sensor_mount_type_->addItem("Eye-to-hand"); sensor_mount_type_->addItem("Eye-in-hand"); - layout_left_top->addRow("Sensor Mount Type", sensor_mount_type_); + layout_left_top->addRow("Sensor configuration", sensor_mount_type_); connect(sensor_mount_type_, SIGNAL(activated(int)), this, SLOT(updateSensorMountType(int))); // Frame name selection area @@ -183,16 +183,16 @@ ContextTabWidget::ContextTabWidget(QWidget* parent) : QWidget(parent), tf_listen frame_group->setLayout(frame_layout); frames_.insert(std::make_pair("sensor", new TFFrameNameComboBox(CAMERA_FRAME))); - frame_layout->addRow("Sensor Frame:", frames_["sensor"]); + frame_layout->addRow("Sensor frame:", frames_["sensor"]); frames_.insert(std::make_pair("object", new TFFrameNameComboBox(ENVIRONMENT_FRAME))); - frame_layout->addRow("Object Frame:", frames_["object"]); + frame_layout->addRow("Object frame:", frames_["object"]); frames_.insert(std::make_pair("eef", new TFFrameNameComboBox(ROBOT_FRAME))); - frame_layout->addRow("End-Effector Frame:", frames_["eef"]); + frame_layout->addRow("End-effector frame:", frames_["eef"]); frames_.insert(std::make_pair("base", new TFFrameNameComboBox(ROBOT_FRAME))); - frame_layout->addRow("Robot Base Frame:", frames_["base"]); + frame_layout->addRow("Robot base frame:", frames_["base"]); for (std::pair& frame : frames_) connect(frame.second, SIGNAL(activated(int)), this, SLOT(updateFrameName(int))); @@ -220,22 +220,22 @@ ContextTabWidget::ContextTabWidget(QWidget* parent) : QWidget(parent), tf_listen QFormLayout* pose_layout = new QFormLayout(); pose_group->setLayout(pose_layout); - guess_pose_.insert(std::make_pair("Tx", new SliderWidget(this, "TranslX", -2.0, 2.0))); + guess_pose_.insert(std::make_pair("Tx", new SliderWidget(this, "X", -2.0, 2.0))); pose_layout->addRow(guess_pose_["Tx"]); - guess_pose_.insert(std::make_pair("Ty", new SliderWidget(this, "TranslY", -2.0, 2.0))); + guess_pose_.insert(std::make_pair("Ty", new SliderWidget(this, "Y", -2.0, 2.0))); pose_layout->addRow(guess_pose_["Ty"]); - guess_pose_.insert(std::make_pair("Tz", new SliderWidget(this, "TranslZ", -2.0, 2.0))); + guess_pose_.insert(std::make_pair("Tz", new SliderWidget(this, "Z", -2.0, 2.0))); pose_layout->addRow(guess_pose_["Tz"]); - guess_pose_.insert(std::make_pair("Rx", new SliderWidget(this, "RotateX", -M_PI, M_PI))); + guess_pose_.insert(std::make_pair("Rx", new SliderWidget(this, "Roll", -M_PI, M_PI))); pose_layout->addRow(guess_pose_["Rx"]); - guess_pose_.insert(std::make_pair("Ry", new SliderWidget(this, "RotateY", -M_PI, M_PI))); + guess_pose_.insert(std::make_pair("Ry", new SliderWidget(this, "Pitch", -M_PI, M_PI))); pose_layout->addRow(guess_pose_["Ry"]); - guess_pose_.insert(std::make_pair("Rz", new SliderWidget(this, "RotateZ", -M_PI, M_PI))); + guess_pose_.insert(std::make_pair("Rz", new SliderWidget(this, "Yaw", -M_PI, M_PI))); pose_layout->addRow(guess_pose_["Rz"]); for (std::pair& dim : guess_pose_) @@ -550,4 +550,4 @@ void ContextTabWidget::fovOnOffBtnToggled(bool checked) updateAllMarkers(); } -} // namgespace moveit_rviz_plugin \ No newline at end of file +} // namgespace moveit_rviz_plugin diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp index 21f60ff..99b9c5d 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp @@ -116,7 +116,7 @@ ControlTabWidget::ControlTabWidget(QWidget* parent) layout->addWidget(auto_progress_); // Pose sample tree view area - QGroupBox* sample_group = new QGroupBox("Pose_sample"); + QGroupBox* sample_group = new QGroupBox("Pose samples"); sample_group->setMinimumWidth(280); calib_layout->addWidget(sample_group); QVBoxLayout* sample_layout = new QVBoxLayout(); @@ -131,11 +131,11 @@ ControlTabWidget::ControlTabWidget(QWidget* parent) sample_tree_view_->setIndentation(10); sample_layout->addWidget(sample_tree_view_); - // Setting area + // Settings area QVBoxLayout* layout_right = new QVBoxLayout(); calib_layout->addLayout(layout_right); - QGroupBox* setting_group = new QGroupBox("Setting"); + QGroupBox* setting_group = new QGroupBox("Settings"); layout_right->addWidget(setting_group); QFormLayout* setting_layout = new QFormLayout(); setting_group->setLayout(setting_layout); @@ -147,15 +147,15 @@ ControlTabWidget::ControlTabWidget(QWidget* parent) connect(group_name_, SIGNAL(activated(const QString&)), this, SLOT(planningGroupNameChanged(const QString&))); setting_layout->addRow("Planning Group", group_name_); - load_joint_state_btn_ = new QPushButton("Load Joint States"); + load_joint_state_btn_ = new QPushButton("Load joint states"); connect(load_joint_state_btn_, SIGNAL(clicked(bool)), this, SLOT(loadJointStateBtnClicked(bool))); setting_layout->addRow(load_joint_state_btn_); - save_joint_state_btn_ = new QPushButton("Save Joint states"); + save_joint_state_btn_ = new QPushButton("Save joint states"); connect(save_joint_state_btn_, SIGNAL(clicked(bool)), this, SLOT(saveJointStateBtnClicked(bool))); setting_layout->addRow(save_joint_state_btn_); - save_camera_pose_btn_ = new QPushButton("Save Camera Pose"); + save_camera_pose_btn_ = new QPushButton("Save camera pose"); connect(save_camera_pose_btn_, SIGNAL(clicked(bool)), this, SLOT(saveCameraPoseBtnClicked(bool))); setting_layout->addRow(save_camera_pose_btn_); @@ -165,12 +165,12 @@ ControlTabWidget::ControlTabWidget(QWidget* parent) QHBoxLayout* control_cal_layout = new QHBoxLayout(); manual_cal_group->setLayout(control_cal_layout); - take_sample_btn_ = new QPushButton("Take Sample"); + take_sample_btn_ = new QPushButton("Take sample"); take_sample_btn_->setMinimumHeight(35); connect(take_sample_btn_, SIGNAL(clicked(bool)), this, SLOT(takeSampleBtnClicked(bool))); control_cal_layout->addWidget(take_sample_btn_); - reset_sample_btn_ = new QPushButton("Reset Sample"); + reset_sample_btn_ = new QPushButton("Reset samples"); reset_sample_btn_->setMinimumHeight(35); connect(reset_sample_btn_, SIGNAL(clicked(bool)), this, SLOT(resetSampleBtnClicked(bool))); control_cal_layout->addWidget(reset_sample_btn_); @@ -430,12 +430,12 @@ void ControlTabWidget::addPoseSampleToTreeView(const geometry_msgs::TransformSta std::ostringstream ss; - QStandardItem* child_1 = new QStandardItem("bTe"); + QStandardItem* child_1 = new QStandardItem("TF base-to-eef"); ss << bTe.transform; child_1->appendRow(new QStandardItem(ss.str().c_str())); parent->appendRow(child_1); - QStandardItem* child_2 = new QStandardItem("cTo"); + QStandardItem* child_2 = new QStandardItem("TF camera-to-target"); ss.str(""); ss << cTo.transform; child_2->appendRow(new QStandardItem(ss.str().c_str())); @@ -825,4 +825,4 @@ void ControlTabWidget::autoSkipBtnClicked(bool clicked) auto_progress_->setValue(auto_progress_->getValue() + 1); } -} // namespace moveit_rviz_plugin \ No newline at end of file +} // namespace moveit_rviz_plugin diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp index 4888081..5b45cec 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp @@ -94,7 +94,7 @@ TargetTabWidget::TargetTabWidget(QWidget* parent) layout->addLayout(layout_left); // Target creation area - QGroupBox* group_left_top = new QGroupBox("Target_Intrinsic_Params", this); + QGroupBox* group_left_top = new QGroupBox("Target Intrinsic Params", this); layout_left->addWidget(group_left_top); QFormLayout* layout_left_top = new QFormLayout(); group_left_top->setLayout(layout_left_top); @@ -115,26 +115,26 @@ TargetTabWidget::TargetTabWidget(QWidget* parent) target_params_["markers_x"]->setText(QString("4")); target_params_["markers_x"]->setValidator(new QIntValidator(1, 50)); - layout_left_top->addRow("Num_Markers_X", target_params_["markers_x"]); + layout_left_top->addRow("Num markers, X", target_params_["markers_x"]); target_params_["markers_y"]->setText(QString("5")); target_params_["markers_y"]->setValidator(new QIntValidator(1, 50)); - layout_left_top->addRow("Num_Markers_Y", target_params_["markers_y"]); + layout_left_top->addRow("Num markers, Y", target_params_["markers_y"]); target_params_["marker_size"]->setText(QString("200")); target_params_["marker_size"]->setValidator(new QIntValidator(100, 1000)); - layout_left_top->addRow("Marker_Size_(pixels)", target_params_["marker_size"]); + layout_left_top->addRow("Marker size (pixels)", target_params_["marker_size"]); target_params_["marker_dist"]->setText(QString("20")); target_params_["marker_dist"]->setValidator(new QIntValidator(10, 200)); - layout_left_top->addRow("Marker_Dist_(pixels)", target_params_["marker_dist"]); + layout_left_top->addRow("Marker spacing (pixels)", target_params_["marker_dist"]); target_params_["marker_border"]->setText(QString("1")); target_params_["marker_border"]->setValidator(new QIntValidator(1, 4)); - layout_left_top->addRow("Marker_Border_(bits)", target_params_["marker_border"]); + layout_left_top->addRow("Marker border (bits)", target_params_["marker_border"]); // Target 3D pose recognition area - QGroupBox* group_left_bottom = new QGroupBox("Target_Pose_Recognition", this); + QGroupBox* group_left_bottom = new QGroupBox("Target Pose Detection", this); layout_left->addWidget(group_left_bottom); QFormLayout* layout_left_bottom = new QFormLayout(); group_left_bottom->setLayout(layout_left_bottom); @@ -156,14 +156,14 @@ TargetTabWidget::TargetTabWidget(QWidget* parent) target_real_dims_["marker_size_real"]->setText("0.0256"); target_real_dims_["marker_size_real"]->setValidator(new QDoubleValidator(0, 2, 4)); - layout_left_bottom->addRow("Marker Size (m)", target_real_dims_["marker_size_real"]); + layout_left_bottom->addRow("Marker size (m)", target_real_dims_["marker_size_real"]); target_real_dims_["marker_dist_real"]->setText("0.0066"); target_real_dims_["marker_dist_real"]->setValidator(new QDoubleValidator(0, 2, 4)); - layout_left_bottom->addRow("Marker Dist (m)", target_real_dims_["marker_dist_real"]); + layout_left_bottom->addRow("Marker spacing (m)", target_real_dims_["marker_dist_real"]); // Target image dislay, create and save area - QGroupBox* group_right = new QGroupBox("Target_Create_Save", this); + QGroupBox* group_right = new QGroupBox("Target", this); group_right->setMinimumWidth(330); layout->addWidget(group_right); QVBoxLayout* layout_right = new QVBoxLayout(); @@ -535,4 +535,4 @@ void TargetTabWidget::cameraInfoComboBoxChanged(const QString& topic) } } -} // namedist moveit_rviz_plugin \ No newline at end of file +} // namedist moveit_rviz_plugin From bfc5fe2138f3ffd9275aa3fa8e399c5d30e9aeec Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Wed, 10 Jun 2020 12:05:52 -0600 Subject: [PATCH 10/26] Merged rviz pr into charuco-board --- moveit_calibration_gui/CMakeLists.txt | 2 ++ moveit_calibration_gui/package.xml | 1 + 2 files changed, 3 insertions(+) diff --git a/moveit_calibration_gui/CMakeLists.txt b/moveit_calibration_gui/CMakeLists.txt index 6e05815..e0e67c6 100644 --- a/moveit_calibration_gui/CMakeLists.txt +++ b/moveit_calibration_gui/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS class_loader geometric_shapes moveit_calibration_plugins + moveit_ros_perception moveit_ros_planning_interface moveit_ros_visualization moveit_visual_tools @@ -50,6 +51,7 @@ catkin_package( INCLUDE_DIRS handeye_calibration_rviz_plugin/include CATKIN_DEPENDS + moveit_ros_perception moveit_ros_planning_interface moveit_visual_tools moveit_calibration_plugins diff --git a/moveit_calibration_gui/package.xml b/moveit_calibration_gui/package.xml index dba61fe..87439a4 100644 --- a/moveit_calibration_gui/package.xml +++ b/moveit_calibration_gui/package.xml @@ -24,6 +24,7 @@ geometric_shapes moveit_ros_visualization moveit_calibration_plugins + moveit_ros_perception moveit_ros_planning_interface moveit_visual_tools pluginlib From 4934340dd2d269795b374e05837b3d93fbb21a64 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Sat, 13 Jun 2020 15:46:15 -0600 Subject: [PATCH 11/26] updated gui to work with changing parameters --- .../handeye_target_widget.h | 20 +- .../src/handeye_target_widget.cpp | 289 ++++++++---------- .../src/handeye_target_charuco.cpp | 6 +- 3 files changed, 150 insertions(+), 165 deletions(-) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h index 094080b..8d43732 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h @@ -119,9 +119,9 @@ class TargetTabWidget : public QWidget void loadWidget(const rviz::Config& config); void saveWidget(rviz::Config& config); - bool loadTargetPlugin(); + bool loadAvailableTargetPlugins(); - bool createTargetInstance(const std::string& plugin_name); + bool createTargetInstance(); void fillDictionaryIds(std::string id = ""); @@ -134,15 +134,15 @@ private Q_SLOTS: // Called when the current item of target_type_ changed void targetTypeComboboxChanged(const QString& text); + // Called to update GUI inputs to match selected target type + bool loadInputWidgetsForTargetType(const std::string& plugin_name); + // Called when the create_target_btn clicked void createTargetImageBtnClicked(bool clicked); // Called when the save_target_btn clicked void saveTargetImageBtnClicked(bool clicked); - // Called when the intrinsic or real params of the target changed - void targetParamsSet(const QString& text = ""); - // Called when the item of image_topic_field_ combobox is selected void imageTopicComboboxChanged(const QString& topic); @@ -160,16 +160,16 @@ private Q_SLOTS: // Qt components // ************************************************************** - // Target intrinsic params + // Target params + QFormLayout* target_param_layout_; QComboBox* target_type_; - QComboBox* dictionary_id_; - std::map target_params_; + std::vector target_plugin_params_; + std::map target_param_inputs_; // Target 3D pose recognition RosTopicComboBox* image_topic_; RosTopicComboBox* camera_info_topic_; std::map ros_topics_; - std::map target_real_dims_; // Target Image display, create and save QLabel* target_display_label_; @@ -202,4 +202,4 @@ private Q_SLOTS: } // namedist moveit_rviz_plugin -#endif \ No newline at end of file +#endif diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp index 5b45cec..74e0538 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Yu Yan */ +/* Author: Yu Yan, John Stechschulte */ #include @@ -85,7 +85,12 @@ void RosTopicComboBox::mousePressEvent(QMouseEvent* event) } TargetTabWidget::TargetTabWidget(QWidget* parent) - : QWidget(parent), nh_("~"), it_(nh_), target_plugins_loader_(nullptr), target_(nullptr) + : QWidget(parent) + , nh_("~") + , it_(nh_) + , target_plugins_loader_(nullptr) + , target_(nullptr) + , target_param_layout_(new QFormLayout()) { // Target setting tab area ----------------------------------------------- QHBoxLayout* layout = new QHBoxLayout(); @@ -94,44 +99,14 @@ TargetTabWidget::TargetTabWidget(QWidget* parent) layout->addLayout(layout_left); // Target creation area - QGroupBox* group_left_top = new QGroupBox("Target Intrinsic Params", this); + QGroupBox* group_left_top = new QGroupBox("Target Params", this); + layout_left->addWidget(group_left_top); - QFormLayout* layout_left_top = new QFormLayout(); - group_left_top->setLayout(layout_left_top); + group_left_top->setLayout(target_param_layout_); target_type_ = new QComboBox(); connect(target_type_, SIGNAL(activated(const QString&)), this, SLOT(targetTypeComboboxChanged(const QString&))); - layout_left_top->addRow("Target Type", target_type_); - - dictionary_id_ = new QComboBox(); - layout_left_top->addRow("Dictionary", dictionary_id_); - - target_params_.clear(); - target_params_.insert(std::make_pair("markers_x", new QLineEdit())); - target_params_.insert(std::make_pair("markers_y", new QLineEdit())); - target_params_.insert(std::make_pair("marker_size", new QLineEdit())); - target_params_.insert(std::make_pair("marker_dist", new QLineEdit())); - target_params_.insert(std::make_pair("marker_border", new QLineEdit())); - - target_params_["markers_x"]->setText(QString("4")); - target_params_["markers_x"]->setValidator(new QIntValidator(1, 50)); - layout_left_top->addRow("Num markers, X", target_params_["markers_x"]); - - target_params_["markers_y"]->setText(QString("5")); - target_params_["markers_y"]->setValidator(new QIntValidator(1, 50)); - layout_left_top->addRow("Num markers, Y", target_params_["markers_y"]); - - target_params_["marker_size"]->setText(QString("200")); - target_params_["marker_size"]->setValidator(new QIntValidator(100, 1000)); - layout_left_top->addRow("Marker size (pixels)", target_params_["marker_size"]); - - target_params_["marker_dist"]->setText(QString("20")); - target_params_["marker_dist"]->setValidator(new QIntValidator(10, 200)); - layout_left_top->addRow("Marker spacing (pixels)", target_params_["marker_dist"]); - - target_params_["marker_border"]->setText(QString("1")); - target_params_["marker_border"]->setValidator(new QIntValidator(1, 4)); - layout_left_top->addRow("Marker border (bits)", target_params_["marker_border"]); + target_param_layout_->addRow("Target Type", target_type_); // Target 3D pose recognition area QGroupBox* group_left_bottom = new QGroupBox("Target Pose Detection", this); @@ -151,17 +126,6 @@ TargetTabWidget::TargetTabWidget(QWidget* parent) connect(ros_topics_["camera_info_topic"], SIGNAL(activated(const QString&)), this, SLOT(cameraInfoComboBoxChanged(const QString&))); - target_real_dims_.insert(std::make_pair("marker_size_real", new QLineEdit())); - target_real_dims_.insert(std::make_pair("marker_dist_real", new QLineEdit())); - - target_real_dims_["marker_size_real"]->setText("0.0256"); - target_real_dims_["marker_size_real"]->setValidator(new QDoubleValidator(0, 2, 4)); - layout_left_bottom->addRow("Marker size (m)", target_real_dims_["marker_size_real"]); - - target_real_dims_["marker_dist_real"]->setText("0.0066"); - target_real_dims_["marker_dist_real"]->setValidator(new QDoubleValidator(0, 2, 4)); - layout_left_bottom->addRow("Marker spacing (m)", target_real_dims_["marker_dist_real"]); - // Target image dislay, create and save area QGroupBox* group_right = new QGroupBox("Target", this); group_right->setMinimumWidth(330); @@ -181,9 +145,8 @@ TargetTabWidget::TargetTabWidget(QWidget* parent) layout_right->addWidget(save_target_btn); connect(save_target_btn, SIGNAL(clicked(bool)), this, SLOT(saveTargetImageBtnClicked(bool))); - // Load target availible plugins - if (loadTargetPlugin() && createTargetInstance(target_type_->currentText().toStdString())) - fillDictionaryIds(); + // Load availible target plugins + loadAvailableTargetPlugins(); // Initialize image publisher image_pub_ = it_.advertise("/handeye_calibration/target_detection", 1); @@ -198,77 +161,74 @@ TargetTabWidget::TargetTabWidget(QWidget* parent) void TargetTabWidget::loadWidget(const rviz::Config& config) { - if (target_type_->count() > 0) - { - QString type; - if (config.mapGetString("target_type", &type) && target_type_->findText(type, Qt::MatchCaseSensitive) != -1) - target_type_->setCurrentText(type); - - createTargetInstance(target_type_->currentText().toStdString()); - - QString dict_name; - config.mapGetString("dictionary", &dict_name); - fillDictionaryIds(dict_name.toStdString()); - } - - int param_int; - for (const std::pair& param : target_params_) - if (config.mapGetInt(param.first.c_str(), ¶m_int)) - param.second->setText(std::to_string(param_int).c_str()); - - for (const std::pair& topic : ros_topics_) - { - QString topic_name; - if (config.mapGetString(topic.first.c_str(), &topic_name)) - { - if (topic.second->hasTopic(topic_name)) - { - topic.second->setCurrentText(topic_name); - try - { - if (!topic.first.compare("image_topic")) - { - image_sub_.shutdown(); - image_sub_ = it_.subscribe(topic_name.toStdString(), 1, &TargetTabWidget::imageCallback, this); - } - - if (!topic.first.compare("camera_info_topic")) - { - camerainfo_sub_.shutdown(); - camerainfo_sub_ = nh_.subscribe(topic_name.toStdString(), 1, &TargetTabWidget::cameraInfoCallback, this); - } - } - catch (const image_transport::TransportLoadException& e) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Subscribe to " << topic_name.toStdString() << " fail: " << e.what()); - } - } - } - } - - for (const std::pair& param : target_real_dims_) - { - float param_double; - if (config.mapGetFloat(param.first.c_str(), ¶m_double)) - param.second->setText(std::to_string(param_double).c_str()); - } + // TODO: get all this to work + // if (target_type_->count() > 0) + //{ + // QString type; + // if (config.mapGetString("target_type", &type) && target_type_->findText(type, Qt::MatchCaseSensitive) != -1) + // target_type_->setCurrentText(type); + + //} + + // int param_int; + // for (const std::pair& param : target_params_) + // if (config.mapGetInt(param.first.c_str(), ¶m_int)) + // param.second->setText(std::to_string(param_int).c_str()); + + // for (const std::pair& topic : ros_topics_) + //{ + // QString topic_name; + // if (config.mapGetString(topic.first.c_str(), &topic_name)) + // { + // if (topic.second->hasTopic(topic_name)) + // { + // topic.second->setCurrentText(topic_name); + // try + // { + // if (!topic.first.compare("image_topic")) + // { + // image_sub_.shutdown(); + // image_sub_ = it_.subscribe(topic_name.toStdString(), 1, &TargetTabWidget::imageCallback, this); + // } + + // if (!topic.first.compare("camera_info_topic")) + // { + // camerainfo_sub_.shutdown(); + // camerainfo_sub_ = nh_.subscribe(topic_name.toStdString(), 1, &TargetTabWidget::cameraInfoCallback, this); + // } + // } + // catch (const image_transport::TransportLoadException& e) + // { + // ROS_ERROR_STREAM_NAMED(LOGNAME, "Subscribe to " << topic_name.toStdString() << " fail: " << e.what()); + // } + // } + // } + //} + + // for (const std::pair& param : target_real_dims_) + //{ + // float param_double; + // if (config.mapGetFloat(param.first.c_str(), ¶m_double)) + // param.second->setText(std::to_string(param_double).c_str()); + //} } void TargetTabWidget::saveWidget(rviz::Config& config) { - config.mapSetValue("target_type", target_type_->currentText()); - config.mapSetValue("dictionary", dictionary_id_->currentText()); - for (const std::pair& param : target_params_) - config.mapSetValue(param.first.c_str(), param.second->text().toInt()); + // TODO: get all this to work + // config.mapSetValue("target_type", target_type_->currentText()); + // config.mapSetValue("dictionary", dictionary_id_->currentText()); + // for (const std::pair& param : target_params_) + // config.mapSetValue(param.first.c_str(), param.second->text().toInt()); - for (const std::pair& topic : ros_topics_) - config.mapSetValue(topic.first.c_str(), topic.second->currentText()); + // for (const std::pair& topic : ros_topics_) + // config.mapSetValue(topic.first.c_str(), topic.second->currentText()); - for (const std::pair& param : target_real_dims_) - config.mapSetValue(param.first.c_str(), param.second->text().toDouble()); + // for (const std::pair& param : target_real_dims_) + // config.mapSetValue(param.first.c_str(), param.second->text().toDouble()); } -bool TargetTabWidget::loadTargetPlugin() +bool TargetTabWidget::loadAvailableTargetPlugins() { if (!target_plugins_loader_) { @@ -300,7 +260,7 @@ bool TargetTabWidget::loadTargetPlugin() return true; } -bool TargetTabWidget::createTargetInstance(const std::string& plugin_name) +bool TargetTabWidget::loadInputWidgetsForTargetType(const std::string& plugin_name) { if (plugin_name.empty()) return false; @@ -308,11 +268,33 @@ bool TargetTabWidget::createTargetInstance(const std::string& plugin_name) try { target_ = target_plugins_loader_->createUniqueInstance(plugin_name); - target_->initialize(target_params_["markers_x"]->text().toInt(), target_params_["markers_y"]->text().toInt(), - target_params_["marker_size"]->text().toInt(), target_params_["marker_dist"]->text().toInt(), - target_params_["marker_border"]->text().toInt(), dictionary_id_->currentText().toStdString(), - target_real_dims_["marker_size_real"]->text().toDouble(), - target_real_dims_["marker_dist_real"]->text().toDouble()); + target_plugin_params_ = target_->getParameters(); + target_param_inputs_.clear(); + // clear out layout, except target type + while (target_param_layout_->rowCount() > 1) + { + target_param_layout_->removeRow(1); + } + for (const auto& param : target_plugin_params_) + { + switch (param.parameter_type_) + { + case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Int: + case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Float: + target_param_inputs_.insert(std::make_pair(param.name_, new QLineEdit())); + target_param_layout_->addRow(param.name_.c_str(), target_param_inputs_[param.name_]); + break; + case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Enum: + QComboBox* combo_box = new QComboBox(); + for (const std::string& value : param.enum_values_) + { + combo_box->addItem(tr(value.c_str())); + } + target_param_inputs_.insert(std::make_pair(param.name_, combo_box)); + target_param_layout_->addRow(param.name_.c_str(), target_param_inputs_[param.name_]); + break; + } + } } catch (pluginlib::PluginlibException& ex) { @@ -320,35 +302,50 @@ bool TargetTabWidget::createTargetInstance(const std::string& plugin_name) target_ = nullptr; return false; } - return true; } -void TargetTabWidget::fillDictionaryIds(std::string id) +bool TargetTabWidget::createTargetInstance() { - if (target_) - { - // Get dictionary ids - const std::vector& ids = target_->getDictionaryIds(); - dictionary_id_->clear(); - for (const std::string& id : ids) - { - dictionary_id_->addItem(tr(id.c_str())); - } + if (!target_) + return false; - // Check if 'id' exists in the list - if (!id.empty()) + try + { + // TODO: load parameters from GUI + for (const auto& param : target_plugin_params_) { - auto it = std::find(ids.begin(), ids.end(), id); - if (it != ids.end()) - dictionary_id_->setCurrentText(QString(id.c_str())); + switch (param.parameter_type_) + { + case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Int: + target_->setParameter(param.name_, + static_cast(target_param_inputs_[param.name_])->text().toInt()); + break; + case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Float: + target_->setParameter(param.name_, + static_cast(target_param_inputs_[param.name_])->text().toFloat()); + break; + case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Enum: + target_->setParameter( + param.name_, static_cast(target_param_inputs_[param.name_])->currentText().toStdString()); + break; + } } + target_->initialize(); } + catch (pluginlib::PluginlibException& ex) + { + QMessageBox::warning(this, tr("Exception while loading a handeye target plugin"), tr(ex.what())); + target_ = nullptr; + return false; + } + + return true; } void TargetTabWidget::imageCallback(const sensor_msgs::ImageConstPtr& msg) { - targetParamsSet(); + createTargetInstance(); // Depth image format `16UC1` cannot be converted to `MONO8` if (msg->encoding == "16UC1") @@ -432,18 +429,15 @@ void TargetTabWidget::targetTypeComboboxChanged(const QString& text) { if (!text.isEmpty()) { - if (createTargetInstance(text.toStdString())) - { - fillDictionaryIds(); - } + loadInputWidgetsForTargetType(text.toStdString()); } } void TargetTabWidget::createTargetImageBtnClicked(bool clicked) { + createTargetInstance(); if (target_) { - targetParamsSet(); target_->createTargetImage(target_image_); } else @@ -461,19 +455,6 @@ void TargetTabWidget::createTargetImageBtnClicked(bool clicked) } } -void TargetTabWidget::targetParamsSet(const QString& text) -{ - if (target_) - { - target_->setTargetIntrinsicParams( - target_params_["markers_x"]->text().toInt(), target_params_["markers_y"]->text().toInt(), - target_params_["marker_size"]->text().toInt(), target_params_["marker_dist"]->text().toInt(), - target_params_["marker_border"]->text().toInt(), dictionary_id_->currentText().toStdString()); - target_->setTargetDimension(target_real_dims_["marker_size_real"]->text().toDouble(), - target_real_dims_["marker_dist_real"]->text().toDouble()); - } -} - void TargetTabWidget::saveTargetImageBtnClicked(bool clicked) { if (target_image_.empty()) diff --git a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp index 237a086..807a9e5 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp +++ b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp @@ -107,7 +107,7 @@ bool HandEyeCharucoTarget::setTargetIntrinsicParams(int squares_x, int squares_y << "squares_y " << std::to_string(squares_y) << "\n" << "marker_size_pixels " << std::to_string(marker_size_pixels) << "\n" << "square_size_pixels " << std::to_string(square_size_pixels) << "\n" - << "border_size_bits" << std::to_string(border_size_bits) << "\n" + << "border_size_bits " << std::to_string(border_size_bits) << "\n" << "margin_size_pixels " << std::to_string(margin_size_pixels) << "\n" << "dictionary_id " << dictionary_id << "\n"); return false; @@ -150,6 +150,8 @@ bool HandEyeCharucoTarget::setTargetDimension(double board_size_meters, double m bool HandEyeCharucoTarget::createTargetImage(cv::Mat& image) const { + if (!target_params_ready_) + return false; cv::Size image_size; image_size.width = squares_x_ * square_size_pixels_ + 2 * margin_size_pixels_; image_size.height = squares_y_ * square_size_pixels_ + 2 * margin_size_pixels_; @@ -175,6 +177,8 @@ bool HandEyeCharucoTarget::createTargetImage(cv::Mat& image) const bool HandEyeCharucoTarget::detectTargetPose(cv::Mat& image) { + if (!target_params_ready_) + return false; std::lock_guard base_lock(base_mutex_); try { From 55ca5be79386837c3e6aea5d52bbfa1740caa7f2 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Thu, 18 Jun 2020 13:40:41 -0600 Subject: [PATCH 12/26] not sure--lots of repeats from charuco-board branch --- .../moveit/handeye_calibration_target/handeye_target_aruco.h | 1 + .../moveit/handeye_calibration_target/handeye_target_charuco.h | 1 + 2 files changed, 2 insertions(+) diff --git a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_aruco.h b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_aruco.h index 8ee681e..833793e 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_aruco.h +++ b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_aruco.h @@ -56,6 +56,7 @@ class HandEyeArucoTarget : public HandEyeTargetBase virtual bool detectTargetPose(cv::Mat& image) override; +protected: virtual bool setTargetIntrinsicParams(int markers_x, int markers_y, int marker_size, int separation, int border_bits, const std::string& dictionary_id); diff --git a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_charuco.h b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_charuco.h index 865bd3a..4806072 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_charuco.h +++ b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_charuco.h @@ -56,6 +56,7 @@ class HandEyeCharucoTarget : public HandEyeTargetBase virtual bool detectTargetPose(cv::Mat& image) override; +protected: virtual bool setTargetIntrinsicParams(int markers_x, int markers_y, int marker_size_pixels, int square_size_pixels, int border_size_bits, int margin_size_pixels, const std::string& dictionary_id); From 80c426677967c762ee19447e745afd02479a9904 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Wed, 24 Jun 2020 16:54:52 -0600 Subject: [PATCH 13/26] added rviz_visual_tools to CMakeLists and package.xml --- moveit_calibration_gui/CMakeLists.txt | 2 ++ moveit_calibration_gui/package.xml | 1 + 2 files changed, 3 insertions(+) diff --git a/moveit_calibration_gui/CMakeLists.txt b/moveit_calibration_gui/CMakeLists.txt index e0e67c6..a889ea1 100644 --- a/moveit_calibration_gui/CMakeLists.txt +++ b/moveit_calibration_gui/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(catkin REQUIRED COMPONENTS rosconsole roscpp rviz + rviz_visual_tools tf2_eigen ) @@ -57,6 +58,7 @@ catkin_package( moveit_calibration_plugins roscpp rviz + rviz_visual_tools DEPENDS EIGEN3 ) diff --git a/moveit_calibration_gui/package.xml b/moveit_calibration_gui/package.xml index 87439a4..ee9c6e8 100644 --- a/moveit_calibration_gui/package.xml +++ b/moveit_calibration_gui/package.xml @@ -31,6 +31,7 @@ rosconsole roscpp rviz + rviz_visual_tools tf2_eigen rostest From 774b4af9861c1b5209de660cd7a27af2944c3e55 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Fri, 26 Jun 2020 11:16:51 -0600 Subject: [PATCH 14/26] saving and loading config works --- .../src/handeye_target_widget.cpp | 142 ++++++++++-------- 1 file changed, 83 insertions(+), 59 deletions(-) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp index 74e0538..538e55b 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp @@ -161,71 +161,95 @@ TargetTabWidget::TargetTabWidget(QWidget* parent) void TargetTabWidget::loadWidget(const rviz::Config& config) { - // TODO: get all this to work - // if (target_type_->count() > 0) - //{ - // QString type; - // if (config.mapGetString("target_type", &type) && target_type_->findText(type, Qt::MatchCaseSensitive) != -1) - // target_type_->setCurrentText(type); - - //} - - // int param_int; - // for (const std::pair& param : target_params_) - // if (config.mapGetInt(param.first.c_str(), ¶m_int)) - // param.second->setText(std::to_string(param_int).c_str()); - - // for (const std::pair& topic : ros_topics_) - //{ - // QString topic_name; - // if (config.mapGetString(topic.first.c_str(), &topic_name)) - // { - // if (topic.second->hasTopic(topic_name)) - // { - // topic.second->setCurrentText(topic_name); - // try - // { - // if (!topic.first.compare("image_topic")) - // { - // image_sub_.shutdown(); - // image_sub_ = it_.subscribe(topic_name.toStdString(), 1, &TargetTabWidget::imageCallback, this); - // } - - // if (!topic.first.compare("camera_info_topic")) - // { - // camerainfo_sub_.shutdown(); - // camerainfo_sub_ = nh_.subscribe(topic_name.toStdString(), 1, &TargetTabWidget::cameraInfoCallback, this); - // } - // } - // catch (const image_transport::TransportLoadException& e) - // { - // ROS_ERROR_STREAM_NAMED(LOGNAME, "Subscribe to " << topic_name.toStdString() << " fail: " << e.what()); - // } - // } - // } - //} - - // for (const std::pair& param : target_real_dims_) - //{ - // float param_double; - // if (config.mapGetFloat(param.first.c_str(), ¶m_double)) - // param.second->setText(std::to_string(param_double).c_str()); - //} + if (target_type_->count() > 0) + { + QString type; + if (config.mapGetString("target_type", &type) && target_type_->findText(type, Qt::MatchCaseSensitive) != -1) + { + target_type_->setCurrentText(type); + targetTypeComboboxChanged(type); + } + } + + int param_int; + float param_float; + QString param_enum; + for (const moveit_handeye_calibration::HandEyeTargetBase::Parameter& param : target_plugin_params_) + { + switch (param.parameter_type_) + { + case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Int: + if (config.mapGetInt(param.name_.c_str(), ¶m_int)) + static_cast(target_param_inputs_[param.name_])->setText(std::to_string(param_int).c_str()); + break; + case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Float: + if (config.mapGetFloat(param.name_.c_str(), ¶m_float)) + static_cast(target_param_inputs_[param.name_])->setText(std::to_string(param_float).c_str()); + break; + case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Enum: + if (config.mapGetString(param.name_.c_str(), ¶m_enum)) + { + int index = static_cast(target_param_inputs_[param.name_])->findText(param_enum); + static_cast(target_param_inputs_[param.name_])->setCurrentIndex(index); + } + break; + } + } + + for (const std::pair& topic : ros_topics_) + { + QString topic_name; + if (config.mapGetString(topic.first.c_str(), &topic_name)) + { + if (topic.second->hasTopic(topic_name)) + { + topic.second->setCurrentText(topic_name); + try + { + if (!topic.first.compare("image_topic")) + { + image_sub_.shutdown(); + image_sub_ = it_.subscribe(topic_name.toStdString(), 1, &TargetTabWidget::imageCallback, this); + } + + if (!topic.first.compare("camera_info_topic")) + { + camerainfo_sub_.shutdown(); + camerainfo_sub_ = nh_.subscribe(topic_name.toStdString(), 1, &TargetTabWidget::cameraInfoCallback, this); + } + } + catch (const image_transport::TransportLoadException& e) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Subscribe to " << topic_name.toStdString() << " fail: " << e.what()); + } + } + } + } } void TargetTabWidget::saveWidget(rviz::Config& config) { - // TODO: get all this to work - // config.mapSetValue("target_type", target_type_->currentText()); - // config.mapSetValue("dictionary", dictionary_id_->currentText()); - // for (const std::pair& param : target_params_) - // config.mapSetValue(param.first.c_str(), param.second->text().toInt()); + config.mapSetValue("target_type", target_type_->currentText()); - // for (const std::pair& topic : ros_topics_) - // config.mapSetValue(topic.first.c_str(), topic.second->currentText()); + QString param_value; + for (const moveit_handeye_calibration::HandEyeTargetBase::Parameter& param : target_plugin_params_) + { + switch (param.parameter_type_) + { + case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Int: + case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Float: + param_value = static_cast(target_param_inputs_[param.name_])->text(); + config.mapSetValue(param.name_.c_str(), param_value); + break; + case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Enum: + param_value = static_cast(target_param_inputs_[param.name_])->currentText(); + config.mapSetValue(param.name_.c_str(), param_value); + break; + } + } - // for (const std::pair& param : target_real_dims_) - // config.mapSetValue(param.first.c_str(), param.second->text().toDouble()); + for (const std::pair& topic : ros_topics_) + config.mapSetValue(topic.first.c_str(), topic.second->currentText()); } bool TargetTabWidget::loadAvailableTargetPlugins() From 3109a10f003a5f1d3da0ddcff8b0619e6f033f3e Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Thu, 2 Jul 2020 10:39:32 -0600 Subject: [PATCH 15/26] check for OpenCV 3.(2+) --- .../handeye_calibration_target/src/handeye_target_aruco.cpp | 2 +- .../handeye_calibration_target/src/handeye_target_charuco.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp index af3d9ca..abad1da 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp +++ b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp @@ -176,7 +176,7 @@ bool HandEyeArucoTarget::detectTargetPose(cv::Mat& image) cv::aruco::GridBoard::create(markers_x_, markers_y_, marker_size_real_, marker_separation_real_, dictionary); aruco_mutex_.unlock(); cv::Ptr params_ptr(new cv::aruco::DetectorParameters()); -#if CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION == 2 +#if CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >= 2 params_ptr->doCornerRefinement = true; #else params_ptr->cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE; diff --git a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp index 807a9e5..4898921 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp +++ b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp @@ -190,7 +190,7 @@ bool HandEyeCharucoTarget::detectTargetPose(cv::Mat& image) cv::aruco::CharucoBoard::create(squares_x_, squares_y_, square_size_meters, marker_size_meters_, dictionary); charuco_mutex_.unlock(); cv::Ptr params_ptr(new cv::aruco::DetectorParameters()); -#if CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION == 2 +#if CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >= 2 params_ptr->doCornerRefinement = true; #else params_ptr->cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE; From 3c2fc4c076ca526c8991b4ee2b3e5622d20054d3 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Thu, 2 Jul 2020 11:23:54 -0600 Subject: [PATCH 16/26] added default values to target parameters --- .../handeye_target_base.h | 32 +++++++++++++++++-- .../src/handeye_target_aruco.cpp | 16 +++++----- .../src/handeye_target_charuco.cpp | 18 +++++------ 3 files changed, 47 insertions(+), 19 deletions(-) diff --git a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h index ca1df32..a613bda 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h +++ b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h @@ -71,13 +71,41 @@ class HandEyeTargetBase } value_; const std::vector enum_values_; - Parameter(std::string name, ParameterType parameter_type) : name_(name), parameter_type_(parameter_type) + Parameter(std::string name, ParameterType parameter_type, int default_value = 0) + : name_(name), parameter_type_(parameter_type) { + if (parameter_type_ == ParameterType::Int) + value_.i = default_value; + else + ROS_ERROR("Integer default value specified for non-integer parameter %s", name.c_str()); } - Parameter(std::string name, ParameterType parameter_type, std::vector enum_values) + Parameter(std::string name, ParameterType parameter_type, float default_value = 0.) + : name_(name), parameter_type_(parameter_type) + { + if (parameter_type_ == ParameterType::Float) + value_.f = default_value; + else + ROS_ERROR("Float default value specified for non-float parameter %s", name.c_str()); + } + + Parameter(std::string name, ParameterType parameter_type, double default_value = 0.) + : name_(name), parameter_type_(parameter_type) + { + if (parameter_type_ == ParameterType::Float) + value_.f = default_value; + else + ROS_ERROR("Float default value specified for non-float parameter %s", name.c_str()); + } + + Parameter(std::string name, ParameterType parameter_type, std::vector enum_values, + size_t default_option = 0) : name_(name), parameter_type_(parameter_type), enum_values_(enum_values) { + if (default_option < enum_values_.size()) + value_.e = default_option; + else + ROS_ERROR("Invalid default option for enum parameter %s", name.c_str()); } }; diff --git a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp index abad1da..18d3b96 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp +++ b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp @@ -51,19 +51,19 @@ const std::map ARUCO_DICTION HandEyeArucoTarget::HandEyeArucoTarget() { - parameters_.push_back(Parameter("markers, X", Parameter::ParameterType::Int)); - parameters_.push_back(Parameter("markers, Y", Parameter::ParameterType::Int)); - parameters_.push_back(Parameter("marker size (px)", Parameter::ParameterType::Int)); - parameters_.push_back(Parameter("marker separation (px)", Parameter::ParameterType::Int)); - parameters_.push_back(Parameter("marker border (bits)", Parameter::ParameterType::Int)); + parameters_.push_back(Parameter("markers, X", Parameter::ParameterType::Int, 3)); + parameters_.push_back(Parameter("markers, Y", Parameter::ParameterType::Int, 4)); + parameters_.push_back(Parameter("marker size (px)", Parameter::ParameterType::Int, 200)); + parameters_.push_back(Parameter("marker separation (px)", Parameter::ParameterType::Int, 20)); + parameters_.push_back(Parameter("marker border (bits)", Parameter::ParameterType::Int, 1)); std::vector dictionaries; for (const auto& kv : ARUCO_DICTIONARY) { dictionaries.push_back(kv.first); } - parameters_.push_back(Parameter("ArUco dictionary", Parameter::ParameterType::Enum, dictionaries)); - parameters_.push_back(Parameter("measured marker size (m)", Parameter::ParameterType::Float)); - parameters_.push_back(Parameter("measured separation (m)", Parameter::ParameterType::Float)); + parameters_.push_back(Parameter("ArUco dictionary", Parameter::ParameterType::Enum, dictionaries, 1)); + parameters_.push_back(Parameter("measured marker size (m)", Parameter::ParameterType::Float, 0.2)); + parameters_.push_back(Parameter("measured separation (m)", Parameter::ParameterType::Float, 0.02)); } bool HandEyeArucoTarget::initialize() diff --git a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp index 4898921..acd9bd8 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp +++ b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp @@ -51,20 +51,20 @@ const std::map ARUCO_DICTION HandEyeCharucoTarget::HandEyeCharucoTarget() { - parameters_.push_back(Parameter("squares, X", Parameter::ParameterType::Int)); - parameters_.push_back(Parameter("squares, Y", Parameter::ParameterType::Int)); - parameters_.push_back(Parameter("marker size (px)", Parameter::ParameterType::Int)); - parameters_.push_back(Parameter("square size (px)", Parameter::ParameterType::Int)); - parameters_.push_back(Parameter("margin size (px)", Parameter::ParameterType::Int)); - parameters_.push_back(Parameter("marker border (bits)", Parameter::ParameterType::Int)); + parameters_.push_back(Parameter("squares, X", Parameter::ParameterType::Int, 5)); + parameters_.push_back(Parameter("squares, Y", Parameter::ParameterType::Int, 7)); + parameters_.push_back(Parameter("marker size (px)", Parameter::ParameterType::Int, 50)); + parameters_.push_back(Parameter("square size (px)", Parameter::ParameterType::Int, 80)); + parameters_.push_back(Parameter("margin size (px)", Parameter::ParameterType::Int, 2)); + parameters_.push_back(Parameter("marker border (bits)", Parameter::ParameterType::Int, 1)); std::vector dictionaries; for (const auto& kv : ARUCO_DICTIONARY) { dictionaries.push_back(kv.first); } - parameters_.push_back(Parameter("ArUco dictionary", Parameter::ParameterType::Enum, dictionaries)); - parameters_.push_back(Parameter("longest board side (m)", Parameter::ParameterType::Float)); - parameters_.push_back(Parameter("measured marker size (m)", Parameter::ParameterType::Float)); + parameters_.push_back(Parameter("ArUco dictionary", Parameter::ParameterType::Enum, dictionaries, 1)); + parameters_.push_back(Parameter("longest board side (m)", Parameter::ParameterType::Float, 0.56)); + parameters_.push_back(Parameter("measured marker size (m)", Parameter::ParameterType::Float, 0.06)); } bool HandEyeCharucoTarget::initialize() From 127c1deea5fc29f8e2e8eed34ca5bf1b187d738f Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Thu, 2 Jul 2020 11:31:26 -0600 Subject: [PATCH 17/26] load default target parameter values in gui --- .../src/handeye_target_widget.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp index 538e55b..87eee84 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp @@ -304,9 +304,14 @@ bool TargetTabWidget::loadInputWidgetsForTargetType(const std::string& plugin_na switch (param.parameter_type_) { case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Int: + target_param_inputs_.insert(std::make_pair(param.name_, new QLineEdit())); + target_param_layout_->addRow(param.name_.c_str(), target_param_inputs_[param.name_]); + static_cast(target_param_inputs_[param.name_])->setText(std::to_string(param.value_.i).c_str()); + break; case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Float: target_param_inputs_.insert(std::make_pair(param.name_, new QLineEdit())); target_param_layout_->addRow(param.name_.c_str(), target_param_inputs_[param.name_]); + static_cast(target_param_inputs_[param.name_])->setText(std::to_string(param.value_.f).c_str()); break; case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Enum: QComboBox* combo_box = new QComboBox(); @@ -316,6 +321,7 @@ bool TargetTabWidget::loadInputWidgetsForTargetType(const std::string& plugin_na } target_param_inputs_.insert(std::make_pair(param.name_, combo_box)); target_param_layout_->addRow(param.name_.c_str(), target_param_inputs_[param.name_]); + static_cast(target_param_inputs_[param.name_])->setCurrentIndex(param.value_.e); break; } } From df1bd19f8dfbcaf821eeaa5ea75c4067db585386 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Thu, 2 Jul 2020 11:41:00 -0600 Subject: [PATCH 18/26] load target parameter input widgets when panel is created --- .../src/handeye_target_widget.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp index 87eee84..d15d776 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp @@ -280,6 +280,7 @@ bool TargetTabWidget::loadAvailableTargetPlugins() for (const std::string& it : classes) target_type_->addItem(tr(it.c_str())); + loadInputWidgetsForTargetType(classes[0]); return true; } From 9998f36c3c303600b8995749b8e8d015bf7d35d2 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Thu, 2 Jul 2020 13:36:12 -0600 Subject: [PATCH 19/26] actual fix to OpenCV version issue --- moveit_calibration_plugins/CMakeLists.txt | 2 +- .../handeye_calibration_target/src/handeye_target_aruco.cpp | 2 +- .../handeye_calibration_target/src/handeye_target_charuco.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_calibration_plugins/CMakeLists.txt b/moveit_calibration_plugins/CMakeLists.txt index 1cc1ba4..64a476b 100644 --- a/moveit_calibration_plugins/CMakeLists.txt +++ b/moveit_calibration_plugins/CMakeLists.txt @@ -19,7 +19,7 @@ find_package(catkin REQUIRED COMPONENTS ) find_package(Eigen3 REQUIRED) -find_package(OpenCV REQUIRED) +find_package(OpenCV REQUIRED aruco) catkin_package( INCLUDE_DIRS diff --git a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp index 18d3b96..06dc1b8 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp +++ b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp @@ -176,7 +176,7 @@ bool HandEyeArucoTarget::detectTargetPose(cv::Mat& image) cv::aruco::GridBoard::create(markers_x_, markers_y_, marker_size_real_, marker_separation_real_, dictionary); aruco_mutex_.unlock(); cv::Ptr params_ptr(new cv::aruco::DetectorParameters()); -#if CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >= 2 +#if CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION == 2 params_ptr->doCornerRefinement = true; #else params_ptr->cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE; diff --git a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp index acd9bd8..1114316 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp +++ b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp @@ -190,7 +190,7 @@ bool HandEyeCharucoTarget::detectTargetPose(cv::Mat& image) cv::aruco::CharucoBoard::create(squares_x_, squares_y_, square_size_meters, marker_size_meters_, dictionary); charuco_mutex_.unlock(); cv::Ptr params_ptr(new cv::aruco::DetectorParameters()); -#if CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >= 2 +#if CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION == 2 params_ptr->doCornerRefinement = true; #else params_ptr->cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE; From 4a86db694f5fbe0e1414c0e6963c03b0d98df4e6 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Sun, 5 Jul 2020 21:19:03 -0600 Subject: [PATCH 20/26] trigger sensor mount update when loading panel --- .../src/handeye_calibration_gui.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_gui.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_gui.cpp index 9ab1344..75f0b3f 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_gui.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_gui.cpp @@ -70,6 +70,7 @@ HandEyeCalibrationGui::HandEyeCalibrationGui(QWidget* parent) : rviz::Panel(pare tab_control_ = new ControlTabWidget(); tab_control_->setTFTool(tf_tools_); + tab_control_->UpdateSensorMountType(0); connect(tab_context_, SIGNAL(sensorMountTypeChanged(int)), tab_control_, SLOT(UpdateSensorMountType(int))); connect(tab_context_, SIGNAL(frameNameChanged(std::map)), tab_control_, SLOT(updateFrameNames(std::map))); @@ -106,4 +107,4 @@ void HandEyeCalibrationGui::load(const rviz::Config& config) ROS_INFO_STREAM("handeye calibration gui loaded."); } -} // namespace moveit_rviz_plugin \ No newline at end of file +} // namespace moveit_rviz_plugin From 2c3a18c26d657d6f6196aad4337d4673d4762098 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Tue, 4 Aug 2020 21:41:34 -0600 Subject: [PATCH 21/26] reload saved solver option --- .../src/handeye_control_widget.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp index 99b9c5d..e43cd78 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp @@ -255,6 +255,20 @@ void ControlTabWidget::loadWidget(const rviz::Config& config) Q_EMIT group_name_->activated(group_name); } } + QString solver_name; + config.mapGetString("solver", &solver_name); + if (!solver_name.isEmpty()) + { + for (size_t i = 0; i < calibration_solver_->count(); ++i) + { + if (calibration_solver_->itemText(i) == solver_name) + { + calibration_solver_->setCurrentText(solver_name); + Q_EMIT calibration_solver_->activated(solver_name); + break; + } + } + } } void ControlTabWidget::saveWidget(rviz::Config& config) From dac9c5e1039d6db5e3cfbadf36f177d34621b63e Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Tue, 4 Aug 2020 22:10:31 -0600 Subject: [PATCH 22/26] more comments from Felix --- .../src/handeye_control_widget.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp index e43cd78..6e144d5 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp @@ -97,6 +97,7 @@ ControlTabWidget::ControlTabWidget(QWidget* parent) , tf_buffer_(new tf2_ros::Buffer()) , tf_listener_(*tf_buffer_) , sensor_mount_type_(mhc::EYE_TO_HAND) + , from_frame_tag_("base") , solver_plugins_loader_(nullptr) , solver_(nullptr) , move_group_(nullptr) @@ -185,13 +186,13 @@ ControlTabWidget::ControlTabWidget(QWidget* parent) auto_cal_layout->addLayout(auto_btns_layout); auto_plan_btn_ = new QPushButton("Plan"); auto_plan_btn_->setMinimumHeight(35); - auto_plan_btn_->setToolTip("Start or resume auto calibration process"); + auto_plan_btn_->setToolTip("Plan next calibration pose"); connect(auto_plan_btn_, SIGNAL(clicked(bool)), this, SLOT(autoPlanBtnClicked(bool))); auto_btns_layout->addWidget(auto_plan_btn_); auto_execute_btn_ = new QPushButton("Execute"); auto_execute_btn_->setMinimumHeight(35); - auto_execute_btn_->setToolTip("Pause the auto calibration process"); + auto_execute_btn_->setToolTip("Execute the planned motion to next calibration pose"); connect(auto_execute_btn_, SIGNAL(clicked(bool)), this, SLOT(autoExecuteBtnClicked(bool))); auto_btns_layout->addWidget(auto_execute_btn_); @@ -398,6 +399,10 @@ bool ControlTabWidget::solveCameraRobotPose() return false; } } + else + { + QMessageBox::warning(this, tr("Solver Failed"), tr("Solver failed to return a calibration.")); + } } else { @@ -541,7 +546,8 @@ void ControlTabWidget::saveCameraPoseBtnClicked(bool clicked) if (from_frame.empty() || to_frame.empty()) { - QMessageBox::warning(this, tr("Empty Frame Name"), tr("Make sure you have set correct frame names.")); + QMessageBox::warning(this, tr("Empty Frame Name"), + tr("Make sure you have selected the correct frames in the Context tab.")); return; } From c256ca65f10e5493ff0d36b3163ba289066e5e6b Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Tue, 4 Aug 2020 22:26:24 -0600 Subject: [PATCH 23/26] more comments --- .../handeye_control_widget.h | 10 +++--- .../src/handeye_control_widget.cpp | 36 +++++++++---------- 2 files changed, 23 insertions(+), 23 deletions(-) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h index fc6794e..4034bc8 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h @@ -117,8 +117,8 @@ class ControlTabWidget : public QWidget void setTFTool(rviz_visual_tools::TFVisualToolsPtr& tf_pub); - void addPoseSampleToTreeView(const geometry_msgs::TransformStamped& cTo, const geometry_msgs::TransformStamped& bTe, - int id); + void addPoseSampleToTreeView(const geometry_msgs::TransformStamped& camera_to_object_tf, + const geometry_msgs::TransformStamped& base_to_eef_tf, int id); bool loadSolverPlugin(std::vector& plugins); @@ -128,7 +128,7 @@ class ControlTabWidget : public QWidget std::string parseSolverName(const std::string& solver_name, char delimiter); - bool takeTranformSamples(); + bool takeTransformSamples(); bool solveCameraRobotPose(); @@ -154,7 +154,7 @@ private Q_SLOTS: void takeSampleBtnClicked(bool clicked); - void resetSampleBtnClicked(bool clicked); + void clearSamplesBtnClicked(bool clicked); void saveCameraPoseBtnClicked(bool clicked); @@ -239,4 +239,4 @@ private Q_SLOTS: }; } // namespace moveit_rviz_plugin -#endif \ No newline at end of file +#endif diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp index 6e144d5..e156168 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp @@ -171,9 +171,9 @@ ControlTabWidget::ControlTabWidget(QWidget* parent) connect(take_sample_btn_, SIGNAL(clicked(bool)), this, SLOT(takeSampleBtnClicked(bool))); control_cal_layout->addWidget(take_sample_btn_); - reset_sample_btn_ = new QPushButton("Reset samples"); + reset_sample_btn_ = new QPushButton("Clear samples"); reset_sample_btn_->setMinimumHeight(35); - connect(reset_sample_btn_, SIGNAL(clicked(bool)), this, SLOT(resetSampleBtnClicked(bool))); + connect(reset_sample_btn_, SIGNAL(clicked(bool)), this, SLOT(clearSamplesBtnClicked(bool))); control_cal_layout->addWidget(reset_sample_btn_); // Auto calibration area @@ -341,25 +341,25 @@ std::string ControlTabWidget::parseSolverName(const std::string& solver_name, ch return tokens.back(); } -bool ControlTabWidget::takeTranformSamples() +bool ControlTabWidget::takeTransformSamples() { // Store the pair of two tf transforms and calculate camera_robot pose try { - geometry_msgs::TransformStamped cTo; - geometry_msgs::TransformStamped bTe; + geometry_msgs::TransformStamped camera_to_object_tf; + geometry_msgs::TransformStamped base_to_eef_tf; // Get the transform of the object w.r.t the camera - cTo = tf_buffer_->lookupTransform(frame_names_["sensor"], frame_names_["object"], ros::Time(0)); + camera_to_object_tf = tf_buffer_->lookupTransform(frame_names_["sensor"], frame_names_["object"], ros::Time(0)); // Get the transform of the end-effector w.r.t the robot base - bTe = tf_buffer_->lookupTransform(frame_names_["base"], frame_names_["eef"], ros::Time(0)); + base_to_eef_tf = tf_buffer_->lookupTransform(frame_names_["base"], frame_names_["eef"], ros::Time(0)); // save the pose samples - effector_wrt_world_.push_back(tf2::transformToEigen(bTe)); - object_wrt_sensor_.push_back(tf2::transformToEigen(cTo)); + effector_wrt_world_.push_back(tf2::transformToEigen(base_to_eef_tf)); + object_wrt_sensor_.push_back(tf2::transformToEigen(camera_to_object_tf)); - ControlTabWidget::addPoseSampleToTreeView(cTo, bTe, effector_wrt_world_.size()); + ControlTabWidget::addPoseSampleToTreeView(camera_to_object_tf, base_to_eef_tf, effector_wrt_world_.size()); } catch (tf2::TransformException& e) { @@ -406,7 +406,7 @@ bool ControlTabWidget::solveCameraRobotPose() } else { - ROS_ERROR_STREAM_NAMED(LOGNAME, "No available handeye calibration solver instance."); + QMessageBox::warning(this, tr("No Solver Available"), tr("No available handeye calibration solver instance.")); return false; } } @@ -440,8 +440,8 @@ void ControlTabWidget::setTFTool(rviz_visual_tools::TFVisualToolsPtr& tf_pub) tf_tools_ = tf_pub; } -void ControlTabWidget::addPoseSampleToTreeView(const geometry_msgs::TransformStamped& cTo, - const geometry_msgs::TransformStamped& bTe, int id) +void ControlTabWidget::addPoseSampleToTreeView(const geometry_msgs::TransformStamped& camera_to_object_tf, + const geometry_msgs::TransformStamped& base_to_eef_tf, int id) { std::string item_name = "Sample " + std::to_string(id); QStandardItem* parent = new QStandardItem(QString(item_name.c_str())); @@ -450,13 +450,13 @@ void ControlTabWidget::addPoseSampleToTreeView(const geometry_msgs::TransformSta std::ostringstream ss; QStandardItem* child_1 = new QStandardItem("TF base-to-eef"); - ss << bTe.transform; + ss << base_to_eef_tf.transform; child_1->appendRow(new QStandardItem(ss.str().c_str())); parent->appendRow(child_1); QStandardItem* child_2 = new QStandardItem("TF camera-to-target"); ss.str(""); - ss << cTo.transform; + ss << camera_to_object_tf.transform; child_2->appendRow(new QStandardItem(ss.str().c_str())); parent->appendRow(child_2); } @@ -491,7 +491,7 @@ void ControlTabWidget::updateFrameNames(std::map names void ControlTabWidget::takeSampleBtnClicked(bool clicked) { - if (frameNamesEmpty() || !takeTranformSamples()) + if (frameNamesEmpty() || !takeTransformSamples()) return; if (effector_wrt_world_.size() == object_wrt_sensor_.size() && effector_wrt_world_.size() > 4) @@ -526,7 +526,7 @@ void ControlTabWidget::takeSampleBtnClicked(bool clicked) } } -void ControlTabWidget::resetSampleBtnClicked(bool clicked) +void ControlTabWidget::clearSamplesBtnClicked(bool clicked) { // Clear recorded transforms effector_wrt_world_.clear(); @@ -832,7 +832,7 @@ void ControlTabWidget::executeFinished() { auto_progress_->setValue(auto_progress_->getValue() + 1); if (!frameNamesEmpty()) - takeTranformSamples(); + takeTransformSamples(); if (effector_wrt_world_.size() == object_wrt_sensor_.size() && effector_wrt_world_.size() > 4) solveCameraRobotPose(); From 04a95e1b61b2c1f3766f78ba5664ea3fa0622815 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Sun, 9 Aug 2020 23:30:19 -0600 Subject: [PATCH 24/26] auto calibration interface changes --- .../src/handeye_control_widget.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp index e156168..9344d20 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp @@ -46,7 +46,7 @@ ProgressBarWidget::ProgressBarWidget(QWidget* parent, int min, int max, int valu row->setContentsMargins(0, 10, 0, 10); // QLabel init - name_label_ = new QLabel("joint_state:", this); + name_label_ = new QLabel("Recorded joint state progress:", this); name_label_->setContentsMargins(0, 0, 0, 0); row->addWidget(name_label_); @@ -61,6 +61,7 @@ ProgressBarWidget::ProgressBarWidget(QWidget* parent, int min, int max, int valu bar_->setMaximum(max); bar_->setValue(value); bar_->setContentsMargins(0, 0, 0, 0); + bar_->setDisabled(max == 0); row->addWidget(bar_); max_label_ = new QLabel(QString::number(max), this); @@ -73,6 +74,7 @@ ProgressBarWidget::ProgressBarWidget(QWidget* parent, int min, int max, int valu void ProgressBarWidget::setMax(int value) { bar_->setMaximum(value); + bar_->setDisabled(value == 0); max_label_->setText(QString::number(value)); } @@ -177,7 +179,7 @@ ControlTabWidget::ControlTabWidget(QWidget* parent) control_cal_layout->addWidget(reset_sample_btn_); // Auto calibration area - QGroupBox* auto_cal_group = new QGroupBox("Auto Calibration"); + QGroupBox* auto_cal_group = new QGroupBox("Calibrate With Recorded Joint States"); layout_right->addWidget(auto_cal_group); QVBoxLayout* auto_cal_layout = new QVBoxLayout(); auto_cal_group->setLayout(auto_cal_layout); From 124cc07b39c7a0fc3b01fe8e1db6145d26f34fc6 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Mon, 10 Aug 2020 00:13:57 -0600 Subject: [PATCH 25/26] more descriptive error messages --- .../handeye_control_widget.h | 14 ++++- .../src/handeye_control_widget.cpp | 62 ++++++++++++++----- 2 files changed, 61 insertions(+), 15 deletions(-) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h index 4034bc8..7710a65 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h @@ -100,6 +100,18 @@ class ProgressBarWidget : public QWidget class ControlTabWidget : public QWidget { Q_OBJECT + + enum PLANNING_RESULT + { + SUCCESS = 0, + FAILURE_NO_JOINT_STATE = 1, + FAILURE_INVALID_JOINT_STATE = 2, + FAILURE_NO_PSM = 3, + FAILURE_NO_MOVE_GROUP = 4, + FAILURE_WRONG_MOVE_GROUP = 5, + FAILURE_PLAN_FAILED = 6 + }; + public: explicit ControlTabWidget(QWidget* parent = Q_NULLPTR); ~ControlTabWidget() @@ -219,7 +231,7 @@ private Q_SLOTS: std::vector> joint_states_; std::vector joint_names_; bool auto_started_; - bool planning_res_; + PLANNING_RESULT planning_res_; // ************************************************************** // Ros components diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp index 9344d20..ed0bd1d 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp @@ -105,7 +105,7 @@ ControlTabWidget::ControlTabWidget(QWidget* parent) , move_group_(nullptr) , camera_robot_pose_(Eigen::Isometry3d::Identity()) , auto_started_(false) - , planning_res_(false) + , planning_res_(ControlTabWidget::SUCCESS) // spinner_(0, &callback_queue_) { QVBoxLayout* layout = new QVBoxLayout(); @@ -742,30 +742,36 @@ void ControlTabWidget::autoPlanBtnClicked(bool clicked) void ControlTabWidget::computePlan() { - planning_res_ = true; + planning_res_ = ControlTabWidget::SUCCESS; int max = auto_progress_->bar_->maximum(); if (max != joint_states_.size() || auto_progress_->getValue() == max) { - planning_res_ = false; + planning_res_ = ControlTabWidget::FAILURE_NO_JOINT_STATE; return; } if (!checkJointStates()) { - planning_res_ = false; + planning_res_ = ControlTabWidget::FAILURE_INVALID_JOINT_STATE; return; } if (!planning_scene_monitor_) { - planning_res_ = false; + planning_res_ = ControlTabWidget::FAILURE_NO_PSM; return; } - if (!move_group_ || move_group_->getActiveJoints() != joint_names_) + if (!move_group_) { - planning_res_ = false; + planning_res_ = ControlTabWidget::FAILURE_NO_MOVE_GROUP; + return; + } + + if (move_group_->getActiveJoints() != joint_names_) + { + planning_res_ = ControlTabWidget::FAILURE_WRONG_MOVE_GROUP; return; } @@ -785,9 +791,11 @@ void ControlTabWidget::computePlan() move_group_->setMaxVelocityScalingFactor(0.5); move_group_->setMaxAccelerationScalingFactor(0.5); current_plan_.reset(new moveit::planning_interface::MoveGroupInterface::Plan()); - planning_res_ = (move_group_->plan(*current_plan_) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + planning_res_ = (move_group_->plan(*current_plan_) == moveit::planning_interface::MoveItErrorCode::SUCCESS) ? + ControlTabWidget::SUCCESS : + ControlTabWidget::FAILURE_PLAN_FAILED; - if (planning_res_) + if (planning_res_ == ControlTabWidget::SUCCESS) ROS_DEBUG_STREAM_NAMED(LOGNAME, "Planning succeed."); else ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning failed."); @@ -808,9 +816,11 @@ void ControlTabWidget::autoExecuteBtnClicked(bool clicked) void ControlTabWidget::computeExecution() { if (move_group_ && current_plan_) - planning_res_ = (move_group_->execute(*current_plan_) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + planning_res_ = (move_group_->execute(*current_plan_) == moveit::planning_interface::MoveItErrorCode::SUCCESS) ? + ControlTabWidget::SUCCESS : + ControlTabWidget::FAILURE_PLAN_FAILED; - if (planning_res_) + if (planning_res_ == ControlTabWidget::SUCCESS) { ROS_DEBUG_STREAM_NAMED(LOGNAME, "Execution succeed."); } @@ -821,9 +831,33 @@ void ControlTabWidget::computeExecution() void ControlTabWidget::planFinished() { auto_plan_btn_->setEnabled(true); - if (!planning_res_) - QMessageBox::warning(this, tr("Error"), - tr("Please check if move_group is started or there are recorded joint states.")); + switch (planning_res_) + { + case ControlTabWidget::FAILURE_NO_JOINT_STATE: + QMessageBox::warning(this, tr("Error"), + tr("Could not compute plan. No more prerecorded joint states to execute.")); + break; + case ControlTabWidget::FAILURE_INVALID_JOINT_STATE: + QMessageBox::warning(this, tr("Error"), + tr("Could not compute plan. Invalid joint states (names wrong or missing).")); + break; + case ControlTabWidget::FAILURE_NO_PSM: + QMessageBox::warning(this, tr("Error"), tr("Could not compute plan. No planning scene monitor.")); + break; + case ControlTabWidget::FAILURE_NO_MOVE_GROUP: + QMessageBox::warning(this, tr("Error"), tr("Could not compute plan. Missing move_group.")); + break; + case ControlTabWidget::FAILURE_WRONG_MOVE_GROUP: + QMessageBox::warning( + this, tr("Error"), + tr("Could not compute plan. Joint names for recorded state do not match names from current planning group.")); + break; + case ControlTabWidget::FAILURE_PLAN_FAILED: + QMessageBox::warning(this, tr("Error"), tr("Could not compute plan. Planning failed.")); + break; + case ControlTabWidget::SUCCESS: + break; + } ROS_DEBUG_NAMED(LOGNAME, "Plan finished"); } From 8ff0ee7d1bba3486016329a7278b6d3e2963a7ee Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Mon, 10 Aug 2020 09:32:24 -0600 Subject: [PATCH 26/26] not sure why this wasn't generating an error --- .../src/handeye_control_widget.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp index ed0bd1d..bdbed0b 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp @@ -404,6 +404,7 @@ bool ControlTabWidget::solveCameraRobotPose() else { QMessageBox::warning(this, tr("Solver Failed"), tr("Solver failed to return a calibration.")); + return false; } } else