From b8ea6360156e876d75000fa2faf5abd15ac7d567 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 26 Dec 2023 15:23:39 +0100 Subject: [PATCH] cleanups MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- drake_ros/core/rgbd_system.cc | 123 ++++++++-------------- drake_ros/core/rgbd_system.h | 1 + drake_ros_examples/examples/rgbd/rgbd.cpp | 7 +- drake_ros_examples/examples/rgbd/rgbd.sdf | 2 +- 4 files changed, 50 insertions(+), 83 deletions(-) diff --git a/drake_ros/core/rgbd_system.cc b/drake_ros/core/rgbd_system.cc index fece8919..d40505ce 100644 --- a/drake_ros/core/rgbd_system.cc +++ b/drake_ros/core/rgbd_system.cc @@ -62,9 +62,10 @@ void RGBDSystem::CalcDepthImage(const drake::systems::Context& context, float* depth_row_value = reinterpret_cast(&depth_value->data[0]); for (size_t x = 0; x < depth_value->data.size() / 4; ++x) { - depth_row_value[x] = depth_row[x]; if (std::numeric_limits::infinity() == depth_row[x]) { depth_row_value[x] = 0.0f; + } else { + depth_row_value[x] = depth_row[x]; } } } @@ -73,16 +74,13 @@ template const InputPort& RGBDSystem::DeclareDepthInputPort( std::string port_name, double publish_period, double start_time) { // Test to confirm valid pixel type. - static_assert(kPixelType == PixelType::kRgba8U || - kPixelType == PixelType::kDepth32F || - kPixelType == PixelType::kDepth16U || - kPixelType == PixelType::kLabel16I || - kPixelType == PixelType::kGrey8U, - "ImageWriter: the only supported pixel types are: kRgba8U, " - "kDepth32F, kDepth16U, kGrey8U, and kLabel16I"); + static_assert( + kPixelType == PixelType::kDepth32F || kPixelType == PixelType::kDepth16U, + "ImageWriter: the only supported pixel types are: kDepth32F " + "and kDepth16U"); if (publish_period <= 0) { - throw std::logic_error("ImageWriter: publish period must be positive"); + throw std::logic_error("RGBDSystem: publish period must be positive"); } // Now configure the system for the valid port declaration. @@ -108,16 +106,13 @@ template const InputPort& RGBDSystem::DeclareImageInputPort( std::string port_name, double publish_period, double start_time) { // Test to confirm valid pixel type. - static_assert(kPixelType == PixelType::kRgba8U || - kPixelType == PixelType::kDepth32F || - kPixelType == PixelType::kDepth16U || - kPixelType == PixelType::kLabel16I || - kPixelType == PixelType::kGrey8U, - "ImageWriter: the only supported pixel types are: kRgba8U, " - "kDepth32F, kDepth16U, kGrey8U, and kLabel16I"); + static_assert( + kPixelType == PixelType::kRgba8U || kPixelType == PixelType::kGrey8U, + "RGBDSystem color image: the only supported pixel types are: kRgba8U " + "and kGrey8U"); if (publish_period <= 0) { - throw std::logic_error("ImageWriter: publish period must be positive"); + throw std::logic_error("RGBDSystem: publish period must be positive"); } // Now configure the system for the valid port declaration. @@ -144,18 +139,23 @@ void RGBDSystem::PubImage(const Context& context, int index) const { const auto& port = get_input_port(index); const Image& image = port.Eval>(context); - image_msgs.data.resize(image.width() * image.height() * 3); + int channels = 4; + std::string encoding = "rgba8"; + + if (kPixelType == PixelType::kRgba8U) { + channels = 4; + encoding = "rgba8"; + } else if (kPixelType == PixelType::kGrey8U) { + channels = 1; + encoding = "mono8"; + } + + image_msgs.data.resize(image.width() * image.height() * channels); image_msgs.height = image.height(); image_msgs.width = image.width(); - image_msgs.step = image.width() * 3; - image_msgs.encoding = "rgb8"; - for (int x = 0; x < image.width(); ++x) { - for (int y = 0; y < image.height(); ++y) { - image_msgs.data[3 * (image.width() * y + x)] = image.at(x, y)[0]; - image_msgs.data[3 * (image.width() * y + x) + 1] = image.at(x, y)[1]; - image_msgs.data[3 * (image.width() * y + x) + 2] = image.at(x, y)[2]; - } - } + image_msgs.step = image.width() * channels; + image_msgs.encoding = encoding; + memcpy(&image_msgs.data[0], image.at(0, 0), image_msgs.data.size()); } template @@ -163,18 +163,23 @@ void RGBDSystem::PubDepth(const Context& context, int index) const { const auto& port = get_input_port(index); const Image& image = port.Eval>(context); - depth_msgs.data.resize(image.width() * image.height() * sizeof(float)); + std::string encoding = "32FC1"; + int octets = sizeof(float); + + if (kPixelType == PixelType::kDepth32F) { + encoding = "32FC1"; + octets = sizeof(float); + } else if (kPixelType == PixelType::kDepth16U) { + encoding = "16UC1"; + octets = sizeof(uint16_t); + } + + depth_msgs.data.resize(image.width() * image.height() * octets); depth_msgs.height = image.height(); depth_msgs.width = image.width(); - depth_msgs.step = image.width() * sizeof(float); - depth_msgs.encoding = "32FC1"; - float* depth_row = reinterpret_cast(&depth_msgs.data[0]); - for (int x = 0; x < image.width(); ++x) { - for (int y = 0; y < image.height(); ++y) { - float value = image.at(x, y)[0]; - depth_row[image.width() * y + x] = value; - } - } + depth_msgs.step = image.width() * octets; + depth_msgs.encoding = encoding; + memcpy(&depth_msgs.data[0], image.at(0, 0), depth_msgs.data.size()); } template const InputPort& @@ -182,78 +187,42 @@ RGBDSystem::DeclareImageInputPort(std::string port_name, double publish_period, double start_time); template const InputPort& -RGBDSystem::DeclareImageInputPort(std::string port_name, - double publish_period, - double start_time); -template const InputPort& -RGBDSystem::DeclareImageInputPort(std::string port_name, - double publish_period, - double start_time); -template const InputPort& -RGBDSystem::DeclareImageInputPort(std::string port_name, - double publish_period, - double start_time); -template const InputPort& RGBDSystem::DeclareImageInputPort(std::string port_name, double publish_period, double start_time); template void RGBDSystem::PubImage( const Context& context, int index) const; -template void RGBDSystem::PubImage( - const Context& context, int index) const; -template void RGBDSystem::PubImage( - const Context& context, int index) const; -template void RGBDSystem::PubImage( - const Context& context, int index) const; template void RGBDSystem::PubImage( const Context& context, int index) const; template const InputPort& -RGBDSystem::DeclareDepthInputPort(std::string port_name, - double publish_period, - double start_time); -template const InputPort& RGBDSystem::DeclareDepthInputPort(std::string port_name, double publish_period, double start_time); template const InputPort& -RGBDSystem::DeclareDepthInputPort(std::string port_name, - double publish_period, - double start_time); -template const InputPort& RGBDSystem::DeclareDepthInputPort(std::string port_name, double publish_period, double start_time); -template const InputPort& -RGBDSystem::DeclareDepthInputPort(std::string port_name, - double publish_period, - double start_time); -template void RGBDSystem::PubDepth( - const Context& context, int index) const; template void RGBDSystem::PubDepth( const Context& context, int index) const; -template void RGBDSystem::PubDepth( - const Context& context, int index) const; template void RGBDSystem::PubDepth( const Context& context, int index) const; -template void RGBDSystem::PubDepth( - const Context& context, int index) const; std::pair RGBDSystem::AddToBuilder( drake::systems::DiagramBuilder* builder, DrakeRos* ros, - const std::string& topic_name, const rclcpp::QoS& qos, + const std::string& color_topic_name, const std::string& depth_topic_name, + const rclcpp::QoS& qos, const std::unordered_set& publish_triggers, double publish_period) { - // std::cout << "Add To builder" << std::endl; auto* pub_color_system = builder->AddSystem(RosPublisherSystem::Make( - topic_name, qos, ros, publish_triggers, publish_period)); + color_topic_name, qos, ros, publish_triggers, publish_period)); auto* pub_depth_system = builder->AddSystem(RosPublisherSystem::Make( - "depth", qos, ros, publish_triggers, publish_period)); + depth_topic_name, qos, ros, publish_triggers, publish_period)); return {pub_color_system, pub_depth_system}; } diff --git a/drake_ros/core/rgbd_system.h b/drake_ros/core/rgbd_system.h index 479b0d1e..f4150129 100644 --- a/drake_ros/core/rgbd_system.h +++ b/drake_ros/core/rgbd_system.h @@ -77,6 +77,7 @@ class RGBDSystem : public drake::systems::LeafSystem { static std::pair AddToBuilder( drake::systems::DiagramBuilder* builder, DrakeRos* ros, const std::string& topic_name = "/image", + const std::string& depth_topic_name = "/depth", const rclcpp::QoS& qos = rclcpp::SystemDefaultsQoS(), const std::unordered_set& publish_triggers = RosPublisherSystem::kDefaultTriggerTypes, diff --git a/drake_ros_examples/examples/rgbd/rgbd.cpp b/drake_ros_examples/examples/rgbd/rgbd.cpp index 466bc9d7..65d58a6a 100644 --- a/drake_ros_examples/examples/rgbd/rgbd.cpp +++ b/drake_ros_examples/examples/rgbd/rgbd.cpp @@ -104,9 +104,7 @@ int do_main() { parser.package_map().GetPath("drake_ros_examples")}; const std::string sdf_url = (fs_path / "rgbd/rgbd.sdf").string(); - Parser(&cart_pole, &scene_graph).AddAllModelsFromFile(sdf_url); - - // visualization::AddDefaultVisualization(&builder); + Parser(&cart_pole, &scene_graph).AddModels(sdf_url); drake_ros::core::init(); auto ros_interface_system = builder.AddSystem( @@ -134,7 +132,7 @@ int do_main() { {"renderer", {640, 480, M_PI_4}, {0.01, 10.0}, {}}, false}; const DepthRenderCamera depth_camera{color_camera.core(), {0.01, 10.0}}; const RigidTransformd X_WB = - ParseCameraPose("0.8, 0.0, 0.7, -2.2, 0.0, 1.57"); + ParseCameraPose("0.0, 1.0, -0.12, 1.57, 3.14, 0.0"); std::get<0>(camera_info_system) ->SetCameraInfo(color_camera.core().intrinsics()); @@ -147,7 +145,6 @@ int do_main() { builder.Connect(scene_graph.get_query_output_port(), camera->query_object_input_port()); - // Broadcast images via LCM for visualization (requires #18862 to see them). const double image_publish_period = 1. / 30; RGBDSystem* rgbd_publisher{nullptr}; rgbd_publisher = builder.template AddSystem(); diff --git a/drake_ros_examples/examples/rgbd/rgbd.sdf b/drake_ros_examples/examples/rgbd/rgbd.sdf index 6c47023d..499dd858 100644 --- a/drake_ros_examples/examples/rgbd/rgbd.sdf +++ b/drake_ros_examples/examples/rgbd/rgbd.sdf @@ -38,7 +38,7 @@ - 0.8 0.0 0.7 -2.2 0.0 1.57 + 0.0 1.0 -0.12 1.57 3.14 0.0