From b11d3714367495e77ddac3e284476babe6c190c7 Mon Sep 17 00:00:00 2001 From: philippewarren <55722645+philippewarren@users.noreply.github.com> Date: Fri, 25 Mar 2022 10:58:59 -0400 Subject: [PATCH] Format (#43) * Updated .clang-format * Adjust trailing whitespace and EOF newline * Ran clang-format on C++ files * Ran autopep8 on Python files * Formatted CMake tabs * Formatted xml tabs * Other formatting * Update opentera-webrtc to formatted version * Updated opentera-webrtc-teleop-frontend to formatted version * Update opentera-webrtc-teleop-frontend --- .clang-format | 67 +++-- drawings/GlobalSetup.drawio | 2 +- drawings/README.md | 1 - map_image_generator/CMakeLists.txt | 97 +++---- .../map_image_generator/GoalConverter.h | 6 +- .../map_image_generator/MapImageGenerator.h | 3 +- .../map_image_generator/MapLabelsConverter.h | 3 +- .../include/map_image_generator/Parameters.h | 91 +++--- .../drawers/GlobalPathImageDrawer.h | 9 +- .../drawers/GoalImageDrawer.h | 9 +- .../map_image_generator/drawers/ImageDrawer.h | 26 +- .../drawers/LabelImageDrawer.h | 9 +- .../drawers/LaserScanImageDrawer.h | 9 +- .../drawers/OccupancyGridImageDrawer.h | 27 +- .../drawers/RobotImageDrawer.h | 3 +- .../drawers/SoundSourceImageDrawer.h | 25 +- .../include/map_image_generator/utils.h | 32 +-- map_image_generator/package.xml | 164 +++++------ map_image_generator/src/GoalConverter.cpp | 20 +- map_image_generator/src/MapImageGenerator.cpp | 31 +-- .../src/MapLabelsConverter.cpp | 18 +- map_image_generator/src/Parameters.cpp | 40 ++- .../src/drawers/GlobalPathImageDrawer.cpp | 20 +- .../src/drawers/GoalImageDrawer.cpp | 53 ++-- .../src/drawers/ImageDrawer.cpp | 42 +-- .../src/drawers/LabelImageDrawer.cpp | 36 ++- .../src/drawers/LaserScanImageDrawer.cpp | 19 +- .../src/drawers/OccupancyGridImageDrawer.cpp | 156 +++++------ .../src/drawers/RobotImageDrawer.cpp | 21 +- .../src/drawers/SoundSourceImageDrawer.cpp | 67 ++--- map_image_generator/src/main.cpp | 3 +- map_image_generator/src/utils.cpp | 49 ++-- .../srv/ImageGoalToMapGoal.srv | 2 +- opentera_client_ros/CMakeLists.txt | 18 +- opentera_client_ros/README.md | 1 - opentera_client_ros/config/client_config.json | 2 +- opentera_client_ros/launch/client.launch | 2 +- opentera_client_ros/package.xml | 106 +++---- opentera_client_ros/requirements.txt | 1 - .../scripts/opentera_client_ros.py | 15 +- opentera_client_ros/setup.py | 6 +- opentera_webrtc_demos/config/model.rviz | 27 +- opentera_webrtc_demos/launch/demo-dev.launch | 2 +- opentera_webrtc_demos/launch/demo-odas.launch | 82 +++--- .../launch/model_viewer.launch | 12 +- .../launch/opentera_signaling_server.launch | 30 +- .../launch/opentera_turtlebot_sim.launch | 214 +++++++-------- .../models/aruco_tag/model.config | 2 +- .../models/aruco_tag/model.sdf | 2 +- .../opentera-webrtc-teleop-frontend | 2 +- opentera_webrtc_demos/scripts/robot_status.py | 47 ++-- opentera_webrtc_robot_gui/CMakeLists.txt | 53 ++-- .../launch/test_gui.launch | 2 +- opentera_webrtc_robot_gui/package.xml | 118 ++++---- opentera_webrtc_robot_gui/requirements.txt | 2 +- .../scripts/opentera_webrtc_robot_gui.py | 13 +- opentera_webrtc_robot_gui/setup.py | 6 +- .../src/ConfigDialog.cpp | 8 +- opentera_webrtc_robot_gui/src/ConfigDialog.h | 16 +- .../src/GraphicsViewToolbar.cpp | 31 ++- .../src/GraphicsViewToolbar.h | 10 +- opentera_webrtc_robot_gui/src/MainWindow.cpp | 258 +++++++++--------- opentera_webrtc_robot_gui/src/MainWindow.h | 157 ++++++----- .../src/ROSCameraView.cpp | 48 ++-- opentera_webrtc_robot_gui/src/ROSCameraView.h | 21 +- opentera_webrtc_robot_gui/src/main.cpp | 27 +- .../resources/opentera_webrtc_robot_gui.qrc | 2 +- .../src/resources/stylesheet.qss | 246 ++++++++--------- opentera_webrtc_ros/CMakeLists.txt | 104 +++---- opentera_webrtc_ros/include/RosAudioSource.h | 4 +- .../include/RosDataChannelBridge.h | 10 +- .../include/RosJsonDataHandler.h | 6 +- .../include/RosNodeParameters.h | 54 ++-- opentera_webrtc_ros/include/RosParamUtils.h | 60 ++-- .../include/RosSignalingServerconfiguration.h | 7 +- opentera_webrtc_ros/include/RosStreamBridge.h | 17 +- opentera_webrtc_ros/include/RosVideoSource.h | 1 - opentera_webrtc_ros/include/RosWebRTCBridge.h | 168 +++++++----- .../launch/opentera_data_test.launch | 42 +-- .../launch/ros_data_channel_client.launch | 40 +-- .../launch/ros_json_data_handler.launch | 26 +- .../launch/ros_stream_client.launch | 144 +++++----- opentera_webrtc_ros/launch/sim_beam.launch | 26 +- opentera_webrtc_ros/opentera-webrtc | 2 +- opentera_webrtc_ros/package.xml | 46 ++-- .../scripts/opentera_webrtc_audio_mixer.py | 11 +- opentera_webrtc_ros/src/RosAudioSource.cpp | 59 ++-- .../src/RosDataChannelBridge.cpp | 50 ++-- .../src/RosJsonDataHandler.cpp | 49 ++-- opentera_webrtc_ros/src/RosNodeParameters.cpp | 68 ++--- opentera_webrtc_ros/src/RosParamUtils.cpp | 130 ++++----- .../src/RosSignalingServerConfiguration.cpp | 19 +- opentera_webrtc_ros/src/RosStreamBridge.cpp | 158 ++++++----- opentera_webrtc_ros/src/RosVideoSource.cpp | 4 +- opentera_webrtc_ros_msgs/CMakeLists.txt | 82 +++--- .../msg/DatabaseEvent.msg | 2 +- opentera_webrtc_ros_msgs/msg/DeviceEvent.msg | 2 +- .../msg/JoinSessionReplyEvent.msg | 2 +- opentera_webrtc_ros_msgs/msg/LogEvent.msg | 2 +- .../msg/OpenTeraEvent.msg | 1 - opentera_webrtc_ros_msgs/msg/Peer.msg | 2 +- opentera_webrtc_ros_msgs/msg/PeerAudio.msg | 2 +- opentera_webrtc_ros_msgs/msg/PeerData.msg | 2 +- opentera_webrtc_ros_msgs/msg/PeerStatus.msg | 2 +- opentera_webrtc_ros_msgs/msg/RobotStatus.msg | 2 +- opentera_webrtc_ros_msgs/msg/UserEvent.msg | 2 +- opentera_webrtc_ros_msgs/msg/Waypoint.msg | 2 +- .../msg/WaypointArray.msg | 2 +- opentera_webrtc_ros_msgs/package.xml | 116 ++++---- turtlebot3_beam_description/CHANGELOG.rst | 2 +- turtlebot3_beam_description/CMakeLists.txt | 10 +- turtlebot3_beam_description/package.xml | 34 +-- turtlebot3_beam_description/rviz/model.rviz | 15 +- .../urdf/accessories/camera.urdf.xacro | 14 +- .../urdf/accessories/r200.urdf.xacro | 2 +- .../urdf/common_properties.xacro | 2 +- ...ot3_waffle_for_open_manipulator.urdf.xacro | 4 +- ..._waffle_pi_for_open_manipulator.urdf.xacro | 2 +- 118 files changed, 2192 insertions(+), 2170 deletions(-) diff --git a/.clang-format b/.clang-format index 1d5cd3f..0a453ce 100644 --- a/.clang-format +++ b/.clang-format @@ -1,18 +1,21 @@ ---- +# LLVM 12 +--- BasedOnStyle: LLVM AccessModifierOffset: -4 -AlignAfterOpenBracket: Align -AlignConsecutiveMacros: AcrossEmptyLinesAndComments +AlignAfterOpenBracket: AlwaysBreak AlignConsecutiveAssignments: None +AlignConsecutiveBitFields: None AlignConsecutiveDeclarations: None +AlignConsecutiveMacros: AcrossComments AlignEscapedNewlines: Right -AlignOperands: true -AlignTrailingComments: true +AlignOperands: Align +AlignTrailingComments: false AllowAllArgumentsOnNextLine: false -AllowAllConstructorInitializersOnNextLine: true +AllowAllConstructorInitializersOnNextLine: false AllowAllParametersOfDeclarationOnNextLine: false -AllowShortBlocksOnASingleLine: Never +AllowShortBlocksOnASingleLine: Always AllowShortCaseLabelsOnASingleLine: false +AllowShortEnumsOnASingleLine: false AllowShortFunctionsOnASingleLine: Inline AllowShortIfStatementsOnASingleLine: Never AllowShortLambdasOnASingleLine: All @@ -20,53 +23,71 @@ AllowShortLoopsOnASingleLine: false AlwaysBreakAfterReturnType: None AlwaysBreakBeforeMultilineStrings: false AlwaysBreakTemplateDeclarations: Yes -BinPackArguments: true -BinPackParameters: true -BreakBeforeBinaryOperators: NonAssignment +AttributeMacros: [] +BinPackArguments: false +BinPackParameters: false +BitFieldColonSpacing: After BreakBeforeBraces: Allman -BreakBeforeTernaryOperators: true +BreakBeforeBinaryOperators: None +BreakBeforeConceptDeclarations: true +BreakBeforeTernaryOperators: false BreakConstructorInitializers: BeforeColon BreakInheritanceList: BeforeColon BreakStringLiterals: true -ColumnLimit: 90 +ColumnLimit: 120 CompactNamespaces: false -ConstructorInitializerAllOnOneLineOrOnePerLine: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true ConstructorInitializerIndentWidth: 4 ContinuationIndentWidth: 4 Cpp11BracedListStyle: true +DeriveLineEnding: false +UseCRLF: false DerivePointerAlignment: false -DisableFormat: false +EmptyLineBeforeAccessModifier: LogicalBlock FixNamespaceComments: false -IncludeBlocks: Regroup +ForEachMacros: [] +IncludeBlocks: Preserve +IndentCaseBlocks: false IndentCaseLabels: true -IndentPPDirectives: BeforeHash +IndentExternBlock: NoIndent +IndentGotoLabels: true +IndentPPDirectives: None IndentWidth: 4 -IndentWrappedFunctionNames: false +IndentWrappedFunctionNames: true KeepEmptyLinesAtTheStartOfBlocks: false Language: Cpp MaxEmptyLinesToKeep: 2 +MacroBlockBegin: "^QT_BEGIN_NAMESPACE" +MacroBlockEnd: "QT_END_NAMESPACE$" NamespaceIndentation: All PointerAlignment: Left ReflowComments: true -SortIncludes: true -SortUsingDeclarations: true +SortIncludes: false +SortUsingDeclarations: false SpaceAfterCStyleCast: false SpaceAfterLogicalNot: false -SpaceAfterTemplateKeyword: true +SpaceAfterTemplateKeyword: false +SpaceAroundPointerQualifiers: Default SpaceBeforeAssignmentOperators: true +SpaceBeforeCaseColon: false SpaceBeforeCpp11BracedList: false SpaceBeforeCtorInitializerColon: true SpaceBeforeInheritanceColon: true SpaceBeforeParens: ControlStatements SpaceBeforeRangeBasedForLoopColon: true +SpaceBeforeSquareBrackets: false +SpaceInEmptyBlock: false SpaceInEmptyParentheses: false -SpacesInAngles: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: Never SpacesInCStyleCastParentheses: false +SpacesInConditionalStatement: false SpacesInContainerLiterals: false SpacesInParentheses: false SpacesInSquareBrackets: false Standard: c++14 +StatementAttributeLikeMacros: [emit] +StatementMacros: [Q_UNUSED, Q_OBJECT] TabWidth: 4 +UseCRLF: false UseTab: Never - -... diff --git a/drawings/GlobalSetup.drawio b/drawings/GlobalSetup.drawio index e5b0e69..1673495 100644 --- a/drawings/GlobalSetup.drawio +++ b/drawings/GlobalSetup.drawio @@ -1 +1 @@ -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 \ No newline at end of file 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 diff --git a/drawings/README.md b/drawings/README.md index 4176801..c30e0fd 100644 --- a/drawings/README.md +++ b/drawings/README.md @@ -2,4 +2,3 @@ Open the following URLs with draw.io. * [GlobalSetup.drawio](https://viewer.diagrams.net/?url=https://github.com/introlab/opentera-webrtc-ros/raw/main/drawings/GlobalSetup.drawio) - diff --git a/map_image_generator/CMakeLists.txt b/map_image_generator/CMakeLists.txt index 5b3dfde..1d1fc38 100644 --- a/map_image_generator/CMakeLists.txt +++ b/map_image_generator/CMakeLists.txt @@ -8,19 +8,19 @@ project(map_image_generator) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS - cv_bridge - geometry_msgs - image_transport - nav_msgs - roscpp - sensor_msgs - std_msgs - tf - rtabmap_ros - message_generation - std_srvs - odas_ros - opentera_webrtc_ros_msgs + cv_bridge + geometry_msgs + image_transport + nav_msgs + roscpp + sensor_msgs + std_msgs + tf + rtabmap_ros + message_generation + std_srvs + odas_ros + opentera_webrtc_ros_msgs ) ## System dependencies are found with CMake's conventions @@ -66,9 +66,9 @@ find_package(OpenCV REQUIRED) ## Generate services in the 'srv' folder add_service_files( - FILES - ImageGoalToMapGoal.srv - ChangeMapView.srv + FILES + ImageGoalToMapGoal.srv + ChangeMapView.srv ) ## Generate actions in the 'action' folder @@ -80,9 +80,9 @@ add_service_files( ## Generate added messages and services with any dependencies listed here generate_messages( - DEPENDENCIES - geometry_msgs - std_msgs + DEPENDENCIES + geometry_msgs + std_msgs ) ################################################ @@ -128,38 +128,39 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations include_directories( - include - ${catkin_INCLUDE_DIRS} + include + ${catkin_INCLUDE_DIRS} ) add_executable(map_image_generator - include/map_image_generator/drawers/GlobalPathImageDrawer.h - include/map_image_generator/drawers/GoalImageDrawer.h - include/map_image_generator/drawers/ImageDrawer.h - include/map_image_generator/drawers/LabelImageDrawer.h - include/map_image_generator/drawers/LaserScanImageDrawer.h - include/map_image_generator/drawers/OccupancyGridImageDrawer.h - include/map_image_generator/drawers/RobotImageDrawer.h - include/map_image_generator/drawers/SoundSourceImageDrawer.h - include/map_image_generator/GoalConverter.h - include/map_image_generator/MapImageGenerator.h - include/map_image_generator/MapLabelsConverter.h - include/map_image_generator/Parameters.h - include/map_image_generator/utils.h - src/drawers/GlobalPathImageDrawer.cpp - src/drawers/GoalImageDrawer.cpp - src/drawers/ImageDrawer.cpp - src/drawers/LabelImageDrawer.cpp - src/drawers/LaserScanImageDrawer.cpp - src/drawers/OccupancyGridImageDrawer.cpp - src/drawers/RobotImageDrawer.cpp - src/drawers/SoundSourceImageDrawer.cpp - src/GoalConverter.cpp - src/main.cpp - src/MapImageGenerator.cpp - src/MapLabelsConverter.cpp - src/Parameters.cpp - src/utils.cpp) + include/map_image_generator/drawers/GlobalPathImageDrawer.h + include/map_image_generator/drawers/GoalImageDrawer.h + include/map_image_generator/drawers/ImageDrawer.h + include/map_image_generator/drawers/LabelImageDrawer.h + include/map_image_generator/drawers/LaserScanImageDrawer.h + include/map_image_generator/drawers/OccupancyGridImageDrawer.h + include/map_image_generator/drawers/RobotImageDrawer.h + include/map_image_generator/drawers/SoundSourceImageDrawer.h + include/map_image_generator/GoalConverter.h + include/map_image_generator/MapImageGenerator.h + include/map_image_generator/MapLabelsConverter.h + include/map_image_generator/Parameters.h + include/map_image_generator/utils.h + src/drawers/GlobalPathImageDrawer.cpp + src/drawers/GoalImageDrawer.cpp + src/drawers/ImageDrawer.cpp + src/drawers/LabelImageDrawer.cpp + src/drawers/LaserScanImageDrawer.cpp + src/drawers/OccupancyGridImageDrawer.cpp + src/drawers/RobotImageDrawer.cpp + src/drawers/SoundSourceImageDrawer.cpp + src/GoalConverter.cpp + src/main.cpp + src/MapImageGenerator.cpp + src/MapLabelsConverter.cpp + src/Parameters.cpp + src/utils.cpp +) target_link_libraries(map_image_generator ${catkin_LIBRARIES} diff --git a/map_image_generator/include/map_image_generator/GoalConverter.h b/map_image_generator/include/map_image_generator/GoalConverter.h index 69c9838..7f3f2bb 100644 --- a/map_image_generator/include/map_image_generator/GoalConverter.h +++ b/map_image_generator/include/map_image_generator/GoalConverter.h @@ -24,13 +24,11 @@ namespace map_image_generator ros::ServiceServer image_goal_to_map_goal_service; public: - GoalConverter(const Parameters& parameters, ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener); + GoalConverter(const Parameters& parameters, ros::NodeHandle& nodeHandle, tf::TransformListener& tfListener); virtual ~GoalConverter(); private: - bool mapImageGoalCallback(ImageGoalToMapGoal::Request& req, - ImageGoalToMapGoal::Response& res); + bool mapImageGoalCallback(ImageGoalToMapGoal::Request& req, ImageGoalToMapGoal::Response& res); }; } #endif diff --git a/map_image_generator/include/map_image_generator/MapImageGenerator.h b/map_image_generator/include/map_image_generator/MapImageGenerator.h index 32fa78d..e829ab0 100644 --- a/map_image_generator/include/map_image_generator/MapImageGenerator.h +++ b/map_image_generator/include/map_image_generator/MapImageGenerator.h @@ -24,8 +24,7 @@ namespace map_image_generator cv_bridge::CvImage m_cvImage; public: - MapImageGenerator(Parameters& parameters, ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener); + MapImageGenerator(Parameters& parameters, ros::NodeHandle& nodeHandle, tf::TransformListener& tfListener); virtual ~MapImageGenerator(); void generate(sensor_msgs::Image& sensorImage); diff --git a/map_image_generator/include/map_image_generator/MapLabelsConverter.h b/map_image_generator/include/map_image_generator/MapLabelsConverter.h index 164679b..3887289 100644 --- a/map_image_generator/include/map_image_generator/MapLabelsConverter.h +++ b/map_image_generator/include/map_image_generator/MapLabelsConverter.h @@ -23,8 +23,7 @@ namespace map_image_generator virtual ~MapLabelsConverter(); private: - void - mapLabelsCallback(const visualization_msgs::MarkerArray::ConstPtr& mapLabels); + void mapLabelsCallback(const visualization_msgs::MarkerArray::ConstPtr& mapLabels); std::vector getDesiredLabels(); }; } diff --git a/map_image_generator/include/map_image_generator/Parameters.h b/map_image_generator/include/map_image_generator/Parameters.h index fc73640..016627a 100644 --- a/map_image_generator/include/map_image_generator/Parameters.h +++ b/map_image_generator/include/map_image_generator/Parameters.h @@ -10,14 +10,14 @@ namespace map_image_generator class Parameters { double m_refreshRate; - int m_resolution; // pixel/m - int m_width; // m - int m_height; // m - int m_xOrigin; // pixel - int m_yOrigin; // pixel - int m_robotVerticalOffset; // pixel - float m_soundSourceRange; // m - float m_soundSourceMaxRange; // m + int m_resolution; // pixel/m + int m_width; // m + int m_height; // m + int m_xOrigin; // pixel + int m_yOrigin; // pixel + int m_robotVerticalOffset; // pixel + float m_soundSourceRange; // m + float m_soundSourceMaxRange; // m double m_scaleFactor; @@ -45,26 +45,26 @@ namespace map_image_generator cv::Scalar m_soundSourceColorFull; cv::Scalar m_soundSourceColorDim; - int m_globalPathThickness; // pixel - int m_robotSize; // pixel - int m_goalSize; // pixel - int m_laserScanSize; // pixel - int m_labelSize; // pixel - int m_soundSourceSize; // pixel + int m_globalPathThickness; // pixel + int m_robotSize; // pixel + int m_goalSize; // pixel + int m_laserScanSize; // pixel + int m_labelSize; // pixel + int m_soundSourceSize; // pixel public: explicit Parameters(ros::NodeHandle& nodeHandle); virtual ~Parameters(); double refreshRate() const; - int resolution() const; // pixel/m - int width() const; // m - int height() const; // m - int xOrigin() const; // pixel - int yOrigin() const; // pixel - int robotVerticalOffset() const; // pixel - float soundSourceRange() const; // m - float soundSourceMaxRange() const; // m + int resolution() const; // pixel/m + int width() const; // m + int height() const; // m + int xOrigin() const; // pixel + int yOrigin() const; // pixel + int robotVerticalOffset() const; // pixel + float soundSourceRange() const; // m + float soundSourceMaxRange() const; // m double scaleFactor() const; void setScaleFactor(double scaleFactor); @@ -95,12 +95,12 @@ namespace map_image_generator const cv::Scalar& soundSourceColorFull() const; const cv::Scalar& soundSourceColorDim() const; - int globalPathThickness() const; // pixel - int robotSize() const; // pixel - int goalSize() const; // pixel - int laserScanSize() const; // pixel - int labelSize() const; // pixel - int soundSourceSize() const; // pixel + int globalPathThickness() const; // pixel + int robotSize() const; // pixel + int goalSize() const; // pixel + int laserScanSize() const; // pixel + int labelSize() const; // pixel + int soundSourceSize() const; // pixel private: void validateParameters(); @@ -136,10 +136,7 @@ namespace map_image_generator inline double Parameters::scaleFactor() const { return m_scaleFactor; } - inline void Parameters::setScaleFactor(double scaleFactor) - { - m_scaleFactor = scaleFactor; - } + inline void Parameters::setScaleFactor(double scaleFactor) { m_scaleFactor = scaleFactor; } inline const std::string& Parameters::robotFrameId() const { return m_robotFrameId; } @@ -163,43 +160,25 @@ namespace map_image_generator inline const cv::Vec3b& Parameters::wallColor() const { return m_wallColor; } - inline const cv::Vec3b& Parameters::freeSpaceColor() const - { - return m_freeSpaceColor; - } + inline const cv::Vec3b& Parameters::freeSpaceColor() const { return m_freeSpaceColor; } - inline const cv::Vec3b& Parameters::unknownSpaceColor() const - { - return m_unknownSpaceColor; - } + inline const cv::Vec3b& Parameters::unknownSpaceColor() const { return m_unknownSpaceColor; } - inline const cv::Scalar& Parameters::globalPathColor() const - { - return m_globalPathColor; - } + inline const cv::Scalar& Parameters::globalPathColor() const { return m_globalPathColor; } inline const cv::Scalar& Parameters::robotColor() const { return m_robotColor; } inline const cv::Scalar& Parameters::goalColor() const { return m_goalColor; } - inline const cv::Scalar& Parameters::laserScanColor() const - { - return m_laserScanColor; - } + inline const cv::Scalar& Parameters::laserScanColor() const { return m_laserScanColor; } inline const cv::Scalar& Parameters::textColor() const { return m_textColor; } inline const cv::Scalar& Parameters::labelColor() const { return m_labelColor; } - inline const cv::Scalar& Parameters::soundSourceColorFull() const - { - return m_soundSourceColorFull; - } + inline const cv::Scalar& Parameters::soundSourceColorFull() const { return m_soundSourceColorFull; } - inline const cv::Scalar& Parameters::soundSourceColorDim() const - { - return m_soundSourceColorDim; - } + inline const cv::Scalar& Parameters::soundSourceColorDim() const { return m_soundSourceColorDim; } inline int Parameters::globalPathThickness() const { return m_globalPathThickness; } diff --git a/map_image_generator/include/map_image_generator/drawers/GlobalPathImageDrawer.h b/map_image_generator/include/map_image_generator/drawers/GlobalPathImageDrawer.h index 6f6cece..e28ed4a 100644 --- a/map_image_generator/include/map_image_generator/drawers/GlobalPathImageDrawer.h +++ b/map_image_generator/include/map_image_generator/drawers/GlobalPathImageDrawer.h @@ -16,8 +16,10 @@ namespace map_image_generator ros::ServiceServer m_clearGlobalPathService; public: - GlobalPathImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener); + GlobalPathImageDrawer( + const Parameters& parameters, + ros::NodeHandle& nodeHandle, + tf::TransformListener& tfListener); ~GlobalPathImageDrawer() override; void draw(cv::Mat& image) override; @@ -25,8 +27,7 @@ namespace map_image_generator private: void globalPathCallback(const nav_msgs::Path::Ptr& globalPath); void drawGlobalPath(cv::Mat& image, tf::Transform& transform); - bool clearGlobalPath(std_srvs::SetBool::Request& req, - std_srvs::SetBool::Response& res); + bool clearGlobalPath(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); }; } #endif diff --git a/map_image_generator/include/map_image_generator/drawers/GoalImageDrawer.h b/map_image_generator/include/map_image_generator/drawers/GoalImageDrawer.h index 6bd223e..8ff19d1 100644 --- a/map_image_generator/include/map_image_generator/drawers/GoalImageDrawer.h +++ b/map_image_generator/include/map_image_generator/drawers/GoalImageDrawer.h @@ -18,8 +18,7 @@ namespace map_image_generator ros::ServiceServer m_clearGoalsService; public: - GoalImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener); + GoalImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle, tf::TransformListener& tfListener); ~GoalImageDrawer() override; void addGoalCallback(const geometry_msgs::PoseStamped::ConstPtr& goal); @@ -28,10 +27,8 @@ namespace map_image_generator void draw(cv::Mat& image) override; private: - void drawGoal(const geometry_msgs::PoseStamped& goal, cv::Mat& image, - tf::Transform& transform); - bool clearGoals(std_srvs::SetBool::Request& req, - std_srvs::SetBool::Response& res); + void drawGoal(const geometry_msgs::PoseStamped& goal, cv::Mat& image, tf::Transform& transform); + bool clearGoals(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); }; } #endif diff --git a/map_image_generator/include/map_image_generator/drawers/ImageDrawer.h b/map_image_generator/include/map_image_generator/drawers/ImageDrawer.h index 47fe426..09f59f6 100644 --- a/map_image_generator/include/map_image_generator/drawers/ImageDrawer.h +++ b/map_image_generator/include/map_image_generator/drawers/ImageDrawer.h @@ -4,11 +4,11 @@ #include "map_image_generator/Parameters.h" #include "map_image_generator/utils.h" -#include // Replace with in C++17 +#include // Replace with in C++17 #include #include -namespace std // Replace with onlu here in C++17 +namespace std // Replace with onlu here in C++17 { using std::experimental::optional; } @@ -26,8 +26,7 @@ namespace map_image_generator ImageDrawer(ImageDrawer&&) = default; public: - ImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener); + ImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle, tf::TransformListener& tfListener); virtual ~ImageDrawer(); ImageDrawer& operator=(const ImageDrawer&) = delete; @@ -36,14 +35,17 @@ namespace map_image_generator virtual void draw(cv::Mat& image) = 0; protected: - void convertTransformToMapCoordinates(const tf::Transform& transform, int& x, - int& y) const; - void convertTransformToInputMapCoordinates(const tf::Transform& transform, - const nav_msgs::MapMetaData& mapInfo, - int& x, int& y) const; - void convertInputMapCoordinatesToTransform(int x, int y, - const nav_msgs::MapMetaData& mapInfo, - tf::Transform& transform) const; + void convertTransformToMapCoordinates(const tf::Transform& transform, int& x, int& y) const; + void convertTransformToInputMapCoordinates( + const tf::Transform& transform, + const nav_msgs::MapMetaData& mapInfo, + int& x, + int& y) const; + void convertInputMapCoordinatesToTransform( + int x, + int y, + const nav_msgs::MapMetaData& mapInfo, + tf::Transform& transform) const; std::optional getTransformInRef(const std::string& frameId) const; diff --git a/map_image_generator/include/map_image_generator/drawers/LabelImageDrawer.h b/map_image_generator/include/map_image_generator/drawers/LabelImageDrawer.h index 3b505cc..c3e338c 100644 --- a/map_image_generator/include/map_image_generator/drawers/LabelImageDrawer.h +++ b/map_image_generator/include/map_image_generator/drawers/LabelImageDrawer.h @@ -15,18 +15,15 @@ namespace map_image_generator opentera_webrtc_ros_msgs::LabelArray::ConstPtr m_lastLabelArray; public: - LabelImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener); + LabelImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle, tf::TransformListener& tfListener); ~LabelImageDrawer() override; void draw(cv::Mat& image) override; private: - void drawLabel(const opentera_webrtc_ros_msgs::Label& label, cv::Mat& image, - tf::Transform& transform); + void drawLabel(const opentera_webrtc_ros_msgs::Label& label, cv::Mat& image, tf::Transform& transform); - void labelArrayCallback( - const opentera_webrtc_ros_msgs::LabelArray::ConstPtr& labelArray); + void labelArrayCallback(const opentera_webrtc_ros_msgs::LabelArray::ConstPtr& labelArray); }; } #endif diff --git a/map_image_generator/include/map_image_generator/drawers/LaserScanImageDrawer.h b/map_image_generator/include/map_image_generator/drawers/LaserScanImageDrawer.h index b9971dd..6196ff1 100644 --- a/map_image_generator/include/map_image_generator/drawers/LaserScanImageDrawer.h +++ b/map_image_generator/include/map_image_generator/drawers/LaserScanImageDrawer.h @@ -14,8 +14,10 @@ namespace map_image_generator sensor_msgs::LaserScan::ConstPtr m_lastLaserScan; public: - LaserScanImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener); + LaserScanImageDrawer( + const Parameters& parameters, + ros::NodeHandle& nodeHandle, + tf::TransformListener& tfListener); ~LaserScanImageDrawer() override; void draw(cv::Mat& image) override; @@ -24,8 +26,7 @@ namespace map_image_generator void laserScanCallback(const sensor_msgs::LaserScan::ConstPtr& laserScan); void drawLaserScan(cv::Mat& image, tf::Transform& transform); - void drawRange(cv::Mat& image, tf::Transform& transform, float range, - float angle); + void drawRange(cv::Mat& image, tf::Transform& transform, float range, float angle); }; } #endif diff --git a/map_image_generator/include/map_image_generator/drawers/OccupancyGridImageDrawer.h b/map_image_generator/include/map_image_generator/drawers/OccupancyGridImageDrawer.h index 05bb194..c54d4af 100644 --- a/map_image_generator/include/map_image_generator/drawers/OccupancyGridImageDrawer.h +++ b/map_image_generator/include/map_image_generator/drawers/OccupancyGridImageDrawer.h @@ -23,18 +23,18 @@ namespace map_image_generator cv::Mat m_zoomedOccupancyGridImage; public: - OccupancyGridImageDrawer(Parameters& parameters, ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener); + OccupancyGridImageDrawer( + Parameters& parameters, + ros::NodeHandle& nodeHandle, + tf::TransformListener& tfListener); ~OccupancyGridImageDrawer() override; void draw(cv::Mat& image) override; private: - void - occupancyGridCallback(const nav_msgs::OccupancyGrid::ConstPtr& occupancyGrid); + void occupancyGridCallback(const nav_msgs::OccupancyGrid::ConstPtr& occupancyGrid); - bool changeMapViewCallback(ChangeMapView::Request& req, - ChangeMapView::Response& res); + bool changeMapViewCallback(ChangeMapView::Request& req, ChangeMapView::Response& res); void drawNotScaledOccupancyGridImage(); void changeNotScaledOccupancyGridImageIfNeeded(); @@ -42,8 +42,7 @@ namespace map_image_generator void scaleOccupancyGridImage(); void changeScaledOccupancyGridImageIfNeeded(); - void rotateImageAboutPoint(cv::Mat& image, double angle, - const cv::Point& point) const; + void rotateImageAboutPoint(cv::Mat& image, double angle, const cv::Point& point) const; void rotateImageAboutCenter(cv::Mat& image, double angle) const; void drawOccupancyGridImage(cv::Mat& image); @@ -65,17 +64,17 @@ namespace map_image_generator int y; }; - static DirectionalValues computePadding(const DirectionalValues& position, - int height, int width); - void adjustPaddingForCenteredRobotOffset(DirectionalValues& padding, int width, - const DirectionalValues& robotPosition); + static DirectionalValues computePadding(const DirectionalValues& position, int height, int width); + void adjustPaddingForCenteredRobotOffset( + DirectionalValues& padding, + int width, + const DirectionalValues& robotPosition); const cv::Mat& getZoomedOccupancyImage(); MapCoordinates getMapCoordinatesFromTf(const tf::Transform& transform) const; static DirectionalValues - getDirectionsFromMapCoordinates(const MapCoordinates& mapCoordinates, - const cv::Mat& map); + getDirectionsFromMapCoordinates(const MapCoordinates& mapCoordinates, const cv::Mat& map); }; } #endif diff --git a/map_image_generator/include/map_image_generator/drawers/RobotImageDrawer.h b/map_image_generator/include/map_image_generator/drawers/RobotImageDrawer.h index 210074f..30a2bfe 100644 --- a/map_image_generator/include/map_image_generator/drawers/RobotImageDrawer.h +++ b/map_image_generator/include/map_image_generator/drawers/RobotImageDrawer.h @@ -11,8 +11,7 @@ namespace map_image_generator class RobotImageDrawer : public ImageDrawer { public: - RobotImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener); + RobotImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle, tf::TransformListener& tfListener); ~RobotImageDrawer() override; void draw(cv::Mat& image) override; diff --git a/map_image_generator/include/map_image_generator/drawers/SoundSourceImageDrawer.h b/map_image_generator/include/map_image_generator/drawers/SoundSourceImageDrawer.h index 149e4d4..66a3db9 100644 --- a/map_image_generator/include/map_image_generator/drawers/SoundSourceImageDrawer.h +++ b/map_image_generator/include/map_image_generator/drawers/SoundSourceImageDrawer.h @@ -18,31 +18,30 @@ namespace map_image_generator sensor_msgs::LaserScan::ConstPtr m_lastLaserScan; public: - SoundSourceImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener); + SoundSourceImageDrawer( + const Parameters& parameters, + ros::NodeHandle& nodeHandle, + tf::TransformListener& tfListener); ~SoundSourceImageDrawer() override; void draw(cv::Mat& image) override; protected: - void - soundSourcesCallback(const odas_ros::OdasSstArrayStamped::ConstPtr& soundSources); + void soundSourcesCallback(const odas_ros::OdasSstArrayStamped::ConstPtr& soundSources); void laserScanCallback(const sensor_msgs::LaserScan::ConstPtr& laserScan); private: void drawWithLidar(cv::Mat& image); - void drawSoundSourcesWithLidar(cv::Mat& image, - const tf::Transform& sourceToLidarTf, - const tf::Transform& lidarToRefTf); + void drawSoundSourcesWithLidar( + cv::Mat& image, + const tf::Transform& sourceToLidarTf, + const tf::Transform& lidarToRefTf); void drawWithoutLidar(cv::Mat& image); - void drawSoundSourcesWithoutLidar(cv::Mat& image, - const tf::Transform& sourceToRefTf); + void drawSoundSourcesWithoutLidar(cv::Mat& image, const tf::Transform& sourceToRefTf); - void drawSoundSource(cv::Mat& image, const odas_ros::OdasSst& source, - tf::Pose poseInRef); - void drawConcentricCircles(cv::Mat& image, int x, int y, int radius, - double colorRatio); + void drawSoundSource(cv::Mat& image, const odas_ros::OdasSst& source, tf::Pose poseInRef); + void drawConcentricCircles(cv::Mat& image, int x, int y, int radius, double colorRatio); static tf::Pose getPoseFromSst(const odas_ros::OdasSst& sst); diff --git a/map_image_generator/include/map_image_generator/utils.h b/map_image_generator/include/map_image_generator/utils.h index 06ee8ad..432aa8f 100644 --- a/map_image_generator/include/map_image_generator/utils.h +++ b/map_image_generator/include/map_image_generator/utils.h @@ -11,26 +11,19 @@ namespace map_image_generator { geometry_msgs::PoseStamped - convertMapToMapImage(const Parameters& parameters, - const geometry_msgs::PoseStamped& mapPose); - geometry_msgs::Pose convertMapToMapImage(const Parameters& parameters, - const geometry_msgs::Pose& mapPose); + convertMapToMapImage(const Parameters& parameters, const geometry_msgs::PoseStamped& mapPose); + geometry_msgs::Pose convertMapToMapImage(const Parameters& parameters, const geometry_msgs::Pose& mapPose); geometry_msgs::Pose - convertRobotCenteredMapCoordinatesToPose(const Parameters& parameters, int x, int y, - double yaw); + convertRobotCenteredMapCoordinatesToPose(const Parameters& parameters, int x, int y, double yaw); geometry_msgs::PoseStamped - convertMapImageToRobot(const Parameters& parameters, - const geometry_msgs::PoseStamped& mapImagePose); - geometry_msgs::Pose convertMapImageToRobot(const Parameters& parameters, - const geometry_msgs::Pose& mapImagePose); + convertMapImageToRobot(const Parameters& parameters, const geometry_msgs::PoseStamped& mapImagePose); + geometry_msgs::Pose convertMapImageToRobot(const Parameters& parameters, const geometry_msgs::Pose& mapImagePose); geometry_msgs::PoseStamped - convertMapImageToMap(const Parameters& parameters, - const geometry_msgs::PoseStamped& mapImagePose); - geometry_msgs::Pose convertMapImageToMap(const Parameters& parameters, - const geometry_msgs::Pose& mapImagePose); + convertMapImageToMap(const Parameters& parameters, const geometry_msgs::PoseStamped& mapImagePose); + geometry_msgs::Pose convertMapImageToMap(const Parameters& parameters, const geometry_msgs::Pose& mapImagePose); void offsetYawByMinus90Degrees(geometry_msgs::Pose& pose); @@ -40,32 +33,31 @@ namespace map_image_generator return angle >= 0 ? angle : angle + TFSIMD_2_PI; } - cv::Scalar interpolateColors(const cv::Scalar& color1, const cv::Scalar& color2, - double ratio); + cv::Scalar interpolateColors(const cv::Scalar& color1, const cv::Scalar& color2, double ratio); void flipYawOnY(geometry_msgs::Pose& pose); void flipYawOnY(tf::Transform& transform); double flipYawOnY(double yaw); - template + template inline T sign(T val) { return static_cast(T{0} < val) - static_cast(val < T{0}); } - template + template inline T restrictToPositive(T val) { return std::max(T{0}, val); } - template + template inline T deg2rad(T deg) { return deg * M_PI / 180; } - template + template inline T rad2deg(T rad) { return rad * 180 / M_PI; diff --git a/map_image_generator/package.xml b/map_image_generator/package.xml index ea91f48..7da4fa4 100644 --- a/map_image_generator/package.xml +++ b/map_image_generator/package.xml @@ -1,97 +1,97 @@ - map_image_generator - 0.0.0 - The map_image_generator package + map_image_generator + 0.0.0 + The map_image_generator package - - - - mahm1904 + + + + mahm1904 - - - - TODO + + + + TODO - - - - + + + + - - - - + + + + - - - - - - - - - - - - - - - - - - - - - catkin - cv_bridge - framework - geometry_msgs - image_transport - nav_msgs - roscpp - sensor_msgs - std_msgs - tf - rtabmap_ros - odas_ros - opentera_webrtc_ros_msgs - cv_bridge - framework - geometry_msgs - image_transport - nav_msgs - roscpp - sensor_msgs - std_msgs - tf - rtabmap_ros - odas_ros - opentera_webrtc_ros_msgs - cv_bridge - framework - geometry_msgs - image_transport - nav_msgs - roscpp - sensor_msgs - std_msgs - tf - rtabmap_ros - odas_ros - opentera_webrtc_ros_msgs - message_generation - message_runtime + + + + + + + + + + + + + + + + + + + + + catkin + cv_bridge + framework + geometry_msgs + image_transport + nav_msgs + roscpp + sensor_msgs + std_msgs + tf + rtabmap_ros + odas_ros + opentera_webrtc_ros_msgs + cv_bridge + framework + geometry_msgs + image_transport + nav_msgs + roscpp + sensor_msgs + std_msgs + tf + rtabmap_ros + odas_ros + opentera_webrtc_ros_msgs + cv_bridge + framework + geometry_msgs + image_transport + nav_msgs + roscpp + sensor_msgs + std_msgs + tf + rtabmap_ros + odas_ros + opentera_webrtc_ros_msgs + message_generation + message_runtime - - - + + + - + diff --git a/map_image_generator/src/GoalConverter.cpp b/map_image_generator/src/GoalConverter.cpp index b1e49fa..1c099f5 100644 --- a/map_image_generator/src/GoalConverter.cpp +++ b/map_image_generator/src/GoalConverter.cpp @@ -2,26 +2,28 @@ using namespace map_image_generator; -GoalConverter::GoalConverter(const Parameters& parameters, ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener) - : m_parameters(parameters), m_nodeHandle(nodeHandle), m_tfListener(tfListener), - image_goal_to_map_goal_service{m_nodeHandle.advertiseService( - "image_goal_to_map_goal", &GoalConverter::mapImageGoalCallback, this)} +GoalConverter::GoalConverter( + const Parameters& parameters, + ros::NodeHandle& nodeHandle, + tf::TransformListener& tfListener) + : m_parameters(parameters), + m_nodeHandle(nodeHandle), + m_tfListener(tfListener), + image_goal_to_map_goal_service{ + m_nodeHandle.advertiseService("image_goal_to_map_goal", &GoalConverter::mapImageGoalCallback, this)} { } GoalConverter::~GoalConverter() = default; -bool GoalConverter::mapImageGoalCallback(ImageGoalToMapGoal::Request& req, - ImageGoalToMapGoal::Response& res) +bool GoalConverter::mapImageGoalCallback(ImageGoalToMapGoal::Request& req, ImageGoalToMapGoal::Response& res) { if (m_parameters.centeredRobot()) { res.map_goal = convertMapImageToRobot(m_parameters, req.image_goal); try { - m_tfListener.transformPose(m_parameters.mapFrameId(), res.map_goal, - res.map_goal); + m_tfListener.transformPose(m_parameters.mapFrameId(), res.map_goal, res.map_goal); } catch (tf::TransformException& ex) { diff --git a/map_image_generator/src/MapImageGenerator.cpp b/map_image_generator/src/MapImageGenerator.cpp index ee7bcb8..c344ea2 100644 --- a/map_image_generator/src/MapImageGenerator.cpp +++ b/map_image_generator/src/MapImageGenerator.cpp @@ -11,52 +11,49 @@ using namespace map_image_generator; using namespace std; -MapImageGenerator::MapImageGenerator(Parameters& parameters, ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener) - : m_parameters(parameters), m_nodeHandle(nodeHandle), m_tfListener(tfListener) +MapImageGenerator::MapImageGenerator( + Parameters& parameters, + ros::NodeHandle& nodeHandle, + tf::TransformListener& tfListener) + : m_parameters(parameters), + m_nodeHandle(nodeHandle), + m_tfListener(tfListener) { // The order of the drawers is important as it determines the layering of the // elements. if (m_parameters.drawOccupancyGrid()) { - m_drawers.push_back(std::make_unique( - parameters, nodeHandle, m_tfListener)); + m_drawers.push_back(std::make_unique(parameters, nodeHandle, m_tfListener)); } if (m_parameters.drawGlobalPath()) { - m_drawers.push_back(std::make_unique( - m_parameters, nodeHandle, m_tfListener)); + m_drawers.push_back(std::make_unique(m_parameters, nodeHandle, m_tfListener)); } if (m_parameters.drawRobot()) { - m_drawers.push_back( - std::make_unique(m_parameters, nodeHandle, m_tfListener)); + m_drawers.push_back(std::make_unique(m_parameters, nodeHandle, m_tfListener)); } if (m_parameters.drawLabels()) { - m_drawers.push_back( - std::make_unique(m_parameters, nodeHandle, m_tfListener)); + m_drawers.push_back(std::make_unique(m_parameters, nodeHandle, m_tfListener)); } if (m_parameters.drawGoals()) { - m_drawers.push_back( - std::make_unique(m_parameters, nodeHandle, m_tfListener)); + m_drawers.push_back(std::make_unique(m_parameters, nodeHandle, m_tfListener)); } if (m_parameters.drawLaserScan()) { - m_drawers.push_back(std::make_unique( - m_parameters, nodeHandle, m_tfListener)); + m_drawers.push_back(std::make_unique(m_parameters, nodeHandle, m_tfListener)); } if (m_parameters.drawSoundSources()) { - m_drawers.push_back(std::make_unique( - m_parameters, nodeHandle, m_tfListener)); + m_drawers.push_back(std::make_unique(m_parameters, nodeHandle, m_tfListener)); } int imageWidth = parameters.resolution() * parameters.width(); diff --git a/map_image_generator/src/MapLabelsConverter.cpp b/map_image_generator/src/MapLabelsConverter.cpp index 35da000..a577199 100644 --- a/map_image_generator/src/MapLabelsConverter.cpp +++ b/map_image_generator/src/MapLabelsConverter.cpp @@ -8,30 +8,26 @@ using namespace map_image_generator; using namespace std; -MapLabelsConverter::MapLabelsConverter(const Parameters& parameters, - ros::NodeHandle& nodeHandle) - : m_parameters(parameters), m_nodeHandle(nodeHandle) +MapLabelsConverter::MapLabelsConverter(const Parameters& parameters, ros::NodeHandle& nodeHandle) + : m_parameters(parameters), + m_nodeHandle(nodeHandle) { - m_mapLabelsSubscriber = m_nodeHandle.subscribe( - "map_labels", 1, &MapLabelsConverter::mapLabelsCallback, this); - m_mapLabelsPublisher = - m_nodeHandle.advertise("map_image_labels", 1); + m_mapLabelsSubscriber = m_nodeHandle.subscribe("map_labels", 1, &MapLabelsConverter::mapLabelsCallback, this); + m_mapLabelsPublisher = m_nodeHandle.advertise("map_image_labels", 1); m_rtabmapListLabelsServiceClient = m_nodeHandle.serviceClient("rtabmap_list_label_service"); } MapLabelsConverter::~MapLabelsConverter() = default; -void MapLabelsConverter::mapLabelsCallback( - const visualization_msgs::MarkerArray::ConstPtr& mapLabels) +void MapLabelsConverter::mapLabelsCallback(const visualization_msgs::MarkerArray::ConstPtr& mapLabels) { std::vector desiredLabels = getDesiredLabels(); visualization_msgs::MarkerArray mapImageLabels; for (const auto& marker : mapLabels->markers) { - if (find(desiredLabels.begin(), desiredLabels.end(), marker.text) - == desiredLabels.end()) + if (find(desiredLabels.begin(), desiredLabels.end(), marker.text) == desiredLabels.end()) { continue; } diff --git a/map_image_generator/src/Parameters.cpp b/map_image_generator/src/Parameters.cpp index 23304f0..9fec56b 100644 --- a/map_image_generator/src/Parameters.cpp +++ b/map_image_generator/src/Parameters.cpp @@ -11,21 +11,20 @@ using namespace std; namespace internal { // Not defined for T: compilation fails if T is not cv::Scalar or cv::Vec3b - template + template T parseColor(const std::string& color); - template <> + template<> cv::Scalar parseColor(const std::string& color) { int r, g, b, a; stringstream ss(color); ss >> r >> g >> b >> a; - return {static_cast(b), static_cast(g), static_cast(r), - static_cast(a)}; + return {static_cast(b), static_cast(g), static_cast(r), static_cast(a)}; } - template <> + template<> cv::Vec3b parseColor(const std::string& color) { int r, g, b; @@ -36,7 +35,7 @@ namespace internal } // Compilation fails if T is not cv::Scalar or cv::Vec3b because of parseColor - template + template T makeColor(const std::string& name, const std::string& defaultColor) { ros::NodeHandle nh{"~"}; @@ -46,10 +45,9 @@ namespace internal } // Not defined if T is cv::Scalar or cv::Vec3b - template - std::enable_if_t< - !std::is_same::value && !std::is_same::value, T> - getParam(const std::string& name, const T& defaultValue) + template + std::enable_if_t::value && !std::is_same::value, T> + getParam(const std::string& name, const T& defaultValue) { ros::NodeHandle nh{"~"}; T value; @@ -58,10 +56,9 @@ namespace internal } // Defined only if T is cv::Scalar or cv::Vec3b - template - std::enable_if_t< - std::is_same::value || std::is_same::value, T> - getParam(const std::string& name, const std::string& defaultValue) + template + std::enable_if_t::value || std::is_same::value, T> + getParam(const std::string& name, const std::string& defaultValue) { return makeColor(name, defaultValue); } @@ -101,8 +98,7 @@ Parameters::Parameters(ros::NodeHandle& nodeHandle) : m_scaleFactor{1.0} m_laserScanColor = getParam("laser_scan_color", "255 0 0 255"); m_textColor = getParam("text_color", "255 255 255 255"); m_labelColor = getParam("label_color", "255 0 255 255"); - m_soundSourceColorFull = - getParam("sound_source_color_full", "255 0 0 255"); + m_soundSourceColorFull = getParam("sound_source_color_full", "255 0 0 255"); m_soundSourceColorDim = getParam("sound_source_color_dim", "0 255 0 255"); @@ -124,16 +120,18 @@ void Parameters::validateParameters() { int maxOffset = static_cast(std::floor((m_height * m_resolution - 1) / 2.0)); int minOffset = static_cast(std::floor(-(m_height * m_resolution - 1) / 2.0)); - if (m_centeredRobot - && (m_robotVerticalOffset > maxOffset || m_robotVerticalOffset < minOffset)) + if (m_centeredRobot && (m_robotVerticalOffset > maxOffset || m_robotVerticalOffset < minOffset)) { int oldOffset = m_robotVerticalOffset; - m_robotVerticalOffset = static_cast(std::floor( - sign(m_robotVerticalOffset) * (m_height * m_resolution - 1) / 2.0)); + m_robotVerticalOffset = + static_cast(std::floor(sign(m_robotVerticalOffset) * (m_height * m_resolution - 1) / 2.0)); ROS_WARN( "Robot vertical offset is [%d], which is out of inclusive range [%d, %d]. " "It will be set to [%d], which is the maximum value based on the sign.", - oldOffset, minOffset, maxOffset, m_robotVerticalOffset); + oldOffset, + minOffset, + maxOffset, + m_robotVerticalOffset); } } diff --git a/map_image_generator/src/drawers/GlobalPathImageDrawer.cpp b/map_image_generator/src/drawers/GlobalPathImageDrawer.cpp index 440b269..1b7652a 100644 --- a/map_image_generator/src/drawers/GlobalPathImageDrawer.cpp +++ b/map_image_generator/src/drawers/GlobalPathImageDrawer.cpp @@ -4,14 +4,14 @@ using namespace map_image_generator; -GlobalPathImageDrawer::GlobalPathImageDrawer(const Parameters& parameters, - ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener) +GlobalPathImageDrawer::GlobalPathImageDrawer( + const Parameters& parameters, + ros::NodeHandle& nodeHandle, + tf::TransformListener& tfListener) : ImageDrawer(parameters, nodeHandle, tfListener), - m_globalPathSubscriber{nodeHandle.subscribe( - "global_path", 1, &GlobalPathImageDrawer::globalPathCallback, this)}, - m_clearGlobalPathService{m_nodeHandle.advertiseService( - "clear_global_path", &GlobalPathImageDrawer::clearGlobalPath, this)} + m_globalPathSubscriber{nodeHandle.subscribe("global_path", 1, &GlobalPathImageDrawer::globalPathCallback, this)}, + m_clearGlobalPathService{ + m_nodeHandle.advertiseService("clear_global_path", &GlobalPathImageDrawer::clearGlobalPath, this)} { } @@ -58,13 +58,11 @@ void GlobalPathImageDrawer::drawGlobalPath(cv::Mat& image, tf::Transform& transf convertTransformToMapCoordinates(startPose, startX, startY); convertTransformToMapCoordinates(endPose, endX, endY); - cv::line(image, cv::Point(startX, startY), cv::Point(endX, endY), color, - thickness, cv::LINE_AA); + cv::line(image, cv::Point(startX, startY), cv::Point(endX, endY), color, thickness, cv::LINE_AA); } } -bool GlobalPathImageDrawer::clearGlobalPath(std_srvs::SetBool::Request& req, - std_srvs::SetBool::Response& res) +bool GlobalPathImageDrawer::clearGlobalPath(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { if (req.data) { diff --git a/map_image_generator/src/drawers/GoalImageDrawer.cpp b/map_image_generator/src/drawers/GoalImageDrawer.cpp index c9947eb..0792bc4 100644 --- a/map_image_generator/src/drawers/GoalImageDrawer.cpp +++ b/map_image_generator/src/drawers/GoalImageDrawer.cpp @@ -7,16 +7,16 @@ using namespace map_image_generator; using namespace std; -GoalImageDrawer::GoalImageDrawer(const Parameters& parameters, - ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener) +GoalImageDrawer::GoalImageDrawer( + const Parameters& parameters, + ros::NodeHandle& nodeHandle, + tf::TransformListener& tfListener) : ImageDrawer(parameters, nodeHandle, tfListener), - m_add_goal_sub{nodeHandle.subscribe("map_image_drawer/add_goal", 10, - &GoalImageDrawer::addGoalCallback, this)}, - m_remove_goal_sub{nodeHandle.subscribe("map_image_drawer/remove_goal", 10, - &GoalImageDrawer::removeGoalCallback, this)}, - m_clearGoalsService{nodeHandle.advertiseService("map_image_drawer/clear_goals", - &GoalImageDrawer::clearGoals, this)} + m_add_goal_sub{nodeHandle.subscribe("map_image_drawer/add_goal", 10, &GoalImageDrawer::addGoalCallback, this)}, + m_remove_goal_sub{ + nodeHandle.subscribe("map_image_drawer/remove_goal", 10, &GoalImageDrawer::removeGoalCallback, this)}, + m_clearGoalsService{ + nodeHandle.advertiseService("map_image_drawer/clear_goals", &GoalImageDrawer::clearGoals, this)} { } @@ -29,9 +29,8 @@ void GoalImageDrawer::addGoalCallback(const geometry_msgs::PoseStamped::ConstPtr void GoalImageDrawer::removeGoalCallback(const geometry_msgs::PoseStamped::ConstPtr& goal) { - if (!m_activeGoals.empty() - && std::round(m_activeGoals.front().pose.position.z) - == std::round(goal->pose.position.z)) + if (!m_activeGoals.empty() && + std::round(m_activeGoals.front().pose.position.z) == std::round(goal->pose.position.z)) { m_activeGoals.pop_front(); } @@ -41,8 +40,7 @@ void GoalImageDrawer::removeGoalCallback(const geometry_msgs::PoseStamped::Const } } -bool GoalImageDrawer::clearGoals(std_srvs::SetBool::Request& req, - std_srvs::SetBool::Response& res) +bool GoalImageDrawer::clearGoals(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { if (req.data) { @@ -65,8 +63,7 @@ void GoalImageDrawer::draw(cv::Mat& image) } } -void GoalImageDrawer::drawGoal(const geometry_msgs::PoseStamped& goal, cv::Mat& image, - tf::Transform& transform) +void GoalImageDrawer::drawGoal(const geometry_msgs::PoseStamped& goal, cv::Mat& image, tf::Transform& transform) { const cv::Scalar& color = m_parameters.goalColor(); int size = m_parameters.goalSize(); @@ -84,10 +81,22 @@ void GoalImageDrawer::drawGoal(const geometry_msgs::PoseStamped& goal, cv::Mat& int endX = static_cast(startX + size * cos(yaw)); int endY = static_cast(startY + size * sin(yaw)); - cv::circle(image, cv::Point(startX, startY), ceilDivision(size, 5.0), color, - cv::FILLED); - cv::arrowedLine(image, cv::Point(startX, startY), cv::Point(endX, endY), color, - ceilDivision(size, 10.0), cv::LINE_8, 0, 0.3); - cv::putText(image, std::to_string(index), cv::Point(startX, startY), - cv::FONT_HERSHEY_DUPLEX, 0.5, m_parameters.textColor(), 1); + cv::circle(image, cv::Point(startX, startY), ceilDivision(size, 5.0), color, cv::FILLED); + cv::arrowedLine( + image, + cv::Point(startX, startY), + cv::Point(endX, endY), + color, + ceilDivision(size, 10.0), + cv::LINE_8, + 0, + 0.3); + cv::putText( + image, + std::to_string(index), + cv::Point(startX, startY), + cv::FONT_HERSHEY_DUPLEX, + 0.5, + m_parameters.textColor(), + 1); } diff --git a/map_image_generator/src/drawers/ImageDrawer.cpp b/map_image_generator/src/drawers/ImageDrawer.cpp index 730d96f..42ecc5d 100644 --- a/map_image_generator/src/drawers/ImageDrawer.cpp +++ b/map_image_generator/src/drawers/ImageDrawer.cpp @@ -2,37 +2,39 @@ using namespace map_image_generator; -ImageDrawer::ImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener) - : m_parameters(parameters), m_nodeHandle(nodeHandle), m_tfListener(tfListener) +ImageDrawer::ImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle, tf::TransformListener& tfListener) + : m_parameters(parameters), + m_nodeHandle(nodeHandle), + m_tfListener(tfListener) { } ImageDrawer::~ImageDrawer() = default; -void ImageDrawer::convertTransformToMapCoordinates(const tf::Transform& transform, int& x, - int& y) const +void ImageDrawer::convertTransformToMapCoordinates(const tf::Transform& transform, int& x, int& y) const { - x = static_cast(transform.getOrigin().getX() * m_parameters.resolution() - * m_parameters.scaleFactor() - + m_parameters.xOrigin()); - y = static_cast(transform.getOrigin().getY() * m_parameters.resolution() - * m_parameters.scaleFactor() - + m_parameters.yOrigin() + m_parameters.robotVerticalOffset()); + x = static_cast( + transform.getOrigin().getX() * m_parameters.resolution() * m_parameters.scaleFactor() + m_parameters.xOrigin()); + y = static_cast( + transform.getOrigin().getY() * m_parameters.resolution() * m_parameters.scaleFactor() + m_parameters.yOrigin() + + m_parameters.robotVerticalOffset()); } void ImageDrawer::convertTransformToInputMapCoordinates( - const tf::Transform& transform, const nav_msgs::MapMetaData& mapInfo, int& x, + const tf::Transform& transform, + const nav_msgs::MapMetaData& mapInfo, + int& x, int& y) const { - x = static_cast((transform.getOrigin().getX() - mapInfo.origin.position.x) - * m_parameters.resolution()); - y = static_cast((transform.getOrigin().getY() - mapInfo.origin.position.y) - * m_parameters.resolution()); + x = static_cast((transform.getOrigin().getX() - mapInfo.origin.position.x) * m_parameters.resolution()); + y = static_cast((transform.getOrigin().getY() - mapInfo.origin.position.y) * m_parameters.resolution()); } void ImageDrawer::convertInputMapCoordinatesToTransform( - int x, int y, const nav_msgs::MapMetaData& mapInfo, tf::Transform& transform) const + int x, + int y, + const nav_msgs::MapMetaData& mapInfo, + tf::Transform& transform) const { transform.setOrigin(tf::Vector3( static_cast(x) / m_parameters.resolution() + mapInfo.origin.position.x, @@ -40,15 +42,13 @@ void ImageDrawer::convertInputMapCoordinatesToTransform( 0.0)); } -std::optional -ImageDrawer::getTransformInRef(const std::string& frameId) const +std::optional ImageDrawer::getTransformInRef(const std::string& frameId) const { tf::StampedTransform transform; try { - m_tfListener.lookupTransform(m_parameters.refFrameId(), frameId, ros::Time(0), - transform); + m_tfListener.lookupTransform(m_parameters.refFrameId(), frameId, ros::Time(0), transform); } catch (tf::TransformException& ex) { diff --git a/map_image_generator/src/drawers/LabelImageDrawer.cpp b/map_image_generator/src/drawers/LabelImageDrawer.cpp index 8b8dde0..c0a6e70 100644 --- a/map_image_generator/src/drawers/LabelImageDrawer.cpp +++ b/map_image_generator/src/drawers/LabelImageDrawer.cpp @@ -4,19 +4,18 @@ using namespace map_image_generator; -LabelImageDrawer::LabelImageDrawer(const Parameters& parameters, - ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener) +LabelImageDrawer::LabelImageDrawer( + const Parameters& parameters, + ros::NodeHandle& nodeHandle, + tf::TransformListener& tfListener) : ImageDrawer(parameters, nodeHandle, tfListener), - m_labelArraySubscriber{m_nodeHandle.subscribe( - "stored_labels", 1, &LabelImageDrawer::labelArrayCallback, this)} + m_labelArraySubscriber{m_nodeHandle.subscribe("stored_labels", 1, &LabelImageDrawer::labelArrayCallback, this)} { } LabelImageDrawer::~LabelImageDrawer() = default; -void LabelImageDrawer::labelArrayCallback( - const opentera_webrtc_ros_msgs::LabelArray::ConstPtr& labelArray) +void LabelImageDrawer::labelArrayCallback(const opentera_webrtc_ros_msgs::LabelArray::ConstPtr& labelArray) { m_lastLabelArray = labelArray; } @@ -39,8 +38,7 @@ void LabelImageDrawer::draw(cv::Mat& image) } } -void LabelImageDrawer::drawLabel(const opentera_webrtc_ros_msgs::Label& label, - cv::Mat& image, tf::Transform& transform) +void LabelImageDrawer::drawLabel(const opentera_webrtc_ros_msgs::Label& label, cv::Mat& image, tf::Transform& transform) { const cv::Scalar& color = m_parameters.labelColor(); int size = m_parameters.labelSize(); @@ -57,8 +55,20 @@ void LabelImageDrawer::drawLabel(const opentera_webrtc_ros_msgs::Label& label, int endX = static_cast(startX + size * cos(yaw)); int endY = static_cast(startY + size * sin(yaw)); - cv::drawMarker(image, cv::Point(startX, startY), color, cv::MARKER_DIAMOND, - ceilDivision(size, 4.0), ceilDivision(size, 12.0), cv::FILLED); - cv::putText(image, label.name, cv::Point(startX, startY), cv::FONT_HERSHEY_DUPLEX, - 0.5, m_parameters.textColor(), 1); + cv::drawMarker( + image, + cv::Point(startX, startY), + color, + cv::MARKER_DIAMOND, + ceilDivision(size, 4.0), + ceilDivision(size, 12.0), + cv::FILLED); + cv::putText( + image, + label.name, + cv::Point(startX, startY), + cv::FONT_HERSHEY_DUPLEX, + 0.5, + m_parameters.textColor(), + 1); } diff --git a/map_image_generator/src/drawers/LaserScanImageDrawer.cpp b/map_image_generator/src/drawers/LaserScanImageDrawer.cpp index 961ad65..4689ba8 100644 --- a/map_image_generator/src/drawers/LaserScanImageDrawer.cpp +++ b/map_image_generator/src/drawers/LaserScanImageDrawer.cpp @@ -6,13 +6,13 @@ using namespace map_image_generator; using namespace std; -LaserScanImageDrawer::LaserScanImageDrawer(const Parameters& parameters, - ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener) +LaserScanImageDrawer::LaserScanImageDrawer( + const Parameters& parameters, + ros::NodeHandle& nodeHandle, + tf::TransformListener& tfListener) : ImageDrawer(parameters, nodeHandle, tfListener) { - m_laserScanSubscriber = m_nodeHandle.subscribe( - "laser_scan", 1, &LaserScanImageDrawer::laserScanCallback, this); + m_laserScanSubscriber = m_nodeHandle.subscribe("laser_scan", 1, &LaserScanImageDrawer::laserScanCallback, this); } LaserScanImageDrawer::~LaserScanImageDrawer() = default; @@ -31,8 +31,7 @@ void LaserScanImageDrawer::draw(cv::Mat& image) } } -void LaserScanImageDrawer::laserScanCallback( - const sensor_msgs::LaserScan::ConstPtr& laserScan) +void LaserScanImageDrawer::laserScanCallback(const sensor_msgs::LaserScan::ConstPtr& laserScan) { m_lastLaserScan = laserScan; } @@ -52,11 +51,9 @@ void LaserScanImageDrawer::drawLaserScan(cv::Mat& image, tf::Transform& transfor } } -void LaserScanImageDrawer::drawRange(cv::Mat& image, tf::Transform& transform, - float range, float angle) +void LaserScanImageDrawer::drawRange(cv::Mat& image, tf::Transform& transform, float range, float angle) { - tf::Pose rangePose(tf::Quaternion(0, 0, 0, 0), - tf::Vector3(range * cos(angle), range * sin(angle), 0)); + tf::Pose rangePose(tf::Quaternion(0, 0, 0, 0), tf::Vector3(range * cos(angle), range * sin(angle), 0)); rangePose = transform * rangePose; adjustTransformForRobotRef(rangePose); diff --git a/map_image_generator/src/drawers/OccupancyGridImageDrawer.cpp b/map_image_generator/src/drawers/OccupancyGridImageDrawer.cpp index 86d2364..42d8c98 100644 --- a/map_image_generator/src/drawers/OccupancyGridImageDrawer.cpp +++ b/map_image_generator/src/drawers/OccupancyGridImageDrawer.cpp @@ -7,14 +7,16 @@ using namespace map_image_generator; -OccupancyGridImageDrawer::OccupancyGridImageDrawer(Parameters& parameters, - ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener) - : ImageDrawer{parameters, nodeHandle, tfListener}, m_mutableParameters{parameters}, - m_occupancyGridSubscriber{m_nodeHandle.subscribe( - "occupancy_grid", 1, &OccupancyGridImageDrawer::occupancyGridCallback, this)}, - m_mapViewChangerService{m_nodeHandle.advertiseService( - "change_map_view", &OccupancyGridImageDrawer::changeMapViewCallback, this)} +OccupancyGridImageDrawer::OccupancyGridImageDrawer( + Parameters& parameters, + ros::NodeHandle& nodeHandle, + tf::TransformListener& tfListener) + : ImageDrawer{parameters, nodeHandle, tfListener}, + m_mutableParameters{parameters}, + m_occupancyGridSubscriber{ + m_nodeHandle.subscribe("occupancy_grid", 1, &OccupancyGridImageDrawer::occupancyGridCallback, this)}, + m_mapViewChangerService{ + m_nodeHandle.advertiseService("change_map_view", &OccupancyGridImageDrawer::changeMapViewCallback, this)} { } @@ -40,14 +42,12 @@ void OccupancyGridImageDrawer::draw(cv::Mat& image) } } -void OccupancyGridImageDrawer::occupancyGridCallback( - const nav_msgs::OccupancyGrid::ConstPtr& occupancyGrid) +void OccupancyGridImageDrawer::occupancyGridCallback(const nav_msgs::OccupancyGrid::ConstPtr& occupancyGrid) { m_lastOccupancyGrid = occupancyGrid; } -bool OccupancyGridImageDrawer::changeMapViewCallback(ChangeMapView::Request& req, - ChangeMapView::Response& res) +bool OccupancyGridImageDrawer::changeMapViewCallback(ChangeMapView::Request& req, ChangeMapView::Response& res) { res.success = true; if (req.view_new == ChangeMapView::Request::VIEW_CENTERED_ROBOT) @@ -85,13 +85,11 @@ void OccupancyGridImageDrawer::drawNotScaledOccupancyGridImage() cv::Point pixelPosition(x, y); if (dataValue == -1) { - m_notScaledOccupancyGridImage.at(pixelPosition) = - unknownSpaceColor; + m_notScaledOccupancyGridImage.at(pixelPosition) = unknownSpaceColor; } else if (dataValue == 0) { - m_notScaledOccupancyGridImage.at(pixelPosition) = - freeSpaceColor; + m_notScaledOccupancyGridImage.at(pixelPosition) = freeSpaceColor; } else { @@ -106,8 +104,7 @@ void OccupancyGridImageDrawer::changeNotScaledOccupancyGridImageIfNeeded() int gridHeight = static_cast(m_lastOccupancyGrid->info.height); int gridWidth = static_cast(m_lastOccupancyGrid->info.width); - if (m_notScaledOccupancyGridImage.rows != gridHeight - || m_notScaledOccupancyGridImage.cols != gridWidth) + if (m_notScaledOccupancyGridImage.rows != gridHeight || m_notScaledOccupancyGridImage.cols != gridWidth) { m_notScaledOccupancyGridImage = cv::Mat(gridHeight, gridWidth, CV_8UC3); } @@ -116,8 +113,7 @@ void OccupancyGridImageDrawer::changeNotScaledOccupancyGridImageIfNeeded() void OccupancyGridImageDrawer::scaleOccupancyGridImage() { changeScaledOccupancyGridImageIfNeeded(); - cv::resize(m_notScaledOccupancyGridImage, m_scaledOccupancyGridImage, - m_scaledOccupancyGridImage.size()); + cv::resize(m_notScaledOccupancyGridImage, m_scaledOccupancyGridImage, m_scaledOccupancyGridImage.size()); } const cv::Mat& OccupancyGridImageDrawer::getZoomedOccupancyImage() @@ -128,8 +124,12 @@ const cv::Mat& OccupancyGridImageDrawer::getZoomedOccupancyImage() } else { - cv::resize(m_scaledOccupancyGridImage, m_zoomedOccupancyGridImage, cv::Size{}, - m_parameters.scaleFactor(), m_parameters.scaleFactor()); + cv::resize( + m_scaledOccupancyGridImage, + m_zoomedOccupancyGridImage, + cv::Size{}, + m_parameters.scaleFactor(), + m_parameters.scaleFactor()); return m_zoomedOccupancyGridImage; } } @@ -140,13 +140,10 @@ void OccupancyGridImageDrawer::changeScaledOccupancyGridImageIfNeeded() int gridWidth = static_cast(m_lastOccupancyGrid->info.width); float gridResolution = m_lastOccupancyGrid->info.resolution; - int height = static_cast(static_cast(gridHeight) * gridResolution - * m_parameters.resolution()); - int width = static_cast(static_cast(gridWidth) * gridResolution - * m_parameters.resolution()); + int height = static_cast(static_cast(gridHeight) * gridResolution * m_parameters.resolution()); + int width = static_cast(static_cast(gridWidth) * gridResolution * m_parameters.resolution()); - if (m_scaledOccupancyGridImage.rows != height - || m_scaledOccupancyGridImage.cols != width) + if (m_scaledOccupancyGridImage.rows != height || m_scaledOccupancyGridImage.cols != width) { m_scaledOccupancyGridImage = cv::Mat(height, width, CV_8UC3); } @@ -159,9 +156,7 @@ std::unique_ptr OccupancyGridImageDrawer::getRobotTransform() con try { - m_tfListener.lookupTransform(m_parameters.mapFrameId(), - m_parameters.robotFrameId(), ros::Time(0), - transform); + m_tfListener.lookupTransform(m_parameters.mapFrameId(), m_parameters.robotFrameId(), ros::Time(0), transform); } catch (tf::TransformException& ex) { @@ -173,12 +168,17 @@ std::unique_ptr OccupancyGridImageDrawer::getRobotTransform() con return std::make_unique(transform); } -void OccupancyGridImageDrawer::rotateImageAboutPoint(cv::Mat& image, double angle, - const cv::Point& point) const +void OccupancyGridImageDrawer::rotateImageAboutPoint(cv::Mat& image, double angle, const cv::Point& point) const { cv::Mat rotationMatrix = cv::getRotationMatrix2D(point, angle, 1.0); - cv::warpAffine(image, image, rotationMatrix, image.size(), cv::INTER_LINEAR, - cv::BORDER_CONSTANT, m_parameters.unknownSpaceColor()); + cv::warpAffine( + image, + image, + rotationMatrix, + image.size(), + cv::INTER_LINEAR, + cv::BORDER_CONSTANT, + m_parameters.unknownSpaceColor()); } void OccupancyGridImageDrawer::rotateImageAboutCenter(cv::Mat& image, double angle) const { @@ -187,8 +187,7 @@ void OccupancyGridImageDrawer::rotateImageAboutCenter(cv::Mat& image, double ang } OccupancyGridImageDrawer::DirectionalValues -OccupancyGridImageDrawer::computePadding(const DirectionalValues& position, int height, - int width) + OccupancyGridImageDrawer::computePadding(const DirectionalValues& position, int height, int width) { return { .top = restrictToPositive((height - 1) / 2 - position.top), @@ -200,17 +199,15 @@ OccupancyGridImageDrawer::computePadding(const DirectionalValues& position, int } OccupancyGridImageDrawer::MapCoordinates -OccupancyGridImageDrawer::getMapCoordinatesFromTf(const tf::Transform& transform) const + OccupancyGridImageDrawer::getMapCoordinatesFromTf(const tf::Transform& transform) const { MapCoordinates mapCoordinates{}; - convertTransformToInputMapCoordinates(transform, m_lastOccupancyGrid->info, - mapCoordinates.x, mapCoordinates.y); + convertTransformToInputMapCoordinates(transform, m_lastOccupancyGrid->info, mapCoordinates.x, mapCoordinates.y); return mapCoordinates; } OccupancyGridImageDrawer::DirectionalValues -OccupancyGridImageDrawer::getDirectionsFromMapCoordinates( - const MapCoordinates& mapCoordinates, const cv::Mat& map) + OccupancyGridImageDrawer::getDirectionsFromMapCoordinates(const MapCoordinates& mapCoordinates, const cv::Mat& map) { return { .top = 0 + mapCoordinates.y, @@ -232,8 +229,7 @@ void OccupancyGridImageDrawer::drawOccupancyGridImage(cv::Mat& image) int outWidth = image.cols; MapCoordinates robotCoordinates = getMapCoordinatesFromTf(*robotTransform); - DirectionalValues robotPosition = - getDirectionsFromMapCoordinates(robotCoordinates, m_scaledOccupancyGridImage); + DirectionalValues robotPosition = getDirectionsFromMapCoordinates(robotCoordinates, m_scaledOccupancyGridImage); double heightBorder = 0.1 * outHeight; double widthBorder = 0.1 * outWidth; @@ -246,13 +242,10 @@ void OccupancyGridImageDrawer::drawOccupancyGridImage(cv::Mat& image) mapOriginPose.setOrigin({0.0, 0.0, 0.0}); MapCoordinates mapOriginCoordinates = getMapCoordinatesFromTf(mapOriginPose); - DirectionalValues mapPosition = - getDirectionsFromMapCoordinates(mapOriginCoordinates, m_scaledOccupancyGridImage); + DirectionalValues mapPosition = getDirectionsFromMapCoordinates(mapOriginCoordinates, m_scaledOccupancyGridImage); - double hScaleFactor = - (0.4 * outWidth) / std::abs(robotPosition.left - mapOriginCoordinates.x); - double vScaleFactor = - (0.4 * outHeight) / std::abs(robotPosition.top - mapOriginCoordinates.y); + double hScaleFactor = (0.4 * outWidth) / std::abs(robotPosition.left - mapOriginCoordinates.x); + double vScaleFactor = (0.4 * outHeight) / std::abs(robotPosition.top - mapOriginCoordinates.y); m_mutableParameters.setScaleFactor(std::min({hScaleFactor, vScaleFactor, 1.0})); @@ -262,21 +255,27 @@ void OccupancyGridImageDrawer::drawOccupancyGridImage(cv::Mat& image) .x = static_cast(mapOriginCoordinates.x * m_parameters.scaleFactor()), .y = static_cast(mapOriginCoordinates.y * m_parameters.scaleFactor()), }; - DirectionalValues zoomedMapPosition = - getDirectionsFromMapCoordinates(zoomedMapOriginCoordinates, zoomedMap); + DirectionalValues zoomedMapPosition = getDirectionsFromMapCoordinates(zoomedMapOriginCoordinates, zoomedMap); DirectionalValues padding = computePadding(zoomedMapPosition, outHeight, outWidth); cv::Mat paddedImage; - cv::copyMakeBorder(zoomedMap, paddedImage, padding.top, padding.bottom, padding.left, - padding.right, cv::BORDER_CONSTANT, - m_parameters.unknownSpaceColor()); + cv::copyMakeBorder( + zoomedMap, + paddedImage, + padding.top, + padding.bottom, + padding.left, + padding.right, + cv::BORDER_CONSTANT, + m_parameters.unknownSpaceColor()); using namespace map_image_generator; cv::Rect roi( - cv::Point(restrictToPositive(zoomedMapPosition.left - (outWidth - 1) / 2), - restrictToPositive(zoomedMapPosition.top - (outHeight - 1) / 2)), + cv::Point( + restrictToPositive(zoomedMapPosition.left - (outWidth - 1) / 2), + restrictToPositive(zoomedMapPosition.top - (outHeight - 1) / 2)), cv::Size(outWidth, outHeight)); paddedImage(roi).copyTo(image); cv::flip(image, image, 1); @@ -296,34 +295,41 @@ void OccupancyGridImageDrawer::drawOccupancyGridImageCenteredAroundRobot(cv::Mat // // Relative to scaled occupancy grid image MapCoordinates robotCoordinates = getMapCoordinatesFromTf(*robotTransform); - DirectionalValues robotPosition = - getDirectionsFromMapCoordinates(robotCoordinates, m_scaledOccupancyGridImage); + DirectionalValues robotPosition = getDirectionsFromMapCoordinates(robotCoordinates, m_scaledOccupancyGridImage); DirectionalValues padding = computePadding(robotPosition, outHeight, outWidth); adjustPaddingForCenteredRobotOffset(padding, outWidth, robotPosition); cv::Mat paddedImage; - cv::copyMakeBorder(m_scaledOccupancyGridImage, paddedImage, padding.top, - padding.bottom, padding.left, padding.right, cv::BORDER_CONSTANT, - m_parameters.unknownSpaceColor()); + cv::copyMakeBorder( + m_scaledOccupancyGridImage, + paddedImage, + padding.top, + padding.bottom, + padding.left, + padding.right, + cv::BORDER_CONSTANT, + m_parameters.unknownSpaceColor()); using namespace map_image_generator; double rotationAngle = rad2deg(tf::getYaw(robotTransform->getRotation())); - cv::Point rotationCenter{robotCoordinates.x + padding.left, - robotCoordinates.y + padding.top}; + cv::Point rotationCenter{robotCoordinates.x + padding.left, robotCoordinates.y + padding.top}; rotateImageAboutPoint(paddedImage, rotationAngle, rotationCenter); - cv::Rect roi(cv::Point(restrictToPositive(robotPosition.left - (outWidth - 1) / 2 - + m_parameters.robotVerticalOffset()), - restrictToPositive(robotPosition.top - (outHeight - 1) / 2)), - cv::Size(outWidth, outHeight)); + cv::Rect roi( + cv::Point( + restrictToPositive(robotPosition.left - (outWidth - 1) / 2 + m_parameters.robotVerticalOffset()), + restrictToPositive(robotPosition.top - (outHeight - 1) / 2)), + cv::Size(outWidth, outHeight)); paddedImage(roi).copyTo(image); cv::flip(image, image, 1); cv::rotate(image, image, cv::ROTATE_90_CLOCKWISE); } void OccupancyGridImageDrawer::adjustPaddingForCenteredRobotOffset( - DirectionalValues& padding, int width, const DirectionalValues& robotPosition) + DirectionalValues& padding, + int width, + const DirectionalValues& robotPosition) { // The vertical axis on which we apply the robot vertical offset is horizontal // currently as we will rotate it later @@ -335,19 +341,15 @@ void OccupancyGridImageDrawer::adjustPaddingForCenteredRobotOffset( { // If the offset is positive, we need to add padding to the top and can // possibly reduce padding to the bottom - bottomPaddingRotated -= std::min( - bottomPaddingRotated, restrictToPositive(m_parameters.robotVerticalOffset())); + bottomPaddingRotated -= std::min(bottomPaddingRotated, restrictToPositive(m_parameters.robotVerticalOffset())); topPaddingRotated += restrictToPositive(m_parameters.robotVerticalOffset()); } if (m_parameters.robotVerticalOffset() < 0) { // If the offset is negative, we need to recompute padding to the bottom and // can possibly reduce padding to the top - bottomPaddingRotated = - restrictToPositive((heightRotated - 1) / 2 - + restrictToPositive(-m_parameters.robotVerticalOffset()) - - bottomPositionRotated); - topPaddingRotated -= std::min( - topPaddingRotated, restrictToPositive(-m_parameters.robotVerticalOffset())); + bottomPaddingRotated = restrictToPositive( + (heightRotated - 1) / 2 + restrictToPositive(-m_parameters.robotVerticalOffset()) - bottomPositionRotated); + topPaddingRotated -= std::min(topPaddingRotated, restrictToPositive(-m_parameters.robotVerticalOffset())); } } diff --git a/map_image_generator/src/drawers/RobotImageDrawer.cpp b/map_image_generator/src/drawers/RobotImageDrawer.cpp index 2f7ba94..b5a87a6 100644 --- a/map_image_generator/src/drawers/RobotImageDrawer.cpp +++ b/map_image_generator/src/drawers/RobotImageDrawer.cpp @@ -6,9 +6,10 @@ using namespace map_image_generator; using namespace std; -RobotImageDrawer::RobotImageDrawer(const Parameters& parameters, - ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener) +RobotImageDrawer::RobotImageDrawer( + const Parameters& parameters, + ros::NodeHandle& nodeHandle, + tf::TransformListener& tfListener) : ImageDrawer(parameters, nodeHandle, tfListener) { } @@ -38,8 +39,14 @@ void RobotImageDrawer::drawRobot(cv::Mat& image, tf::Transform& robotTransform) int endX = static_cast(startX + size * cos(yaw)); int endY = static_cast(startY + size * sin(yaw)); - cv::circle(image, cv::Point(startX, startY), ceilDivision(size, 5.0), color, - cv::FILLED, cv::LINE_8); - cv::arrowedLine(image, cv::Point(startX, startY), cv::Point(endX, endY), color, - ceilDivision(size, 10.0), cv::LINE_8, 0, 0.3); + cv::circle(image, cv::Point(startX, startY), ceilDivision(size, 5.0), color, cv::FILLED, cv::LINE_8); + cv::arrowedLine( + image, + cv::Point(startX, startY), + cv::Point(endX, endY), + color, + ceilDivision(size, 10.0), + cv::LINE_8, + 0, + 0.3); } diff --git a/map_image_generator/src/drawers/SoundSourceImageDrawer.cpp b/map_image_generator/src/drawers/SoundSourceImageDrawer.cpp index f40f584..0d66192 100644 --- a/map_image_generator/src/drawers/SoundSourceImageDrawer.cpp +++ b/map_image_generator/src/drawers/SoundSourceImageDrawer.cpp @@ -7,27 +7,25 @@ using namespace map_image_generator; using namespace std; -SoundSourceImageDrawer::SoundSourceImageDrawer(const Parameters& parameters, - ros::NodeHandle& nodeHandle, - tf::TransformListener& tfListener) +SoundSourceImageDrawer::SoundSourceImageDrawer( + const Parameters& parameters, + ros::NodeHandle& nodeHandle, + tf::TransformListener& tfListener) : ImageDrawer(parameters, nodeHandle, tfListener), - m_soundSourcesArraySubscriber{nodeHandle.subscribe( - "sound_sources", 1, &SoundSourceImageDrawer::soundSourcesCallback, this)}, - m_laserScanSubscriber{nodeHandle.subscribe( - "laser_scan", 1, &SoundSourceImageDrawer::laserScanCallback, this)} + m_soundSourcesArraySubscriber{ + nodeHandle.subscribe("sound_sources", 1, &SoundSourceImageDrawer::soundSourcesCallback, this)}, + m_laserScanSubscriber{nodeHandle.subscribe("laser_scan", 1, &SoundSourceImageDrawer::laserScanCallback, this)} { } SoundSourceImageDrawer::~SoundSourceImageDrawer() = default; -void SoundSourceImageDrawer::soundSourcesCallback( - const odas_ros::OdasSstArrayStamped::ConstPtr& soundSources) +void SoundSourceImageDrawer::soundSourcesCallback(const odas_ros::OdasSstArrayStamped::ConstPtr& soundSources) { m_lastSoundSourcesArray = soundSources; } -void SoundSourceImageDrawer::laserScanCallback( - const sensor_msgs::LaserScan::ConstPtr& laserScan) +void SoundSourceImageDrawer::laserScanCallback(const sensor_msgs::LaserScan::ConstPtr& laserScan) { m_lastLaserScan = laserScan; } @@ -72,9 +70,11 @@ void SoundSourceImageDrawer::drawWithLidar(cv::Mat& image) try { - m_tfListener.lookupTransform(m_lastLaserScan->header.frame_id, - m_lastSoundSourcesArray->header.frame_id, - ros::Time(0), sourceToLidarTf); + m_tfListener.lookupTransform( + m_lastLaserScan->header.frame_id, + m_lastSoundSourcesArray->header.frame_id, + ros::Time(0), + sourceToLidarTf); } catch (tf::TransformException& ex) { @@ -99,7 +99,8 @@ void SoundSourceImageDrawer::drawWithoutLidar(cv::Mat& image) } void SoundSourceImageDrawer::drawSoundSourcesWithLidar( - cv::Mat& image, const tf::Transform& sourceToLidarTf, + cv::Mat& image, + const tf::Transform& sourceToLidarTf, const tf::Transform& lidarToRefTf) { for (const auto& source : m_lastSoundSourcesArray->sources) @@ -114,8 +115,7 @@ void SoundSourceImageDrawer::drawSoundSourcesWithLidar( } } -void SoundSourceImageDrawer::drawSoundSourcesWithoutLidar( - cv::Mat& image, const tf::Transform& sourceToRefTf) +void SoundSourceImageDrawer::drawSoundSourcesWithoutLidar(cv::Mat& image, const tf::Transform& sourceToRefTf) { for (const auto& source : m_lastSoundSourcesArray->sources) { @@ -126,9 +126,7 @@ void SoundSourceImageDrawer::drawSoundSourcesWithoutLidar( } } -void SoundSourceImageDrawer::drawSoundSource(cv::Mat& image, - const odas_ros::OdasSst& source, - tf::Pose poseInRef) +void SoundSourceImageDrawer::drawSoundSource(cv::Mat& image, const odas_ros::OdasSst& source, tf::Pose poseInRef) { int size = m_parameters.soundSourceSize(); @@ -140,17 +138,13 @@ void SoundSourceImageDrawer::drawSoundSource(cv::Mat& image, drawConcentricCircles(image, centerX, centerY, size, source.activity); } -void SoundSourceImageDrawer::drawConcentricCircles(cv::Mat& image, int x, int y, - int radius, double colorRatio) +void SoundSourceImageDrawer::drawConcentricCircles(cv::Mat& image, int x, int y, int radius, double colorRatio) { const cv::Scalar& color = - interpolateColors(m_parameters.soundSourceColorFull(), - m_parameters.soundSourceColorDim(), colorRatio); + interpolateColors(m_parameters.soundSourceColorFull(), m_parameters.soundSourceColorDim(), colorRatio); - cv::circle(image, cv::Point(x, y), ceilDivision(radius, 25.0), color, cv::FILLED, - cv::LINE_8); - cv::circle(image, cv::Point(x, y), ceilDivision(radius, 8.0), color, - ceilDivision(radius, 30.0), cv::LINE_8); + cv::circle(image, cv::Point(x, y), ceilDivision(radius, 25.0), color, cv::FILLED, cv::LINE_8); + cv::circle(image, cv::Point(x, y), ceilDivision(radius, 8.0), color, ceilDivision(radius, 30.0), cv::LINE_8); } float SoundSourceImageDrawer::getRangeForAngle(double angle) @@ -160,16 +154,13 @@ float SoundSourceImageDrawer::getRangeForAngle(double angle) return -1; } - int index = static_cast(std::floor((angle - m_lastLaserScan->angle_min) - / m_lastLaserScan->angle_increment)); + int index = static_cast(std::floor((angle - m_lastLaserScan->angle_min) / m_lastLaserScan->angle_increment)); if (index < 0 || index >= m_lastLaserScan->ranges.size()) { std::ostringstream oss; - oss << "Index out of range, this shouldn't happen. => angle: " << angle - << " ; angle_range: [" << m_lastLaserScan->angle_min << ", " - << m_lastLaserScan->angle_max - << "] ; increment: " << m_lastLaserScan->angle_increment - << " ; index: " << index + oss << "Index out of range, this shouldn't happen. => angle: " << angle << " ; angle_range: [" + << m_lastLaserScan->angle_min << ", " << m_lastLaserScan->angle_max + << "] ; increment: " << m_lastLaserScan->angle_increment << " ; index: " << index << " ; ranges.size(): " << m_lastLaserScan->ranges.size(); throw std::runtime_error(oss.str()); } @@ -198,8 +189,7 @@ tf::Pose SoundSourceImageDrawer::getRangePose(const tf::Pose& lidarPose) range = m_parameters.soundSourceMaxRange(); } - return tf::Pose(tf::Quaternion(0, 0, 0, 0), - tf::Vector3(range * cos(angle), range * sin(angle), 0)); + return tf::Pose(tf::Quaternion(0, 0, 0, 0), tf::Vector3(range * cos(angle), range * sin(angle), 0)); } tf::Pose SoundSourceImageDrawer::getRefEndPose(const tf::Pose& refPose) @@ -207,6 +197,5 @@ tf::Pose SoundSourceImageDrawer::getRefEndPose(const tf::Pose& refPose) double angle = tf::getYaw(refPose.getRotation()); float range = m_parameters.soundSourceRange(); - return tf::Pose(tf::Quaternion(0, 0, 0, 0), - tf::Vector3(range * cos(angle), range * sin(angle), 0)); + return tf::Pose(tf::Quaternion(0, 0, 0, 0), tf::Vector3(range * cos(angle), range * sin(angle), 0)); } diff --git a/map_image_generator/src/main.cpp b/map_image_generator/src/main.cpp index 1ce05cf..93ca535 100644 --- a/map_image_generator/src/main.cpp +++ b/map_image_generator/src/main.cpp @@ -23,8 +23,7 @@ int main(int argc, char** argv) MapLabelsConverter mapLabelsConverter{parameters, nodeHandle}; image_transport::ImageTransport imageTransport{nodeHandle}; - image_transport::Publisher mapImagePublisher = - imageTransport.advertise("map_image", 1); + image_transport::Publisher mapImagePublisher = imageTransport.advertise("map_image", 1); sensor_msgs::Image mapImage; ROS_INFO("MapImage initialized, starting image generation after first cycle..."); diff --git a/map_image_generator/src/utils.cpp b/map_image_generator/src/utils.cpp index 4dac9ca..3867b08 100644 --- a/map_image_generator/src/utils.cpp +++ b/map_image_generator/src/utils.cpp @@ -8,8 +8,7 @@ using namespace std; namespace map_image_generator { geometry_msgs::PoseStamped - convertMapToMapImage(const Parameters& parameters, - const geometry_msgs::PoseStamped& mapPose) + convertMapToMapImage(const Parameters& parameters, const geometry_msgs::PoseStamped& mapPose) { geometry_msgs::PoseStamped mapImagePose; mapImagePose.header.seq = mapPose.header.seq; @@ -20,17 +19,13 @@ namespace map_image_generator return mapImagePose; } - geometry_msgs::Pose convertMapToMapImage(const Parameters& parameters, - const geometry_msgs::Pose& mapPose) + geometry_msgs::Pose convertMapToMapImage(const Parameters& parameters, const geometry_msgs::Pose& mapPose) { geometry_msgs::Pose mapImagePose; double mapImageWidth = parameters.resolution() * parameters.width(); - mapImagePose.position.x = - mapImageWidth - - (mapPose.position.x * parameters.resolution() + parameters.xOrigin()); - mapImagePose.position.y = - mapPose.position.y * parameters.resolution() + parameters.yOrigin(); + mapImagePose.position.x = mapImageWidth - (mapPose.position.x * parameters.resolution() + parameters.xOrigin()); + mapImagePose.position.y = mapPose.position.y * parameters.resolution() + parameters.yOrigin(); mapImagePose.position.z = 0; mapImagePose.orientation = mapPose.orientation; @@ -38,17 +33,15 @@ namespace map_image_generator return mapImagePose; } - geometry_msgs::Pose - convertRobotCenteredMapCoordinatesToPose(const Parameters& parameters, int x, int y, - double yaw) + geometry_msgs::Pose convertRobotCenteredMapCoordinatesToPose(const Parameters& parameters, int x, int y, double yaw) { geometry_msgs::Pose pose; int rows = parameters.height() * parameters.resolution(); int cols = parameters.width() * parameters.resolution(); double centerY = rows / 2.0; double centerX = cols / 2.0; - pose.position.x = -(y - centerY - parameters.robotVerticalOffset()) - / static_cast(parameters.resolution()); + pose.position.x = + -(y - centerY - parameters.robotVerticalOffset()) / static_cast(parameters.resolution()); pose.position.y = -(x - centerX) / static_cast(parameters.resolution()); pose.position.z = 0; pose.orientation = tf::createQuaternionMsgFromYaw(yaw); @@ -58,8 +51,7 @@ namespace map_image_generator } geometry_msgs::PoseStamped - convertMapImageToRobot(const Parameters& parameters, - const geometry_msgs::PoseStamped& mapImagePose) + convertMapImageToRobot(const Parameters& parameters, const geometry_msgs::PoseStamped& mapImagePose) { geometry_msgs::PoseStamped robotPose; robotPose.header.seq = mapImagePose.header.seq; @@ -70,18 +62,17 @@ namespace map_image_generator return robotPose; } - geometry_msgs::Pose convertMapImageToRobot(const Parameters& parameters, - const geometry_msgs::Pose& mapImagePose) + geometry_msgs::Pose convertMapImageToRobot(const Parameters& parameters, const geometry_msgs::Pose& mapImagePose) { return convertRobotCenteredMapCoordinatesToPose( - parameters, static_cast(mapImagePose.position.x), + parameters, + static_cast(mapImagePose.position.x), static_cast(mapImagePose.position.y), tf::getYaw(mapImagePose.orientation)); } geometry_msgs::PoseStamped - convertMapImageToMap(const Parameters& parameters, - const geometry_msgs::PoseStamped& mapImagePose) + convertMapImageToMap(const Parameters& parameters, const geometry_msgs::PoseStamped& mapImagePose) { geometry_msgs::PoseStamped mapPose; mapPose.header.seq = mapImagePose.header.seq; @@ -92,17 +83,14 @@ namespace map_image_generator return mapPose; } - geometry_msgs::Pose convertMapImageToMap(const Parameters& parameters, - const geometry_msgs::Pose& mapImagePose) + geometry_msgs::Pose convertMapImageToMap(const Parameters& parameters, const geometry_msgs::Pose& mapImagePose) { geometry_msgs::Pose mapPose; - double flippedXOnY = - parameters.resolution() * parameters.width() - mapImagePose.position.x; - mapPose.position.x = (flippedXOnY - parameters.xOrigin()) - / parameters.resolution() / parameters.scaleFactor(); - mapPose.position.y = (mapImagePose.position.y - parameters.yOrigin()) - / parameters.resolution() / parameters.scaleFactor(); + double flippedXOnY = parameters.resolution() * parameters.width() - mapImagePose.position.x; + mapPose.position.x = (flippedXOnY - parameters.xOrigin()) / parameters.resolution() / parameters.scaleFactor(); + mapPose.position.y = + (mapImagePose.position.y - parameters.yOrigin()) / parameters.resolution() / parameters.scaleFactor(); mapPose.position.z = 0; mapPose.orientation = mapImagePose.orientation; @@ -117,8 +105,7 @@ namespace map_image_generator pose.orientation = tf::createQuaternionMsgFromYaw(yaw); } - cv::Scalar interpolateColors(const cv::Scalar& color1, const cv::Scalar& color2, - double ratio) + cv::Scalar interpolateColors(const cv::Scalar& color1, const cv::Scalar& color2, double ratio) { cv::Scalar color; color[0] = (color2[0] - color1[0]) * ratio + color1[0]; diff --git a/map_image_generator/srv/ImageGoalToMapGoal.srv b/map_image_generator/srv/ImageGoalToMapGoal.srv index 354bd38..9d2837a 100644 --- a/map_image_generator/srv/ImageGoalToMapGoal.srv +++ b/map_image_generator/srv/ImageGoalToMapGoal.srv @@ -1,3 +1,3 @@ geometry_msgs/PoseStamped image_goal --- -geometry_msgs/PoseStamped map_goal \ No newline at end of file +geometry_msgs/PoseStamped map_goal diff --git a/opentera_client_ros/CMakeLists.txt b/opentera_client_ros/CMakeLists.txt index 5f7ec9b..315edd0 100644 --- a/opentera_client_ros/CMakeLists.txt +++ b/opentera_client_ros/CMakeLists.txt @@ -10,10 +10,10 @@ project(opentera_client_ros) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS - rospy - std_msgs - message_runtime - opentera_webrtc_ros_msgs + rospy + std_msgs + message_runtime + opentera_webrtc_ros_msgs ) ## System dependencies are found with CMake's conventions @@ -120,7 +120,7 @@ catkin_package( ## Your package locations should be listed before other locations include_directories( # include - ${catkin_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} ) ## Declare a C++ library @@ -158,9 +158,11 @@ include_directories( ############# #Install executables -install(PROGRAMS - scripts/opentera_client_ros.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install( + PROGRAMS + scripts/opentera_client_ros.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) # all install targets should use catkin DESTINATION variables diff --git a/opentera_client_ros/README.md b/opentera_client_ros/README.md index 5082b83..1c0e722 100644 --- a/opentera_client_ros/README.md +++ b/opentera_client_ros/README.md @@ -1,2 +1 @@ TO BE DOCUMENTED - diff --git a/opentera_client_ros/config/client_config.json b/opentera_client_ros/config/client_config.json index a4d91d7..2bc7bfc 100644 --- a/opentera_client_ros/config/client_config.json +++ b/opentera_client_ros/config/client_config.json @@ -1,4 +1,4 @@ { "client_token": "Generate a token with opentera device", "url": "enter url here like https://localhost:port" -} \ No newline at end of file +} diff --git a/opentera_client_ros/launch/client.launch b/opentera_client_ros/launch/client.launch index 7e7d144..37f7cce 100644 --- a/opentera_client_ros/launch/client.launch +++ b/opentera_client_ros/launch/client.launch @@ -5,4 +5,4 @@ - + diff --git a/opentera_client_ros/package.xml b/opentera_client_ros/package.xml index ad522f9..c43837a 100644 --- a/opentera_client_ros/package.xml +++ b/opentera_client_ros/package.xml @@ -1,68 +1,68 @@ - opentera_client_ros + opentera_client_ros - 0.0.0 - The opentera_client_ros package + 0.0.0 + The opentera_client_ros package - - - - Dominic Létourneau + + + + Dominic Létourneau - - - - Apache-2.0 + + + + Apache-2.0 - - - - + + + + - - - - + + + + - - - - - - - - - - - - - - - - - - - - - catkin - rospy - std_msgs - opentera_webrtc_ros_msgs - rospy - std_msgs - opentera_webrtc_ros_msgs - rospy - std_msgs - opentera_webrtc_ros_msgs + + + + + + + + + + + + + + + + + + + + + catkin + rospy + std_msgs + opentera_webrtc_ros_msgs + rospy + std_msgs + opentera_webrtc_ros_msgs + rospy + std_msgs + opentera_webrtc_ros_msgs - - - + + + - + diff --git a/opentera_client_ros/requirements.txt b/opentera_client_ros/requirements.txt index b419dbb..ff6e700 100644 --- a/opentera_client_ros/requirements.txt +++ b/opentera_client_ros/requirements.txt @@ -1,4 +1,3 @@ opentera==1.0.8 aiohttp asyncio - diff --git a/opentera_client_ros/scripts/opentera_client_ros.py b/opentera_client_ros/scripts/opentera_client_ros.py index a797406..03b1046 100755 --- a/opentera_client_ros/scripts/opentera_client_ros.py +++ b/opentera_client_ros/scripts/opentera_client_ros.py @@ -37,8 +37,10 @@ class OpenTeraROSClient: def __init__(self, url: str, token: str): self.__base_url = url self.__token = token - self.__event_publisher = rospy.Publisher('events', OpenTeraEvent, queue_size=10) - self.__robot_status_subscriber = rospy.Subscriber('robot_status', RobotStatus, self.robot_status_callback) + self.__event_publisher = rospy.Publisher( + 'events', OpenTeraEvent, queue_size=10) + self.__robot_status_subscriber = rospy.Subscriber( + 'robot_status', RobotStatus, self.robot_status_callback) self.__robot_status = {} def robot_status_callback(self, status: RobotStatus): @@ -83,7 +85,7 @@ async def _opentera_main_loop(self, url, token): # Create alive publishing task status_task = asyncio.get_event_loop().create_task( - self._opentera_send_device_status(client, url + OpenTeraROSClient.status_api_endpoint, token)) + self._opentera_send_device_status(client, url + OpenTeraROSClient.status_api_endpoint, token)) while not rospy.is_shutdown(): msg = await ws.receive() @@ -106,7 +108,8 @@ def run(self): try: loop = asyncio.get_event_loop() - main_task = asyncio.ensure_future(self._opentera_main_loop(self.__base_url, self.__token)) + main_task = asyncio.ensure_future( + self._opentera_main_loop(self.__base_url, self.__token)) for signal in [SIGINT, SIGTERM]: loop.add_signal_handler(signal, main_task.cancel) @@ -136,7 +139,8 @@ async def _opentera_send_device_status(self, client: aiohttp.ClientSession, url: async def _parse_message(self, client: aiohttp.ClientSession, msg_dict: dict): try: if 'message' in msg_dict: - message = ParseDict(msg_dict['message'], messages.TeraEvent(), ignore_unknown_fields=True) + message = ParseDict( + msg_dict['message'], messages.TeraEvent(), ignore_unknown_fields=True) # All events in same message opentera_events = OpenTeraEvent() @@ -256,4 +260,3 @@ async def _parse_message(self, client: aiohttp.ClientSession, msg_dict: dict): client = OpenTeraROSClient(url=url, token=token) client.run() - diff --git a/opentera_client_ros/setup.py b/opentera_client_ros/setup.py index f311f8a..1e31863 100644 --- a/opentera_client_ros/setup.py +++ b/opentera_client_ros/setup.py @@ -4,8 +4,8 @@ from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( - packages=['opentera_client_ros'], - package_dir={'': 'src'} - ) + packages=['opentera_client_ros'], + package_dir={'': 'src'} +) setup(**d) diff --git a/opentera_webrtc_demos/config/model.rviz b/opentera_webrtc_demos/config/model.rviz index 3863700..b31b57d 100644 --- a/opentera_webrtc_demos/config/model.rviz +++ b/opentera_webrtc_demos/config/model.rviz @@ -175,27 +175,18 @@ Visualization Manager: Tree: base_footprint: base_link: - base_scan: - {} - caster_back_left_link: - {} - caster_back_right_link: - {} - front_camera_link: - {} - imu_link: - {} + base_scan: {} + caster_back_left_link: {} + caster_back_right_link: {} + front_camera_link: {} + imu_link: {} r200_link: r200_depth_frame: - r200_depth_optical_frame: - {} + r200_depth_optical_frame: {} r200_rgb_frame: - r200_rgb_optical_frame: - {} - wheel_left_link: - {} - wheel_right_link: - {} + r200_rgb_optical_frame: {} + wheel_left_link: {} + wheel_right_link: {} Update Interval: 0 Value: true Enabled: true diff --git a/opentera_webrtc_demos/launch/demo-dev.launch b/opentera_webrtc_demos/launch/demo-dev.launch index ee1dcac..241e284 100644 --- a/opentera_webrtc_demos/launch/demo-dev.launch +++ b/opentera_webrtc_demos/launch/demo-dev.launch @@ -10,4 +10,4 @@ - + diff --git a/opentera_webrtc_demos/launch/demo-odas.launch b/opentera_webrtc_demos/launch/demo-odas.launch index 8ca1955..b54354b 100644 --- a/opentera_webrtc_demos/launch/demo-odas.launch +++ b/opentera_webrtc_demos/launch/demo-odas.launch @@ -1,48 +1,48 @@ - - - - - - - + + + + + + + - - - - - - - - - - - + + + + + + + + + + + - - - - - - - - - + + + + + + + + + - - + + - - - - - - - - - - - - + + + + + + + + + + + + diff --git a/opentera_webrtc_demos/launch/model_viewer.launch b/opentera_webrtc_demos/launch/model_viewer.launch index 1fb810d..120463e 100644 --- a/opentera_webrtc_demos/launch/model_viewer.launch +++ b/opentera_webrtc_demos/launch/model_viewer.launch @@ -1,14 +1,14 @@ - + - - + + - + - + - + diff --git a/opentera_webrtc_demos/launch/opentera_signaling_server.launch b/opentera_webrtc_demos/launch/opentera_signaling_server.launch index ab385c8..a798bdc 100644 --- a/opentera_webrtc_demos/launch/opentera_signaling_server.launch +++ b/opentera_webrtc_demos/launch/opentera_signaling_server.launch @@ -1,28 +1,28 @@ - - + + - - - + + + - + - - - + + + - - + + - - - + - + diff --git a/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch b/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch index cdd6883..b7671a2 100644 --- a/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch +++ b/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch @@ -1,115 +1,115 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - + + + - - - - - - - - - - - - - - - - - - - - - diff --git a/opentera_webrtc_demos/models/aruco_tag/model.config b/opentera_webrtc_demos/models/aruco_tag/model.config index ecf4faf..2482c72 100644 --- a/opentera_webrtc_demos/models/aruco_tag/model.config +++ b/opentera_webrtc_demos/models/aruco_tag/model.config @@ -13,4 +13,4 @@ aruco tag model 6x6_1000-0 - \ No newline at end of file + diff --git a/opentera_webrtc_demos/models/aruco_tag/model.sdf b/opentera_webrtc_demos/models/aruco_tag/model.sdf index 8569ba4..1ccebea 100644 --- a/opentera_webrtc_demos/models/aruco_tag/model.sdf +++ b/opentera_webrtc_demos/models/aruco_tag/model.sdf @@ -22,4 +22,4 @@ - \ No newline at end of file + diff --git a/opentera_webrtc_demos/opentera-webrtc-teleop-frontend b/opentera_webrtc_demos/opentera-webrtc-teleop-frontend index e188f70..8360a39 160000 --- a/opentera_webrtc_demos/opentera-webrtc-teleop-frontend +++ b/opentera_webrtc_demos/opentera-webrtc-teleop-frontend @@ -1 +1 @@ -Subproject commit e188f708e88ea79083e8d4406b07b0a8c4c84289 +Subproject commit 8360a39a001bcebb84c4b067026c17ab67c04489 diff --git a/opentera_webrtc_demos/scripts/robot_status.py b/opentera_webrtc_demos/scripts/robot_status.py index 7074dad..654a4af 100755 --- a/opentera_webrtc_demos/scripts/robot_status.py +++ b/opentera_webrtc_demos/scripts/robot_status.py @@ -9,28 +9,30 @@ from std_msgs.msg import String import json + class RobotStatusPublisher(): def __init__(self): rospy.init_node("robot_status_publisher") - self.status_pub = rospy.Publisher('/robot_status', RobotStatus, queue_size=10) - self.status_webrtc_pub = rospy.Publisher('/webrtc_data_outgoing', String, queue_size=10) + self.status_pub = rospy.Publisher( + '/robot_status', RobotStatus, queue_size=10) + self.status_webrtc_pub = rospy.Publisher( + '/webrtc_data_outgoing', String, queue_size=10) self.pub_rate = 1 - def get_ip_address(self, ifname: str): try: - address = os.popen('ip addr show ' + ifname).read().split("inet ")[1].split("/")[0] + address = os.popen('ip addr show ' + + ifname).read().split("inet ")[1].split("/")[0] except Exception as e: address = '127.0.0.1' finally: return address - def get_disk_usage(self, mount_point='/'): - result=os.statvfs(mount_point) - block_size=result.f_frsize - total_blocks=result.f_blocks - free_blocks=result.f_bfree + result = os.statvfs(mount_point) + block_size = result.f_frsize + total_blocks = result.f_blocks + free_blocks = result.f_bfree return 100 - (free_blocks * 100 / total_blocks) def run(self): @@ -45,31 +47,36 @@ def run(self): status.battery_voltage = float(i) status.battery_current = 1.0 status.cpu_usage = psutil.cpu_percent() - status.mem_usage = 100 - (psutil.virtual_memory().available * 100 / psutil.virtual_memory().total) + status.mem_usage = 100 - \ + (psutil.virtual_memory().available * + 100 / psutil.virtual_memory().total) status.disk_usage = self.get_disk_usage() - - subprocess_result = subprocess.Popen('iwgetid',shell=True,stdout=subprocess.PIPE) - subprocess_output = subprocess_result.communicate()[0],subprocess_result.returncode + subprocess_result = subprocess.Popen( + 'iwgetid', shell=True, stdout=subprocess.PIPE) + subprocess_output = subprocess_result.communicate()[ + 0], subprocess_result.returncode network_name = subprocess_output[0].decode('utf-8') status.wifi_network = network_name if status.wifi_network: wifi_interface_name = status.wifi_network.split()[0] command = "iwconfig %s | grep 'Link Quality='" % wifi_interface_name - subprocess_result = subprocess.Popen(command, shell=True,stdout=subprocess.PIPE) - subprocess_output = subprocess_result.communicate()[0],subprocess_result.returncode + subprocess_result = subprocess.Popen( + command, shell=True, stdout=subprocess.PIPE) + subprocess_output = subprocess_result.communicate()[ + 0], subprocess_result.returncode decoded_output = subprocess_output[0].decode('utf-8') - numerator = int(re.search('=(.+?)/', decoded_output).group(1)) - denominator = int(re.search('/(.+?) ', decoded_output).group(1)) + numerator = int( + re.search('=(.+?)/', decoded_output).group(1)) + denominator = int( + re.search('/(.+?) ', decoded_output).group(1)) status.wifi_strength = numerator / denominator * 100 status.local_ip = self.get_ip_address(wifi_interface_name) else: status.wifi_strength = 0 status.local_ip = '127.0.0.1' - - # Publish for ROS self.status_pub.publish(status) @@ -94,7 +101,7 @@ def run(self): if rospy.is_shutdown(): break - + r.sleep() diff --git a/opentera_webrtc_robot_gui/CMakeLists.txt b/opentera_webrtc_robot_gui/CMakeLists.txt index 9c61648..5a6c2e9 100644 --- a/opentera_webrtc_robot_gui/CMakeLists.txt +++ b/opentera_webrtc_robot_gui/CMakeLists.txt @@ -8,11 +8,11 @@ project(opentera_webrtc_robot_gui) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS - opentera_webrtc_ros_msgs - roscpp - rospy - std_msgs - sensor_msgs + opentera_webrtc_ros_msgs + roscpp + rospy + std_msgs + sensor_msgs ) # Find Ubuntu release version @@ -29,27 +29,27 @@ set(opentera_webrtc_robot_gui_components Core Widgets Gui OpenGL) find_package(Qt5 COMPONENTS ${opentera_webrtc_robot_gui_components} REQUIRED) set(srcs - src/main.cpp - src/MainWindow.cpp - src/ROSCameraView.cpp - src/GraphicsViewToolbar.cpp - src/ConfigDialog.cpp + src/main.cpp + src/MainWindow.cpp + src/ROSCameraView.cpp + src/GraphicsViewToolbar.cpp + src/ConfigDialog.cpp ) set(headers - src/MainWindow.h - src/ROSCameraView.h - src/GraphicsViewToolbar.h - src/ConfigDialog.h + src/MainWindow.h + src/ROSCameraView.h + src/GraphicsViewToolbar.h + src/ConfigDialog.h ) set(uis - src/ui/MainWindow.ui - src/ui/ConfigDialog.ui + src/ui/MainWindow.ui + src/ui/ConfigDialog.ui ) set(qrcs - src/resources/opentera_webrtc_robot_gui.qrc + src/resources/opentera_webrtc_robot_gui.qrc ) #Generate .h files from the .ui files @@ -164,9 +164,9 @@ catkin_package( ## Your package locations should be listed before other locations include_directories( # include - ${catkin_INCLUDE_DIRS} - ${CMAKE_CURRENT_SOURCE_DIR}/src - ${CMAKE_CURRENT_BINARY_DIR} + ${catkin_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${CMAKE_CURRENT_BINARY_DIR} ) add_executable(${PROJECT_NAME}_node ${srcs} ${uis} ${headers} ${project_moc_uis} ${project_qrcs} ${project_moc_srcs}) @@ -200,8 +200,8 @@ add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catk ## Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME}_node - ${catkin_LIBRARIES} - ${opentera_webrtc_robot_gui_components_link} + ${catkin_LIBRARIES} + ${opentera_webrtc_robot_gui_components_link} ) ############# @@ -213,15 +213,16 @@ target_link_libraries(${PROJECT_NAME}_node ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination -catkin_install_python(PROGRAMS - scripts/opentera_webrtc_robot_gui.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +catkin_install_python( + PROGRAMS + scripts/opentera_webrtc_robot_gui.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) ## Mark executables for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html install(TARGETS ${PROJECT_NAME}_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) ## Mark libraries for installation diff --git a/opentera_webrtc_robot_gui/launch/test_gui.launch b/opentera_webrtc_robot_gui/launch/test_gui.launch index ca6b82b..40a0da6 100644 --- a/opentera_webrtc_robot_gui/launch/test_gui.launch +++ b/opentera_webrtc_robot_gui/launch/test_gui.launch @@ -12,4 +12,4 @@ - \ No newline at end of file + diff --git a/opentera_webrtc_robot_gui/package.xml b/opentera_webrtc_robot_gui/package.xml index e686f99..3dd8032 100644 --- a/opentera_webrtc_robot_gui/package.xml +++ b/opentera_webrtc_robot_gui/package.xml @@ -1,73 +1,73 @@ - opentera_webrtc_robot_gui - 0.0.0 - The opentera_webrtc_robot_gui package + opentera_webrtc_robot_gui + 0.0.0 + The opentera_webrtc_robot_gui package - - - - Dominic Letourneau + + + + Dominic Letourneau - - - - Apache-v2 + + + + Apache-v2 - - - - + + + + - - - - + + + + - - - - - - - - - - - - - - - - - - - - - catkin - opentera_webrtc_ros_msgs - roscpp - rospy - std_msgs - sensor_msgs - opentera_webrtc_ros_msgs - roscpp - rospy - std_msgs - sensor_msgs - opentera_webrtc_ros_msgs - roscpp - rospy - std_msgs - sensor_msgs + + + + + + + + + + + + + + + + + + + + + catkin + opentera_webrtc_ros_msgs + roscpp + rospy + std_msgs + sensor_msgs + opentera_webrtc_ros_msgs + roscpp + rospy + std_msgs + sensor_msgs + opentera_webrtc_ros_msgs + roscpp + rospy + std_msgs + sensor_msgs - - - + + + - + diff --git a/opentera_webrtc_robot_gui/requirements.txt b/opentera_webrtc_robot_gui/requirements.txt index 37a69c4..2d07ca0 100644 --- a/opentera_webrtc_robot_gui/requirements.txt +++ b/opentera_webrtc_robot_gui/requirements.txt @@ -1 +1 @@ -PyQt5 \ No newline at end of file +PyQt5 diff --git a/opentera_webrtc_robot_gui/scripts/opentera_webrtc_robot_gui.py b/opentera_webrtc_robot_gui/scripts/opentera_webrtc_robot_gui.py index 67098cf..da3f99e 100644 --- a/opentera_webrtc_robot_gui/scripts/opentera_webrtc_robot_gui.py +++ b/opentera_webrtc_robot_gui/scripts/opentera_webrtc_robot_gui.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # PYTHONPATH is set properly when loading a workspace. -#Qt +# Qt from PyQt5.QtWidgets import QApplication, QMainWindow, QVBoxLayout, QWidget, QLabel from PyQt5.QtGui import QImage, QPixmap @@ -22,19 +22,20 @@ def __init__(self, parent=None): self._layout.addWidget(self._label) self._label.setText('Hello!') - def setImage(self, frame: Image): width = frame.width height = frame.height encoding = frame.encoding - image = QImage(frame.data, frame.width, frame.height, QImage.Format_RGB888).rgbSwapped() + image = QImage(frame.data, frame.width, frame.height, + QImage.Format_RGB888).rgbSwapped() self._label.setPixmap(QPixmap.fromImage(image)) class MainWindow(QMainWindow): def __init__(self, parent=None): super(QMainWindow, self).__init__(parent) - self._peer_image_subscriber = rospy.Subscriber('/webrtc_image', PeerImage, self._on_peer_image, queue_size=10) + self._peer_image_subscriber = rospy.Subscriber( + '/webrtc_image', PeerImage, self._on_peer_image, queue_size=10) self._image_view = ImageView(self) self.setCentralWidget(self._image_view) @@ -42,12 +43,10 @@ def _on_peer_image(self, image: PeerImage): print('image from', image.sender.id) self._image_view.setImage(image.frame) - if __name__ == '__main__': # Init ROS rospy.init_node('opentera_webrtc_robot_gui', anonymous=True) - # Init Qt App # Create an instance of QApplication @@ -59,5 +58,3 @@ def _on_peer_image(self, image: PeerImage): # Execute application sys.exit(app.exec_()) - - diff --git a/opentera_webrtc_robot_gui/setup.py b/opentera_webrtc_robot_gui/setup.py index c6679a4..07babdc 100644 --- a/opentera_webrtc_robot_gui/setup.py +++ b/opentera_webrtc_robot_gui/setup.py @@ -4,8 +4,8 @@ from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( - packages=['opentera_webrtc_robot_gui'], - package_dir={'': 'src'} - ) + packages=['opentera_webrtc_robot_gui'], + package_dir={'': 'src'} +) setup(**d) diff --git a/opentera_webrtc_robot_gui/src/ConfigDialog.cpp b/opentera_webrtc_robot_gui/src/ConfigDialog.cpp index 33092d3..20d9663 100644 --- a/opentera_webrtc_robot_gui/src/ConfigDialog.cpp +++ b/opentera_webrtc_robot_gui/src/ConfigDialog.cpp @@ -2,14 +2,10 @@ #include "ui_ConfigDialog.h" -ConfigDialog::ConfigDialog(QWidget *parent) - : m_ui(new Ui::ConfigDialog()) +ConfigDialog::ConfigDialog(QWidget* parent) : m_ui(new Ui::ConfigDialog()) { m_ui->setupUi(this); setWindowFlags(Qt::Dialog | Qt::FramelessWindowHint | Qt::WindowTitleHint); } -ConfigDialog::~ConfigDialog() -{ - -} \ No newline at end of file +ConfigDialog::~ConfigDialog() {} diff --git a/opentera_webrtc_robot_gui/src/ConfigDialog.h b/opentera_webrtc_robot_gui/src/ConfigDialog.h index 1523c0f..cdf3e4e 100644 --- a/opentera_webrtc_robot_gui/src/ConfigDialog.h +++ b/opentera_webrtc_robot_gui/src/ConfigDialog.h @@ -5,7 +5,10 @@ QT_BEGIN_NAMESPACE -namespace Ui { class ConfigDialog; } + namespace Ui + { + class ConfigDialog; + } QT_END_NAMESPACE @@ -13,13 +16,12 @@ class ConfigDialog : public QDialog { Q_OBJECT - public: - ConfigDialog(QWidget *parent=nullptr); +public: + ConfigDialog(QWidget* parent = nullptr); ~ConfigDialog(); - protected: - - Ui::ConfigDialog *m_ui; +protected: + Ui::ConfigDialog* m_ui; }; -#endif //#define _CONFIG_DIALOG_H_ +#endif //#define _CONFIG_DIALOG_H_ diff --git a/opentera_webrtc_robot_gui/src/GraphicsViewToolbar.cpp b/opentera_webrtc_robot_gui/src/GraphicsViewToolbar.cpp index c55a077..7ab0ff5 100644 --- a/opentera_webrtc_robot_gui/src/GraphicsViewToolbar.cpp +++ b/opentera_webrtc_robot_gui/src/GraphicsViewToolbar.cpp @@ -3,8 +3,7 @@ #include #include -GraphicsViewToolbar::GraphicsViewToolbar(QWidget *parent) - : QGraphicsView(parent) +GraphicsViewToolbar::GraphicsViewToolbar(QWidget* parent) : QGraphicsView(parent) { m_scene = new QGraphicsScene(this); @@ -15,7 +14,7 @@ GraphicsViewToolbar::GraphicsViewToolbar(QWidget *parent) m_batteryTextItem = new QGraphicsTextItem(nullptr); m_batteryTextItem->setPlainText("Battery"); m_batteryTextItem->setFont(f); - m_batteryTextItem->setPos(0,0); + m_batteryTextItem->setPos(0, 0); m_batteryTextItem->adjustSize(); m_batteryTextItem->setDefaultTextColor(Qt::red); m_batteryTextItem->show(); @@ -34,25 +33,27 @@ GraphicsViewToolbar::GraphicsViewToolbar(QWidget *parent) this->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); } -void GraphicsViewToolbar::resizeEvent(QResizeEvent *event) +void GraphicsViewToolbar::resizeEvent(QResizeEvent* event) { - //qDebug() << "resizeEvent" << event; + // qDebug() << "resizeEvent" << event; - m_scene->setSceneRect(0,0,event->size().width(), this->height()); - //resize(event->size()); + m_scene->setSceneRect(0, 0, event->size().width(), this->height()); + // resize(event->size()); QGraphicsView::resizeEvent(event); } -void GraphicsViewToolbar::setBatteryStatus(bool is_charging, float battery_voltage, - float battery_current, float battery_level) +void GraphicsViewToolbar::setBatteryStatus( + bool is_charging, + float battery_voltage, + float battery_current, + float battery_level) { m_batteryTextItem->setPlainText(QString("C: %1, %2 V, %3 A L:%4 %") - .arg(is_charging) - .arg(battery_voltage) - .arg(battery_current) - .arg(battery_level)); + .arg(is_charging) + .arg(battery_voltage) + .arg(battery_current) + .arg(battery_level)); m_batteryTextItem->adjustSize(); - -} \ No newline at end of file +} diff --git a/opentera_webrtc_robot_gui/src/GraphicsViewToolbar.h b/opentera_webrtc_robot_gui/src/GraphicsViewToolbar.h index 88e698a..b895fea 100644 --- a/opentera_webrtc_robot_gui/src/GraphicsViewToolbar.h +++ b/opentera_webrtc_robot_gui/src/GraphicsViewToolbar.h @@ -11,18 +11,16 @@ class GraphicsViewToolbar : public QGraphicsView { Q_OBJECT public: - GraphicsViewToolbar(QWidget *parent=nullptr); + GraphicsViewToolbar(QWidget* parent = nullptr); public slots: void setBatteryStatus(bool is_charging, float battery_voltage, float battery_current, float battery_level); private: + QGraphicsScene* m_scene; + QGraphicsTextItem* m_batteryTextItem; - QGraphicsScene *m_scene; - QGraphicsTextItem *m_batteryTextItem; - - virtual void resizeEvent(QResizeEvent *event) override; - + virtual void resizeEvent(QResizeEvent* event) override; }; diff --git a/opentera_webrtc_robot_gui/src/MainWindow.cpp b/opentera_webrtc_robot_gui/src/MainWindow.cpp index 45289ea..e3f828c 100644 --- a/opentera_webrtc_robot_gui/src/MainWindow.cpp +++ b/opentera_webrtc_robot_gui/src/MainWindow.cpp @@ -5,42 +5,39 @@ #include #include -MainWindow::MainWindow(QWidget *parent) - : QMainWindow(parent) - , m_ui(new Ui::MainWindow) +MainWindow::MainWindow(QWidget* parent) : QMainWindow(parent), m_ui(new Ui::MainWindow) { m_ui->setupUi(this); - //Buttons + // Buttons setupButtons(); - //Toolbar + // Toolbar m_toolbar = new GraphicsViewToolbar(m_ui->toolboxWidget); - //Create camera view + // Create camera view m_cameraView = new ROSCameraView("Local", m_ui->imageWidget); m_ui->imageWidgetLayout->addWidget(m_cameraView); - //Setup ROS + // Setup ROS setupROS(); - //Connect signals/slot + // Connect signals/slot connect(this, &MainWindow::newLocalImage, this, &MainWindow::_onLocalImage, Qt::QueuedConnection); connect(this, &MainWindow::newPeerImage, this, &MainWindow::_onPeerImage, Qt::QueuedConnection); connect(this, &MainWindow::newPeerStatus, this, &MainWindow::_onPeerStatus, Qt::QueuedConnection); connect(this, &MainWindow::newRobotStatus, this, &MainWindow::_onRobotStatus, Qt::QueuedConnection); - //Buttons + // Buttons connect(m_ui->configButton, &QPushButton::clicked, this, &MainWindow::_onConfigButtonClicked); // Signaling events connect(this, &MainWindow::eventJoinSession, this, &MainWindow::_onJoinSessionEvent, Qt::QueuedConnection); connect(this, &MainWindow::eventLeaveSession, this, &MainWindow::_onLeaveSessionEvent, Qt::QueuedConnection); connect(this, &MainWindow::eventStopSession, this, &MainWindow::_onStopSessionEvent, Qt::QueuedConnection); - } MainWindow::~MainWindow() @@ -50,153 +47,141 @@ MainWindow::~MainWindow() void MainWindow::setupROS() { - //Setup subscribers - m_localImageSubscriber = m_nodeHandle.subscribe("/front_camera/image_raw", - 10, - &MainWindow::localImageCallback, - this); - + // Setup subscribers + m_localImageSubscriber = + m_nodeHandle.subscribe("/front_camera/image_raw", 10, &MainWindow::localImageCallback, this); - m_peerImageSubscriber = m_nodeHandle.subscribe("/webrtc_image", - 10, - &MainWindow::peerImageCallback, - this); - m_peerStatusSubscriber = m_nodeHandle.subscribe("/webrtc_peer_status", - 10, - &MainWindow::peerStatusCallback, - this); + m_peerImageSubscriber = m_nodeHandle.subscribe("/webrtc_image", 10, &MainWindow::peerImageCallback, this); + m_peerStatusSubscriber = m_nodeHandle.subscribe("/webrtc_peer_status", 10, &MainWindow::peerStatusCallback, this); - m_openteraEventSubscriber = m_nodeHandle.subscribe("/events", - 10, - &MainWindow::openteraEventCallback, - this); - m_robotStatusSubscriber = m_nodeHandle.subscribe("/robot_status", - 10, - &MainWindow::robotStatusCallback, - this); + m_openteraEventSubscriber = m_nodeHandle.subscribe("/events", 10, &MainWindow::openteraEventCallback, this); - //Setup publishers + m_robotStatusSubscriber = m_nodeHandle.subscribe("/robot_status", 10, &MainWindow::robotStatusCallback, this); + // Setup publishers } void MainWindow::localImageCallback(const sensor_msgs::ImageConstPtr& msg) { - //WARNING THIS IS CALLED FROM ANOTHER THREAD (ROS SPINNER) - //qDebug() << "localImageCallback thread" << QThread::currentThread(); + // WARNING THIS IS CALLED FROM ANOTHER THREAD (ROS SPINNER) + // qDebug() << "localImageCallback thread" << QThread::currentThread(); - if (msg->encoding == "rgb8") { - //Step #1 Transform ROS Image to QtImage + if (msg->encoding == "rgb8") + { + // Step #1 Transform ROS Image to QtImage QImage image(&msg->data[0], msg->width, msg->height, QImage::Format_RGB888); - //Step #2 emit new signal with image + // Step #2 emit new signal with image emit newLocalImage(image.copy()); } - else if (msg->encoding == "bgr8") { - //Step #1 Transform ROS Image to QtImage + else if (msg->encoding == "bgr8") + { + // Step #1 Transform ROS Image to QtImage QImage image(&msg->data[0], msg->width, msg->height, QImage::Format_RGB888); - //Step #2 emit new signal with image - //Invert R & B here + // Step #2 emit new signal with image + // Invert R & B here emit newLocalImage(image.rgbSwapped()); } - else { + else + { qDebug() << "Unhandled image encoding: " << QString::fromStdString(msg->encoding); ROS_ERROR("Unhandled image encoding: %s", msg->encoding.c_str()); } - } -void MainWindow::openteraEventCallback(const opentera_webrtc_ros_msgs::OpenTeraEventConstPtr &msg) +void MainWindow::openteraEventCallback(const opentera_webrtc_ros_msgs::OpenTeraEventConstPtr& msg) { - //WARNING THIS IS CALLED FROM ANOTHER THREAD (ROS SPINNER) + // WARNING THIS IS CALLED FROM ANOTHER THREAD (ROS SPINNER) - //We are only interested in JoinSession, StopSession, LeaveSession events for now - for (auto i=0; i < msg->join_session_events.size(); i++) + // We are only interested in JoinSession, StopSession, LeaveSession events for now + for (auto i = 0; i < msg->join_session_events.size(); i++) { QList session_participants; - for (auto j=0; j < msg->join_session_events[i].session_participants.size(); j++) + for (auto j = 0; j < msg->join_session_events[i].session_participants.size(); j++) { session_participants.append(QString::fromStdString(msg->join_session_events[i].session_participants[j])); } QList session_users; - for (auto j=0; j < msg->join_session_events[i].session_users.size(); j++) + for (auto j = 0; j < msg->join_session_events[i].session_users.size(); j++) { session_users.append(QString::fromStdString(msg->join_session_events[i].session_users[j])); } QList session_devices; - for (auto j=0; j < msg->join_session_events[i].session_devices.size(); j++) + for (auto j = 0; j < msg->join_session_events[i].session_devices.size(); j++) { session_devices.append(QString::fromStdString(msg->join_session_events[i].session_devices[j])); } - emit eventJoinSession(QString::fromStdString(msg->join_session_events[i].session_url), - QString::fromStdString(msg->join_session_events[i].session_creator_name), - QString::fromStdString(msg->join_session_events[i].session_uuid), - session_participants, - session_users, - session_devices, - QString::fromStdString(msg->join_session_events[i].join_msg), - QString::fromStdString(msg->join_session_events[i].session_parameters), - QString::fromStdString(msg->join_session_events[i].service_uuid)); - + emit eventJoinSession( + QString::fromStdString(msg->join_session_events[i].session_url), + QString::fromStdString(msg->join_session_events[i].session_creator_name), + QString::fromStdString(msg->join_session_events[i].session_uuid), + session_participants, + session_users, + session_devices, + QString::fromStdString(msg->join_session_events[i].join_msg), + QString::fromStdString(msg->join_session_events[i].session_parameters), + QString::fromStdString(msg->join_session_events[i].service_uuid)); } - for (auto i=0; i < msg->leave_session_events.size(); i++) + for (auto i = 0; i < msg->leave_session_events.size(); i++) { QList leaving_participants; - for (auto j=0; j < msg->leave_session_events[i].leaving_participants.size(); j++) + for (auto j = 0; j < msg->leave_session_events[i].leaving_participants.size(); j++) { leaving_participants.append(QString::fromStdString(msg->leave_session_events[i].leaving_participants[j])); } QList leaving_users; - for (auto j=0; j < msg->leave_session_events[i].leaving_users.size(); j++) + for (auto j = 0; j < msg->leave_session_events[i].leaving_users.size(); j++) { leaving_users.append(QString::fromStdString(msg->leave_session_events[i].leaving_users[j])); } QList leaving_devices; - for (auto j=0; j < msg->leave_session_events[i].leaving_devices.size(); j++) + for (auto j = 0; j < msg->leave_session_events[i].leaving_devices.size(); j++) { leaving_devices.append(QString::fromStdString(msg->leave_session_events[i].leaving_devices[j])); } - emit eventLeaveSession(QString::fromStdString(msg->leave_session_events[i].session_uuid), - QString::fromStdString(msg->leave_session_events[i].service_uuid), - leaving_participants, - leaving_users, - leaving_devices); + emit eventLeaveSession( + QString::fromStdString(msg->leave_session_events[i].session_uuid), + QString::fromStdString(msg->leave_session_events[i].service_uuid), + leaving_participants, + leaving_users, + leaving_devices); } - for (auto i=0; i < msg->stop_session_events.size(); i++) + for (auto i = 0; i < msg->stop_session_events.size(); i++) { - emit eventStopSession(QString::fromStdString(msg->stop_session_events[i].session_uuid), + emit eventStopSession( + QString::fromStdString(msg->stop_session_events[i].session_uuid), QString::fromStdString(msg->stop_session_events[i].service_uuid)); } - } void MainWindow::_onLocalImage(const QImage& image) { - //qDebug() << "_onLocalImage Current Thread " << QThread::currentThread(); + // qDebug() << "_onLocalImage Current Thread " << QThread::currentThread(); m_cameraView->setImage(image); } -void MainWindow::_onPeerImage(const QString &id, const QString &name, const QImage& image) +void MainWindow::_onPeerImage(const QString& id, const QString& name, const QImage& image) { if (!m_remoteViews.contains(id)) { - ROSCameraView *camera = new ROSCameraView(name, nullptr); + ROSCameraView* camera = new ROSCameraView(name, nullptr); camera->setImage(image); m_ui->imageWidgetLayout->addWidget(camera); m_remoteViews[id] = camera; - m_cameraView->setMaximumSize(320,240); + m_cameraView->setMaximumSize(320, 240); } else { @@ -205,121 +190,145 @@ void MainWindow::_onPeerImage(const QString &id, const QString &name, const QIma } } -void MainWindow::_onPeerStatus(const QString &id, const QString& name, int status) +void MainWindow::_onPeerStatus(const QString& id, const QString& name, int status) { switch (status) { case opentera_webrtc_ros_msgs::PeerStatus::STATUS_CLIENT_CONNECTED: - break; + break; case opentera_webrtc_ros_msgs::PeerStatus::STATUS_CLIENT_DISCONNECTED: - if (m_remoteViews.contains(id)) - { - m_remoteViews[id]->deleteLater(); - m_remoteViews.remove(id); - - if (m_remoteViews.empty()) + if (m_remoteViews.contains(id)) { - //Put back full size self camera - m_cameraView->setMaximumSize(QWIDGETSIZE_MAX,QWIDGETSIZE_MAX); + m_remoteViews[id]->deleteLater(); + m_remoteViews.remove(id); + + if (m_remoteViews.empty()) + { + // Put back full size self camera + m_cameraView->setMaximumSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX); + } } - } - break; + break; case opentera_webrtc_ros_msgs::PeerStatus::STATUS_REMOTE_STREAM_ADDED: - break; + break; case opentera_webrtc_ros_msgs::PeerStatus::STATUS_REMOTE_STREAM_REMOVED: - break; + break; default: qWarning() << "Status not handled " << status; ROS_WARN("Status not handled : %i", status); - break; + break; } } -void MainWindow::setImage(const QImage &image) +void MainWindow::setImage(const QImage& image) { m_cameraView->setImage(image); } -void MainWindow::closeEvent(QCloseEvent *event) +void MainWindow::closeEvent(QCloseEvent* event) { QMainWindow::closeEvent(event); QApplication::quit(); } -void MainWindow::peerImageCallback(const opentera_webrtc_ros_msgs::PeerImageConstPtr &msg) +void MainWindow::peerImageCallback(const opentera_webrtc_ros_msgs::PeerImageConstPtr& msg) { - //Step #1 Transform ROS Image to QtImage + // Step #1 Transform ROS Image to QtImage QImage image(&msg->frame.data[0], msg->frame.width, msg->frame.height, QImage::Format_RGB888); - //Step #2 Emit signal (will be handled in Qt main thread) - //Image will be automatically deleted when required - //Invert R & B here - emit newPeerImage(QString::fromStdString(msg->sender.id), QString::fromStdString(msg->sender.name), image.rgbSwapped()); + // Step #2 Emit signal (will be handled in Qt main thread) + // Image will be automatically deleted when required + // Invert R & B here + emit newPeerImage( + QString::fromStdString(msg->sender.id), + QString::fromStdString(msg->sender.name), + image.rgbSwapped()); } -void MainWindow::peerStatusCallback(const opentera_webrtc_ros_msgs::PeerStatusConstPtr &msg) +void MainWindow::peerStatusCallback(const opentera_webrtc_ros_msgs::PeerStatusConstPtr& msg) { emit newPeerStatus(QString::fromStdString(msg->sender.id), QString::fromStdString(msg->sender.name), msg->status); } -void MainWindow::robotStatusCallback(const opentera_webrtc_ros_msgs::RobotStatusConstPtr &msg) +void MainWindow::robotStatusCallback(const opentera_webrtc_ros_msgs::RobotStatusConstPtr& msg) { /* void newRobotStatus(bool charging, float battery_voltage, float battery_current, float battery_level, float cpu_usage, float mem_usage, float disk_usage, const QString &wifi_network, float wifi_strength, const QString &local_ip); */ - emit newRobotStatus(msg->is_charging, msg->battery_voltage, msg->battery_current, msg->battery_level, - msg->cpu_usage, msg->mem_usage, msg->disk_usage, QString::fromStdString(msg->wifi_network), - msg->wifi_strength, QString::fromStdString(msg->local_ip), msg->is_muted, msg->is_camera_on); + emit newRobotStatus( + msg->is_charging, + msg->battery_voltage, + msg->battery_current, + msg->battery_level, + msg->cpu_usage, + msg->mem_usage, + msg->disk_usage, + QString::fromStdString(msg->wifi_network), + msg->wifi_strength, + QString::fromStdString(msg->local_ip), + msg->is_muted, + msg->is_camera_on); } -void MainWindow::_onJoinSessionEvent(const QString &session_url, - const QString &session_creator_name, - const QString &session_uuid, +void MainWindow::_onJoinSessionEvent( + const QString& session_url, + const QString& session_creator_name, + const QString& session_uuid, QList session_participants, QList session_users, QList session_devices, - const QString &join_msg, - const QString &session_parameters, - const QString &service_uuid) + const QString& join_msg, + const QString& session_parameters, + const QString& service_uuid) { - } -void MainWindow::_onStopSessionEvent(const QString &session_uuid, const QString &service_uuid) +void MainWindow::_onStopSessionEvent(const QString& session_uuid, const QString& service_uuid) { qDebug() << "_onStopSessionEvent(const QString &session_uuid, const QString &service_uuid)"; ROS_DEBUG("_onStopSessionEvent(const QString &session_uuid, const QString &service_uuid)"); - //Remove all remote views - foreach (QString key, m_remoteViews.keys()) { + // Remove all remote views + foreach (QString key, m_remoteViews.keys()) + { m_remoteViews[key]->deleteLater(); } m_remoteViews.clear(); - //Put back full size self camera - m_cameraView->setMaximumSize(QWIDGETSIZE_MAX,QWIDGETSIZE_MAX); + // Put back full size self camera + m_cameraView->setMaximumSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX); } -void MainWindow::_onLeaveSessionEvent(const QString &session_uuid, - const QString &service_uuid, +void MainWindow::_onLeaveSessionEvent( + const QString& session_uuid, + const QString& service_uuid, QList leaving_participants, QList leaving_users, QList leaving_devices) { - } -void MainWindow::_onRobotStatus(bool is_charging, float battery_voltage, float battery_current, float battery_level, - float cpu_usage, float mem_usage, float disk_usage, const QString &wifi_network, - float wifi_strength, const QString &local_ip, bool is_muted, bool is_camera_on) +void MainWindow::_onRobotStatus( + bool is_charging, + float battery_voltage, + float battery_current, + float battery_level, + float cpu_usage, + float mem_usage, + float disk_usage, + const QString& wifi_network, + float wifi_strength, + const QString& local_ip, + bool is_muted, + bool is_camera_on) { m_toolbar->setBatteryStatus(is_charging, battery_voltage, battery_current, battery_level); } @@ -340,11 +349,10 @@ void MainWindow::setupButtons() m_ui->speakerButton->setIcon(QIcon(":/volume.png")); m_ui->speakerButton->setText(""); - } void MainWindow::_onConfigButtonClicked() { ConfigDialog dialog(this); dialog.exec(); -} \ No newline at end of file +} diff --git a/opentera_webrtc_robot_gui/src/MainWindow.h b/opentera_webrtc_robot_gui/src/MainWindow.h index 897bd0d..957374b 100644 --- a/opentera_webrtc_robot_gui/src/MainWindow.h +++ b/opentera_webrtc_robot_gui/src/MainWindow.h @@ -15,7 +15,10 @@ #include QT_BEGIN_NAMESPACE -namespace Ui { class MainWindow; } + namespace Ui + { + class MainWindow; + } QT_END_NAMESPACE class MainWindow : public QMainWindow @@ -23,101 +26,121 @@ class MainWindow : public QMainWindow Q_OBJECT public: - MainWindow(QWidget *parent = nullptr); + MainWindow(QWidget* parent = nullptr); ~MainWindow(); - void setImage(const QImage &image); + void setImage(const QImage& image); signals: void newLocalImage(const QImage& image); - void newPeerImage(const QString &id, const QString &name, const QImage &image); - void newPeerStatus(const QString &id, const QString &name, int status); - void newRobotStatus(bool is_charging, float battery_voltage, float battery_current, float battery_level, - float cpu_usage, float mem_usage, float disk_usage, const QString &wifi_network, - float wifi_strength, const QString &local_ip, bool is_muted, bool is_camera_on); - void eventJoinSession(const QString &session_url, - const QString &session_creator_name, - const QString &session_uuid, - QList session_participants, - QList session_users, - QList session_devices, - const QString &join_msg, - const QString &session_parameters, - const QString &service_uuid); - - void eventStopSession(const QString &session_uuid, const QString &service_uuid); - - void eventLeaveSession(const QString &session_uuid, - const QString &service_uuid, - QList leaving_participants, - QList leaving_users, - QList leaving_devices); + void newPeerImage(const QString& id, const QString& name, const QImage& image); + void newPeerStatus(const QString& id, const QString& name, int status); + void newRobotStatus( + bool is_charging, + float battery_voltage, + float battery_current, + float battery_level, + float cpu_usage, + float mem_usage, + float disk_usage, + const QString& wifi_network, + float wifi_strength, + const QString& local_ip, + bool is_muted, + bool is_camera_on); + void eventJoinSession( + const QString& session_url, + const QString& session_creator_name, + const QString& session_uuid, + QList session_participants, + QList session_users, + QList session_devices, + const QString& join_msg, + const QString& session_parameters, + const QString& service_uuid); + + void eventStopSession(const QString& session_uuid, const QString& service_uuid); + + void eventLeaveSession( + const QString& session_uuid, + const QString& service_uuid, + QList leaving_participants, + QList leaving_users, + QList leaving_devices); private slots: void _onLocalImage(const QImage& image); void _onPeerImage(const QString& id, const QString& name, const QImage& image); - void _onPeerStatus(const QString &id, const QString& name, int status); - - void _onJoinSessionEvent(const QString &session_url, - const QString &session_creator_name, - const QString &session_uuid, - QList session_participants, - QList session_users, - QList session_devices, - const QString &join_msg, - const QString &session_parameters, - const QString &service_uuid); - - void _onStopSessionEvent(const QString &session_uuid, const QString &service_uuid); - - void _onLeaveSessionEvent(const QString &session_uuid, - const QString &service_uuid, - QList leaving_participants, - QList leaving_users, - QList leaving_devices); - - void _onRobotStatus(bool is_charging, float battery_voltage, float battery_current, float battery_level, - float cpu_usage, float mem_usage, float disk_usage, const QString &wif_network, - float wifi_strength, const QString &local_ip, bool is_muted, bool is_camera_on); + void _onPeerStatus(const QString& id, const QString& name, int status); + + void _onJoinSessionEvent( + const QString& session_url, + const QString& session_creator_name, + const QString& session_uuid, + QList session_participants, + QList session_users, + QList session_devices, + const QString& join_msg, + const QString& session_parameters, + const QString& service_uuid); + + void _onStopSessionEvent(const QString& session_uuid, const QString& service_uuid); + + void _onLeaveSessionEvent( + const QString& session_uuid, + const QString& service_uuid, + QList leaving_participants, + QList leaving_users, + QList leaving_devices); + + void _onRobotStatus( + bool is_charging, + float battery_voltage, + float battery_current, + float battery_level, + float cpu_usage, + float mem_usage, + float disk_usage, + const QString& wif_network, + float wifi_strength, + const QString& local_ip, + bool is_muted, + bool is_camera_on); void _onConfigButtonClicked(); private: - void setupROS(); void setupButtons(); - void closeEvent(QCloseEvent *event) override; + void closeEvent(QCloseEvent* event) override; - Ui::MainWindow *m_ui; + Ui::MainWindow* m_ui; - //Toolbar - GraphicsViewToolbar *m_toolbar; + // Toolbar + GraphicsViewToolbar* m_toolbar; - //Remote views + // Remote views QMap m_remoteViews; - //Main View - ROSCameraView *m_cameraView; + // Main View + ROSCameraView* m_cameraView; - //ROS + // ROS - //ROS Callbacks + // ROS Callbacks void localImageCallback(const sensor_msgs::ImageConstPtr& msg); - void peerImageCallback(const opentera_webrtc_ros_msgs::PeerImageConstPtr &msg); - void peerStatusCallback(const opentera_webrtc_ros_msgs::PeerStatusConstPtr &msg); - void openteraEventCallback(const opentera_webrtc_ros_msgs::OpenTeraEventConstPtr &msg); - void robotStatusCallback(const opentera_webrtc_ros_msgs::RobotStatusConstPtr &msg); + void peerImageCallback(const opentera_webrtc_ros_msgs::PeerImageConstPtr& msg); + void peerStatusCallback(const opentera_webrtc_ros_msgs::PeerStatusConstPtr& msg); + void openteraEventCallback(const opentera_webrtc_ros_msgs::OpenTeraEventConstPtr& msg); + void robotStatusCallback(const opentera_webrtc_ros_msgs::RobotStatusConstPtr& msg); ros::NodeHandle m_nodeHandle; - ros::Subscriber m_peerImageSubscriber; + ros::Subscriber m_peerImageSubscriber; ros::Subscriber m_localImageSubscriber; ros::Subscriber m_peerStatusSubscriber; ros::Subscriber m_openteraEventSubscriber; ros::Subscriber m_robotStatusSubscriber; - - - }; -#endif // MAINWINDOW_H +#endif // MAINWINDOW_H diff --git a/opentera_webrtc_robot_gui/src/ROSCameraView.cpp b/opentera_webrtc_robot_gui/src/ROSCameraView.cpp index e5fe88e..91f8b11 100644 --- a/opentera_webrtc_robot_gui/src/ROSCameraView.cpp +++ b/opentera_webrtc_robot_gui/src/ROSCameraView.cpp @@ -4,80 +4,80 @@ #include #include -GLCameraWidget::GLCameraWidget(QWidget *parent) - : QGLWidget(parent) +GLCameraWidget::GLCameraWidget(QWidget* parent) : QGLWidget(parent) { - //resize(640,480); + // resize(640,480); } -void GLCameraWidget::setImage(const QImage &image) +void GLCameraWidget::setImage(const QImage& image) { - //Make sure we copy the image (could be deleted somewhere else) + // Make sure we copy the image (could be deleted somewhere else) m_image = image.copy(); update(); } -void GLCameraWidget::paintEvent(QPaintEvent *event) +void GLCameraWidget::paintEvent(QPaintEvent* event) { QPainter p(this); - //Set the painter to use a smooth scaling algorithm. + // Set the painter to use a smooth scaling algorithm. p.setRenderHint(QPainter::SmoothPixmapTransform, 1); - //Draw Black Background + // Draw Black Background p.fillRect(this->rect(), QBrush(Qt::black)); if (m_image.width() > 0 && m_image.height() > 0) { - //Find minimal scale - float scale = std::min((float) this->width() / m_image.width(), (float) this->height() / m_image.height()); + // Find minimal scale + float scale = std::min((float)this->width() / m_image.width(), (float)this->height() / m_image.height()); float new_width = scale * m_image.width(); float new_height = scale * m_image.height(); int offset_x = (rect().width() - new_width) / 2; int offset_y = (rect().height() - new_height) / 2; - //Draw image - QRect drawingRect(std::max(0,offset_x),std::max(0,offset_y),new_width, new_height); + // Draw image + QRect drawingRect(std::max(0, offset_x), std::max(0, offset_y), new_width, new_height); - //Paint in current rect + // Paint in current rect p.drawImage(drawingRect, m_image); - //This will strech the image... - //p.drawImage(this->rect(), m_image); + // This will strech the image... + // p.drawImage(this->rect(), m_image); } } -ROSCameraView::ROSCameraView(QWidget *parent) - : QWidget(parent), m_layout(nullptr), m_label(nullptr), m_cameraWidget(nullptr) +ROSCameraView::ROSCameraView(QWidget* parent) + : QWidget(parent), + m_layout(nullptr), + m_label(nullptr), + m_cameraWidget(nullptr) { m_layout = new QVBoxLayout(this); - //Label + // Label m_label = new QLabel(this); m_label->setAlignment(Qt::AlignHCenter | Qt::AlignVCenter); m_label->setMaximumHeight(25); m_layout->addWidget(m_label); - //CameraWidget + // CameraWidget m_cameraWidget = new GLCameraWidget(this); m_layout->addWidget(m_cameraWidget); - } -ROSCameraView::ROSCameraView(const QString &label, QWidget *parent) - : ROSCameraView(parent) +ROSCameraView::ROSCameraView(const QString& label, QWidget* parent) : ROSCameraView(parent) { m_label->setText(label); } -void ROSCameraView::setText(const QString &text) +void ROSCameraView::setText(const QString& text) { if (m_label) m_label->setText(text); } -void ROSCameraView::setImage(const QImage &image) +void ROSCameraView::setImage(const QImage& image) { if (m_cameraWidget) m_cameraWidget->setImage(image); diff --git a/opentera_webrtc_robot_gui/src/ROSCameraView.h b/opentera_webrtc_robot_gui/src/ROSCameraView.h index cded89c..ac56ea3 100644 --- a/opentera_webrtc_robot_gui/src/ROSCameraView.h +++ b/opentera_webrtc_robot_gui/src/ROSCameraView.h @@ -13,18 +13,16 @@ class GLCameraWidget : public QGLWidget Q_OBJECT public: - GLCameraWidget(QWidget *parent = nullptr); + GLCameraWidget(QWidget* parent = nullptr); - void setImage(const QImage &image); + void setImage(const QImage& image); protected: - void paintEvent(QPaintEvent* event) override; private: QImage m_image; - }; @@ -33,19 +31,18 @@ class ROSCameraView : public QWidget Q_OBJECT public: - ROSCameraView(QWidget *parent = nullptr); - ROSCameraView(const QString &label, QWidget *parent = nullptr); + ROSCameraView(QWidget* parent = nullptr); + ROSCameraView(const QString& label, QWidget* parent = nullptr); public slots: - void setText(const QString &text); - void setImage(const QImage &image); + void setText(const QString& text); + void setImage(const QImage& image); private: - QVBoxLayout *m_layout; - GLCameraWidget *m_cameraWidget; - QLabel *m_label; + QVBoxLayout* m_layout; + GLCameraWidget* m_cameraWidget; + QLabel* m_label; }; - #endif diff --git a/opentera_webrtc_robot_gui/src/main.cpp b/opentera_webrtc_robot_gui/src/main.cpp index 14fc471..69f4fd4 100644 --- a/opentera_webrtc_robot_gui/src/main.cpp +++ b/opentera_webrtc_robot_gui/src/main.cpp @@ -12,30 +12,29 @@ // SIGINT handler, will quit Qt event loop void termination_handler(int signum) { - QApplication::quit(); + QApplication::quit(); } -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { - ros::init(argc, argv, "opentera_webrtc_robot_gui_node"); ros::NodeHandle nh("~"); bool fullScreen = false; nh.getParam("fullScreen", fullScreen); - ros::AsyncSpinner spinner(1); + ros::AsyncSpinner spinner(1); /* Set up the structure to specify the action */ struct sigaction action; - action.sa_handler = termination_handler; - sigemptyset(&action.sa_mask); - action.sa_flags = 0; - sigaction(SIGINT, &action, NULL); + action.sa_handler = termination_handler; + sigemptyset(&action.sa_mask); + action.sa_flags = 0; + sigaction(SIGINT, &action, NULL); QApplication app(argc, argv); - //Stylesheet + // Stylesheet QFile file(":/stylesheet.qss"); file.open(QFile::ReadOnly); QString stylesheet = QLatin1String(file.readAll()); @@ -52,19 +51,19 @@ int main(int argc, char *argv[]) w.show(); } - //Load test image from QRC + // Load test image from QRC QImage testImage(":/Test_640x480.png"); qDebug() << testImage; w.setImage(testImage); - //Will start ROS loop in background + // Will start ROS loop in background spinner.start(); - //will run app event loop (infinite) - //qDebug() << "MainThread " << QThread::currentThread(); + // will run app event loop (infinite) + // qDebug() << "MainThread " << QThread::currentThread(); app.exec(); - //Stop ROS loop + // Stop ROS loop spinner.stop(); } diff --git a/opentera_webrtc_robot_gui/src/resources/opentera_webrtc_robot_gui.qrc b/opentera_webrtc_robot_gui/src/resources/opentera_webrtc_robot_gui.qrc index 0174f51..d31785a 100644 --- a/opentera_webrtc_robot_gui/src/resources/opentera_webrtc_robot_gui.qrc +++ b/opentera_webrtc_robot_gui/src/resources/opentera_webrtc_robot_gui.qrc @@ -10,4 +10,4 @@ volume-mute.png webcam.png - \ No newline at end of file + diff --git a/opentera_webrtc_robot_gui/src/resources/stylesheet.qss b/opentera_webrtc_robot_gui/src/resources/stylesheet.qss index dd0b821..7bf3317 100644 --- a/opentera_webrtc_robot_gui/src/resources/stylesheet.qss +++ b/opentera_webrtc_robot_gui/src/resources/stylesheet.qss @@ -1,308 +1,308 @@ /* - Copyright 2013 Emanuel Claesson + Copyright 2013 Emanuel Claesson - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at - http://www.apache.org/licenses/LICENSE-2.0 + http://www.apache.org/licenses/LICENSE-2.0 - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. */ /* - COLOR_DARK = #191919 - COLOR_MEDIUM = #353535f - COLOR_MEDLIGHT = #5A5A5A - COLOR_LIGHT = #DDDDDD - COLOR_ACCENT = #3D7848 + COLOR_DARK = #191919 + COLOR_MEDIUM = #353535f + COLOR_MEDLIGHT = #5A5A5A + COLOR_LIGHT = #DDDDDD + COLOR_ACCENT = #3D7848 */ * { - background: #191919; - color: #DDDDDD; - border: 0px solid #5A5A5A; + background: #191919; + color: #DDDDDD; + border: 0px solid #5A5A5A; padding: 0px 0px 0px 0px; margin: 0px 0px 0px 0px; } QDialog { - opacity: 1.000; + opacity: 1.000; } QPushButton { - color: #5A5A5A; - background-color: darkgray; - min-height: 50px; - min-width: 50px; - border-radius: 10px; - padding: 2px 2px 2px 2px; - margin: 2px 2px 2px 2px; + color: #5A5A5A; + background-color: darkgray; + min-height: 50px; + min-width: 50px; + border-radius: 10px; + padding: 2px 2px 2px 2px; + margin: 2px 2px 2px 2px; } QDialogButtonBox::close-button { - min-width: 200px; + min-width: 200px; } QGroupBox { - padding: 5px 0px 5px 0px; + padding: 5px 0px 5px 0px; margin: 0px 0px 0px 0px; - background: #3f3d3d + background: #3f3d3d } QWidget::item:selected { - background: #3D7848; + background: #3D7848; } QCheckBox, QRadioButton { - border: none; + border: none; } QRadioButton::indicator, QCheckBox::indicator { - width: 13px; - height: 13px; + width: 13px; + height: 13px; } QRadioButton::indicator::unchecked, QCheckBox::indicator::unchecked { - border: 1px solid #5A5A5A; - background: none; + border: 1px solid #5A5A5A; + background: none; } QRadioButton::indicator:unchecked:hover, QCheckBox::indicator:unchecked:hover { - border: 1px solid #DDDDDD; + border: 1px solid #DDDDDD; } QRadioButton::indicator::checked, QCheckBox::indicator::checked { - border: 1px solid #5A5A5A; - background: #5A5A5A; + border: 1px solid #5A5A5A; + background: #5A5A5A; } QRadioButton::indicator:checked:hover, QCheckBox::indicator:checked:hover { - border: 1px solid #DDDDDD; - background: #DDDDDD; + border: 1px solid #DDDDDD; + background: #DDDDDD; } QGroupBox { - margin-top: 6px; + margin-top: 6px; } QGroupBox::title { - top: -7px; - left: 7px; + top: -7px; + left: 7px; } QScrollBar { - border: 1px solid #5A5A5A; - background: #191919; + border: 1px solid #5A5A5A; + background: #191919; } QScrollBar:horizontal { - height: 15px; - margin: 0px 0px 0px 32px; + height: 15px; + margin: 0px 0px 0px 32px; } QScrollBar:vertical { - width: 15px; - margin: 32px 0px 0px 0px; + width: 15px; + margin: 32px 0px 0px 0px; } QScrollBar::handle { - background: #353535; - border: 1px solid #5A5A5A; + background: #353535; + border: 1px solid #5A5A5A; } QScrollBar::handle:horizontal { - border-width: 0px 1px 0px 1px; + border-width: 0px 1px 0px 1px; } QScrollBar::handle:vertical { - border-width: 1px 0px 1px 0px; + border-width: 1px 0px 1px 0px; } QScrollBar::handle:horizontal { - min-width: 20px; + min-width: 20px; } QScrollBar::handle:vertical { - min-height: 20px; + min-height: 20px; } QScrollBar::add-line, QScrollBar::sub-line { - background:#353535; - border: 1px solid #5A5A5A; - subcontrol-origin: margin; + background:#353535; + border: 1px solid #5A5A5A; + subcontrol-origin: margin; } QScrollBar::add-line { - position: absolute; + position: absolute; } QScrollBar::add-line:horizontal { - width: 15px; - subcontrol-position: left; - left: 15px; + width: 15px; + subcontrol-position: left; + left: 15px; } QScrollBar::add-line:vertical { - height: 15px; - subcontrol-position: top; - top: 15px; + height: 15px; + subcontrol-position: top; + top: 15px; } QScrollBar::sub-line:horizontal { - width: 15px; - subcontrol-position: top left; + width: 15px; + subcontrol-position: top left; } QScrollBar::sub-line:vertical { - height: 15px; - subcontrol-position: top; + height: 15px; + subcontrol-position: top; } QScrollBar:left-arrow, QScrollBar::right-arrow, QScrollBar::up-arrow, QScrollBar::down-arrow { - border: 1px solid #5A5A5A; - width: 3px; - height: 3px; + border: 1px solid #5A5A5A; + width: 3px; + height: 3px; } QScrollBar::add-page, QScrollBar::sub-page { - background: none; + background: none; } QAbstractButton:hover { - background: #353535; + background: #353535; } QAbstractButton:pressed { - background: #5A5A5A; + background: #5A5A5A; } QAbstractItemView { - show-decoration-selected: 1; - selection-background-color: #3D7848; - selection-color: #DDDDDD; - alternate-background-color: #353535; + show-decoration-selected: 1; + selection-background-color: #3D7848; + selection-color: #DDDDDD; + alternate-background-color: #353535; } QHeaderView { - border: 1px solid #5A5A5A; + border: 1px solid #5A5A5A; } QHeaderView::section { - background: #191919; - border: 1px solid #5A5A5A; - padding: 4px; + background: #191919; + border: 1px solid #5A5A5A; + padding: 4px; } QHeaderView::section:selected, QHeaderView::section::checked { - background: #353535; + background: #353535; } QTableView { - gridline-color: #5A5A5A; + gridline-color: #5A5A5A; } QTabBar { - margin-left: 2px; + margin-left: 2px; } QTabBar::tab { - border-radius: 0px; - padding: 4px; - margin: 4px; + border-radius: 0px; + padding: 4px; + margin: 4px; } QTabBar::tab:selected { - background: #353535; + background: #353535; } QComboBox::down-arrow { - border: 1px solid #5A5A5A; - background: #353535; + border: 1px solid #5A5A5A; + background: #353535; } QComboBox::drop-down { - border: 1px solid #5A5A5A; - background: #353535; + border: 1px solid #5A5A5A; + background: #353535; } QComboBox::down-arrow { - width: 3px; - height: 3px; - border: 1px solid #5A5A5A; + width: 3px; + height: 3px; + border: 1px solid #5A5A5A; } QAbstractSpinBox { - padding-right: 15px; + padding-right: 15px; } QAbstractSpinBox::up-button, QAbstractSpinBox::down-button { - border: 1px solid #5A5A5A; - background: #353535; - subcontrol-origin: border; + border: 1px solid #5A5A5A; + background: #353535; + subcontrol-origin: border; } QAbstractSpinBox::up-arrow, QAbstractSpinBox::down-arrow { - width: 3px; - height: 3px; - border: 1px solid #5A5A5A; + width: 3px; + height: 3px; + border: 1px solid #5A5A5A; } QSlider { - border: none; + border: none; } QSlider::groove:horizontal { - height: 5px; - margin: 4px 0px 4px 0px; + height: 5px; + margin: 4px 0px 4px 0px; } QSlider::groove:vertical { - width: 5px; - margin: 0px 4px 0px 4px; + width: 5px; + margin: 0px 4px 0px 4px; } QSlider::handle { - border: 1px solid #5A5A5A; - background: #353535; + border: 1px solid #5A5A5A; + background: #353535; } QSlider::handle:horizontal { - width: 15px; - margin: -4px 0px -4px 0px; + width: 15px; + margin: -4px 0px -4px 0px; } QSlider::handle:vertical { - height: 15px; - margin: 0px -4px 0px -4px; + height: 15px; + margin: 0px -4px 0px -4px; } QSlider::add-page:vertical, QSlider::sub-page:horizontal { - background: #3D7848; + background: #3D7848; } QSlider::sub-page:vertical, QSlider::add-page:horizontal { - background: #353535; + background: #353535; } QLabel { - border: 1px; + border: 1px; } QProgressBar { - text-align: center; + text-align: center; } QProgressBar::chunk { - width: 1px; - background-color: #3D7848; + width: 1px; + background-color: #3D7848; } QMenu::separator { - background: #353535; -} \ No newline at end of file + background: #353535; +} diff --git a/opentera_webrtc_ros/CMakeLists.txt b/opentera_webrtc_ros/CMakeLists.txt index 25f2f54..f94065f 100644 --- a/opentera_webrtc_ros/CMakeLists.txt +++ b/opentera_webrtc_ros/CMakeLists.txt @@ -9,25 +9,25 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_FLAGS "-fPIC") if(NOT CMAKE_BUILD_TYPE ) - MESSAGE(STATUS "not define build type, set to release" ) - set(CMAKE_BUILD_TYPE Release ) + MESSAGE(STATUS "not define build type, set to release" ) + set(CMAKE_BUILD_TYPE Release ) elseif(NOT (${CMAKE_BUILD_TYPE} STREQUAL "Release" OR ${CMAKE_BUILD_TYPE} STREQUAL "Debug" )) - message(FATAL_ERROR "CMAKE_BUILD_TYPE must be either Release or Debug") + message(FATAL_ERROR "CMAKE_BUILD_TYPE must be either Release or Debug") endif() ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - cv_bridge - message_generation - std_msgs - sensor_msgs - audio_utils - geometry_msgs - opentera_webrtc_ros_msgs - std_srvs - visualization_msgs + roscpp + rospy + cv_bridge + message_generation + std_msgs + sensor_msgs + audio_utils + geometry_msgs + opentera_webrtc_ros_msgs + std_srvs + visualization_msgs ) ## Uncomment this if the package has a setup.py. This macro ensures @@ -89,15 +89,15 @@ link_directories( ${OPENTERA_WEBRTC_NATIVE_LIB_DIR}/boringssl_asm ) -set (opentera_webrtc_native_client_libs - opencv_core - opencv_imgproc - OpenteraWebrtcNativeClient - boringssl - webrtc - dl - X11 - expat +set(opentera_webrtc_native_client_libs + opencv_core + opencv_imgproc + OpenteraWebrtcNativeClient + boringssl + webrtc + dl + X11 + expat ) ########### @@ -106,31 +106,31 @@ set (opentera_webrtc_native_client_libs ## Specify additional locations of header files include_directories( - include - ${catkin_INCLUDE_DIRS} - lib + include + ${catkin_INCLUDE_DIRS} + lib ) ## Declare a C++ executable add_executable(${PROJECT_NAME}_topic_streamer - src/RosVideoSource.cpp - src/RosAudioSource.cpp - src/RosSignalingServerConfiguration.cpp - src/RosStreamBridge.cpp - src/RosNodeParameters.cpp - src/RosParamUtils.cpp + src/RosVideoSource.cpp + src/RosAudioSource.cpp + src/RosSignalingServerConfiguration.cpp + src/RosStreamBridge.cpp + src/RosNodeParameters.cpp + src/RosParamUtils.cpp ) add_executable(${PROJECT_NAME}_data_channel_bridge - src/RosSignalingServerConfiguration.cpp - src/RosDataChannelBridge.cpp - src/RosNodeParameters.cpp - src/RosParamUtils.cpp + src/RosSignalingServerConfiguration.cpp + src/RosDataChannelBridge.cpp + src/RosNodeParameters.cpp + src/RosParamUtils.cpp ) add_executable(${PROJECT_NAME}_json_data_handler - src/RosJsonDataHandler.cpp + src/RosJsonDataHandler.cpp ) ## Rename C++ executable without prefix @@ -140,18 +140,18 @@ set_target_properties(${PROJECT_NAME}_json_data_handler PROPERTIES OUTPUT_NAME j ## Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME}_topic_streamer - ${opentera_webrtc_native_client_libs} - ${catkin_LIBRARIES} + ${opentera_webrtc_native_client_libs} + ${catkin_LIBRARIES} ) target_link_libraries(${PROJECT_NAME}_data_channel_bridge - ${opentera_webrtc_native_client_libs} - ${catkin_LIBRARIES} + ${opentera_webrtc_native_client_libs} + ${catkin_LIBRARIES} ) target_link_libraries(${PROJECT_NAME}_json_data_handler - ${opentera_webrtc_native_client_libs} - ${catkin_LIBRARIES} + ${opentera_webrtc_native_client_libs} + ${catkin_LIBRARIES} ) ## ROS message dependencies @@ -164,17 +164,19 @@ add_dependencies(${PROJECT_NAME}_json_data_handler ${catkin_EXPORTED_TARGETS}) ## Install ## ############# -install(PROGRAMS - scripts/opentera_webrtc_audio_mixer.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install( + PROGRAMS + scripts/opentera_webrtc_audio_mixer.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) # all install targets should use catkin DESTINATION variables ## Mark executables for installation install( - TARGETS - ${PROJECT_NAME}_topic_streamer - ${PROJECT_NAME}_data_channel_bridge - ${PROJECT_NAME}_json_data_handler - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + TARGETS + ${PROJECT_NAME}_topic_streamer + ${PROJECT_NAME}_data_channel_bridge + ${PROJECT_NAME}_json_data_handler + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/opentera_webrtc_ros/include/RosAudioSource.h b/opentera_webrtc_ros/include/RosAudioSource.h index a3a2c4e..30ec119 100644 --- a/opentera_webrtc_ros/include/RosAudioSource.h +++ b/opentera_webrtc_ros/include/RosAudioSource.h @@ -14,9 +14,9 @@ namespace opentera */ class RosAudioSource : public AudioSource { - public: - RosAudioSource(unsigned int soundCardTotalDelayMs = 40, + RosAudioSource( + unsigned int soundCardTotalDelayMs = 40, bool echoCancellation = true, bool autoGainControl = true, bool noiseSuppression = true, diff --git a/opentera_webrtc_ros/include/RosDataChannelBridge.h b/opentera_webrtc_ros/include/RosDataChannelBridge.h index dce8774..8cffa21 100644 --- a/opentera_webrtc_ros/include/RosDataChannelBridge.h +++ b/opentera_webrtc_ros/include/RosDataChannelBridge.h @@ -13,24 +13,24 @@ namespace opentera /** * @brief Implement a ROS node to bridge WebRTC data channel to ROS topics */ - class RosDataChannelBridge: public RosWebRTCBridge + class RosDataChannelBridge : public RosWebRTCBridge { ros::Subscriber m_dataSubscriber; ros::Publisher m_dataPublisher; - void initSignalingClient(const opentera::SignalingServerConfiguration &signalingServerConfiguration); + void initSignalingClient(const opentera::SignalingServerConfiguration& signalingServerConfiguration); void initAdvertiseTopics(); void stopAdvertiseTopics(); void initDataChannelCallback(); void stopDataChannelCallback(); - void onJoinSessionEvents(const std::vector &events) override; - void onStopSessionEvents(const std::vector &events) override; + void onJoinSessionEvents(const std::vector& events) override; + void onStopSessionEvents(const std::vector& events) override; void onSignalingConnectionClosed() override; void onSignalingConnectionError(const std::string& msg) override; - void onRosData(const std_msgs::StringConstPtr & msg); + void onRosData(const std_msgs::StringConstPtr& msg); public: RosDataChannelBridge(const ros::NodeHandle& nh); diff --git a/opentera_webrtc_ros/include/RosJsonDataHandler.h b/opentera_webrtc_ros/include/RosJsonDataHandler.h index 356e799..9310491 100644 --- a/opentera_webrtc_ros/include/RosJsonDataHandler.h +++ b/opentera_webrtc_ros/include/RosJsonDataHandler.h @@ -44,8 +44,7 @@ namespace opentera ros::NodeHandle m_nh; ros::NodeHandle m_p_nh; - virtual void onWebRTCDataReceived( - const ros::MessageEvent& event); + virtual void onWebRTCDataReceived(const ros::MessageEvent& event); public: RosJsonDataHandler(const ros::NodeHandle& nh, const ros::NodeHandle& p_nh); @@ -54,8 +53,7 @@ namespace opentera static void run(); private: - static opentera_webrtc_ros_msgs::Waypoint - getWpFromData(const nlohmann::json& data); + static opentera_webrtc_ros_msgs::Waypoint getWpFromData(const nlohmann::json& data); }; } diff --git a/opentera_webrtc_ros/include/RosNodeParameters.h b/opentera_webrtc_ros/include/RosNodeParameters.h index 69ce0db..fb225f9 100644 --- a/opentera_webrtc_ros/include/RosNodeParameters.h +++ b/opentera_webrtc_ros/include/RosNodeParameters.h @@ -10,39 +10,39 @@ namespace opentera */ class RosNodeParameters { - public: - template - static bool isInParams(const std::string &key, const std::map &dict); + template + static bool isInParams(const std::string& key, const std::map& dict); static bool isStandAlone(); - static void loadSignalingParams(std::string &clientName,std::string &room); - static void loadSignalingParams(std::string &serverUrl, - std::string &clientName, - std::string &room, - std::string &password); + static void loadSignalingParams(std::string& clientName, std::string& room); + static void loadSignalingParams( + std::string& serverUrl, + std::string& clientName, + std::string& room, + std::string& password); - static void loadSignalingParamsVerifySSL(bool &verifySSL); + static void loadSignalingParamsVerifySSL(bool& verifySSL); static void loadVideoStreamParams( - bool &canSendVideoStream, - bool &canReceiveVideoStream, - bool &denoise, - bool &screencast); + bool& canSendVideoStream, + bool& canReceiveVideoStream, + bool& denoise, + bool& screencast); static void loadAudioStreamParams( - bool &canSendAudioStream, - bool &canReceiveAudioStream, - unsigned int &soundCardTotalDelayMs, - bool &echoCancellation, - bool &autoGainControl, - bool &noiseSuppression, - bool &highPassFilter, - bool &stereoSwapping, - bool &typingDetection, - bool &residualEchoDetector, - bool &transientSuppression); + bool& canSendAudioStream, + bool& canReceiveAudioStream, + unsigned int& soundCardTotalDelayMs, + bool& echoCancellation, + bool& autoGainControl, + bool& noiseSuppression, + bool& highPassFilter, + bool& stereoSwapping, + bool& typingDetection, + bool& residualEchoDetector, + bool& transientSuppression); }; /** @@ -52,11 +52,11 @@ namespace opentera * @param dict Dictionary (std::map) to search in * @return true if found otherwise false */ - template - bool RosNodeParameters::isInParams(const std::string &key, const std::map &dict) + template + bool RosNodeParameters::isInParams(const std::string& key, const std::map& dict) { return dict.find(key) != dict.end(); } } -#endif \ No newline at end of file +#endif diff --git a/opentera_webrtc_ros/include/RosParamUtils.h b/opentera_webrtc_ros/include/RosParamUtils.h index f1bed49..e1d7c93 100644 --- a/opentera_webrtc_ros/include/RosParamUtils.h +++ b/opentera_webrtc_ros/include/RosParamUtils.h @@ -19,16 +19,11 @@ namespace opentera * \throws InvalidNameException If the parameter key begins with a tilde, or is an * otherwise invalid graph resource name */ - bool getParam(const ros::NodeHandle& nh, const std::string& key, - std::map& map); - bool getParam(const ros::NodeHandle& nh, const std::string& key, - std::map& map); - bool getParam(const ros::NodeHandle& nh, const std::string& key, - std::map& map); - bool getParam(const ros::NodeHandle& nh, const std::string& key, - std::map& map); - bool getParam(const ros::NodeHandle& nh, const std::string& key, - std::map& map); + bool getParam(const ros::NodeHandle& nh, const std::string& key, std::map& map); + bool getParam(const ros::NodeHandle& nh, const std::string& key, std::map& map); + bool getParam(const ros::NodeHandle& nh, const std::string& key, std::map& map); + bool getParam(const ros::NodeHandle& nh, const std::string& key, std::map& map); + bool getParam(const ros::NodeHandle& nh, const std::string& key, std::map& map); /** \brief Get a {double, float, int, bool, string} map value from the parameter * server using the cache. @@ -42,16 +37,11 @@ namespace opentera * \throws InvalidNameException If the parameter key begins with a tilde, or is an * otherwise invalid graph resource name */ - bool getParamCached(const ros::NodeHandle& nh, const std::string& key, - std::map& map); - bool getParamCached(const ros::NodeHandle& nh, const std::string& key, - std::map& map); - bool getParamCached(const ros::NodeHandle& nh, const std::string& key, - std::map& map); - bool getParamCached(const ros::NodeHandle& nh, const std::string& key, - std::map& map); - bool getParamCached(const ros::NodeHandle& nh, const std::string& key, - std::map& map); + bool getParamCached(const ros::NodeHandle& nh, const std::string& key, std::map& map); + bool getParamCached(const ros::NodeHandle& nh, const std::string& key, std::map& map); + bool getParamCached(const ros::NodeHandle& nh, const std::string& key, std::map& map); + bool getParamCached(const ros::NodeHandle& nh, const std::string& key, std::map& map); + bool getParamCached(const ros::NodeHandle& nh, const std::string& key, std::map& map); /** \brief Get a {double, float, int, bool, string} vector value from the * parameter server. @@ -65,16 +55,11 @@ namespace opentera * \throws InvalidNameException If the parameter key begins with a tilde, or is an * otherwise invalid graph resource name */ - bool getParam(const ros::NodeHandle& nh, const std::string& key, - std::vector& vec); - bool getParam(const ros::NodeHandle& nh, const std::string& key, - std::vector& vec); - bool getParam(const ros::NodeHandle& nh, const std::string& key, - std::vector& vec); - bool getParam(const ros::NodeHandle& nh, const std::string& key, - std::vector& vec); - bool getParam(const ros::NodeHandle& nh, const std::string& key, - std::vector& vec); + bool getParam(const ros::NodeHandle& nh, const std::string& key, std::vector& vec); + bool getParam(const ros::NodeHandle& nh, const std::string& key, std::vector& vec); + bool getParam(const ros::NodeHandle& nh, const std::string& key, std::vector& vec); + bool getParam(const ros::NodeHandle& nh, const std::string& key, std::vector& vec); + bool getParam(const ros::NodeHandle& nh, const std::string& key, std::vector& vec); /** \brief Get a {double, float, int, bool, string} vector value from the * parameter server using the cache. @@ -88,16 +73,11 @@ namespace opentera * \throws InvalidNameException If the parameter key begins with a tilde, or is an * otherwise invalid graph resource name */ - bool getParamCached(const ros::NodeHandle& nh, const std::string& key, - std::vector& vec); - bool getParamCached(const ros::NodeHandle& nh, const std::string& key, - std::vector& vec); - bool getParamCached(const ros::NodeHandle& nh, const std::string& key, - std::vector& vec); - bool getParamCached(const ros::NodeHandle& nh, const std::string& key, - std::vector& vec); - bool getParamCached(const ros::NodeHandle& nh, const std::string& key, - std::vector& vec); + bool getParamCached(const ros::NodeHandle& nh, const std::string& key, std::vector& vec); + bool getParamCached(const ros::NodeHandle& nh, const std::string& key, std::vector& vec); + bool getParamCached(const ros::NodeHandle& nh, const std::string& key, std::vector& vec); + bool getParamCached(const ros::NodeHandle& nh, const std::string& key, std::vector& vec); + bool getParamCached(const ros::NodeHandle& nh, const std::string& key, std::vector& vec); } } diff --git a/opentera_webrtc_ros/include/RosSignalingServerconfiguration.h b/opentera_webrtc_ros/include/RosSignalingServerconfiguration.h index 0afa529..30b9a47 100644 --- a/opentera_webrtc_ros/include/RosSignalingServerconfiguration.h +++ b/opentera_webrtc_ros/include/RosSignalingServerconfiguration.h @@ -9,16 +9,15 @@ namespace opentera /** * @brief Utility to build signaling server configuration from ROS parameter server */ - class RosSignalingServerConfiguration: public SignalingServerConfiguration + class RosSignalingServerConfiguration : public SignalingServerConfiguration { - static std::string getQueryFrom(const std::string& query, const std::string& queries); public: static SignalingServerConfiguration fromRosParam(); static SignalingServerConfiguration fromUrl(const std::string& url); - static std::string getIceServerUrl(const std::string &url); - static std::string getBaseUrl(const std::string &url); + static std::string getIceServerUrl(const std::string& url); + static std::string getBaseUrl(const std::string& url); }; } diff --git a/opentera_webrtc_ros/include/RosStreamBridge.h b/opentera_webrtc_ros/include/RosStreamBridge.h index 59ef32e..3762062 100644 --- a/opentera_webrtc_ros/include/RosStreamBridge.h +++ b/opentera_webrtc_ros/include/RosStreamBridge.h @@ -18,7 +18,7 @@ namespace opentera * * View README.md for more details */ - class RosStreamBridge: public RosWebRTCBridge + class RosStreamBridge : public RosWebRTCBridge { std::shared_ptr m_videoSource; std::shared_ptr m_audioSource; @@ -34,10 +34,10 @@ namespace opentera bool m_canReceiveAudioStream; bool m_canReceiveVideoStream; - void init(const opentera::SignalingServerConfiguration &signalingServerConfiguration); + void init(const opentera::SignalingServerConfiguration& signalingServerConfiguration); - void onJoinSessionEvents(const std::vector &events) override; - void onStopSessionEvents(const std::vector &events) override; + void onJoinSessionEvents(const std::vector& events) override; + void onStopSessionEvents(const std::vector& events) override; void onSignalingConnectionOpened() override; void onSignalingConnectionClosed() override; @@ -45,19 +45,22 @@ namespace opentera void onSignalingConnectionError(const std::string& msg) override; void onVideoFrameReceived(const Client& client, const cv::Mat& bgrImg, uint64_t timestampUs); - void onAudioFrameReceived(const Client& client, + void onAudioFrameReceived( + const Client& client, const void* audioData, int bitsPerSample, int sampleRate, size_t numberOfChannels, size_t numberOfFrames); - void onMixedAudioFrameReceived(const void* audioData, + void onMixedAudioFrameReceived( + const void* audioData, int bitsPerSample, int sampleRate, size_t numberOfChannels, size_t numberOfFrames); - audio_utils::AudioFrame createAudioFrame(const void* audioData, + audio_utils::AudioFrame createAudioFrame( + const void* audioData, int bitsPerSample, int sampleRate, size_t numberOfChannels, diff --git a/opentera_webrtc_ros/include/RosVideoSource.h b/opentera_webrtc_ros/include/RosVideoSource.h index 8b94d68..810a76c 100644 --- a/opentera_webrtc_ros/include/RosVideoSource.h +++ b/opentera_webrtc_ros/include/RosVideoSource.h @@ -13,7 +13,6 @@ namespace opentera */ class RosVideoSource : public VideoSource { - public: RosVideoSource(bool needsDenoising, bool isScreenCast); void sendFrame(const sensor_msgs::ImageConstPtr& msg); diff --git a/opentera_webrtc_ros/include/RosWebRTCBridge.h b/opentera_webrtc_ros/include/RosWebRTCBridge.h index ccb5823..25e770a 100644 --- a/opentera_webrtc_ros/include/RosWebRTCBridge.h +++ b/opentera_webrtc_ros/include/RosWebRTCBridge.h @@ -23,7 +23,8 @@ #include #include -namespace opentera { +namespace opentera +{ /** * @brief Interface a ROS node to bridge WebRTC to ROS topics @@ -46,15 +47,16 @@ namespace opentera { virtual void disconnect(); virtual void onEvent(const ros::MessageEvent& event); - virtual void onDataBaseEvents(const std::vector &events); - virtual void onDeviceEvents(const std::vector &events); - virtual void onJoinSessionEvents(const std::vector &events); - virtual void onJoinSessionReplyEvents(const std::vector &events); - virtual void onLeaveSessionEvents(const std::vector &events); - virtual void onLogEvents(const std::vector &events); - virtual void onParticipantEvents(const std::vector &events); - virtual void onStopSessionEvents(const std::vector &events); - virtual void onUserEvents(const std::vector &events); + virtual void onDataBaseEvents(const std::vector& events); + virtual void onDeviceEvents(const std::vector& events); + virtual void onJoinSessionEvents(const std::vector& events); + virtual void + onJoinSessionReplyEvents(const std::vector& events); + virtual void onLeaveSessionEvents(const std::vector& events); + virtual void onLogEvents(const std::vector& events); + virtual void onParticipantEvents(const std::vector& events); + virtual void onStopSessionEvents(const std::vector& events); + virtual void onUserEvents(const std::vector& events); virtual void connectSignalingClientEvents(); virtual void onSignalingConnectionOpened(); @@ -67,9 +69,9 @@ namespace opentera { virtual void onClientDisconnected(const Client& client); virtual void onError(const std::string& error); - template + template void publishPeerFrame(ros::Publisher& publisher, const Client& client, const decltype(PeerMsg::frame)& frame); - void publishPeerStatus(const Client &client, int status); + void publishPeerStatus(const Client& client, int status); public: RosWebRTCBridge(const ros::NodeHandle& nh); @@ -84,13 +86,11 @@ namespace opentera { * @param nh node handle. */ template - RosWebRTCBridge::RosWebRTCBridge(const ros::NodeHandle& nh): m_signalingClient(nullptr), m_nh(nh) + RosWebRTCBridge::RosWebRTCBridge(const ros::NodeHandle& nh) : m_signalingClient(nullptr), + m_nh(nh) { // On CTRL+C exit - signal(SIGINT, [](int sig) - { - ros::requestShutdown(); - }); + signal(SIGINT, [](int sig) { ros::requestShutdown(); }); nodeName = ros::this_node::getName(); @@ -122,7 +122,9 @@ namespace opentera { connectSignalingClientEvents(); if (m_signalingClient != nullptr) { - ROS_INFO_STREAM(nodeName << " --> " << "Connecting to signaling server..."); + ROS_INFO_STREAM( + nodeName << " --> " + << "Connecting to signaling server..."); m_signalingClient->connect(); } } @@ -135,10 +137,12 @@ namespace opentera { { if (m_signalingClient != nullptr) { - ROS_INFO_STREAM(nodeName << " --> " << "Disconnecting from signaling server..."); + ROS_INFO_STREAM( + nodeName << " --> " + << "Disconnecting from signaling server..."); m_signalingClient->closeSync(); - //Reset client + // Reset client m_signalingClient = nullptr; } } @@ -151,8 +155,11 @@ namespace opentera { * @param frame The message to peer with a client */ template - template - void RosWebRTCBridge::publishPeerFrame(ros::Publisher& publisher, const Client& client, const decltype(PeerMsg::frame)& frame) + template + void RosWebRTCBridge::publishPeerFrame( + ros::Publisher& publisher, + const Client& client, + const decltype(PeerMsg::frame)& frame) { PeerMsg peerFrameMsg; peerFrameMsg.sender.id = client.id(); @@ -162,7 +169,7 @@ namespace opentera { } template - void RosWebRTCBridge::publishPeerStatus(const Client &client, int status) + void RosWebRTCBridge::publishPeerStatus(const Client& client, int status) { opentera_webrtc_ros_msgs::PeerStatus msg; msg.sender.id = client.id(); @@ -196,55 +203,55 @@ namespace opentera { if (!msg.database_events.empty()) { - //ROS_INFO("DATABASE_EVENTS"); + // ROS_INFO("DATABASE_EVENTS"); onDataBaseEvents(msg.database_events); } if (!msg.device_events.empty()) { - //ROS_INFO("DEVICE_EVENTS"); + // ROS_INFO("DEVICE_EVENTS"); onDeviceEvents(msg.device_events); } if (!msg.join_session_events.empty()) { - //ROS_INFO("JOIN_SESSION_EVENTS"); + // ROS_INFO("JOIN_SESSION_EVENTS"); onJoinSessionEvents(msg.join_session_events); } if (!msg.join_session_reply_events.empty()) { - //ROS_INFO("JOIN_SESSION_REPLY_EVENTS"); + // ROS_INFO("JOIN_SESSION_REPLY_EVENTS"); onJoinSessionReplyEvents(msg.join_session_reply_events); } if (!msg.leave_session_events.empty()) { - //ROS_INFO("LEAVE_SESSION_EVENTS"); + // ROS_INFO("LEAVE_SESSION_EVENTS"); onLeaveSessionEvents(msg.leave_session_events); } if (!msg.log_events.empty()) { - //ROS_INFO("LOG_EVENTS"); + // ROS_INFO("LOG_EVENTS"); onLogEvents(msg.log_events); } if (!msg.participant_events.empty()) { - //ROS_INFO("PARTICIPANT_EVENTS"); + // ROS_INFO("PARTICIPANT_EVENTS"); onParticipantEvents(msg.participant_events); } if (!msg.stop_session_events.empty()) { - //ROS_INFO("STOP_SESSION_EVENTS"); + // ROS_INFO("STOP_SESSION_EVENTS"); onStopSessionEvents(msg.stop_session_events); } if (!msg.user_events.empty()) { - //ROS_INFO("USER_EVENTS"); + // ROS_INFO("USER_EVENTS"); onUserEvents(msg.user_events); } } @@ -255,7 +262,9 @@ namespace opentera { * @param events A vector of database event message */ template - void RosWebRTCBridge::onDataBaseEvents(const std::vector &events) {} + void RosWebRTCBridge::onDataBaseEvents(const std::vector& events) + { + } /** * @brief Callback that is call when their's a device event received @@ -263,7 +272,9 @@ namespace opentera { * @param events A vector of device event message */ template - void RosWebRTCBridge::onDeviceEvents(const std::vector &events) {} + void RosWebRTCBridge::onDeviceEvents(const std::vector& events) + { + } /** * @brief Callback that is call when their's a join session event received @@ -271,7 +282,9 @@ namespace opentera { * @param events A vector of join session event message */ template - void RosWebRTCBridge::onJoinSessionEvents(const std::vector &events) {} + void RosWebRTCBridge::onJoinSessionEvents(const std::vector& events) + { + } /** * @brief Callback that is call when their's a join session reply event received @@ -279,7 +292,10 @@ namespace opentera { * @param events A vector of join session reply event message */ template - void RosWebRTCBridge::onJoinSessionReplyEvents(const std::vector &events) {} + void RosWebRTCBridge::onJoinSessionReplyEvents( + const std::vector& events) + { + } /** * @brief Callback that is call when their's a leave session event received @@ -287,7 +303,10 @@ namespace opentera { * @param events A vector of leave session event message */ template - void RosWebRTCBridge::onLeaveSessionEvents(const std::vector &events) {} + void + RosWebRTCBridge::onLeaveSessionEvents(const std::vector& events) + { + } /** * @brief Callback that is call when their's a log event event received @@ -295,7 +314,9 @@ namespace opentera { * @param events A vector of log session event message */ template - void RosWebRTCBridge::onLogEvents(const std::vector &events) {} + void RosWebRTCBridge::onLogEvents(const std::vector& events) + { + } /** * @brief Callback that is call when their's a participant event received @@ -303,7 +324,9 @@ namespace opentera { * @param events A vector of participant event message */ template - void RosWebRTCBridge::onParticipantEvents(const std::vector &events) {} + void RosWebRTCBridge::onParticipantEvents(const std::vector& events) + { + } /** * @brief Callback that is call when their's a stop session event received @@ -311,7 +334,9 @@ namespace opentera { * @param events A vector of stop session event message */ template - void RosWebRTCBridge::onStopSessionEvents(const std::vector &events) {} + void RosWebRTCBridge::onStopSessionEvents(const std::vector& events) + { + } /** * @brief Callback that is call when their's a user event received @@ -319,7 +344,9 @@ namespace opentera { * @param events A vector of user event message */ template - void RosWebRTCBridge::onUserEvents(const std::vector &events) {} + void RosWebRTCBridge::onUserEvents(const std::vector& events) + { + } /************************************************************************ * SIGNALING CLIENT CALLBACK @@ -333,17 +360,25 @@ namespace opentera { { if (m_signalingClient != nullptr) { - m_signalingClient->setOnSignalingConnectionOpened(std::bind(&RosWebRTCBridge::onSignalingConnectionOpened, this)); - m_signalingClient->setOnSignalingConnectionClosed(std::bind(&RosWebRTCBridge::onSignalingConnectionClosed, this)); - m_signalingClient->setOnSignalingConnectionError(std::bind(&RosWebRTCBridge::onSignalingConnectionError, this, std::placeholders::_1)); - - m_signalingClient->setOnRoomClientsChanged(std::bind(&RosWebRTCBridge::onRoomClientsChanged, this, std::placeholders::_1)); - - m_signalingClient->setCallAcceptor(std::bind(&RosWebRTCBridge::callAcceptor, this, std::placeholders::_1)); - m_signalingClient->setOnCallRejected(std::bind(&RosWebRTCBridge::onCallRejected, this, std::placeholders::_1)); - - m_signalingClient->setOnClientConnected(std::bind(&RosWebRTCBridge::onClientConnected, this, std::placeholders::_1)); - m_signalingClient->setOnClientDisconnected(std::bind(&RosWebRTCBridge::onClientDisconnected, this, std::placeholders::_1)); + m_signalingClient->setOnSignalingConnectionOpened( + std::bind(&RosWebRTCBridge::onSignalingConnectionOpened, this)); + m_signalingClient->setOnSignalingConnectionClosed( + std::bind(&RosWebRTCBridge::onSignalingConnectionClosed, this)); + m_signalingClient->setOnSignalingConnectionError( + std::bind(&RosWebRTCBridge::onSignalingConnectionError, this, std::placeholders::_1)); + + m_signalingClient->setOnRoomClientsChanged( + std::bind(&RosWebRTCBridge::onRoomClientsChanged, this, std::placeholders::_1)); + + m_signalingClient->setCallAcceptor( + std::bind(&RosWebRTCBridge::callAcceptor, this, std::placeholders::_1)); + m_signalingClient->setOnCallRejected( + std::bind(&RosWebRTCBridge::onCallRejected, this, std::placeholders::_1)); + + m_signalingClient->setOnClientConnected( + std::bind(&RosWebRTCBridge::onClientConnected, this, std::placeholders::_1)); + m_signalingClient->setOnClientDisconnected( + std::bind(&RosWebRTCBridge::onClientDisconnected, this, std::placeholders::_1)); m_signalingClient->setOnError(std::bind(&RosWebRTCBridge::onError, this, std::placeholders::_1)); } @@ -355,7 +390,9 @@ namespace opentera { template void RosWebRTCBridge::onSignalingConnectionOpened() { - ROS_INFO_STREAM(nodeName << " --> " << "Signaling connection opened, streaming topic..."); + ROS_INFO_STREAM( + nodeName << " --> " + << "Signaling connection opened, streaming topic..."); } /** @@ -364,7 +401,9 @@ namespace opentera { template void RosWebRTCBridge::onSignalingConnectionClosed() { - ROS_WARN_STREAM(nodeName << " --> " << "Signaling connection closed."); + ROS_WARN_STREAM( + nodeName << " --> " + << "Signaling connection closed."); } /** @@ -375,7 +414,9 @@ namespace opentera { template void RosWebRTCBridge::onSignalingConnectionError(const std::string& msg) { - ROS_ERROR_STREAM(nodeName << " --> " << "Signaling connection error " << msg.c_str() << ", shutting down..."); + ROS_ERROR_STREAM( + nodeName << " --> " + << "Signaling connection error " << msg.c_str() << ", shutting down..."); ros::requestShutdown(); } @@ -390,7 +431,8 @@ namespace opentera { std::string log = nodeName + " --> Signaling on room clients changed:"; for (const auto& client : roomClients) { - log += "\n\tid: " + client.id() + ", name: " + client.name() + ", isConnected: " + (client.isConnected() ? "true" : "false"); + log += "\n\tid: " + client.id() + ", name: " + client.name() + + ", isConnected: " + (client.isConnected() ? "true" : "false"); } ROS_INFO_STREAM(log); } @@ -427,8 +469,10 @@ namespace opentera { void RosWebRTCBridge::onClientConnected(const Client& client) { publishPeerStatus(client, opentera_webrtc_ros_msgs::PeerStatus::STATUS_CLIENT_CONNECTED); - ROS_INFO_STREAM(nodeName << " --> " - << "Signaling on client connected: " << "id: " << client.id() << ", name: " << client.name()); + ROS_INFO_STREAM( + nodeName << " --> " + << "Signaling on client connected: " + << "id: " << client.id() << ", name: " << client.name()); } /** @@ -440,8 +484,10 @@ namespace opentera { void RosWebRTCBridge::onClientDisconnected(const Client& client) { publishPeerStatus(client, opentera_webrtc_ros_msgs::PeerStatus::STATUS_CLIENT_DISCONNECTED); - ROS_INFO_STREAM(nodeName << " --> " - << "Signaling on client disconnected: " << "id: " << client.id() << ", name: " << client.name()); + ROS_INFO_STREAM( + nodeName << " --> " + << "Signaling on client disconnected: " + << "id: " << client.id() << ", name: " << client.name()); } /** @@ -456,4 +502,4 @@ namespace opentera { } } -#endif \ No newline at end of file +#endif diff --git a/opentera_webrtc_ros/launch/opentera_data_test.launch b/opentera_webrtc_ros/launch/opentera_data_test.launch index 9c51fea..43f8b0f 100644 --- a/opentera_webrtc_ros/launch/opentera_data_test.launch +++ b/opentera_webrtc_ros/launch/opentera_data_test.launch @@ -1,27 +1,27 @@ - - - + + + - - - - + + + + - - - + + + - - $(arg is_stand_alone) - - { - server_url: $(arg server_url), # Signaling server URL - client_name: $(arg client_name), # Peer name as which to join the room - room_name: $(arg room_name), # Room name to join - room_password: $(arg room_password) # Room password - } - - + + $(arg is_stand_alone) + + { + server_url: $(arg server_url), # Signaling server URL + client_name: $(arg client_name), # Peer name as which to join the room + room_name: $(arg room_name), # Room name to join + room_password: $(arg room_password) # Room password + } + + diff --git a/opentera_webrtc_ros/launch/ros_data_channel_client.launch b/opentera_webrtc_ros/launch/ros_data_channel_client.launch index 10d2e9d..8404ab1 100644 --- a/opentera_webrtc_ros/launch/ros_data_channel_client.launch +++ b/opentera_webrtc_ros/launch/ros_data_channel_client.launch @@ -1,24 +1,24 @@ - - - + + + - - - - - + + + + + - - $(arg is_stand_alone) - - { - server_url: $(arg server_url), # Signaling server URL - client_name: $(arg client_name), # Peer name as which to join the room - room_name: $(arg room_name), # Room name to join - room_password: $(arg room_password), # Room password - verify_ssl: $(arg verify_ssl) # SSL peer verification - } - - + + $(arg is_stand_alone) + + { + server_url: $(arg server_url), # Signaling server URL + client_name: $(arg client_name), # Peer name as which to join the room + room_name: $(arg room_name), # Room name to join + room_password: $(arg room_password), # Room password + verify_ssl: $(arg verify_ssl) # SSL peer verification + } + + diff --git a/opentera_webrtc_ros/launch/ros_json_data_handler.launch b/opentera_webrtc_ros/launch/ros_json_data_handler.launch index 8254b74..5916e34 100644 --- a/opentera_webrtc_ros/launch/ros_json_data_handler.launch +++ b/opentera_webrtc_ros/launch/ros_json_data_handler.launch @@ -1,17 +1,17 @@ - - - - - - + + + + + + - - - - - - + + + + + + - \ No newline at end of file + diff --git a/opentera_webrtc_ros/launch/ros_stream_client.launch b/opentera_webrtc_ros/launch/ros_stream_client.launch index 9500c0a..a86cee2 100644 --- a/opentera_webrtc_ros/launch/ros_stream_client.launch +++ b/opentera_webrtc_ros/launch/ros_stream_client.launch @@ -1,83 +1,83 @@ - - - + + + - - - - - + + + + + - - - - - - - - - - - - + + + + + + + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - - $(arg is_stand_alone) - - { - can_send_video_stream: $(arg can_send_video_stream), # Is the node can send video stream to the signaling server - can_receive_video_stream: $(arg can_receive_video_stream), # Is the node can receive video stream from the signaling server - is_screen_cast: $(arg is_screen_cast), # Is the image source a screen capture? - needs_denoising: $(arg needs_denoising) # Does the image source needs denoising? - } - - - { - can_send_audio_stream: $(arg can_send_audio_stream), # Is the node can send audio stream to the signaling server - can_receive_audio_stream: $(arg can_receive_audio_stream), # Is the node can receive audio stream from the signaling server - sound_card_total_delay_ms: $(arg sound_card_total_delay_ms), - echo_cancellation: $(arg echo_cancellation), - auto_gain_control: $(arg auto_gain_control), - noise_suppression: $(arg noise_suppression), - high_pass_filter: $(arg high_pass_filter), - stereo_swapping: $(arg stereo_swapping), - typing_detection: $(arg typing_detection), - residual_echo_detector: $(arg residual_echo_detector), - transient_suppression: $(arg transient_suppression) - } - + + $(arg is_stand_alone) + + { + can_send_video_stream: $(arg can_send_video_stream), # Is the node can send video stream to the signaling server + can_receive_video_stream: $(arg can_receive_video_stream), # Is the node can receive video stream from the signaling server + is_screen_cast: $(arg is_screen_cast), # Is the image source a screen capture? + needs_denoising: $(arg needs_denoising) # Does the image source needs denoising? + } + + + { + can_send_audio_stream: $(arg can_send_audio_stream), # Is the node can send audio stream to the signaling server + can_receive_audio_stream: $(arg can_receive_audio_stream), # Is the node can receive audio stream from the signaling server + sound_card_total_delay_ms: $(arg sound_card_total_delay_ms), + echo_cancellation: $(arg echo_cancellation), + auto_gain_control: $(arg auto_gain_control), + noise_suppression: $(arg noise_suppression), + high_pass_filter: $(arg high_pass_filter), + stereo_swapping: $(arg stereo_swapping), + typing_detection: $(arg typing_detection), + residual_echo_detector: $(arg residual_echo_detector), + transient_suppression: $(arg transient_suppression) + } + - - { - server_url: $(arg server_url), # Signaling server URL - client_name: $(arg client_name), # Peer name as which to join the room - room_name: $(arg room_name), # Room name to join - room_password: $(arg room_password), # Room password - verify_ssl: $(arg verify_ssl) # SSL peer verification - } - + + { + server_url: $(arg server_url), # Signaling server URL + client_name: $(arg client_name), # Peer name as which to join the room + room_name: $(arg room_name), # Room name to join + room_password: $(arg room_password), # Room password + verify_ssl: $(arg verify_ssl) # SSL peer verification + } + - - - - - - + + + + + + diff --git a/opentera_webrtc_ros/launch/sim_beam.launch b/opentera_webrtc_ros/launch/sim_beam.launch index 06ad959..2a69edf 100644 --- a/opentera_webrtc_ros/launch/sim_beam.launch +++ b/opentera_webrtc_ros/launch/sim_beam.launch @@ -3,9 +3,9 @@ - - - + + + @@ -14,7 +14,7 @@ - + - + @@ -99,11 +99,11 @@ - + - \ No newline at end of file + diff --git a/opentera_webrtc_ros/opentera-webrtc b/opentera_webrtc_ros/opentera-webrtc index a108ac1..a8d6af0 160000 --- a/opentera_webrtc_ros/opentera-webrtc +++ b/opentera_webrtc_ros/opentera-webrtc @@ -1 +1 @@ -Subproject commit a108ac130be91946534c5d1faabff472ec7e938d +Subproject commit a8d6af0d599ca95b8e95ffdb2abdb3169f99558a diff --git a/opentera_webrtc_ros/package.xml b/opentera_webrtc_ros/package.xml index 62abc1a..6b37870 100644 --- a/opentera_webrtc_ros/package.xml +++ b/opentera_webrtc_ros/package.xml @@ -1,28 +1,28 @@ - opentera_webrtc_ros - 0.1.0 - Provides Nodes to join a WebRTC call from ROS + opentera_webrtc_ros + 0.1.0 + Provides Nodes to join a WebRTC call from ROS - Cédric Godin - Apache-2.0 - https://github.com/introlab/opentera-webrtc-ros - Cédric Godin + Cédric Godin + Apache-2.0 + https://github.com/introlab/opentera-webrtc-ros + Cédric Godin - catkin - message_generation - audio_utils - opentera_webrtc_ros_msgs - map_image_generator - rospy - roscpp - cv_bridge - std_msgs - sensor_msgs - message_runtime - std_srvs - rospy - visualization_msgs - - + catkin + message_generation + audio_utils + opentera_webrtc_ros_msgs + map_image_generator + rospy + roscpp + cv_bridge + std_msgs + sensor_msgs + message_runtime + std_srvs + rospy + visualization_msgs + + diff --git a/opentera_webrtc_ros/scripts/opentera_webrtc_audio_mixer.py b/opentera_webrtc_ros/scripts/opentera_webrtc_audio_mixer.py index 8e3d013..9324b46 100755 --- a/opentera_webrtc_ros/scripts/opentera_webrtc_audio_mixer.py +++ b/opentera_webrtc_ros/scripts/opentera_webrtc_audio_mixer.py @@ -15,6 +15,7 @@ output_device_index = 0 + class AudioWriter: def __init__(self, peer_id: str): self._peer_id = peer_id @@ -51,7 +52,8 @@ def _run(self): channels=audio.frame.channel_count, rate=audio.frame.sampling_frequency, output_device_index=output_device_index, - frames_per_buffer=int(audio.frame.frame_sample_count * 20), + frames_per_buffer=int( + audio.frame.frame_sample_count * 20), output=True) # Fill buffer with zeros ? # for _ in range(10): @@ -59,7 +61,8 @@ def _run(self): stream.write(audio.frame.data) else: - print('Unsupported format: ', audio.frame.format, self._peer_id) + print('Unsupported format: ', + audio.frame.format, self._peer_id) except queue.Empty as e: # An exception will occur when queue is empty @@ -83,7 +86,8 @@ def stop(self): class AudioMixerROS: def __init__(self): - self._subscriber = rospy.Subscriber('/webrtc_audio', PeerAudio, self._on_peer_audio, queue_size=100) + self._subscriber = rospy.Subscriber( + '/webrtc_audio', PeerAudio, self._on_peer_audio, queue_size=100) self._writers = dict() # Cleanup timer every second self._timer = rospy.Timer(rospy.Duration(1), self._on_cleanup_timeout) @@ -131,4 +135,3 @@ def _on_peer_audio(self, audio: PeerAudio): mixer = AudioMixerROS() rospy.spin() mixer.shutdown() - diff --git a/opentera_webrtc_ros/src/RosAudioSource.cpp b/opentera_webrtc_ros/src/RosAudioSource.cpp index 42f0485..2ce65d0 100644 --- a/opentera_webrtc_ros/src/RosAudioSource.cpp +++ b/opentera_webrtc_ros/src/RosAudioSource.cpp @@ -4,35 +4,48 @@ using namespace opentera; -RosAudioSource::RosAudioSource(unsigned int soundCardTotalDelayMs, - bool echoCancellation, - bool autoGainControl, - bool noiseSuppression, - bool highPassFilter, - bool stereoSwapping, - bool typingDetection, - bool residualEchoDetector, - bool transientSuppression) - //Creating a default configuration for now, int bitsPerSample, int sampleRate, size_t numberOfChannels); - : AudioSource(AudioSourceConfiguration::create(soundCardTotalDelayMs, - echoCancellation, autoGainControl, noiseSuppression, highPassFilter, - stereoSwapping, typingDetection, residualEchoDetector, transientSuppression), 16, 48000, 1) +RosAudioSource::RosAudioSource( + unsigned int soundCardTotalDelayMs, + bool echoCancellation, + bool autoGainControl, + bool noiseSuppression, + bool highPassFilter, + bool stereoSwapping, + bool typingDetection, + bool residualEchoDetector, + bool transientSuppression) + // Creating a default configuration for now, int bitsPerSample, int sampleRate, size_t numberOfChannels); + : AudioSource( + AudioSourceConfiguration::create( + soundCardTotalDelayMs, + echoCancellation, + autoGainControl, + noiseSuppression, + highPassFilter, + stereoSwapping, + typingDetection, + residualEchoDetector, + transientSuppression), + 16, + 48000, + 1) { } - void RosAudioSource::sendFrame(const audio_utils::AudioFrameConstPtr& msg) - { - //Checking if frame fits default configuration and send frame - if(msg->channel_count == 1 && - msg->sampling_frequency == 48000 && - msg->format == "signed_16") +void RosAudioSource::sendFrame(const audio_utils::AudioFrameConstPtr& msg) +{ + // Checking if frame fits default configuration and send frame + if (msg->channel_count == 1 && msg->sampling_frequency == 48000 && msg->format == "signed_16") { - //ROS_INFO("Frame size %i", msg->frame_sample_count); + // ROS_INFO("Frame size %i", msg->frame_sample_count); AudioSource::sendFrame(msg->data.data(), msg->frame_sample_count); } else { - ROS_ERROR("Invalid audio frame (channel_count=%i, sampling_frequency=%i, format=%s)", - msg->channel_count, msg->sampling_frequency, msg->format.c_str()); + ROS_ERROR( + "Invalid audio frame (channel_count=%i, sampling_frequency=%i, format=%s)", + msg->channel_count, + msg->sampling_frequency, + msg->format.c_str()); } - } \ No newline at end of file +} diff --git a/opentera_webrtc_ros/src/RosDataChannelBridge.cpp b/opentera_webrtc_ros/src/RosDataChannelBridge.cpp index 283f4c5..24f8750 100644 --- a/opentera_webrtc_ros/src/RosDataChannelBridge.cpp +++ b/opentera_webrtc_ros/src/RosDataChannelBridge.cpp @@ -13,7 +13,7 @@ using namespace opentera_webrtc_ros_msgs; /** * @brief Construct a data channel bridge */ -RosDataChannelBridge::RosDataChannelBridge(const ros::NodeHandle& nh): RosWebRTCBridge(nh) +RosDataChannelBridge::RosDataChannelBridge(const ros::NodeHandle& nh) : RosWebRTCBridge(nh) { if (RosNodeParameters::isStandAlone()) { @@ -43,13 +43,12 @@ RosDataChannelBridge::~RosDataChannelBridge() * * @param signalingServerConfiguration Signaling server configuration */ -void RosDataChannelBridge::initSignalingClient(const SignalingServerConfiguration &signalingServerConfiguration) +void RosDataChannelBridge::initSignalingClient(const SignalingServerConfiguration& signalingServerConfiguration) { string iceServersUrl = RosSignalingServerConfiguration::getIceServerUrl(signalingServerConfiguration.url()); ROS_INFO("Fetching ice servers from : %s", iceServersUrl.c_str()); vector iceServers; - if (!IceServer::fetchFromServer(iceServersUrl, - signalingServerConfiguration.password(), iceServers)) + if (!IceServer::fetchFromServer(iceServersUrl, signalingServerConfiguration.password(), iceServers)) { ROS_ERROR("Error fetching ice servers from %s", iceServersUrl.c_str()); iceServers.clear(); @@ -57,9 +56,9 @@ void RosDataChannelBridge::initSignalingClient(const SignalingServerConfiguratio // Create signaling client m_signalingClient = make_unique( - signalingServerConfiguration, - WebrtcConfiguration::create(iceServers), - DataChannelConfiguration::create()); + signalingServerConfiguration, + WebrtcConfiguration::create(iceServers), + DataChannelConfiguration::create()); bool verifySSL; RosNodeParameters::loadSignalingParamsVerifySSL(verifySSL); @@ -90,16 +89,17 @@ void RosDataChannelBridge::stopAdvertiseTopics() void RosDataChannelBridge::initDataChannelCallback() { // Setup data channel callback - m_signalingClient->setOnDataChannelMessageString([&](const Client& client, const string& data) - { - PeerData msg; - //TODO PeerData should have a json string sent by client.data() - msg.data = data; - - msg.sender.id = client.id(); - msg.sender.name = client.name(); - m_dataPublisher.publish(msg); - }); + m_signalingClient->setOnDataChannelMessageString( + [&](const Client& client, const string& data) + { + PeerData msg; + // TODO PeerData should have a json string sent by client.data() + msg.data = data; + + msg.sender.id = client.id(); + msg.sender.name = client.name(); + m_dataPublisher.publish(msg); + }); } /** @@ -111,7 +111,7 @@ void RosDataChannelBridge::stopDataChannelCallback() m_signalingClient->setOnDataChannelMessageString(nullptr); } -void RosDataChannelBridge::onJoinSessionEvents(const std::vector &events) +void RosDataChannelBridge::onJoinSessionEvents(const std::vector& events) { initSignalingClient(RosSignalingServerConfiguration::fromUrl(events[0].session_url)); initAdvertiseTopics(); @@ -119,7 +119,7 @@ void RosDataChannelBridge::onJoinSessionEvents(const std::vector &events) +void RosDataChannelBridge::onStopSessionEvents(const std::vector& events) { stopAdvertiseTopics(); stopDataChannelCallback(); @@ -129,7 +129,9 @@ void RosDataChannelBridge::onStopSessionEvents(const std::vector " << "shutting down..."); + ROS_WARN_STREAM( + nodeName << " --> " + << "shutting down..."); ros::requestShutdown(); } @@ -167,8 +169,12 @@ int main(int argc, char** argv) { init(argc, argv, "data_channel_bridge"); ros::NodeHandle nh; - ROS_INFO_STREAM(ros::this_node::getName() << " --> " << "starting..."); + ROS_INFO_STREAM( + ros::this_node::getName() << " --> " + << "starting..."); RosDataChannelBridge node(nh); node.run(); - ROS_INFO_STREAM(ros::this_node::getName()<< " --> " << "done..."); + ROS_INFO_STREAM( + ros::this_node::getName() << " --> " + << "done..."); } diff --git a/opentera_webrtc_ros/src/RosJsonDataHandler.cpp b/opentera_webrtc_ros/src/RosJsonDataHandler.cpp index 10de45f..f8798e5 100644 --- a/opentera_webrtc_ros/src/RosJsonDataHandler.cpp +++ b/opentera_webrtc_ros/src/RosJsonDataHandler.cpp @@ -2,9 +2,7 @@ using namespace opentera; -RosJsonDataHandler::RosJsonDataHandler(const ros::NodeHandle& nh, - const ros::NodeHandle& p_nh) - : m_nh(nh), m_p_nh(p_nh) +RosJsonDataHandler::RosJsonDataHandler(const ros::NodeHandle& nh, const ros::NodeHandle& p_nh) : m_nh(nh), m_p_nh(p_nh) { m_p_nh.param("linear_multiplier", m_linear_multiplier, 0.15); m_p_nh.param("angular_multiplier", m_angular_multiplier, 0.15); @@ -12,48 +10,37 @@ RosJsonDataHandler::RosJsonDataHandler(const ros::NodeHandle& nh, m_stopPub = m_nh.advertise("stop", 1); m_startPub = m_nh.advertise("start", 1); m_cmdVelPublisher = m_nh.advertise("cmd_vel", 1); - m_waypointsPub = - m_nh.advertise("waypoints", 1); + m_waypointsPub = m_nh.advertise("waypoints", 1); m_navigateToLabelPub = m_nh.advertise("navigate_to_label", 1); m_removeLabelPub = m_nh.advertise("remove_label_by_name", 1); - m_addLabelPub = - m_nh.advertise("add_label_simple", 1); - m_editLabelPub = - m_nh.advertise("edit_label_simple", 1); + m_addLabelPub = m_nh.advertise("add_label_simple", 1); + m_editLabelPub = m_nh.advertise("edit_label_simple", 1); - m_webrtcDataSubscriber = - m_nh.subscribe("webrtc_data", 1, &RosJsonDataHandler::onWebRTCDataReceived, this); + m_webrtcDataSubscriber = m_nh.subscribe("webrtc_data", 1, &RosJsonDataHandler::onWebRTCDataReceived, this); m_dockingClient = m_nh.serviceClient("do_docking"); m_muteClient = m_nh.serviceClient("mute"); m_enableCameraClient = m_nh.serviceClient("enableCamera"); - m_setMovementModeClient = - m_nh.serviceClient("set_movement_mode"); + m_setMovementModeClient = m_nh.serviceClient("set_movement_mode"); - m_localizationModeClient = - m_nh.serviceClient("/rtabmap/set_mode_localization"); - m_mappingModeClient = - m_nh.serviceClient("/rtabmap/set_mode_mapping"); + m_localizationModeClient = m_nh.serviceClient("/rtabmap/set_mode_localization"); + m_mappingModeClient = m_nh.serviceClient("/rtabmap/set_mode_mapping"); - m_changeMapViewClient = - m_nh.serviceClient("change_map_view"); + m_changeMapViewClient = m_nh.serviceClient("change_map_view"); } RosJsonDataHandler::~RosJsonDataHandler() = default; -opentera_webrtc_ros_msgs::Waypoint -RosJsonDataHandler::getWpFromData(const nlohmann::json& data) +opentera_webrtc_ros_msgs::Waypoint RosJsonDataHandler::getWpFromData(const nlohmann::json& data) { opentera_webrtc_ros_msgs::Waypoint wp; wp.x = static_cast(data["coordinate"]["x"]); wp.y = static_cast(data["coordinate"]["y"]); - wp.yaw = - static_cast(static_cast(data["coordinate"]["yaw"]) * M_PI / 180); + wp.yaw = static_cast(static_cast(data["coordinate"]["yaw"]) * M_PI / 180); return wp; } -void RosJsonDataHandler::onWebRTCDataReceived( - const ros::MessageEvent& event) +void RosJsonDataHandler::onWebRTCDataReceived(const ros::MessageEvent& event) { const opentera_webrtc_ros_msgs::PeerData msg = *(event.getMessage()); @@ -77,8 +64,7 @@ void RosJsonDataHandler::onWebRTCDataReceived( geometry_msgs::Twist twist; // Multiply by 0.15 in order to control the speed of the movement twist.linear.x = static_cast(serializedData["x"]) * m_linear_multiplier; - twist.angular.z = - static_cast(serializedData["yaw"]) * m_angular_multiplier; + twist.angular.z = static_cast(serializedData["yaw"]) * m_angular_multiplier; m_cmdVelPublisher.publish(twist); } else if (serializedData["type"] == "waypointArray") @@ -125,8 +111,7 @@ void RosJsonDataHandler::onWebRTCDataReceived( srv.request.data = serializedData["cmd"]; if (!m_setMovementModeClient.call(srv)) { - ROS_ERROR("Set movement mode service call error: %s", - srv.response.message.c_str()); + ROS_ERROR("Set movement mode service call error: %s", srv.response.message.c_str()); } } } @@ -145,8 +130,7 @@ void RosJsonDataHandler::onWebRTCDataReceived( srv.request.data = serializedData["value"]; if (!m_enableCameraClient.call(srv)) { - ROS_ERROR("EnableCamera service call error: %s", - srv.response.message.c_str()); + ROS_ERROR("EnableCamera service call error: %s", srv.response.message.c_str()); } } else if (serializedData["type"] == "changeMapView") @@ -156,8 +140,7 @@ void RosJsonDataHandler::onWebRTCDataReceived( srv.request.view_old = serializedData["old"]; if (!m_changeMapViewClient.call(srv)) { - ROS_ERROR("change_map_view service call error: %s", - srv.response.message.c_str()); + ROS_ERROR("change_map_view service call error: %s", srv.response.message.c_str()); } } else if (serializedData["type"] == "goToLabel") diff --git a/opentera_webrtc_ros/src/RosNodeParameters.cpp b/opentera_webrtc_ros/src/RosNodeParameters.cpp index 86ae1bd..7381fc1 100644 --- a/opentera_webrtc_ros/src/RosNodeParameters.cpp +++ b/opentera_webrtc_ros/src/RosNodeParameters.cpp @@ -18,7 +18,7 @@ bool RosNodeParameters::isStandAlone() * @param clientName Client's name * @param room Room's name */ -void RosNodeParameters::loadSignalingParams(std::string &clientName, std::string &room) +void RosNodeParameters::loadSignalingParams(std::string& clientName, std::string& room) { NodeHandle pnh("~"); @@ -37,12 +37,15 @@ void RosNodeParameters::loadSignalingParams(std::string &clientName, std::string * @param room Room's name * @param password Room's password */ -void RosNodeParameters::loadSignalingParams(std::string &serverUrl, std::string &clientName, - std::string &room, std::string &password) +void RosNodeParameters::loadSignalingParams( + std::string& serverUrl, + std::string& clientName, + std::string& room, + std::string& password) { NodeHandle pnh("~"); - //String parameters + // String parameters std::map dictString; opentera::param::getParam(pnh, "signaling", dictString); serverUrl = isInParams("server_url", dictString) ? dictString["server_url"] : "http://localhost:8080"; @@ -56,11 +59,11 @@ void RosNodeParameters::loadSignalingParams(std::string &serverUrl, std::string * * @param verifySSL Verify SSL peer */ -void RosNodeParameters::loadSignalingParamsVerifySSL(bool &verifySSL) +void RosNodeParameters::loadSignalingParamsVerifySSL(bool& verifySSL) { NodeHandle pnh("~"); - //Bool parameters - std::map dictBool; + // Bool parameters + std::map dictBool; opentera::param::getParam(pnh, "signaling", dictBool); verifySSL = isInParams("verify_ssl", dictBool) ? dictBool["verify_ssl"] : true; } @@ -74,10 +77,10 @@ void RosNodeParameters::loadSignalingParamsVerifySSL(bool &verifySSL) * @param screencast whether the images are a screen capture */ void RosNodeParameters::loadVideoStreamParams( - bool &canSendVideoStream, - bool &canReceiveVideoStream, - bool &denoise, - bool &screencast) + bool& canSendVideoStream, + bool& canReceiveVideoStream, + bool& denoise, + bool& screencast) { NodeHandle pnh("~"); @@ -97,17 +100,17 @@ void RosNodeParameters::loadVideoStreamParams( * @param canReceiveAudioStream whether the node can received audio stream from the signaling server */ void RosNodeParameters::loadAudioStreamParams( - bool &canSendAudioStream, - bool &canReceiveAudioStream, - unsigned int &soundCardTotalDelayMs, - bool &echoCancellation, - bool &autoGainControl, - bool &noiseSuppression, - bool &highPassFilter, - bool &stereoSwapping, - bool &typingDetection, - bool &residualEchoDetector, - bool &transientSuppression) + bool& canSendAudioStream, + bool& canReceiveAudioStream, + unsigned int& soundCardTotalDelayMs, + bool& echoCancellation, + bool& autoGainControl, + bool& noiseSuppression, + bool& highPassFilter, + bool& stereoSwapping, + bool& typingDetection, + bool& residualEchoDetector, + bool& transientSuppression) { NodeHandle pnh("~"); @@ -120,13 +123,14 @@ void RosNodeParameters::loadAudioStreamParams( canSendAudioStream = isInParams("can_send_audio_stream", dict) ? dict["can_send_audio_stream"] : false; canReceiveAudioStream = isInParams("can_receive_audio_stream", dict) ? dict["can_receive_audio_stream"] : false; - soundCardTotalDelayMs= isInParams("sound_card_total_delay_ms", dictInt) ? dictInt["sound_card_total_delay_ms"] : 40; - echoCancellation= isInParams("echo_cancellation", dict) ? dict["echo_cancellation"] : true; - autoGainControl= isInParams("auto_gain_control", dict) ? dict["auto_gain_control"] : true; - noiseSuppression= isInParams("noise_suppression", dict) ? dict["noise_suppression"] : true; - highPassFilter= isInParams("high_pass_filter", dict) ? dict["high_pass_filter"] : false; - stereoSwapping= isInParams("stereo_swapping", dict) ? dict["stereo_swapping"] : false; - typingDetection= isInParams("typing_detection", dict) ? dict["typing_detection"] : false; - residualEchoDetector= isInParams("residual_echo_detector", dict) ? dict["residual_echo_detector"] : true; - transientSuppression= isInParams("transient_suppression", dict) ? dict["transient_suppression"] : true; -} \ No newline at end of file + soundCardTotalDelayMs = + isInParams("sound_card_total_delay_ms", dictInt) ? dictInt["sound_card_total_delay_ms"] : 40; + echoCancellation = isInParams("echo_cancellation", dict) ? dict["echo_cancellation"] : true; + autoGainControl = isInParams("auto_gain_control", dict) ? dict["auto_gain_control"] : true; + noiseSuppression = isInParams("noise_suppression", dict) ? dict["noise_suppression"] : true; + highPassFilter = isInParams("high_pass_filter", dict) ? dict["high_pass_filter"] : false; + stereoSwapping = isInParams("stereo_swapping", dict) ? dict["stereo_swapping"] : false; + typingDetection = isInParams("typing_detection", dict) ? dict["typing_detection"] : false; + residualEchoDetector = isInParams("residual_echo_detector", dict) ? dict["residual_echo_detector"] : true; + transientSuppression = isInParams("transient_suppression", dict) ? dict["transient_suppression"] : true; +} diff --git a/opentera_webrtc_ros/src/RosParamUtils.cpp b/opentera_webrtc_ros/src/RosParamUtils.cpp index 81bf974..f404a3d 100644 --- a/opentera_webrtc_ros/src/RosParamUtils.cpp +++ b/opentera_webrtc_ros/src/RosParamUtils.cpp @@ -39,57 +39,57 @@ namespace opentera */ namespace impl { - template + template T xml_cast(const XmlRpc::XmlRpcValue& xml_value) { return static_cast(xml_value); } - template + template bool xml_castable(int XmlType) { return false; } - template <> + template<> bool xml_castable(int XmlType) { return XmlType == XmlRpc::XmlRpcValue::TypeString; } - template <> + template<> bool xml_castable(int XmlType) { - return (XmlType == XmlRpc::XmlRpcValue::TypeDouble - || XmlType == XmlRpc::XmlRpcValue::TypeInt - || XmlType == XmlRpc::XmlRpcValue::TypeBoolean); + return ( + XmlType == XmlRpc::XmlRpcValue::TypeDouble || XmlType == XmlRpc::XmlRpcValue::TypeInt || + XmlType == XmlRpc::XmlRpcValue::TypeBoolean); } - template <> + template<> bool xml_castable(int XmlType) { - return (XmlType == XmlRpc::XmlRpcValue::TypeDouble - || XmlType == XmlRpc::XmlRpcValue::TypeInt - || XmlType == XmlRpc::XmlRpcValue::TypeBoolean); + return ( + XmlType == XmlRpc::XmlRpcValue::TypeDouble || XmlType == XmlRpc::XmlRpcValue::TypeInt || + XmlType == XmlRpc::XmlRpcValue::TypeBoolean); } - template <> + template<> bool xml_castable(int XmlType) { - return (XmlType == XmlRpc::XmlRpcValue::TypeDouble - || XmlType == XmlRpc::XmlRpcValue::TypeInt - || XmlType == XmlRpc::XmlRpcValue::TypeBoolean); + return ( + XmlType == XmlRpc::XmlRpcValue::TypeDouble || XmlType == XmlRpc::XmlRpcValue::TypeInt || + XmlType == XmlRpc::XmlRpcValue::TypeBoolean); } - template <> + template<> bool xml_castable(int XmlType) { - return (XmlType == XmlRpc::XmlRpcValue::TypeDouble - || XmlType == XmlRpc::XmlRpcValue::TypeInt - || XmlType == XmlRpc::XmlRpcValue::TypeBoolean); + return ( + XmlType == XmlRpc::XmlRpcValue::TypeDouble || XmlType == XmlRpc::XmlRpcValue::TypeInt || + XmlType == XmlRpc::XmlRpcValue::TypeBoolean); } - template <> + template<> double xml_cast(const XmlRpc::XmlRpcValue& xml_value) { using namespace XmlRpc; @@ -106,7 +106,7 @@ namespace opentera }; } - template <> + template<> float xml_cast(const XmlRpc::XmlRpcValue& xml_value) { using namespace XmlRpc; @@ -123,7 +123,7 @@ namespace opentera }; } - template <> + template<> int xml_cast(const XmlRpc::XmlRpcValue& xml_value) { using namespace XmlRpc; @@ -140,7 +140,7 @@ namespace opentera }; } - template <> + template<> bool xml_cast(const XmlRpc::XmlRpcValue& xml_value) { using namespace XmlRpc; @@ -157,9 +157,12 @@ namespace opentera }; } - template - bool getImpl(const NodeHandle& nh, const std::string& key, - std::map& map, bool cached = false) + template + bool getImpl( + const NodeHandle& nh, + const std::string& key, + std::map& map, + bool cached = false) { XmlRpc::XmlRpcValue xml_value; if (cached) @@ -197,9 +200,8 @@ namespace opentera return true; } - template - bool getImpl(const NodeHandle& nh, const std::string& key, - std::vector& vec, bool cached = false) + template + bool getImpl(const NodeHandle& nh, const std::string& key, std::vector& vec, bool cached = false) { XmlRpc::XmlRpcValue xml_array; if (cached) @@ -224,13 +226,12 @@ namespace opentera } // Resize the target vector (destructive) - std::size_t count = - std::accumulate(std::begin(xml_array), std::end(xml_array), 0, - [](const auto& a, const auto& b) { - return a - + static_cast( - xml_castable(b.second.getType())); - }); + std::size_t count = std::accumulate( + std::begin(xml_array), + std::end(xml_array), + 0, + [](const auto& a, const auto& b) + { return a + static_cast(xml_castable(b.second.getType())); }); vec.reserve(count); // Fill the vector with stuff @@ -248,65 +249,53 @@ namespace opentera } } - bool getParam(const NodeHandle& nh, const std::string& key, - std::map& map) + bool getParam(const NodeHandle& nh, const std::string& key, std::map& map) { return impl::getImpl(nh, key, map, false); } - bool getParam(const NodeHandle& nh, const std::string& key, - std::map& map) + bool getParam(const NodeHandle& nh, const std::string& key, std::map& map) { return impl::getImpl(nh, key, map, false); } - bool getParam(const NodeHandle& nh, const std::string& key, - std::map& map) + bool getParam(const NodeHandle& nh, const std::string& key, std::map& map) { return impl::getImpl(nh, key, map, false); } - bool getParam(const NodeHandle& nh, const std::string& key, - std::map& map) + bool getParam(const NodeHandle& nh, const std::string& key, std::map& map) { return impl::getImpl(nh, key, map, false); } - bool getParam(const NodeHandle& nh, const std::string& key, - std::map& map) + bool getParam(const NodeHandle& nh, const std::string& key, std::map& map) { return impl::getImpl(nh, key, map, false); } - bool getParamCached(const NodeHandle& nh, const std::string& key, - std::map& map) + bool getParamCached(const NodeHandle& nh, const std::string& key, std::map& map) { return impl::getImpl(nh, key, map, true); } - bool getParamCached(const NodeHandle& nh, const std::string& key, - std::map& map) + bool getParamCached(const NodeHandle& nh, const std::string& key, std::map& map) { return impl::getImpl(nh, key, map, true); } - bool getParamCached(const NodeHandle& nh, const std::string& key, - std::map& map) + bool getParamCached(const NodeHandle& nh, const std::string& key, std::map& map) { return impl::getImpl(nh, key, map, true); } - bool getParamCached(const NodeHandle& nh, const std::string& key, - std::map& map) + bool getParamCached(const NodeHandle& nh, const std::string& key, std::map& map) { return impl::getImpl(nh, key, map, true); } - bool getParamCached(const NodeHandle& nh, const std::string& key, - std::map& map) + bool getParamCached(const NodeHandle& nh, const std::string& key, std::map& map) { return impl::getImpl(nh, key, map, true); } - bool getParam(const NodeHandle& nh, const std::string& key, - std::vector& vec) + bool getParam(const NodeHandle& nh, const std::string& key, std::vector& vec) { return impl::getImpl(nh, key, vec, false); } - bool getParam(const NodeHandle& nh, const std::string& key, - std::vector& vec) + bool getParam(const NodeHandle& nh, const std::string& key, std::vector& vec) { return impl::getImpl(nh, key, vec, false); } @@ -314,39 +303,32 @@ namespace opentera { return impl::getImpl(nh, key, vec, false); } - bool getParam(const NodeHandle& nh, const std::string& key, - std::vector& vec) + bool getParam(const NodeHandle& nh, const std::string& key, std::vector& vec) { return impl::getImpl(nh, key, vec, false); } - bool getParam(const NodeHandle& nh, const std::string& key, - std::vector& vec) + bool getParam(const NodeHandle& nh, const std::string& key, std::vector& vec) { return impl::getImpl(nh, key, vec, false); } - bool getParamCached(const NodeHandle& nh, const std::string& key, - std::vector& vec) + bool getParamCached(const NodeHandle& nh, const std::string& key, std::vector& vec) { return impl::getImpl(nh, key, vec, true); } - bool getParamCached(const NodeHandle& nh, const std::string& key, - std::vector& vec) + bool getParamCached(const NodeHandle& nh, const std::string& key, std::vector& vec) { return impl::getImpl(nh, key, vec, true); } - bool getParamCached(const NodeHandle& nh, const std::string& key, - std::vector& vec) + bool getParamCached(const NodeHandle& nh, const std::string& key, std::vector& vec) { return impl::getImpl(nh, key, vec, true); } - bool getParamCached(const NodeHandle& nh, const std::string& key, - std::vector& vec) + bool getParamCached(const NodeHandle& nh, const std::string& key, std::vector& vec) { return impl::getImpl(nh, key, vec, true); } - bool getParamCached(const NodeHandle& nh, const std::string& key, - std::vector& vec) + bool getParamCached(const NodeHandle& nh, const std::string& key, std::vector& vec) { return impl::getImpl(nh, key, vec, true); } diff --git a/opentera_webrtc_ros/src/RosSignalingServerConfiguration.cpp b/opentera_webrtc_ros/src/RosSignalingServerConfiguration.cpp index 7162c9f..5e395d7 100644 --- a/opentera_webrtc_ros/src/RosSignalingServerConfiguration.cpp +++ b/opentera_webrtc_ros/src/RosSignalingServerConfiguration.cpp @@ -71,7 +71,7 @@ std::string RosSignalingServerConfiguration::getQueryFrom(const std::string& que * @param url The full url to extract from. * @return std::string The ice server url. */ -std::string RosSignalingServerConfiguration::getIceServerUrl(const std::string &url) +std::string RosSignalingServerConfiguration::getIceServerUrl(const std::string& url) { ROS_INFO_STREAM("getIceServerUrl from url:" << url); return RosSignalingServerConfiguration::getBaseUrl(url) + "/iceservers"; @@ -83,21 +83,18 @@ std::string RosSignalingServerConfiguration::getIceServerUrl(const std::string & * @param url The full url to extract from. * @return std::string The base url (up to the last / of the path). */ -std::string RosSignalingServerConfiguration::getBaseUrl(const std::string &url) +std::string RosSignalingServerConfiguration::getBaseUrl(const std::string& url) { const string prot_end("://"); - string::const_iterator prot_i = search(url.begin(), url.end(), - prot_end.begin(), prot_end.end()); + string::const_iterator prot_i = search(url.begin(), url.end(), prot_end.begin(), prot_end.end()); std::string protocol; protocol.reserve(distance(url.begin(), prot_i)); - transform(url.begin(), prot_i, - back_inserter(protocol), - ptr_fun(tolower)); // protocol is icase + transform(url.begin(), prot_i, back_inserter(protocol), ptr_fun(tolower)); // protocol is icase - if( prot_i == url.end() ) + if (prot_i == url.end()) { ROS_ERROR_STREAM("No protocol defined in url: " << url); return url; @@ -118,9 +115,7 @@ std::string RosSignalingServerConfiguration::getBaseUrl(const std::string &url) std::string host; host.reserve(distance(prot_i, path_i)); - transform(prot_i, path_i, - back_inserter(host), - ptr_fun(tolower)); // host is icase + transform(prot_i, path_i, back_inserter(host), ptr_fun(tolower)); // host is icase return protocol + prot_end + host; -} \ No newline at end of file +} diff --git a/opentera_webrtc_ros/src/RosStreamBridge.cpp b/opentera_webrtc_ros/src/RosStreamBridge.cpp index 8dc6fd0..6eb2114 100644 --- a/opentera_webrtc_ros/src/RosStreamBridge.cpp +++ b/opentera_webrtc_ros/src/RosStreamBridge.cpp @@ -20,7 +20,10 @@ using namespace audio_utils; /** * @brief construct a topic streamer node */ -RosStreamBridge::RosStreamBridge(const ros::NodeHandle& nh): RosWebRTCBridge(nh), m_videoSource(nullptr), m_audioSource(nullptr) +RosStreamBridge::RosStreamBridge(const ros::NodeHandle& nh) + : RosWebRTCBridge(nh), + m_videoSource(nullptr), + m_audioSource(nullptr) { if (RosNodeParameters::isStandAlone()) { @@ -34,7 +37,7 @@ RosStreamBridge::RosStreamBridge(const ros::NodeHandle& nh): RosWebRTCBridge(nh) * * @param signalingServerConfiguration Signaling server configuration */ -void RosStreamBridge::init(const opentera::SignalingServerConfiguration &signalingServerConfiguration) +void RosStreamBridge::init(const opentera::SignalingServerConfiguration& signalingServerConfiguration) { bool needsDenoising, isScreencast; unsigned int soundCardTotalDelayMs; @@ -48,7 +51,8 @@ void RosStreamBridge::init(const opentera::SignalingServerConfiguration &signali bool transientSuppression; // Load ROS parameters - RosNodeParameters::loadAudioStreamParams(m_canSendAudioStream, + RosNodeParameters::loadAudioStreamParams( + m_canSendAudioStream, m_canReceiveAudioStream, soundCardTotalDelayMs, echoCancellation, @@ -61,7 +65,11 @@ void RosStreamBridge::init(const opentera::SignalingServerConfiguration &signali transientSuppression); - RosNodeParameters::loadVideoStreamParams( m_canSendVideoStream, m_canReceiveVideoStream, needsDenoising, isScreencast); + RosNodeParameters::loadVideoStreamParams( + m_canSendVideoStream, + m_canReceiveVideoStream, + needsDenoising, + isScreencast); // WebRTC video stream interfaces m_videoSource = make_shared(needsDenoising, isScreencast); @@ -79,18 +87,17 @@ void RosStreamBridge::init(const opentera::SignalingServerConfiguration &signali string iceServersUrl = RosSignalingServerConfiguration::getIceServerUrl(signalingServerConfiguration.url()); ROS_INFO("RosStreamBridge Fetching ice servers from : %s", iceServersUrl.c_str()); vector iceServers; - if (!IceServer::fetchFromServer(iceServersUrl, - signalingServerConfiguration.password(), iceServers)) + if (!IceServer::fetchFromServer(iceServersUrl, signalingServerConfiguration.password(), iceServers)) { ROS_ERROR("RosStreamBridge Error fetching ice servers from %s", iceServersUrl.c_str()); iceServers.clear(); } m_signalingClient = make_unique( - signalingServerConfiguration, - WebrtcConfiguration::create(iceServers), - (m_canReceiveVideoStream || m_canSendVideoStream ? m_videoSource : nullptr), - (m_canReceiveAudioStream || m_canSendAudioStream ? m_audioSource : nullptr)); + signalingServerConfiguration, + WebrtcConfiguration::create(iceServers), + (m_canReceiveVideoStream || m_canSendVideoStream ? m_videoSource : nullptr), + (m_canReceiveAudioStream || m_canSendAudioStream ? m_audioSource : nullptr)); bool verifySSL; @@ -100,18 +107,24 @@ void RosStreamBridge::init(const opentera::SignalingServerConfiguration &signali if (m_canReceiveAudioStream || m_canReceiveVideoStream) { // Stream event - m_signalingClient->setOnAddRemoteStream([this](const Client& client) - { - publishPeerStatus(client, PeerStatus::STATUS_REMOTE_STREAM_ADDED); - ROS_INFO_STREAM(nodeName << " --> " - << "RosStreamBridge Signaling on add remote stream: " << "id: " << client.id() << ", name: " << client.name()); - }); - m_signalingClient->setOnRemoveRemoteStream([this](const Client& client) - { - publishPeerStatus(client, PeerStatus::STATUS_REMOTE_STREAM_REMOVED); - ROS_INFO_STREAM(nodeName << " --> " - << "RosStreamBridge Signaling on remove remote stream: " << "id: " << client.id() << ", name: " << client.name()); - }); + m_signalingClient->setOnAddRemoteStream( + [this](const Client& client) + { + publishPeerStatus(client, PeerStatus::STATUS_REMOTE_STREAM_ADDED); + ROS_INFO_STREAM( + nodeName << " --> " + << "RosStreamBridge Signaling on add remote stream: " + << "id: " << client.id() << ", name: " << client.name()); + }); + m_signalingClient->setOnRemoveRemoteStream( + [this](const Client& client) + { + publishPeerStatus(client, PeerStatus::STATUS_REMOTE_STREAM_REMOVED); + ROS_INFO_STREAM( + nodeName << " --> " + << "RosStreamBridge Signaling on remove remote stream: " + << "id: " << client.id() << ", name: " << client.name()); + }); if (m_canReceiveAudioStream) { @@ -119,25 +132,26 @@ void RosStreamBridge::init(const opentera::SignalingServerConfiguration &signali m_mixedAudioPublisher = m_nh.advertise("audio_mixed", 100, false); m_signalingClient->setOnAudioFrameReceived( - [this](auto&& PH1, auto&& PH2, auto&& PH3, auto&& PH4, auto&& PH5, - auto&& PH6) + [this](auto&& PH1, auto&& PH2, auto&& PH3, auto&& PH4, auto&& PH5, auto&& PH6) { - onAudioFrameReceived(std::forward(PH1), - std::forward(PH2), - std::forward(PH3), - std::forward(PH4), - std::forward(PH5), - std::forward(PH6)); + onAudioFrameReceived( + std::forward(PH1), + std::forward(PH2), + std::forward(PH3), + std::forward(PH4), + std::forward(PH5), + std::forward(PH6)); }); m_signalingClient->setOnMixedAudioFrameReceived( [this](auto&& PH1, auto&& PH2, auto&& PH3, auto&& PH4, auto&& PH5) { - onMixedAudioFrameReceived(std::forward(PH1), - std::forward(PH2), - std::forward(PH3), - std::forward(PH4), - std::forward(PH5)); + onMixedAudioFrameReceived( + std::forward(PH1), + std::forward(PH2), + std::forward(PH3), + std::forward(PH4), + std::forward(PH5)); }); } @@ -148,18 +162,19 @@ void RosStreamBridge::init(const opentera::SignalingServerConfiguration &signali m_signalingClient->setOnVideoFrameReceived( [this](auto&& PH1, auto&& PH2, auto&& PH3) { - onVideoFrameReceived(std::forward(PH1), - std::forward(PH2), - std::forward(PH3)); + onVideoFrameReceived( + std::forward(PH1), + std::forward(PH2), + std::forward(PH3)); }); } } } -void RosStreamBridge::onJoinSessionEvents(const std::vector &events) +void RosStreamBridge::onJoinSessionEvents(const std::vector& events) { - //Already in a session ? - //Should disconnect + // Already in a session ? + // Should disconnect disconnect(); ROS_INFO_STREAM(nodeName << " onJoinSessionEvents " << events[0].session_url); @@ -169,7 +184,7 @@ void RosStreamBridge::onJoinSessionEvents(const std::vector &events) +void RosStreamBridge::onStopSessionEvents(const std::vector& events) { disconnect(); } @@ -180,36 +195,32 @@ void RosStreamBridge::onSignalingConnectionOpened() if (m_canSendAudioStream) { - //Audio - m_audioSubscriber = m_nh.subscribe( - "audio_in", - 1, - &RosStreamBridge::audioCallback, - this); + // Audio + m_audioSubscriber = m_nh.subscribe("audio_in", 1, &RosStreamBridge::audioCallback, this); } if (m_canSendVideoStream) { - //Video - m_imageSubscriber = m_nh.subscribe( - "ros_image", - 1, - &RosStreamBridge::imageCallback, - this); + // Video + m_imageSubscriber = m_nh.subscribe("ros_image", 1, &RosStreamBridge::imageCallback, this); } } void RosStreamBridge::onSignalingConnectionClosed() { RosWebRTCBridge::onSignalingConnectionClosed(); - ROS_ERROR_STREAM(nodeName << " --> " << "RosStreamBridge Signaling connection closed, shutting down..."); + ROS_ERROR_STREAM( + nodeName << " --> " + << "RosStreamBridge Signaling connection closed, shutting down..."); ros::requestShutdown(); } void RosStreamBridge::onSignalingConnectionError(const std::string& msg) { RosWebRTCBridge::onSignalingConnectionError(msg); - ROS_ERROR_STREAM(nodeName << " --> " << "RosStreamBridge Signaling connection error " << msg.c_str() << ", shutting down..."); + ROS_ERROR_STREAM( + nodeName << " --> " + << "RosStreamBridge Signaling connection error " << msg.c_str() << ", shutting down..."); ros::requestShutdown(); } @@ -240,41 +251,44 @@ void RosStreamBridge::onVideoFrameReceived(const Client& client, const cv::Mat& * @param numberOfChannels * @param numberOfFrames */ -void RosStreamBridge::onAudioFrameReceived(const Client& client, +void RosStreamBridge::onAudioFrameReceived( + const Client& client, const void* audioData, int bitsPerSample, int sampleRate, size_t numberOfChannels, size_t numberOfFrames) { - audio_utils::AudioFrame frame = createAudioFrame(audioData, bitsPerSample, sampleRate, - numberOfChannels, numberOfFrames); + audio_utils::AudioFrame frame = + createAudioFrame(audioData, bitsPerSample, sampleRate, numberOfChannels, numberOfFrames); publishPeerFrame(m_audioPublisher, client, frame); } /** * @brief Format a AudioFrame message for publishing - * @param audioData + * @param audioData * @param bitsPerSample format of the sample * @param sampleRate Sampling frequency * @param numberOfChannels * @param numberOfFrames */ -void RosStreamBridge::onMixedAudioFrameReceived(const void* audioData, +void RosStreamBridge::onMixedAudioFrameReceived( + const void* audioData, int bitsPerSample, int sampleRate, size_t numberOfChannels, size_t numberOfFrames) { - m_mixedAudioPublisher.publish(createAudioFrame(audioData, bitsPerSample, sampleRate, - numberOfChannels, numberOfFrames)); + m_mixedAudioPublisher.publish( + createAudioFrame(audioData, bitsPerSample, sampleRate, numberOfChannels, numberOfFrames)); } -audio_utils::AudioFrame RosStreamBridge::createAudioFrame(const void* audioData, - int bitsPerSample, - int sampleRate, - size_t numberOfChannels, - size_t numberOfFrames) +audio_utils::AudioFrame RosStreamBridge::createAudioFrame( + const void* audioData, + int bitsPerSample, + int sampleRate, + size_t numberOfChannels, + size_t numberOfFrames) { audio_utils::AudioFrame frame; frame.format = "signed_" + to_string(bitsPerSample); @@ -319,8 +333,12 @@ int main(int argc, char** argv) init(argc, argv, "stream_bridge"); ros::NodeHandle nh; - ROS_INFO_STREAM(ros::this_node::getName() << " --> " << "starting..."); + ROS_INFO_STREAM( + ros::this_node::getName() << " --> " + << "starting..."); RosStreamBridge node(nh); node.run(); - ROS_INFO_STREAM(ros::this_node::getName()<< " --> " << "done..."); + ROS_INFO_STREAM( + ros::this_node::getName() << " --> " + << "done..."); } diff --git a/opentera_webrtc_ros/src/RosVideoSource.cpp b/opentera_webrtc_ros/src/RosVideoSource.cpp index 6bcf512..8743505 100644 --- a/opentera_webrtc_ros/src/RosVideoSource.cpp +++ b/opentera_webrtc_ros/src/RosVideoSource.cpp @@ -14,8 +14,8 @@ using namespace opentera; * @param needsDenoising denoising should be applied to the video stream by the image transport layer * @param isScreenCast the transport layer should be configured to stream a screen rather then a camera */ -RosVideoSource::RosVideoSource(bool needsDenoising, bool isScreenCast): - VideoSource(VideoSourceConfiguration::create(needsDenoising, isScreenCast)) +RosVideoSource::RosVideoSource(bool needsDenoising, bool isScreenCast) + : VideoSource(VideoSourceConfiguration::create(needsDenoising, isScreenCast)) { } diff --git a/opentera_webrtc_ros_msgs/CMakeLists.txt b/opentera_webrtc_ros_msgs/CMakeLists.txt index d3e69aa..4d39ba9 100644 --- a/opentera_webrtc_ros_msgs/CMakeLists.txt +++ b/opentera_webrtc_ros_msgs/CMakeLists.txt @@ -2,60 +2,60 @@ cmake_minimum_required(VERSION 3.0.2) project(opentera_webrtc_ros_msgs) find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - geometry_msgs - message_generation - sensor_msgs - audio_utils + roscpp + rospy + std_msgs + geometry_msgs + message_generation + sensor_msgs + audio_utils ) add_message_files( - FILES - DatabaseEvent.msg - DeviceEvent.msg - JoinSessionEvent.msg - JoinSessionReplyEvent.msg - LabelArray.msg - LabelEdit.msg - Label.msg - LabelSimple.msg - LabelSimpleArray.msg - LabelSimpleEdit.msg - LeaveSessionEvent.msg - LogEvent.msg - ParticipantEvent.msg - StopSessionEvent.msg - UserEvent.msg - OpenTeraEvent.msg - Peer.msg - PeerData.msg - PeerImage.msg - PeerAudio.msg - PeerStatus.msg - RobotStatus.msg - Waypoint.msg - WaypointArray.msg + FILES + DatabaseEvent.msg + DeviceEvent.msg + JoinSessionEvent.msg + JoinSessionReplyEvent.msg + LabelArray.msg + LabelEdit.msg + Label.msg + LabelSimple.msg + LabelSimpleArray.msg + LabelSimpleEdit.msg + LeaveSessionEvent.msg + LogEvent.msg + ParticipantEvent.msg + StopSessionEvent.msg + UserEvent.msg + OpenTeraEvent.msg + Peer.msg + PeerData.msg + PeerImage.msg + PeerAudio.msg + PeerStatus.msg + RobotStatus.msg + Waypoint.msg + WaypointArray.msg ) add_service_files( - FILES - SetString.srv + FILES + SetString.srv ) generate_messages( - DEPENDENCIES - std_msgs - sensor_msgs - audio_utils - geometry_msgs + DEPENDENCIES + std_msgs + sensor_msgs + audio_utils + geometry_msgs ) catkin_package( - CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs message_runtime + CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs message_runtime ) include_directories( - ${catkin_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} ) diff --git a/opentera_webrtc_ros_msgs/msg/DatabaseEvent.msg b/opentera_webrtc_ros_msgs/msg/DatabaseEvent.msg index f4e20f6..7e13171 100644 --- a/opentera_webrtc_ros_msgs/msg/DatabaseEvent.msg +++ b/opentera_webrtc_ros_msgs/msg/DatabaseEvent.msg @@ -3,7 +3,7 @@ int32 DB_UNKNOWN=0 int32 DB_CREATE=1 int32 DB_UPDATE=2 int32 DB_DELETE=3 - + int32 type string object_type string object_value diff --git a/opentera_webrtc_ros_msgs/msg/DeviceEvent.msg b/opentera_webrtc_ros_msgs/msg/DeviceEvent.msg index 855631e..e355eec 100644 --- a/opentera_webrtc_ros_msgs/msg/DeviceEvent.msg +++ b/opentera_webrtc_ros_msgs/msg/DeviceEvent.msg @@ -7,7 +7,7 @@ int32 DEVICE_ADDED=4 int32 DEVICE_JOINED_SESSION=5 int32 DEVICE_LEFT_SESSION=6 int32 DEVICE_STATUS_CHANGED=7 - + string device_uuid int32 type string device_name diff --git a/opentera_webrtc_ros_msgs/msg/JoinSessionReplyEvent.msg b/opentera_webrtc_ros_msgs/msg/JoinSessionReplyEvent.msg index 3a39df7..d5d9fa2 100644 --- a/opentera_webrtc_ros_msgs/msg/JoinSessionReplyEvent.msg +++ b/opentera_webrtc_ros_msgs/msg/JoinSessionReplyEvent.msg @@ -4,7 +4,7 @@ int32 REPLY_ACCEPTED=1 int32 REPLY_DENIED=2 int32 REPLY_BUSY=3 int32 REPLY_TIMEOUT=4 - + string session_uuid string user_uuid string participant_uuid diff --git a/opentera_webrtc_ros_msgs/msg/LogEvent.msg b/opentera_webrtc_ros_msgs/msg/LogEvent.msg index d067330..1890e64 100644 --- a/opentera_webrtc_ros_msgs/msg/LogEvent.msg +++ b/opentera_webrtc_ros_msgs/msg/LogEvent.msg @@ -7,7 +7,7 @@ int32 LOGLEVEL_WARNING=4 int32 LOGLEVEL_CRITICAL=3 int32 LOGLEVEL_ERROR=2 int32 LOGLEVEL_FATAL=1 - + int32 level float64 timestamp string sender diff --git a/opentera_webrtc_ros_msgs/msg/OpenTeraEvent.msg b/opentera_webrtc_ros_msgs/msg/OpenTeraEvent.msg index 008e637..e562a7d 100644 --- a/opentera_webrtc_ros_msgs/msg/OpenTeraEvent.msg +++ b/opentera_webrtc_ros_msgs/msg/OpenTeraEvent.msg @@ -8,4 +8,3 @@ opentera_webrtc_ros_msgs/LogEvent[] log_events opentera_webrtc_ros_msgs/ParticipantEvent[] participant_events opentera_webrtc_ros_msgs/StopSessionEvent[] stop_session_events opentera_webrtc_ros_msgs/UserEvent[] user_events - diff --git a/opentera_webrtc_ros_msgs/msg/Peer.msg b/opentera_webrtc_ros_msgs/msg/Peer.msg index ff7d867..6959aa2 100644 --- a/opentera_webrtc_ros_msgs/msg/Peer.msg +++ b/opentera_webrtc_ros_msgs/msg/Peer.msg @@ -1,2 +1,2 @@ string id -string name \ No newline at end of file +string name diff --git a/opentera_webrtc_ros_msgs/msg/PeerAudio.msg b/opentera_webrtc_ros_msgs/msg/PeerAudio.msg index d0a1e9c..e39e430 100644 --- a/opentera_webrtc_ros_msgs/msg/PeerAudio.msg +++ b/opentera_webrtc_ros_msgs/msg/PeerAudio.msg @@ -1,2 +1,2 @@ Peer sender -audio_utils/AudioFrame frame \ No newline at end of file +audio_utils/AudioFrame frame diff --git a/opentera_webrtc_ros_msgs/msg/PeerData.msg b/opentera_webrtc_ros_msgs/msg/PeerData.msg index 8d5be81..3270842 100644 --- a/opentera_webrtc_ros_msgs/msg/PeerData.msg +++ b/opentera_webrtc_ros_msgs/msg/PeerData.msg @@ -1,2 +1,2 @@ Peer sender -string data \ No newline at end of file +string data diff --git a/opentera_webrtc_ros_msgs/msg/PeerStatus.msg b/opentera_webrtc_ros_msgs/msg/PeerStatus.msg index 61016bf..f270dc3 100644 --- a/opentera_webrtc_ros_msgs/msg/PeerStatus.msg +++ b/opentera_webrtc_ros_msgs/msg/PeerStatus.msg @@ -4,4 +4,4 @@ int32 STATUS_REMOTE_STREAM_ADDED=2 int32 STATUS_REMOTE_STREAM_REMOVED=3 Peer sender -int32 status \ No newline at end of file +int32 status diff --git a/opentera_webrtc_ros_msgs/msg/RobotStatus.msg b/opentera_webrtc_ros_msgs/msg/RobotStatus.msg index bde93ed..7f7d8f3 100644 --- a/opentera_webrtc_ros_msgs/msg/RobotStatus.msg +++ b/opentera_webrtc_ros_msgs/msg/RobotStatus.msg @@ -4,7 +4,7 @@ Header header bool is_charging float32 battery_voltage float32 battery_current -float32 battery_level # 0 - 100 +float32 battery_level # 0 - 100 float32 cpu_usage float32 mem_usage float32 disk_usage diff --git a/opentera_webrtc_ros_msgs/msg/UserEvent.msg b/opentera_webrtc_ros_msgs/msg/UserEvent.msg index 664e047..38b6613 100644 --- a/opentera_webrtc_ros_msgs/msg/UserEvent.msg +++ b/opentera_webrtc_ros_msgs/msg/UserEvent.msg @@ -7,7 +7,7 @@ int32 USER_ADDED=4 int32 USER_UPDATED=5 int32 USER_JOINED_SESSION=6 int32 USER_LEFT_SESSION=7 - + string user_uuid int32 type string user_fullname diff --git a/opentera_webrtc_ros_msgs/msg/Waypoint.msg b/opentera_webrtc_ros_msgs/msg/Waypoint.msg index bfa5c4c..180007e 100644 --- a/opentera_webrtc_ros_msgs/msg/Waypoint.msg +++ b/opentera_webrtc_ros_msgs/msg/Waypoint.msg @@ -1,3 +1,3 @@ float32 x float32 y -float32 yaw \ No newline at end of file +float32 yaw diff --git a/opentera_webrtc_ros_msgs/msg/WaypointArray.msg b/opentera_webrtc_ros_msgs/msg/WaypointArray.msg index 30098a8..7f4c233 100644 --- a/opentera_webrtc_ros_msgs/msg/WaypointArray.msg +++ b/opentera_webrtc_ros_msgs/msg/WaypointArray.msg @@ -1 +1 @@ -opentera_webrtc_ros_msgs/Waypoint[] waypoints \ No newline at end of file +opentera_webrtc_ros_msgs/Waypoint[] waypoints diff --git a/opentera_webrtc_ros_msgs/package.xml b/opentera_webrtc_ros_msgs/package.xml index 5a612d3..7af37e3 100644 --- a/opentera_webrtc_ros_msgs/package.xml +++ b/opentera_webrtc_ros_msgs/package.xml @@ -1,72 +1,72 @@ - opentera_webrtc_ros_msgs - 0.0.0 - The opentera_webrtc_ros_msgs package + opentera_webrtc_ros_msgs + 0.0.0 + The opentera_webrtc_ros_msgs package - - - - Dominic Létourneau + + + + Dominic Létourneau - - - - Apache-2.0 + + + + Apache-2.0 - - - - + + + + - - - - + + + + - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - geometry_msgs - message_generation - roscpp - rospy - std_msgs - geometry_msgs - roscpp - rospy - std_msgs - geometry_msgs - message_runtime + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + geometry_msgs + message_generation + roscpp + rospy + std_msgs + geometry_msgs + roscpp + rospy + std_msgs + geometry_msgs + message_runtime - - - + + + - + diff --git a/turtlebot3_beam_description/CHANGELOG.rst b/turtlebot3_beam_description/CHANGELOG.rst index 34456fb..9df647e 100644 --- a/turtlebot3_beam_description/CHANGELOG.rst +++ b/turtlebot3_beam_description/CHANGELOG.rst @@ -58,7 +58,7 @@ Changelog for package turtlebot3_description 0.2.0 (2018-03-12) ------------------ * added waffle pi model (urdf and gazebo) -* modified topic of gazebo plugin +* modified topic of gazebo plugin * refactoring for release * modified r200 tf tree * modified gazebo imu link diff --git a/turtlebot3_beam_description/CMakeLists.txt b/turtlebot3_beam_description/CMakeLists.txt index aa2757e..ea44ace 100644 --- a/turtlebot3_beam_description/CMakeLists.txt +++ b/turtlebot3_beam_description/CMakeLists.txt @@ -8,8 +8,8 @@ project(turtlebot3_beam_description) # Find catkin packages and libraries for catkin and system dependencies ################################################################################ find_package(catkin REQUIRED COMPONENTS - urdf - xacro + urdf + xacro ) ################################################################################ @@ -28,21 +28,21 @@ find_package(catkin REQUIRED COMPONENTS # Declare catkin specific configuration to be passed to dependent projects ################################################################################ catkin_package( - CATKIN_DEPENDS urdf xacro + CATKIN_DEPENDS urdf xacro ) ################################################################################ # Build ################################################################################ include_directories( - ${catkin_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} ) ################################################################################ # Install ################################################################################ install(DIRECTORY meshes rviz urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) ################################################################################ diff --git a/turtlebot3_beam_description/package.xml b/turtlebot3_beam_description/package.xml index 10a01c5..f87be39 100644 --- a/turtlebot3_beam_description/package.xml +++ b/turtlebot3_beam_description/package.xml @@ -1,20 +1,20 @@ - turtlebot3_beam_description - 1.2.5 - - 3D models of the TurtleBot3 for simulation and visualization - - Apache 2.0 - Pyo - Darby Lim - Gilbert - Will Son - http://wiki.ros.org/turtlebot3_description - http://turtlebot3.robotis.com - https://github.com/ROBOTIS-GIT/turtlebot3 - https://github.com/ROBOTIS-GIT/turtlebot3/issues - catkin - urdf - xacro + turtlebot3_beam_description + 1.2.5 + + 3D models of the TurtleBot3 for simulation and visualization + + Apache 2.0 + Pyo + Darby Lim + Gilbert + Will Son + http://wiki.ros.org/turtlebot3_description + http://turtlebot3.robotis.com + https://github.com/ROBOTIS-GIT/turtlebot3 + https://github.com/ROBOTIS-GIT/turtlebot3/issues + catkin + urdf + xacro diff --git a/turtlebot3_beam_description/rviz/model.rviz b/turtlebot3_beam_description/rviz/model.rviz index 85b4c0f..3cd306e 100644 --- a/turtlebot3_beam_description/rviz/model.rviz +++ b/turtlebot3_beam_description/rviz/model.rviz @@ -165,16 +165,11 @@ Visualization Manager: odom: base_footprint: base_link: - base_scan: - {} - caster_back_link: - {} - imu_link: - {} - wheel_left_link: - {} - wheel_right_link: - {} + base_scan: {} + caster_back_link: {} + imu_link: {} + wheel_left_link: {} + wheel_right_link: {} Update Interval: 0 Value: true Enabled: true diff --git a/turtlebot3_beam_description/urdf/accessories/camera.urdf.xacro b/turtlebot3_beam_description/urdf/accessories/camera.urdf.xacro index 119b8cb..76e24bd 100644 --- a/turtlebot3_beam_description/urdf/accessories/camera.urdf.xacro +++ b/turtlebot3_beam_description/urdf/accessories/camera.urdf.xacro @@ -1,7 +1,7 @@ - - + + @@ -27,8 +27,8 @@ - - + + 30.0 @@ -65,7 +65,7 @@ - + - - \ No newline at end of file + + diff --git a/turtlebot3_beam_description/urdf/accessories/r200.urdf.xacro b/turtlebot3_beam_description/urdf/accessories/r200.urdf.xacro index 195dee2..a4c93f1 100644 --- a/turtlebot3_beam_description/urdf/accessories/r200.urdf.xacro +++ b/turtlebot3_beam_description/urdf/accessories/r200.urdf.xacro @@ -102,4 +102,4 @@ - \ No newline at end of file + diff --git a/turtlebot3_beam_description/urdf/common_properties.xacro b/turtlebot3_beam_description/urdf/common_properties.xacro index b26a853..6f46676 100644 --- a/turtlebot3_beam_description/urdf/common_properties.xacro +++ b/turtlebot3_beam_description/urdf/common_properties.xacro @@ -2,7 +2,7 @@ - + diff --git a/turtlebot3_beam_description/urdf/turtlebot3_waffle_for_open_manipulator.urdf.xacro b/turtlebot3_beam_description/urdf/turtlebot3_waffle_for_open_manipulator.urdf.xacro index 3792b6f..a9f41e3 100644 --- a/turtlebot3_beam_description/urdf/turtlebot3_waffle_for_open_manipulator.urdf.xacro +++ b/turtlebot3_beam_description/urdf/turtlebot3_waffle_for_open_manipulator.urdf.xacro @@ -31,7 +31,7 @@ - + @@ -167,7 +167,7 @@ - + diff --git a/turtlebot3_beam_description/urdf/turtlebot3_waffle_pi_for_open_manipulator.urdf.xacro b/turtlebot3_beam_description/urdf/turtlebot3_waffle_pi_for_open_manipulator.urdf.xacro index 3aa03e0..170a99d 100644 --- a/turtlebot3_beam_description/urdf/turtlebot3_waffle_pi_for_open_manipulator.urdf.xacro +++ b/turtlebot3_beam_description/urdf/turtlebot3_waffle_pi_for_open_manipulator.urdf.xacro @@ -160,7 +160,7 @@ - +