diff --git a/.github/workflows/ros-humble.yaml b/.github/workflows/ros-humble.yaml new file mode 100644 index 00000000..559cea03 --- /dev/null +++ b/.github/workflows/ros-humble.yaml @@ -0,0 +1,23 @@ +name: ROS2 Humble + +on: + push: + branches: [ main ] + pull_request: + branches: [ main ] + workflow_dispatch: + +jobs: + build: + runs-on: ubuntu-22.04 + steps: + # Set up ROS + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: humble + + # Run ROS CI + - uses: ros-tooling/action-ros-ci@v0.4 + with: + package-name: plotjuggler_ros + target-ros2-distro: humble diff --git a/.github/workflows/ros-jazzy.yaml b/.github/workflows/ros-jazzy.yaml new file mode 100644 index 00000000..56d2d900 --- /dev/null +++ b/.github/workflows/ros-jazzy.yaml @@ -0,0 +1,23 @@ +name: ROS2 Jazzy + +on: + push: + branches: [ main ] + pull_request: + branches: [ main ] + workflow_dispatch: + +jobs: + build: + runs-on: ubuntu-24.04 + steps: + # Set up ROS + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: jazzy + + # Run ROS CI + - uses: ros-tooling/action-ros-ci@v0.4 + with: + package-name: plotjuggler_ros + target-ros2-distro: jazzy diff --git a/.github/workflows/ros-rolling.yaml b/.github/workflows/ros-rolling.yaml new file mode 100644 index 00000000..e17fa729 --- /dev/null +++ b/.github/workflows/ros-rolling.yaml @@ -0,0 +1,23 @@ +name: ROS2 Rolling + +on: + push: + branches: [ main ] + pull_request: + branches: [ main ] + workflow_dispatch: + +jobs: + build: + runs-on: ubuntu-24.04 + steps: + # Set up ROS + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: rolling + + # Run ROS CI + - uses: ros-tooling/action-ros-ci@v0.4 + with: + package-name: plotjuggler_ros + target-ros2-distro: rolling diff --git a/.github/workflows/ros1.yaml b/.github/workflows/ros1.yaml deleted file mode 100644 index 91bf0c28..00000000 --- a/.github/workflows/ros1.yaml +++ /dev/null @@ -1,17 +0,0 @@ -name: ros1 - -on: [push, pull_request] - -jobs: - industrial_ci: - strategy: - matrix: - env: - - {ROS_DISTRO: noetic, ROS_REPO: main} - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v1 - - uses: 'ros-industrial/industrial_ci@master' - env: ${{matrix.env}} - with: - package-name: plotjuggler_ros diff --git a/.github/workflows/ros2.yaml b/.github/workflows/ros2.yaml deleted file mode 100644 index b7cb9067..00000000 --- a/.github/workflows/ros2.yaml +++ /dev/null @@ -1,19 +0,0 @@ -name: ros2 - -on: [push, pull_request] - -jobs: - industrial_ci: - strategy: - matrix: - env: - - {ROS_DISTRO: jazzy, ROS_REPO: main} - - {ROS_DISTRO: kilted, ROS_REPO: main} - - {ROS_DISTRO: rolling, ROS_REPO: main} - runs-on: ubuntu-24.04 - steps: - - uses: actions/checkout@v1 - - uses: 'ros-industrial/industrial_ci@master' - env: ${{matrix.env}} - with: - package-name: plotjuggler_ros diff --git a/CMakeLists.txt b/CMakeLists.txt index 8eda4dfd..506bbf9e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,49 +2,15 @@ cmake_minimum_required(VERSION 3.5) PROJECT(plotjuggler_ros) -find_package(ament_cmake QUIET) -find_package(catkin QUIET) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcpputils REQUIRED) +find_package(rosbag2_transport REQUIRED) +find_package(Boost REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(plotjuggler REQUIRED) -# http://answers.ros.org/question/230877/optionally-build-a-package-with-catkin/ -if( catkin_FOUND OR CATKIN_DEVEL_PREFIX OR CATKIN_BUILD_BINARY_PACKAGE) - set(COMPILING_WITH_CATKIN 1) - - message(STATUS "---------------------------------------------------------------------") - message(STATUS "PlotJuggler is being built using CATKIN. ROS plugins will be compiled") - message(STATUS "---------------------------------------------------------------------") - - set(ROS_DEPENDENCIES - rosbag_storage - roscpp - roscpp_serialization - ros_type_introspection - tf2_ros - plotjuggler ) - - find_package(catkin REQUIRED COMPONENTS ${ROS_DEPENDENCIES} ) - - catkin_package( - CATKIN_DEPENDS ${ROS_DEPENDENCIES} ) - -elseif( ament_cmake_FOUND ) - set(COMPILING_WITH_AMENT 1) - - message(STATUS "---------------------------------------------------------------------") - message(STATUS "PlotJuggler is being built using AMENT. ROS2 plugins will be compiled") - message(STATUS "---------------------------------------------------------------------") - - find_package(rclcpp REQUIRED) - find_package(rcpputils REQUIRED) - find_package(rosbag2 REQUIRED) - find_package(rosbag2_transport REQUIRED) - find_package(Boost REQUIRED) - find_package(tf2_msgs REQUIRED) - find_package(tf2_ros REQUIRED) - find_package(plotjuggler REQUIRED) - -else() - message(FATAL_ERROR "PlotJuggler is being WITHOUT any ROS support") -endif() cmake_policy (SET CMP0020 NEW) @@ -96,47 +62,20 @@ IF (NOT WIN32) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") ENDIF() -include_directories( - 3rdparty - ${Qt5Core_INCLUDE_DIRS} - ${Qt5Widgets_INCLUDE_DIRS} - ${Qt5Concurrent_INCLUDE_DIRS} - ${Qt5Xml_INCLUDE_DIRS} - ${Qt5WebSockets_INCLUDE_DIRS} - ${Qt5Svg_INCLUDE_DIRS} -) add_definitions(-DFMT_HEADER_ONLY) ######################### INSTALL #################################### -if(COMPILING_WITH_CATKIN) - set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}) - set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}) - set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}) - install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME}) +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) -elseif(COMPILING_WITH_AMENT) - find_package(ament_cmake REQUIRED) - set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) - set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) - - install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}) -else() - set(CMAKE_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX}) - set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) - set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin/plotjuggler) -endif() - -############################################################# - add_subdirectory( src ) -if(COMPILING_WITH_AMENT) - ament_package() -endif() +ament_package() + diff --git a/README.md b/README.md index 2eab04f7..2d87d077 100644 --- a/README.md +++ b/README.md @@ -14,19 +14,15 @@ ROS is supported through external plugins that can be found in this [repository] - **Re-publisher** similar to `rosbag play` (ROS only). -## Install with Debians (TODO) +## Install with Debians Install PlotJuggler and its ROS plugins with: sudo apt install ros-${ROS_DISTRO}-plotjuggler-ros - -To launch PlotJuggler on ROS, use the command: - - rosrun plotjuggler plotjuggler or, if have ROS2 installed: - ros2 run plotjuggler plotjuggler + ros2 run plotjuggler plotjuggler ## How to compile PlotJuggler from source @@ -41,12 +37,12 @@ Create a catkin workspace and clone the repositories: Now, it is time to compile: cd ~/ws_plotjuggler - rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y - catkin build - -Enjoy: + rosdep install --from-paths src --ignore-src -y + colcon build + +# For ROS (1) users + +From version 2.3+, this library will support only ROS2 (Hubmle or older) - source devel/setup.bash - roslaunch plotjuggler_ros plotjuggler.launch diff --git a/package.xml b/package.xml index 4e6060b4..46ed8f6e 100644 --- a/package.xml +++ b/package.xml @@ -11,22 +11,12 @@ Davide Faconti - - catkin - - rosbag_storage - roscpp - roscpp_serialization - ros_type_introspection - - ament_cmake + ament_cmake - ros_environment - rclcpp - rcpputils - rosbag2 - rosbag2_transport + rclcpp + rcpputils + rosbag2_transport plotjuggler tf2_ros @@ -42,7 +32,6 @@ - catkin - ament_cmake + ament_cmake diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3fbcac7d..fb688685 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -8,115 +8,55 @@ QT5_WRAP_UI ( COMMON_UI_SRC dialog_with_itemlist.ui publisher_select_dialog.ui ) -SET( COMMON_SRC - dialog_select_ros_topics.h + +add_library( commonROS STATIC + dialog_select_ros_topics.h dialog_select_ros_topics.cpp dialog_with_itemlist.h publisher_select_dialog.h parser_configuration.cpp - parser_configuration.h ) - -############# - -include_directories( - ${catkin_INCLUDE_DIRS} - ${plotjuggler_INCLUDE_DIRS} - ${CMAKE_CURRENT_SOURCE_DIR} ) - -if(COMPILING_WITH_CATKIN) - - add_library( commonROS STATIC - ${COMMON_SRC} - ${COMMON_UI_SRC} - qnodedialog.cpp) + parser_configuration.h + ros_parsers/ros2_parser.cpp + ${COMMON_UI_SRC} +) + +target_link_libraries( commonROS + PUBLIC + Qt5::Widgets + Qt5::Xml + rclcpp::rclcpp + rcpputils::rcpputils + rosbag2_transport::rosbag2_transport + tf2_ros::tf2_ros + plotjuggler::plotjuggler_base + ${tf2_msgs_TARGETS} + ) - target_link_libraries( commonROS - ${Qt5Widgets_LIBRARIES} - ${Qt5Xml_LIBRARIES} - ${catkin_LIBRARIES} - ${plotjuggler_LIBRARIES}) - - ######### - add_library( DataLoadROS SHARED - DataLoadROS/dataload_ros.cpp) - target_link_libraries( DataLoadROS commonROS) - ############ - add_library( DataStreamROS SHARED - DataStreamROS/datastream_ROS.cpp ) - target_link_libraries( DataStreamROS commonROS ) - ############# - add_library( RosTopicPublisher SHARED - TopicPublisherROS/statepublisher_rostopic.cpp) - target_link_libraries( RosTopicPublisher commonROS ) - ############# - QT5_WRAP_UI ( ROSOUT_UI RosoutPublisher/logwidget.ui ) - add_library( RosoutPublisher SHARED ${ROSOUT_UI} - RosoutPublisher/rosout_publisher.cpp - RosoutPublisher/logs_table_model.cpp - RosoutPublisher/logwidget.cpp - RosoutPublisher/modelfilter.cpp - ) - target_link_libraries( RosoutPublisher commonROS ) ####################################################################### -elseif(COMPILING_WITH_AMENT) +add_library( DataLoadROS2 SHARED DataLoadROS2/dataload_ros2.cpp) +target_link_libraries( DataLoadROS2 commonROS) - set(AMENT_DEPENDENCIES - rclcpp - rosbag2 - rcpputils - rosbag2_transport - tf2_msgs - tf2_ros - plotjuggler ) - add_library( commonROS STATIC - ${COMMON_SRC} - ${COMMON_UI_SRC} - ros_parsers/ros2_parser.cpp ) +add_library( DataStreamROS2 SHARED DataStreamROS2/datastream_ros2.cpp) +target_link_libraries( DataStreamROS2 commonROS) - target_link_libraries( commonROS - ${Qt5Widgets_LIBRARIES} - ${Qt5Xml_LIBRARIES} - ${plotjuggler_LIBRARIES}) - ament_target_dependencies( commonROS ${AMENT_DEPENDENCIES}) - ############# - add_library( DataLoadROS2 SHARED - DataLoadROS2/dataload_ros2.cpp) - target_link_libraries( DataLoadROS2 commonROS) - ament_target_dependencies( DataLoadROS2 ${AMENT_DEPENDENCIES}) - ############# - add_library( DataStreamROS2 SHARED - DataStreamROS2/datastream_ros2.cpp) - target_link_libraries( DataStreamROS2 commonROS) - ament_target_dependencies( DataStreamROS2 ${AMENT_DEPENDENCIES}) - ############# - add_library( TopicPublisherROS2 SHARED - TopicPublisherROS2/publisher_ros2.cpp) - target_link_libraries( TopicPublisherROS2 commonROS) - ament_target_dependencies( TopicPublisherROS2 ${AMENT_DEPENDENCIES}) +add_library( TopicPublisherROS2 SHARED TopicPublisherROS2/publisher_ros2.cpp) +target_link_libraries( TopicPublisherROS2 commonROS) - if("$ENV{ROS_DISTRO}" STREQUAL "humble") +if("$ENV{ROS_DISTRO}" STREQUAL "humble") message(STATUS "Detected Humble") target_compile_definitions(DataLoadROS2 PUBLIC ROS_HUMBLE) target_compile_definitions(TopicPublisherROS2 PUBLIC ROS_HUMBLE) - endif() endif() - ####################################################################### -if(COMPILING_WITH_CATKIN) - install(TARGETS - DataLoadROS - DataStreamROS - RosTopicPublisher - RosoutPublisher - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/ ) -elseif(COMPILING_WITH_AMENT) - install(TARGETS + +install(TARGETS DataLoadROS2 DataStreamROS2 TopicPublisherROS2 - DESTINATION lib/${PROJECT_NAME} ) -endif() + DESTINATION lib/${PROJECT_NAME} + ) + diff --git a/src/DataLoadROS/dataload_ros.cpp b/src/DataLoadROS/dataload_ros.cpp deleted file mode 100644 index 8f1bf02d..00000000 --- a/src/DataLoadROS/dataload_ros.cpp +++ /dev/null @@ -1,252 +0,0 @@ -#include "dataload_ros.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "dialog_select_ros_topics.h" -#include "shape_shifter_factory.hpp" -#include "dialog_with_itemlist.h" - -DataLoadROS::DataLoadROS() -{ - _extensions.push_back("bag"); - QSettings settings; - _config.loadFromSettings(settings, "DataLoadROS"); -} - -DataLoadROS::~DataLoadROS() -{ -} - -void StrCat(const std::string& a, const std::string& b, std::string& out) -{ - out.clear(); - out.reserve(a.size() + b.size()); - out.assign(a); - out.append(b); -} - -const std::vector& DataLoadROS::compatibleFileExtensions() const -{ - return _extensions; -} - -std::vector> -DataLoadROS::getAllTopics(const rosbag::Bag* bag, PJ::CompositeParser& parser) -{ - std::vector> all_topics; - rosbag::View bag_view(*bag, ros::TIME_MIN, ros::TIME_MAX, true); - - bool ignoreAll = false; - - for (auto& conn : bag_view.getConnections()) - { - const auto& topic = conn->topic; - const auto& md5sum = conn->md5sum; - const auto& datatype = conn->datatype; - const auto& definition = conn->msg_def; - - all_topics.push_back(std::make_pair(QString(topic.c_str()), QString(datatype.c_str()))); - try - { - auto ros_parser = CreateParserROS(*parserFactories(), topic, datatype, definition, *_plot_map); - parser.addParser(topic, ros_parser); - RosIntrospectionFactory::registerMessage(topic, md5sum, datatype, definition); - } - catch (std::exception& ex) - { - // there was a problem with this topic - // a real life problem example can be found here: - // https://github.com/rosjava/rosjava_bootstrap/issues/16 - all_topics.pop_back(); - - if (ignoreAll) - { - // this is not the first error with this load and the - // user has accepted to ignore all errors - continue; - } - - // prompt user to abort or continue - QMessageBox msgBox(nullptr); - msgBox.setWindowTitle("ROS bag error"); - msgBox.setText(QString("Topic ") + QString(topic.c_str()) + QString(": ") + QString(ex.what())); - - QPushButton* buttonCancel = msgBox.addButton(tr("Cancel"), QMessageBox::RejectRole); - QPushButton* buttonIgnore = msgBox.addButton(tr("Ignore"), QMessageBox::YesRole); - QPushButton* buttonIgnoreAll = msgBox.addButton(tr("Ignore all"), QMessageBox::AcceptRole); - msgBox.setDefaultButton(buttonIgnoreAll); - msgBox.exec(); - if (msgBox.clickedButton() == buttonCancel) - { - // abort the file loading - throw; - } - if (msgBox.clickedButton() == buttonIgnoreAll) - { - // accept this and all future errors for this load - ignoreAll = true; - } - } - } - return all_topics; -} - -bool DataLoadROS::readDataFromFile(PJ::FileLoadInfo* info, PJ::PlotDataMapRef& plot_map) -{ - _plot_map = &plot_map; - auto temp_bag = std::make_shared(); - - try - { - temp_bag->open(info->filename.toStdString(), rosbag::bagmode::Read); - } - catch (rosbag::BagException& ex) - { - QMessageBox::warning(nullptr, tr("Error"), QString("rosbag::open thrown an exception:\n") + QString(ex.what())); - return false; - } - - PJ::CompositeParser ros_parser; - auto all_topics = getAllTopics(temp_bag.get(), ros_parser); - - //---------------------------------- - // FIXME: we keep this here for backward compatibility - if (info->plugin_config.hasChildNodes()) - { - xmlLoadState(info->plugin_config.firstChildElement()); - } - - if (!info->plugin_config.hasChildNodes() || _config.topics.empty()) - { - QSettings settings; - _config.loadFromSettings(settings, "DataLoadROS"); - - DialogSelectRosTopics* dialog = new DialogSelectRosTopics(all_topics, _config); - - if (dialog->exec() != static_cast(QDialog::Accepted)) - { - return false; - } - _config = dialog->getResult(); - } - - //--- Swith the previous bag with this one - // clean up previous MessageInstances - plot_map.user_defined.clear(); - if (_bag) - { - _bag->close(); - } - _bag = temp_bag; - //--------------------------------------- - - { - QSettings settings; - _config.saveToSettings(settings, "DataLoadROS"); - } - - ros_parser.setConfig(_config); - - //----------------------------------- - std::set topic_selected; - for (const auto& topic : _config.topics) - { - topic_selected.insert(topic.toStdString()); - } - - QProgressDialog progress_dialog; - progress_dialog.setWindowTitle("Loading the rosbag"); - progress_dialog.setLabelText("Loading... please wait"); - progress_dialog.setWindowModality(Qt::ApplicationModal); - - rosbag::View bag_view(*_bag); - - progress_dialog.setRange(0, bag_view.size() - 1); - progress_dialog.show(); - - int msg_count = 0; - - QElapsedTimer timer; - timer.start(); - - PJ::PlotDataAny& plot_consecutive = plot_map.addUserDefined("plotjuggler::rosbag1::consecutive_messages")->second; - - std::vector buffer; - - for (const rosbag::MessageInstance& msg_instance : bag_view) - { - const std::string& topic_name = msg_instance.getTopic(); - double msg_time = msg_instance.getTime().toSec(); - - //----- skip not selected ----------- - if (topic_selected.find(topic_name) == topic_selected.end()) - { - continue; - } - - //------ progress dialog -------------- - if (msg_count++ % 100 == 0) - { - progress_dialog.setValue(msg_count); - QApplication::processEvents(); - - if (progress_dialog.wasCanceled()) - { - return false; - } - } - - //----- parse! ----------- - const size_t msg_size = msg_instance.size(); - buffer.resize(msg_size); - ros::serialization::OStream stream(buffer.data(), msg_size); - msg_instance.write(stream); - - PJ::MessageRef msg_serialized(buffer.data(), buffer.size()); - - double tmp_timestamp = msg_time; - ros_parser.parseMessage(topic_name, msg_serialized, tmp_timestamp); - - //------ save msg reference in PlotAny ---- - auto data_point = PJ::PlotDataAny::Point(tmp_timestamp, std::any(msg_instance)); - plot_consecutive.pushBack(data_point); - - auto plot_pair = plot_map.user_defined.find(topic_name); - if (plot_pair == plot_map.user_defined.end()) - { - plot_pair = plot_map.addUserDefined(topic_name); - } - PJ::PlotDataAny& plot_raw = plot_pair->second; - plot_raw.pushBack(data_point); - } - - qDebug() << "The loading operation took" << timer.elapsed() << "milliseconds"; - - return true; -} - -bool DataLoadROS::xmlSaveState(QDomDocument& doc, QDomElement& plugin_elem) const -{ - _config.xmlSaveState(doc, plugin_elem); - return true; -} - -bool DataLoadROS::xmlLoadState(const QDomElement& parent_element) -{ - _config.xmlLoadState(parent_element); - return true; -} - diff --git a/src/DataLoadROS/dataload_ros.h b/src/DataLoadROS/dataload_ros.h deleted file mode 100644 index 989ae9cc..00000000 --- a/src/DataLoadROS/dataload_ros.h +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef DATALOAD_ROS_H -#define DATALOAD_ROS_H - -#include -#include - -#include -#include - -#include -#include "dialog_select_ros_topics.h" -#include "ros_parsers/ros1_parser.h" -#include "parser_configuration.h" - -class DataLoadROS : public PJ::DataLoader -{ - Q_OBJECT - Q_PLUGIN_METADATA(IID "facontidavide.PlotJuggler3.ROSDataLoader") - Q_INTERFACES(PJ::DataLoader) - -public: - DataLoadROS(); - - virtual ~DataLoadROS() override; - - virtual const std::vector& compatibleFileExtensions() const override; - - virtual bool readDataFromFile(PJ::FileLoadInfo* fileload_info, - PJ::PlotDataMapRef& destination) override; - - virtual const char* name() const override - { - return "DataLoad ROS bags"; - } - - virtual bool xmlSaveState(QDomDocument& doc, QDomElement& parent_element) const override; - - virtual bool xmlLoadState(const QDomElement& parent_element) override; - -protected: - - std::shared_ptr _bag; - -private: - std::vector _extensions; - - PJ::RosParserConfig _config; - - std::vector> getAllTopics(const rosbag::Bag* bag, PJ::CompositeParser &parser); - - PJ::PlotDataMapRef* _plot_map = nullptr; -}; - -#endif // DATALOAD_CSV_H diff --git a/src/DataStreamROS/datastream_ROS.cpp b/src/DataStreamROS/datastream_ROS.cpp deleted file mode 100644 index 43af9888..00000000 --- a/src/DataStreamROS/datastream_ROS.cpp +++ /dev/null @@ -1,459 +0,0 @@ -#include "datastream_ROS.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "dialog_select_ros_topics.h" -#include "qnodedialog.h" -#include "shape_shifter_factory.hpp" - -using PJ::PlotDataAny; -using PJ::PlotData; - -DataStreamROS::DataStreamROS() : DataStreamer(), _node(nullptr) -, _action_saveIntoRosbag(nullptr) -, _prev_clock_time(0) -{ - _running = false; - _first_warning = true; - _periodic_timer = new QTimer(); - connect(_periodic_timer, &QTimer::timeout, this, &DataStreamROS::timerCallback); - - { - QSettings settings; - _config.loadFromSettings(settings, "DataStreamROS"); - } - - _action_saveIntoRosbag = new QAction(QString("Save cached value in a rosbag")); - connect(_action_saveIntoRosbag, &QAction::changed, this, [this]() { - DataStreamROS::saveIntoRosbag(); - }); - - _action_saveAny = new QAction(QString("Store messages in memory for Re-publishing")); - connect(_action_saveAny, &QAction::triggered, this, [this]() { - QSettings settings; - settings.value("DataStreamROS/storemessagesInMemory", _action_saveAny->isChecked() ); - }); - _action_saveAny->setCheckable( true ); - - QSettings settings; - bool store_msg = settings.value("DataStreamROS/storemessagesInMemory", false).toBool(); - _action_saveAny->setChecked( store_msg ); - - _available_actions.push_back( _action_saveIntoRosbag ); - _available_actions.push_back( _action_saveAny ); -} - -void DataStreamROS::topicCallback(const RosIntrospection::ShapeShifter::ConstPtr& msg, const std::string& topic_name) -{ - if (!_running) - { - return; - } - - emit dataReceived(); - - using namespace RosIntrospection; - const auto& md5sum = msg->getMD5Sum(); - const auto& datatype = msg->getDataType(); - const auto& definition = msg->getMessageDefinition(); - - // register the message type - if(!_parser.hasParser(topic_name)) - { - auto ros_parser = CreateParserROS(*parserFactories(), topic_name, datatype, definition, dataMap()); - _parser.addParser(topic_name, ros_parser); - RosIntrospectionFactory::registerMessage(topic_name, md5sum, datatype, definition); - } - - //------------------------------------ - std::vector buffer; - buffer.resize(msg->size()); - - ros::serialization::OStream stream(buffer.data(), buffer.size()); - msg->write(stream); - - double msg_time = ros::Time::now().toSec(); - if (msg_time == 0) - { - // corner case: use_sim_time == true but topic /clock is not published - msg_time = ros::WallTime::now().toSec(); - - auto tmp_config = _config; - tmp_config.use_header_stamp = false; - _parser.setConfig(tmp_config); - } - - // time wrapping may happen using use_sim_time = true and - // rosbag play --clock --loop - if (msg_time < _prev_clock_time) - { - // clear - for (auto& it : dataMap().numeric) - { - it.second.clear(); - } - for (auto& it : _user_defined_buffers) - { - it.second.clear(); - } - emit clearBuffers(); - } - _prev_clock_time = msg_time; - - PJ::MessageRef buffer_view(buffer); - - try - { - // before pushing, lock the mutex - std::lock_guard lock(mutex()); - _parser.parseMessage(topic_name, buffer_view, msg_time); - } - catch (std::runtime_error& ex) - { - if( _first_warning ) { - _first_warning = false; - QMessageBox::warning(nullptr, tr("Error"), - QString("rosbag::open thrown an exception:\n") + - QString(ex.what()) + - "\nThis message will be shown only once."); - } - } - - const std::string prefixed_topic_name = _prefix + topic_name; - - bool save_any = _action_saveAny->isChecked(); - // adding raw serialized msg for future uses. - // do this before msg_time normalization - if( save_any ) - { - auto plot_pair = _user_defined_buffers.find(prefixed_topic_name); - if (plot_pair == _user_defined_buffers.end()) - { - plot_pair = _user_defined_buffers.emplace( - std::piecewise_construct, - std::forward_as_tuple(prefixed_topic_name), - std::forward_as_tuple(prefixed_topic_name, PJ::PlotGroup::Ptr())).first; - } - PlotDataAny& user_defined_data = plot_pair->second; - user_defined_data.pushBack(PlotDataAny::Point(msg_time, std::any(std::move(buffer)))); - } - - //------------------------------ - if( save_any ) - { - int& index = _msg_index[topic_name]; - index++; - const std::string key = prefixed_topic_name + ("/_MSG_INDEX_"); - auto index_it = dataMap().numeric.find(key); - if (index_it == dataMap().numeric.end()) - { - index_it = dataMap().addNumeric(key); - } - index_it->second.pushBack(PlotData::Point(msg_time, index)); - } -} - -void DataStreamROS::extractInitialSamples() -{ - using namespace std::chrono; - milliseconds wait_time_ms(1000); - - QProgressDialog progress_dialog; - progress_dialog.setLabelText("Collecting ROS topic samples to understand data layout. "); - progress_dialog.setRange(0, wait_time_ms.count()); - progress_dialog.setAutoClose(true); - progress_dialog.setAutoReset(true); - - progress_dialog.show(); - - auto start_time = system_clock::now(); - - while (system_clock::now() - start_time < (wait_time_ms)) - { - ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.1)); - int i = duration_cast(system_clock::now() - start_time).count(); - progress_dialog.setValue(i); - QApplication::processEvents(); - if (progress_dialog.wasCanceled()) - { - break; - } - } - - if (progress_dialog.wasCanceled() == false) - { - progress_dialog.cancel(); - } -} - -void DataStreamROS::timerCallback() -{ - if (_running && ros::master::check() == false) - { - auto ret = QMessageBox::warning(nullptr, tr("Disconnected!"), - tr("The roscore master is not reachable anymore.\n\n" - "Do you want to try reconnecting to it? \n"), - tr("Stop Streaming"), tr("Try reconnect"), nullptr); - if (ret == 1) - { - this->shutdown(); - _node = RosManager::getNode(); - - if (!_node) - { - emit closed(); - return; - } - _parser.clear(); - subscribe(); - - _running = true; - _spinner = std::make_shared(1); - _spinner->start(); - _periodic_timer->start(); - } - else if (ret == 0) - { - this->shutdown(); - emit closed(); - } - } - - if( !ros::ok() ) - { - QMessageBox::warning(nullptr, tr("ROS Stopped"), "The plugin will be stopped"); - this->shutdown(); - - emit closed(); - } -} - -void DataStreamROS::saveIntoRosbag() -{ - if (_user_defined_buffers.empty()) - { - QMessageBox::warning(nullptr, tr("Warning"), tr("Your buffer is empty. Nothing to save.\n")); - return; - } - - QFileDialog saveDialog; - saveDialog.setAcceptMode(QFileDialog::AcceptSave); - saveDialog.setDefaultSuffix("bag"); - saveDialog.exec(); - - if (saveDialog.result() != QDialog::Accepted || saveDialog.selectedFiles().empty()) - { - return; - } - - QString fileName = saveDialog.selectedFiles().first(); - - if (fileName.size() > 0) - { - rosbag::Bag rosbag(fileName.toStdString(), rosbag::bagmode::Write); - - for (const auto& it : _user_defined_buffers) - { - const std::string& topicname = it.first; - const auto& plotdata = it.second; - - auto registered_msg_type = RosIntrospectionFactory::get().getShapeShifter(topicname); - if (!registered_msg_type){ - continue; - } - - RosIntrospection::ShapeShifter msg; - msg.morph(registered_msg_type->getMD5Sum(), registered_msg_type->getDataType(), - registered_msg_type->getMessageDefinition()); - - for (int i = 0; i < plotdata.size(); i++) - { - const auto& point = plotdata.at(i); - const double msg_time = point.x; - const std::any& type_erased_buffer = point.y; - - if (type_erased_buffer.type() != typeid(std::vector)) - { - // can't cast to expected type - continue; - } - - std::vector raw_buffer = std::any_cast>(type_erased_buffer); - ros::serialization::IStream stream(raw_buffer.data(), raw_buffer.size()); - msg.read(stream); - - rosbag.write(topicname, ros::Time(msg_time), msg); - } - } - rosbag.close(); - - QProcess process; - QStringList args; - args << "reindex" << fileName; - process.start("rosbag", args); - } -} - -void DataStreamROS::subscribe() -{ - _subscribers.clear(); - - for (int i = 0; i < _config.topics.size(); i++) - { - const std::string topic_name = _config.topics[i].toStdString(); - boost::function callback; - callback = [this, topic_name](const RosIntrospection::ShapeShifter::ConstPtr& msg) -> void { - this->topicCallback(msg, topic_name); - }; - - ros::SubscribeOptions ops; - ops.initByFullCallbackType(topic_name, 1, callback); - ops.transport_hints = ros::TransportHints().tcpNoDelay(); - - _subscribers.insert({ topic_name, _node->subscribe(ops) }); - } -} - -bool DataStreamROS::start(QStringList* selected_datasources) -{ - if (!_node) - { - _node = RosManager::getNode(); - } - - if (!_node) - { - return false; - } - - { - std::lock_guard lock(mutex()); - dataMap().numeric.clear(); - dataMap().user_defined.clear(); - _parser.clear(); - } - - using namespace RosIntrospection; - - std::vector> all_topics; - ros::master::V_TopicInfo topic_infos; - ros::master::getTopics(topic_infos); - for (ros::master::TopicInfo topic_info : topic_infos) - { - all_topics.push_back(std::make_pair(QString(topic_info.name.c_str()), QString(topic_info.datatype.c_str()))); - } - - QTimer timer; - timer.setSingleShot(false); - timer.setInterval(1000); - timer.start(); - - DialogSelectRosTopics dialog(all_topics, _config); - - connect(&timer, &QTimer::timeout, [&]() { - all_topics.clear(); - topic_infos.clear(); - ros::master::getTopics(topic_infos); - for (ros::master::TopicInfo topic_info : topic_infos) - { - all_topics.push_back({ QString::fromStdString(topic_info.name), QString::fromStdString(topic_info.datatype) }); - } - dialog.updateTopicList(all_topics); - }); - - int res = dialog.exec(); - _config = dialog.getResult(); - timer.stop(); - - if (res != QDialog::Accepted || _config.topics.empty()) - { - return false; - } - - { - QSettings settings; - _config.saveToSettings(settings, "DataStreamROS"); - } - - _parser.setConfig(_config); - - //------------------------- - subscribe(); - _running = true; - _first_warning = true; - - extractInitialSamples(); - - _spinner = std::make_shared(1); - _spinner->start(); - - _periodic_timer->setInterval(500); - _periodic_timer->start(); - - return true; -} - -bool DataStreamROS::isRunning() const -{ - return _running; -} - -void DataStreamROS::shutdown() -{ - _periodic_timer->stop(); - - if (_spinner) - { - _spinner->stop(); - } - _spinner.reset(); - - if(_node ){ - _node->shutdown(); - } - _node.reset(); - - _subscribers.clear(); - _running = false; - _parser.clear(); -} - -DataStreamROS::~DataStreamROS() -{ - shutdown(); -} - -bool DataStreamROS::xmlSaveState(QDomDocument& doc, QDomElement& plugin_elem) const -{ - _config.xmlSaveState(doc, plugin_elem); - return true; -} - -bool DataStreamROS::xmlLoadState(const QDomElement& parent_element) -{ - _config.xmlLoadState(parent_element); - return true; -} - -const std::vector &DataStreamROS::availableActions() -{ - return _available_actions; -} - diff --git a/src/DataStreamROS/datastream_ROS.h b/src/DataStreamROS/datastream_ROS.h deleted file mode 100644 index 07e442f4..00000000 --- a/src/DataStreamROS/datastream_ROS.h +++ /dev/null @@ -1,90 +0,0 @@ -#ifndef DATASTREAM_ROS_TOPIC_H -#define DATASTREAM_ROS_TOPIC_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include "qnodedialog.h" -#include "dialog_select_ros_topics.h" -#include "ros_parsers/ros1_parser.h" - -class DataStreamROS : public PJ::DataStreamer -{ - Q_OBJECT - Q_PLUGIN_METADATA(IID "facontidavide.PlotJuggler3.DataStreamer") - Q_INTERFACES(PJ::DataStreamer) - -public: - DataStreamROS(); - - virtual bool start(QStringList* selected_datasources) override; - - virtual void shutdown() override; - - virtual bool isRunning() const override; - - virtual ~DataStreamROS() override; - - virtual const char* name() const override - { - return "ROS Topic Subscriber"; - } - - virtual bool xmlSaveState(QDomDocument& doc, QDomElement& parent_element) const override; - - virtual bool xmlLoadState(const QDomElement& parent_element) override; - - virtual const std::vector& availableActions() override; - -private: - void topicCallback(const RosIntrospection::ShapeShifter::ConstPtr& msg, const std::string& topic_name); - - void extractInitialSamples(); - - void timerCallback(); - - void subscribe(); - - bool _running; - bool _first_warning; - - std::shared_ptr _spinner; - - double _initial_time; - - std::string _prefix; - - ros::NodeHandlePtr _node; - - std::map _subscribers; - - int _received_msg_count; - - QAction* _action_saveIntoRosbag; - - QAction* _action_saveAny; - - std::vector _available_actions; - - std::map _msg_index; - - PJ::RosParserConfig _config; - - PJ::CompositeParser _parser; - - QTimer* _periodic_timer; - - double _prev_clock_time; - - std::unordered_map _user_defined_buffers; - -private: - void saveIntoRosbag(); -}; - -#endif // DATALOAD_CSV_H diff --git a/src/RosoutPublisher/logs_table_model.cpp b/src/RosoutPublisher/logs_table_model.cpp deleted file mode 100644 index 054cb39e..00000000 --- a/src/RosoutPublisher/logs_table_model.cpp +++ /dev/null @@ -1,283 +0,0 @@ -#include "logs_table_model.hpp" -#include -#include -#include -#include - -LogsTableModel::LogsTableModel(QObject* parent) : QAbstractTableModel(parent), _logs(MAX_CAPACITY) // initial capacity -{ - _count = 0; -} - -QVariant LogsTableModel::headerData(int section, Qt::Orientation orientation, int role) const -{ - if (role != Qt::DisplayRole) - return QVariant(); - - if (orientation == Qt::Horizontal) - { - switch (section) - { - case 0: - return "#"; - break; - case 1: - return "Time"; - break; - case 2: - return "Severity"; - break; - case 3: - return "Node"; - break; - case 4: - return "Message"; - break; - case 5: - return "Source"; - break; - } - } - else - { - return QString("%1").arg(section); - } - - return QVariant(); -} - -int LogsTableModel::rowCount(const QModelIndex& parent) const -{ - if (parent.isValid()) - return 0; - - return _logs.size(); -} - -int LogsTableModel::columnCount(const QModelIndex& parent) const -{ - if (parent.isValid()) - return 0; - - return 5; -} - -QVariant LogsTableModel::data(const QModelIndex& index, int role) const -{ - if (!index.isValid()) - return QVariant(); - - if (index.row() >= _logs.size()) - return QVariant(); - - const LogItem& log = _logs[index.row()]; - - if (role == Qt::DisplayRole) - { - switch (index.column()) - { - case 0: - return (int)log.count; - case 1: - return log.time_text; - case 2: { - switch (log.level_raw) - { - case DEBUG: - return "DEBUG"; - case INFO: - return "INFO"; - case WARNINGS: - return "WARNINGS"; - case ERROR: - return "ERROR"; - } - } - break; - case 3: - return (*log.node); - case 4: - return log.message; - case 5: - return (*log.source); - } - } - else if (role == Qt::ForegroundRole) - { - switch (log.level_raw) - { - case DEBUG: - return QBrush(QColor::fromRgb(50, 50, 50)); // black - case INFO: - return QBrush(QColor::fromRgb(0, 0, 255)); // blue - case WARNINGS: - return QBrush(QColor::fromRgb(240, 120, 0)); // orange - case ERROR: - return QBrush(QColor::fromRgb(255, 0, 0)); // red - } - } - else if (role == Qt::UserRole) - { - switch (index.column()) - { - case 0: - return (int)log.count; - case 1: { - auto usec = (long long)log.time_usec_since_epoch; - return QVariant(usec); - } - case 2: - return log.level_raw; - case 3: - return *log.node; - case 4: - return log.message; - case 5: - return *log.source; - } - } - else - { - return QVariant(); - } - return QVariant(); -} - -LogsTableModel::LogItem LogsTableModel::convertRosout(const rosgraph_msgs::Log& log) -{ - _count++; - - LogItem item; - switch (log.level) - { - case rosgraph_msgs::Log::DEBUG: - item.level_raw = DEBUG; - break; - case rosgraph_msgs::Log::INFO: - item.level_raw = INFO; - break; - case rosgraph_msgs::Log::WARN: - item.level_raw = WARNINGS; - break; - case rosgraph_msgs::Log::ERROR: - item.level_raw = ERROR; - break; - } - - item.count = _count; - - QString node_name = QString::fromStdString(log.name); - auto node_it = _node_list.find(node_name); - - if (node_it == _node_list.end()) - { - auto inserted_ret = _node_list.insert(node_name); - node_it = inserted_ret.first; - } - item.node = &(*node_it); - - QString source_name(log.file.c_str()); - source_name += (" "); - source_name += QString::fromStdString(log.function); - source_name += (":"); - source_name += QString::number(log.line); - - auto source_it = _source_list.find(source_name); - - if (source_it == _source_list.end()) - { - auto inserted_ret = _source_list.insert(source_name); - source_it = inserted_ret.first; - } - item.source = &(*source_it); - - item.message = log.msg.c_str(); - - item.time_usec_since_epoch = log.header.stamp.toNSec() / 1000; - item.time_text = QDateTime::fromMSecsSinceEpoch(item.time_usec_since_epoch / 1000).toString("d/M/yy HH:mm::ss.zzz"); - return item; -} - -void LogsTableModel::push_back(const rosgraph_msgs::Log::ConstPtr& pushed_log) -{ - bool to_shift = (_logs.size() == MAX_CAPACITY); - - _logs.push_back(convertRosout(*pushed_log)); - - if (to_shift) - { - this->beginRemoveRows(QModelIndex(), 0, 0); - this->endRemoveRows(); - emit dataChanged(index(0, 0), index(rowCount() - 1, columnCount() - 1)); - } - - this->beginInsertRows(QModelIndex(), _logs.size() - 1, _logs.size() - 1); - this->endInsertRows(); -} - -void LogsTableModel::push_back(const std::vector& pushed_logs) -{ - size_t old_size = _logs.size(); - size_t new_size = old_size + pushed_logs.size(); - - int to_add = pushed_logs.size(); - int to_shift = 0; - - if (new_size > MAX_CAPACITY) - { - to_add = (MAX_CAPACITY - old_size); - to_shift = new_size - MAX_CAPACITY; - new_size = MAX_CAPACITY; - } - - const size_t last_row = new_size - 1; - const size_t first_row = new_size - pushed_logs.size(); - - for (int i = 0; i < pushed_logs.size(); i++) - { - _logs.push_back(convertRosout(*pushed_logs[i])); - } - - std::sort(_logs.begin(), _logs.end(), - [](const LogItem& a, const LogItem& b) { return a.time_usec_since_epoch < b.time_usec_since_epoch; }); - - if (to_shift > 0) - { - this->beginRemoveRows(QModelIndex(), 0, 0); - this->endRemoveRows(); - - emit dataChanged(index(0, 0), index(rowCount() - 1, columnCount() - 1)); - } - - this->beginInsertRows(QModelIndex(), first_row, last_row); - this->endInsertRows(); -} - -const QString& LogsTableModel::message(int index) const -{ - return _logs[index].message; -} - -const QString& LogsTableModel::nodeName(int index) const -{ - return *(_logs[index].node); -} - -LogsTableModel::Severity LogsTableModel::severity(int index) const -{ - return _logs[index].level_raw; -} - -TimePoint LogsTableModel::timestamp(int index) const -{ - std::chrono::microseconds since_epoch(_logs[index].time_usec_since_epoch); - return TimePoint() + since_epoch; -} - -void LogsTableModel::clear() -{ - this->beginRemoveRows(QModelIndex(), 0, _logs.size() - 1); - this->endRemoveRows(); - _logs.clear(); - _count = 0; -} diff --git a/src/RosoutPublisher/logs_table_model.hpp b/src/RosoutPublisher/logs_table_model.hpp deleted file mode 100644 index 65b548c0..00000000 --- a/src/RosoutPublisher/logs_table_model.hpp +++ /dev/null @@ -1,94 +0,0 @@ -#ifndef LOGSTABLEMODEL_HPP -#define LOGSTABLEMODEL_HPP - -#include -#include -#include -#include -#include -#include -#include -#include - -#ifdef _WIN32 -#undef ERROR -#endif - -typedef std::chrono::high_resolution_clock::time_point TimePoint; - - -class LogsTableModel : public QAbstractTableModel -{ - Q_OBJECT - -public: - explicit LogsTableModel(QObject *parent = 0); - - typedef enum{ - DEBUG = 0, - INFO = 1, - WARNINGS = 2, - ERROR = 3 - }Severity; - - // Header: - QVariant headerData(int section, Qt::Orientation orientation, int role = Qt::DisplayRole) const override; - - // Basic functionality: - int rowCount(const QModelIndex &parent = QModelIndex()) const override; - - int columnCount(const QModelIndex &parent = QModelIndex()) const override; - - QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override; - - void push_back(const rosgraph_msgs::Log::ConstPtr& pushed_log); - - void push_back(const std::vector& pushed_logs); - - const QString& message(int index) const; - - const QString &nodeName(int index) const; - - Severity severity(int index) const; - - TimePoint timestamp(int index) const; - - int size() const { return _logs.size(); } - - void clear(); - -private: - - std::set _source_list; - std::set _node_list; - - typedef struct{ - size_t count; - int64_t time_usec_since_epoch; - QString time_text; - Severity level_raw; - const QString* node; - QString message; - const QString* source; - }LogItem; - - boost::circular_buffer _logs; - - size_t _count; - - enum{ MAX_CAPACITY = 20000 }; // max capacity of the circular buffer - - LogItem convertRosout(const rosgraph_msgs::Log &log); - -#ifdef USE_ROSOUT2 - LogItem convertRosout(const rosout2_msg::LogMsg &log); -#endif - -signals: - - void rowsShifted(int); - -}; - - -#endif // LOGSTABLEMODEL_HPP diff --git a/src/RosoutPublisher/logwidget.cpp b/src/RosoutPublisher/logwidget.cpp deleted file mode 100644 index f4535277..00000000 --- a/src/RosoutPublisher/logwidget.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/** - * @file /src/main_window.cpp - * - * @brief Implementation for the qt gui. - * - * @date February 2011 - **/ -/***************************************************************************** -** Includes -*****************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include "logwidget.hpp" - -namespace rqt_console_plus -{ -using namespace Qt; - -LogWidget::LogWidget(LogsTableModel& tablemodel, QWidget* parent) - : QWidget(parent), model(tablemodel), proxy_model(this) -{ - ui.setupUi(this); // Calling this incidentally connects all ui's triggers to on_...() callbacks in this class. - - proxy_model.setSourceModel(&model); - ui.tableView->setModel(&proxy_model); - // ui.tableView->setModel( &model ); - - ui.tableView->horizontalHeader()->setSectionResizeMode(0, QHeaderView::ResizeToContents); - ui.tableView->horizontalHeader()->setSectionResizeMode(1, QHeaderView::ResizeToContents); - ui.tableView->horizontalHeader()->setSectionResizeMode(2, QHeaderView::ResizeToContents); - ui.tableView->horizontalHeader()->setSectionResizeMode(3, QHeaderView::Interactive); - ui.tableView->horizontalHeader()->setSectionResizeMode(4, QHeaderView::Stretch); - // ui.tableView->horizontalHeader()->setSectionResizeMode(5, QHeaderView::Stretch); - - ui.tableView->verticalHeader()->setVisible(false); - - connect(&model, &LogsTableModel::rowsInserted, this, &LogWidget::on_rowsInserted); - - proxy_model.setSeverityDebugEnabled(ui.buttonEnableDebug->isChecked()); - proxy_model.setSeverityWarningsEnabled(ui.buttonEnableWarnings->isChecked()); - proxy_model.setSeverityErrorEnabled(ui.buttonEnableError->isChecked()); - proxy_model.setSeverityInfoEnabled(ui.buttonEnableInfo->isChecked()); - - proxy_model.setMessageFilterEnabled(ui.checkBoxMessageFilter->isChecked()); - proxy_model.setNodeFilterEnabled(ui.checkBoxLoggerFilter->isChecked()); - proxy_model.setTimeFilterEnabled(true); -} - -LogWidget::~LogWidget() -{ -} - -void LogWidget::on_lineEditMessageFilter_textEdited(const QString& filter) -{ - proxy_model.messageFilterUpdated(static_cast(ui.comboBoxMessageFilter->currentIndex()), - filter); - ui.tableView->resizeColumnToContents(3); -} - -void LogWidget::on_comboBoxMessageFilter_currentIndexChanged(int mode) -{ - proxy_model.messageFilterUpdated(static_cast(mode), ui.lineEditMessageFilter->text()); - ui.tableView->resizeColumnToContents(4); -} - -void LogWidget::on_checkBoxMessageFilter_toggled(bool checked) -{ - ui.labelMessageFilter->setEnabled(checked); - ui.comboBoxMessageFilter->setEnabled(checked); - ui.lineEditMessageFilter->setEnabled(checked); - - proxy_model.setMessageFilterEnabled(checked); - ui.tableView->resizeColumnToContents(4); -} - -void LogWidget::on_lineEditLoggerFilter_textEdited(const QString& filter) -{ - proxy_model.nodeFilterUpdated(static_cast(ui.comboBoxLoggerFilter->currentIndex()), filter); - ui.tableView->resizeColumnToContents(3); -} - -void LogWidget::on_comboBoxLoggerFilter_currentIndexChanged(int mode) -{ - proxy_model.nodeFilterUpdated(static_cast(mode), ui.lineEditLoggerFilter->text()); - ui.tableView->resizeColumnToContents(3); -} -void LogWidget::on_checkBoxLoggerFilter_toggled(bool checked) - -{ - ui.labelLoggerFilter->setEnabled(checked); - ui.comboBoxLoggerFilter->setEnabled(checked); - ui.lineEditLoggerFilter->setEnabled(checked); - - proxy_model.setNodeFilterEnabled(checked); - ui.tableView->resizeColumnToContents(3); -} - -void LogWidget::on_rowsInserted(const QModelIndex&, int first_row, int last_row) -{ - ui.tableView->scrollToBottom(); -} - -void LogWidget::on_buttonEnableDebug_toggled(bool checked) -{ - proxy_model.setSeverityDebugEnabled(checked); - ui.tableView->resizeColumnToContents(2); -} - -void LogWidget::on_buttonEnableInfo_toggled(bool checked) -{ - proxy_model.setSeverityInfoEnabled(checked); - ui.tableView->resizeColumnToContents(2); -} - -void LogWidget::on_buttonEnableWarnings_toggled(bool checked) -{ - proxy_model.setSeverityWarningsEnabled(checked); - ui.tableView->resizeColumnToContents(2); -} - -void LogWidget::on_buttonEnableError_toggled(bool checked) -{ - proxy_model.setSeverityErrorEnabled(checked); - ui.tableView->resizeColumnToContents(2); -} - -void LogWidget::on_timeRangeChanged(TimePoint time_min, TimePoint time_max) -{ - using namespace std::chrono; - { - auto msec_since_epoch_A = duration_cast(time_min.time_since_epoch()); - auto datetimeA = QDateTime::fromMSecsSinceEpoch(msec_since_epoch_A.count()); - ui.timeRangeMin->setDateTime(datetimeA); - } - - { - auto msec_since_epoch_B = duration_cast(time_max.time_since_epoch()); - auto datetimeB = QDateTime::fromMSecsSinceEpoch(msec_since_epoch_B.count()); - ui.timeRangeMax->setDateTime(datetimeB); - } - - // qDebug() << msec_since_epoch_A.count() << " " << msec_since_epoch_B.count(); - // qDebug() << datetimeA << " " << datetimeB; - - proxy_model.timeMinMaxUpdated(time_min, time_max); - ui.tableView->scrollToBottom(); - ui.tableView->resizeColumnToContents(0); - ui.tableView->resizeColumnToContents(1); -} - -} // namespace rqt_console_plus diff --git a/src/RosoutPublisher/logwidget.hpp b/src/RosoutPublisher/logwidget.hpp deleted file mode 100644 index 90336041..00000000 --- a/src/RosoutPublisher/logwidget.hpp +++ /dev/null @@ -1,74 +0,0 @@ -/** - * @file /include/rqt_console_plus/main_window.hpp - * - * @brief Qt based gui for rqt_console_plus. - * - * @date November 2010 - **/ -#ifndef _LOG_WIDGET_H -#define _LOG_WIDGET_H - -/***************************************************************************** -** Includes -*****************************************************************************/ - -#include -#include -#include "logs_table_model.hpp" -#include "ui_logwidget.h" -#include "modelfilter.hpp" - -/***************************************************************************** -** Namespace -*****************************************************************************/ - -namespace rqt_console_plus { - - -class LogWidget : public QWidget -{ - Q_OBJECT - -public: - explicit LogWidget(LogsTableModel& tablemodel, QWidget *parent = 0); - ~LogWidget(); - - -private slots: - void on_lineEditMessageFilter_textEdited(const QString &filter); - - void on_checkBoxMessageFilter_toggled(bool checked); - - void on_checkBoxLoggerFilter_toggled(bool checked); - - void on_comboBoxMessageFilter_currentIndexChanged(int index); - - void on_buttonEnableDebug_toggled(bool checked); - - void on_rowsInserted(const QModelIndex & parent, int first_row, int last_row); - - void on_buttonEnableInfo_toggled(bool checked); - - void on_buttonEnableWarnings_toggled(bool checked); - - void on_buttonEnableError_toggled(bool checked); - - void on_lineEditLoggerFilter_textEdited(const QString &arg1); - - void on_comboBoxLoggerFilter_currentIndexChanged(int index); - -public slots: - - void on_timeRangeChanged(TimePoint time_min, TimePoint time_max); - -private: - - Ui::LogWidgetDesign ui; - LogsTableModel& model; - - ModelFilter proxy_model; -}; - -} // namespace rqt_console_plus - -#endif // rqt_console_plus_MAIN_WINDOW_H diff --git a/src/RosoutPublisher/logwidget.ui b/src/RosoutPublisher/logwidget.ui deleted file mode 100644 index 8d8c7d6f..00000000 --- a/src/RosoutPublisher/logwidget.ui +++ /dev/null @@ -1,459 +0,0 @@ - - - LogWidgetDesign - - - - 0 - 0 - 500 - 700 - - - - - 500 - 0 - - - - - - - - - - - - - QAbstractItemView::NoEditTriggers - - - false - - - QAbstractItemView::NoSelection - - - QAbstractItemView::SelectRows - - - false - - - true - - - true - - - - - - - QFrame::StyledPanel - - - QFrame::Raised - - - - 0 - - - 0 - - - 0 - - - 0 - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - true - - - Show if message - - - - - - - true - - - - 100 - 0 - - - - - 150 - 16777215 - - - - - contains - - - - - matches wildcard - - - - - matches regex - - - - - - - - true - - - - 200 - 0 - - - - - 500 - 16777215 - - - - - - - - - - - true - - - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - false - - - Show if logger name - - - - - - - false - - - - 100 - 0 - - - - - 150 - 16777215 - - - - - contains - - - - - matches wildcard - - - - - matches regex - - - - - - - - false - - - - 200 - 0 - - - - - 500 - 16777215 - - - - - - - - - - - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - true - - - Time range: - - - - - - - true - - - - 250 - 16777215 - - - - true - - - false - - - d/M/yy HH:mm::ss.zzz - - - - - - - true - - - - 250 - 16777215 - - - - true - - - false - - - d/M/yy HH:mm::ss.zzz - - - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - 100 - 0 - - - - Debug - - - true - - - true - - - - - - - - 100 - 0 - - - - Info - - - true - - - true - - - - - - - - 100 - 0 - - - - Warnings - - - true - - - true - - - - - - - - 100 - 0 - - - - Error - - - true - - - true - - - - - - - - - - - - &Quit - - - Ctrl+Q - - - Qt::ApplicationShortcut - - - - - &Preferences - - - - - &About - - - - - false - - - Read rosbag - - - - - Listen /rosout - - - - - Listen /rosout2 - - - - - - diff --git a/src/RosoutPublisher/modelfilter.cpp b/src/RosoutPublisher/modelfilter.cpp deleted file mode 100644 index 2abbd163..00000000 --- a/src/RosoutPublisher/modelfilter.cpp +++ /dev/null @@ -1,222 +0,0 @@ -#include "modelfilter.hpp" -#include "logs_table_model.hpp" - -ModelFilter::ModelFilter(QObject* parent) : QSortFilterProxyModel(parent) -{ - _msg_filter_enabled = false; - _node_filter_enabled = false; - _source_filter_enabled = false; - _time_filter_enabled = true; - - _info_filter_enabled = false; - _error_filter_enabled = false; - _warn_filter_enabled = false; - _debug_filter_enabled = false; -} - -void ModelFilter::setMessageFilterEnabled(bool enabled) -{ - _msg_filter_enabled = enabled; - invalidateFilter(); -} - -void ModelFilter::setNodeFilterEnabled(bool enabled) -{ - _node_filter_enabled = enabled; - invalidateFilter(); -} - -void ModelFilter::setSourceFilterEnabled(bool enabled) -{ - _source_filter_enabled = enabled; - invalidateFilter(); -} - -void ModelFilter::setTimeFilterEnabled(bool enabled) -{ - _time_filter_enabled = enabled; - invalidateFilter(); -} - -void ModelFilter::messageFilterUpdated(ModelFilter::FilterMode mode, const QString& filter) -{ - _msg_mode = mode; - _msg_text = filter; - - if (mode == WILDCARDS) - { - QRegExp regexp(filter, Qt::CaseSensitive, QRegExp::Wildcard); - _msg_validator.setRegExp(regexp); - } - else if (mode == REGEX) - { - QRegExp regexp(filter, Qt::CaseSensitive, QRegExp::RegExp2); - _msg_validator.setRegExp(regexp); - } - - invalidateFilter(); -} - -void ModelFilter::nodeFilterUpdated(ModelFilter::FilterMode mode, const QString& filter) -{ - _node_mode = mode; - _node_text = filter; - - if (mode == WILDCARDS) - { - QRegExp regexp(filter, Qt::CaseSensitive, QRegExp::Wildcard); - _node_validator.setRegExp(regexp); - } - else if (mode == REGEX) - { - QRegExp regexp(filter, Qt::CaseSensitive, QRegExp::RegExp2); - _node_validator.setRegExp(regexp); - } - invalidateFilter(); -} - -void ModelFilter::sourceFilterUpdated(ModelFilter::FilterMode mode, const QString& filter) -{ - _source_mode = mode; - _source_text = filter; - - if (mode == WILDCARDS) - { - QRegExp regexp(filter, Qt::CaseSensitive, QRegExp::Wildcard); - _source_validator.setRegExp(regexp); - } - else if (mode == REGEX) - { - QRegExp regexp(filter, Qt::CaseSensitive, QRegExp::RegExp2); - _source_validator.setRegExp(regexp); - } - invalidateFilter(); -} - -void ModelFilter::timeMinMaxUpdated(TimePoint min, TimePoint max) -{ - _min = min; - _max = max; - invalidateFilter(); -} - -void ModelFilter::setSeverityInfoEnabled(bool enabled) -{ - _info_filter_enabled = enabled; - invalidateFilter(); -} - -void ModelFilter::setSeverityDebugEnabled(bool enabled) -{ - _debug_filter_enabled = enabled; - invalidateFilter(); -} - -void ModelFilter::setSeverityErrorEnabled(bool enabled) -{ - _error_filter_enabled = enabled; - invalidateFilter(); -} - -void ModelFilter::setSeverityWarningsEnabled(bool enabled) -{ - _warn_filter_enabled = enabled; - invalidateFilter(); -} - -bool ModelFilter::filterAcceptsRow(int sourceRow, const QModelIndex& sourceParent) const -{ - QModelIndex index_time = sourceModel()->index(sourceRow, 1, sourceParent); - QModelIndex index_severity = sourceModel()->index(sourceRow, 2, sourceParent); - QModelIndex index_node = sourceModel()->index(sourceRow, 3, sourceParent); - QModelIndex index_message = sourceModel()->index(sourceRow, 4, sourceParent); - QModelIndex index_source = sourceModel()->index(sourceRow, 5, sourceParent); - - int severity = sourceModel()->data(index_severity, Qt::UserRole).toInt(); - - if (!_info_filter_enabled && severity == LogsTableModel::INFO) - return false; - if (!_error_filter_enabled && severity == LogsTableModel::ERROR) - return false; - if (!_warn_filter_enabled && severity == LogsTableModel::WARNINGS) - return false; - if (!_debug_filter_enabled && severity == LogsTableModel::DEBUG) - return false; - - if (_time_filter_enabled) - { - int64_t usec = sourceModel()->data(index_time, Qt::UserRole).toLongLong(); - auto timestamp = TimePoint() + std::chrono::microseconds(usec); - - if (timestamp < _min || timestamp > _max) - { - return false; - } - } - - if (_msg_filter_enabled) - { - const QString& text = sourceModel()->data(index_message, Qt::UserRole).toString(); - bool ret = applyFilter(_msg_text, _msg_mode, text, &_msg_validator); - if (!ret) - { - return false; - } - } - - if (_source_filter_enabled) - { - const QString& text = sourceModel()->data(index_source, Qt::UserRole).toString(); - bool ret = applyFilter(_source_text, _source_mode, text, &_source_validator); - if (!ret) - { - return false; - } - } - - if (_node_filter_enabled) - { - const QString& text = sourceModel()->data(index_node, Qt::UserRole).toString(); - bool ret = applyFilter(_node_text, _node_mode, text, &_node_validator); - if (!ret) - { - return false; - } - } - - return true; -} - -bool ModelFilter::applyFilter(const QString& filter, ModelFilter::FilterMode mode, const QString& text_to_parse, - const QRegExpValidator* validator) const -{ - // accept if no filter - if (filter.count() == 0) - { - return true; - } - - assert(!(validator == nullptr && (mode == WILDCARDS || mode == REGEX))); - - if (mode == CONTAINS_ONE) - { - QStringList filter_words = filter.split(QRegExp("\\s"), QString::SkipEmptyParts); - - for (int i = 0; i < filter_words.size(); i++) - { - if (text_to_parse.contains(filter_words[i], Qt::CaseSensitive) == true) - { - return true; - } - } - return false; - } - //----------------------- - if (mode == WILDCARDS || mode == REGEX) - { - QString message = text_to_parse; - int pos = 0; - return validator->validate(message, pos) == QValidator::Acceptable; - } - return false; -} diff --git a/src/RosoutPublisher/modelfilter.hpp b/src/RosoutPublisher/modelfilter.hpp deleted file mode 100644 index c5873061..00000000 --- a/src/RosoutPublisher/modelfilter.hpp +++ /dev/null @@ -1,77 +0,0 @@ -#ifndef MODELFILTER_HPP -#define MODELFILTER_HPP - -#include -#include -#include -#include -#include - -typedef std::chrono::high_resolution_clock::time_point TimePoint; - -class ModelFilter : public QSortFilterProxyModel -{ - Q_OBJECT -public: - explicit ModelFilter(QObject *parent = 0); - - typedef enum{ - CONTAINS_ONE = 0, - WILDCARDS = 1, - REGEX = 2 - }FilterMode; - -signals: - -public slots: - - void setMessageFilterEnabled(bool enabled); - void setNodeFilterEnabled(bool enabled); - void setSourceFilterEnabled(bool enabled); - void setTimeFilterEnabled(bool enabled); - - void messageFilterUpdated(FilterMode mode, const QString& text ); - void nodeFilterUpdated(FilterMode mode, const QString& text ); - void sourceFilterUpdated(FilterMode mode, const QString& text ); - void timeMinMaxUpdated(TimePoint min, TimePoint max); - - void setSeverityInfoEnabled(bool enabled); - void setSeverityDebugEnabled(bool enabled); - void setSeverityErrorEnabled(bool enabled); - void setSeverityWarningsEnabled(bool enabled); - -private: - bool applyFilter(const QString &filter, ModelFilter::FilterMode mode, - const QString& text_to_parse, const QRegExpValidator *validator) const; - -protected: - virtual bool filterAcceptsRow(int sourceRow, - const QModelIndex &sourceParent) const override; - - TimePoint _min; - TimePoint _max; - - bool _node_filter_enabled; - bool _source_filter_enabled; - bool _msg_filter_enabled; - bool _time_filter_enabled; - - bool _debug_filter_enabled; - bool _info_filter_enabled; - bool _error_filter_enabled; - bool _warn_filter_enabled; - - FilterMode _node_mode; - FilterMode _msg_mode; - FilterMode _source_mode; - - QString _node_text; - QString _msg_text; - QString _source_text; - - QRegExpValidator _node_validator; - QRegExpValidator _msg_validator; - QRegExpValidator _source_validator; -}; - -#endif // MODELFILTER_HPP diff --git a/src/RosoutPublisher/rosout_publisher.cpp b/src/RosoutPublisher/rosout_publisher.cpp deleted file mode 100644 index 6a1c9f46..00000000 --- a/src/RosoutPublisher/rosout_publisher.cpp +++ /dev/null @@ -1,177 +0,0 @@ -#include "rosout_publisher.h" -#include "shape_shifter_factory.hpp" -#include "qnodedialog.h" -#include -#include - -RosoutPublisher::RosoutPublisher() : _enabled(false), _tablemodel(nullptr), _log_window(nullptr) -{ -} - -RosoutPublisher::~RosoutPublisher() -{ -} - -void RosoutPublisher::setEnabled(bool to_enable) -{ - _enabled = to_enable; - - if (enabled()) - { - _minimum_time_usec = std::numeric_limits::max(); - _maximum_time_usec = std::numeric_limits::min(); - - if (_tablemodel == nullptr) - { - _tablemodel = new LogsTableModel(this); - } - - _log_window = new RosoutWindow(); - - auto logwidget = new rqt_console_plus::LogWidget(*_tablemodel, _log_window); - _log_window->setCentralWidget(logwidget); - Qt::WindowFlags flags = _log_window->windowFlags(); - _log_window->setWindowFlags(flags | Qt::SubWindow); - - connect(this, &RosoutPublisher::timeRangeChanged, logwidget, &rqt_console_plus::LogWidget::on_timeRangeChanged); - - connect(_log_window, &RosoutWindow::closed, this, &RosoutPublisher::onWindowClosed); - - QSettings settings; - _log_window->restoreGeometry(settings.value("RosoutPublisher.geometry").toByteArray()); - - _log_window->show(); - } - else - { - if (_log_window) - { - _log_window->close(); - } - } -} - -void RosoutPublisher::onWindowClosed() -{ - QSettings settings; - settings.setValue("RosoutPublisher.geometry", _log_window->saveGeometry()); - - if (_tablemodel) - { - _tablemodel->deleteLater(); - _tablemodel = nullptr; - } - if (_log_window) - { - _log_window->deleteLater(); - _log_window = nullptr; - } - _enabled = false; - - emit closed(); -} - -std::vector RosoutPublisher::findRosoutTimeseries() -{ - std::vector logs_timeseries; - - for (const auto& data_it : _datamap->user_defined) - { - const std::string& topic_name = data_it.first; - - // check if I registered this message before - const RosIntrospection::ShapeShifter* registered_shapeshifted_msg = - RosIntrospectionFactory::get().getShapeShifter(topic_name); - if (!registered_shapeshifted_msg) - { - continue; // will not be able to use this anyway, just skip - } - - if (registered_shapeshifted_msg->getMD5Sum() != - std::string(ros::message_traits::MD5Sum::value())) - { - continue; // it is NOT a rosgraph_msgs::Log - } - - logs_timeseries.push_back(&data_it.second); - } - - return logs_timeseries; -} - -void RosoutPublisher::syncWithTableModel(const std::vector& logs_timeseries) -{ - const int64_t threshold_time = _maximum_time_usec; - - std::vector logs; - logs.reserve(100); - - // most of the time we expect logs_timeseries to have just 1 element - for (const PlotDataAny* type_erased_logs : logs_timeseries) - { - const int first_index = type_erased_logs->getIndexFromX(threshold_time); - - if (first_index != -1) - { - for (int i = first_index; i < type_erased_logs->size(); i++) - { - const auto& any_msg = type_erased_logs->at(i); - const std::any& any_value = any_msg.y; - - const bool isRawBuffer = any_value.type() == typeid(std::vector); - const bool isRosbagMessage = any_value.type() == typeid(rosbag::MessageInstance); - std::vector raw_buffer; - - if (isRawBuffer) - { - raw_buffer = std::any_cast>(any_value); - } - else if (isRosbagMessage) - { - const rosbag::MessageInstance& msg_instance = std::any_cast(any_value); - raw_buffer.resize(msg_instance.size()); - ros::serialization::OStream stream(raw_buffer.data(), raw_buffer.size()); - msg_instance.write(stream); - } - else - { - continue; - } - - rosgraph_msgs::LogPtr p(boost::make_shared()); - ros::serialization::IStream stream(raw_buffer.data(), raw_buffer.size()); - ros::serialization::deserialize(stream, *p); - - int64_t usec = p->header.stamp.toNSec() / 1000; - _minimum_time_usec = std::min(_minimum_time_usec, usec); - _maximum_time_usec = std::max(_maximum_time_usec, usec); - - if (usec >= threshold_time) - { - logs.push_back(p); - } - } - } - } - std::sort(logs.begin(), logs.end(), [](const rosgraph_msgs::LogConstPtr& a, const rosgraph_msgs::LogConstPtr& b) { - return a->header.stamp < b->header.stamp; - }); - _tablemodel->push_back(logs); -} - -void RosoutPublisher::updateState(double current_time) -{ - if (!_enabled && !_tablemodel) - return; - - std::vector logs_timeseries = findRosoutTimeseries(); - - syncWithTableModel(logs_timeseries); - - using namespace std::chrono; - TimePoint p_min = TimePoint() + microseconds(_minimum_time_usec); - // TimePoint p_max = TimePoint() + microseconds(_maximum_time_usec); - TimePoint p_curr = TimePoint() + microseconds((int64_t)(current_time * 1000000)); - - emit timeRangeChanged(p_min, p_curr); -} diff --git a/src/RosoutPublisher/rosout_publisher.h b/src/RosoutPublisher/rosout_publisher.h deleted file mode 100644 index 5f8bd2a7..00000000 --- a/src/RosoutPublisher/rosout_publisher.h +++ /dev/null @@ -1,80 +0,0 @@ -#ifndef ROSOUT_PUBLISHER_ROS_H -#define ROSOUT_PUBLISHER_ROS_H - -#include -#include -#include -#include -#include -#include -#include -#include "logwidget.hpp" - -using namespace PJ; - -class RosoutWindow : public QMainWindow -{ - Q_OBJECT -public: - RosoutWindow() : QMainWindow() - { - } - ~RosoutWindow() override = default; - - void closeEvent(QCloseEvent*) override - { - emit closed(); - } -signals: - void closed(); -}; - -class RosoutPublisher : public PJ::StatePublisher -{ - Q_OBJECT - Q_PLUGIN_METADATA(IID "facontidavide.PlotJuggler3.StatePublisher") - Q_INTERFACES(PJ::StatePublisher) - -public: - RosoutPublisher(); - - virtual void updateState(double current_time) override; - virtual const char* name() const override - { - return "ROS /rosout Visualization"; - } - virtual ~RosoutPublisher(); - - virtual bool enabled() const override - { - return _enabled; - } - - virtual void play(double interval) override - { - // TODO - } - -public slots: - virtual void setEnabled(bool enabled) override; - -private slots: - - void onWindowClosed(); - -private: - bool _enabled; - int64_t _minimum_time_usec, _maximum_time_usec; - - LogsTableModel* _tablemodel; - rqt_console_plus::LogWidget* _log_widget; - - std::vector findRosoutTimeseries(); - void syncWithTableModel(const std::vector& logs_timeseries); - - RosoutWindow* _log_window; -signals: - void timeRangeChanged(TimePoint time_min, TimePoint time_max); -}; - -#endif // ROSOUT_PUBLISHER_ROS_H diff --git a/src/TopicPublisherROS/statepublisher_rostopic.cpp b/src/TopicPublisherROS/statepublisher_rostopic.cpp deleted file mode 100644 index 8cfe6d65..00000000 --- a/src/TopicPublisherROS/statepublisher_rostopic.cpp +++ /dev/null @@ -1,464 +0,0 @@ -#include "statepublisher_rostopic.h" -#include "qnodedialog.h" - -#include "ros_type_introspection/ros_introspection.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "publisher_select_dialog.h" - -using namespace PJ; - -TopicPublisherROS::TopicPublisherROS() : - _enabled(false) -, _node(nullptr) -, _publish_clock(true) -{ - QSettings settings; - _publish_clock = settings.value("TopicPublisherROS/publish_clock", true).toBool(); - - _select_topics_to_publish = new QAction(QString("Select topics to be published")); - connect(_select_topics_to_publish, &QAction::triggered, - this, &TopicPublisherROS::filterDialog); - - _available_actions.push_back(_select_topics_to_publish); -} - -TopicPublisherROS::~TopicPublisherROS() -{ - _enabled = false; -} - -const std::vector &TopicPublisherROS::availableActions() -{ - return _available_actions; -} - -void TopicPublisherROS::setEnabled(bool to_enable) -{ - if (!_node && to_enable) - { - _node = RosManager::getNode(); - } - _enabled = (to_enable && _node); - - if (_enabled) - { - filterDialog(true); - - if (!_tf_broadcaster) - { - _tf_broadcaster = std::make_unique(); - } - if (!_tf_static_broadcaster) - { - _tf_static_broadcaster = std::make_unique(); - } - - _previous_play_index = std::numeric_limits::max(); - - if (_publish_clock) - { - _clock_publisher = _node->advertise("/clock", 10, true); - } - else - { - _clock_publisher.shutdown(); - } - } - else - { - _node.reset(); - _publishers.clear(); - _clock_publisher.shutdown(); - - _tf_broadcaster.reset(); - _tf_static_broadcaster.reset(); - - emit closed(); - } -} - -void TopicPublisherROS::filterDialog(bool autoconfirm) -{ - auto all_topics = RosIntrospectionFactory::get().getTopicList(); - - if (all_topics.empty()){ - return; - } - - PublisherSelectDialog *dialog = new PublisherSelectDialog(); - - std::map checkbox; - - for (const auto& topic : all_topics) - { - auto cb = new QCheckBox(dialog); - auto filter_it = _topics_to_publish.find(*topic); - if (filter_it == _topics_to_publish.end()) - { - cb->setChecked(true); - } - else - { - cb->setChecked(filter_it->second); - } - cb->setFocusPolicy(Qt::NoFocus); - dialog->ui()->formLayout->addRow(new QLabel(QString::fromStdString(*topic)), cb); - checkbox.insert(std::make_pair(*topic, cb)); - connect(dialog->ui()->pushButtonSelect, &QPushButton::pressed, [cb]() { cb->setChecked(true); }); - connect(dialog->ui()->pushButtonDeselect, &QPushButton::pressed, [cb]() { cb->setChecked(false); }); - } - - if (!autoconfirm) - { - dialog->exec(); - } - - if (autoconfirm || dialog->result() == QDialog::Accepted) - { - _topics_to_publish.clear(); - for (const auto& it : checkbox) - { - _topics_to_publish.insert({ it.first, it.second->isChecked() }); - } - - // remove already created publisher if not needed anymore - for (auto it = _publishers.begin(); it != _publishers.end(); /* no increment */) - { - const std::string& topic_name = it->first; - if (!toPublish(topic_name)) - { - it = _publishers.erase(it); - } - else - { - it++; - } - } - - _publish_clock = dialog->ui()->radioButtonClock->isChecked(); - - if (_enabled && _publish_clock) - { - _clock_publisher = _node->advertise("/clock", 10, true); - } - else - { - _clock_publisher.shutdown(); - } - - dialog->deleteLater(); - - QSettings settings; - settings.setValue("TopicPublisherROS/publish_clock", _publish_clock); - } -} - -void TopicPublisherROS::broadcastTF(double current_time) -{ - using StringPair = std::pair; - - std::map transforms; - std::map static_transforms; - - for (const auto& data_it : _datamap->user_defined) - { - const std::string& topic_name = data_it.first; - const PlotDataAny& plot_any = data_it.second; - - if (!toPublish(topic_name)) - { - continue; // Not selected - } - - if (topic_name != "/tf_static" && topic_name != "/tf") - { - continue; - } - - const PlotDataAny* tf_data = &plot_any; - int last_index = tf_data->getIndexFromX(current_time); - if (last_index < 0) - { - continue; - } - - auto transforms_ptr = (topic_name == "/tf_static") ? & static_transforms : &transforms; - - std::vector raw_buffer; - // 2 seconds in the past (to be configurable in the future) - int initial_index = tf_data->getIndexFromX(current_time - 2.0); - - if (_previous_play_index < last_index && _previous_play_index > initial_index) - { - initial_index = _previous_play_index; - } - - for (size_t index = std::max(0, initial_index); index <= last_index; index++) - { - const std::any& any_value = tf_data->at(index).y; - - const bool isRosbagMessage = any_value.type() == typeid(rosbag::MessageInstance); - - if (!isRosbagMessage) - { - continue; - } - - const auto& msg_instance = std::any_cast(any_value); - - raw_buffer.resize(msg_instance.size()); - ros::serialization::OStream ostream(raw_buffer.data(), raw_buffer.size()); - msg_instance.write(ostream); - - tf2_msgs::TFMessage tf_msg; - ros::serialization::IStream istream(raw_buffer.data(), raw_buffer.size()); - ros::serialization::deserialize(istream, tf_msg); - - for (const auto& stamped_transform : tf_msg.transforms) - { - const auto& parent_id = stamped_transform.header.frame_id; - const auto& child_id = stamped_transform.child_frame_id; - StringPair trans_id = std::make_pair(parent_id, child_id); - auto it = transforms_ptr->find(trans_id); - if (it == transforms_ptr->end()) - { - transforms_ptr->insert({ trans_id, stamped_transform }); - } - else if (it->second.header.stamp <= stamped_transform.header.stamp) - { - it->second = stamped_transform; - } - } - } - // ready to broadacast - std::vector transforms_vector; - transforms_vector.reserve(transforms_ptr->size()); - - const auto now = ros::Time::now(); - for (auto& trans : *transforms_ptr) - { - if (!_publish_clock) - { - trans.second.header.stamp = now; - } - transforms_vector.emplace_back(std::move(trans.second)); - } - if( transforms_vector.size() > 0) - { - if( transforms_ptr == &transforms ){ - _tf_broadcaster->sendTransform(transforms_vector); - } - else{ - _tf_static_broadcaster->sendTransform(transforms_vector); - } - } - } -} - -bool TopicPublisherROS::toPublish(const std::string& topic_name) -{ - auto it = _topics_to_publish.find(topic_name); - if (it == _topics_to_publish.end()) - { - return false; - } - else - { - return it->second; - } -} - -void TopicPublisherROS::publishAnyMsg(const rosbag::MessageInstance& msg_instance) -{ - using namespace RosIntrospection; - - const auto& topic_name = msg_instance.getTopic(); - RosIntrospection::ShapeShifter* shapeshifted = RosIntrospectionFactory::get().getShapeShifter(topic_name); - - if (!shapeshifted) - { - return; // Not registered, just skip - } - - std::vector raw_buffer; - raw_buffer.resize(msg_instance.size()); - ros::serialization::OStream ostream(raw_buffer.data(), raw_buffer.size()); - msg_instance.write(ostream); - - if (!_publish_clock) - { - const ROSMessageInfo* msg_info = RosIntrospectionFactory::parser().getMessageInfo(topic_name); - if (msg_info && msg_info->message_tree.croot()->children().size() >= 1) - { - const auto& first_field = msg_info->message_tree.croot()->child(0)->value(); - if (first_field->type().baseName() == "std_msgs/Header") - { - std_msgs::Header msg; - ros::serialization::IStream is(raw_buffer.data(), raw_buffer.size()); - ros::serialization::deserialize(is, msg); - msg.stamp = ros::Time::now(); - ros::serialization::OStream os(raw_buffer.data(), raw_buffer.size()); - ros::serialization::serialize(os, msg); - } - } - } - - ros::serialization::IStream istream(raw_buffer.data(), raw_buffer.size()); - shapeshifted->read(istream); - - auto publisher_it = _publishers.find(topic_name); - if (publisher_it == _publishers.end()) - { - auto res = _publishers.insert({ topic_name, shapeshifted->advertise(*_node, topic_name, 10, true) }); - publisher_it = res.first; - } - - const ros::Publisher& publisher = publisher_it->second; - publisher.publish(*shapeshifted); -} - -void TopicPublisherROS::updateState(double current_time) -{ - if (!_enabled || !_node) - return; - - if (!ros::master::check()) - { - QMessageBox::warning(nullptr, tr("Disconnected!"), - "The roscore master cannot be detected.\n" - "The publisher will be disabled."); - emit closed(); - return; - } - - //----------------------------------------------- - broadcastTF(current_time); - //----------------------------------------------- - - auto data_it = _datamap->user_defined.find("plotjuggler::rosbag1::consecutive_messages"); - if (data_it != _datamap->user_defined.end()) - { - const PlotDataAny& continuous_msgs = data_it->second; - _previous_play_index = continuous_msgs.getIndexFromX(current_time); - // qDebug() << QString("u: %1").arg( current_index ).arg(current_time, 0, 'f', 4 ); - } - - for (const auto& data_it : _datamap->user_defined) - { - const std::string& topic_name = data_it.first; - const PlotDataAny& plot_any = data_it.second; - if (!toPublish(topic_name)) - { - continue; // Not selected - } - - const RosIntrospection::ShapeShifter* shapeshifter = RosIntrospectionFactory::get().getShapeShifter(topic_name); - - if (shapeshifter->getDataType() == "tf/tfMessage" || shapeshifter->getDataType() == "tf2_msgs/TFMessage") - { - continue; - } - - int last_index = plot_any.getIndexFromX(current_time); - if (last_index < 0) - { - continue; - } - - const auto& any_value = plot_any.at(last_index).y; - - if (any_value.type() == typeid(rosbag::MessageInstance)) - { - const auto& msg_instance = std::any_cast(any_value); - publishAnyMsg(msg_instance); - } - } - - if (_publish_clock) - { - rosgraph_msgs::Clock clock; - try - { - clock.clock.fromSec(current_time); - _clock_publisher.publish(clock); - } - catch (...) - { - qDebug() << "error: " << current_time; - } - } -} - -void TopicPublisherROS::play(double current_time) -{ - if (!_enabled || !_node) - return; - - if (!ros::master::check()) - { - QMessageBox::warning(nullptr, tr("Disconnected!"), - "The roscore master cannot be detected.\n" - "The publisher will be disabled."); - emit closed(); - return; - } - - auto data_it = _datamap->user_defined.find("plotjuggler::rosbag1::consecutive_messages"); - if (data_it == _datamap->user_defined.end()) - { - return; - } - const PlotDataAny& continuous_msgs = data_it->second; - int current_index = continuous_msgs.getIndexFromX(current_time); - - if (_previous_play_index > current_index) - { - _previous_play_index = current_index; - updateState(current_time); - return; - } - else - { - const PlotDataAny& consecutive_msg = data_it->second; - for (int index = _previous_play_index + 1; index <= current_index; index++) - { - const auto& any_value = consecutive_msg.at(index).y; - if (any_value.type() == typeid(rosbag::MessageInstance)) - { - const auto& msg_instance = std::any_cast(any_value); - - if (!toPublish(msg_instance.getTopic())) - { - continue; // Not selected - } - - // qDebug() << QString("p: %1").arg( index ); - publishAnyMsg(msg_instance); - - if (_publish_clock) - { - rosgraph_msgs::Clock clock; - clock.clock = msg_instance.getTime(); - _clock_publisher.publish(clock); - } - } - } - _previous_play_index = current_index; - } -} diff --git a/src/TopicPublisherROS/statepublisher_rostopic.h b/src/TopicPublisherROS/statepublisher_rostopic.h deleted file mode 100644 index cc76d3e8..00000000 --- a/src/TopicPublisherROS/statepublisher_rostopic.h +++ /dev/null @@ -1,78 +0,0 @@ -#ifndef STATE_PUBLISHER_ROSTOPIC_H -#define STATE_PUBLISHER_ROSTOPIC_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include "shape_shifter_factory.hpp" -#include - -class TopicPublisherROS : public PJ::StatePublisher -{ - Q_OBJECT - Q_PLUGIN_METADATA(IID "facontidavide.PlotJuggler3.StatePublisher") - Q_INTERFACES(PJ::StatePublisher) - -public: - TopicPublisherROS(); - - virtual ~TopicPublisherROS() override; - - virtual void updateState(double current_time) override; - - virtual const char* name() const override - { - return "ROS Topic Re-Publisher"; - } - - virtual bool enabled() const override - { - return _enabled; - } - - virtual void play(double interval) override; - - const std::vector& availableActions() override; - -public slots: - virtual void setEnabled(bool enabled) override; - - void filterDialog(bool autoconfirm); - -private: - void broadcastTF(double current_time); - - std::map _publishers; - - bool _enabled; - - ros::NodeHandlePtr _node; - - bool _publish_clock; - - std::shared_ptr _tf_broadcaster; - std::shared_ptr _tf_static_broadcaster; - - ros::Publisher _clock_publisher; - - QAction* _select_topics_to_publish; - - std::unordered_map _topics_to_publish; - - bool toPublish(const std::string& topic_name); - - double previous_time; - - int _previous_play_index; - - std::vector _available_actions; - - void publishAnyMsg(const rosbag::MessageInstance& msg_instance); -}; - -#endif // DATALOAD_CSV_H diff --git a/src/ros_parsers/ros1_parser.h b/src/ros_parsers/ros1_parser.h deleted file mode 100644 index 3e4c38bc..00000000 --- a/src/ros_parsers/ros1_parser.h +++ /dev/null @@ -1,15 +0,0 @@ -#pragma once - -#include -#include - -inline std::shared_ptr -CreateParserROS(const PJ::ParserFactories &factories, - const std::string& topic_name, - const std::string& type_name, - const std::string& definition, - PJ::PlotDataMapRef& data) -{ - return factories.at("ros1msg")->createParser(topic_name, type_name, definition, data); -} - diff --git a/src/shape_shifter_factory.hpp b/src/shape_shifter_factory.hpp deleted file mode 100644 index 1ea469d1..00000000 --- a/src/shape_shifter_factory.hpp +++ /dev/null @@ -1,93 +0,0 @@ -#ifndef SHAPE_SHIFTER_FACTORY_HPP -#define SHAPE_SHIFTER_FACTORY_HPP - -#include - -class RosIntrospectionFactory{ - -public: - static RosIntrospectionFactory &get(); - - static void registerMessage(const std::string& topic_name, - const std::string &md5sum, - const std::string& datatype, - const std::string& definition ); - - static RosIntrospection::ShapeShifter* getShapeShifter(const std::string& topic_name); - - static std::vector getTopicList(); - - static RosIntrospection::Parser& parser() - { - return get()._parser; - } - - static bool isRegistered(const std::string& topic_name); - - static void reset(); - -private: - RosIntrospectionFactory() = default; - std::map _ss_map; - RosIntrospection::Parser _parser; - -}; -//--------------------------------------------- - -inline RosIntrospectionFactory& RosIntrospectionFactory::get() -{ - static RosIntrospectionFactory instance; - return instance; -} - -// return true if added -inline void RosIntrospectionFactory::registerMessage(const std::string &topic_name, - const std::string &md5sum, - const std::string &datatype, - const std::string &definition) -{ - auto& instance = get(); - auto it = instance._ss_map.find(topic_name); - if( it == instance._ss_map.end() || it->second.getMD5Sum() != md5sum ) - { - RosIntrospection::ShapeShifter msg; - msg.morph(md5sum, datatype,definition); - instance._ss_map.insert( std::make_pair(topic_name, std::move(msg) )); - parser().registerMessageDefinition( topic_name, RosIntrospection::ROSType(datatype), definition); - } -} - -inline RosIntrospection::ShapeShifter* RosIntrospectionFactory::getShapeShifter(const std::string &topic_name) -{ - auto& instance = get(); - auto it = instance._ss_map.find( topic_name ); - return ( it == instance._ss_map.end()) ? nullptr : &(it->second); -} - -inline std::vector RosIntrospectionFactory::getTopicList() -{ - std::vector out; - auto& instance = get(); - out.reserve( instance._ss_map.size() ); - - for (const auto& ss: instance._ss_map) - { - out.push_back( &(ss.first) ); - } - return out; -} - -inline bool RosIntrospectionFactory::isRegistered(const std::string &topic_name) -{ - return get()._ss_map.count(topic_name) != 0; -} - -inline void RosIntrospectionFactory::reset() -{ - get()._ss_map.clear(); -} - -#endif // SHAPE_SHIFTER_FACTORY_HPP - - -