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;
+}