From 34e3491bdb580d6143f62808e9074bb0f85143b6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 26 Dec 2023 10:47:51 +0100 Subject: [PATCH] make linters happy 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/camera_info_system.cc | 17 ++- drake_ros/core/camera_info_system.h | 16 +-- drake_ros/core/rgbd_system.cc | 128 +++++++++++----------- drake_ros/core/rgbd_system.h | 34 +++--- drake_ros_examples/examples/rgbd/rgbd.cpp | 103 +++++++++-------- 5 files changed, 146 insertions(+), 152 deletions(-) diff --git a/drake_ros/core/camera_info_system.cc b/drake_ros/core/camera_info_system.cc index bc82353c..6e82373d 100644 --- a/drake_ros/core/camera_info_system.cc +++ b/drake_ros/core/camera_info_system.cc @@ -5,16 +5,16 @@ using drake_ros::core::CameraInfoSystem; using drake_ros::core::RosPublisherSystem; -CameraInfoSystem::CameraInfoSystem(): camera_info(1, 1, 1, 1, 0.5, 0.5) { - DeclareAbstractOutputPort("CameraInfoSystem", &CameraInfoSystem::CalcCameraInfo); +CameraInfoSystem::CameraInfoSystem() : camera_info(1, 1, 1, 1, 0.5, 0.5) { + DeclareAbstractOutputPort("CameraInfoSystem", + &CameraInfoSystem::CalcCameraInfo); } CameraInfoSystem::~CameraInfoSystem() {} void CameraInfoSystem::CalcCameraInfo( const drake::systems::Context& context, - sensor_msgs::msg::CameraInfo* output_value) const -{ + sensor_msgs::msg::CameraInfo* output_value) const { rclcpp::Time now{0, 0, RCL_ROS_TIME}; now += rclcpp::Duration::from_seconds(context.get_time()); output_value->header.stamp = now; @@ -45,17 +45,16 @@ void CameraInfoSystem::CalcCameraInfo( output_value->p[10] = 1.0; } -void CameraInfoSystem::SetCameraInfo(const CameraInfo & _camera_info) -{ +void CameraInfoSystem::SetCameraInfo(const CameraInfo& _camera_info) { this->camera_info = _camera_info; } -std::tuple CameraInfoSystem::AddToBuilder( +std::tuple +CameraInfoSystem::AddToBuilder( drake::systems::DiagramBuilder* builder, DrakeRos* ros, const std::string& topic_name, const rclcpp::QoS& qos, const std::unordered_set& publish_triggers, - double publish_period) -{ + double publish_period) { auto* camera_info_system = builder->AddSystem(); auto* pub_system = diff --git a/drake_ros/core/camera_info_system.h b/drake_ros/core/camera_info_system.h index 12746fd5..db80ff6e 100644 --- a/drake_ros/core/camera_info_system.h +++ b/drake_ros/core/camera_info_system.h @@ -7,16 +7,16 @@ #include #include +#include #include #include #include -#include - using drake::systems::sensors::CameraInfo; namespace drake_ros::core { -/** A system that convert's drake's time to a sensor_msgs/msg/CameraInfo message. +/** A system that convert's drake's time to a sensor_msgs/msg/CameraInfo + * message. */ class CameraInfoSystem : public drake::systems::LeafSystem { public: @@ -26,14 +26,14 @@ class CameraInfoSystem : public drake::systems::LeafSystem { ~CameraInfoSystem() override; - void SetCameraInfo(const CameraInfo & _camera_info); + void SetCameraInfo(const CameraInfo& _camera_info); /** Add a CameraInfoSystem and RosPublisherSystem to a diagram builder. * * This adds both a CameraInfoSystem and a RosPublisherSystem that publishes - * time to a `/image/camera_info` topic. All nodes should have their `use_sim_time` - * parameter set to `True` so they use the published topic as their source - * of time. + * time to a `/image/camera_info` topic. All nodes should have their + * `use_sim_time` parameter set to `True` so they use the published topic as + * their source of time. */ static std::tuple AddToBuilder( drake::systems::DiagramBuilder* builder, DrakeRos* ros, @@ -44,7 +44,7 @@ class CameraInfoSystem : public drake::systems::LeafSystem { double publish_period = 0.0); private: - CameraInfo camera_info; + CameraInfo camera_info; protected: void CalcCameraInfo(const drake::systems::Context& context, diff --git a/drake_ros/core/rgbd_system.cc b/drake_ros/core/rgbd_system.cc index bd67d75d..fece8919 100644 --- a/drake_ros/core/rgbd_system.cc +++ b/drake_ros/core/rgbd_system.cc @@ -1,20 +1,25 @@ #include "drake_ros/core/rgbd_system.h" +#include +#include +#include +#include +#include + #include +using drake::systems::sensors::Image; +using drake::systems::sensors::ImageRgba8U; using drake_ros::core::RGBDSystem; using drake_ros::core::RosPublisherSystem; -using drake::systems::sensors::ImageRgba8U; -using drake::systems::sensors::Image; -using Eigen::Vector3d; +using drake::Value; using drake::math::RigidTransformd; using drake::math::RollPitchYawd; -using drake::systems::PublishEvent; -using drake::Value; +using drake::systems::Context; using drake::systems::EventStatus; +using drake::systems::PublishEvent; using drake::systems::TriggerType; -using drake::systems::Context; RGBDSystem::RGBDSystem() { DeclareAbstractOutputPort("rgbd_color", &RGBDSystem::CalcColorImage); @@ -24,8 +29,7 @@ RGBDSystem::RGBDSystem() { RGBDSystem::~RGBDSystem() {} void RGBDSystem::CalcColorImage(const drake::systems::Context& context, - sensor_msgs::msg::Image* color_value) const -{ + sensor_msgs::msg::Image* color_value) const { color_value->header.frame_id = "CartPole/camera_optical"; rclcpp::Time now{0, 0, RCL_ROS_TIME}; @@ -41,8 +45,7 @@ void RGBDSystem::CalcColorImage(const drake::systems::Context& context, } void RGBDSystem::CalcDepthImage(const drake::systems::Context& context, - sensor_msgs::msg::Image* depth_value) const -{ + sensor_msgs::msg::Image* depth_value) const { depth_value->header.frame_id = "CartPole/camera_optical"; rclcpp::Time now{0, 0, RCL_ROS_TIME}; @@ -55,14 +58,12 @@ void RGBDSystem::CalcDepthImage(const drake::systems::Context& context, depth_value->encoding = depth_msgs.encoding; depth_value->data.resize(depth_msgs.data.size()); - float * depth_row = reinterpret_cast(&depth_msgs.data[0]); - float * depth_row_value = reinterpret_cast(&depth_value->data[0]); + float* depth_row = reinterpret_cast(&depth_msgs.data[0]); + float* depth_row_value = reinterpret_cast(&depth_value->data[0]); - for (size_t x = 0; x < depth_value->data.size() / 4; ++x) - { + 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]) - { + if (std::numeric_limits::infinity() == depth_row[x]) { depth_row_value[x] = 0.0f; } } @@ -70,9 +71,7 @@ void RGBDSystem::CalcDepthImage(const drake::systems::Context& context, template const InputPort& RGBDSystem::DeclareDepthInputPort( - std::string port_name, double publish_period, - double start_time) -{ + std::string port_name, double publish_period, double start_time) { // Test to confirm valid pixel type. static_assert(kPixelType == PixelType::kRgba8U || kPixelType == PixelType::kDepth32F || @@ -107,9 +106,7 @@ const InputPort& RGBDSystem::DeclareDepthInputPort( template const InputPort& RGBDSystem::DeclareImageInputPort( - std::string port_name, double publish_period, - double start_time) -{ + std::string port_name, double publish_period, double start_time) { // Test to confirm valid pixel type. static_assert(kPixelType == PixelType::kRgba8U || kPixelType == PixelType::kDepth32F || @@ -152,10 +149,8 @@ void RGBDSystem::PubImage(const Context& context, int index) const { 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) - { + 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]; @@ -173,32 +168,35 @@ void RGBDSystem::PubDepth(const Context& context, int index) const { 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* 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; } } } -template const InputPort& RGBDSystem::DeclareImageInputPort< - PixelType::kRgba8U>(std::string port_name, - double publish_period, double start_time); -template const InputPort& RGBDSystem::DeclareImageInputPort< - PixelType::kDepth32F>(std::string port_name, - double publish_period, double start_time); -template const InputPort& RGBDSystem::DeclareImageInputPort< - PixelType::kLabel16I>(std::string port_name, - double publish_period, double start_time); -template const InputPort& RGBDSystem::DeclareImageInputPort< - PixelType::kDepth16U>(std::string port_name, - double publish_period, double start_time); -template const InputPort& RGBDSystem::DeclareImageInputPort< - PixelType::kGrey8U>(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 const InputPort& +RGBDSystem::DeclareImageInputPort(std::string port_name, + double publish_period, + double start_time); template void RGBDSystem::PubImage( const Context& context, int index) const; @@ -211,21 +209,26 @@ template void RGBDSystem::PubImage( template void RGBDSystem::PubImage( const Context& context, int index) const; -template const InputPort& RGBDSystem::DeclareDepthInputPort< - PixelType::kRgba8U>(std::string port_name, - double publish_period, double start_time); -template const InputPort& RGBDSystem::DeclareDepthInputPort< - PixelType::kDepth32F>(std::string port_name, - double publish_period, double start_time); -template const InputPort& RGBDSystem::DeclareDepthInputPort< - PixelType::kLabel16I>(std::string port_name, - double publish_period, double start_time); -template const InputPort& RGBDSystem::DeclareDepthInputPort< - PixelType::kDepth16U>(std::string port_name, - double publish_period, double start_time); -template const InputPort& RGBDSystem::DeclareDepthInputPort< - PixelType::kGrey8U>(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 const InputPort& +RGBDSystem::DeclareDepthInputPort(std::string port_name, + double publish_period, + double start_time); template void RGBDSystem::PubDepth( const Context& context, int index) const; @@ -242,8 +245,7 @@ std::pair RGBDSystem::AddToBuilder( drake::systems::DiagramBuilder* builder, DrakeRos* ros, const std::string& topic_name, const rclcpp::QoS& qos, const std::unordered_set& publish_triggers, - double publish_period) -{ + double publish_period) { // std::cout << "Add To builder" << std::endl; auto* pub_color_system = builder->AddSystem(RosPublisherSystem::Make( diff --git a/drake_ros/core/rgbd_system.h b/drake_ros/core/rgbd_system.h index 358915e6..479b0d1e 100644 --- a/drake_ros/core/rgbd_system.h +++ b/drake_ros/core/rgbd_system.h @@ -4,26 +4,27 @@ #include #include #include +#include +#include +#include "drake/systems/sensors/rgbd_sensor.h" #include #include #include -#include #include -#include "drake/systems/sensors/rgbd_sensor.h" +#include -using drake::systems::sensors::RgbdSensor; -using drake::systems::sensors::PixelType; -using drake::systems::InputPort; using drake::systems::Context; - +using drake::systems::InputPort; +using drake::systems::sensors::PixelType; +using drake::systems::sensors::RgbdSensor; namespace drake_ros::core { /** A system that convert's drake's time to a sensor_msgs/msg/Image message. */ class RGBDSystem : public drake::systems::LeafSystem { -public: + public: /** A constructor for the rbgd system. */ RGBDSystem(); @@ -70,8 +71,8 @@ class RGBDSystem : public drake::systems::LeafSystem { * * This adds both a RGBDSystem and a RosPublisherSystem that publishes * time to a `/image` topic. All nodes should have their `use_sim_time` - * parameter set to `True` so they use the published topic as their source - * of time. + * parameter set to `True` so they use the published topic as their source of + * time. */ static std::pair AddToBuilder( drake::systems::DiagramBuilder* builder, DrakeRos* ros, @@ -81,18 +82,15 @@ class RGBDSystem : public drake::systems::LeafSystem { RosPublisherSystem::kDefaultTriggerTypes, double publish_period = 0.0); -// private: -// RgbdSensor * camera_; -private: -mutable sensor_msgs::msg::Image image_msgs; -mutable sensor_msgs::msg::Image depth_msgs; + private: + mutable sensor_msgs::msg::Image image_msgs; + mutable sensor_msgs::msg::Image depth_msgs; protected: void CalcColorImage(const drake::systems::Context& context, - sensor_msgs::msg::Image* color_value) const; + sensor_msgs::msg::Image* color_value) const; void CalcDepthImage(const drake::systems::Context& context, - sensor_msgs::msg::Image* depth_value) const; - + sensor_msgs::msg::Image* depth_value) const; }; -} +} // namespace drake_ros::core diff --git a/drake_ros_examples/examples/rgbd/rgbd.cpp b/drake_ros_examples/examples/rgbd/rgbd.cpp index 0092d519..69b4bab5 100644 --- a/drake_ros_examples/examples/rgbd/rgbd.cpp +++ b/drake_ros_examples/examples/rgbd/rgbd.cpp @@ -1,39 +1,33 @@ #include -#include - -#include - #include "drake/common/drake_assert.h" +#include "drake/geometry/drake_visualizer.h" +#include "drake/geometry/render_vtk/factory.h" #include "drake/geometry/scene_graph.h" +#include "drake/lcm/drake_lcm.h" +#include "drake/math/rigid_transform.h" #include "drake/multibody/parsing/parser.h" #include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/tree/prismatic_joint.h" #include "drake/multibody/tree/revolute_joint.h" #include "drake/systems/analysis/simulator.h" #include "drake/systems/framework/diagram_builder.h" -#include "drake/visualization/visualization_config_functions.h" - -#include -#include -#include -#include -#include -#include - #include "drake/systems/lcm/lcm_publisher_system.h" #include "drake/systems/sensors/image.h" #include "drake/systems/sensors/image_to_lcm_image_array_t.h" #include "drake/systems/sensors/image_writer.h" #include "drake/systems/sensors/pixel_types.h" #include "drake/systems/sensors/rgbd_sensor.h" +#include "drake/visualization/visualization_config_functions.h" +#include +#include +#include +#include +#include +#include #include - -#include "drake/geometry/drake_visualizer.h" - -#include "drake/geometry/render_vtk/factory.h" -#include "drake/math/rigid_transform.h" -#include "drake/lcm/drake_lcm.h" +#include +#include namespace drake { namespace examples { @@ -52,27 +46,26 @@ using drake::multibody::RevoluteJoint; using drake_ros::core::CameraInfoSystem; using drake_ros::core::ClockSystem; -using drake_ros::core::RGBDSystem; using drake_ros::core::DrakeRos; +using drake_ros::core::RGBDSystem; using drake_ros::core::RosInterfaceSystem; using drake_ros::viz::RvizVisualizer; - using geometry::render::ColorRenderCamera; using geometry::render::DepthRenderCamera; -using Eigen::Vector3d; using drake::math::RigidTransformd; using drake::math::RollPitchYawd; +using Eigen::Vector3d; using drake::lcm::DrakeLcm; -using drake::systems::sensors::PixelType; -using drake::systems::sensors::RgbdSensor; +using drake::systems::TriggerType; using drake::systems::lcm::LcmPublisherSystem; using drake::systems::sensors::ImageToLcmImageArrayT; using drake::systems::sensors::ImageWriter; -using drake::systems::TriggerType; +using drake::systems::sensors::PixelType; +using drake::systems::sensors::RgbdSensor; using drake::geometry::RenderEngineVtkParams; using drake::multibody::ContactModel; @@ -85,9 +78,9 @@ DEFINE_double(simulation_time, 100.0, "Desired duration of the simulation in seconds."); DEFINE_double(time_step, 0, - "If greater than zero, the plant is modeled as a system with " - "discrete updates and period equal to this time_step. " - "If 0, the plant is modeled as a continuous system."); + "If greater than zero, the plant is modeled as a system with " + "discrete updates and period equal to this time_step. " + "If 0, the plant is modeled as a continuous system."); RigidTransformd ParseCameraPose(const std::string& input_str) { const char delimiter = ','; @@ -117,7 +110,7 @@ int do_main() { AddMultibodyPlantSceneGraph(&builder, FLAGS_time_step); scene_graph.AddRenderer("renderer", - MakeRenderEngineVtk(RenderEngineVtkParams())); + MakeRenderEngineVtk(RenderEngineVtkParams())); auto parser = Parser(&cart_pole); @@ -127,8 +120,7 @@ int do_main() { std::filesystem::path fs_path{ parser.package_map().GetPath("drake_ros_examples")}; - const std::string sdf_url = - (fs_path / "rgbd/rgbd.sdf").string(); + const std::string sdf_url = (fs_path / "rgbd/rgbd.sdf").string(); Parser(&cart_pole, &scene_graph).AddAllModelsFromFile(sdf_url); // visualization::AddDefaultVisualization(&builder); @@ -148,22 +140,25 @@ int do_main() { ClockSystem::AddToBuilder(&builder, ros_interface_system->get_ros_interface()); - auto camera_info_system = CameraInfoSystem::AddToBuilder(&builder, - ros_interface_system->get_ros_interface()); + auto camera_info_system = CameraInfoSystem::AddToBuilder( + &builder, ros_interface_system->get_ros_interface()); - auto depth_camera_info_system = CameraInfoSystem::AddToBuilder(&builder, - ros_interface_system->get_ros_interface(), - "/depth/camera_info"); + auto depth_camera_info_system = CameraInfoSystem::AddToBuilder( + &builder, ros_interface_system->get_ros_interface(), + "/depth/camera_info"); DrakeLcm lcm; drake::geometry::DrakeVisualizerd::AddToBuilder(&builder, scene_graph, &lcm); const ColorRenderCamera color_camera{ {"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"); + const RigidTransformd X_WB = + ParseCameraPose("0.8, 0.0, 0.7, -2.2, 0.0, 1.57"); - std::get<0>(camera_info_system)->SetCameraInfo(color_camera.core().intrinsics()); - std::get<0>(depth_camera_info_system)->SetCameraInfo(depth_camera.core().intrinsics()); + std::get<0>(camera_info_system) + ->SetCameraInfo(color_camera.core().intrinsics()); + std::get<0>(depth_camera_info_system) + ->SetCameraInfo(depth_camera.core().intrinsics()); const auto world_id = scene_graph.world_frame_id(); RgbdSensor* camera = @@ -186,14 +181,15 @@ int do_main() { builder.Connect(image_to_lcm_image_array->image_array_t_msg_output_port(), image_array_lcm_publisher->get_input_port()); -// ImageWriter* image_writer{nullptr}; -// image_writer = builder.template AddSystem(); + // ImageWriter* image_writer{nullptr}; + // image_writer = builder.template AddSystem(); RGBDSystem* rgbd_publisher{nullptr}; rgbd_publisher = builder.template AddSystem(); const std::string filename = - (std::filesystem::path("/home/ahcorde/drake_ros_ws/images") / "{image_type}_{count:03}") + (std::filesystem::path("/home/ahcorde/drake_ros_ws/images") / + "{image_type}_{count:03}") .string(); const auto& port = @@ -226,19 +222,18 @@ int do_main() { builder.Connect(scene_graph.get_query_output_port(), scene_tf_broadcaster->get_graph_query_input_port()); -// const auto& writer_port = -// image_writer->DeclareImageInputPort( -// "depth", filename, image_publish_period, 0.); -// builder.Connect(camera->depth_image_32F_output_port(), writer_port); + // const auto& writer_port = + // image_writer->DeclareImageInputPort( + // "depth", filename, image_publish_period, 0.); + // builder.Connect(camera->depth_image_32F_output_port(), writer_port); -// const auto& writer_port_color = -// image_writer->DeclareImageInputPort( -// "color", filename, image_publish_period, 0.); -// builder.Connect(camera->color_image_output_port(), writer_port_color); + // const auto& writer_port_color = + // image_writer->DeclareImageInputPort( + // "color", filename, image_publish_period, 0.); + // builder.Connect(camera->color_image_output_port(), writer_port_color); - auto [pub_color_system, pub_depth_system] = - RGBDSystem::AddToBuilder(&builder, - ros_interface_system->get_ros_interface()); + auto [pub_color_system, pub_depth_system] = RGBDSystem::AddToBuilder( + &builder, ros_interface_system->get_ros_interface()); builder.Connect(rgbd_publisher->GetOutputPort("rgbd_color"), pub_color_system->get_input_port()); @@ -247,7 +242,7 @@ int do_main() { pub_depth_system->get_input_port()); // Now the model is complete. -// cart_pole.set_contact_model(ContactModel::kPoint); + // cart_pole.set_contact_model(ContactModel::kPoint); cart_pole.Finalize(); auto diagram = builder.Build();