Changes have been pushed for the repository "fawkesrobotics/fawkes".
Clone: https://github.com/fawkesrobotics/fawkes.git Gitweb: https://github.com/fawkesrobotics/fawkes The branch, neltester/realsense2_save_image has been updated discards 467bae611534aca292b37adf613418111ba9d33f (commit) discards ed9729d4044a14a35305736c088a3c35e26cac1a (commit) discards 72303826489a249ddfd13d227d55b6c04b40b061 (commit) discards a37921951acc64b10d1d4efed1b5932b4e0dac2f (commit) discards 871f463fc0abe8ea27cc1b700bea8c155c4bf120 (commit) discards f4c673996af5d5a8c86c93249b015e02c52cefea (commit) discards f7c45e4454350e3495e5a01be22ea37871c35c3b (commit) discards d4542e97612081aa01743333f20e9a75969e28f9 (commit) discards 5ef52e91bed67e80f3d2d2666008c356b0e6953c (commit) discards dae24a7bcf8916f876d5bc136a404cf25edba2bd (commit) discards fc1f98c0f92d37b1dae4c27318a07cbbe20084e6 (commit) discards e95158bbd7a4e4330fb321ceb3cf7cf2b49b049e (commit) to 330683044ec8caab87b388aacd03d7a9bfc1cd60 (commit) via db1c7eedcf255a9524880d347f8cfc793d782a76 (commit) via 14d1d2dbafd9b9858ee545ff2c404a1a5ff1f5a6 (commit) via fda4e26e199d81bda0cc6aeee71510f1edd5a621 (commit) via 85b5cf827057cfd7a489968a23368a1f6b8d2430 (commit) via e1afc6f4307e4e0bab9d89895a7f9ec0d2f7bc07 (commit) via 26bf04aebab202c9e8f62843a690f83bc7dd9dc0 (commit) via 80db069df1ed70fabfde5779acfc3906f18626d9 (commit) via 7b73b01077c9509c9089f64733844aedd4d47176 (commit) via 78faef1a736efac516bd84ed12e79bfdc29662b0 (commit) via 1f033fe9026a03d90a305a15c64204326c928136 (commit) via cc714af6bd49eb120f0676381716bb2dd0876049 (commit) via 946e7534bab7d3f55b9271c4a26ba27270553bfc (commit) via aee7aa3584df2ac17f15d01f7a30368d0a2d3d42 (commit) via f8cb4a45c2ae8ced1db2c702d7a8a64e043f82d1 (commit) via ae88f4164034a716294321d54166b76183c557f8 (commit) via f683e50874516e06166001743999ae3267f7532b (commit) via 31e80315fa202065e27c118ffc37b22f8dec1fbd (commit) via ece4db1665c307ad05e38d49648be3884e64eca9 (commit) via 1b5a85935e2ab4696e12e03d993e6390af421632 (commit) via 21513a6b332548a048b8da49d540fa7c7c239553 (commit) via b4518daca8505d0d835706970ec04fd54457dfdd (commit) via d64373709e8f99ad890d181de13b969e1b669de4 (commit) via 01b75acdd0b1d754329b433e777f5f07a5fec43f (commit) via 1d2a9b6196565c4fdce2762e926c8a3378f480e1 (commit) via 04a531602ad06f0ccdb3f029f082719a155576de (commit) via 08d760f7b5cecb050f258709e04059a3e10ea928 (commit) via 199733c017b58523e4381f1504f60d53c819ff22 (commit) via 0f4d55858794c039f06517aa4adddea56382967f (commit) via a86d81a1469f620195b6dd7dc18562904b56eb4c (commit) via 3f77d53b13251e21e07422083f60062dc26b3c25 (commit) via 3a280611acf957106fe3f26413a9d280d546cb99 (commit) via 8d712a02ded845913367cddc85fb8d957d371cdb (commit) via 3fcc4301a0e1d09c1a5af997992bace6443dbc9c (commit) via a3feb1a5c47cd608910e2597d4861c6cd120639d (commit) via 2d4b44e3b9818e29f836fde02c2a503fc5485865 (commit) via 0681b3ee86e8a1f96a7db9da2c7af0e251a75805 (commit) via 600ba09ec2f73afd339d397c6108e963809eb54e (commit) via 6e0917a75f6b285a32b4e225453c84d97e14cc73 (commit) via d6fa4e4587574b1c0294afd3be69a0bd8ed30659 (commit) via b7761311e5a9c83af5911183f8d4485b4ce3b01f (commit) via 3142040b0d429779fd313d6bc6ff3e74eabb7d3a (commit) via c1122ada5c2958a9c101c3183e9cdd49262fd1ba (commit) via 28da35ee9ae5b6ef00c71688f309817187c349d3 (commit) via 74bbfd6de130c19339070682e379350c83c1e672 (commit) via 6fc9e126f25ae5a82e86460c2d2844fc0a7e1888 (commit) This update added new revisions after undoing existing revisions. That is to say, the old revision is not a strict subset of the new revision. This situation occurs when you --force push a change and generate a repository containing something like this: * -- * -- B -- O -- O -- O (467bae611534aca292b37adf613418111ba9d33f) \ N -- N -- N (330683044ec8caab87b388aacd03d7a9bfc1cd60) When this happens we assume that you've already had alert emails for all of the O revisions, and so we here report only the revisions in the N branch from the common base, B. https://github.com/fawkesrobotics/fawkes/tree/neltester/realsense2_save_image Those revisions listed above that are new to this repository have not appeared on any other notification email; so we list those revisions in full, below. - *Log* --------------------------------------------------------------- commit cc714af6bd49eb120f0676381716bb2dd0876049 Author: Niklas Sebastian Eltester <[email protected]> AuthorDate: Sun Oct 4 14:00:30 2020 +0200 Commit: Niklas Sebastian Eltester <[email protected]> CommitDate: Wed Nov 11 16:00:51 2020 +0100 interfaces: add message to save image This adds a message to the CameraControlInterface to save an image https://github.com/fawkesrobotics/fawkes/commit/cc714af6b - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 1f033fe9026a03d90a305a15c64204326c928136 Author: Niklas Sebastian Eltester <[email protected]> AuthorDate: Sun Oct 4 14:02:13 2020 +0200 Commit: Niklas Sebastian Eltester <[email protected]> CommitDate: Wed Nov 11 16:00:51 2020 +0100 realsense2: Change required librealsense version 2.32 https://github.com/fawkesrobotics/fawkes/commit/1f033fe90 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 78faef1a736efac516bd84ed12e79bfdc29662b0 Author: Niklas Sebastian Eltester <[email protected]> AuthorDate: Sun Oct 4 14:04:00 2020 +0200 Commit: Niklas Sebastian Eltester <[email protected]> CommitDate: Wed Nov 11 16:00:51 2020 +0100 realsense2: Add functionality to save an image This adds the option to save an image from a rs2::color_frame https://github.com/fawkesrobotics/fawkes/commit/78faef1a7 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 7b73b01077c9509c9089f64733844aedd4d47176 Author: Niklas Sebastian Eltester <[email protected]> AuthorDate: Sun Oct 4 13:59:22 2020 +0200 Commit: Niklas Sebastian Eltester <[email protected]> CommitDate: Wed Nov 11 16:00:51 2020 +0100 cfg: Add config values for Image Saving https://github.com/fawkesrobotics/fawkes/commit/7b73b0107 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 80db069df1ed70fabfde5779acfc3906f18626d9 Author: Niklas Sebastian Eltester <[email protected]> AuthorDate: Sun Oct 4 15:27:43 2020 +0200 Commit: Niklas Sebastian Eltester <[email protected]> CommitDate: Wed Nov 11 16:00:51 2020 +0100 realsense2: change camera control if to open_for_writing https://github.com/fawkesrobotics/fawkes/commit/80db069df - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 26bf04aebab202c9e8f62843a690f83bc7dd9dc0 Author: Niklas Sebastian Eltester <[email protected]> AuthorDate: Sun Oct 4 16:38:19 2020 +0200 Commit: Niklas Sebastian Eltester <[email protected]> CommitDate: Wed Nov 11 16:00:51 2020 +0100 realsense2: fix include of stb_image_write https://github.com/fawkesrobotics/fawkes/commit/26bf04aeb - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit e1afc6f4307e4e0bab9d89895a7f9ec0d2f7bc07 Author: Niklas Sebastian Eltester <[email protected]> AuthorDate: Sun Oct 4 16:38:45 2020 +0200 Commit: Niklas Sebastian Eltester <[email protected]> CommitDate: Wed Nov 11 16:00:51 2020 +0100 realsense2: Add library to save frame as png This adds the stb_image_write library to save a rs2::frame as a png Refer to librealsense/tree/master/examples/save-to-disk https://github.com/fawkesrobotics/fawkes/commit/e1afc6f43 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 85b5cf827057cfd7a489968a23368a1f6b8d2430 Author: Niklas Sebastian Eltester <[email protected]> AuthorDate: Thu Oct 8 16:27:08 2020 +0200 Commit: Niklas Sebastian Eltester <[email protected]> CommitDate: Wed Nov 11 16:00:51 2020 +0100 cfg: Improve comment on SaveImage in CamCtrlIf https://github.com/fawkesrobotics/fawkes/commit/85b5cf827 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit fda4e26e199d81bda0cc6aeee71510f1edd5a621 Author: Niklas Sebastian Eltester <[email protected]> AuthorDate: Thu Oct 8 16:27:49 2020 +0200 Commit: Niklas Sebastian Eltester <[email protected]> CommitDate: Wed Nov 11 16:00:51 2020 +0100 realsense2: include CameraControlInterface in Makefile https://github.com/fawkesrobotics/fawkes/commit/fda4e26e1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 14d1d2dbafd9b9858ee545ff2c404a1a5ff1f5a6 Author: Niklas Sebastian Eltester <[email protected]> AuthorDate: Thu Oct 8 16:28:44 2020 +0200 Commit: Niklas Sebastian Eltester <[email protected]> CommitDate: Wed Nov 11 16:00:51 2020 +0100 realsense2: Create separate pipeline for color_sensor This creates a second pipeline for the same realsense device https://github.com/fawkesrobotics/fawkes/commit/14d1d2dba - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit db1c7eedcf255a9524880d347f8cfc793d782a76 Author: Niklas Sebastian Eltester <[email protected]> AuthorDate: Wed Nov 11 15:48:27 2020 +0100 Commit: Niklas Sebastian Eltester <[email protected]> CommitDate: Wed Nov 11 16:00:51 2020 +0100 realsense2: switch to firevision png writer https://github.com/fawkesrobotics/fawkes/commit/db1c7eedc - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 330683044ec8caab87b388aacd03d7a9bfc1cd60 Author: Niklas Sebastian Eltester <[email protected]> AuthorDate: Wed Nov 11 15:49:01 2020 +0100 Commit: Niklas Sebastian Eltester <[email protected]> CommitDate: Wed Nov 11 16:00:51 2020 +0100 cfg: Add rgb config options https://github.com/fawkesrobotics/fawkes/commit/330683044 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *Summary* ----------------------------------------------------------- etc/buildsys/fvconf.mk | 9 +-- etc/buildsys/lua.mk | 2 +- etc/buildsys/ros.mk | 2 +- src/libs/config/config.tolua | 3 + src/libs/logging/fd_redirect.cpp | 2 +- .../libs/webview/microhttpd_compat.h | 17 +++++ src/libs/webview/request.cpp | 9 +-- src/libs/webview/request_dispatcher.cpp | 28 ++++---- src/libs/webview/request_dispatcher.h | 40 ++++++------ src/libs/webview/server.cpp | 3 +- src/libs/webview/webview.mk | 6 ++ src/plugins/Makefile | 3 +- .../clips-executive/clips/coordination-mutex.clp | 36 +++++++---- src/plugins/clips-executive/clips/lock-actions.clp | 15 +++-- .../clips-executive/clips/wm-robmem-sync.clp | 2 +- .../clips_robot_memory_thread.cpp | 75 +++++++++------------- .../clips-robot-memory/clips_robot_memory_thread.h | 4 +- src/plugins/gazebo/gazebo.mk | 5 ++ .../gazebo/gazsim-localization/gazebo-plugin/gps.h | 2 +- .../gazsim-robotino/gazebo-plugin-gyro/gyro.h | 2 +- .../gazsim-robotino/gazebo-plugin-motor/motor.h | 2 +- src/plugins/laser/sick_tim55x_ethernet_aqt.cpp | 2 +- src/plugins/nao/button_thread.cpp | 2 +- src/plugins/nao/dcm_thread.cpp | 2 +- src/plugins/openprs/openprs_thread.cpp | 2 +- src/plugins/openprs/utils/openprs_mp_proxy.cpp | 2 +- src/plugins/openprs/utils/openprs_server_proxy.cpp | 2 +- src/plugins/openrave/environment.cpp | 2 +- src/plugins/plexil/protobuf_adapter.cpp | 1 + src/plugins/robot-memory/event_trigger_manager.cpp | 2 +- src/plugins/robotino/direct_com_thread.cpp | 2 +- src/plugins/webview/Makefile | 4 +- 32 files changed, 163 insertions(+), 127 deletions(-) copy doc/headers/lichead_c.GPL => src/libs/webview/microhttpd_compat.h (65%) - *Diffs* ------------------------------------------------------------- - *commit* cc714af6bd49eb120f0676381716bb2dd0876049 - - - - - - - - - - Author: Niklas Sebastian Eltester <[email protected]> Date: Sun Oct 4 14:00:30 2020 +0200 Subject: interfaces: add message to save image src/libs/interfaces/CameraControlInterface.xml | 6 ++++++ 1 file changed, 6 insertions(+) _Diff for modified files_: diff --git a/src/libs/interfaces/CameraControlInterface.xml b/src/libs/interfaces/CameraControlInterface.xml index 06576834d..aa00eafc2 100644 --- a/src/libs/interfaces/CameraControlInterface.xml +++ b/src/libs/interfaces/CameraControlInterface.xml @@ -26,6 +26,7 @@ <field type="uint32" name="zoom_min">Minimum zoom</field> <field type="bool" name="mirror">Is the image mirrored?</field> <field type="bool" name="mirror_supported">Is mirroring supported?</field> + <field type="string" name="image_name" length="32">Filename of the image file</field> </data> <message name="SetEffect"> @@ -42,4 +43,9 @@ <comment>Set mirror state.</comment> <ref>mirror</ref> </message> + + <message name="SaveImage"> + <comment>Take a frame as an image</comment> + <ref>image_name</ref> + </message> </interface> - *commit* 1f033fe9026a03d90a305a15c64204326c928136 - - - - - - - - - - Author: Niklas Sebastian Eltester <[email protected]> Date: Sun Oct 4 14:02:13 2020 +0200 Subject: realsense2: Change required librealsense version 2.32 src/plugins/realsense2/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) _Diff for modified files_: diff --git a/src/plugins/realsense2/Makefile b/src/plugins/realsense2/Makefile index 5763c2863..ee3c342b2 100644 --- a/src/plugins/realsense2/Makefile +++ b/src/plugins/realsense2/Makefile @@ -33,7 +33,7 @@ OBJS_realsense2 = realsense2_plugin.o realsense2_thread.o OBJS_all = $(OBJS_realsense2) PLUGINS_all = $(PLUGINDIR)/realsense2.$(SOEXT) -REALSENSE_MIN_VERSION = 2.16.0 +REALSENSE_MIN_VERSION = 2.32.0 ifeq ($(shell $(PKGCONFIG) --exists realsense2; echo $$?),0) ifeq ($(shell $(PKGCONFIG) --atleast-version $(REALSENSE_MIN_VERSION) realsense2; echo $$?),0) - *commit* 78faef1a736efac516bd84ed12e79bfdc29662b0 - - - - - - - - - - Author: Niklas Sebastian Eltester <[email protected]> Date: Sun Oct 4 14:04:00 2020 +0200 Subject: realsense2: Add functionality to save an image src/plugins/realsense2/realsense2_thread.cpp | 72 ++++++++++++++++++++++++++++ src/plugins/realsense2/realsense2_thread.h | 55 ++++++++++++--------- 2 files changed, 105 insertions(+), 22 deletions(-) _Diff for modified files_: diff --git a/src/plugins/realsense2/realsense2_thread.cpp b/src/plugins/realsense2/realsense2_thread.cpp index 36bc137cf..5a7945354 100644 --- a/src/plugins/realsense2/realsense2_thread.cpp +++ b/src/plugins/realsense2/realsense2_thread.cpp @@ -23,6 +23,7 @@ #include "realsense2_thread.h" +#include <interfaces/CameraControlInterface.h> #include <interfaces/SwitchInterface.h> using namespace fawkes; @@ -30,6 +31,7 @@ using namespace fawkes; /** @class Realsense2Thread 'realsense2_thread.h' * Driver for the Intel RealSense Camera providing Depth Data as Pointcloud * Inspired by realsense fawkes plugin + * Provides functionality to save a camera frame into a PNG File * @author Christoph Gollok */ @@ -56,6 +58,12 @@ Realsense2Thread::init() cfg_use_switch_ = config->get_bool_or_default((cfg_prefix + "use_switch").c_str(), true); + //rgb image path + rgb_path_ = config->get_string_or_default((cfg_prefix + "rgb_path").c_str(), + "/home/robotino/realsense_images"); + camera_if_name_ = + config->get_string_or_default((cfg_prefix + "camera_interface_name").c_str(), "realsense2_cam"); + if (cfg_use_switch_) { logger->log_info(name(), "Switch enabled"); } else { @@ -66,6 +74,9 @@ Realsense2Thread::init() switch_if_->set_enabled(true); switch_if_->write(); + //CameraControlInterface + camera_if_ = blackboard->open_for_reading<CameraControlInterface>(camera_if_name_.c_str()); + camera_scale_ = 1; // initalize pointcloud realsense_depth_refptr_ = new Cloud(); @@ -87,6 +98,31 @@ Realsense2Thread::loop() camera_running_ = start_camera(); return; } + // take picture + if (enable_camera_ && read_camera_control() != "") { + if (rs_pipe_->poll_for_frames(&rs_data_)) { + error_counter_ = 0; + rs2::video_frame color_frame = rs_data_.first(RS2_STREAM_COLOR, RS2_FORMAT_YUYV); + image_name_ = + rgb_path_ + read_camera_control() + color_frame.get_profile().stream_name() + ".png"; + stbi_write_png(image_name_.c_str(), + color_frame.get_width(), + color_frame.get_height(), + color_frame.get_bytes_per_pixel(), + color_frame.get_data(), + color_frame.get_stride_in_bytes()); + logger->log_info(name(), "Saving image to %s", image_name_.c_str()); + } else { + error_counter_++; + logger->log_warn(name(), "Poll for frames not successful ()"); + if (error_counter_ >= restart_after_num_errors_) { + logger->log_warn(name(), "Polling failed, restarting device"); + error_counter_ = 0; + stop_camera(); + start_camera(); + } + } + } if (cfg_use_switch_) { read_switch(); @@ -158,6 +194,7 @@ Realsense2Thread::start_camera() if (!get_camera(rs_device_)) { return false; } + // depth config rs2::config config; config.enable_stream(RS2_STREAM_DEPTH, RS2_FORMAT_Z16, frame_rate_); rs2::pipeline_profile rs_pipeline_profile_ = rs_pipe_->start(config); @@ -176,6 +213,20 @@ Realsense2Thread::start_camera() camera_scale_, frame_rate_); + //rgb config + rs2::config rgb_config; + rgb_config.enable_stream(RS2_STREAM_COLOR, 1920, 1080, RS2_FORMAT_YUYV, frame_rate_); + rs2::pipeline_profile rs_pipeline_profile_rgb_ = rs_pipe_->start(rgb_config); + auto rgb_stream = + rs_pipeline_profile_rgb_.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>(); + rgb_intrinsics_ = rgb_stream.get_intrinsics(); + rs2::color_sensor rgb_sensor = rs_device_.first<rs2::color_sensor>(); + logger->log_info(name(), + "RGB Height: %d RGB Width: %d FPS: %d", + rgb_intrinsics_.height, + rgb_intrinsics_.width, + frame_rate_); + return true; } catch (const rs2::error &e) { @@ -347,3 +398,24 @@ Realsense2Thread::read_switch() switch_if_->write(); return switch_if_->is_enabled(); } + +/** + * Read the CameraControl interface to take a picture. + * @return object name. + */ +std::string +Realsense2Thread::read_camera_control() +{ + std::string object_name_ = ""; + while (!camera_if_->msgq_empty()) { + if (camera_if_->msgq_first_is<CameraControlInterface::SaveImageMessage>()) { + CameraControlInterface::SaveImageMessage *msg = + camera_if_->msgq_first<CameraControlInterface::SaveImageMessage>(); + object_name_ = std::string(msg->image_name()); + } else { + logger->log_warn(name(), "Unknown message received"); + } + camera_if_->msgq_pop(); + } + return object_name_; +} diff --git a/src/plugins/realsense2/realsense2_thread.h b/src/plugins/realsense2/realsense2_thread.h index f092571d7..0b0337123 100644 --- a/src/plugins/realsense2/realsense2_thread.h +++ b/src/plugins/realsense2/realsense2_thread.h @@ -22,27 +22,32 @@ */ #ifndef _PLUGINS_REALSENSE2THREAD_H_ -#define _PLUGINS_REALSENSE2THREAD_H_ - -#include <aspect/blackboard.h> -#include <aspect/blocked_timing.h> -#include <aspect/clock.h> -#include <aspect/configurable.h> -#include <aspect/logging.h> -#include <aspect/pointcloud.h> -#include <core/threading/thread.h> -#include <librealsense2/rsutil.h> -#include <pcl/point_cloud.h> -#include <pcl/point_types.h> - -#include <librealsense2/rs.hpp> -#include <librealsense2/rs_advanced_mode.hpp> -#include <string> -#include <thread> +# define _PLUGINS_REALSENSE2THREAD_H_ + +# ifndef STB_IMAGE_WRTIE_IMPLEMENTATION +# define STB_IMAGE_WRITE_IMPLEMENTATION + +# include <aspect/blackboard.h> +# include <aspect/blocked_timing.h> +# include <aspect/clock.h> +# include <aspect/configurable.h> +# include <aspect/logging.h> +# include <aspect/pointcloud.h> +# include <core/threading/thread.h> +# include <librealsense2/rsutil.h> +# include <pcl/point_cloud.h> +# include <pcl/point_types.h> + +# include <librealsense2/rs.hpp> +# include <librealsense2/rs_advanced_mode.hpp> +# include <stb_image_write.h> +# include <string> +# include <thread> namespace fawkes { class SwitchInterface; -} +class CameraControlInterface; +} // namespace fawkes class Realsense2Thread : public fawkes::Thread, public fawkes::BlockedTimingAspect, @@ -75,11 +80,13 @@ protected: } protected: - bool read_switch(); + bool read_switch(); + std::string read_camera_control(); private: - fawkes::SwitchInterface *switch_if_; - bool cfg_use_switch_; + fawkes::SwitchInterface * switch_if_; + bool cfg_use_switch_; + fawkes::CameraControlInterface *camera_if_; typedef pcl::PointXYZ PointType; typedef pcl::PointCloud<PointType> Cloud; @@ -95,11 +102,15 @@ private: rs2::device rs_device_; rs2::frameset rs_data_; rs2_intrinsics intrinsics_; + rs2_intrinsics rgb_intrinsics_; float camera_scale_; std::string frame_id_; std::string pcl_id_; std::string switch_if_name_; + std::string rgb_path_; + std::string image_name_; + std::string camera_if_name_; uint frame_rate_; float laser_power_; bool camera_running_ = false; @@ -109,4 +120,4 @@ private: uint error_counter_ = 0; }; -#endif +# endif - *commit* 7b73b01077c9509c9089f64733844aedd4d47176 - - - - - - - - - - Author: Niklas Sebastian Eltester <[email protected]> Date: Sun Oct 4 13:59:22 2020 +0200 Subject: cfg: Add config values for Image Saving cfg/conf.d/realsense2.yaml | 6 ++++++ 1 file changed, 6 insertions(+) _Diff for modified files_: diff --git a/cfg/conf.d/realsense2.yaml b/cfg/conf.d/realsense2.yaml index d1daa810d..0be7c13ef 100644 --- a/cfg/conf.d/realsense2.yaml +++ b/cfg/conf.d/realsense2.yaml @@ -25,3 +25,9 @@ realsense2: device_options: # Laser power from 0 - 15 as integer laser_power: 15 + + # Path to directory to save images + rgb_path: "/home/robotino/realsense_images" + + # Camera Control Interface name + camera_interface_name: "realsense2_cam" - *commit* 80db069df1ed70fabfde5779acfc3906f18626d9 - - - - - - - - - - Author: Niklas Sebastian Eltester <[email protected]> Date: Sun Oct 4 15:27:43 2020 +0200 Subject: realsense2: change camera control if to open_for_writing src/plugins/realsense2/realsense2_thread.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) _Diff for modified files_: diff --git a/src/plugins/realsense2/realsense2_thread.cpp b/src/plugins/realsense2/realsense2_thread.cpp index 5a7945354..a17567723 100644 --- a/src/plugins/realsense2/realsense2_thread.cpp +++ b/src/plugins/realsense2/realsense2_thread.cpp @@ -75,7 +75,7 @@ Realsense2Thread::init() switch_if_->write(); //CameraControlInterface - camera_if_ = blackboard->open_for_reading<CameraControlInterface>(camera_if_name_.c_str()); + camera_if_ = blackboard->open_for_writing<CameraControlInterface>(camera_if_name_.c_str()); camera_scale_ = 1; // initalize pointcloud - *commit* 26bf04aebab202c9e8f62843a690f83bc7dd9dc0 - - - - - - - - - - Author: Niklas Sebastian Eltester <[email protected]> Date: Sun Oct 4 16:38:19 2020 +0200 Subject: realsense2: fix include of stb_image_write src/plugins/realsense2/realsense2_thread.cpp | 3 +++ src/plugins/realsense2/realsense2_thread.h | 40 +++++++++++++--------------- 2 files changed, 21 insertions(+), 22 deletions(-) _Diff for modified files_: diff --git a/src/plugins/realsense2/realsense2_thread.cpp b/src/plugins/realsense2/realsense2_thread.cpp index a17567723..bd44df252 100644 --- a/src/plugins/realsense2/realsense2_thread.cpp +++ b/src/plugins/realsense2/realsense2_thread.cpp @@ -26,6 +26,9 @@ #include <interfaces/CameraControlInterface.h> #include <interfaces/SwitchInterface.h> +#define STB_IMAGE_WRITE_IMPLEMENTATION +#include "stb_image_write.h" + using namespace fawkes; /** @class Realsense2Thread 'realsense2_thread.h' diff --git a/src/plugins/realsense2/realsense2_thread.h b/src/plugins/realsense2/realsense2_thread.h index 0b0337123..ee8c3bbaa 100644 --- a/src/plugins/realsense2/realsense2_thread.h +++ b/src/plugins/realsense2/realsense2_thread.h @@ -22,27 +22,23 @@ */ #ifndef _PLUGINS_REALSENSE2THREAD_H_ -# define _PLUGINS_REALSENSE2THREAD_H_ - -# ifndef STB_IMAGE_WRTIE_IMPLEMENTATION -# define STB_IMAGE_WRITE_IMPLEMENTATION - -# include <aspect/blackboard.h> -# include <aspect/blocked_timing.h> -# include <aspect/clock.h> -# include <aspect/configurable.h> -# include <aspect/logging.h> -# include <aspect/pointcloud.h> -# include <core/threading/thread.h> -# include <librealsense2/rsutil.h> -# include <pcl/point_cloud.h> -# include <pcl/point_types.h> - -# include <librealsense2/rs.hpp> -# include <librealsense2/rs_advanced_mode.hpp> -# include <stb_image_write.h> -# include <string> -# include <thread> +#define _PLUGINS_REALSENSE2THREAD_H_ + +#include <aspect/blackboard.h> +#include <aspect/blocked_timing.h> +#include <aspect/clock.h> +#include <aspect/configurable.h> +#include <aspect/logging.h> +#include <aspect/pointcloud.h> +#include <core/threading/thread.h> +#include <librealsense2/rsutil.h> +#include <pcl/point_cloud.h> +#include <pcl/point_types.h> + +#include <librealsense2/rs.hpp> +#include <librealsense2/rs_advanced_mode.hpp> +#include <string> +#include <thread> namespace fawkes { class SwitchInterface; @@ -120,4 +116,4 @@ private: uint error_counter_ = 0; }; -# endif +#endif - *commit* e1afc6f4307e4e0bab9d89895a7f9ec0d2f7bc07 - - - - - - - - - - Author: Niklas Sebastian Eltester <[email protected]> Date: Sun Oct 4 16:38:45 2020 +0200 Subject: realsense2: Add library to save frame as png src/plugins/realsense2/stb_image_write.h | 2083 ++++++++++++++++++++++++++++++ 1 file changed, 2083 insertions(+) - *commit* 85b5cf827057cfd7a489968a23368a1f6b8d2430 - - - - - - - - - - Author: Niklas Sebastian Eltester <[email protected]> Date: Thu Oct 8 16:27:08 2020 +0200 Subject: cfg: Improve comment on SaveImage in CamCtrlIf src/libs/interfaces/CameraControlInterface.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) _Diff for modified files_: diff --git a/src/libs/interfaces/CameraControlInterface.xml b/src/libs/interfaces/CameraControlInterface.xml index aa00eafc2..31d328092 100644 --- a/src/libs/interfaces/CameraControlInterface.xml +++ b/src/libs/interfaces/CameraControlInterface.xml @@ -45,7 +45,7 @@ </message> <message name="SaveImage"> - <comment>Take a frame as an image</comment> + <comment>Save a frame as an image.</comment> <ref>image_name</ref> </message> </interface> - *commit* fda4e26e199d81bda0cc6aeee71510f1edd5a621 - - - - - - - - - - Author: Niklas Sebastian Eltester <[email protected]> Date: Thu Oct 8 16:27:49 2020 +0200 Subject: realsense2: include CameraControlInterface in Makefile src/plugins/realsense2/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) _Diff for modified files_: diff --git a/src/plugins/realsense2/Makefile b/src/plugins/realsense2/Makefile index ee3c342b2..6158b11c2 100644 --- a/src/plugins/realsense2/Makefile +++ b/src/plugins/realsense2/Makefile @@ -26,7 +26,7 @@ LDFLAGS += $(LDFLAGS_TF) $(LDFLAGS_PCL) LIBS_realsense2 = m fawkescore fawkesutils fawkesaspects fawkesbaseapp \ fawkesblackboard fawkesinterface \ - fawkespcl_utils SwitchInterface + fawkespcl_utils SwitchInterface CameraControlInterface OBJS_realsense2 = realsense2_plugin.o realsense2_thread.o - *commit* 14d1d2dbafd9b9858ee545ff2c404a1a5ff1f5a6 - - - - - - - - - - Author: Niklas Sebastian Eltester <[email protected]> Date: Thu Oct 8 16:28:44 2020 +0200 Subject: realsense2: Create separate pipeline for color_sensor src/plugins/realsense2/realsense2_thread.cpp | 21 ++++++++++++++++----- src/plugins/realsense2/realsense2_thread.h | 2 ++ 2 files changed, 18 insertions(+), 5 deletions(-) _Diff for modified files_: diff --git a/src/plugins/realsense2/realsense2_thread.cpp b/src/plugins/realsense2/realsense2_thread.cpp index bd44df252..1c86e5eeb 100644 --- a/src/plugins/realsense2/realsense2_thread.cpp +++ b/src/plugins/realsense2/realsense2_thread.cpp @@ -90,8 +90,9 @@ Realsense2Thread::init() realsense_depth_->resize(0); pcl_manager->add_pointcloud(pcl_id_.c_str(), realsense_depth_refptr_); - rs_pipe_ = new rs2::pipeline(); - rs_context_ = new rs2::context(); + rs_pipe_ = new rs2::pipeline(); + rs_context_ = new rs2::context(); + rs_rgb_pipe_ = new rs2::pipeline(); } void @@ -103,9 +104,9 @@ Realsense2Thread::loop() } // take picture if (enable_camera_ && read_camera_control() != "") { - if (rs_pipe_->poll_for_frames(&rs_data_)) { + if (rs_rgb_pipe_->poll_for_frames(&rs_rgb_data_)) { error_counter_ = 0; - rs2::video_frame color_frame = rs_data_.first(RS2_STREAM_COLOR, RS2_FORMAT_YUYV); + rs2::video_frame color_frame = rs_rgb_data_.first(RS2_STREAM_COLOR, RS2_FORMAT_YUYV); image_name_ = rgb_path_ + read_camera_control() + color_frame.get_profile().stream_name() + ".png"; stbi_write_png(image_name_.c_str(), @@ -177,9 +178,11 @@ Realsense2Thread::finalize() stop_camera(); delete rs_pipe_; delete rs_context_; + delete rs_rgb_pipe_; realsense_depth_refptr_.reset(); pcl_manager->remove_pointcloud(pcl_id_.c_str()); blackboard->close(switch_if_); + blackboard->close(camera_if_); } /* Create RS context and start the depth stream @@ -192,6 +195,10 @@ Realsense2Thread::start_camera() rs_pipe_->stop(); } catch (const std::exception &e) { } + try { + rs_rgb_pipe_->stop(); + } catch (const std::exception &e) { + } try { if (!get_camera(rs_device_)) { @@ -219,7 +226,7 @@ Realsense2Thread::start_camera() //rgb config rs2::config rgb_config; rgb_config.enable_stream(RS2_STREAM_COLOR, 1920, 1080, RS2_FORMAT_YUYV, frame_rate_); - rs2::pipeline_profile rs_pipeline_profile_rgb_ = rs_pipe_->start(rgb_config); + rs2::pipeline_profile rs_pipeline_profile_rgb_ = rs_rgb_pipe_->start(rgb_config); auto rgb_stream = rs_pipeline_profile_rgb_.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>(); rgb_intrinsics_ = rgb_stream.get_intrinsics(); @@ -379,6 +386,10 @@ Realsense2Thread::stop_camera() rs_pipe_->stop(); } catch (const std::exception &e) { } + try { + rs_rgb_pipe_->stop(); + } catch (const std::exception &e) { + } } /** diff --git a/src/plugins/realsense2/realsense2_thread.h b/src/plugins/realsense2/realsense2_thread.h index ee8c3bbaa..aaf271693 100644 --- a/src/plugins/realsense2/realsense2_thread.h +++ b/src/plugins/realsense2/realsense2_thread.h @@ -97,6 +97,8 @@ private: rs2::context * rs_context_; rs2::device rs_device_; rs2::frameset rs_data_; + rs2::pipeline *rs_rgb_pipe_; + rs2::frameset rs_rgb_data_; rs2_intrinsics intrinsics_; rs2_intrinsics rgb_intrinsics_; - *commit* db1c7eedcf255a9524880d347f8cfc793d782a76 - - - - - - - - - - Author: Niklas Sebastian Eltester <[email protected]> Date: Wed Nov 11 15:48:27 2020 +0100 Subject: realsense2: switch to firevision png writer src/plugins/realsense2/Makefile | 4 +- src/plugins/realsense2/realsense2_thread.cpp | 27 +- src/plugins/realsense2/realsense2_thread.h | 6 + src/plugins/realsense2/stb_image_write.h | 2083 -------------------------- 4 files changed, 22 insertions(+), 2098 deletions(-) _Diff for modified files_: diff --git a/src/plugins/realsense2/Makefile b/src/plugins/realsense2/Makefile index 6158b11c2..76485ca29 100644 --- a/src/plugins/realsense2/Makefile +++ b/src/plugins/realsense2/Makefile @@ -25,7 +25,7 @@ CFLAGS += $(CFLAGS_TF) $(CFLAGS_PCL) $(CFFLAGS_conveyor_pose) LDFLAGS += $(LDFLAGS_TF) $(LDFLAGS_PCL) LIBS_realsense2 = m fawkescore fawkesutils fawkesaspects fawkesbaseapp \ - fawkesblackboard fawkesinterface \ + fawkesblackboard fawkesinterface fvutils \ fawkespcl_utils SwitchInterface CameraControlInterface OBJS_realsense2 = realsense2_plugin.o realsense2_thread.o @@ -37,7 +37,7 @@ REALSENSE_MIN_VERSION = 2.32.0 ifeq ($(shell $(PKGCONFIG) --exists realsense2; echo $$?),0) ifeq ($(shell $(PKGCONFIG) --atleast-version $(REALSENSE_MIN_VERSION) realsense2; echo $$?),0) - CFLAGS += $(shell $(PKGCONFIG) --cflags realsense2) + CFLAGS += $(shell $(PKGCONFIG) --cflags realsense2) -Wno-error LDFLAGS += $(shell $(PKGCONFIG) --libs realsense2) LIBS_realsense2 += realsense2 PLUGINS_build = $(PLUGINS_all) diff --git a/src/plugins/realsense2/realsense2_thread.cpp b/src/plugins/realsense2/realsense2_thread.cpp index 1c86e5eeb..63291d4f1 100644 --- a/src/plugins/realsense2/realsense2_thread.cpp +++ b/src/plugins/realsense2/realsense2_thread.cpp @@ -26,9 +26,6 @@ #include <interfaces/CameraControlInterface.h> #include <interfaces/SwitchInterface.h> -#define STB_IMAGE_WRITE_IMPLEMENTATION -#include "stb_image_write.h" - using namespace fawkes; /** @class Realsense2Thread 'realsense2_thread.h' @@ -64,6 +61,11 @@ Realsense2Thread::init() //rgb image path rgb_path_ = config->get_string_or_default((cfg_prefix + "rgb_path").c_str(), "/home/robotino/realsense_images"); + //rgb camera resolution/frame rate + rgb_width_ = config->get_uint_or_default((cfg_prefix + "rgb_width").c_str(), 1920); + rgb_heigth_ = config->get_uint_or_default((cfg_prefix + "rgb_heigth").c_str(), 1080); + rgb_frame_rate_ = config->get_uint_or_default((cfg_prefix + "rgb_framerate").c_str(), 15); + camera_if_name_ = config->get_string_or_default((cfg_prefix + "camera_interface_name").c_str(), "realsense2_cam"); @@ -106,19 +108,17 @@ Realsense2Thread::loop() if (enable_camera_ && read_camera_control() != "") { if (rs_rgb_pipe_->poll_for_frames(&rs_rgb_data_)) { error_counter_ = 0; - rs2::video_frame color_frame = rs_rgb_data_.first(RS2_STREAM_COLOR, RS2_FORMAT_YUYV); + rs2::video_frame color_frame = rs_rgb_data_.first(RS2_STREAM_COLOR, RS2_FORMAT_RGB8); image_name_ = rgb_path_ + read_camera_control() + color_frame.get_profile().stream_name() + ".png"; - stbi_write_png(image_name_.c_str(), - color_frame.get_width(), - color_frame.get_height(), - color_frame.get_bytes_per_pixel(), - color_frame.get_data(), - color_frame.get_stride_in_bytes()); + png_writer_.set_filename(image_name_.c_str()); + png_writer_.set_dimensions(color_frame.get_width(), color_frame.get_height()); + png_writer_.set_buffer(firevision::RGB, (unsigned char *)color_frame.get_data()); + png_writer_.write(); logger->log_info(name(), "Saving image to %s", image_name_.c_str()); } else { error_counter_++; - logger->log_warn(name(), "Poll for frames not successful ()"); + logger->log_warn(name(), "Poll for rgb frames not successful ()"); if (error_counter_ >= restart_after_num_errors_) { logger->log_warn(name(), "Polling failed, restarting device"); error_counter_ = 0; @@ -225,7 +225,8 @@ Realsense2Thread::start_camera() //rgb config rs2::config rgb_config; - rgb_config.enable_stream(RS2_STREAM_COLOR, 1920, 1080, RS2_FORMAT_YUYV, frame_rate_); + rgb_config.enable_stream( + RS2_STREAM_COLOR, rgb_width_, rgb_heigth_, RS2_FORMAT_RGB8, rgb_frame_rate_); rs2::pipeline_profile rs_pipeline_profile_rgb_ = rs_rgb_pipe_->start(rgb_config); auto rgb_stream = rs_pipeline_profile_rgb_.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>(); @@ -235,7 +236,7 @@ Realsense2Thread::start_camera() "RGB Height: %d RGB Width: %d FPS: %d", rgb_intrinsics_.height, rgb_intrinsics_.width, - frame_rate_); + rgb_frame_rate_); return true; diff --git a/src/plugins/realsense2/realsense2_thread.h b/src/plugins/realsense2/realsense2_thread.h index aaf271693..3653c39cd 100644 --- a/src/plugins/realsense2/realsense2_thread.h +++ b/src/plugins/realsense2/realsense2_thread.h @@ -31,6 +31,7 @@ #include <aspect/logging.h> #include <aspect/pointcloud.h> #include <core/threading/thread.h> +#include <fvutils/writers/png.h> #include <librealsense2/rsutil.h> #include <pcl/point_cloud.h> #include <pcl/point_types.h> @@ -90,6 +91,8 @@ private: typedef Cloud::Ptr CloudPtr; typedef Cloud::ConstPtr CloudConstPtr; + firevision::PNGWriter png_writer_; + fawkes::RefPtr<Cloud> realsense_depth_refptr_; CloudPtr realsense_depth_; @@ -110,6 +113,9 @@ private: std::string image_name_; std::string camera_if_name_; uint frame_rate_; + uint rgb_frame_rate_; + uint rgb_width_; + uint rgb_heigth_; float laser_power_; bool camera_running_ = false; bool enable_camera_ = true; - *commit* 330683044ec8caab87b388aacd03d7a9bfc1cd60 - - - - - - - - - - Author: Niklas Sebastian Eltester <[email protected]> Date: Wed Nov 11 15:49:01 2020 +0100 Subject: cfg: Add rgb config options cfg/conf.d/realsense2.yaml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) _Diff for modified files_: diff --git a/cfg/conf.d/realsense2.yaml b/cfg/conf.d/realsense2.yaml index 0be7c13ef..d1c1a9576 100644 --- a/cfg/conf.d/realsense2.yaml +++ b/cfg/conf.d/realsense2.yaml @@ -18,6 +18,12 @@ realsense2: # 0 for don't care frame_rate: 30 + # Resolution of the RGB camera + rgb_width: 1920 + rgb_heigth: 1080 + # Desired/Supported RGB frame rate at the desired resolution + rgb_frame_rate: 15 + # Number of retries after unsuccessful polling before restarting the camera restart_after_num_errors: 50 @@ -27,7 +33,7 @@ realsense2: laser_power: 15 # Path to directory to save images - rgb_path: "/home/robotino/realsense_images" + rgb_path: "/home/robotino/realsense_images/" # Camera Control Interface name camera_interface_name: "realsense2_cam" _______________________________________________ fawkes-commits mailing list [email protected] https://lists.kbsg.rwth-aachen.de/listinfo/fawkes-commits
