From 301df571833208a6ecc9ca8df04daef8089228b2 Mon Sep 17 00:00:00 2001 From: chapulina Date: Mon, 10 Dec 2018 20:57:22 -0800 Subject: [PATCH] [ros2] Camera and triggered camera (#827) * move gazebo_ros_camera and some functionality from gazebo_ros_camera_utils, needs master branch of image_transport and message_filters, not functional, but compiling * port PutCameraData, needs common_interfaces PR #58 * move camera worlds, fix compilation, image can be seen on RViz * Port camera test: simplify world, use ServerFixture for better control and not to need launch - test is hanging on exit, not sure why * fix test hanging on exit * port camera16bit test and fix world copying on make * Start porting camera distortion tests: must port cam_info, 2nd test failing * sortout camera_name and frame_name * Port gazebo_ros_camera_triggered as part of gazebo_ros_camera, with test * Use camera_info_manager from branch ci_manager_port_louise, enable barrel distortion test - passes but segfaults at teardown, could be a problem with having 2 plugins side-by-side. * linters and comment out crashing test * Demo worlds, doxygen, more node tests * Use image_transport remapping * adapt to new image_transport pointer API * new API --- .../gazebo_plugins/gazebo_ros_camera.h | 54 -- .../gazebo_plugins/gazebo_ros_camera_utils.h | 87 --- .../gazebo_ros_triggered_camera.h | 77 --- .../gazebo_plugins/src/gazebo_ros_camera.cpp | 117 ---- .../src/gazebo_ros_camera_utils.cpp | 565 ------------------ .../src/gazebo_ros_triggered_camera.cpp | 133 ----- .../gazebo_plugins/test/camera/camera.cpp | 15 - .../gazebo_plugins/test/camera/camera.h | 87 --- .../gazebo_plugins/test/camera/camera.test | 17 - .../gazebo_plugins/test/camera/camera.world | 138 ----- .../test/camera/camera16bit.cpp | 15 - .../test/camera/camera16bit.test | 17 - .../test/camera/camera16bit.world | 138 ----- .../gazebo_plugins/test/camera/distortion.h | 183 ------ .../test/camera/distortion_barrel.cpp | 13 - .../test/camera/distortion_barrel.test | 12 - .../test/camera/triggered_camera.cpp | 114 ---- .../test/camera/triggered_camera.test | 17 - .../test/camera/triggered_camera.world | 140 ----- gazebo_plugins/CMakeLists.txt | 29 +- .../gazebo_plugins/gazebo_ros_camera.hpp | 103 ++++ gazebo_plugins/package.xml | 4 + gazebo_plugins/src/gazebo_ros_camera.cpp | 359 +++++++++++ gazebo_plugins/test/CMakeLists.txt | 13 +- .../test/test_gazebo_ros_camera.cpp | 92 +++ .../test_gazebo_ros_camera_distortion.cpp | 222 +++++++ .../test/test_gazebo_ros_camera_triggered.cpp | 99 +++ .../test/worlds/gazebo_ros_camera.world | 43 ++ .../test/worlds/gazebo_ros_camera_16bit.world | 48 ++ .../gazebo_ros_camera_distortion_barrel.world | 58 +- ...ebo_ros_camera_distortion_pincushion.world | 62 +- .../worlds/gazebo_ros_camera_triggered.world | 49 ++ .../worlds/gazebo_ros_camera_demo.world | 151 +++++ ...bo_ros_camera_distortion_barrel_demo.world | 149 +++++ ...os_camera_distortion_pincushion_demo.world | 149 +++++ .../gazebo_ros_camera_triggered_demo.world | 154 +++++ gazebo_ros/test/test_node.cpp | 36 ++ 37 files changed, 1728 insertions(+), 2031 deletions(-) delete mode 100644 .ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera.h delete mode 100644 .ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_camera.h delete mode 100644 .ros1_unported/gazebo_plugins/src/gazebo_ros_camera.cpp delete mode 100644 .ros1_unported/gazebo_plugins/src/gazebo_ros_triggered_camera.cpp delete mode 100644 .ros1_unported/gazebo_plugins/test/camera/camera.cpp delete mode 100644 .ros1_unported/gazebo_plugins/test/camera/camera.h delete mode 100644 .ros1_unported/gazebo_plugins/test/camera/camera.test delete mode 100644 .ros1_unported/gazebo_plugins/test/camera/camera.world delete mode 100644 .ros1_unported/gazebo_plugins/test/camera/camera16bit.cpp delete mode 100644 .ros1_unported/gazebo_plugins/test/camera/camera16bit.test delete mode 100644 .ros1_unported/gazebo_plugins/test/camera/camera16bit.world delete mode 100644 .ros1_unported/gazebo_plugins/test/camera/distortion.h delete mode 100644 .ros1_unported/gazebo_plugins/test/camera/distortion_barrel.cpp delete mode 100644 .ros1_unported/gazebo_plugins/test/camera/distortion_barrel.test delete mode 100644 .ros1_unported/gazebo_plugins/test/camera/triggered_camera.cpp delete mode 100644 .ros1_unported/gazebo_plugins/test/camera/triggered_camera.test delete mode 100644 .ros1_unported/gazebo_plugins/test/camera/triggered_camera.world create mode 100644 gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera.hpp create mode 100644 gazebo_plugins/src/gazebo_ros_camera.cpp create mode 100644 gazebo_plugins/test/test_gazebo_ros_camera.cpp create mode 100644 gazebo_plugins/test/test_gazebo_ros_camera_distortion.cpp create mode 100644 gazebo_plugins/test/test_gazebo_ros_camera_triggered.cpp create mode 100644 gazebo_plugins/test/worlds/gazebo_ros_camera.world create mode 100644 gazebo_plugins/test/worlds/gazebo_ros_camera_16bit.world rename .ros1_unported/gazebo_plugins/test/camera/distortion_barrel.world => gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_barrel.world (58%) rename .ros1_unported/gazebo_plugins/test/camera/distortion_pincushion.world => gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_pincushion.world (55%) create mode 100644 gazebo_plugins/test/worlds/gazebo_ros_camera_triggered.world create mode 100644 gazebo_plugins/worlds/gazebo_ros_camera_demo.world create mode 100644 gazebo_plugins/worlds/gazebo_ros_camera_distortion_barrel_demo.world create mode 100644 gazebo_plugins/worlds/gazebo_ros_camera_distortion_pincushion_demo.world create mode 100644 gazebo_plugins/worlds/gazebo_ros_camera_triggered_demo.world diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera.h b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera.h deleted file mode 100644 index 33f41b84e..000000000 --- a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera.h +++ /dev/null @@ -1,54 +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. - * -*/ -/* - * Desc: A dynamic controller plugin that publishes ROS image_raw - * camera_info topic for generic camera sensor. -*/ - -#ifndef GAZEBO_ROS_CAMERA_HH -#define GAZEBO_ROS_CAMERA_HH - -#include - -// library for processing camera data for gazebo / ros conversions -#include - -#include - -namespace gazebo -{ - class GazeboRosCamera : public CameraPlugin, GazeboRosCameraUtils - { - /// \brief Constructor - /// \param parent The parent entity, must be a Model or a Sensor - public: GazeboRosCamera(); - - /// \brief Destructor - public: ~GazeboRosCamera(); - - /// \brief Load the plugin - /// \param take in SDF root element - public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); - - /// \brief Update the controller - protected: virtual void OnNewFrame(const unsigned char *_image, - unsigned int _width, unsigned int _height, - unsigned int _depth, const std::string &_format); - }; -} -#endif - diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h index eb8132d9f..a1b75411e 100644 --- a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h +++ b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h @@ -82,13 +82,6 @@ namespace gazebo public: event::ConnectionPtr OnLoad(const boost::function&); - private: void Init(); - - /// \brief Put camera data to the ROS topic - protected: void PutCameraData(const unsigned char *_src); - protected: void PutCameraData(const unsigned char *_src, - common::Time &last_update_time); - /// \brief Keep track of number of image connections protected: boost::shared_ptr image_connect_count_; /// \brief A mutex to lock access to image_connect_count_ @@ -105,27 +98,6 @@ namespace gazebo private: void SetHFOV(const std_msgs::Float64::ConstPtr& hfov); private: void SetUpdateRate(const std_msgs::Float64::ConstPtr& update_rate); - /// \brief A pointer to the ROS node. - /// A node will be instantiated if it does not exist. - protected: ros::NodeHandle* rosnode_; - protected: image_transport::Publisher image_pub_; - private: image_transport::ImageTransport* itnode_; - - /// \brief ROS image message - protected: sensor_msgs::Image image_msg_; - - /// \brief for setting ROS name space - private: std::string robot_namespace_; - - /// \brief ROS camera name - private: std::string camera_name_; - - /// \brief tf prefix - private: std::string tf_prefix_; - - /// \brief ROS image topic name - protected: std::string image_topic_name_; - /// \brief Publish CameraInfo to the ROS topic protected: void PublishCameraInfo(ros::Publisher camera_info_publisher); protected: void PublishCameraInfo(common::Time &last_update_time); @@ -138,39 +110,13 @@ namespace gazebo protected: std::string camera_info_topic_name_; protected: common::Time last_info_update_time_; - /// \brief ROS frame transform name to use in the image message header. - /// This should typically match the link name the sensor is attached. - protected: std::string frame_name_; /// update rate of this sensor - protected: double update_rate_; - protected: double update_period_; - protected: common::Time last_update_time_; - - protected: double cx_prime_; - protected: double cx_; - protected: double cy_; - protected: double focal_length_; - protected: double hack_baseline_; - protected: double distortion_k1_; - protected: double distortion_k2_; - protected: double distortion_k3_; - protected: double distortion_t1_; - protected: double distortion_t2_; - - protected: bool auto_distortion_; - protected: bool border_crop_; - protected: boost::shared_ptr camera_info_manager_; - /// \brief A mutex to lock access to fields /// that are used in ROS message callbacks protected: boost::mutex lock_; - /// \brief size of image buffer - protected: std::string type_; - protected: int skip_; - private: ros::Subscriber cameraHFOVSubscriber_; private: ros::Subscriber cameraUpdateRateSubscriber_; @@ -186,44 +132,11 @@ namespace gazebo protected: void CameraQueueThread(); protected: boost::thread callback_queue_thread_; - - // copied from CameraPlugin - protected: unsigned int width_, height_, depth_; - protected: std::string format_; - - protected: sensors::SensorPtr parentSensor_; - protected: rendering::CameraPtr camera_; - - // Pointer to the world - protected: physics::WorldPtr world_; - - private: event::ConnectionPtr newFrameConnection_; - - protected: common::Time sensor_update_time_; - - // maintain for one more release for backwards compatibility - protected: physics::WorldPtr world; - // deferred load in case ros is blocking - private: sdf::ElementPtr sdf; private: void LoadThread(); private: boost::thread deferred_load_thread_; private: event::EventT load_event_; - // make a trigger function that the child classes can override - // and a function that returns bool to indicate whether the trigger - // should be used - protected: virtual void TriggerCamera(); - protected: virtual bool CanTriggerCamera(); - private: void TriggerCameraInternal(const std_msgs::Empty::ConstPtr &dummy); - private: ros::Subscriber trigger_subscriber_; - - /// \brief ROS trigger topic name - protected: std::string trigger_topic_name_; - - /// \brief True if camera util is initialized - protected: bool initialized_; - friend class GazeboRosMultiCamera; friend class GazeboRosTriggeredMultiCamera; }; diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_camera.h b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_camera.h deleted file mode 100644 index b99debba8..000000000 --- a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_camera.h +++ /dev/null @@ -1,77 +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_CAMERA_HH -#define GAZEBO_ROS_TRIGGERED_CAMERA_HH - -#include -#include - -// library for processing camera data for gazebo / ros conversions -#include - -#include - -namespace gazebo -{ - class GazeboRosTriggeredMultiCamera; - class GazeboRosTriggeredCamera : public CameraPlugin, GazeboRosCameraUtils - { - /// \brief Constructor - /// \param parent The parent entity, must be a Model or a Sensor - public: GazeboRosTriggeredCamera(); - - /// \brief Destructor - public: ~GazeboRosTriggeredCamera(); - - /// \brief Load the plugin - /// \param take in SDF root element - public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); - - /// \brief Load the plugin. - /// \param[in] _parent Take in SDF root element. - /// \param[in] _sdf SDF values. - /// \param[in] _camera_name_suffix Suffix of the camera name. - /// \param[in] _hack_baseline Multiple camera baseline. - public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, - const std::string &_camera_name_suffix, - double _hack_baseline); - - /// \brief Update the controller - protected: virtual void OnNewFrame(const unsigned char *_image, - unsigned int _width, unsigned int _height, - unsigned int _depth, const std::string &_format); - - protected: virtual void TriggerCamera(); - - protected: virtual bool CanTriggerCamera(); - - protected: event::ConnectionPtr preRenderConnection_; - - public: void SetCameraEnabled(const bool _enabled); - - protected: void PreRender(); - - protected: int triggered = 0; - - protected: std::mutex mutex; - - friend class GazeboRosTriggeredMultiCamera; - }; -} -#endif - diff --git a/.ros1_unported/gazebo_plugins/src/gazebo_ros_camera.cpp b/.ros1_unported/gazebo_plugins/src/gazebo_ros_camera.cpp deleted file mode 100644 index f5f692b7b..000000000 --- a/.ros1_unported/gazebo_plugins/src/gazebo_ros_camera.cpp +++ /dev/null @@ -1,117 +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. - * -*/ - -/* - @mainpage - Desc: GazeboRosCamera plugin for simulating cameras in Gazebo - Author: John Hsu - Date: 24 Sept 2008 -*/ - -#include "gazebo_plugins/gazebo_ros_camera.h" - -#include - -#include -#include -#include - -namespace gazebo -{ -// Register this plugin with the simulator -GZ_REGISTER_SENSOR_PLUGIN(GazeboRosCamera) - -//////////////////////////////////////////////////////////////////////////////// -// Constructor -GazeboRosCamera::GazeboRosCamera() -{ -} - -//////////////////////////////////////////////////////////////////////////////// -// Destructor -GazeboRosCamera::~GazeboRosCamera() -{ - ROS_DEBUG_STREAM_NAMED("camera","Unloaded"); -} - -void GazeboRosCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) -{ - // Make sure the ROS node for Gazebo has already been initialized - if (!ros::isInitialized()) - { - ROS_FATAL_STREAM_NAMED("camera", "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; - } - - CameraPlugin::Load(_parent, _sdf); - // copying from CameraPlugin into GazeboRosCameraUtils - this->parentSensor_ = this->parentSensor; - this->width_ = this->width; - this->height_ = this->height; - this->depth_ = this->depth; - this->format_ = this->format; - this->camera_ = this->camera; - - GazeboRosCameraUtils::Load(_parent, _sdf); -} - -//////////////////////////////////////////////////////////////////////////////// -// Update the controller -void GazeboRosCamera::OnNewFrame(const unsigned char *_image, - unsigned int _width, unsigned int _height, unsigned int _depth, - const std::string &_format) -{ -# if GAZEBO_MAJOR_VERSION >= 7 - common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime(); -# else - common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime(); -# endif - - if (!this->parentSensor->IsActive()) - { - if ((*this->image_connect_count_) > 0) - // do this first so there's chance for sensor to run once after activated - this->parentSensor->SetActive(true); - } - else - { - if ((*this->image_connect_count_) > 0) - { - if (sensor_update_time < this->last_update_time_) - { - ROS_WARN_NAMED("camera", "Negative sensor update time difference detected."); - this->last_update_time_ = sensor_update_time; - } - - // OnNewFrame is triggered at the gazebo sensor - // while there is also a plugin that can throttle the - // rate down further (but then why not reduce the sensor rate? - // what is the use case?). - // Setting the to zero will make this plugin - // update at the gazebo sensor , update_period_ will be - // zero and the conditional always will be true. - if (sensor_update_time - this->last_update_time_ >= this->update_period_) - { - this->PutCameraData(_image, sensor_update_time); - this->PublishCameraInfo(sensor_update_time); - this->last_update_time_ = sensor_update_time; - } - } - } -} -} diff --git a/.ros1_unported/gazebo_plugins/src/gazebo_ros_camera_utils.cpp b/.ros1_unported/gazebo_plugins/src/gazebo_ros_camera_utils.cpp index 325c390f9..1108690c6 100644 --- a/.ros1_unported/gazebo_plugins/src/gazebo_ros_camera_utils.cpp +++ b/.ros1_unported/gazebo_plugins/src/gazebo_ros_camera_utils.cpp @@ -49,13 +49,6 @@ namespace gazebo // Constructor GazeboRosCameraUtils::GazeboRosCameraUtils() { - this->last_update_time_ = common::Time(0); - this->last_info_update_time_ = common::Time(0); - this->height_ = 0; - this->width_ = 0; - this->skip_ = 0; - this->format_ = ""; - this->initialized_ = false; } void GazeboRosCameraUtils::configCallback( @@ -73,194 +66,6 @@ void GazeboRosCameraUtils::configCallback( // Destructor GazeboRosCameraUtils::~GazeboRosCameraUtils() { - this->parentSensor_->SetActive(false); - this->rosnode_->shutdown(); - this->camera_queue_.clear(); - this->camera_queue_.disable(); - this->callback_queue_thread_.join(); - delete this->rosnode_; -} - -//////////////////////////////////////////////////////////////////////////////// -// Load the controller -void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent, - sdf::ElementPtr _sdf, - const std::string &_camera_name_suffix, - double _hack_baseline) -{ - // default Load: - // provide _camera_name_suffix to prevent LoadThread() creating the ros::NodeHandle with - //an incomplete this->camera_name_ namespace. There was a race condition when the _camera_name_suffix - //was appended in this function. - this->Load(_parent, _sdf, _camera_name_suffix); - - // overwrite hack baseline if specified at load - // example usage in gazebo_ros_multicamera - this->hack_baseline_ = _hack_baseline; -} - -//////////////////////////////////////////////////////////////////////////////// -// Load the controller -void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent, - sdf::ElementPtr _sdf, - const std::string &_camera_name_suffix) -{ - // Get the world name. - std::string world_name = _parent->WorldName(); - - // Get the world_ - this->world_ = physics::get_world(world_name); - - // save pointers - this->sdf = _sdf; - - // maintain for one more release for backwards compatibility with - // pr2_gazebo_plugins - this->world = this->world_; - - std::stringstream ss; - this->robot_namespace_ = GetRobotNamespace(_parent, _sdf, "Camera"); - - this->image_topic_name_ = "image_raw"; - if (this->sdf->HasElement("imageTopicName")) - this->image_topic_name_ = this->sdf->Get("imageTopicName"); - - this->trigger_topic_name_ = "image_trigger"; - if (this->sdf->HasElement("triggerTopicName")) - this->trigger_topic_name_ = this->sdf->Get("triggerTopicName"); - - this->camera_info_topic_name_ = "camera_info"; - if (this->sdf->HasElement("cameraInfoTopicName")) - this->camera_info_topic_name_ = - this->sdf->Get("cameraInfoTopicName"); - - if (!this->sdf->HasElement("cameraName")) - ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , default to empty"); - else - this->camera_name_ = this->sdf->Get("cameraName"); - - // overwrite camera suffix - // example usage in gazebo_ros_multicamera - this->camera_name_ += _camera_name_suffix; - - if (!this->sdf->HasElement("frameName")) - ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to /world"); - else - this->frame_name_ = this->sdf->Get("frameName"); - - if (!this->sdf->HasElement("updateRate")) - { - ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to unlimited (0)."); - this->update_rate_ = 0; - } - else - this->update_rate_ = this->sdf->Get("updateRate"); - - if (!this->sdf->HasElement("CxPrime")) - { - ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); - this->cx_prime_ = 0; - } - else - this->cx_prime_ = this->sdf->Get("CxPrime"); - - if (!this->sdf->HasElement("Cx")) - { - ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); - this->cx_= 0; - } - else - this->cx_ = this->sdf->Get("Cx"); - - if (!this->sdf->HasElement("Cy")) - { - ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); - this->cy_= 0; - } - else - this->cy_ = this->sdf->Get("Cy"); - - if (!this->sdf->HasElement("focalLength")) - { - ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); - this->focal_length_= 0; - } - else - this->focal_length_ = this->sdf->Get("focalLength"); - - if (!this->sdf->HasElement("hackBaseline")) - { - ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); - this->hack_baseline_= 0; - } - else - this->hack_baseline_ = this->sdf->Get("hackBaseline"); - - if (!this->sdf->HasElement("distortionK1")) - { - ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); - this->distortion_k1_= 0; - } - else - this->distortion_k1_ = this->sdf->Get("distortionK1"); - - if (!this->sdf->HasElement("distortionK2")) - { - ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); - this->distortion_k2_= 0; - } - else - this->distortion_k2_ = this->sdf->Get("distortionK2"); - - if (!this->sdf->HasElement("distortionK3")) - { - ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); - this->distortion_k3_= 0; - } - else - this->distortion_k3_ = this->sdf->Get("distortionK3"); - - if (!this->sdf->HasElement("distortionT1")) - { - ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); - this->distortion_t1_= 0; - } - else - this->distortion_t1_ = this->sdf->Get("distortionT1"); - - if (!this->sdf->HasElement("distortionT2")) - { - ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); - this->distortion_t2_= 0; - } - else - this->distortion_t2_ = this->sdf->Get("distortionT2"); - - // TODO: make default behavior auto_distortion_ = true - if (!this->sdf->HasElement("autoDistortion")) - { - ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to false"); - this->auto_distortion_ = false; - } - else - this->auto_distortion_ = this->sdf->Get("autoDistortion"); - - if (!this->sdf->HasElement("borderCrop")) - { - ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to true"); - this->border_crop_ = true; - } - else - this->border_crop_ = this->sdf->Get("borderCrop"); - - // initialize shared_ptr members - if (!this->image_connect_count_) this->image_connect_count_ = boost::shared_ptr(new int(0)); - if (!this->image_connect_count_lock_) this->image_connect_count_lock_ = boost::shared_ptr(new boost::mutex); - if (!this->was_active_) this->was_active_ = boost::shared_ptr(new bool(false)); - - // ros callback queue for processing subscription - this->deferred_load_thread_ = boost::thread( - boost::bind(&GazeboRosCameraUtils::LoadThread, this)); } event::ConnectionPtr GazeboRosCameraUtils::OnLoad(const boost::function& load_function) @@ -268,125 +73,6 @@ event::ConnectionPtr GazeboRosCameraUtils::OnLoad(const boost::function& return load_event_.Connect(load_function); } -//////////////////////////////////////////////////////////////////////////////// -// Load the controller -void GazeboRosCameraUtils::LoadThread() -{ - // Exit if no ROS - if (!ros::isInitialized()) - { - gzerr << "Not loading plugin since ROS hasn't been " - << "properly initialized. Try starting gazebo with ros plugin:\n" - << " gazebo -s libgazebo_ros_api_plugin.so\n"; - return; - } - - // Sensor generation off by default. Must do this before advertising the - // associated ROS topics. - this->parentSensor_->SetActive(false); - - this->rosnode_ = new ros::NodeHandle(this->robot_namespace_ + "/" + this->camera_name_); - - // initialize camera_info_manager - this->camera_info_manager_.reset(new camera_info_manager::CameraInfoManager( - *this->rosnode_, this->camera_name_)); - - this->itnode_ = new image_transport::ImageTransport(*this->rosnode_); - - // resolve tf prefix - this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_); - if(this->tf_prefix_.empty()) { - this->tf_prefix_ = this->robot_namespace_; - boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/")); - } - this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_); - - ROS_INFO_NAMED("camera_utils", "Camera Plugin (ns = %s) , set to \"%s\"", - this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); - - - if (!this->camera_name_.empty()) - { - dyn_srv_ = - new dynamic_reconfigure::Server - (*this->rosnode_); - dynamic_reconfigure::Server - ::CallbackType f = - boost::bind(&GazeboRosCameraUtils::configCallback, this, _1, _2); - dyn_srv_->setCallback(f); - } - else - { - ROS_WARN_NAMED("camera_utils", "dynamic reconfigure is not enabled for this image topic [%s]" - " becuase is not specified", - this->image_topic_name_.c_str()); - } - - this->image_pub_ = this->itnode_->advertise( - this->image_topic_name_, 2, - boost::bind(&GazeboRosCameraUtils::ImageConnect, this), - boost::bind(&GazeboRosCameraUtils::ImageDisconnect, this), - ros::VoidPtr(), true); - - // camera info publish rate will be synchronized to image sensor - // publish rates. - // If someone connects to camera_info, sensor will be activated - // and camera_info will be published alongside image_raw with the - // same timestamps. This incurrs additional computational cost when - // there are subscribers to camera_info, but better mimics behavior - // of image_pipeline. - ros::AdvertiseOptions cio = - ros::AdvertiseOptions::create( - this->camera_info_topic_name_, 2, - boost::bind(&GazeboRosCameraUtils::ImageConnect, this), - boost::bind(&GazeboRosCameraUtils::ImageDisconnect, this), - ros::VoidPtr(), &this->camera_queue_); - this->camera_info_pub_ = this->rosnode_->advertise(cio); - - /* disabling fov and rate setting for each camera - ros::SubscribeOptions zoom_so = - ros::SubscribeOptions::create( - "set_hfov", 1, - boost::bind(&GazeboRosCameraUtils::SetHFOV, this, _1), - ros::VoidPtr(), &this->camera_queue_); - this->cameraHFOVSubscriber_ = this->rosnode_->subscribe(zoom_so); - - ros::SubscribeOptions rate_so = - ros::SubscribeOptions::create( - "set_update_rate", 1, - boost::bind(&GazeboRosCameraUtils::SetUpdateRate, this, _1), - ros::VoidPtr(), &this->camera_queue_); - this->cameraUpdateRateSubscriber_ = this->rosnode_->subscribe(rate_so); - */ - - if (this->CanTriggerCamera()) - { - ros::SubscribeOptions trigger_so = - ros::SubscribeOptions::create( - this->trigger_topic_name_, 1, - boost::bind(&GazeboRosCameraUtils::TriggerCameraInternal, this, _1), - ros::VoidPtr(), &this->camera_queue_); - this->trigger_subscriber_ = this->rosnode_->subscribe(trigger_so); - } - - this->Init(); -} - -void GazeboRosCameraUtils::TriggerCamera() -{ -} - -bool GazeboRosCameraUtils::CanTriggerCamera() -{ - return false; -} - -void GazeboRosCameraUtils::TriggerCameraInternal( - const std_msgs::Empty::ConstPtr &dummy) -{ - TriggerCamera(); -} - //////////////////////////////////////////////////////////////////////////////// // Set Horizontal Field of View void GazeboRosCameraUtils::SetHFOV(const std_msgs::Float64::ConstPtr& hfov) @@ -398,14 +84,6 @@ void GazeboRosCameraUtils::SetHFOV(const std_msgs::Float64::ConstPtr& hfov) #endif } -//////////////////////////////////////////////////////////////////////////////// -// Set Update Rate -void GazeboRosCameraUtils::SetUpdateRate( - const std_msgs::Float64::ConstPtr& update_rate) -{ - this->parentSensor_->SetUpdateRate(update_rate->data); -} - //////////////////////////////////////////////////////////////////////////////// // Increment count void GazeboRosCameraUtils::ImageConnect() @@ -435,235 +113,6 @@ void GazeboRosCameraUtils::ImageDisconnect() this->parentSensor_->SetActive(false); } -//////////////////////////////////////////////////////////////////////////////// -// Initialize the controller -void GazeboRosCameraUtils::Init() -{ - // prepare to throttle this plugin at the same rate - // ideally, we should invoke a plugin update when the sensor updates, - // have to think about how to do that properly later - if (this->update_rate_ > 0.0) - this->update_period_ = 1.0/this->update_rate_; - else - this->update_period_ = 0.0; - - // set buffer size - if (this->format_ == "L8" || this->format_ == "L_INT8") - { - this->type_ = sensor_msgs::image_encodings::MONO8; - this->skip_ = 1; - } - else if (this->format_ == "L16" || this->format_ == "L_INT16") - { - this->type_ = sensor_msgs::image_encodings::MONO16; - this->skip_ = 2; - } - else if (this->format_ == "R8G8B8" || this->format_ == "RGB_INT8") - { - this->type_ = sensor_msgs::image_encodings::RGB8; - this->skip_ = 3; - } - else if (this->format_ == "B8G8R8" || this->format_ == "BGR_INT8") - { - this->type_ = sensor_msgs::image_encodings::BGR8; - this->skip_ = 3; - } - else if (this->format_ == "R16G16B16" || this->format_ == "RGB_INT16") - { - this->type_ = sensor_msgs::image_encodings::RGB16; - this->skip_ = 6; - } - else if (this->format_ == "BAYER_RGGB8") - { - ROS_INFO_NAMED("camera_utils", "bayer simulation maybe computationally expensive."); - this->type_ = sensor_msgs::image_encodings::BAYER_RGGB8; - this->skip_ = 1; - } - else if (this->format_ == "BAYER_BGGR8") - { - ROS_INFO_NAMED("camera_utils", "bayer simulation maybe computationally expensive."); - this->type_ = sensor_msgs::image_encodings::BAYER_BGGR8; - this->skip_ = 1; - } - else if (this->format_ == "BAYER_GBRG8") - { - ROS_INFO_NAMED("camera_utils", "bayer simulation maybe computationally expensive."); - this->type_ = sensor_msgs::image_encodings::BAYER_GBRG8; - this->skip_ = 1; - } - else if (this->format_ == "BAYER_GRBG8") - { - ROS_INFO_NAMED("camera_utils", "bayer simulation maybe computationally expensive."); - this->type_ = sensor_msgs::image_encodings::BAYER_GRBG8; - this->skip_ = 1; - } - else - { - ROS_ERROR_NAMED("camera_utils", "Unsupported Gazebo ImageFormat\n"); - this->type_ = sensor_msgs::image_encodings::BGR8; - this->skip_ = 3; - } - - /// Compute camera_ parameters if set to 0 - if (this->cx_prime_ == 0) - this->cx_prime_ = (static_cast(this->width_) + 1.0) /2.0; - if (this->cx_ == 0) - this->cx_ = (static_cast(this->width_) + 1.0) /2.0; - if (this->cy_ == 0) - this->cy_ = (static_cast(this->height_) + 1.0) /2.0; - - - double hfov = this->camera_->HFOV().Radian(); - double computed_focal_length = - (static_cast(this->width_)) / - (2.0 * tan(hfov / 2.0)); - - if (this->focal_length_ == 0) - { - this->focal_length_ = computed_focal_length; - } - else - { - // check against float precision - if (!ignition::math::equal(this->focal_length_, computed_focal_length)) - { - ROS_WARN_NAMED("camera_utils", "The [%f] you have provided for camera_ [%s]" - " is inconsistent with specified image_width [%d] and" - " HFOV [%f]. Please double check to see that" - " focal_length = width_ / (2.0 * tan(HFOV/2.0))," - " the explected focal_lengtth value is [%f]," - " please update your camera_ model description accordingly.", - this->focal_length_, this->parentSensor_->Name().c_str(), - this->width_, hfov, - computed_focal_length); - } - } - - // fill CameraInfo - sensor_msgs::CameraInfo camera_info_msg; - - camera_info_msg.header.frame_id = this->frame_name_; - - camera_info_msg.height = this->height_; - camera_info_msg.width = this->width_; - // distortion -#if ROS_VERSION_MINIMUM(1, 3, 0) - camera_info_msg.distortion_model = "plumb_bob"; - camera_info_msg.D.resize(5); -#endif - // 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. - if(this->camera_->LensDistortion()) - { - this->camera_->LensDistortion()->SetCrop(this->border_crop_); - } - - // Get distortion parameters from gazebo sensor if auto_distortion is true - if(this->auto_distortion_) - { -#if GAZEBO_MAJOR_VERSION >= 8 - this->distortion_k1_ = this->camera_->LensDistortion()->K1(); - this->distortion_k2_ = this->camera_->LensDistortion()->K2(); - this->distortion_k3_ = this->camera_->LensDistortion()->K3(); - this->distortion_t1_ = this->camera_->LensDistortion()->P1(); - this->distortion_t2_ = this->camera_->LensDistortion()->P2(); -#else - // TODO: remove version gaurd once gazebo7 is not supported - this->distortion_k1_ = this->camera_->LensDistortion()->GetK1(); - this->distortion_k2_ = this->camera_->LensDistortion()->GetK2(); - this->distortion_k3_ = this->camera_->LensDistortion()->GetK3(); - this->distortion_t1_ = this->camera_->LensDistortion()->GetP1(); - this->distortion_t2_ = this->camera_->LensDistortion()->GetP2(); -#endif - } - - // 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] = this->distortion_k1_; - camera_info_msg.D[1] = this->distortion_k2_; - camera_info_msg.D[2] = this->distortion_t1_; - camera_info_msg.D[3] = this->distortion_t2_; - camera_info_msg.D[4] = this->distortion_k3_; - // original camera_ matrix - camera_info_msg.K[0] = this->focal_length_; - camera_info_msg.K[1] = 0.0; - camera_info_msg.K[2] = this->cx_; - camera_info_msg.K[3] = 0.0; - camera_info_msg.K[4] = this->focal_length_; - camera_info_msg.K[5] = this->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] = this->focal_length_; - camera_info_msg.P[1] = 0.0; - camera_info_msg.P[2] = this->cx_; - camera_info_msg.P[3] = -this->focal_length_ * this->hack_baseline_; - camera_info_msg.P[4] = 0.0; - camera_info_msg.P[5] = this->focal_length_; - camera_info_msg.P[6] = this->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; - - this->camera_info_manager_->setCameraInfo(camera_info_msg); - - // start custom queue for camera_ - this->callback_queue_thread_ = boost::thread( - boost::bind(&GazeboRosCameraUtils::CameraQueueThread, this)); - - load_event_(); - this->initialized_ = true; -} - -//////////////////////////////////////////////////////////////////////////////// -// Put camera_ data to the interface -void GazeboRosCameraUtils::PutCameraData(const unsigned char *_src, - common::Time &last_update_time) -{ - this->sensor_update_time_ = last_update_time; - this->PutCameraData(_src); -} - -void GazeboRosCameraUtils::PutCameraData(const unsigned char *_src) -{ - if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) - return; - - /// don't bother if there are no subscribers - if ((*this->image_connect_count_) > 0) - { - boost::mutex::scoped_lock lock(this->lock_); - - // copy data into image - this->image_msg_.header.frame_id = this->frame_name_; - this->image_msg_.header.stamp.sec = this->sensor_update_time_.sec; - this->image_msg_.header.stamp.nsec = this->sensor_update_time_.nsec; - - // copy from src to image_msg_ - fillImage(this->image_msg_, this->type_, this->height_, this->width_, - this->skip_*this->width_, reinterpret_cast(_src)); - - // publish to ros - this->image_pub_.publish(this->image_msg_); - } -} - //////////////////////////////////////////////////////////////////////////////// // Put camera_ data to the interface void GazeboRosCameraUtils::PublishCameraInfo(common::Time &last_update_time) @@ -701,18 +150,4 @@ void GazeboRosCameraUtils::PublishCameraInfo( camera_info_publisher.publish(camera_info_msg); } - - -//////////////////////////////////////////////////////////////////////////////// -// Put camera_ data to the interface -void GazeboRosCameraUtils::CameraQueueThread() -{ - static const double timeout = 0.001; - - while (this->rosnode_->ok()) - { - /// take care of callback queue - this->camera_queue_.callAvailable(ros::WallDuration(timeout)); - } -} } diff --git a/.ros1_unported/gazebo_plugins/src/gazebo_ros_triggered_camera.cpp b/.ros1_unported/gazebo_plugins/src/gazebo_ros_triggered_camera.cpp deleted file mode 100644 index 7ea48d073..000000000 --- a/.ros1_unported/gazebo_plugins/src/gazebo_ros_triggered_camera.cpp +++ /dev/null @@ -1,133 +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 "gazebo_plugins/gazebo_ros_triggered_camera.h" - -#include -#include - -#include -#include -#include - -namespace gazebo -{ - -// Register this plugin with the simulator -GZ_REGISTER_SENSOR_PLUGIN(GazeboRosTriggeredCamera) - -//////////////////////////////////////////////////////////////////////////////// -// Constructor -GazeboRosTriggeredCamera::GazeboRosTriggeredCamera() -{ -} - -//////////////////////////////////////////////////////////////////////////////// -// Destructor -GazeboRosTriggeredCamera::~GazeboRosTriggeredCamera() -{ - ROS_DEBUG_STREAM_NAMED("camera","Unloaded"); -} - -void GazeboRosTriggeredCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _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; - } - - CameraPlugin::Load(_parent, _sdf); - // copying from CameraPlugin into GazeboRosTriggeredCameraUtils - this->parentSensor_ = this->parentSensor; - this->width_ = this->width; - this->height_ = this->height; - this->depth_ = this->depth; - this->format_ = this->format; - this->camera_ = this->camera; - - GazeboRosCameraUtils::Load(_parent, _sdf); - - this->SetCameraEnabled(false); - this->preRenderConnection_ = - event::Events::ConnectPreRender( - std::bind(&GazeboRosTriggeredCamera::PreRender, this)); -} - -void GazeboRosTriggeredCamera::Load(sensors::SensorPtr _parent, - sdf::ElementPtr _sdf, - const std::string &_camera_name_suffix, - double _hack_baseline) -{ - GazeboRosCameraUtils::Load(_parent, _sdf, _camera_name_suffix, _hack_baseline); - - this->SetCameraEnabled(false); - this->preRenderConnection_ = - event::Events::ConnectPreRender( - std::bind(&GazeboRosTriggeredCamera::PreRender, this)); -} - -//////////////////////////////////////////////////////////////////////////////// -// Update the controller -void GazeboRosTriggeredCamera::OnNewFrame(const unsigned char *_image, - unsigned int _width, unsigned int _height, unsigned int _depth, - const std::string &_format) -{ - this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime(); - - if ((*this->image_connect_count_) > 0) - { - this->PutCameraData(_image); - this->PublishCameraInfo(); - } - this->SetCameraEnabled(false); - - std::lock_guard lock(this->mutex); - this->triggered = std::max(this->triggered-1, 0); -} - -void GazeboRosTriggeredCamera::TriggerCamera() -{ - std::lock_guard lock(this->mutex); - if (!this->parentSensor_) - return; - this->triggered++; -} - -bool GazeboRosTriggeredCamera::CanTriggerCamera() -{ - return true; -} - -void GazeboRosTriggeredCamera::PreRender() -{ - std::lock_guard lock(this->mutex); - if (this->triggered > 0) - { - this->SetCameraEnabled(true); - } -} - -void GazeboRosTriggeredCamera::SetCameraEnabled(const bool _enabled) -{ - this->parentSensor_->SetActive(_enabled); - this->parentSensor_->SetUpdateRate(_enabled ? 0.0 : DBL_MIN); -} - -} diff --git a/.ros1_unported/gazebo_plugins/test/camera/camera.cpp b/.ros1_unported/gazebo_plugins/test/camera/camera.cpp deleted file mode 100644 index 92d6078d4..000000000 --- a/.ros1_unported/gazebo_plugins/test/camera/camera.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#include "camera.h" - -// Test if the camera image is published at all, and that the timestamp -// is not too long in the past. -TEST_F(CameraTest, cameraSubscribeTest) -{ - subscribeTest(); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "gazebo_camera_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/.ros1_unported/gazebo_plugins/test/camera/camera.h b/.ros1_unported/gazebo_plugins/test/camera/camera.h deleted file mode 100644 index a2f997bfc..000000000 --- a/.ros1_unported/gazebo_plugins/test/camera/camera.h +++ /dev/null @@ -1,87 +0,0 @@ -#ifndef GAZEBO_PLUGINS_TEST_CAMERA_CAMERA_H -#define GAZEBO_PLUGINS_TEST_CAMERA_CAMERA_H - -#include -#include -#include - -class CameraTest : public testing::Test -{ -protected: - virtual void SetUp() - { - has_new_image_ = false; - } - - ros::NodeHandle nh_; - image_transport::Subscriber cam_sub_; - bool has_new_image_; - ros::Time image_stamp_; -public: - void subscribeTest(); - - void imageCallback(const sensor_msgs::ImageConstPtr& msg) - { - image_stamp_ = msg->header.stamp; - has_new_image_ = true; - } -}; - -// Test if the camera image is published at all, and that the timestamp -// is not too long in the past. -void CameraTest::subscribeTest() -{ - image_transport::ImageTransport it(nh_); - cam_sub_ = it.subscribe("camera1/image_raw", 1, - &CameraTest::imageCallback, - dynamic_cast(this)); - -#if 0 - // wait for gazebo to start publishing - // TODO(lucasw) this isn't really necessary since this test - // is purely passive - bool wait_for_topic = true; - while (wait_for_topic) - { - // @todo this fails without the additional 0.5 second sleep after the - // publisher comes online, which means on a slower or more heavily - // loaded system it may take longer than 0.5 seconds, and the test - // would hang until the timeout is reached and fail. - if (cam_sub_.getNumPublishers() > 0) - wait_for_topic = false; - ros::Duration(0.5).sleep(); - } -#endif - - while (!has_new_image_) - { - ros::spinOnce(); - ros::Duration(0.1).sleep(); - } - - // This check depends on the update period being much longer - // than the expected difference between now and the received image time - // TODO(lucasw) - // this likely isn't that robust - what if the testing system is really slow? - double time_diff = (ros::Time::now() - image_stamp_).toSec(); - ROS_INFO_STREAM(time_diff); - EXPECT_LT(time_diff, 1.0); - cam_sub_.shutdown(); - - // make sure nothing is subscribing to image_trigger topic - // there is no easy API, so call getSystemState - XmlRpc::XmlRpcValue args, result, payload; - args[0] = ros::this_node::getName(); - EXPECT_TRUE(ros::master::execute("getSystemState", args, result, payload, true)); - // [publishers, subscribers, services] - // subscribers in index 1 of payload - for (int i = 0; i < payload[1].size(); ++i) - { - // [ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ] - // topic name i is in index 0 - std::string topic = payload[1][i][0]; - EXPECT_EQ(topic.find("image_trigger"), std::string::npos); - } -} - -#endif diff --git a/.ros1_unported/gazebo_plugins/test/camera/camera.test b/.ros1_unported/gazebo_plugins/test/camera/camera.test deleted file mode 100644 index 4de8fd0bb..000000000 --- a/.ros1_unported/gazebo_plugins/test/camera/camera.test +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - diff --git a/.ros1_unported/gazebo_plugins/test/camera/camera.world b/.ros1_unported/gazebo_plugins/test/camera/camera.world deleted file mode 100644 index 895c60f39..000000000 --- a/.ros1_unported/gazebo_plugins/test/camera/camera.world +++ /dev/null @@ -1,138 +0,0 @@ - - - - - - model://ground_plane - - - - - model://sun - - - - - - 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 - orbit - - - - - false - 2.0 0.0 4.0 0.0 0.0 0.0 - - 0.0 0.0 0.0 0.0 0.0 0.0 - - 0.0 0.0 0.0 0.0 0.0 0.0 - - 1.0 - 0.0 - 0.0 - 1.0 - 0.0 - 1.0 - - 10.0 - - - 0.0 0.0 0.0 0.0 0.0 0.0 - - - 0.5 - - - - 0.03 0.5 0.5 1.0 - - - true - 100.0 - - - 0.0 0.0 0.0 0.0 0.0 0.0 - 250 - - - 0.5 - - - - - - 0.5 - 0.2 - 1.0 0 0 - 0 - 0 - - - - 0 - 1000000.0 - - - - 0 - 0.2 - 1e15 - 1e13 - 100.0 - 0.0001 - - - - 100.0 - - - - - - true - 0.0 0.0 0.5 0.0 0.0 0.0 - - 0.0 0.0 0.0 0.0 0.0 0.0 - - 0.5 - - 1.3962634 - - 640 - 480 - R8G8B8 - - - 0.02 - 300 - - - gaussian - - 0.0 - 0.007 - - - - true - - 0.0 - camera1 - image_raw - camera_info - camera_link - 0.07 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - diff --git a/.ros1_unported/gazebo_plugins/test/camera/camera16bit.cpp b/.ros1_unported/gazebo_plugins/test/camera/camera16bit.cpp deleted file mode 100644 index bea1b007b..000000000 --- a/.ros1_unported/gazebo_plugins/test/camera/camera16bit.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#include "camera.h" - -// Test if the camera image is published at all, and that the timestamp -// is not too long in the past. -TEST_F(CameraTest, camera16bitSubscribeTest) -{ - subscribeTest(); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "gazebo_camera_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/.ros1_unported/gazebo_plugins/test/camera/camera16bit.test b/.ros1_unported/gazebo_plugins/test/camera/camera16bit.test deleted file mode 100644 index ba9c0af71..000000000 --- a/.ros1_unported/gazebo_plugins/test/camera/camera16bit.test +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - diff --git a/.ros1_unported/gazebo_plugins/test/camera/camera16bit.world b/.ros1_unported/gazebo_plugins/test/camera/camera16bit.world deleted file mode 100644 index a6c455a24..000000000 --- a/.ros1_unported/gazebo_plugins/test/camera/camera16bit.world +++ /dev/null @@ -1,138 +0,0 @@ - - - - - - model://ground_plane - - - - - model://sun - - - - - - 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 - orbit - - - - - false - 2.0 0.0 4.0 0.0 0.0 0.0 - - 0.0 0.0 0.0 0.0 0.0 0.0 - - 0.0 0.0 0.0 0.0 0.0 0.0 - - 1.0 - 0.0 - 0.0 - 1.0 - 0.0 - 1.0 - - 10.0 - - - 0.0 0.0 0.0 0.0 0.0 0.0 - - - 0.5 - - - - 0.03 0.5 0.5 1.0 - - - true - 100.0 - - - 0.0 0.0 0.0 0.0 0.0 0.0 - 250 - - - 0.5 - - - - - - 0.5 - 0.2 - 1.0 0 0 - 0 - 0 - - - - 0 - 1000000.0 - - - - 0 - 0.2 - 1e15 - 1e13 - 100.0 - 0.0001 - - - - 100.0 - - - - - - true - 0.0 0.0 0.5 0.0 0.0 0.0 - - 0.0 0.0 0.0 0.0 0.0 0.0 - - 0.5 - - 1.3962634 - - 640 - 480 - R16G16B16 - - - 0.02 - 300 - - - gaussian - - 0.0 - 0.007 - - - - true - - 0.0 - camera1 - image_raw - camera_info - camera_link - 0.07 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - diff --git a/.ros1_unported/gazebo_plugins/test/camera/distortion.h b/.ros1_unported/gazebo_plugins/test/camera/distortion.h deleted file mode 100644 index f08ad79ab..000000000 --- a/.ros1_unported/gazebo_plugins/test/camera/distortion.h +++ /dev/null @@ -1,183 +0,0 @@ -#ifndef GAZEBO_PLUGINS_TEST_CAMERA_DISTORTION_H -#define GAZEBO_PLUGINS_TEST_CAMERA_DISTORTION_H - -// OpenCV includes -#include -#include - -// Test includes -#include - -// ROS includes -#include -#include -#include - -using namespace cv; - -void diffBetween(Mat& orig, Mat& diff, long& total_diff) -{ - MatIterator_ it, end; - Vec3b orig_pixel, diff_pixel; - total_diff = 0; - - for(int i=0; i(i,j); - diff_pixel = diff.at(i,j); - total_diff += abs(orig_pixel[0] - diff_pixel[0]) + - abs(orig_pixel[1] - diff_pixel[1]) + - abs(orig_pixel[2] - diff_pixel[2]); - } - } -} - -class DistortionTest : public testing::Test -{ - protected: - ros::NodeHandle nh_; - - // Used to listen for images - image_transport::Subscriber cam_sub_distorted_; - image_transport::Subscriber cam_sub_undistorted_; - - // Stores found images - sensor_msgs::ImageConstPtr cam_image_distorted_; - sensor_msgs::ImageConstPtr cam_image_undistorted_; - - // Listens for camera metadata to be published - ros::Subscriber cam_info_distorted_sub_; - // Stores received camera metadata - sensor_msgs::CameraInfoConstPtr cam_info_distorted_; - - public: - void cameraDistortionTest(); - - void imageCallback(const sensor_msgs::ImageConstPtr& msg, int cam_index) - { - // for now, only support 2 cameras - assert(cam_index == 0 || cam_index == 1); - if(cam_index == 0) - { - cam_image_undistorted_ = msg; - } - else - { - cam_image_distorted_ = msg; - } - } - void camInfoCallback(const sensor_msgs::CameraInfoConstPtr& msg) - { - cam_info_distorted_ = msg; - } -}; - -void DistortionTest::cameraDistortionTest() -{ - ros::AsyncSpinner spinner(2); - spinner.start(); - - image_transport::ImageTransport trans(nh_); - cam_sub_undistorted_ = - trans.subscribe("/camera_undistorted/image_raw", - 1, - boost::bind(&DistortionTest::imageCallback, - dynamic_cast(this), _1, 0) - ); - - cam_info_distorted_ = nullptr; - cam_image_distorted_ = nullptr; - // acquire information from ROS topics - cam_sub_distorted_ = - trans.subscribe("/camera_distorted/image_raw", - 1, - boost::bind(&DistortionTest::imageCallback, - dynamic_cast(this), _1, 1) - ); - cam_info_distorted_sub_ = - nh_.subscribe("/camera_distorted/camera_info", - 1, - &DistortionTest::camInfoCallback, - dynamic_cast(this) - ); - - // keep waiting until we have an image - - if(cam_info_distorted_ && cam_image_distorted_) { - std::cerr << "available immediately" << std::endl; - } - while (!cam_info_distorted_ || - !cam_image_distorted_ || - !cam_image_undistorted_) - { - ros::Duration(0.1).sleep(); - } - cam_sub_undistorted_.shutdown(); - cam_sub_distorted_.shutdown(); - cam_info_distorted_sub_.shutdown(); - - // load camera coefficients from published ROS information - Mat intrinsic_distorted_matrix = Mat(3, 3, CV_64F); - if(cam_info_distorted_->K.size() == 9) - { - memcpy(intrinsic_distorted_matrix.data, cam_info_distorted_->K.data(), - cam_info_distorted_->K.size()*sizeof(double)); - } - Mat distortion_coeffs = Mat(5, 1, CV_64F); - if(cam_info_distorted_->D.size() == 5) - { - memcpy(distortion_coeffs.data, cam_info_distorted_->D.data(), - cam_info_distorted_->D.size()*sizeof(double)); - } - - // Information acquired, now test the quality of the undistortion - - Mat distorted = Mat(cv_bridge::toCvCopy(cam_image_distorted_)->image); - Mat fixed = distorted.clone(); - Mat undistorted = Mat(cv_bridge::toCvCopy(cam_image_undistorted_)->image); - - //crop the image to remove black borders leftover from (un)distortion - int cropBorder = 50; - cv::Rect myROI(cropBorder, cropBorder, - fixed.rows - 2 * cropBorder, fixed.cols - 2 * cropBorder); - cv::Mat fixed_crop = fixed(myROI); - cv::Mat undistorted_crop = undistorted(myROI); - - - undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs); - - //Ensure that we didn't crop away everything - ASSERT_GT(distorted.rows, 0); - ASSERT_GT(distorted.cols, 0); - ASSERT_GT(undistorted.rows, 0); - ASSERT_GT(undistorted.cols, 0); - ASSERT_GT(fixed.rows, 0); - ASSERT_GT(fixed.cols, 0); - - // The difference between the undistorted image and the no-distortion camera - // image should be the lowest when we use the correct distortion parameters. - long diff1 = 0, diff2 = 0; - diffBetween(fixed_crop, undistorted_crop, diff1); - - const double OFFSET = 0.01; - - // test each parameter, varying one at a time - for(size_t i = 0; i < 5; ++i) - { - distortion_coeffs.at(i,0) += OFFSET; - undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs); - diffBetween(fixed_crop, undistorted_crop, diff2); - EXPECT_GE(diff2, diff1); - distortion_coeffs.at(i,0) -= OFFSET; - - distortion_coeffs.at(i,0) -= OFFSET; - undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs); - diffBetween(fixed_crop, undistorted_crop, diff2); - EXPECT_GE(diff2, diff1); - distortion_coeffs.at(i,0) += OFFSET; - } -} - -#endif diff --git a/.ros1_unported/gazebo_plugins/test/camera/distortion_barrel.cpp b/.ros1_unported/gazebo_plugins/test/camera/distortion_barrel.cpp deleted file mode 100644 index b1361641a..000000000 --- a/.ros1_unported/gazebo_plugins/test/camera/distortion_barrel.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#include "distortion.h" - -TEST_F(DistortionTest, barrelDistortion) -{ - cameraDistortionTest(); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "gazebo_camera_barrel_distortion_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/.ros1_unported/gazebo_plugins/test/camera/distortion_barrel.test b/.ros1_unported/gazebo_plugins/test/camera/distortion_barrel.test deleted file mode 100644 index 7187b9326..000000000 --- a/.ros1_unported/gazebo_plugins/test/camera/distortion_barrel.test +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - diff --git a/.ros1_unported/gazebo_plugins/test/camera/triggered_camera.cpp b/.ros1_unported/gazebo_plugins/test/camera/triggered_camera.cpp deleted file mode 100644 index 9d7a51a3b..000000000 --- a/.ros1_unported/gazebo_plugins/test/camera/triggered_camera.cpp +++ /dev/null @@ -1,114 +0,0 @@ -#include -#include -#include -#include - -class TriggeredCameraTest : public testing::Test -{ -protected: - virtual void SetUp() - { - images_received_ = 0; - } - - ros::NodeHandle nh_; - image_transport::Subscriber cam_sub_; - int images_received_; - ros::Time image_stamp_; -public: - void imageCallback(const sensor_msgs::ImageConstPtr& msg) - { - image_stamp_ = msg->header.stamp; - images_received_++; - } -}; - -// Test if the camera image is published at all, and that the timestamp -// is not too long in the past. -TEST_F(TriggeredCameraTest, cameraSubscribeTest) -{ - image_transport::ImageTransport it(nh_); - cam_sub_ = it.subscribe("camera1/image_raw", 5, - &TriggeredCameraTest::imageCallback, - dynamic_cast(this)); - - // wait for 3 seconds to confirm that we don't receive any images - for (unsigned int i = 0; i < 30; ++i) - { - ros::spinOnce(); - ros::Duration(0.1).sleep(); - } - EXPECT_EQ(images_received_, 0); - - // make sure something is subscribing to image_trigger topic - // there is no easy API, so call getSystemState - XmlRpc::XmlRpcValue args, result, payload; - args[0] = ros::this_node::getName(); - EXPECT_TRUE(ros::master::execute("getSystemState", args, result, payload, true)); - // [publishers, subscribers, services] - // subscribers in index 1 of payload - int trigger_listeners = 0; - for (int i = 0; i < payload[1].size(); ++i) - { - // [ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ] - // topic name i is in index 0 - std::string topic = payload[1][i][0]; - if (topic.find("image_trigger") != std::string::npos) - { - trigger_listeners++; - } - } - EXPECT_EQ(trigger_listeners, 1); - - // publish to trigger topic and expect an update within one second: - ros::Publisher trigger_pub = nh_.advertise("camera1/image_trigger", 1, true); - std_msgs::Empty msg; - - trigger_pub.publish(msg); - for (unsigned int i = 0; i < 10 && !images_received_; ++i) - { - ros::spinOnce(); - ros::Duration(0.1).sleep(); - } - EXPECT_EQ(images_received_, 1); - - // then wait for 3 seconds to confirm that we don't receive any more images - for (unsigned int i = 0; i < 30; ++i) - { - ros::spinOnce(); - ros::Duration(0.1).sleep(); - } - EXPECT_EQ(images_received_, 1); - - // then send two trigger messages very close together, and expect two more - // images - trigger_pub.publish(msg); - ros::spinOnce(); - ros::Duration(0.01).sleep(); - trigger_pub.publish(msg); - ros::spinOnce(); - ros::Duration(0.01).sleep(); - for (unsigned int i = 0; i < 10 && images_received_ < 2; ++i) - { - ros::spinOnce(); - ros::Duration(0.1).sleep(); - } - EXPECT_EQ(images_received_, 3); - - // then wait for 3 seconds to confirm that we don't receive any more images - for (unsigned int i = 0; i < 30; ++i) - { - ros::spinOnce(); - ros::Duration(0.1).sleep(); - } - EXPECT_EQ(images_received_, 3); - - cam_sub_.shutdown(); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "gazebo_camera_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/.ros1_unported/gazebo_plugins/test/camera/triggered_camera.test b/.ros1_unported/gazebo_plugins/test/camera/triggered_camera.test deleted file mode 100644 index e9f6ec5cf..000000000 --- a/.ros1_unported/gazebo_plugins/test/camera/triggered_camera.test +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - diff --git a/.ros1_unported/gazebo_plugins/test/camera/triggered_camera.world b/.ros1_unported/gazebo_plugins/test/camera/triggered_camera.world deleted file mode 100644 index 30cb34037..000000000 --- a/.ros1_unported/gazebo_plugins/test/camera/triggered_camera.world +++ /dev/null @@ -1,140 +0,0 @@ - - - - - - model://ground_plane - - - - - model://sun - - - - - - 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 - orbit - - - - - false - 2.0 0.0 4.0 0.0 0.0 0.0 - - 0.0 0.0 0.0 0.0 0.0 0.0 - - 0.0 0.0 0.0 0.0 0.0 0.0 - - 1.0 - 0.0 - 0.0 - 1.0 - 0.0 - 1.0 - - 10.0 - - - 0.0 0.0 0.0 0.0 0.0 0.0 - - - 0.5 - - - - 0.03 0.5 0.5 1.0 - - - true - 100.0 - - - 0.0 0.0 0.0 0.0 0.0 0.0 - 250 - - - 0.5 - - - - - - 0.5 - 0.2 - 1.0 0 0 - 0 - 0 - - - - 0 - 1000000.0 - - - - 0 - 0.2 - 1e15 - 1e13 - 100.0 - 0.0001 - - - - 100.0 - - - - - - true - 0.0 0.0 0.5 0.0 0.0 0.0 - - 0.0 0.0 0.0 0.0 0.0 0.0 - - 0 - 1 - - 1.3962634 - - 640 - 480 - R8G8B8 - - - 0.02 - 300 - - - gaussian - - 0.0 - 0.007 - - - - true - image_trigger - - 0.0 - camera1 - image_raw - camera_info - camera_link - 0.07 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 92d352ac1..94691b2c1 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -12,24 +12,30 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(camera_info_manager REQUIRED) find_package(gazebo_dev REQUIRED) find_package(gazebo_ros REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(rclcpp REQUIRED) +find_package(image_transport REQUIRED) find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) include_directories(include + ${gazebo_dev_INCLUDE_DIRS} ${gazebo_ros_INCLUDE_DIRS} ${geometry_msgs_INCLUDE_DIRS} + ${image_transport_INCLUDE_DIRS} ${nav_msgs_INCLUDE_DIRS} ${sensor_msgs_INCLUDE_DIRS} ${tf2_ros_INCLUDE_DIRS} ${tf2_INCLUDE_DIRS} ) +link_directories(${gazebo_dev_LIBRARY_DIRS}) # gazebo_ros_joint_state_publisher add_library(gazebo_ros_joint_state_publisher SHARED @@ -82,6 +88,25 @@ ament_target_dependencies(gazebo_ros_template ) ament_export_libraries(gazebo_ros_template) +# gazebo_ros_camera +add_library(gazebo_ros_camera SHARED + src/gazebo_ros_camera.cpp +) +ament_target_dependencies(gazebo_ros_camera + "camera_info_manager" + "gazebo_dev" + "gazebo_ros" + "image_transport" + "rclcpp" + "sensor_msgs" + "std_msgs" +) +target_link_libraries(gazebo_ros_camera + CameraPlugin +) +ament_export_libraries(gazebo_ros_camera) + +# gazebo_ros_imu_sensor add_library(gazebo_ros_imu_sensor SHARED src/gazebo_ros_imu_sensor.cpp ) @@ -92,6 +117,7 @@ ament_target_dependencies(gazebo_ros_imu_sensor ) ament_export_libraries(gazebo_ros_imu_sensor) +# gazebo_ros_ray_sensor add_library(gazebo_ros_ray_sensor SHARED src/gazebo_ros_ray_sensor.cpp ) @@ -118,6 +144,7 @@ install(DIRECTORY include/ DESTINATION include) install(TARGETS + gazebo_ros_camera gazebo_ros_diff_drive gazebo_ros_force gazebo_ros_imu_sensor diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera.hpp new file mode 100644 index 000000000..ff3124264 --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera.hpp @@ -0,0 +1,103 @@ +// 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_PLUGINS__GAZEBO_ROS_CAMERA_HPP_ +#define GAZEBO_PLUGINS__GAZEBO_ROS_CAMERA_HPP_ + +#include +#include + +#include +#include + +namespace gazebo_plugins +{ +class GazeboRosCameraPrivate; + +/// A plugin that publishes raw images and camera info for generic camera sensors. +/** + Example Usage: + \code{.xml} + + + + custom_ns + image_raw:=custom_img + camera_info:=custom_info + image_trigger:=custom_trigger + + + + custom_camera + + + custom_frame + + + true + + 0.07 + + \endcode +*/ +class GazeboRosCamera : public gazebo::CameraPlugin +{ +public: + /// Constructor + GazeboRosCamera(); + + /// Destructor + ~GazeboRosCamera(); + +protected: + // Documentation inherited + void Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) override; + + /// Callback when 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 + */ + void OnNewFrame( + const unsigned char * _image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string & _format) 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_PLUGINS__GAZEBO_ROS_CAMERA_HPP_ diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index ff41bebf2..d28ce83bb 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -19,9 +19,12 @@ ament_cmake + camera_info_manager geometry_msgs + image_transport nav_msgs sensor_msgs + std_msgs tf2_geometry_msgs tf2_ros @@ -36,6 +39,7 @@ ament_cmake_gtest ament_lint_auto ament_lint_common + cv_bridge ament_cmake diff --git a/gazebo_plugins/src/gazebo_ros_camera.cpp b/gazebo_plugins/src/gazebo_ros_camera.cpp new file mode 100644 index 000000000..88895446b --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_camera.cpp @@ -0,0 +1,359 @@ +// 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gazebo_plugins +{ +class GazeboRosCameraPrivate +{ +public: + /// A pointer to the GazeboROS node. + gazebo_ros::Node::SharedPtr ros_node_{nullptr}; + + /// Image publisher. + image_transport::Publisher image_pub_; + + /// Camera info publisher. + rclcpp::Publisher::SharedPtr camera_info_pub_{nullptr}; + + /// Trigger subscriber, in case it's a triggered camera + rclcpp::Subscription::SharedPtr trigger_sub_{nullptr}; + + /// Camera info manager + std::shared_ptr camera_info_manager_; + + /// Image encoding + std::string type_; + + /// Camera name, to be used on topics. + std::string camera_name_; + + /// Frame name, to be used by TF. + std::string frame_name_; + + /// Step size + int skip_; + + /// 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_; +}; + +GazeboRosCamera::GazeboRosCamera() +: impl_(std::make_unique()) +{ +} + +GazeboRosCamera::~GazeboRosCamera() +{ + impl_->image_pub_.shutdown(); + + // TODO(louise) Why does this hang for the 2nd camera plugin? + // Commenting it out here just pushes the problem somewhere else. + // impl_->ros_node_.reset(); +} + +void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) +{ + gazebo::CameraPlugin::Load(_sensor, _sdf); + + // Camera name + impl_->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); + + // Image publisher + // TODO(louise) Migrate image_connect logic once SubscriberStatusCallback is ported to ROS2 + impl_->image_pub_ = image_transport::create_publisher(impl_->ros_node_.get(), + impl_->camera_name_ + "/image_raw"); + + // TODO(louise) Uncomment this once image_transport::Publisher has a function to return the + // full topic. + // RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing images to [%s]", + // impl_->image_pub_.getTopic()); + + // Camera info publisher + // TODO(louise) Migrate ImageConnect logic once SubscriberStatusCallback is ported to ROS2 + impl_->camera_info_pub_ = impl_->ros_node_->create_publisher( + impl_->camera_name_ + "/camera_info"); + + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing camera info to [%s]", + impl_->camera_info_pub_->get_topic_name()); + + // Trigger + if (_sdf->Get("triggered", false).first) { + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + qos_profile.depth = 1; + + impl_->trigger_sub_ = impl_->ros_node_->create_subscription( + impl_->camera_name_ + "/image_trigger", + std::bind(&GazeboRosCamera::OnTrigger, this, std::placeholders::_1), + qos_profile); + + 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(&GazeboRosCamera::PreRender, this)); + } + + // Dynamic reconfigure +// dyn_srv_ = +// new dynamic_reconfigure::Server +// (*this->rosnode_); +// dynamic_reconfigure::Server +// ::CallbackType f = +// boost::bind(&GazeboRosCameraUtils::configCallback, this, _1, _2); +// dyn_srv_->setCallback(f); +// } + + // Buffer size + if (this->format == "L8" || this->format == "L_INT8") { + impl_->type_ = sensor_msgs::image_encodings::MONO8; + impl_->skip_ = 1; + } else if (this->format == "L16" || this->format == "L_INT16") { + impl_->type_ = sensor_msgs::image_encodings::MONO16; + impl_->skip_ = 2; + } else if (this->format == "R8G8B8" || this->format == "RGB_INT8") { + impl_->type_ = sensor_msgs::image_encodings::RGB8; + impl_->skip_ = 3; + } else if (this->format == "B8G8R8" || this->format == "BGR_INT8") { + impl_->type_ = sensor_msgs::image_encodings::BGR8; + impl_->skip_ = 3; + } else if (this->format == "R16G16B16" || this->format == "RGB_INT16") { + impl_->type_ = sensor_msgs::image_encodings::RGB16; + impl_->skip_ = 6; + } else if (this->format == "BAYER_RGGB8") { + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "bayer simulation may be computationally expensive."); + impl_->type_ = sensor_msgs::image_encodings::BAYER_RGGB8; + impl_->skip_ = 1; + } else if (this->format == "BAYER_BGGR8") { + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "bayer simulation may be computationally expensive."); + impl_->type_ = sensor_msgs::image_encodings::BAYER_BGGR8; + impl_->skip_ = 1; + } else if (this->format == "BAYER_GBRG8") { + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "bayer simulation may be computationally expensive."); + impl_->type_ = sensor_msgs::image_encodings::BAYER_GBRG8; + impl_->skip_ = 1; + } else if (this->format == "BAYER_GRBG8") { + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "bayer simulation may be computationally expensive."); + impl_->type_ = sensor_msgs::image_encodings::BAYER_GRBG8; + impl_->skip_ = 1; + } else { + RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Unsupported Gazebo ImageFormat, using BGR8\n"); + impl_->type_ = sensor_msgs::image_encodings::BGR8; + impl_->skip_ = 3; + } + + // C parameters + auto default_cx = (static_cast(this->width) + 1.0) / 2.0; + auto cx = _sdf->Get("cx", default_cx).first; + + auto default_cy = (static_cast(this->height) + 1.0) / 2.0; + auto cy = _sdf->Get("cy", default_cy).first; + + double hfov = this->camera->HFOV().Radian(); + double computed_focal_length = (static_cast(this->width)) / (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(), this->width, 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 = this->height; + camera_info_msg.width = this->width; + 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 (this->camera->LensDistortion()) { + this->camera->LensDistortion()->SetCrop(border_crop); + + distortion_k1 = this->camera->LensDistortion()->K1(); + distortion_k2 = this->camera->LensDistortion()->K2(); + distortion_k3 = this->camera->LensDistortion()->K3(); + distortion_t1 = this->camera->LensDistortion()->P1(); + distortion_t2 = this->camera->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_ = std::make_shared( + impl_->ros_node_.get(), impl_->camera_name_); + impl_->camera_info_manager_->setCameraInfo(camera_info_msg); +} + +//////////////////////////////////////////////////////////////////////////////// +void GazeboRosCamera::OnNewFrame( + const unsigned char * _image, + unsigned int _width, + unsigned int _height, + unsigned int /*_depth*/, + const std::string & /*_format*/) +{ + // TODO(louise) Enable / disable sensor once SubscriberStatusCallback has been ported to ROS2 + + auto sensor_update_time = this->parentSensor->LastMeasurementTime(); + + // Publish image + sensor_msgs::msg::Image image_msg; + 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_->type_, _height, _width, + impl_->skip_ * _width, reinterpret_cast(_image)); + + impl_->image_pub_.publish(image_msg); + + // Publish camera info + auto camera_info_msg = impl_->camera_info_manager_->getCameraInfo(); + camera_info_msg.header.stamp = gazebo_ros::Convert( + sensor_update_time); + + impl_->camera_info_pub_->publish(camera_info_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 GazeboRosCamera::SetCameraEnabled(const bool _enabled) +{ + this->parentSensor->SetActive(_enabled); + this->parentSensor->SetUpdateRate(_enabled ? 0.0 : ignition::math::MIN_D); +} + +void GazeboRosCamera::PreRender() +{ + std::lock_guard lock(impl_->trigger_mutex_); + if (impl_->triggered > 0) { + SetCameraEnabled(true); + } +} + +void GazeboRosCamera::OnTrigger(const std_msgs::msg::Empty::SharedPtr) +{ + std::lock_guard lock(impl_->trigger_mutex_); + impl_->triggered++; +} + +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosCamera) +} // namespace gazebo_plugins diff --git a/gazebo_plugins/test/CMakeLists.txt b/gazebo_plugins/test/CMakeLists.txt index 8c1c43d37..ec0e346cc 100644 --- a/gazebo_plugins/test/CMakeLists.txt +++ b/gazebo_plugins/test/CMakeLists.txt @@ -1,10 +1,19 @@ find_package(ament_cmake_gtest REQUIRED) +find_package(cv_bridge REQUIRED) # Worlds -file(COPY worlds DESTINATION .) +file(GLOB worlds RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} "worlds/*.world") +foreach(world ${worlds}) + # Use configure_file so this is rerun each time make is invoked (as opposed to just cmake) + configure_file(${world} ${world} COPYONLY) +endforeach() # Tests set(tests + test_gazebo_ros_camera + # TODO(louise) Test hangs on teardown while destroying 2nd node + # test_gazebo_ros_camera_distortion + test_gazebo_ros_camera_triggered test_gazebo_ros_diff_drive test_gazebo_ros_force test_gazebo_ros_imu_sensor @@ -25,9 +34,11 @@ foreach(test ${tests}) gazebo_test_fixture ) ament_target_dependencies(${test} + "cv_bridge" "gazebo_dev" "gazebo_ros" "geometry_msgs" + "image_transport" "nav_msgs" "rclcpp" "sensor_msgs" diff --git a/gazebo_plugins/test/test_gazebo_ros_camera.cpp b/gazebo_plugins/test/test_gazebo_ros_camera.cpp new file mode 100644 index 000000000..f68133ca0 --- /dev/null +++ b/gazebo_plugins/test/test_gazebo_ros_camera.cpp @@ -0,0 +1,92 @@ +// Copyright 2018 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; + + /// Raw image topic to subscribe to + std::string topic; +}; + +class GazeboRosCameraTest + : public gazebo::ServerFixture, public ::testing::WithParamInterface +{ +}; + +// Test that the camera image is published and has correct timestamp +TEST_P(GazeboRosCameraTest, 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_camera_test"); + ASSERT_NE(nullptr, node); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + // Subscribe + unsigned int msg_count{0}; + builtin_interfaces::msg::Time image_stamp; + + auto sub = image_transport::create_subscription(node.get(), GetParam().topic, + [&](const sensor_msgs::msg::Image::ConstSharedPtr & msg) { + image_stamp = msg->header.stamp; + ++msg_count; + }, + "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); + executor.spin_once(100ms); + + EXPECT_EQ(1u, msg_count); + EXPECT_EQ(2.0, image_stamp.sec); + + // Clean up + sub.shutdown(); +} + +INSTANTIATE_TEST_CASE_P(GazeboRosCamera, GazeboRosCameraTest, ::testing::Values( + TestParams({"worlds/gazebo_ros_camera.world", + "test_cam/camera/image_test"}), + TestParams({"worlds/gazebo_ros_camera_16bit.world", + "test_cam_16bit/image_test_16bit"}) + ), ); + +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/test_gazebo_ros_camera_distortion.cpp b/gazebo_plugins/test/test_gazebo_ros_camera_distortion.cpp new file mode 100644 index 000000000..f5c16103c --- /dev/null +++ b/gazebo_plugins/test/test_gazebo_ros_camera_distortion.cpp @@ -0,0 +1,222 @@ +// Copyright 2018 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 +#include + +#include +#include + +using namespace std::literals::chrono_literals; // NOLINT + +/// Test parameters +struct TestParams +{ + /// Path to world file + std::string world; + + /// Undistorted image topic to subscribe to + std::string undistorted_topic; + + /// Distorted image topic to subscribe to + std::string distorted_topic; + + /// Distorted camera info topic to subscribe to + std::string distorted_cam_topic; +}; + +void DiffBetween(cv::Mat & orig, cv::Mat & diff, int64 & total_diff) +{ + cv::MatIterator_ it, end; + cv::Vec3b orig_pixel, diff_pixel; + total_diff = 0; + + for (int i = 0; i < orig.rows; ++i) { + for (int j = 0; j < orig.cols; ++j) { + orig_pixel = orig.at(i, j); + diff_pixel = diff.at(i, j); + total_diff += + abs(orig_pixel[0] - diff_pixel[0]) + + abs(orig_pixel[1] - diff_pixel[1]) + + abs(orig_pixel[2] - diff_pixel[2]); + } + } +} + +class GazeboRosCameraDistortionTest + : public gazebo::ServerFixture, public ::testing::WithParamInterface +{ +public: + void TearDown() override + { + // Make sure they're destroyed even if test fails by ASSERT + cam_sub_undistorted_.shutdown(); + cam_sub_distorted_.shutdown(); + cam_info_distorted_sub_.reset(); + ServerFixture::TearDown(); + } + + /// Subscribe to distorted images. + image_transport::Subscriber cam_sub_distorted_; + + /// Subscribe to undistorted images. + image_transport::Subscriber cam_sub_undistorted_; + + /// Subscribe to distorted camera info. + rclcpp::Subscription::SharedPtr cam_info_distorted_sub_; +}; + +TEST_P(GazeboRosCameraDistortionTest, 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_camera_distortion_test"); + ASSERT_NE(nullptr, node); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + // Subscribe to undistorted image + sensor_msgs::msg::Image::ConstSharedPtr cam_image_undistorted; + cam_sub_undistorted_ = image_transport::create_subscription(node, + GetParam().undistorted_topic, + [&cam_image_undistorted](const sensor_msgs::msg::Image::ConstSharedPtr & _msg) { + cam_image_undistorted = _msg; + }, + "raw"); + + // Subscribe to distorted image + sensor_msgs::msg::Image::ConstSharedPtr cam_image_distorted; + cam_sub_distorted_ = image_transport::create_subscription(node, + GetParam().distorted_topic, + [&cam_image_distorted](const sensor_msgs::msg::Image::ConstSharedPtr & _msg) { + cam_image_distorted = _msg; + }, + "raw"); + + // Subscribe to distorted camera info + sensor_msgs::msg::CameraInfo::SharedPtr cam_info_distorted; + cam_info_distorted_sub_ = node->create_subscription( + GetParam().distorted_cam_topic, + [&cam_info_distorted](const sensor_msgs::msg::CameraInfo::SharedPtr _msg) { + cam_info_distorted = _msg; + }); + + // 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); + + executor.spin_once(100ms); + executor.spin_once(100ms); + executor.spin_once(100ms); + executor.spin_once(100ms); + + ASSERT_NE(nullptr, cam_image_undistorted); + ASSERT_NE(nullptr, cam_image_distorted); + ASSERT_NE(nullptr, cam_info_distorted); + + // Cleanup transport so we don't get new messages while proceeding the test + cam_sub_undistorted_.shutdown(); + cam_sub_distorted_.shutdown(); + cam_info_distorted_sub_.reset(); + + // Load camera coefficients from published ROS information + auto intrinsic_distorted_matrix = cv::Mat(3, 3, CV_64F); + if (cam_info_distorted->k.size() == 9) { + memcpy(intrinsic_distorted_matrix.data, cam_info_distorted->k.data(), + cam_info_distorted->k.size() * sizeof(double)); + } + + auto distortion_coeffs = cv::Mat(5, 1, CV_64F); + if (cam_info_distorted->d.size() == 5) { + memcpy(distortion_coeffs.data, cam_info_distorted->d.data(), + cam_info_distorted->d.size() * sizeof(double)); + } + + // Information acquired, now test the quality of the undistortion + auto distorted = cv::Mat(cv_bridge::toCvCopy(cam_image_distorted)->image); + auto fixed = distorted.clone(); + auto undistorted = cv::Mat(cv_bridge::toCvCopy(cam_image_undistorted)->image); + + // crop the image to remove black borders leftover from (un)distortion + int crop_border = 50; + cv::Rect myROI(crop_border, crop_border, + fixed.rows - 2 * crop_border, fixed.cols - 2 * crop_border); + cv::Mat fixed_crop = fixed(myROI); + auto undistorted_crop = undistorted(myROI); + + undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs); + + // Ensure that we didn't crop away everything + ASSERT_GT(distorted.rows, 0); + ASSERT_GT(distorted.cols, 0); + ASSERT_GT(undistorted.rows, 0); + ASSERT_GT(undistorted.cols, 0); + ASSERT_GT(fixed.rows, 0); + ASSERT_GT(fixed.cols, 0); + + // The difference between the undistorted image and the no-distortion camera + // image should be the lowest when we use the correct distortion parameters. + int64 diff1 = 0, diff2 = 0; + DiffBetween(fixed_crop, undistorted_crop, diff1); + + const double OFFSET = 0.01; + + // test each parameter, varying one at a time + for (size_t i = 0; i < 5; ++i) { + distortion_coeffs.at(i, 0) += OFFSET; + undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs); + DiffBetween(fixed_crop, undistorted_crop, diff2); + EXPECT_GE(diff2, diff1); + distortion_coeffs.at(i, 0) -= OFFSET; + + distortion_coeffs.at(i, 0) -= OFFSET; + undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs); + DiffBetween(fixed_crop, undistorted_crop, diff2); + EXPECT_GE(diff2, diff1); + distortion_coeffs.at(i, 0) += OFFSET; + } +} + +INSTANTIATE_TEST_CASE_P(GazeboRosCameraDistortion, GazeboRosCameraDistortionTest, + ::testing::Values( + TestParams({ + "worlds/gazebo_ros_camera_distortion_barrel.world", + "undistorted_image", + "distorted_image", + "distorted_info" +}) +// TestParams({"worlds/gazebo_ros_camera_distortion_pincushion.world", +// "undistorted_image", +// "distorted_image", +// "distorted_info"}) + ), ); + +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/test_gazebo_ros_camera_triggered.cpp b/gazebo_plugins/test/test_gazebo_ros_camera_triggered.cpp new file mode 100644 index 000000000..a18b0f1e1 --- /dev/null +++ b/gazebo_plugins/test/test_gazebo_ros_camera_triggered.cpp @@ -0,0 +1,99 @@ +// Copyright 2018 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 + +class GazeboRosTriggeredCameraTest : public gazebo::ServerFixture +{ +}; + +// Test if the camera image is published at all, and that the timestamp +// is not too long in the past. +TEST_F(GazeboRosTriggeredCameraTest, CameraSubscribeTest) +{ + // Load test world and start paused + this->Load("worlds/gazebo_ros_camera_triggered.world", true); + + // World + auto world = gazebo::physics::get_world(); + ASSERT_NE(nullptr, world); + + // Create node and executor + auto node = std::make_shared("gazebo_ros_camera_triggered_test"); + ASSERT_NE(nullptr, node); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + // Subscribe + unsigned int msg_count{0}; + builtin_interfaces::msg::Time image_stamp; + + auto sub = image_transport::create_subscription(node.get(), + "test_triggered_cam/image_raw_test", + [&](const sensor_msgs::msg::Image::ConstSharedPtr & msg) { + image_stamp = msg->header.stamp; + ++msg_count; + }, + "raw"); + + // Step a bit and check that we do not get any messages + world->Step(100); + executor.spin_once(100ms); + + EXPECT_EQ(0u, msg_count); + + // Trigger camera once + auto pub = node->create_publisher( + "test_triggered_cam/image_trigger_test"); + std_msgs::msg::Empty msg; + pub->publish(msg); + + // Step a bit and check that we get exactly one message + world->Step(100); + executor.spin_once(100ms); + + EXPECT_EQ(1u, msg_count); + + // Trigger camera twice + pub->publish(msg); + executor.spin_once(100ms); + executor.spin_once(100ms); + pub->publish(msg); + executor.spin_once(100ms); + + // Step a bit and check that we get exactly two messages + world->Step(100); + executor.spin_once(100ms); + + EXPECT_EQ(3u, msg_count); + + sub.shutdown(); +} + +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_camera.world b/gazebo_plugins/test/worlds/gazebo_ros_camera.world new file mode 100644 index 000000000..38b7c1c06 --- /dev/null +++ b/gazebo_plugins/test/worlds/gazebo_ros_camera.world @@ -0,0 +1,43 @@ + + + + + true + + + 0.5 + + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + + test_cam + camera1/image_raw:=camera/image_test + camera_info:=camera_info_test + + + + 0.07 + + + + + + diff --git a/gazebo_plugins/test/worlds/gazebo_ros_camera_16bit.world b/gazebo_plugins/test/worlds/gazebo_ros_camera_16bit.world new file mode 100644 index 000000000..247838a6e --- /dev/null +++ b/gazebo_plugins/test/worlds/gazebo_ros_camera_16bit.world @@ -0,0 +1,48 @@ + + + + + true + + + 0.5 + + 1.3962634 + + 640 + 480 + R16G16B16 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + + test_cam_16bit + test_camera_name/image_raw:=image_test_16bit + camera_info:=camera_info_test_16bit + + test_camera_name + + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + diff --git a/.ros1_unported/gazebo_plugins/test/camera/distortion_barrel.world b/gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_barrel.world similarity index 58% rename from .ros1_unported/gazebo_plugins/test/camera/distortion_barrel.world rename to gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_barrel.world index 8bc111106..b0f95dfcf 100644 --- a/.ros1_unported/gazebo_plugins/test/camera/distortion_barrel.world +++ b/gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_barrel.world @@ -1,5 +1,5 @@ - + @@ -11,14 +11,6 @@ true 0.0 0.0 2.5 0.0 1.5707 0.0 - 0 0 0 0 0 0 - - - - 0.1 0.1 0.1 - - - 30.0 @@ -46,19 +38,14 @@ - true - 0.0 - camera_distorted - image_raw - camera_info - camera_distorted_link - 0.07 - - true - false + + distorted_camera/image_raw:=distorted_image + distorted_camera/camera_info:=distorted_info + + distorted_camera + frame_name + 0.07 + false @@ -68,14 +55,6 @@ true 0.0 0.0 2.5 0.0 1.5707 0.0 - 0 0 0 0 0 0 - - - - 0.1 0.1 0.1 - - - 30.0 @@ -99,18 +78,11 @@ - true - 0.0 - camera_undistorted - image_raw - camera_info - camera_undistorted_link - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 + + undistorted_camera/image_raw:=undistorted_image + + undistorted_camera + frame_name @@ -119,5 +91,5 @@ model://checkerboard_plane - + diff --git a/.ros1_unported/gazebo_plugins/test/camera/distortion_pincushion.world b/gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_pincushion.world similarity index 55% rename from .ros1_unported/gazebo_plugins/test/camera/distortion_pincushion.world rename to gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_pincushion.world index 0b9f49c92..4aba7f1a0 100644 --- a/.ros1_unported/gazebo_plugins/test/camera/distortion_pincushion.world +++ b/gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_pincushion.world @@ -1,5 +1,5 @@ - + @@ -11,14 +11,6 @@ true 0.0 0.0 2.5 0.0 1.5707 0.0 - 0 0 0 0 0 0 - - - - 0.1 0.1 0.1 - - - 30.0 @@ -46,23 +38,14 @@ - true - 0.0 - camera_distorted - image_raw - camera_info - camera_distorted_link - 0.07 - - 0.1 - 0.1 - 0.0 - 0.0 - 0.1 - false + + distorted_camera/image_raw:=distorted_image + distorted_camera/camera_info:=distorted_info + + distorted_camera + frame_name + 0.07 + false @@ -72,14 +55,6 @@ true 0.0 0.0 2.5 0.0 1.5707 0.0 - 0 0 0 0 0 0 - - - - 0.1 0.1 0.1 - - - 30.0 @@ -103,18 +78,11 @@ - true - 0.0 - camera_undistorted - image_raw - camera_info - camera_undistorted_link - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 + + undistorted_camera/image_raw:=undistorted_image + + undistorted_camera + frame_name @@ -123,5 +91,5 @@ model://checkerboard_plane - + diff --git a/gazebo_plugins/test/worlds/gazebo_ros_camera_triggered.world b/gazebo_plugins/test/worlds/gazebo_ros_camera_triggered.world new file mode 100644 index 000000000..cdd6d6016 --- /dev/null +++ b/gazebo_plugins/test/worlds/gazebo_ros_camera_triggered.world @@ -0,0 +1,49 @@ + + + + + + model://sun + + + + true + 0.0 0.0 0.5 0.0 0.0 0.0 + + + 0 + 1 + + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + + test_triggered_cam + camera1/image_raw:=image_raw_test + camera1/image_trigger:=image_trigger_test + + true + 0.07 + + + + + + diff --git a/gazebo_plugins/worlds/gazebo_ros_camera_demo.world b/gazebo_plugins/worlds/gazebo_ros_camera_demo.world new file mode 100644 index 000000000..76f04a054 --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_camera_demo.world @@ -0,0 +1,151 @@ + + + + + + + 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 + + + 60 + true + + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + + demo_cam + + image_raw:=image_demo + camera_info:=camera_info_demo + + + + + + + + + diff --git a/gazebo_plugins/worlds/gazebo_ros_camera_distortion_barrel_demo.world b/gazebo_plugins/worlds/gazebo_ros_camera_distortion_barrel_demo.world new file mode 100644 index 000000000..47df11771 --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_camera_distortion_barrel_demo.world @@ -0,0 +1,149 @@ + + + + + + + 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 + + + 60 + true + + 0.927295218 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + -0.1 + -0.1 + 0 + 0 + -0.1 +
0.5 0.5
+ + + + false + + + + + + + -1.5 0 1 0 1.57 0 + model://checkerboard_plane + + + diff --git a/gazebo_plugins/worlds/gazebo_ros_camera_distortion_pincushion_demo.world b/gazebo_plugins/worlds/gazebo_ros_camera_distortion_pincushion_demo.world new file mode 100644 index 000000000..0f6d07a77 --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_camera_distortion_pincushion_demo.world @@ -0,0 +1,149 @@ + + + + + + + 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 + + + 60 + true + + 0.927295218 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + 0.1 + 0.1 + 0 + 0 + 0.1 +
0.5 0.5
+ + + + false + + + + + + + -1.5 0 1 0 1.57 0 + model://checkerboard_plane + + + diff --git a/gazebo_plugins/worlds/gazebo_ros_camera_triggered_demo.world b/gazebo_plugins/worlds/gazebo_ros_camera_triggered_demo.world new file mode 100644 index 000000000..d74ad5a10 --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_camera_triggered_demo.world @@ -0,0 +1,154 @@ + + + + + + + + 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 + + + 60 + true + + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + + demo_triggered_cam + + true + + + + + + diff --git a/gazebo_ros/test/test_node.cpp b/gazebo_ros/test/test_node.cpp index 9f2ed8856..6a1c88c66 100644 --- a/gazebo_ros/test/test_node.cpp +++ b/gazebo_ros/test/test_node.cpp @@ -50,6 +50,42 @@ TEST(TestNode, StaticNode) EXPECT_NE(address_1.str(), address_3.str()); } +TEST(TestNode, GetSdf) +{ + // Plugin SDF + auto sdf_str = + "" + "" + "" + "" + "" + ""; + + sdf::SDF sdf; + sdf.SetFromString(sdf_str); + auto plugin_sdf = sdf.Root()->GetElement("world")->GetElement("plugin"); + + // Create a node + auto node_1 = gazebo_ros::Node::Get(plugin_sdf); + ASSERT_NE(nullptr, node_1); + EXPECT_STREQ("node_name", node_1->get_name()); + + // Create another node + auto node_2 = gazebo_ros::Node::Get(plugin_sdf); + ASSERT_NE(nullptr, node_2); + EXPECT_STREQ("node_name", node_2->get_name()); + EXPECT_NE(node_1, node_2); + + // Reset both + node_1.reset(); + node_2.reset(); + + // Create another node + auto node_3 = gazebo_ros::Node::Get(plugin_sdf); + ASSERT_NE(nullptr, node_3); + EXPECT_STREQ("node_name", node_3->get_name()); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv);