From f9724bd16298ed64bad85536ec3a2ecbe19c92bf Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Sat, 24 Aug 2019 04:52:53 +0530 Subject: [PATCH] [backport] Backport multicamera to dashing (#984) * [backport] Backport multicamera to dashing * fix test - use correct world Signed-off-by: Louise Poubel --- .../gazebo_plugins/MultiCameraPlugin.h | 56 --- .../gazebo_plugins/gazebo_ros_multicamera.h | 65 --- .../gazebo_ros_triggered_multicamera.h | 67 --- .../gazebo_plugins/src/MultiCameraPlugin.cpp | 118 ----- .../src/gazebo_ros_multicamera.cpp | 134 ------ .../src/gazebo_ros_triggered_multicamera.cpp | 110 ----- gazebo_plugins/CMakeLists.txt | 26 ++ .../gazebo_plugins/multi_camera_plugin.hpp | 79 ++++ gazebo_plugins/src/gazebo_ros_camera.cpp | 8 + .../src/gazebo_ros_multi_camera.cpp | 410 ++++++++++++++++++ .../src/gazebo_ros_multi_camera.hpp | 105 +++++ gazebo_plugins/src/multi_camera_plugin.cpp | 104 +++++ gazebo_plugins/test/CMakeLists.txt | 1 + .../test/test_gazebo_ros_multi_camera.cpp | 120 +++++ .../test/worlds/gazebo_ros_multi_camera.world | 65 +++ .../worlds/gazebo_ros_multi_camera_demo.world | 185 ++++++++ 16 files changed, 1103 insertions(+), 550 deletions(-) delete mode 100644 .ros1_unported/gazebo_plugins/include/gazebo_plugins/MultiCameraPlugin.h delete mode 100644 .ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h delete mode 100644 .ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_multicamera.h delete mode 100644 .ros1_unported/gazebo_plugins/src/MultiCameraPlugin.cpp delete mode 100644 .ros1_unported/gazebo_plugins/src/gazebo_ros_multicamera.cpp delete mode 100644 .ros1_unported/gazebo_plugins/src/gazebo_ros_triggered_multicamera.cpp create mode 100644 gazebo_plugins/include/gazebo_plugins/multi_camera_plugin.hpp create mode 100644 gazebo_plugins/src/gazebo_ros_multi_camera.cpp create mode 100644 gazebo_plugins/src/gazebo_ros_multi_camera.hpp create mode 100644 gazebo_plugins/src/multi_camera_plugin.cpp create mode 100644 gazebo_plugins/test/test_gazebo_ros_multi_camera.cpp create mode 100644 gazebo_plugins/test/worlds/gazebo_ros_multi_camera.world create mode 100644 gazebo_plugins/worlds/gazebo_ros_multi_camera_demo.world diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/MultiCameraPlugin.h b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/MultiCameraPlugin.h deleted file mode 100644 index b3092d0c9..000000000 --- a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/MultiCameraPlugin.h +++ /dev/null @@ -1,56 +0,0 @@ -/* - * Copyright 2012 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ -#ifndef _GAZEBO_MULTI_CAMERA_PLUGIN_HH_ -#define _GAZEBO_MULTI_CAMERA_PLUGIN_HH_ - -#include -#include - -#include -#include -#include -#include - -namespace gazebo -{ - class MultiCameraPlugin : public SensorPlugin - { - public: MultiCameraPlugin(); - - /// \brief Destructor - public: virtual ~MultiCameraPlugin(); - - public: virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf); - - public: virtual void OnNewFrameLeft(const unsigned char *_image, - unsigned int _width, unsigned int _height, - unsigned int _depth, const std::string &_format); - public: virtual void OnNewFrameRight(const unsigned char *_image, - unsigned int _width, unsigned int _height, - unsigned int _depth, const std::string &_format); - - protected: sensors::MultiCameraSensorPtr parentSensor; - - protected: std::vector width, height, depth; - protected: std::vector format; - - protected: std::vector camera; - - private: std::vector newFrameConnection; - }; -} -#endif diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h deleted file mode 100644 index aa9970541..000000000 --- a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h +++ /dev/null @@ -1,65 +0,0 @@ -/* - * Copyright 2012 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#ifndef GAZEBO_ROS_MULTICAMERA_HH -#define GAZEBO_ROS_MULTICAMERA_HH - -#include -#include - -// library for processing camera data for gazebo / ros conversions -#include -#include - -namespace gazebo -{ - class GazeboRosMultiCamera : public MultiCameraPlugin - { - /// \brief Constructor - /// \param parent The parent entity, must be a Model or a Sensor - public: GazeboRosMultiCamera(); - - /// \brief Destructor - public: ~GazeboRosMultiCamera(); - - /// \brief Load the plugin - /// \param take in SDF root element - public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); - - std::vector utils; - - protected: void OnNewFrame(const unsigned char *_image, - GazeboRosCameraUtils* util); - /// \brief Update the controller - /// FIXME: switch to function vectors - protected: virtual void OnNewFrameLeft(const unsigned char *_image, - unsigned int _width, unsigned int _height, - unsigned int _depth, const std::string &_format); - protected: virtual void OnNewFrameRight(const unsigned char *_image, - unsigned int _width, unsigned int _height, - unsigned int _depth, const std::string &_format); - - /// Bookkeeping flags that will be passed into the underlying - /// GazeboRosCameraUtils objects to let them share state about the parent - /// sensor. - private: boost::shared_ptr image_connect_count_; - private: boost::shared_ptr image_connect_count_lock_; - private: boost::shared_ptr was_active_; - }; -} -#endif - diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_multicamera.h b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_multicamera.h deleted file mode 100644 index 44769cf8a..000000000 --- a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_multicamera.h +++ /dev/null @@ -1,67 +0,0 @@ -/* - * Copyright 2017 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#ifndef GAZEBO_ROS_TRIGGERED_MULTICAMERA_HH -#define GAZEBO_ROS_TRIGGERED_MULTICAMERA_HH - -#include -#include - -// library for processing camera data for gazebo / ros conversions -#include -#include - -namespace gazebo -{ - class GazeboRosTriggeredMultiCamera : public MultiCameraPlugin - { - /// \brief Constructor - /// \param parent The parent entity, must be a Model or a Sensor - public: GazeboRosTriggeredMultiCamera(); - - /// \brief Destructor - public: ~GazeboRosTriggeredMultiCamera(); - - /// \brief Load the plugin - /// \param take in SDF root element - public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); - - std::vector triggered_cameras; - - /// \brief Update the controller - /// FIXME: switch to function vectors - protected: virtual void OnNewFrameLeft(const unsigned char *_image, - unsigned int _width, unsigned int _height, - unsigned int _depth, const std::string &_format); - protected: virtual void OnNewFrameRight(const unsigned char *_image, - unsigned int _width, unsigned int _height, - unsigned int _depth, const std::string &_format); - - /// Bookkeeping flags that will be passed into the underlying - /// GazeboRosCameraUtils objects to let them share state about the parent - /// sensor. - private: boost::shared_ptr image_connect_count_; - private: boost::shared_ptr image_connect_count_lock_; - private: boost::shared_ptr was_active_; - - protected: event::ConnectionPtr preRenderConnection_; - protected: void SetCameraEnabled(const bool _enabled); - protected: void PreRender(); - }; -} -#endif - diff --git a/.ros1_unported/gazebo_plugins/src/MultiCameraPlugin.cpp b/.ros1_unported/gazebo_plugins/src/MultiCameraPlugin.cpp deleted file mode 100644 index 457d7cc3b..000000000 --- a/.ros1_unported/gazebo_plugins/src/MultiCameraPlugin.cpp +++ /dev/null @@ -1,118 +0,0 @@ -/* - * Copyright 2013 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ -#include -#include -#include -#include - -using namespace gazebo; -GZ_REGISTER_SENSOR_PLUGIN(MultiCameraPlugin) - -///////////////////////////////////////////////// -MultiCameraPlugin::MultiCameraPlugin() : SensorPlugin() -{ -} - -///////////////////////////////////////////////// -MultiCameraPlugin::~MultiCameraPlugin() -{ - this->parentSensor.reset(); - this->camera.clear(); -} - -///////////////////////////////////////////////// -void MultiCameraPlugin::Load(sensors::SensorPtr _sensor, - sdf::ElementPtr /*_sdf*/) -{ - if (!_sensor) - gzerr << "Invalid sensor pointer.\n"; - - GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST; - this->parentSensor = - dynamic_pointer_cast(_sensor); - - if (!this->parentSensor) - { - gzerr << "MultiCameraPlugin requires a CameraSensor.\n"; - if (dynamic_pointer_cast(_sensor)) - gzmsg << "It is a depth camera sensor\n"; - if (dynamic_pointer_cast(_sensor)) - gzmsg << "It is a camera sensor\n"; - } - - if (!this->parentSensor) - { - gzerr << "MultiCameraPlugin not attached to a camera sensor\n"; - return; - } - - for (unsigned int i = 0; i < this->parentSensor->CameraCount(); ++i) - { - this->camera.push_back(this->parentSensor->Camera(i)); - - // save camera attributes - this->width.push_back(this->camera[i]->ImageWidth()); - this->height.push_back(this->camera[i]->ImageHeight()); - this->depth.push_back(this->camera[i]->ImageDepth()); - this->format.push_back(this->camera[i]->ImageFormat()); - - std::string cameraName = this->parentSensor->Camera(i)->Name(); - // gzdbg << "camera(" << i << ") name [" << cameraName << "]\n"; - - // FIXME: hardcoded 2 camera support only - if (cameraName.find("left") != std::string::npos) - { - this->newFrameConnection.push_back(this->camera[i]->ConnectNewImageFrame( - boost::bind(&MultiCameraPlugin::OnNewFrameLeft, - this, _1, _2, _3, _4, _5))); - } - else if (cameraName.find("right") != std::string::npos) - { - this->newFrameConnection.push_back(this->camera[i]->ConnectNewImageFrame( - boost::bind(&MultiCameraPlugin::OnNewFrameRight, - this, _1, _2, _3, _4, _5))); - } - } - - this->parentSensor->SetActive(true); -} - -///////////////////////////////////////////////// -void MultiCameraPlugin::OnNewFrameLeft(const unsigned char * /*_image*/, - unsigned int /*_width*/, - unsigned int /*_height*/, - unsigned int /*_depth*/, - const std::string &/*_format*/) -{ - /*rendering::Camera::SaveFrame(_image, this->width, - this->height, this->depth, this->format, - "/tmp/camera/me.jpg"); - */ -} - -///////////////////////////////////////////////// -void MultiCameraPlugin::OnNewFrameRight(const unsigned char * /*_image*/, - unsigned int /*_width*/, - unsigned int /*_height*/, - unsigned int /*_depth*/, - const std::string &/*_format*/) -{ - /*rendering::Camera::SaveFrame(_image, this->width, - this->height, this->depth, this->format, - "/tmp/camera/me.jpg"); - */ -} diff --git a/.ros1_unported/gazebo_plugins/src/gazebo_ros_multicamera.cpp b/.ros1_unported/gazebo_plugins/src/gazebo_ros_multicamera.cpp deleted file mode 100644 index fcfa6e98a..000000000 --- a/.ros1_unported/gazebo_plugins/src/gazebo_ros_multicamera.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/* - * Copyright 2013 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ -/* - * Desc: Syncronizes shutters across multiple cameras - * Author: John Hsu - * Date: 10 June 2013 - */ - -#include - -#include -#include -#include - -#include "gazebo_plugins/gazebo_ros_multicamera.h" - -namespace gazebo -{ -// Register this plugin with the simulator -GZ_REGISTER_SENSOR_PLUGIN(GazeboRosMultiCamera) - -//////////////////////////////////////////////////////////////////////////////// -// Constructor -GazeboRosMultiCamera::GazeboRosMultiCamera() -{ -} - -//////////////////////////////////////////////////////////////////////////////// -// Destructor -GazeboRosMultiCamera::~GazeboRosMultiCamera() -{ -} - -void GazeboRosMultiCamera::Load(sensors::SensorPtr _parent, - sdf::ElementPtr _sdf) -{ - MultiCameraPlugin::Load(_parent, _sdf); - - // Make sure the ROS node for Gazebo has already been initialized - if (!ros::isInitialized()) - { - ROS_FATAL_STREAM_NAMED("multicamera", "A ROS node for Gazebo has not been initialized, unable to load plugin. " - << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); - return; - } - - // initialize shared_ptr members - this->image_connect_count_ = boost::shared_ptr(new int(0)); - this->image_connect_count_lock_ = boost::shared_ptr(new boost::mutex); - this->was_active_ = boost::shared_ptr(new bool(false)); - - // copying from CameraPlugin into GazeboRosCameraUtils - for (unsigned i = 0; i < this->camera.size(); ++i) - { - GazeboRosCameraUtils* util = new GazeboRosCameraUtils(); - util->parentSensor_ = this->parentSensor; - util->width_ = this->width[i]; - util->height_ = this->height[i]; - util->depth_ = this->depth[i]; - util->format_ = this->format[i]; - util->camera_ = this->camera[i]; - // Set up a shared connection counter - util->image_connect_count_ = this->image_connect_count_; - util->image_connect_count_lock_ = this->image_connect_count_lock_; - util->was_active_ = this->was_active_; - if (this->camera[i]->Name().find("left") != std::string::npos) - { - // FIXME: hardcoded, left hack_baseline_ 0 - util->Load(_parent, _sdf, "/left", 0.0); - } - else if (this->camera[i]->Name().find("right") != std::string::npos) - { - double hackBaseline = 0.0; - if (_sdf->HasElement("hackBaseline")) - hackBaseline = _sdf->Get("hackBaseline"); - util->Load(_parent, _sdf, "/right", hackBaseline); - } - this->utils.push_back(util); - } -} - -//////////////////////////////////////////////////////////////////////////////// - -void GazeboRosMultiCamera::OnNewFrame(const unsigned char *_image, - GazeboRosCameraUtils* util) -{ -# if GAZEBO_MAJOR_VERSION >= 7 - common::Time sensor_update_time = util->parentSensor_->LastMeasurementTime(); -# else - common::Time sensor_update_time = util->parentSensor_->GetLastMeasurementTime(); -# endif - - if (util->parentSensor_->IsActive()) - { - if (sensor_update_time - util->last_update_time_ >= util->update_period_) - { - util->PutCameraData(_image, sensor_update_time); - util->PublishCameraInfo(sensor_update_time); - util->last_update_time_ = sensor_update_time; - } - } -} - -// Update the controller -void GazeboRosMultiCamera::OnNewFrameLeft(const unsigned char *_image, - unsigned int _width, unsigned int _height, unsigned int _depth, - const std::string &_format) -{ - OnNewFrame(_image, this->utils[0]); -} - -//////////////////////////////////////////////////////////////////////////////// -// Update the controller -void GazeboRosMultiCamera::OnNewFrameRight(const unsigned char *_image, - unsigned int _width, unsigned int _height, unsigned int _depth, - const std::string &_format) -{ - OnNewFrame(_image, this->utils[1]); -} -} diff --git a/.ros1_unported/gazebo_plugins/src/gazebo_ros_triggered_multicamera.cpp b/.ros1_unported/gazebo_plugins/src/gazebo_ros_triggered_multicamera.cpp deleted file mode 100644 index 11cbff5a9..000000000 --- a/.ros1_unported/gazebo_plugins/src/gazebo_ros_triggered_multicamera.cpp +++ /dev/null @@ -1,110 +0,0 @@ -/* - * Copyright 2017 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#include - -#include -#include -#include - -#include "gazebo_plugins/gazebo_ros_triggered_multicamera.h" - -namespace gazebo -{ -// Register this plugin with the simulator -GZ_REGISTER_SENSOR_PLUGIN(GazeboRosTriggeredMultiCamera) - -//////////////////////////////////////////////////////////////////////////////// -// Constructor -GazeboRosTriggeredMultiCamera::GazeboRosTriggeredMultiCamera() -{ -} - -//////////////////////////////////////////////////////////////////////////////// -// Destructor -GazeboRosTriggeredMultiCamera::~GazeboRosTriggeredMultiCamera() -{ -} - -void GazeboRosTriggeredMultiCamera::Load(sensors::SensorPtr _parent, - sdf::ElementPtr _sdf) -{ - MultiCameraPlugin::Load(_parent, _sdf); - - // Make sure the ROS node for Gazebo has already been initialized - if (!ros::isInitialized()) - { - ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " - << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); - return; - } - - // initialize shared_ptr members - this->image_connect_count_ = boost::shared_ptr(new int(0)); - this->image_connect_count_lock_ = boost::shared_ptr(new boost::mutex); - this->was_active_ = boost::shared_ptr(new bool(false)); - - // copying from CameraPlugin into GazeboRosCameraUtils - for (unsigned i = 0; i < this->camera.size(); ++i) - { - GazeboRosTriggeredCamera * cam = new GazeboRosTriggeredCamera(); - cam->parentSensor_ = this->parentSensor; - cam->width_ = this->width[i]; - cam->height_ = this->height[i]; - cam->depth_ = this->depth[i]; - cam->format_ = this->format[i]; - cam->camera_ = this->camera[i]; - // Set up a shared connection counter - cam->image_connect_count_ = this->image_connect_count_; - cam->image_connect_count_lock_ = this->image_connect_count_lock_; - cam->was_active_ = this->was_active_; - if (this->camera[i]->Name().find("left") != std::string::npos) - { - // FIXME: hardcoded, left hack_baseline_ 0 - cam->Load(_parent, _sdf, "/left", 0.0); - } - else if (this->camera[i]->Name().find("right") != std::string::npos) - { - double hackBaseline = 0.0; - if (_sdf->HasElement("hackBaseline")) - hackBaseline = _sdf->Get("hackBaseline"); - cam->Load(_parent, _sdf, "/right", hackBaseline); - } - this->triggered_cameras.push_back(cam); - } -} - -//////////////////////////////////////////////////////////////////////////////// -// Update the controller -void GazeboRosTriggeredMultiCamera::OnNewFrameLeft(const unsigned char *_image, - unsigned int _width, unsigned int _height, unsigned int _depth, - const std::string &_format) -{ - GazeboRosTriggeredCamera * cam = this->triggered_cameras[0]; - cam->OnNewFrame(_image, _width, _height, _depth, _format); -} - -//////////////////////////////////////////////////////////////////////////////// -// Update the controller -void GazeboRosTriggeredMultiCamera::OnNewFrameRight(const unsigned char *_image, - unsigned int _width, unsigned int _height, unsigned int _depth, - const std::string &_format) -{ - GazeboRosTriggeredCamera * cam = this->triggered_cameras[1]; - cam->OnNewFrame(_image, _width, _height, _depth, _format); -} -} diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 4c921cbc9..7394e17a1 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -93,6 +93,29 @@ ament_target_dependencies(gazebo_ros_template ) ament_export_libraries(gazebo_ros_template) +# multi_camera_plugin +add_library(multi_camera_plugin SHARED + src/multi_camera_plugin.cpp +) + +# gazebo_ros_multi_camera +add_library(gazebo_ros_multi_camera SHARED + src/gazebo_ros_multi_camera.cpp +) +ament_target_dependencies(gazebo_ros_multi_camera + "camera_info_manager" + "gazebo_dev" + "gazebo_ros" + "image_transport" + "rclcpp" + "sensor_msgs" + "std_msgs" +) +target_link_libraries(gazebo_ros_multi_camera + multi_camera_plugin +) +ament_export_libraries(gazebo_ros_multi_camera) + # gazebo_ros_depth_camera add_library(gazebo_ros_depth_camera SHARED src/gazebo_ros_depth_camera.cpp @@ -127,6 +150,7 @@ ament_target_dependencies(gazebo_ros_camera target_link_libraries(gazebo_ros_camera CameraPlugin gazebo_ros_depth_camera + gazebo_ros_multi_camera ) ament_export_libraries(gazebo_ros_camera) @@ -377,6 +401,7 @@ install(TARGETS gazebo_ros_gps_sensor gazebo_ros_joint_pose_trajectory gazebo_ros_joint_state_publisher + gazebo_ros_multi_camera gazebo_ros_planar_move gazebo_ros_projector gazebo_ros_ray_sensor @@ -385,6 +410,7 @@ install(TARGETS gazebo_ros_tricycle_drive gazebo_ros_vacuum_gripper gazebo_ros_video + multi_camera_plugin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) diff --git a/gazebo_plugins/include/gazebo_plugins/multi_camera_plugin.hpp b/gazebo_plugins/include/gazebo_plugins/multi_camera_plugin.hpp new file mode 100644 index 000000000..0bfe812a4 --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/multi_camera_plugin.hpp @@ -0,0 +1,79 @@ +// Copyright 2019 Open Source Robotics Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GAZEBO_PLUGINS__MULTI_CAMERA_PLUGIN_HPP_ +#define GAZEBO_PLUGINS__MULTI_CAMERA_PLUGIN_HPP_ + +#include +#include +#include +#include + +#include +#include +#include + +namespace gazebo_plugins +{ +class MultiCameraPluginPrivate; + +class GAZEBO_VISIBLE MultiCameraPlugin : public gazebo::SensorPlugin +{ +public: + /// Constructor + MultiCameraPlugin(); + + /// Destructor + virtual ~MultiCameraPlugin(); + +protected: + // Documentation inherited + virtual void Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _sdf); + + /// Callback when multi camera produces a new image. + /* + * \details This is called at the camera's update rate. + * \details Not called when the camera isn't active. For a triggered camera, it will only be + * called after triggered. + * \param[in] _image Image + * \param[in] _width Image width + * \param[in] _height Image height + * \param[in] _depth Image depth + * \param[in] _format Image format + * \param[in] _camera_num Index number of camera + */ + virtual void OnNewMultiFrame( + const unsigned char * _image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string & _format, const int _camera_num); + + // Pointer to multicamera sensor + gazebo::sensors::MultiCameraSensorPtr parent_sensor_; + + // Store dimensions of camera image + std::vector width_, height_, depth_; + + // Store image format_ + std::vector format_; + + // Pointer to multicamera + std::vector camera_; + +private: + /// Private data pointer + std::unique_ptr impl_; +}; +} // namespace gazebo_plugins + +#endif // GAZEBO_PLUGINS__MULTI_CAMERA_PLUGIN_HPP_ diff --git a/gazebo_plugins/src/gazebo_ros_camera.cpp b/gazebo_plugins/src/gazebo_ros_camera.cpp index 93aa9d1aa..b8e5cde49 100644 --- a/gazebo_plugins/src/gazebo_ros_camera.cpp +++ b/gazebo_plugins/src/gazebo_ros_camera.cpp @@ -34,6 +34,7 @@ // Not installed on Dashing because it will be removed on Eloquent #include "gazebo_ros_depth_camera.hpp" +#include "gazebo_ros_multi_camera.hpp" namespace gazebo_plugins { @@ -78,6 +79,9 @@ class GazeboRosCameraPrivate /// Pointer to GazeboROS Depth Camera std::shared_ptr depth_camera_; + + /// Pointer to GazeboROS Multi Camera + std::shared_ptr multi_camera_; }; GazeboRosCamera::GazeboRosCamera() @@ -96,6 +100,10 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _ impl_->depth_camera_ = std::make_shared(); impl_->depth_camera_->Load(_sensor, _sdf); return; + } else if (std::dynamic_pointer_cast(_sensor)) { + impl_->multi_camera_ = std::make_shared(); + impl_->multi_camera_->Load(_sensor, _sdf); + return; } gazebo::CameraPlugin::Load(_sensor, _sdf); diff --git a/gazebo_plugins/src/gazebo_ros_multi_camera.cpp b/gazebo_plugins/src/gazebo_ros_multi_camera.cpp new file mode 100644 index 000000000..74f4b6ecc --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_multi_camera.cpp @@ -0,0 +1,410 @@ +// Copyright 2019 Open Source Robotics Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +// Not installed on Dashing because it will be removed on Eloquent +#include "gazebo_ros_multi_camera.hpp" + +namespace gazebo_plugins +{ +class GazeboRosMultiCameraPrivate +{ +public: + /// A pointer to the GazeboROS node. + gazebo_ros::Node::SharedPtr ros_node_{nullptr}; + + /// Image publishers. + std::vector image_pub_; + + /// Camera info publishers. + std::vector::SharedPtr> camera_info_pub_; + + /// Trigger subscriber, in case it's a triggered camera + rclcpp::Subscription::SharedPtr trigger_sub_{nullptr}; + + /// Camera info managers + std::vector> camera_info_manager_; + + /// Image encodings + std::vector img_encoding_; + + /// Frame name, to be used by TF. + std::string frame_name_; + + /// Step sizes for fillImage + std::vector img_step_; + + /// Connects to pre-render events. + gazebo::event::ConnectionPtr pre_render_connection_; + + /// Keeps track of how many times the camera has been triggered since it last published an image. + int triggered{0}; + + /// Protects trigger. + std::mutex trigger_mutex_; +}; + +GazeboRosMultiCamera::GazeboRosMultiCamera() +: impl_(std::make_unique()) +{ +} + +GazeboRosMultiCamera::~GazeboRosMultiCamera() +{ + for (auto pub : impl_->image_pub_) { + pub.shutdown(); + } +} + +void GazeboRosMultiCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) +{ + MultiCameraPlugin::Load(_sensor, _sdf); + + // Camera name + auto camera_name = _sdf->Get("camera_name", _sensor->Name()).first; + + // Get tf frame for output + impl_->frame_name_ = gazebo_ros::SensorFrameID(*_sensor, *_sdf); + + // Initialize ROS node + impl_->ros_node_ = gazebo_ros::Node::Get(_sdf); + + auto num_cameras = MultiCameraPlugin::parent_sensor_->CameraCount(); + + for (uint64_t i = 0; i < num_cameras; ++i) { + // Image publisher + impl_->image_pub_.push_back(image_transport::create_publisher(impl_->ros_node_.get(), + camera_name + "/" + MultiCameraPlugin::camera_[i]->Name() + "/image_raw")); + + // RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing %s camera images to [%s]", + // MultiCameraPlugin::camera_[i]->Name().c_str(), + // impl_->image_pub_.back().getTopic()); + + // Camera info publisher + impl_->camera_info_pub_.push_back( + impl_->ros_node_->create_publisher( + camera_name + "/" + MultiCameraPlugin::camera_[i]->Name() + "/camera_info", + rclcpp::SensorDataQoS())); + + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing %s camera info to [%s]", + MultiCameraPlugin::camera_[i]->Name().c_str(), + impl_->camera_info_pub_[i]->get_topic_name()); + } + + // Trigger + if (_sdf->Get("triggered", false).first) { + impl_->trigger_sub_ = impl_->ros_node_->create_subscription( + camera_name + "/image_trigger", + rclcpp::QoS(rclcpp::KeepLast(1)), + std::bind(&GazeboRosMultiCamera::OnTrigger, this, std::placeholders::_1)); + + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Subscribed to [%s]", + impl_->trigger_sub_->get_topic_name()); + + SetCameraEnabled(false); + impl_->pre_render_connection_ = gazebo::event::Events::ConnectPreRender( + std::bind(&GazeboRosMultiCamera::PreRender, this)); + } + + auto image_format = MultiCameraPlugin::format_; + + for (const auto & format : image_format) { + if (format == "L8" || format == "L_INT8") { + impl_->img_encoding_.emplace_back(sensor_msgs::image_encodings::MONO8); + impl_->img_step_.push_back(1); + } else if (format == "L16" || format == "L_INT16") { + impl_->img_encoding_.emplace_back(sensor_msgs::image_encodings::MONO16); + impl_->img_step_.push_back(2); + } else if (format == "R8G8B8" || format == "RGB_INT8") { + impl_->img_encoding_.emplace_back(sensor_msgs::image_encodings::RGB8); + impl_->img_step_.push_back(3); + } else if (format == "B8G8R8" || format == "BGR_INT8") { + impl_->img_encoding_.emplace_back(sensor_msgs::image_encodings::BGR8); + impl_->img_step_.push_back(3); + } else if (format == "R16G16B16" || format == "RGB_INT16") { + impl_->img_encoding_.emplace_back(sensor_msgs::image_encodings::RGB16); + impl_->img_step_.push_back(6); + } else if (format == "BAYER_RGGB8") { + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "bayer simulation may be computationally expensive."); + impl_->img_encoding_.emplace_back(sensor_msgs::image_encodings::BAYER_RGGB8); + impl_->img_step_.push_back(1); + } else if (format == "BAYER_BGGR8") { + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "bayer simulation may be computationally expensive."); + impl_->img_encoding_.emplace_back(sensor_msgs::image_encodings::BAYER_BGGR8); + impl_->img_step_.push_back(1); + } else if (format == "BAYER_GBRG8") { + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "bayer simulation may be computationally expensive."); + impl_->img_encoding_.emplace_back(sensor_msgs::image_encodings::BAYER_GBRG8); + impl_->img_step_.push_back(1); + } else if (format == "BAYER_GRBG8") { + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "bayer simulation may be computationally expensive."); + impl_->img_encoding_.emplace_back(sensor_msgs::image_encodings::BAYER_GRBG8); + impl_->img_step_.push_back(1); + } else { + RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Unsupported Gazebo ImageFormat, using BGR8\n"); + impl_->img_encoding_.emplace_back(sensor_msgs::image_encodings::BGR8); + impl_->img_step_.push_back(3); + } + } + + auto width = MultiCameraPlugin::width_; + auto height = MultiCameraPlugin::height_; + auto camera = MultiCameraPlugin::camera_; + + for (uint64_t i = 0; i < num_cameras; ++i) { + // C parameters + auto default_cx = (static_cast(width[i]) + 1.0) / 2.0; + auto cx = _sdf->Get("cx", default_cx).first; + + auto default_cy = (static_cast(height[i]) + 1.0) / 2.0; + auto cy = _sdf->Get("cy", default_cy).first; + + auto hfov = camera[i]->HFOV().Radian(); + + double computed_focal_length = + (static_cast(width[i])) / (2.0 * tan(hfov / 2.0)); + + // Focal length + auto focal_length = _sdf->Get("focal_length", 0.0).first; + if (focal_length == 0) { + focal_length = computed_focal_length; + } else if (!ignition::math::equal(focal_length, computed_focal_length)) { + RCLCPP_WARN(impl_->ros_node_->get_logger(), + "The [%f] you have provided for camera [%s]" + " is inconsistent with specified [%d] and" + " HFOV [%f]. Please double check to see that" + " focal_length = width / (2.0 * tan(HFOV/2.0))." + " The expected focal_length value is [%f]," + " please update your camera model description accordingly.", + focal_length, _sensor->Name().c_str(), width[i], hfov, computed_focal_length); + } + + // CameraInfo + sensor_msgs::msg::CameraInfo camera_info_msg; + camera_info_msg.header.frame_id = impl_->frame_name_; + camera_info_msg.height = height[i]; + camera_info_msg.width = width[i]; + camera_info_msg.distortion_model = "plumb_bob"; + camera_info_msg.d.resize(5); + + // Allow the user to disable automatic cropping (used to remove barrel + // distortion black border. The crop can be useful, but also skewes + // the lens distortion, making the supplied k and t values incorrect. + auto border_crop = _sdf->Get("border_crop", true).first; + auto hack_baseline = _sdf->Get("hack_baseline", 0.0).first; + + // Get distortion from camera + double distortion_k1{0.0}; + double distortion_k2{0.0}; + double distortion_k3{0.0}; + double distortion_t1{0.0}; + double distortion_t2{0.0}; + if (camera[i]->LensDistortion()) { + camera[i]->LensDistortion()->SetCrop(border_crop); + + distortion_k1 = camera[i]->LensDistortion()->K1(); + distortion_k2 = camera[i]->LensDistortion()->K2(); + distortion_k3 = camera[i]->LensDistortion()->K3(); + distortion_t1 = camera[i]->LensDistortion()->P1(); + distortion_t2 = camera[i]->LensDistortion()->P2(); + } + + // D = {k1, k2, t1, t2, k3}, as specified in: + // - sensor_msgs/CameraInfo: http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html + // - OpenCV: http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html + camera_info_msg.d[0] = distortion_k1; + camera_info_msg.d[1] = distortion_k2; + camera_info_msg.d[2] = distortion_t1; + camera_info_msg.d[3] = distortion_t2; + camera_info_msg.d[4] = distortion_k3; + + // Original camera matrix + camera_info_msg.k[0] = focal_length; + camera_info_msg.k[1] = 0.0; + camera_info_msg.k[2] = cx; + camera_info_msg.k[3] = 0.0; + camera_info_msg.k[4] = focal_length; + camera_info_msg.k[5] = cy; + camera_info_msg.k[6] = 0.0; + camera_info_msg.k[7] = 0.0; + camera_info_msg.k[8] = 1.0; + + // rectification + camera_info_msg.r[0] = 1.0; + camera_info_msg.r[1] = 0.0; + camera_info_msg.r[2] = 0.0; + camera_info_msg.r[3] = 0.0; + camera_info_msg.r[4] = 1.0; + camera_info_msg.r[5] = 0.0; + camera_info_msg.r[6] = 0.0; + camera_info_msg.r[7] = 0.0; + camera_info_msg.r[8] = 1.0; + + // camera_ projection matrix (same as camera_ matrix due + // to lack of distortion/rectification) (is this generated?) + camera_info_msg.p[0] = focal_length; + camera_info_msg.p[1] = 0.0; + camera_info_msg.p[2] = cx; + camera_info_msg.p[3] = -focal_length * hack_baseline; + camera_info_msg.p[4] = 0.0; + camera_info_msg.p[5] = focal_length; + camera_info_msg.p[6] = cy; + camera_info_msg.p[7] = 0.0; + camera_info_msg.p[8] = 0.0; + camera_info_msg.p[9] = 0.0; + camera_info_msg.p[10] = 1.0; + camera_info_msg.p[11] = 0.0; + + // Initialize camera_info_manager + impl_->camera_info_manager_.push_back(std::make_shared( + impl_->ros_node_.get(), camera_name)); + impl_->camera_info_manager_.back()->setCameraInfo(camera_info_msg); + } + + impl_->ros_node_->declare_parameter( + "update_rate", MultiCameraPlugin::parent_sensor_->UpdateRate()); + + auto existing_callback = impl_->ros_node_->set_on_parameters_set_callback(nullptr); + auto param_change_callback = + [this, existing_callback](std::vector parameters) { + auto result = rcl_interfaces::msg::SetParametersResult(); + if (nullptr != existing_callback) { + result = existing_callback(parameters); + if (!result.successful) { + return result; + } + } + + result.successful = true; + for (const auto & parameter : parameters) { + std::string param_name = parameter.get_name(); + if (param_name == "update_rate") { + if (nullptr != impl_->trigger_sub_) { + RCLCPP_WARN(impl_->ros_node_->get_logger(), + "Cannot set update rate for triggered camera"); + result.successful = false; + } else { + rclcpp::ParameterType parameter_type = parameter.get_type(); + if (rclcpp::ParameterType::PARAMETER_DOUBLE == parameter_type) { + double rate = parameter.as_double(); + MultiCameraPlugin::parent_sensor_->SetUpdateRate(rate); + + if (rate >= 0.0) { + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "Camera update rate changed to [%.2f Hz]", rate); + } else { + RCLCPP_WARN(impl_->ros_node_->get_logger(), + "Camera update rate should be positive. Setting to maximum"); + } + } else { + RCLCPP_WARN(impl_->ros_node_->get_logger(), + "Value for param [update_rate] has to be of double type."); + result.successful = false; + } + } + } + } + return result; + }; + + impl_->ros_node_->set_on_parameters_set_callback(param_change_callback); +} + +//////////////////////////////////////////////////////////////////////////////// +void GazeboRosMultiCamera::OnNewMultiFrame( + const unsigned char * _image, + unsigned int _width, + unsigned int _height, + unsigned int /*_depth*/, + const std::string & /*_format*/, + const int _camera_num) +{ + // TODO(shivesh) Enable / disable sensor once SubscriberStatusCallback has been ported to ROS2 + + gazebo::common::Time sensor_update_time = + MultiCameraPlugin::parent_sensor_->LastMeasurementTime(); + + // Publish camera info + auto camera_info_msg = impl_->camera_info_manager_[_camera_num]->getCameraInfo(); + camera_info_msg.header.stamp = gazebo_ros::Convert( + sensor_update_time); + + impl_->camera_info_pub_[_camera_num]->publish(camera_info_msg); + + // Publish image + auto image_msg = sensor_msgs::msg::Image(); + image_msg.header.frame_id = impl_->frame_name_; + image_msg.header.stamp = gazebo_ros::Convert(sensor_update_time); + + // Copy from src to image_msg + sensor_msgs::fillImage(image_msg, impl_->img_encoding_[_camera_num], _height, _width, + impl_->img_step_[_camera_num] * _width, reinterpret_cast(_image)); + + impl_->image_pub_[_camera_num].publish(image_msg); + + // Disable camera if it's a triggered camera + if (nullptr != impl_->trigger_sub_) { + SetCameraEnabled(false); + + std::lock_guard lock(impl_->trigger_mutex_); + impl_->triggered = std::max(impl_->triggered - 1, 0); + } +} + +void GazeboRosMultiCamera::SetCameraEnabled(const bool _enabled) +{ + parent_sensor_->SetActive(_enabled); + parent_sensor_->SetUpdateRate(_enabled ? 0.0 : ignition::math::MIN_D); +} + +void GazeboRosMultiCamera::PreRender() +{ + std::lock_guard lock(impl_->trigger_mutex_); + if (impl_->triggered > 0) { + SetCameraEnabled(true); + } +} + +void GazeboRosMultiCamera::OnTrigger(const std_msgs::msg::Empty::SharedPtr) +{ + std::lock_guard lock(impl_->trigger_mutex_); + impl_->triggered++; +} + +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosMultiCamera) +} // namespace gazebo_plugins diff --git a/gazebo_plugins/src/gazebo_ros_multi_camera.hpp b/gazebo_plugins/src/gazebo_ros_multi_camera.hpp new file mode 100644 index 000000000..3958a96ad --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_multi_camera.hpp @@ -0,0 +1,105 @@ +// Copyright 2019 Open Source Robotics Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GAZEBO_ROS_MULTI_CAMERA_HPP_ +#define GAZEBO_ROS_MULTI_CAMERA_HPP_ + +#include +#include + +#include +#include + +namespace gazebo_plugins +{ +class GazeboRosMultiCameraPrivate; + +/// A plugin that publishes images and camera info for multi camera sensors. +/** + Example Usage: + \code{.xml} + + + + custom_ns + custom_camera/camera_name/image_raw:=custom_camera/camera_name/custom_image_raw + custom_camera/camera_name/camera_info:=custom_camera/camera_name/custom_camera_info + custom_camera/image_trigger:=custom_camera/custom_trigger + + + + custom_camera + + + custom_frame + + + true + + 0.07 + + \endcode +*/ +class GazeboRosMultiCamera : public MultiCameraPlugin +{ +public: + /// Constructor + GazeboRosMultiCamera(); + + /// Destructor + ~GazeboRosMultiCamera(); + + // Documentation inherited + void Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) override; + +protected: + /// Callback when multi camera produces a new image. + /* + * \details This is called at the multi camera's update rate. + * \details Not called when the camera isn't active. For a triggered multi camera, it will only be + * called after triggered. + * \param[in] _image Image + * \param[in] _width Image width + * \param[in] _height Image height + * \param[in] _depth Image depth + * \param[in] _format Image format + * \param[in] _camera_num Index number of camera + */ + + void OnNewMultiFrame( + const unsigned char * _image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string & _format, const int _camera_num) override; + + /// Callback when camera is triggered. + void OnTrigger(const std_msgs::msg::Empty::SharedPtr _dummy); + + /// Callback on pre-render event. + void PreRender(); + + /// Enables or disables the camera so it produces messages or not. + /// param[in] _enabled True to enable. + void SetCameraEnabled(const bool _enabled); + +private: + /// Private data pointer + std::unique_ptr impl_; +}; +} // namespace gazebo_plugins + +#endif // GAZEBO_ROS_MULTI_CAMERA_HPP_ diff --git a/gazebo_plugins/src/multi_camera_plugin.cpp b/gazebo_plugins/src/multi_camera_plugin.cpp new file mode 100644 index 000000000..af5901781 --- /dev/null +++ b/gazebo_plugins/src/multi_camera_plugin.cpp @@ -0,0 +1,104 @@ +// Copyright 2019 Open Source Robotics Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include +#include + +namespace gazebo_plugins +{ +class MultiCameraPluginPrivate +{ +public: + /// Connects to pre-render events. + std::vector new_frame_connection_; +}; + +///////////////////////////////////////////////// +MultiCameraPlugin::MultiCameraPlugin() +: SensorPlugin(), + impl_(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +MultiCameraPlugin::~MultiCameraPlugin() +{ + for (auto conn : impl_->new_frame_connection_) { + conn.reset(); + } + impl_->new_frame_connection_.clear(); + + parent_sensor_.reset(); + camera_.clear(); +} + +///////////////////////////////////////////////// +void MultiCameraPlugin::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr /*_sdf*/) +{ + if (!_sensor) { + gzerr << "Invalid sensor pointer.\n"; + } + + parent_sensor_ = std::dynamic_pointer_cast(_sensor); + + if (!parent_sensor_) { + gzerr << "MultiCameraPlugin requires a CameraSensor.\n"; + if (std::dynamic_pointer_cast(_sensor)) { + gzmsg << "It is a depth camera sensor\n"; + } + if (std::dynamic_pointer_cast(_sensor)) { + gzmsg << "It is a camera sensor\n"; + } + } + + if (!parent_sensor_) { + gzerr << "MultiCameraPlugin not attached to a camera sensor\n"; + return; + } + for (unsigned int i = 0; i < parent_sensor_->CameraCount(); ++i) { + camera_.push_back(parent_sensor_->Camera(i)); + + // save camera attributes + width_.push_back(camera_[i]->ImageWidth()); + height_.push_back(camera_[i]->ImageHeight()); + depth_.push_back(camera_[i]->ImageDepth()); + format_.push_back(camera_[i]->ImageFormat()); + + impl_->new_frame_connection_.push_back(camera_[i]->ConnectNewImageFrame( + std::bind(&MultiCameraPlugin::OnNewMultiFrame, + this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, i))); + } + + parent_sensor_->SetActive(true); +} + +///////////////////////////////////////////////// +void MultiCameraPlugin::OnNewMultiFrame( + const unsigned char * /*_image*/, + unsigned int /*_width*/, + unsigned int /*_height*/, + unsigned int /*_depth*/, + const std::string & /*_format*/, + const int /*_camera_num*/) +{ +} +GZ_REGISTER_SENSOR_PLUGIN(MultiCameraPlugin) +} // namespace gazebo_plugins diff --git a/gazebo_plugins/test/CMakeLists.txt b/gazebo_plugins/test/CMakeLists.txt index 27471498b..8dc81881d 100644 --- a/gazebo_plugins/test/CMakeLists.txt +++ b/gazebo_plugins/test/CMakeLists.txt @@ -37,6 +37,7 @@ set(tests ${tests} test_gazebo_ros_camera_distortion test_gazebo_ros_camera_triggered test_gazebo_ros_depth_camera + test_gazebo_ros_multi_camera test_gazebo_ros_projector test_gazebo_ros_video ) diff --git a/gazebo_plugins/test/test_gazebo_ros_multi_camera.cpp b/gazebo_plugins/test/test_gazebo_ros_multi_camera.cpp new file mode 100644 index 000000000..3c7af3b32 --- /dev/null +++ b/gazebo_plugins/test/test_gazebo_ros_multi_camera.cpp @@ -0,0 +1,120 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include +#include + +using namespace std::literals::chrono_literals; // NOLINT + +/// Test parameters +struct TestParams +{ + /// Path to world file + std::string world; + + /// Image topics to subscribe to + std::string left_topic, right_topic; +}; + +class GazeboRosMultiCameraTest + : public gazebo::ServerFixture, public ::testing::WithParamInterface +{ +}; + +// Test that the multi camera images are published and have correct timestamp +TEST_P(GazeboRosMultiCameraTest, CameraSubscribeTest) +{ + // Load test world and start paused + this->Load(GetParam().world, true); + + // World + auto world = gazebo::physics::get_world(); + ASSERT_NE(nullptr, world); + + // Create node and executor + auto node = std::make_shared("gazebo_ros_multicamera_test"); + ASSERT_NE(nullptr, node); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + // Subscribe + unsigned int msg_count_left{0}; + builtin_interfaces::msg::Time image_stamp_left; + + auto sub_left = image_transport::create_subscription(node.get(), GetParam().left_topic, + [&](const sensor_msgs::msg::Image::ConstSharedPtr & msg) { + image_stamp_left = msg->header.stamp; + ++msg_count_left; + }, + "raw"); + + // Update rate is 0.5 Hz, so we step 3s sim time to be sure we get exactly 1 image at 2s + world->Step(3000); + unsigned int sleep = 0; + unsigned int max_sleep = 30; + while (sleep < max_sleep && msg_count_left == 0) { + executor.spin_once(100ms); + sleep++; + } + + EXPECT_EQ(1u, msg_count_left); + EXPECT_EQ(2.0, image_stamp_left.sec); + + // Clean up + sub_left.shutdown(); + + unsigned int msg_count_right{0}; + builtin_interfaces::msg::Time image_stamp_right; + auto sub_right = image_transport::create_subscription(node.get(), GetParam().right_topic, + [&](const sensor_msgs::msg::Image::ConstSharedPtr & msg) { + image_stamp_right = msg->header.stamp; + ++msg_count_right; + }, + "raw"); + + // Update rate is 0.5 Hz, so we step 3s sim time to be sure we get exactly 1 image at 4s + world->Step(3000); + sleep = 0; + max_sleep = 30; + while (sleep < max_sleep && msg_count_right == 0) { + executor.spin_once(100ms); + sleep++; + } + + EXPECT_EQ(1u, msg_count_right); + EXPECT_EQ(4.0, image_stamp_right.sec); + + // Clean up + sub_right.shutdown(); +} + +INSTANTIATE_TEST_CASE_P(GazeboRosMultiCamera, GazeboRosMultiCameraTest, ::testing::Values( + TestParams({"worlds/gazebo_ros_multi_camera.world", + "test_cam/camera/left/image_test", + "test_cam/camera/right/image_test"}) + ), ); + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/gazebo_plugins/test/worlds/gazebo_ros_multi_camera.world b/gazebo_plugins/test/worlds/gazebo_ros_multi_camera.world new file mode 100644 index 000000000..16f995d2f --- /dev/null +++ b/gazebo_plugins/test/worlds/gazebo_ros_multi_camera.world @@ -0,0 +1,65 @@ + + + + + true + + + 0.5 + + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + + test_cam + camera1/left/image_raw:=camera/left/image_test + camera1/right/image_raw:=camera/right/image_test + camera1/left/camera_info:=camera/left/camera_info_test + camera1/right/camera_info:=camera/right/camera_info_test + + + + 0.07 + + + + + + diff --git a/gazebo_plugins/worlds/gazebo_ros_multi_camera_demo.world b/gazebo_plugins/worlds/gazebo_ros_multi_camera_demo.world new file mode 100644 index 000000000..8894ff8c8 --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_multi_camera_demo.world @@ -0,0 +1,185 @@ + + + + + + + model://sun + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + 1.0 + 0 + + + + 10000 + 0.001 + + + + + + false + + + 0 0 1 + 100 100 + + + + + + + + + + + false + -1 0 1 0 0 0 + false + + + 0.026 + + + 1.664e-5 + 1.664e-5 + 1.664e-5 + 0 + 0 + 0 + + + + + + 0.04 + + + + + 1.0 + 0 + + + + 10000 + 0.001 + + + + + + + + 0.04 + + + + + + + + 0 0 0.5 0 0 3.14 + true + + + 0 -0.2 0 0 0 0 + 60 + true + + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + 0 0.4 0 0 0 0 + 0.7 + + 320 + 480 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + + demo_cam + + mycamera/left/image_raw:=mycamera/left/image_demo + mycamera/right/image_raw:=mycamera/right/image_demo + mycamera/left/camera_info:=mycamera/left/camera_info_demo + mycamera/right/camera_info:=mycamera/right/camera_info_demo + + + mycamera + + + + + false + + + + + +