From 18be2615ffcefc8d7430b9a51fa69adaeb48949e Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Tue, 2 Mar 2021 19:02:59 +0100 Subject: [PATCH] Implement ROS bridge for calling set_rate on cameras and lidars. Add it to select robots. (#791) --- .../launch/vehicle_topics.launch | 41 +++++ .../package.xml | 1 + .../launch/vehicle_topics.launch | 20 +++ .../explorer_ds1_sensor_config_1/package.xml | 8 +- .../launch/vehicle_topics.launch | 28 ++++ .../explorer_x1_sensor_config_1/package.xml | 1 + .../launch/vehicle_topics.launch | 30 +++- .../explorer_x1_sensor_config_2/package.xml | 1 + .../launch/vehicle_topics.launch | 20 +++ .../marble_qav500_sensor_config_1/package.xml | 8 +- .../launch/vehicle_topics.launch | 25 ++++ .../ssci_x4_sensor_config_1/package.xml | 8 +- subt_msgs/CMakeLists.txt | 1 + subt_msgs/srv/SetRate.srv | 2 + subt_ros/CMakeLists.txt | 14 +- .../models_common/set_rate_relay.launch | 10 ++ subt_ros/launch/vehicle_topics.launch | 30 ++++ subt_ros/src/SetRateRelay.cc | 140 ++++++++++++++++++ 18 files changed, 377 insertions(+), 11 deletions(-) create mode 100644 subt_msgs/srv/SetRate.srv create mode 100644 subt_ros/launch/models_common/set_rate_relay.launch create mode 100644 subt_ros/src/SetRateRelay.cc diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/vehicle_topics.launch index 62c2582e..5466e094 100644 --- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/vehicle_topics.launch +++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/vehicle_topics.launch @@ -183,6 +183,11 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ignition-gazebo4 roslaunch + subt_ros xacro diff --git a/submitted_models/explorer_ds1_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/explorer_ds1_sensor_config_1/launch/vehicle_topics.launch index 6aa2584b..ab7461b0 100755 --- a/submitted_models/explorer_ds1_sensor_config_1/launch/vehicle_topics.launch +++ b/submitted_models/explorer_ds1_sensor_config_1/launch/vehicle_topics.launch @@ -122,6 +122,11 @@ + + + + + @@ -179,6 +184,11 @@ + + + + + @@ -236,6 +246,11 @@ + + + + + @@ -247,6 +262,11 @@ args="$(arg sensor_prefix)/front_laser/scan/points@sensor_msgs/PointCloud2[ignition.msgs.PointCloudPacked"> + + + + + diff --git a/submitted_models/explorer_ds1_sensor_config_1/package.xml b/submitted_models/explorer_ds1_sensor_config_1/package.xml index c67d97c1..fba06ead 100644 --- a/submitted_models/explorer_ds1_sensor_config_1/package.xml +++ b/submitted_models/explorer_ds1_sensor_config_1/package.xml @@ -11,8 +11,10 @@ BSD catkin - roslaunch - urdf - xacro + + roslaunch + subt_ros + urdf + xacro diff --git a/submitted_models/explorer_x1_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/explorer_x1_sensor_config_1/launch/vehicle_topics.launch index 73343caa..ea2e1d06 100644 --- a/submitted_models/explorer_x1_sensor_config_1/launch/vehicle_topics.launch +++ b/submitted_models/explorer_x1_sensor_config_1/launch/vehicle_topics.launch @@ -101,6 +101,11 @@ + + + + + @@ -154,6 +159,12 @@ + + + + + + @@ -207,6 +218,12 @@ + + + + + + @@ -260,6 +277,12 @@ + + + + + + @@ -270,6 +293,11 @@ args="$(arg sensor_prefix)/front_laser/scan/points@sensor_msgs/PointCloud2[ignition.msgs.PointCloudPacked"> + + + + + diff --git a/submitted_models/explorer_x1_sensor_config_1/package.xml b/submitted_models/explorer_x1_sensor_config_1/package.xml index bfafa5fa..e2014531 100644 --- a/submitted_models/explorer_x1_sensor_config_1/package.xml +++ b/submitted_models/explorer_x1_sensor_config_1/package.xml @@ -12,6 +12,7 @@ catkin roslaunch lms1xx + subt_ros urdf xacro diff --git a/submitted_models/explorer_x1_sensor_config_2/launch/vehicle_topics.launch b/submitted_models/explorer_x1_sensor_config_2/launch/vehicle_topics.launch index ac307b36..17d6cbc6 100644 --- a/submitted_models/explorer_x1_sensor_config_2/launch/vehicle_topics.launch +++ b/submitted_models/explorer_x1_sensor_config_2/launch/vehicle_topics.launch @@ -101,6 +101,12 @@ + + + + + + @@ -154,6 +160,12 @@ + + + + + + @@ -207,6 +219,12 @@ + + + + + + @@ -260,6 +278,12 @@ + + + + + + @@ -270,7 +294,11 @@ args="$(arg sensor_prefix)/front_laser/scan/points@sensor_msgs/PointCloud2[ignition.msgs.PointCloudPacked"> - + + + + + catkin roslaunch lms1xx + subt_ros urdf xacro diff --git a/submitted_models/marble_qav500_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/marble_qav500_sensor_config_1/launch/vehicle_topics.launch index e509d4d4..98fae2eb 100755 --- a/submitted_models/marble_qav500_sensor_config_1/launch/vehicle_topics.launch +++ b/submitted_models/marble_qav500_sensor_config_1/launch/vehicle_topics.launch @@ -102,6 +102,11 @@ + + + + + + + + + + + + + + + + + + + + BSD catkin - roslaunch - urdf - xacro + + roslaunch + subt_ros + urdf + xacro diff --git a/submitted_models/ssci_x4_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/ssci_x4_sensor_config_1/launch/vehicle_topics.launch index e1ce8122..96ef70d9 100755 --- a/submitted_models/ssci_x4_sensor_config_1/launch/vehicle_topics.launch +++ b/submitted_models/ssci_x4_sensor_config_1/launch/vehicle_topics.launch @@ -97,6 +97,11 @@ args="$(arg sensor_prefix)/front_laser/scan/points@sensor_msgs/PointCloud2[ignition.msgs.PointCloudPacked"> + + + + + @@ -168,6 +173,11 @@ + + + + + @@ -180,6 +190,11 @@ args="$(arg sensor_prefix)/front_laser/scan@sensor_msgs/LaserScan[ignition.msgs.LaserScan"> + + + + + @@ -191,6 +206,11 @@ args="$(arg sensor_prefix)/top_laser/scan@sensor_msgs/LaserScan[ignition.msgs.LaserScan"> + + + + + @@ -202,6 +222,11 @@ args="$(arg sensor_prefix)/bottom_laser/scan@sensor_msgs/LaserScan[ignition.msgs.LaserScan"> + + + + + diff --git a/submitted_models/ssci_x4_sensor_config_1/package.xml b/submitted_models/ssci_x4_sensor_config_1/package.xml index d9700111..f1c82e75 100644 --- a/submitted_models/ssci_x4_sensor_config_1/package.xml +++ b/submitted_models/ssci_x4_sensor_config_1/package.xml @@ -13,8 +13,10 @@ BSD catkin - roslaunch - urdf - xacro + + roslaunch + subt_ros + urdf + xacro diff --git a/subt_msgs/CMakeLists.txt b/subt_msgs/CMakeLists.txt index ce0054a5..5eec3a01 100644 --- a/subt_msgs/CMakeLists.txt +++ b/subt_msgs/CMakeLists.txt @@ -12,6 +12,7 @@ add_service_files( DatagramRos.srv Bind.srv SetPose.srv + SetRate.srv PoseFromArtifact.srv Register.srv Unregister.srv diff --git a/subt_msgs/srv/SetRate.srv b/subt_msgs/srv/SetRate.srv new file mode 100644 index 00000000..bc8ce1e0 --- /dev/null +++ b/subt_msgs/srv/SetRate.srv @@ -0,0 +1,2 @@ +float64 rate +--- diff --git a/subt_ros/CMakeLists.txt b/subt_ros/CMakeLists.txt index 5f2c48ce..3edd7df2 100644 --- a/subt_ros/CMakeLists.txt +++ b/subt_ros/CMakeLists.txt @@ -102,8 +102,20 @@ target_link_libraries(set_pose_relay ) add_dependencies(set_pose_relay ${catkin_EXPORTED_TARGETS}) +add_executable(set_rate_relay src/SetRateRelay.cc) +target_include_directories(set_rate_relay + PRIVATE ${CATKIN_DEVEL_PREFIX}/include) +target_link_libraries(set_rate_relay + PUBLIC + ignition-common3::ignition-common3 + ignition-msgs6::ignition-msgs6 + ignition-transport9::ignition-transport9 + ${catkin_LIBRARIES} +) +add_dependencies(set_rate_relay ${catkin_EXPORTED_TARGETS}) + install(TARGETS pose_tf_broadcaster subt_ros_relay set_pose_relay bridge_logger - optical_frame_publisher + optical_frame_publisher set_rate_relay DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) install(DIRECTORY launch diff --git a/subt_ros/launch/models_common/set_rate_relay.launch b/subt_ros/launch/models_common/set_rate_relay.launch new file mode 100644 index 00000000..c0cf4150 --- /dev/null +++ b/subt_ros/launch/models_common/set_rate_relay.launch @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/subt_ros/launch/vehicle_topics.launch b/subt_ros/launch/vehicle_topics.launch index dd03a228..a6ed8e45 100644 --- a/subt_ros/launch/vehicle_topics.launch +++ b/subt_ros/launch/vehicle_topics.launch @@ -79,6 +79,11 @@ + + + + + @@ -91,6 +96,11 @@ args="$(arg sensor_prefix)/front_laser/scan/points@sensor_msgs/PointCloud2[ignition.msgs.PointCloudPacked"> + + + + + @@ -133,6 +143,16 @@ + + + + + + + + + + @@ -162,6 +182,11 @@ + + + + + @@ -174,6 +199,11 @@ args="$(arg sensor_prefix)/front_laser/scan@sensor_msgs/LaserScan[ignition.msgs.LaserScan"> + + + + + diff --git a/subt_ros/src/SetRateRelay.cc b/subt_ros/src/SetRateRelay.cc new file mode 100644 index 00000000..83939549 --- /dev/null +++ b/subt_ros/src/SetRateRelay.cc @@ -0,0 +1,140 @@ +/* + * Copyright (C) 2019 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include + +#include + +#include + +#include +#include + +/// \brief This is a Ign-ROS relay for set_rate service for cameras +/// (added in https://github.com/ignitionrobotics/ign-sensors/pull/95). +/// +/// ROS parameters: +/// - ~service string Full name of the Ignition set_rate service. +/// - ~timeout uint Timeout for calling the service (in ms). Default is 1000. +/// +/// Advertised ROS services: +/// - ~${service} (subt_msgs/SetRate) Set rate of the connected camera to the given +/// value. The ROS service is named the same as +/// the Ignition service it is relayed to. +class SetRateRelay +{ + /// \brief Constructor + public: SetRateRelay(); + + /// \brief Destructor + public: ~SetRateRelay() = default; + + /// \brief ROS service callback triggered when the service is called. + /// \param[in] _req The message containing the desired publishing rate for + /// a camera. + /// \param[out] _res The response message. + public: bool OnSetRateCall(subt_msgs::SetRate::Request &_req, + subt_msgs::SetRate::Response &_res); + + /// \brief Ignition Transport node. + public: ignition::transport::Node node; + + /// \brief The ROS node handler used for private communications. + public: ros::NodeHandle pnh; + + /// \brief ROS service to receive a call to set camera rate. + public: ros::ServiceServer setRateService; + + /// \brief Name of the set_rate service in both Ignition Transport and ROS. + public: std::string serviceName; +}; + +////////////////////////////////////////////////// +SetRateRelay::SetRateRelay() : pnh("~") +{ + if (!this->pnh.getParam("service", this->serviceName)) { + ROS_ERROR("Cannot operate without parameter '~service' set to the path to the Ignition " + "Transport service set_rate for the camera."); + ros::shutdown(); + return; + } + std::string topicStats; + ignition::common::env("IGN_TRANSPORT_TOPIC_STATISTICS", topicStats, true); + + std::vector services; + bool firstLoop {true}; + bool waitedAtLeastOnce = false; + while (ros::ok() && std::find(services.begin(), services.end(), this->serviceName) == services.end()) + { + if (firstLoop) + { + firstLoop = false; + } + else + { + std::string topicStatsWarning; + if (topicStats == "1" && waitedAtLeastOnce) + { + topicStatsWarning = " IGN_TRANSPORT_TOPIC_STATISTICS is set to 1. Make sure all parts of the simulator " + "run with this setting, otherwise the parts with different values of this variable " + "would not be able to communicate with each other."; + } + + ROS_WARN_STREAM_DELAYED_THROTTLE(60.0, + "Waiting for ignition service " << this->serviceName << " to appear." << topicStatsWarning); + + ros::WallDuration(1, 0).sleep(); + waitedAtLeastOnce = true; + } + this->node.ServiceList(services); + } + + if (!ros::ok()) + return; + + if (waitedAtLeastOnce) + ROS_INFO_STREAM("Ignition Service " << this->serviceName << " found."); + + this->setRateService = this->pnh.advertiseService( + this->serviceName, &SetRateRelay::OnSetRateCall, this); + + ROS_INFO_STREAM("Started set_rate relay from Ign service " << this->serviceName + << " to ROS service " << this->setRateService.getService()); +} + +///////////////////////////////////////////////// +bool SetRateRelay::OnSetRateCall(subt_msgs::SetRate::Request &_req, + subt_msgs::SetRate::Response &_res) +{ + ignition::msgs::Double req; + req.set_data(_req.rate); + + ROS_DEBUG_STREAM("Relaying " << this->setRateService.getService() << ": " << _req); + + // Pass the request onto ignition transport + return this->node.Request(this->serviceName, req); +} + +////////////////////////////////////////////////// +int main(int argc, char * argv[]) +{ + ros::init(argc, argv, "subt_set_rate_relay"); + + SetRateRelay relay; + + ros::spin(); + return 0; +}