diff --git a/README.md b/README.md index 4b8febf7d..c8928b5fb 100644 --- a/README.md +++ b/README.md @@ -15,12 +15,14 @@ For some displays, the [documentation is updated](docs/FEATURES.md). | Displays | Tools | View Controller | Panels | | --------------------- | ------------- | --------------------- | --------------- | | Camera | Move Camera | Orbit | Displays | -| Grid | Focus Camera | XY Orbit | Help | -| Grid Cells | Measure | First Person | Selections | -| Image | Select | Third Person Follower | Tool Properties | -| Laser Scan | 2D Nav Goal | Top Down Orthographic | Views | -| Map | Publish Point | | | -| Marker | Initial Pose | +| Fluid Pressure | Focus Camera | XY Orbit | Help | +| Grid | Measure | First Person | Selections | +| Grid Cells | Select | Third Person Follower | Tool Properties | +| Illuminance | 2D Nav Goal | Top Down Orthographic | Views | +| Image | Publish Point | +| Laser Scan | Initial Pose | +| Map | +| Marker | | Marker Array | | Odometry | | Point Cloud (1 and 2) | @@ -29,9 +31,12 @@ For some displays, the [documentation is updated](docs/FEATURES.md). | Pose | | Pose Array | | Range | +| Relative Humidity | | Robot Model | +| Temperature | | TF | + ### Not yet ported These features have not been ported to `ros2/rviz` yet. @@ -40,13 +45,9 @@ These features have not been ported to `ros2/rviz` yet. | Axes | Interact | Time | | DepthCloud | | Effort | -| Fluid Pressure | -| Illuminance | | Interactive Marker | | Oculus | | Pose With Covariance | -| Relative Humidity | -| Temperature | | Wrench | Other features: diff --git a/rviz/src/rviz/default_plugin/fluid_pressure_display.cpp b/rviz/src/rviz/default_plugin/fluid_pressure_display.cpp deleted file mode 100644 index 3713bd41f..000000000 --- a/rviz/src/rviz/default_plugin/fluid_pressure_display.cpp +++ /dev/null @@ -1,156 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -#include - -#include "rviz/default_plugin/point_cloud_common.h" -#include "rviz/default_plugin/point_cloud_transformers.h" -#include "rviz/display_context.h" -#include "rviz/frame_manager.h" -#include "rviz/ogre_helpers/point_cloud.h" -#include "rviz/properties/int_property.h" -#include "rviz/validate_floats.h" - -#include "fluid_pressure_display.h" - -namespace rviz -{ - -FluidPressureDisplay::FluidPressureDisplay() - : point_cloud_common_( new PointCloudCommon( this )) -{ - queue_size_property_ = new IntProperty( "Queue Size", 10, - "Advanced: set the size of the incoming FluidPressure message queue. " - " Increasing this is useful if your incoming TF data is delayed significantly " - "from your FluidPressure data, but it can greatly increase memory usage if the messages are big.", - this, SLOT( updateQueueSize() )); - - // PointCloudCommon sets up a callback queue with a thread for each - // instance. Use that for processing incoming messages. - update_nh_.setCallbackQueue( point_cloud_common_->getCallbackQueue() ); -} - -FluidPressureDisplay::~FluidPressureDisplay() -{ - delete point_cloud_common_; -} - -void FluidPressureDisplay::onInitialize() -{ - MFDClass::onInitialize(); - point_cloud_common_->initialize( context_, scene_node_ ); - - // Set correct initial values - subProp("Channel Name")->setValue("fluid_pressure"); - subProp("Autocompute Intensity Bounds")->setValue(false); - subProp("Min Intensity")->setValue(98000); // Typical 'low' atmosphereic pressure in Pascal - subProp("Max Intensity")->setValue(105000); // Typica 'high' atmosphereic pressure in Pascal -} - -void FluidPressureDisplay::updateQueueSize() -{ - tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() ); -} - -void FluidPressureDisplay::processMessage( const sensor_msgs::FluidPressureConstPtr& msg ) -{ - // Filter any nan values out of the cloud. Any nan values that make it through to PointCloudBase - // will get their points put off in lala land, but it means they still do get processed/rendered - // which can be a big performance hit - sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2); - - // Create fields - sensor_msgs::PointField x; - x.name = "x"; - x.offset = 0; - x.datatype = sensor_msgs::PointField::FLOAT32; - x.count = 1; - sensor_msgs::PointField y; - y.name = "y"; - y.offset = 4; - y.datatype = sensor_msgs::PointField::FLOAT32; - y.count = 1; - sensor_msgs::PointField z; - z.name = "z"; - z.offset = 8; - z.datatype = sensor_msgs::PointField::FLOAT32; - z.count = 1; - sensor_msgs::PointField fluid_pressure; - fluid_pressure.name = "fluid_pressure"; - fluid_pressure.offset = 12; - fluid_pressure.datatype = sensor_msgs::PointField::FLOAT64; - fluid_pressure.count = 1; - - // Create pointcloud from message - filtered->header = msg->header; - filtered->fields.push_back(x); - filtered->fields.push_back(y); - filtered->fields.push_back(z); - filtered->fields.push_back(fluid_pressure); - filtered->data.resize(20); - const float zero_float = 0.0; // FluidPressure is always on its tf frame - memcpy(&filtered->data[x.offset], &zero_float, 4); - memcpy(&filtered->data[y.offset], &zero_float, 4); - memcpy(&filtered->data[z.offset], &zero_float, 4); - memcpy(&filtered->data[fluid_pressure.offset], &msg->fluid_pressure, 8); - filtered->height = 1; - filtered->width = 1; - filtered->is_bigendian = false; - filtered->point_step = 20; - filtered->row_step = 1; - - // Give to point_cloud_common to draw - point_cloud_common_->addMessage( filtered ); -} - - -void FluidPressureDisplay::update( float wall_dt, float ros_dt ) -{ - point_cloud_common_->update( wall_dt, ros_dt ); - - // Hide unneeded properties - subProp("Position Transformer")->hide(); - subProp("Color Transformer")->hide(); - subProp("Channel Name")->hide(); - subProp("Autocompute Intensity Bounds")->hide(); -} - -void FluidPressureDisplay::reset() -{ - MFDClass::reset(); - point_cloud_common_->reset(); -} - -} // namespace rviz - -#include -PLUGINLIB_EXPORT_CLASS( rviz::FluidPressureDisplay, rviz::Display ) diff --git a/rviz/src/rviz/default_plugin/illuminance_display.cpp b/rviz/src/rviz/default_plugin/illuminance_display.cpp deleted file mode 100644 index b5168979a..000000000 --- a/rviz/src/rviz/default_plugin/illuminance_display.cpp +++ /dev/null @@ -1,156 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -#include - -#include "rviz/default_plugin/point_cloud_common.h" -#include "rviz/default_plugin/point_cloud_transformers.h" -#include "rviz/display_context.h" -#include "rviz/frame_manager.h" -#include "rviz/ogre_helpers/point_cloud.h" -#include "rviz/properties/int_property.h" -#include "rviz/validate_floats.h" - -#include "illuminance_display.h" - -namespace rviz -{ - -IlluminanceDisplay::IlluminanceDisplay() - : point_cloud_common_( new PointCloudCommon( this )) -{ - queue_size_property_ = new IntProperty( "Queue Size", 10, - "Advanced: set the size of the incoming Illuminance message queue. " - " Increasing this is useful if your incoming TF data is delayed significantly " - "from your Illuminance data, but it can greatly increase memory usage if the messages are big.", - this, SLOT( updateQueueSize() )); - - // PointCloudCommon sets up a callback queue with a thread for each - // instance. Use that for processing incoming messages. - update_nh_.setCallbackQueue( point_cloud_common_->getCallbackQueue() ); -} - -IlluminanceDisplay::~IlluminanceDisplay() -{ - delete point_cloud_common_; -} - -void IlluminanceDisplay::onInitialize() -{ - MFDClass::onInitialize(); - point_cloud_common_->initialize( context_, scene_node_ ); - - // Set correct initial values - subProp("Channel Name")->setValue("illuminance"); - subProp("Autocompute Intensity Bounds")->setValue(false); - subProp("Min Intensity")->setValue(0); - subProp("Max Intensity")->setValue(1000); -} - -void IlluminanceDisplay::updateQueueSize() -{ - tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() ); -} - -void IlluminanceDisplay::processMessage( const sensor_msgs::IlluminanceConstPtr& msg ) -{ - // Filter any nan values out of the cloud. Any nan values that make it through to PointCloudBase - // will get their points put off in lala land, but it means they still do get processed/rendered - // which can be a big performance hit - sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2); - - // Create fields - sensor_msgs::PointField x; - x.name = "x"; - x.offset = 0; - x.datatype = sensor_msgs::PointField::FLOAT32; - x.count = 1; - sensor_msgs::PointField y; - y.name = "y"; - y.offset = 4; - y.datatype = sensor_msgs::PointField::FLOAT32; - y.count = 1; - sensor_msgs::PointField z; - z.name = "z"; - z.offset = 8; - z.datatype = sensor_msgs::PointField::FLOAT32; - z.count = 1; - sensor_msgs::PointField illuminance; - illuminance.name = "illuminance"; - illuminance.offset = 12; - illuminance.datatype = sensor_msgs::PointField::FLOAT64; - illuminance.count = 1; - - // Create pointcloud from message - filtered->header = msg->header; - filtered->fields.push_back(x); - filtered->fields.push_back(y); - filtered->fields.push_back(z); - filtered->fields.push_back(illuminance); - filtered->data.resize(20); - const float zero_float = 0.0; // Illuminance is always on its tf frame - memcpy(&filtered->data[x.offset], &zero_float, 4); - memcpy(&filtered->data[y.offset], &zero_float, 4); - memcpy(&filtered->data[z.offset], &zero_float, 4); - memcpy(&filtered->data[illuminance.offset], &msg->illuminance, 8); - filtered->height = 1; - filtered->width = 1; - filtered->is_bigendian = false; - filtered->point_step = 20; - filtered->row_step = 1; - - // Give to point_cloud_common to draw - point_cloud_common_->addMessage( filtered ); -} - - -void IlluminanceDisplay::update( float wall_dt, float ros_dt ) -{ - point_cloud_common_->update( wall_dt, ros_dt ); - - // Hide unneeded properties - subProp("Position Transformer")->hide(); - subProp("Color Transformer")->hide(); - subProp("Channel Name")->hide(); - subProp("Autocompute Intensity Bounds")->hide(); -} - -void IlluminanceDisplay::reset() -{ - MFDClass::reset(); - point_cloud_common_->reset(); -} - -} // namespace rviz - -#include -PLUGINLIB_EXPORT_CLASS( rviz::IlluminanceDisplay, rviz::Display ) diff --git a/rviz/src/rviz/default_plugin/relative_humidity_display.cpp b/rviz/src/rviz/default_plugin/relative_humidity_display.cpp deleted file mode 100644 index 2039e3bd7..000000000 --- a/rviz/src/rviz/default_plugin/relative_humidity_display.cpp +++ /dev/null @@ -1,153 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -#include - -#include "rviz/default_plugin/point_cloud_common.h" -#include "rviz/default_plugin/point_cloud_transformers.h" -#include "rviz/display_context.h" -#include "rviz/frame_manager.h" -#include "rviz/ogre_helpers/point_cloud.h" -#include "rviz/properties/int_property.h" -#include "rviz/validate_floats.h" - -#include "relative_humidity_display.h" - -namespace rviz -{ - -RelativeHumidityDisplay::RelativeHumidityDisplay() - : point_cloud_common_( new PointCloudCommon( this )) -{ - queue_size_property_ = new IntProperty( "Queue Size", 10, - "Advanced: set the size of the incoming RelativeHumidity message queue. " - " Increasing this is useful if your incoming TF data is delayed significantly " - "from your RelativeHumidity data, but it can greatly increase memory usage if the messages are big.", - this, SLOT( updateQueueSize() )); - - // PointCloudCommon sets up a callback queue with a thread for each - // instance. Use that for processing incoming messages. - update_nh_.setCallbackQueue( point_cloud_common_->getCallbackQueue() ); -} - -RelativeHumidityDisplay::~RelativeHumidityDisplay() -{ - delete point_cloud_common_; -} - -void RelativeHumidityDisplay::onInitialize() -{ - MFDClass::onInitialize(); - point_cloud_common_->initialize( context_, scene_node_ ); - - // Set correct initial values - subProp("Channel Name")->setValue("relative_humidity"); - subProp("Autocompute Intensity Bounds")->setValue(false); - subProp("Min Intensity")->setValue(0.0); // 0% relative humidity - subProp("Max Intensity")->setValue(1.0); // 100% relative humidity -} - -void RelativeHumidityDisplay::updateQueueSize() -{ - tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() ); -} - -void RelativeHumidityDisplay::processMessage( const sensor_msgs::RelativeHumidityConstPtr& msg ) -{ - sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2); - - // Create fields - sensor_msgs::PointField x; - x.name = "x"; - x.offset = 0; - x.datatype = sensor_msgs::PointField::FLOAT32; - x.count = 1; - sensor_msgs::PointField y; - y.name = "y"; - y.offset = 4; - y.datatype = sensor_msgs::PointField::FLOAT32; - y.count = 1; - sensor_msgs::PointField z; - z.name = "z"; - z.offset = 8; - z.datatype = sensor_msgs::PointField::FLOAT32; - z.count = 1; - sensor_msgs::PointField relative_humidity; - relative_humidity.name = "relative_humidity"; - relative_humidity.offset = 12; - relative_humidity.datatype = sensor_msgs::PointField::FLOAT64; - relative_humidity.count = 1; - - // Create pointcloud from message - filtered->header = msg->header; - filtered->fields.push_back(x); - filtered->fields.push_back(y); - filtered->fields.push_back(z); - filtered->fields.push_back(relative_humidity); - filtered->data.resize(20); - const float zero_float = 0.0; // RelativeHumidity is always on its tf frame - memcpy(&filtered->data[x.offset], &zero_float, 4); - memcpy(&filtered->data[y.offset], &zero_float, 4); - memcpy(&filtered->data[z.offset], &zero_float, 4); - memcpy(&filtered->data[relative_humidity.offset], &msg->relative_humidity, 8); - filtered->height = 1; - filtered->width = 1; - filtered->is_bigendian = false; - filtered->point_step = 20; - filtered->row_step = 1; - - // Give to point_cloud_common to draw - point_cloud_common_->addMessage( filtered ); -} - - -void RelativeHumidityDisplay::update( float wall_dt, float ros_dt ) -{ - point_cloud_common_->update( wall_dt, ros_dt ); - - // Hide unneeded properties - subProp("Position Transformer")->hide(); - subProp("Color Transformer")->hide(); - subProp("Channel Name")->hide(); - subProp("Autocompute Intensity Bounds")->hide(); -} - -void RelativeHumidityDisplay::reset() -{ - MFDClass::reset(); - point_cloud_common_->reset(); -} - -} // namespace rviz - -#include -PLUGINLIB_EXPORT_CLASS( rviz::RelativeHumidityDisplay, rviz::Display ) diff --git a/rviz/src/rviz/default_plugin/temperature_display.cpp b/rviz/src/rviz/default_plugin/temperature_display.cpp deleted file mode 100644 index e57c3b984..000000000 --- a/rviz/src/rviz/default_plugin/temperature_display.cpp +++ /dev/null @@ -1,155 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -#include - -#include "rviz/default_plugin/point_cloud_common.h" -#include "rviz/default_plugin/point_cloud_transformers.h" -#include "rviz/display_context.h" -#include "rviz/frame_manager.h" -#include "rviz/ogre_helpers/point_cloud.h" -#include "rviz/properties/int_property.h" -#include "rviz/validate_floats.h" - -#include "temperature_display.h" - -namespace rviz -{ - -TemperatureDisplay::TemperatureDisplay() - : point_cloud_common_( new PointCloudCommon( this )) -{ - queue_size_property_ = new IntProperty( "Queue Size", 10, - "Advanced: set the size of the incoming Temperature message queue. " - " Increasing this is useful if your incoming TF data is delayed significantly " - "from your Temperature data, but it can greatly increase memory usage if the messages are big.", - this, SLOT( updateQueueSize() )); - - // PointCloudCommon sets up a callback queue with a thread for each - // instance. Use that for processing incoming messages. - update_nh_.setCallbackQueue( point_cloud_common_->getCallbackQueue() ); -} - -TemperatureDisplay::~TemperatureDisplay() -{ - delete point_cloud_common_; -} - -void TemperatureDisplay::onInitialize() -{ - MFDClass::onInitialize(); - point_cloud_common_->initialize( context_, scene_node_ ); - - // Set correct initial values - subProp("Channel Name")->setValue("temperature"); - subProp("Autocompute Intensity Bounds")->setValue(false); - subProp("Invert Rainbow")->setValue(true); - subProp("Min Intensity")->setValue(0); // Water Freezing - subProp("Max Intensity")->setValue(100); // Water Boiling -} - -void TemperatureDisplay::updateQueueSize() -{ - tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() ); -} - -void TemperatureDisplay::processMessage( const sensor_msgs::TemperatureConstPtr& msg ) -{ - sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2); - - // Create fields - sensor_msgs::PointField x; - x.name = "x"; - x.offset = 0; - x.datatype = sensor_msgs::PointField::FLOAT32; - x.count = 1; - sensor_msgs::PointField y; - y.name = "y"; - y.offset = 4; - y.datatype = sensor_msgs::PointField::FLOAT32; - y.count = 1; - sensor_msgs::PointField z; - z.name = "z"; - z.offset = 8; - z.datatype = sensor_msgs::PointField::FLOAT32; - z.count = 1; - sensor_msgs::PointField temperature; - temperature.name = "temperature"; - temperature.offset = 12; - temperature.datatype = sensor_msgs::PointField::FLOAT64; - temperature.count = 1; - - // Create pointcloud from message - filtered->header = msg->header; - filtered->fields.push_back(x); - filtered->fields.push_back(y); - filtered->fields.push_back(z); - filtered->fields.push_back(temperature); - filtered->data.resize(20); - const float zero_float = 0.0; // RelativeHumidity is always on its tf frame - memcpy(&filtered->data[x.offset], &zero_float, 4); - memcpy(&filtered->data[y.offset], &zero_float, 4); - memcpy(&filtered->data[z.offset], &zero_float, 4); - memcpy(&filtered->data[temperature.offset], &msg->temperature, 8); - filtered->height = 1; - filtered->width = 1; - filtered->is_bigendian = false; - filtered->point_step = 20; - filtered->row_step = 1; - - // Give to point_cloud_common to draw - point_cloud_common_->addMessage( filtered ); -} - - -void TemperatureDisplay::update( float wall_dt, float ros_dt ) -{ - point_cloud_common_->update( wall_dt, ros_dt ); - - // Hide unneeded properties - subProp("Position Transformer")->hide(); - subProp("Color Transformer")->hide(); - subProp("Channel Name")->hide(); - subProp("Invert Rainbow")->hide(); - subProp("Autocompute Intensity Bounds")->hide(); -} - -void TemperatureDisplay::reset() -{ - MFDClass::reset(); - point_cloud_common_->reset(); -} - -} // namespace rviz - -#include -PLUGINLIB_EXPORT_CLASS( rviz::TemperatureDisplay, rviz::Display ) diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index ed00ee097..0b569b696 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/rviz_default_plugins/CMakeLists.txt @@ -74,8 +74,10 @@ find_package(visualization_msgs REQUIRED) # These need to be added in the add_library() call so AUTOMOC detects them. set(rviz_default_plugins_headers_to_moc include/rviz_default_plugins/displays/camera/camera_display.hpp + include/rviz_default_plugins/displays/fluid_pressure/fluid_pressure_display.hpp include/rviz_default_plugins/displays/grid/grid_display.hpp include/rviz_default_plugins/displays/grid_cells/grid_cells_display.hpp + include/rviz_default_plugins/displays/illuminance/illuminance_display.hpp include/rviz_default_plugins/displays/image/image_display.hpp include/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp include/rviz_default_plugins/displays/map/map_display.hpp @@ -92,6 +94,7 @@ set(rviz_default_plugins_headers_to_moc include/rviz_default_plugins/displays/pointcloud/point_cloud_transformer.hpp include/rviz_default_plugins/displays/pointcloud/point_cloud_transformer_factory.hpp include/rviz_default_plugins/displays/pointcloud/point_cloud_helpers.hpp + include/rviz_default_plugins/displays/pointcloud/point_cloud_scalar_display.hpp include/rviz_default_plugins/displays/pointcloud/transformers/axis_color_pc_transformer.hpp include/rviz_default_plugins/displays/pointcloud/transformers/flat_color_pc_transformer.hpp include/rviz_default_plugins/displays/pointcloud/transformers/intensity_pc_transformer.hpp @@ -101,7 +104,9 @@ set(rviz_default_plugins_headers_to_moc include/rviz_default_plugins/displays/pose/pose_display.hpp include/rviz_default_plugins/displays/pose_array/pose_array_display.hpp include/rviz_default_plugins/displays/range/range_display.hpp + include/rviz_default_plugins/displays/relative_humidity/relative_humidity_display.hpp include/rviz_default_plugins/displays/robot_model/robot_model_display.hpp + include/rviz_default_plugins/displays/temperature/temperature_display.hpp include/rviz_default_plugins/displays/tf/frame_info.hpp include/rviz_default_plugins/displays/tf/tf_display.hpp include/rviz_default_plugins/robot/robot.hpp @@ -124,6 +129,8 @@ set(rviz_default_plugins_source_files src/rviz_default_plugins/displays/camera/camera_display.cpp src/rviz_default_plugins/displays/grid/grid_display.cpp src/rviz_default_plugins/displays/grid_cells/grid_cells_display.cpp + src/rviz_default_plugins/displays/fluid_pressure/fluid_pressure_display.cpp + src/rviz_default_plugins/displays/illuminance/illuminance_display.cpp src/rviz_default_plugins/displays/image/image_display.cpp src/rviz_default_plugins/displays/image/ros_image_texture.cpp src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp @@ -166,7 +173,9 @@ set(rviz_default_plugins_source_files src/rviz_default_plugins/displays/pose_array/pose_array_display.cpp src/rviz_default_plugins/displays/pose_array/flat_arrows_array.cpp src/rviz_default_plugins/displays/range/range_display.cpp + src/rviz_default_plugins/displays/relative_humidity/relative_humidity_display.cpp src/rviz_default_plugins/displays/robot_model/robot_model_display.cpp + src/rviz_default_plugins/displays/temperature/temperature_display.cpp src/rviz_default_plugins/displays/tf/frame_info.cpp src/rviz_default_plugins/displays/tf/frame_selection_handler.cpp src/rviz_default_plugins/displays/tf/tf_display.cpp @@ -480,6 +489,15 @@ if(BUILD_TESTING) target_link_libraries(point_cloud_common_test ${TEST_LINK_LIBRARIES}) endif() + ament_add_gmock(point_cloud_scalar_display_test + test/rviz_default_plugins/displays/pointcloud/point_cloud_scalar_display_test.cpp + test/rviz_default_plugins/displays/display_test_fixture.cpp + ${SKIP_DISPLAY_TESTS}) + if(TARGET point_cloud_scalar_display_test) + target_include_directories(point_cloud_scalar_display_test PUBLIC ${TEST_INCLUDE_DIRS}) + target_link_libraries(point_cloud_scalar_display_test ${TEST_LINK_LIBRARIES}) + endif() + ament_add_gmock(point_cloud_transformers_test test/rviz_default_plugins/pointcloud_messages.hpp test/rviz_default_plugins/pointcloud_messages.cpp @@ -591,7 +609,6 @@ if(BUILD_TESTING) test/rviz_default_plugins/displays/camera/camera_display_visual_test.cpp test/rviz_default_plugins/publishers/camera_info_publisher.hpp test/rviz_default_plugins/page_objects/camera_display_page_object.cpp - test/rviz_default_plugins/page_objects/point_cloud_display_page_object.cpp test/rviz_default_plugins/page_objects/point_cloud_common_page_object.cpp test/rviz_default_plugins/publishers/image_publisher.hpp test/rviz_default_plugins/publishers/point_cloud_publisher.hpp @@ -606,6 +623,22 @@ if(BUILD_TESTING) rviz_visual_testing_framework::rviz_visual_testing_framework) endif() + ament_add_gtest(fluid_pressure_display_visual_test + test/rviz_default_plugins/displays/fluid_pressure/fluid_pressure_display_visual_test.cpp + test/rviz_default_plugins/page_objects/point_cloud_common_page_object.cpp + test/rviz_default_plugins/pointcloud_messages.cpp + test/rviz_default_plugins/publishers/fluid_pressure_publisher.hpp + ${SKIP_VISUAL_TESTS} + TIMEOUT 180) + if(TARGET fluid_pressure_display_visual_test) + target_include_directories(fluid_pressure_display_visual_test PUBLIC + ${TEST_INCLUDE_DIRS} + ${rviz_visual_testing_framework_INCLUDE_DIRS}) + target_link_libraries(fluid_pressure_display_visual_test + ${TEST_LINK_LIBRARIES} + rviz_visual_testing_framework::rviz_visual_testing_framework) + endif() + ament_add_gtest(grid_display_visual_test test/rviz_default_plugins/displays/grid/grid_display_visual_test.cpp test/rviz_default_plugins/page_objects/grid_display_page_object.cpp @@ -634,6 +667,22 @@ if(BUILD_TESTING) rviz_visual_testing_framework::rviz_visual_testing_framework) endif() + ament_add_gtest(illuminance_display_visual_test + test/rviz_default_plugins/displays/illuminance/illuminance_display_visual_test.cpp + test/rviz_default_plugins/page_objects/point_cloud_common_page_object.cpp + test/rviz_default_plugins/pointcloud_messages.cpp + test/rviz_default_plugins/publishers/illuminance_publisher.hpp + ${SKIP_VISUAL_TESTS} + TIMEOUT 180) + if(TARGET illuminance_display_visual_test) + target_include_directories(illuminance_display_visual_test PUBLIC + ${TEST_INCLUDE_DIRS} + ${rviz_visual_testing_framework_INCLUDE_DIRS}) + target_link_libraries(illuminance_display_visual_test + ${TEST_LINK_LIBRARIES} + rviz_visual_testing_framework::rviz_visual_testing_framework) + endif() + ament_add_gtest(image_display_visual_test test/rviz_default_plugins/displays/image/image_display_visual_test.cpp test/rviz_default_plugins/page_objects/image_display_page_object.cpp @@ -649,6 +698,20 @@ if(BUILD_TESTING) rviz_visual_testing_framework::rviz_visual_testing_framework) endif() + ament_add_gtest(laser_scan_display_visual_test + test/rviz_default_plugins/displays/laser_scan/laser_scan_display_visual_test.cpp + test/rviz_default_plugins/page_objects/point_cloud_common_page_object.cpp + ${SKIP_VISUAL_TESTS} + TIMEOUT 180) + if(TARGET laser_scan_display_visual_test) + target_include_directories(laser_scan_display_visual_test PUBLIC + ${TEST_INCLUDE_DIRS} + ${rviz_visual_testing_framework_INCLUDE_DIRS}) + target_link_libraries(laser_scan_display_visual_test + ${TEST_LINK_LIBRARIES} + rviz_visual_testing_framework::rviz_visual_testing_framework) + endif() + ament_add_gtest(map_display_visual_test test/rviz_default_plugins/displays/map/map_display_visual_test.cpp test/rviz_default_plugins/page_objects/map_display_page_object.cpp @@ -666,21 +729,6 @@ if(BUILD_TESTING) rviz_visual_testing_framework::rviz_visual_testing_framework) endif() - ament_add_gtest(laser_scan_display_visual_test - test/rviz_default_plugins/displays/laser_scan/laser_scan_display_visual_test.cpp - test/rviz_default_plugins/page_objects/point_cloud_common_page_object.cpp - test/rviz_default_plugins/page_objects/laser_scan_display_page_object.cpp - ${SKIP_VISUAL_TESTS} - TIMEOUT 180) - if(TARGET laser_scan_display_visual_test) - target_include_directories(laser_scan_display_visual_test PUBLIC - ${TEST_INCLUDE_DIRS} - ${rviz_visual_testing_framework_INCLUDE_DIRS}) - target_link_libraries(laser_scan_display_visual_test - ${TEST_LINK_LIBRARIES} - rviz_visual_testing_framework::rviz_visual_testing_framework) - endif() - ament_add_gtest(marker_array_display_visual_test test/rviz_default_plugins/displays/marker_array/marker_array_display_visual_test.cpp test/rviz_default_plugins/page_objects/marker_array_display_page_object.cpp @@ -759,7 +807,6 @@ if(BUILD_TESTING) ament_add_gtest(point_cloud_display_visual_test test/rviz_default_plugins/displays/pointcloud/point_cloud_display_visual_test.cpp - test/rviz_default_plugins/page_objects/point_cloud_display_page_object.cpp test/rviz_default_plugins/page_objects/point_cloud_common_page_object.cpp test/rviz_default_plugins/publishers/point_cloud_publisher.hpp ${SKIP_VISUAL_TESTS} @@ -775,7 +822,6 @@ if(BUILD_TESTING) ament_add_gtest(point_cloud2_display_visual_test test/rviz_default_plugins/displays/pointcloud/point_cloud2_display_visual_test.cpp - test/rviz_default_plugins/page_objects/point_cloud2_display_page_object.cpp test/rviz_default_plugins/publishers/point_cloud2_publisher.hpp test/rviz_default_plugins/page_objects/point_cloud_common_page_object.cpp test/rviz_default_plugins/pointcloud_messages.cpp @@ -835,6 +881,22 @@ if(BUILD_TESTING) rviz_visual_testing_framework::rviz_visual_testing_framework) endif() + ament_add_gtest(relative_humidity_display_visual_test + test/rviz_default_plugins/displays/relative_humidity/relative_humidity_display_visual_test.cpp + test/rviz_default_plugins/page_objects/point_cloud_common_page_object.cpp + test/rviz_default_plugins/pointcloud_messages.cpp + test/rviz_default_plugins/publishers/relative_humidity_publisher.hpp + ${SKIP_VISUAL_TESTS} + TIMEOUT 180) + if(TARGET relative_humidity_display_visual_test) + target_include_directories(relative_humidity_display_visual_test PUBLIC + ${TEST_INCLUDE_DIRS} + ${rviz_visual_testing_framework_INCLUDE_DIRS}) + target_link_libraries(relative_humidity_display_visual_test + ${TEST_LINK_LIBRARIES} + rviz_visual_testing_framework::rviz_visual_testing_framework) + endif() + ament_add_gtest(robot_model_display_visual_test test/rviz_default_plugins/displays/robot_model/robot_model_display_visual_test.cpp test/rviz_default_plugins/page_objects/robot_model_display_page_object.cpp @@ -850,6 +912,22 @@ if(BUILD_TESTING) rviz_visual_testing_framework::rviz_visual_testing_framework) endif() + ament_add_gtest(temperature_display_visual_test + test/rviz_default_plugins/displays/temperature/temperature_display_visual_test.cpp + test/rviz_default_plugins/page_objects/point_cloud_common_page_object.cpp + test/rviz_default_plugins/pointcloud_messages.cpp + test/rviz_default_plugins/publishers/temperature_publisher.hpp + ${SKIP_VISUAL_TESTS} + TIMEOUT 180) + if(TARGET temperature_display_visual_test) + target_include_directories(temperature_display_visual_test PUBLIC + ${TEST_INCLUDE_DIRS} + ${rviz_visual_testing_framework_INCLUDE_DIRS}) + target_link_libraries(temperature_display_visual_test + ${TEST_LINK_LIBRARIES} + rviz_visual_testing_framework::rviz_visual_testing_framework) + endif() + ament_add_gtest(tf_display_visual_test test/rviz_default_plugins/displays/tf/tf_display_visual_test.cpp test/rviz_default_plugins/page_objects/tf_display_page_object.cpp diff --git a/rviz/src/rviz/default_plugin/fluid_pressure_display.h b/rviz_default_plugins/include/rviz_default_plugins/displays/fluid_pressure/fluid_pressure_display.hpp similarity index 63% rename from rviz/src/rviz/default_plugin/fluid_pressure_display.h rename to rviz_default_plugins/include/rviz_default_plugins/displays/fluid_pressure/fluid_pressure_display.hpp index 541aae91e..1a2286c4f 100644 --- a/rviz/src/rviz/default_plugin/fluid_pressure_display.h +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/fluid_pressure/fluid_pressure_display.hpp @@ -1,5 +1,6 @@ /* * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2018, TNG Technology Consulting GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -27,51 +28,42 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef RVIZ_FLUID_PRESSURE_DISPLAY_H -#define RVIZ_FLUID_PRESSURE_DISPLAY_H +#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__FLUID_PRESSURE__FLUID_PRESSURE_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__FLUID_PRESSURE__FLUID_PRESSURE_DISPLAY_HPP_ -#include -#include +#include "rviz_default_plugins/displays/pointcloud/point_cloud_scalar_display.hpp" +#include "sensor_msgs/msg/fluid_pressure.hpp" -#include "rviz/message_filter_display.h" - -namespace rviz +namespace rviz_default_plugins { -class IntProperty; class PointCloudCommon; +namespace displays +{ + +/// Display a FluidPressure message of type sensor_msgs::FluidPressure /** * \class FluidPressureDisplay - * \brief Displays an FluidPressure message of type sensor_msgs::FluidPressure - * */ -class FluidPressureDisplay: public MessageFilterDisplay + +class RVIZ_DEFAULT_PLUGINS_PUBLIC FluidPressureDisplay + : public PointCloudScalarDisplay { -Q_OBJECT + Q_OBJECT + public: FluidPressureDisplay(); - ~FluidPressureDisplay(); - - virtual void reset(); - - virtual void update( float wall_dt, float ros_dt ); - -private Q_SLOTS: - void updateQueueSize(); - -protected: - /** @brief Do initialization. Overridden from MessageFilterDisplay. */ - virtual void onInitialize(); - - /** @brief Process a single message. Overridden from MessageFilterDisplay. */ - virtual void processMessage( const sensor_msgs::FluidPressureConstPtr& msg ); + ~FluidPressureDisplay() override; - IntProperty* queue_size_property_; +private: + void processMessage(const sensor_msgs::msg::FluidPressure::ConstSharedPtr message) override; - PointCloudCommon* point_cloud_common_; + void setInitialValues() override; + void hideUnneededProperties() override; }; -} // namespace rviz +} // namespace displays +} // namespace rviz_default_plugins -#endif +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__FLUID_PRESSURE__FLUID_PRESSURE_DISPLAY_HPP_ diff --git a/rviz/src/rviz/default_plugin/illuminance_display.h b/rviz_default_plugins/include/rviz_default_plugins/displays/illuminance/illuminance_display.hpp similarity index 64% rename from rviz/src/rviz/default_plugin/illuminance_display.h rename to rviz_default_plugins/include/rviz_default_plugins/displays/illuminance/illuminance_display.hpp index a2efee2c8..a71b2a937 100644 --- a/rviz/src/rviz/default_plugin/illuminance_display.h +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/illuminance/illuminance_display.hpp @@ -1,5 +1,6 @@ /* * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2018, TNG Technology Consulting GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -27,51 +28,42 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef RVIZ_ILLUMINANCE_DISPLAY_H -#define RVIZ_ILLUMINANCE_DISPLAY_H +#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__ILLUMINANCE__ILLUMINANCE_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__ILLUMINANCE__ILLUMINANCE_DISPLAY_HPP_ -#include -#include +#include "rviz_default_plugins/displays/pointcloud/point_cloud_scalar_display.hpp" +#include "sensor_msgs/msg/illuminance.hpp" -#include "rviz/message_filter_display.h" - -namespace rviz +namespace rviz_default_plugins { -class IntProperty; class PointCloudCommon; +namespace displays +{ + +/// Display an Illuminance message of type sensor_msgs::Illuminance /** * \class IlluminanceDisplay - * \brief Displays an Illuminance message of type sensor_msgs::Illuminance - * */ -class IlluminanceDisplay: public MessageFilterDisplay + +class RVIZ_DEFAULT_PLUGINS_PUBLIC IlluminanceDisplay + : public PointCloudScalarDisplay { -Q_OBJECT + Q_OBJECT + public: IlluminanceDisplay(); - ~IlluminanceDisplay(); - - virtual void reset(); - - virtual void update( float wall_dt, float ros_dt ); - -private Q_SLOTS: - void updateQueueSize(); - -protected: - /** @brief Do initialization. Overridden from MessageFilterDisplay. */ - virtual void onInitialize(); - - /** @brief Process a single message. Overridden from MessageFilterDisplay. */ - virtual void processMessage( const sensor_msgs::IlluminanceConstPtr& msg ); + ~IlluminanceDisplay() override; - IntProperty* queue_size_property_; +private: + void processMessage(const sensor_msgs::msg::Illuminance::ConstSharedPtr message) override; - PointCloudCommon* point_cloud_common_; + void setInitialValues() override; + void hideUnneededProperties() override; }; -} // namespace rviz +} // namespace displays +} // namespace rviz_default_plugins -#endif +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__ILLUMINANCE__ILLUMINANCE_DISPLAY_HPP_ diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_scalar_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_scalar_display.hpp new file mode 100644 index 000000000..81bffc40b --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_scalar_display.hpp @@ -0,0 +1,209 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2018, TNG Technology Consulting GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_SCALAR_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_SCALAR_DISPLAY_HPP_ + +#include +#include + +#include +#include + +#include "rviz_common/display_context.hpp" +#include "rviz_common/frame_manager_iface.hpp" +#include "rviz_common/ros_topic_display.hpp" +#include "rviz_common/properties/queue_size_property.hpp" +#include "rviz_common/validate_floats.hpp" +#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" +#include "rviz_default_plugins/visibility_control.hpp" +#include "rviz_rendering/objects/point_cloud.hpp" + +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "std_msgs/msg/header.hpp" + +namespace rviz_default_plugins +{ + +namespace displays +{ + +/// This is the parent class for several scalar type message displays +/// that use a point cloud to display their data +/** + * \class PointCloudScalarDisplay + */ + +template +class RVIZ_DEFAULT_PLUGINS_PUBLIC PointCloudScalarDisplay + : public rviz_common::RosTopicDisplay +{ +public: + PointCloudScalarDisplay() + : queue_size_property_(std::make_unique(this, 10)), + point_cloud_common_(std::make_shared(this)) + {} + + ~PointCloudScalarDisplay() override = default; + + std::shared_ptr createPointCloud2Message( + const std_msgs::msg::Header & header, double scalar_value, const std::string & channelName) + { + auto point_cloud_message = std::make_shared(); + unsigned int field_size_total = 0; + + point_cloud_message->header = header; + + field_size_total = addFieldsAndReturnSize(point_cloud_message, channelName); + point_cloud_message->data.resize(field_size_total); + + copyCoordinates(point_cloud_message); + copyScalarValue(point_cloud_message, scalar_value); + + point_cloud_message->height = 1; + point_cloud_message->width = 1; + point_cloud_message->is_bigendian = false; + point_cloud_message->point_step = field_size_total; + point_cloud_message->row_step = 1; + + return point_cloud_message; + } + +protected: + virtual void setInitialValues() = 0; + virtual void hideUnneededProperties() = 0; + + std::unique_ptr queue_size_property_; + std::shared_ptr point_cloud_common_; + +private: + void onInitialize() override + { + rviz_common::RosTopicDisplay::onInitialize(); + point_cloud_common_->initialize( + this->context_, this->scene_node_); + setInitialValues(); + } + + void update(float wall_dt, float ros_dt) override + { + point_cloud_common_->update(wall_dt, ros_dt); + hideUnneededProperties(); + } + + void onEnable() override + { + rviz_common::RosTopicDisplay::onEnable(); + } + + void onDisable() override + { + rviz_common::RosTopicDisplay::onDisable(); + point_cloud_common_->onDisable(); + } + + void reset() override + { + rviz_common::RosTopicDisplay::reset(); + point_cloud_common_->reset(); + } + + int addFieldsAndReturnSize( + std::shared_ptr point_cloud_message, + const std::string & channelName) + { + unsigned int field_size_increment = 0; + + field_size_increment = + addField32andReturnOffset(point_cloud_message, field_size_increment, "x"); + field_size_increment = + addField32andReturnOffset(point_cloud_message, field_size_increment, "y"); + field_size_increment = + addField32andReturnOffset(point_cloud_message, field_size_increment, "z"); + field_size_increment = + addField64andReturnOffset(point_cloud_message, field_size_increment, channelName); + return field_size_increment; + } + + int addField32andReturnOffset( + std::shared_ptr point_cloud_message, + unsigned int offset, std::string field_name) + { + sensor_msgs::msg::PointField field; + field.name = field_name; + field.offset = offset; + field.datatype = sensor_msgs::msg::PointField::FLOAT32; + field.count = 1; + point_cloud_message->fields.push_back(field); + offset += field_size_32_; + + return offset; + } + + int addField64andReturnOffset( + std::shared_ptr point_cloud_message, + unsigned int offset, std::string field_name) + { + sensor_msgs::msg::PointField field; + field.name = field_name; + field.offset = offset; + field.datatype = sensor_msgs::msg::PointField::FLOAT64; + field.count = 1; + point_cloud_message->fields.push_back(field); + offset += field_size_64_; + + return offset; + } + + void copyCoordinates(std::shared_ptr point_cloud_message) + { + float coordinate_value = 0.0; + + for (int i = 0; i < 3; i++) { + memcpy(&point_cloud_message->data[point_cloud_message->fields[i].offset], + &coordinate_value, field_size_32_); + } + } + + void copyScalarValue( + std::shared_ptr point_cloud_message, double scalar_value) + { + memcpy(&point_cloud_message->data[point_cloud_message->fields[3].offset], + &scalar_value, field_size_64_); + } + + const unsigned int field_size_32_ = 4; + const unsigned int field_size_64_ = 8; +}; + +} // namespace displays +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_SCALAR_DISPLAY_HPP_ diff --git a/rviz/src/rviz/default_plugin/relative_humidity_display.h b/rviz_default_plugins/include/rviz_default_plugins/displays/relative_humidity/relative_humidity_display.hpp similarity index 63% rename from rviz/src/rviz/default_plugin/relative_humidity_display.h rename to rviz_default_plugins/include/rviz_default_plugins/displays/relative_humidity/relative_humidity_display.hpp index 9eef371e4..be96714f3 100644 --- a/rviz/src/rviz/default_plugin/relative_humidity_display.h +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/relative_humidity/relative_humidity_display.hpp @@ -1,5 +1,6 @@ /* * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2018, TNG Technology Consulting GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -27,51 +28,42 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef RVIZ_RELATIVE_HUMIDITY_DISPLAY_H -#define RVIZ_RELATIVE_HUMIDITY_DISPLAY_H +#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__RELATIVE_HUMIDITY__RELATIVE_HUMIDITY_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__RELATIVE_HUMIDITY__RELATIVE_HUMIDITY_DISPLAY_HPP_ -#include -#include +#include "rviz_default_plugins/displays/pointcloud/point_cloud_scalar_display.hpp" +#include "sensor_msgs/msg/relative_humidity.hpp" -#include "rviz/message_filter_display.h" - -namespace rviz +namespace rviz_default_plugins { -class IntProperty; class PointCloudCommon; +namespace displays +{ + +/// Display a RelativeHumidity message of type sensor_msgs::RelativeHumidity /** * \class RelativeHumidityDisplay - * \brief Displays a RelativeHumidity message of type sensor_msgs::RelativeHumidity - * */ -class RelativeHumidityDisplay: public MessageFilterDisplay + +class RVIZ_DEFAULT_PLUGINS_PUBLIC RelativeHumidityDisplay + : public PointCloudScalarDisplay { -Q_OBJECT + Q_OBJECT + public: RelativeHumidityDisplay(); - ~RelativeHumidityDisplay(); - - virtual void reset(); - - virtual void update( float wall_dt, float ros_dt ); - -private Q_SLOTS: - void updateQueueSize(); - -protected: - /** @brief Do initialization. Overridden from MessageFilterDisplay. */ - virtual void onInitialize(); - - /** @brief Process a single message. Overridden from MessageFilterDisplay. */ - virtual void processMessage( const sensor_msgs::RelativeHumidityConstPtr& msg ); + ~RelativeHumidityDisplay() override; - IntProperty* queue_size_property_; +private: + void processMessage(const sensor_msgs::msg::RelativeHumidity::ConstSharedPtr message) override; - PointCloudCommon* point_cloud_common_; + void setInitialValues() override; + void hideUnneededProperties() override; }; -} // namespace rviz +} // namespace displays +} // namespace rviz_default_plugins -#endif +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__RELATIVE_HUMIDITY__RELATIVE_HUMIDITY_DISPLAY_HPP_ diff --git a/rviz/src/rviz/default_plugin/temperature_display.h b/rviz_default_plugins/include/rviz_default_plugins/displays/temperature/temperature_display.hpp similarity index 64% rename from rviz/src/rviz/default_plugin/temperature_display.h rename to rviz_default_plugins/include/rviz_default_plugins/displays/temperature/temperature_display.hpp index 63bf343c1..8c9603c0f 100644 --- a/rviz/src/rviz/default_plugin/temperature_display.h +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/temperature/temperature_display.hpp @@ -1,5 +1,6 @@ /* * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2018, TNG Technology Consulting GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -27,51 +28,42 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef RVIZ_TEMPERATURE_DISPLAY_H -#define RVIZ_TEMPERATURE_DISPLAY_H +#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__TEMPERATURE__TEMPERATURE_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__TEMPERATURE__TEMPERATURE_DISPLAY_HPP_ -#include -#include +#include "rviz_default_plugins/displays/pointcloud/point_cloud_scalar_display.hpp" +#include "sensor_msgs/msg/temperature.hpp" -#include "rviz/message_filter_display.h" - -namespace rviz +namespace rviz_default_plugins { -class IntProperty; class PointCloudCommon; +namespace displays +{ + +/// Display a Temperature message of type sensor_msgs::Temperature /** * \class TemperatureDisplay - * \brief Displays a Temperature message of type sensor_msgs::Temperature - * */ -class TemperatureDisplay: public MessageFilterDisplay + +class RVIZ_DEFAULT_PLUGINS_PUBLIC TemperatureDisplay + : public PointCloudScalarDisplay { -Q_OBJECT + Q_OBJECT + public: TemperatureDisplay(); - ~TemperatureDisplay(); - - virtual void reset(); - - virtual void update( float wall_dt, float ros_dt ); - -private Q_SLOTS: - void updateQueueSize(); - -protected: - /** @brief Do initialization. Overridden from MessageFilterDisplay. */ - virtual void onInitialize(); - - /** @brief Process a single message. Overridden from MessageFilterDisplay. */ - virtual void processMessage( const sensor_msgs::TemperatureConstPtr& msg ); + ~TemperatureDisplay() override; - IntProperty* queue_size_property_; + void processMessage(const sensor_msgs::msg::Temperature::ConstSharedPtr message) override; - PointCloudCommon* point_cloud_common_; +private: + void setInitialValues() override; + void hideUnneededProperties() override; }; -} // namespace rviz +} // namespace displays +} // namespace rviz_default_plugins -#endif +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__TEMPERATURE__TEMPERATURE_DISPLAY_HPP_ diff --git a/rviz_default_plugins/plugins_description.xml b/rviz_default_plugins/plugins_description.xml index 1d58a70fd..e27c8f209 100644 --- a/rviz_default_plugins/plugins_description.xml +++ b/rviz_default_plugins/plugins_description.xml @@ -35,6 +35,28 @@ nav_msgs/GridCells + + + Displays data from a sensor_msgs::FluidPressure as pointcloud with one point + + sensor_msgs/FluidPressure + + + + + Displays data from a sensor_msgs::Illuminance as pointcloud with one point + + sensor_msgs/Illuminance + + + base_class_type="rviz_common::Display" + > Displays the data from sensor_msgs::Range messages as cones. <a href="http://www.ros.org/wiki/rviz/DisplayTypes/Range">More Information</a> sensor_msgs/Range + + + Displays the data from sensor_msgs::RelativeHumidity messages as point cloud with one point + + sensor_msgs/RelativeHumidity + + + + + Displays the data from sensor_msgs::Temperature messages as point cloud with one point + + sensor_msgs/Temperature + + setValue("fluid_pressure"); + subProp("Autocompute Intensity Bounds")->setValue(false); + subProp("Min Intensity")->setValue(98000); + subProp("Max Intensity")->setValue(105000); +} + +void FluidPressureDisplay::hideUnneededProperties() +{ + subProp("Position Transformer")->hide(); + subProp("Color Transformer")->hide(); + subProp("Channel Name")->hide(); + subProp("Autocompute Intensity Bounds")->hide(); +} + +void FluidPressureDisplay::processMessage(sensor_msgs::msg::FluidPressure::ConstSharedPtr message) +{ + auto point_cloud2_message = + createPointCloud2Message(message->header, message->fluid_pressure, "fluid_pressure"); + + point_cloud_common_->addMessage(point_cloud2_message); +} + +} // namespace displays +} // namespace rviz_default_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::displays::FluidPressureDisplay, rviz_common::Display) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/illuminance/illuminance_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/illuminance/illuminance_display.cpp new file mode 100644 index 000000000..4334a0865 --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/illuminance/illuminance_display.cpp @@ -0,0 +1,72 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, TNG Technology Consulting GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "rviz_default_plugins/displays/illuminance/illuminance_display.hpp" + +namespace rviz_default_plugins +{ + +namespace displays +{ + +IlluminanceDisplay::IlluminanceDisplay() +{} + +IlluminanceDisplay::~IlluminanceDisplay() = default; + +void IlluminanceDisplay::setInitialValues() +{ + subProp("Channel Name")->setValue("illuminance"); + subProp("Autocompute Intensity Bounds")->setValue(false); + subProp("Min Intensity")->setValue(0); + subProp("Max Intensity")->setValue(1000); +} + +void IlluminanceDisplay::hideUnneededProperties() +{ + subProp("Position Transformer")->hide(); + subProp("Color Transformer")->hide(); + subProp("Channel Name")->hide(); + subProp("Autocompute Intensity Bounds")->hide(); +} + +void IlluminanceDisplay::processMessage(sensor_msgs::msg::Illuminance::ConstSharedPtr message) +{ + auto point_cloud2_message = + createPointCloud2Message(message->header, message->illuminance, "illuminance"); + + point_cloud_common_->addMessage(point_cloud2_message); +} + +} // namespace displays +} // namespace rviz_default_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::displays::IlluminanceDisplay, rviz_common::Display) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/relative_humidity/relative_humidity_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/relative_humidity/relative_humidity_display.cpp new file mode 100644 index 000000000..d5ab6888e --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/relative_humidity/relative_humidity_display.cpp @@ -0,0 +1,74 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, TNG Technology Consulting GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "rviz_default_plugins/displays/relative_humidity/relative_humidity_display.hpp" + +namespace rviz_default_plugins +{ + +namespace displays +{ + +RelativeHumidityDisplay::RelativeHumidityDisplay() +{} + +RelativeHumidityDisplay::~RelativeHumidityDisplay() = default; + +void RelativeHumidityDisplay::setInitialValues() +{ + subProp("Channel Name")->setValue("relative_humidity"); + subProp("Autocompute Intensity Bounds")->setValue(false); + subProp("Min Intensity")->setValue(0.); + subProp("Max Intensity")->setValue(1.); +} + +void RelativeHumidityDisplay::hideUnneededProperties() +{ + subProp("Position Transformer")->hide(); + subProp("Color Transformer")->hide(); + subProp("Channel Name")->hide(); + subProp("Autocompute Intensity Bounds")->hide(); +} + +void RelativeHumidityDisplay::processMessage( + sensor_msgs::msg::RelativeHumidity::ConstSharedPtr message) +{ + auto point_cloud2_message = + createPointCloud2Message(message->header, message->relative_humidity, "relative_humidity"); + + point_cloud_common_->addMessage(point_cloud2_message); +} + +} // namespace displays +} // namespace rviz_default_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::displays::RelativeHumidityDisplay, + rviz_common::Display) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/temperature/temperature_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/temperature/temperature_display.cpp new file mode 100644 index 000000000..6be7167c3 --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/temperature/temperature_display.cpp @@ -0,0 +1,74 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, TNG Technology Consulting GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "rviz_default_plugins/displays/temperature/temperature_display.hpp" + +namespace rviz_default_plugins +{ + +namespace displays +{ + +TemperatureDisplay::TemperatureDisplay() +{} + +TemperatureDisplay::~TemperatureDisplay() = default; + +void TemperatureDisplay::setInitialValues() +{ + subProp("Channel Name")->setValue("temperature"); + subProp("Autocompute Intensity Bounds")->setValue(false); + subProp("Invert Rainbow")->setValue(true); + subProp("Min Intensity")->setValue(0); + subProp("Max Intensity")->setValue(100); +} + +void TemperatureDisplay::hideUnneededProperties() +{ + subProp("Position Transformer")->hide(); + subProp("Color Transformer")->hide(); + subProp("Channel Name")->hide(); + subProp("Invert Rainbow")->hide(); + subProp("Autocompute Intensity Bounds")->hide(); +} + +void TemperatureDisplay::processMessage(sensor_msgs::msg::Temperature::ConstSharedPtr message) +{ + auto point_cloud2_message = + createPointCloud2Message(message->header, message->temperature, "temperature"); + + point_cloud_common_->addMessage(point_cloud2_message); +} + +} // namespace displays +} // namespace rviz_default_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::displays::TemperatureDisplay, rviz_common::Display) diff --git a/rviz_default_plugins/test/reference_images/fluid_pressure_display_high_fluid_pressure_ref.png b/rviz_default_plugins/test/reference_images/fluid_pressure_display_high_fluid_pressure_ref.png new file mode 100644 index 000000000..374dd03c6 Binary files /dev/null and b/rviz_default_plugins/test/reference_images/fluid_pressure_display_high_fluid_pressure_ref.png differ diff --git a/rviz_default_plugins/test/reference_images/fluid_pressure_display_low_fluid_pressure_ref.png b/rviz_default_plugins/test/reference_images/fluid_pressure_display_low_fluid_pressure_ref.png new file mode 100644 index 000000000..83c97fefd Binary files /dev/null and b/rviz_default_plugins/test/reference_images/fluid_pressure_display_low_fluid_pressure_ref.png differ diff --git a/rviz_default_plugins/test/reference_images/illuminance_display_high_illuminance_ref.png b/rviz_default_plugins/test/reference_images/illuminance_display_high_illuminance_ref.png new file mode 100644 index 000000000..c96664b2c Binary files /dev/null and b/rviz_default_plugins/test/reference_images/illuminance_display_high_illuminance_ref.png differ diff --git a/rviz_default_plugins/test/reference_images/illuminance_display_low_illuminance_ref.png b/rviz_default_plugins/test/reference_images/illuminance_display_low_illuminance_ref.png new file mode 100644 index 000000000..35cab2f36 Binary files /dev/null and b/rviz_default_plugins/test/reference_images/illuminance_display_low_illuminance_ref.png differ diff --git a/rviz_default_plugins/test/reference_images/relative_humidity_display_high_relative_humidity_ref.png b/rviz_default_plugins/test/reference_images/relative_humidity_display_high_relative_humidity_ref.png new file mode 100644 index 000000000..65b3ed833 Binary files /dev/null and b/rviz_default_plugins/test/reference_images/relative_humidity_display_high_relative_humidity_ref.png differ diff --git a/rviz_default_plugins/test/reference_images/relative_humidity_display_low_relative_humidity_ref.png b/rviz_default_plugins/test/reference_images/relative_humidity_display_low_relative_humidity_ref.png new file mode 100644 index 000000000..35cab2f36 Binary files /dev/null and b/rviz_default_plugins/test/reference_images/relative_humidity_display_low_relative_humidity_ref.png differ diff --git a/rviz_default_plugins/test/reference_images/temperature_display_high_temperature_ref.png b/rviz_default_plugins/test/reference_images/temperature_display_high_temperature_ref.png new file mode 100644 index 000000000..35cab2f36 Binary files /dev/null and b/rviz_default_plugins/test/reference_images/temperature_display_high_temperature_ref.png differ diff --git a/rviz_default_plugins/test/reference_images/temperature_display_low_temperature_ref.png b/rviz_default_plugins/test/reference_images/temperature_display_low_temperature_ref.png new file mode 100644 index 000000000..65b3ed833 Binary files /dev/null and b/rviz_default_plugins/test/reference_images/temperature_display_low_temperature_ref.png differ diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/camera/camera_display_visual_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/camera/camera_display_visual_test.cpp index 44a087bd8..c722935d0 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/camera/camera_display_visual_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/camera/camera_display_visual_test.cpp @@ -39,9 +39,18 @@ #include "../../page_objects/camera_display_page_object.hpp" #include "../../publishers/camera_info_publisher.hpp" #include "../../publishers/image_publisher.hpp" -#include "../../page_objects/point_cloud_display_page_object.hpp" +#include "../../page_objects/point_cloud_common_page_object.hpp" #include "../../publishers/point_cloud_publisher.hpp" +class PointCloudDisplayPageObject + : public PointCloudCommonPageObject +{ +public: + PointCloudDisplayPageObject() + : PointCloudCommonPageObject("PointCloud") + {} +}; + TEST_F(VisualTestFixture, test_camera_display_with_published_image) { auto points = {nodes::createPoint(0, 0, 10)}; std::vector publishers = { diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud2_display_page_object.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/fluid_pressure/fluid_pressure_display_visual_test.cpp similarity index 51% rename from rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud2_display_page_object.cpp rename to rviz_default_plugins/test/rviz_default_plugins/displays/fluid_pressure/fluid_pressure_display_visual_test.cpp index b9c9f8441..c5782c454 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud2_display_page_object.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/fluid_pressure/fluid_pressure_display_visual_test.cpp @@ -1,5 +1,6 @@ /* - * Copyright (c) 2018, Bosch Software Innovations GmbH. + * Copyright (c) 2017, Bosch Software Innovations GmbH. + * Copyright (c) 2018, TNG Technology Consulting GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -27,18 +28,46 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "point_cloud2_display_page_object.hpp" - #include -#include +#include -#include // NOLINT +#include "rviz_visual_testing_framework/visual_test_fixture.hpp" +#include "rviz_visual_testing_framework/visual_test_publisher.hpp" -PointCloud2DisplayPageObject::PointCloud2DisplayPageObject() -: PointCloudCommonPageObject("PointCloud2") -{} +#include "../../page_objects/point_cloud_common_page_object.hpp" +#include "../../publishers/fluid_pressure_publisher.hpp" -void PointCloud2DisplayPageObject::setQueueSize(int queue_size) +class FluidPressureDisplayPageObject + : public PointCloudCommonPageObject { - setInt("Queue Size", queue_size); +public: + FluidPressureDisplayPageObject() + : PointCloudCommonPageObject("FluidPressure") + {} +}; + +TEST_F(VisualTestFixture, sphere_changes_color_depending_on_fluid_pressure) { + auto fluid_pressure_publisher = std::make_shared(); + auto fluid_pressure_visual_publisher = + std::make_unique( + fluid_pressure_publisher, "fluid_pressure_frame"); + + setCamPose(Ogre::Vector3(0, 0, 16)); + setCamLookAt(Ogre::Vector3(0, 0, 0)); + + auto fluid_pressure_display = addDisplay(); + fluid_pressure_display->setTopic("/fluid_pressure"); + fluid_pressure_display->setStyle("Spheres"); + fluid_pressure_display->setSizeMeters(11); + + fluid_pressure_publisher->setFluidPressure(99000); + captureMainWindow("fluid_pressure_display_low_fluid_pressure"); + + executor_->queueAction([fluid_pressure_publisher]() + { + fluid_pressure_publisher->setFluidPressure(104000); + }); + + captureMainWindow("fluid_pressure_display_high_fluid_pressure"); + assertScreenShotsIdentity(); } diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/illuminance/illuminance_display_visual_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/illuminance/illuminance_display_visual_test.cpp new file mode 100644 index 000000000..330e198fb --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/illuminance/illuminance_display_visual_test.cpp @@ -0,0 +1,73 @@ +/* +* Copyright (c) 2017, Bosch Software Innovations GmbH. +* Copyright (c) 2018, TNG Technology Consulting GmbH. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of the copyright holder nor the names of its contributors +* may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include + +#include "rviz_visual_testing_framework/visual_test_fixture.hpp" +#include "rviz_visual_testing_framework/visual_test_publisher.hpp" + +#include "../../page_objects/point_cloud_common_page_object.hpp" +#include "../../publishers/illuminance_publisher.hpp" + +class IlluminanceDisplayPageObject + : public PointCloudCommonPageObject +{ +public: + IlluminanceDisplayPageObject() + : PointCloudCommonPageObject("Illuminance") + {} +}; + +TEST_F(VisualTestFixture, sphere_changes_color_depending_on_illuminance) { + auto illuminance_publisher = std::make_shared(); + auto illuminance_visual_publisher = + std::make_unique( + illuminance_publisher, "illuminance_frame"); + + setCamPose(Ogre::Vector3(0, 0, 16)); + setCamLookAt(Ogre::Vector3(0, 0, 0)); + + auto illuminance_display = addDisplay(); + illuminance_display->setTopic("/illuminance"); + illuminance_display->setStyle("Spheres"); + illuminance_display->setSizeMeters(11); + + illuminance_publisher->setIlluminance(150); + captureMainWindow("illuminance_display_low_illuminance"); + + executor_->queueAction([illuminance_publisher]() + { + illuminance_publisher->setIlluminance(800); + }); + + captureMainWindow("illuminance_display_high_illuminance"); + assertScreenShotsIdentity(); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/laser_scan/laser_scan_display_visual_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/laser_scan/laser_scan_display_visual_test.cpp index f1f35ab70..16b631af3 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/laser_scan/laser_scan_display_visual_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/laser_scan/laser_scan_display_visual_test.cpp @@ -33,9 +33,18 @@ #include "rviz_visual_testing_framework/visual_test_fixture.hpp" #include "rviz_visual_testing_framework/visual_test_publisher.hpp" -#include "../../page_objects/laser_scan_display_page_object.hpp" +#include "../../page_objects/point_cloud_common_page_object.hpp" #include "../../publishers/laser_scan_publisher.hpp" +class LaserScanDisplayPageObject + : public PointCloudCommonPageObject +{ +public: + LaserScanDisplayPageObject() + : PointCloudCommonPageObject("LaserScan") + {} +}; + TEST_F(VisualTestFixture, laser_scan_display) { auto laser_scan_publisher = std::make_unique( diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud2_display_visual_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud2_display_visual_test.cpp index 6812663c2..d74d2f500 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud2_display_visual_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud2_display_visual_test.cpp @@ -33,9 +33,18 @@ #include "rviz_visual_testing_framework/visual_test_fixture.hpp" #include "rviz_visual_testing_framework/visual_test_publisher.hpp" -#include "../../page_objects/point_cloud2_display_page_object.hpp" +#include "../../page_objects/point_cloud_common_page_object.hpp" #include "../../publishers/point_cloud2_publisher.hpp" +class PointCloud2DisplayPageObject + : public PointCloudCommonPageObject +{ +public: + PointCloud2DisplayPageObject() + : PointCloudCommonPageObject("PointCloud2") + {} +}; + TEST_F(VisualTestFixture, pointcloud2_containing_one_big_point) { auto pointcloud2_publisher = std::make_unique( diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_display_visual_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_display_visual_test.cpp index 5ce27d802..840c0e935 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_display_visual_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_display_visual_test.cpp @@ -33,9 +33,18 @@ #include "rviz_visual_testing_framework/visual_test_fixture.hpp" #include "rviz_visual_testing_framework/visual_test_publisher.hpp" -#include "../../page_objects/point_cloud_display_page_object.hpp" +#include "../../page_objects/point_cloud_common_page_object.hpp" #include "../../publishers/point_cloud_publisher.hpp" +class PointCloudDisplayPageObject + : public PointCloudCommonPageObject +{ +public: + PointCloudDisplayPageObject() + : PointCloudCommonPageObject("PointCloud") + {} +}; + TEST_F(VisualTestFixture, pointcloud_containing_one_big_point) { auto pointcloud_publisher = std::make_unique( diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_scalar_display_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_scalar_display_test.cpp new file mode 100644 index 000000000..fae4608e8 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_scalar_display_test.cpp @@ -0,0 +1,123 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * Copyright (c) 2018, TNG Technology Consulting GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holders nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable : 4996) +#endif + +#include +#include + +#ifdef _WIN32 +# pragma warning(pop) +#endif + +#include "rviz_default_plugins/displays/pointcloud/point_cloud_scalar_display.hpp" +#include "sensor_msgs/msg/illuminance.hpp" +#include "../display_test_fixture.hpp" + +using namespace ::testing; // NOLINT + +/* + * w.l.o.g. use illuminance message + * all scalar messages have identical structure + */ + +sensor_msgs::msg::Illuminance::ConstSharedPtr createIlluminanceMessage( + const double illuminance = 100., const double variance = 1.) +{ + auto message = std::make_shared(); + message->header = std_msgs::msg::Header(); + message->header.frame_id = "illuminance_frame"; + message->header.stamp = rclcpp::Clock().now(); + + message->illuminance = illuminance; + message->variance = variance; + + return message; +} + +class PointCloudScalarDisplayImplementation + : public rviz_default_plugins::displays::PointCloudScalarDisplay +{ +public: + PointCloudScalarDisplayImplementation() {} + +protected: + void processMessage(sensor_msgs::msg::Illuminance::ConstSharedPtr) override {} + void setInitialValues() override {} + void hideUnneededProperties() override {} +}; + +class PointCloudScalarDisplayFixture : public DisplayTestFixture +{ +public: + PointCloudScalarDisplayFixture() + : display_(new PointCloudScalarDisplayImplementation) + {} + + ~PointCloudScalarDisplayFixture() + { + display_.reset(); + } + + std::shared_ptr display_; +}; + +TEST_F(PointCloudScalarDisplayFixture, translates_scalar_message_into_point_cloud_message_correctly) +{ + auto illuminance_message = createIlluminanceMessage(); + auto point_cloud_message = + display_->createPointCloud2Message( + illuminance_message->header, illuminance_message->illuminance, "illuminance"); + + ASSERT_THAT(point_cloud_message->point_step, Eq(20u)); + + ASSERT_THAT(point_cloud_message->header.frame_id, illuminance_message->header.frame_id); + + ASSERT_THAT(point_cloud_message->fields[3].name, Eq("illuminance")); + + uint8_t offset_ptr = point_cloud_message->fields[3].offset; + double * scalar_value_ptr = + reinterpret_cast(point_cloud_message->data.data() + offset_ptr); + ASSERT_THAT(*scalar_value_ptr, Eq(100.)); + + for (int i = 0; i < 3; i++) { + uint8_t offset_ptr = point_cloud_message->fields[i].offset; + float * coordinate_value_ptr = + reinterpret_cast(point_cloud_message->data.data() + offset_ptr); + ASSERT_THAT(*coordinate_value_ptr, Eq(0.f)); + } +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/relative_humidity/relative_humidity_display_visual_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/relative_humidity/relative_humidity_display_visual_test.cpp new file mode 100644 index 000000000..564f96d07 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/relative_humidity/relative_humidity_display_visual_test.cpp @@ -0,0 +1,73 @@ +/* + * Copyright (c) 2017, Bosch Software Innovations GmbH. + * Copyright (c) 2018, TNG Technology Consulting GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include "rviz_visual_testing_framework/visual_test_fixture.hpp" +#include "rviz_visual_testing_framework/visual_test_publisher.hpp" + +#include "../../page_objects/point_cloud_common_page_object.hpp" +#include "../../publishers/relative_humidity_publisher.hpp" + +class RelativeHumidityDisplayPageObject + : public PointCloudCommonPageObject +{ +public: + RelativeHumidityDisplayPageObject() + : PointCloudCommonPageObject("RelativeHumidity") + {} +}; + +TEST_F(VisualTestFixture, sphere_changes_color_depending_on_relative_humidity) { + auto relative_humidity_publisher = std::make_shared(); + auto relative_humidity_visual_publisher = + std::make_unique( + relative_humidity_publisher, "relative_humidity_frame"); + + setCamPose(Ogre::Vector3(0, 0, 16)); + setCamLookAt(Ogre::Vector3(0, 0, 0)); + + auto relative_humidity_display = addDisplay(); + relative_humidity_display->setTopic("/relative_humidity"); + relative_humidity_display->setStyle("Spheres"); + relative_humidity_display->setSizeMeters(11); + + relative_humidity_publisher->setRelativeHumidity(0.15); + captureMainWindow("relative_humidity_display_low_relative_humidity"); + + executor_->queueAction([relative_humidity_publisher]() + { + relative_humidity_publisher->setRelativeHumidity(0.85); + }); + + captureMainWindow("relative_humidity_display_high_relative_humidity"); + assertScreenShotsIdentity(); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud_display_page_object.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/temperature/temperature_display_visual_test.cpp similarity index 52% rename from rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud_display_page_object.cpp rename to rviz_default_plugins/test/rviz_default_plugins/displays/temperature/temperature_display_visual_test.cpp index 7183f6b70..0533a1a31 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud_display_page_object.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/temperature/temperature_display_visual_test.cpp @@ -1,5 +1,6 @@ /* - * Copyright (c) 2018, Bosch Software Innovations GmbH. + * Copyright (c) 2017, Bosch Software Innovations GmbH. + * Copyright (c) 2018, TNG Technology Consulting GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -27,18 +28,46 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "point_cloud_display_page_object.hpp" - #include -#include +#include -#include // NOLINT +#include "rviz_visual_testing_framework/visual_test_fixture.hpp" +#include "rviz_visual_testing_framework/visual_test_publisher.hpp" -PointCloudDisplayPageObject::PointCloudDisplayPageObject() -: PointCloudCommonPageObject("PointCloud") -{} +#include "../../page_objects/point_cloud_common_page_object.hpp" +#include "../../publishers/temperature_publisher.hpp" -void PointCloudDisplayPageObject::setQueueSize(int queue_size) +class TemperatureDisplayPageObject + : public PointCloudCommonPageObject { - setInt("Queue Size", queue_size); +public: + TemperatureDisplayPageObject() + : PointCloudCommonPageObject("Temperature") + {} +}; + +TEST_F(VisualTestFixture, sphere_changes_color_depending_on_temperature) { + auto temperature_publisher = std::make_shared(); + auto temperature_visual_publisher = + std::make_unique( + temperature_publisher, "temperature_frame"); + + setCamPose(Ogre::Vector3(0, 0, 16)); + setCamLookAt(Ogre::Vector3(0, 0, 0)); + + auto temperature_display = addDisplay(); + temperature_display->setTopic("/temperature"); + temperature_display->setStyle("Spheres"); + temperature_display->setSizeMeters(11); + + temperature_publisher->setTemperature(15.); + captureMainWindow("temperature_display_low_temperature"); + + executor_->queueAction([temperature_publisher]() + { + temperature_publisher->setTemperature(85.); + }); + + captureMainWindow("temperature_display_high_temperature"); + assertScreenShotsIdentity(); } diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/laser_scan_display_page_object.cpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/laser_scan_display_page_object.cpp deleted file mode 100644 index dfe185a01..000000000 --- a/rviz_default_plugins/test/rviz_default_plugins/page_objects/laser_scan_display_page_object.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/* - * Copyright (c) 2018, Bosch Software Innovations GmbH. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "./laser_scan_display_page_object.hpp" - -#include // NOLINT - -LaserScanDisplayPageObject::LaserScanDisplayPageObject() -: PointCloudCommonPageObject("LaserScan") -{} - -void LaserScanDisplayPageObject::setQueueSize(int queue_size) -{ - setInt("Queue Size", queue_size); -} diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/laser_scan_display_page_object.hpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/laser_scan_display_page_object.hpp deleted file mode 100644 index 08c491768..000000000 --- a/rviz_default_plugins/test/rviz_default_plugins/page_objects/laser_scan_display_page_object.hpp +++ /dev/null @@ -1,45 +0,0 @@ -/* - * Copyright (c) 2018, Bosch Software Innovations GmbH. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__LASER_SCAN_DISPLAY_PAGE_OBJECT_HPP_ -#define RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__LASER_SCAN_DISPLAY_PAGE_OBJECT_HPP_ - -#include "rviz_visual_testing_framework/page_objects/base_page_object.hpp" - -#include "./point_cloud_common_page_object.hpp" - -class LaserScanDisplayPageObject : public PointCloudCommonPageObject -{ -public: - LaserScanDisplayPageObject(); - - void setQueueSize(int queue_size); -}; - -#endif // RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__LASER_SCAN_DISPLAY_PAGE_OBJECT_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud2_display_page_object.hpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud2_display_page_object.hpp deleted file mode 100644 index f0077f744..000000000 --- a/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud2_display_page_object.hpp +++ /dev/null @@ -1,45 +0,0 @@ -/* - * Copyright (c) 2018, Bosch Software Innovations GmbH. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__POINT_CLOUD2_DISPLAY_PAGE_OBJECT_HPP_ -#define RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__POINT_CLOUD2_DISPLAY_PAGE_OBJECT_HPP_ - -#include "rviz_visual_testing_framework/page_objects/base_page_object.hpp" - -#include "point_cloud_common_page_object.hpp" - -class PointCloud2DisplayPageObject : public PointCloudCommonPageObject -{ -public: - PointCloud2DisplayPageObject(); - - void setQueueSize(int queue_size); -}; - -#endif // RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__POINT_CLOUD2_DISPLAY_PAGE_OBJECT_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud_common_page_object.cpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud_common_page_object.cpp index 713f6c358..8e9fe1048 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud_common_page_object.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud_common_page_object.cpp @@ -98,3 +98,8 @@ void PointCloudCommonPageObject::setColor(int red, int green, int blue) setString("Color", color_code); } + +void PointCloudCommonPageObject::setQueueSize(int queue_size) +{ + setInt("Queue Size", queue_size); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud_common_page_object.hpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud_common_page_object.hpp index 3fbddd364..f64ac7279 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud_common_page_object.hpp +++ b/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud_common_page_object.hpp @@ -48,6 +48,7 @@ class PointCloudCommonPageObject : public BasePageObject void setPositionTransformer(QString position_transformer); void setColorTransformer(QString color_transformer); void setColor(int red, int green, int blue); + void setQueueSize(int queue_size); }; #endif // RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__POINT_CLOUD_COMMON_PAGE_OBJECT_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud_display_page_object.hpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud_display_page_object.hpp deleted file mode 100644 index 511a8d2e0..000000000 --- a/rviz_default_plugins/test/rviz_default_plugins/page_objects/point_cloud_display_page_object.hpp +++ /dev/null @@ -1,45 +0,0 @@ -/* - * Copyright (c) 2018, Bosch Software Innovations GmbH. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__POINT_CLOUD_DISPLAY_PAGE_OBJECT_HPP_ -#define RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__POINT_CLOUD_DISPLAY_PAGE_OBJECT_HPP_ - -#include "rviz_visual_testing_framework/page_objects/base_page_object.hpp" - -#include "./point_cloud_common_page_object.hpp" - -class PointCloudDisplayPageObject : public PointCloudCommonPageObject -{ -public: - PointCloudDisplayPageObject(); - - void setQueueSize(int queue_size); -}; - -#endif // RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__POINT_CLOUD_DISPLAY_PAGE_OBJECT_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/publishers/fluid_pressure_publisher.hpp b/rviz_default_plugins/test/rviz_default_plugins/publishers/fluid_pressure_publisher.hpp new file mode 100644 index 000000000..3cb82468d --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/publishers/fluid_pressure_publisher.hpp @@ -0,0 +1,96 @@ +/* + * Copyright (c) 2017, Bosch Software Innovations GmbH. + * Copyright (c) 2018, TNG Technology Consulting GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__PUBLISHERS__FLUID_PRESSURE_PUBLISHER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__PUBLISHERS__FLUID_PRESSURE_PUBLISHER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "sensor_msgs/msg/fluid_pressure.hpp" +#include "std_msgs/msg/header.hpp" + +using namespace std::chrono_literals; //NOLINT + +namespace nodes +{ + +class FluidPressurePublisher : public rclcpp::Node +{ +public: + FluidPressurePublisher(); + void setFluidPressure(double fluid_pressure); + +private: + sensor_msgs::msg::FluidPressure createFluidPressureMessage(); + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + + double fluid_pressure_; +}; + +FluidPressurePublisher::FluidPressurePublisher() +: Node("fluid_pressure_publisher"), fluid_pressure_(0.) +{ + publisher_ = this->create_publisher("fluid_pressure"); + + auto timer_callback = + [this]() -> void { + auto message = createFluidPressureMessage(); + this->publisher_->publish(message); + }; + timer_ = this->create_wall_timer(500ms, timer_callback); +} + +sensor_msgs::msg::FluidPressure FluidPressurePublisher::createFluidPressureMessage() +{ + sensor_msgs::msg::FluidPressure fluidPressureMessage; + + fluidPressureMessage.header = std_msgs::msg::Header(); + fluidPressureMessage.header.frame_id = "fluid_pressure_frame"; + fluidPressureMessage.header.stamp = rclcpp::Clock().now(); + + fluidPressureMessage.fluid_pressure = fluid_pressure_; + fluidPressureMessage.variance = 1.0; + + return fluidPressureMessage; +} + +void FluidPressurePublisher::setFluidPressure(double fluid_pressure) +{ + fluid_pressure_ = fluid_pressure; +} + +} // namespace nodes + +#endif // RVIZ_DEFAULT_PLUGINS__PUBLISHERS__FLUID_PRESSURE_PUBLISHER_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/publishers/illuminance_publisher.hpp b/rviz_default_plugins/test/rviz_default_plugins/publishers/illuminance_publisher.hpp new file mode 100644 index 000000000..9e03c0a41 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/publishers/illuminance_publisher.hpp @@ -0,0 +1,96 @@ +/* + * Copyright (c) 2017, Bosch Software Innovations GmbH. + * Copyright (c) 2018, TNG Technology Consulting GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__PUBLISHERS__ILLUMINANCE_PUBLISHER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__PUBLISHERS__ILLUMINANCE_PUBLISHER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "sensor_msgs/msg/illuminance.hpp" +#include "std_msgs/msg/header.hpp" + +using namespace std::chrono_literals; //NOLINT + +namespace nodes +{ + +class IlluminancePublisher : public rclcpp::Node +{ +public: + IlluminancePublisher(); + void setIlluminance(double illuminance); + +private: + sensor_msgs::msg::Illuminance createIlluminanceMessage(); + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + + double illuminance_; +}; + +IlluminancePublisher::IlluminancePublisher() +: Node("illuminance_publisher"), illuminance_(0.) +{ + publisher_ = this->create_publisher("illuminance"); + + auto timer_callback = + [this]() -> void { + auto message = createIlluminanceMessage(); + this->publisher_->publish(message); + }; + timer_ = this->create_wall_timer(500ms, timer_callback); +} + +sensor_msgs::msg::Illuminance IlluminancePublisher::createIlluminanceMessage() +{ + sensor_msgs::msg::Illuminance illuminanceMessage; + + illuminanceMessage.header = std_msgs::msg::Header(); + illuminanceMessage.header.frame_id = "illuminance_frame"; + illuminanceMessage.header.stamp = rclcpp::Clock().now(); + + illuminanceMessage.illuminance = illuminance_; + illuminanceMessage.variance = 1.0; + + return illuminanceMessage; +} + +void IlluminancePublisher::setIlluminance(double illuminance) +{ + illuminance_ = illuminance; +} + +} // namespace nodes + +#endif // RVIZ_DEFAULT_PLUGINS__PUBLISHERS__ILLUMINANCE_PUBLISHER_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/publishers/relative_humidity_publisher.hpp b/rviz_default_plugins/test/rviz_default_plugins/publishers/relative_humidity_publisher.hpp new file mode 100644 index 000000000..269bdf3d4 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/publishers/relative_humidity_publisher.hpp @@ -0,0 +1,96 @@ +/* + * Copyright (c) 2017, Bosch Software Innovations GmbH. + * Copyright (c) 2018, TNG Technology Consulting GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__PUBLISHERS__RELATIVE_HUMIDITY_PUBLISHER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__PUBLISHERS__RELATIVE_HUMIDITY_PUBLISHER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "sensor_msgs/msg/relative_humidity.hpp" +#include "std_msgs/msg/header.hpp" + +using namespace std::chrono_literals; //NOLINT + +namespace nodes +{ + +class RelativeHumidityPublisher : public rclcpp::Node +{ +public: + RelativeHumidityPublisher(); + void setRelativeHumidity(double relative_humidity); + +private: + sensor_msgs::msg::RelativeHumidity createRelativeHumidityMessage(); + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + + double relative_humidity_; +}; + +RelativeHumidityPublisher::RelativeHumidityPublisher() +: Node("relative_humidity_publisher"), relative_humidity_(0.) +{ + publisher_ = this->create_publisher("relative_humidity"); + + auto timer_callback = + [this]() -> void { + auto message = createRelativeHumidityMessage(); + this->publisher_->publish(message); + }; + timer_ = this->create_wall_timer(500ms, timer_callback); +} + +sensor_msgs::msg::RelativeHumidity RelativeHumidityPublisher::createRelativeHumidityMessage() +{ + sensor_msgs::msg::RelativeHumidity relativeHumidityMessage; + + relativeHumidityMessage.header = std_msgs::msg::Header(); + relativeHumidityMessage.header.frame_id = "relative_humidity_frame"; + relativeHumidityMessage.header.stamp = rclcpp::Clock().now(); + + relativeHumidityMessage.relative_humidity = relative_humidity_; + relativeHumidityMessage.variance = 1.0; + + return relativeHumidityMessage; +} + +void RelativeHumidityPublisher::setRelativeHumidity(double relative_humidity) +{ + relative_humidity_ = relative_humidity; +} + +} // namespace nodes + +#endif // RVIZ_DEFAULT_PLUGINS__PUBLISHERS__RELATIVE_HUMIDITY_PUBLISHER_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/publishers/temperature_publisher.hpp b/rviz_default_plugins/test/rviz_default_plugins/publishers/temperature_publisher.hpp new file mode 100644 index 000000000..80b96db34 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/publishers/temperature_publisher.hpp @@ -0,0 +1,96 @@ +/* + * Copyright (c) 2017, Bosch Software Innovations GmbH. + * Copyright (c) 2018, TNG Technology Consulting GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__PUBLISHERS__TEMPERATURE_PUBLISHER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__PUBLISHERS__TEMPERATURE_PUBLISHER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "sensor_msgs/msg/temperature.hpp" +#include "std_msgs/msg/header.hpp" + +using namespace std::chrono_literals; //NOLINT + +namespace nodes +{ + +class TemperaturePublisher : public rclcpp::Node +{ +public: + TemperaturePublisher(); + void setTemperature(double temperature); + +private: + sensor_msgs::msg::Temperature createTemperatureMessage(); + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + + double temperature_; +}; + +TemperaturePublisher::TemperaturePublisher() +: Node("temperature_publisher"), temperature_(0.) +{ + publisher_ = this->create_publisher("temperature"); + + auto timer_callback = + [this]() -> void { + auto message = createTemperatureMessage(); + this->publisher_->publish(message); + }; + timer_ = this->create_wall_timer(500ms, timer_callback); +} + +sensor_msgs::msg::Temperature TemperaturePublisher::createTemperatureMessage() +{ + sensor_msgs::msg::Temperature temperatureMessage; + + temperatureMessage.header = std_msgs::msg::Header(); + temperatureMessage.header.frame_id = "temperature_frame"; + temperatureMessage.header.stamp = rclcpp::Clock().now(); + + temperatureMessage.temperature = temperature_; + temperatureMessage.variance = 1.0; + + return temperatureMessage; +} + +void TemperaturePublisher::setTemperature(double temperature) +{ + temperature_ = temperature; +} + +} // namespace nodes + +#endif // RVIZ_DEFAULT_PLUGINS__PUBLISHERS__TEMPERATURE_PUBLISHER_HPP_ diff --git a/rviz_visual_testing_framework/docs/README.md b/rviz_visual_testing_framework/docs/README.md index 3f1545d44..11a58b60f 100644 --- a/rviz_visual_testing_framework/docs/README.md +++ b/rviz_visual_testing_framework/docs/README.md @@ -60,12 +60,14 @@ After this method has been called in a test, interaction with RViz is no longer ## Running the tests -In order to run tests, use the following command: +In order to run the tests, they have to be build first and executed seperately. Use the following commands: - ament test --cmake-args -DEnableVisualTests=True + colcon build --cmake-args -DEnableVisualTests=True -DBUILD_TESTING=1 + + colcon test This will make the tests run and the screenshots will be compared to the existing reference images. -**NB**: CMake will cache the flag value, so that after setting them to `True` or `False`, the following time `ament test` is run without specifying the flag, the previously set value will be used. +**NB**: CMake will cache the flag value, so that after setting them to `True` or `False`, the following time `colcon test` is run without specifying the flag, the previously set value will be used. Furthermore, the reference images can be updated by running the tests after setting the environmental variable `GenerateReferenceImages` to `True`. diff --git a/rviz_visual_testing_framework/docs/documentation.md b/rviz_visual_testing_framework/docs/documentation.md index bd10dd02a..993d85505 100644 --- a/rviz_visual_testing_framework/docs/documentation.md +++ b/rviz_visual_testing_framework/docs/documentation.md @@ -30,9 +30,13 @@ To ensure that such resizing is always possible, test images are a lot wider tha For RViz itself: the CMake flag `EnableVisualTests` is provided to enable visual tests: - ament test --cmake-args -DEnableVisualTests=True + colcon build --cmake-args -DEnableVisualTests=True -DBUILD_TESTING=1 -This will make the tests run and the screenshots will be compared to the existing reference images. +This will build the tests. Afterwards run + + colcon test + +to execute the tests and the screenshots will be compared to the existing reference images. Furthermore, the reference images can be updated by running the tests after setting the environmental variable `GenerateReferenceImages` to `True`.