From 5e65797126ee6eae9ce3f140c3a46d14339d07dc Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 13 Apr 2020 20:40:50 -0700 Subject: [PATCH 1/5] check topic heartbeat and log /X*/pose topics --- subt_ros/src/BridgeLogger.cc | 47 +++++++++++++++++++++++++++++++++++- 1 file changed, 46 insertions(+), 1 deletion(-) diff --git a/subt_ros/src/BridgeLogger.cc b/subt_ros/src/BridgeLogger.cc index afea780c..4cdb3ff6 100644 --- a/subt_ros/src/BridgeLogger.cc +++ b/subt_ros/src/BridgeLogger.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -69,6 +70,11 @@ class BridgeLogger /// \brief Timer that triggers the update function. private: ros::Timer updateTimer; + + private: std::map> topicTimestamp; + + private: std::chrono::time_point lastHeartbeatTime; }; ///////////////////////////////////////////////// @@ -97,6 +103,7 @@ void BridgeLogger::Update(const ros::TimerEvent &) // Skip if the topic has already been added to `this->streams` or if // the topic is one that we don't monitor, such as parameter updates. + if (this->streams.find(info.name) != this->streams.end() || info.name.find("parameter_descriptions") != std::string::npos || info.name.find("parameter_updates") != std::string::npos || @@ -115,7 +122,8 @@ void BridgeLogger::Update(const ros::TimerEvent &) info.name.find("magnetic_field") != std::string::npos || info.name.find("air_pressure") != std::string::npos || info.name.find("battery_state") != std::string::npos || - info.name.find("imu") != std::string::npos) + info.name.find("imu") != std::string::npos || + info.name.find("pose") != std::string::npos) { boost::function< void(const topic_tools::ShapeShifter::ConstPtr&)> callback; @@ -150,6 +158,7 @@ void BridgeLogger::Update(const ros::TimerEvent &) this->subscribers.push_back( this->n.subscribe(info.name, 1000, callback)); } + } } @@ -204,6 +213,11 @@ void BridgeLogger::OnSensorMsg(const topic_tools::ShapeShifter::ConstPtr &_msg, const auto msg = _msg->instantiate(); seq = msg->header.seq; } + else if (_msg->getDataType() == "geometry_msgs/TransformStamped") + { + const auto msg = _msg->instantiate(); + seq = msg->header.seq; + } else { // Debug output. @@ -211,6 +225,35 @@ void BridgeLogger::OnSensorMsg(const topic_tools::ShapeShifter::ConstPtr &_msg, return; } + // check heartbeat + auto tdiff = systemTime - this->lastHeartbeatTime; + auto seconds = + std::chrono::duration_cast(tdiff).count() / + 1000.0; + if (seconds > 5.0) + { + // check topic heartbeat + for (auto &tIt : this->streams) + { + std::string topic = tIt.first; + auto it = this->topicTimestamp.find(topic); + if (it != this->topicTimestamp.end()) + { + auto d = systemTime - it->second; + seconds = std::chrono::duration_cast(d).count() / + 1000.0; + if (seconds > 5.0) + { + this->streams[topic] << "***Error: No messages received for more than " + << "5 seconds ***\n"; + } + } + } + this->lastHeartbeatTime = systemTime; + } + // log msg time received in wall clock time + this->topicTimestamp[_topic] = systemTime; + // Check if a message was missed. if (this->prevSeq[_topic] + 1 != seq && this->prevSeq[_topic] >= 0) this->streams[_topic] << "***Error: Missed message(s) ***\n"; @@ -223,6 +266,8 @@ void BridgeLogger::OnSensorMsg(const topic_tools::ShapeShifter::ConstPtr &_msg, << diff.count() << std::endl; this->prevSeq[_topic] = seq; + + } ///////////////////////////////////////////////// From 4c4c22c305839f789bbd91dc417602a0c5997fec Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 16 Apr 2020 20:14:12 -0700 Subject: [PATCH 2/5] use pose_v and subt to pose_static --- ign_migration_scripts/launch/preview.ign | 6 ++++++ .../scripts/spawn_preview_model.py | 3 +++ .../launch/spawner.rb | 3 +++ .../launch/vehicle_topics.launch | 9 +++++++- .../launch/spawner.rb | 3 +++ .../launch/vehicle_topics.launch | 10 +++++++-- .../launch/spawner.rb | 3 +++ .../launch/vehicle_topics.launch | 9 +++++++- .../launch/spawner.rb | 3 +++ .../launch/vehicle_topics.launch | 9 +++++++- .../launch/spawner.rb | 3 +++ .../launch/vehicle_topics.launch | 9 +++++++- .../ssci_x2_sensor_config_1/launch/spawner.rb | 3 +++ .../launch/vehicle_topics.launch | 9 +++++++- .../ssci_x4_sensor_config_1/launch/spawner.rb | 3 +++ .../launch/vehicle_topics.launch | 9 +++++++- .../ssci_x4_sensor_config_2/launch/spawner.rb | 3 +++ .../launch/vehicle_topics.launch | 9 +++++++- .../x1_sensor_config_6/launch/spawner.rb | 3 +++ .../launch/vehicle_topics.launch | 9 +++++++- .../x2_sensor_config_7/launch/spawner.rb | 3 +++ .../launch/vehicle_topics.launch | 9 +++++++- .../x3_sensor_config_5/launch/spawner.rb | 3 +++ .../launch/vehicle_topics.launch | 9 +++++++- .../x4_sensor_config_6/launch/spawner.rb | 3 +++ .../launch/vehicle_topics.launch | 9 +++++++- subt_ign/launch/cave_circuit.ign | 12 +++++++++++ subt_ign/launch/cloudsim_sim.ign | 12 +++++++++++ subt_ign/launch/competition.ign | 12 +++++++++++ subt_ign/launch/competition_no_ros.ign | 12 +++++++++++ subt_ign/launch/tunnel_circuit_practice.ign | 12 +++++++++++ subt_ign/launch/urban_circuit.ign | 12 +++++++++++ subt_ign/launch/validate_connections.ign | 3 +++ subt_ign/launch/virtual_stix.ign | 12 +++++++++++ subt_ign/launch/virtual_stix_headless.ign | 12 +++++++++++ subt_ign/test/test.ign | 12 +++++++++++ subt_ros/launch/vehicle_topics.launch | 9 +++++++- subt_ros/src/BridgeLogger.cc | 3 +-- subt_ros/src/pose_tf_broadcaster.cpp | 21 ++++++++++++++----- 39 files changed, 277 insertions(+), 21 deletions(-) diff --git a/ign_migration_scripts/launch/preview.ign b/ign_migration_scripts/launch/preview.ign index 56ac8797..eef598ea 100644 --- a/ign_migration_scripts/launch/preview.ign +++ b/ign_migration_scripts/launch/preview.ign @@ -210,6 +210,9 @@ false false #{$enableGroundTruth} + true + true + 1 @@ -256,6 +259,9 @@ false false #{$enableGroundTruth} + true + true + 1 diff --git a/ign_migration_scripts/scripts/spawn_preview_model.py b/ign_migration_scripts/scripts/spawn_preview_model.py index 7d8eebfc..511b2d1e 100755 --- a/ign_migration_scripts/scripts/spawn_preview_model.py +++ b/ign_migration_scripts/scripts/spawn_preview_model.py @@ -74,6 +74,9 @@ def spawn_preview_model(args): etree.SubElement(plugin, 'publish_collision_pose').text = 'false' etree.SubElement(plugin, 'publish_visual_pose').text = 'false' etree.SubElement(plugin, 'publish_nested_model_pose').text = 'true' + etree.SubElement(plugin, 'use_pose_vector_msg').text = 'true' + etree.SubElement(plugin, 'static_pose_publisher').text = 'true' + etree.SubElement(plugin, 'static_pose_update_frequency').text = '1' sdf_esc = etree.tostring(sdf, encoding="unicode", with_tail=False).replace('"', r'\"').replace('\n', '') diff --git a/submitted_models/costar_husky_sensor_config_1/launch/spawner.rb b/submitted_models/costar_husky_sensor_config_1/launch/spawner.rb index 0b481bc2..b8eba806 100644 --- a/submitted_models/costar_husky_sensor_config_1/launch/spawner.rb +++ b/submitted_models/costar_husky_sensor_config_1/launch/spawner.rb @@ -30,6 +30,9 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) false false #{$enableGroundTruth} + true + true + 1 + args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V"> + + + false false #{$enableGroundTruth} + true + true + 1 + args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V"> - + + + false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " + args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V"> + + + false false #{$enableGroundTruth} + true + true + 1 + args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V"> + + + false false #{$enableGroundTruth} + true + true + 1 diff --git a/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/vehicle_topics.launch index ad1c2784..fe30a5eb 100644 --- a/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/vehicle_topics.launch +++ b/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/vehicle_topics.launch @@ -18,9 +18,16 @@ pkg="ros_ign_bridge" type="parameter_bridge" name="ros_ign_bridge_pose" - args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose"> + args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V"> + + + false false #{$enableGroundTruth} + true + true + 1 + args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V"> + + + false false #{$enableGroundTruth} + true + true + 1 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 25758838..9e82ad3f 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 @@ -34,9 +34,16 @@ pkg="ros_ign_bridge" type="parameter_bridge" name="ros_ign_bridge_pose" - args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose"> + args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V"> + + + false false #{$enableGroundTruth} + true + true + 1 diff --git a/submitted_models/ssci_x4_sensor_config_2/launch/vehicle_topics.launch b/submitted_models/ssci_x4_sensor_config_2/launch/vehicle_topics.launch index 04651d28..a59e43f0 100755 --- a/submitted_models/ssci_x4_sensor_config_2/launch/vehicle_topics.launch +++ b/submitted_models/ssci_x4_sensor_config_2/launch/vehicle_topics.launch @@ -34,9 +34,16 @@ pkg="ros_ign_bridge" type="parameter_bridge" name="ros_ign_bridge_pose" - args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose"> + args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V"> + + + false false #{$enableGroundTruth} + true + true + 1 + args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V"> + + + false false #{$enableGroundTruth} + true + true + 1 + args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V"> + + + false false #{$enableGroundTruth} + true + true + 1 diff --git a/submitted_models/x3_sensor_config_5/launch/vehicle_topics.launch b/submitted_models/x3_sensor_config_5/launch/vehicle_topics.launch index 69dd96fc..e8fda481 100644 --- a/submitted_models/x3_sensor_config_5/launch/vehicle_topics.launch +++ b/submitted_models/x3_sensor_config_5/launch/vehicle_topics.launch @@ -24,9 +24,16 @@ pkg="ros_ign_bridge" type="parameter_bridge" name="ros_ign_bridge_pose" - args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose"> + args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V"> + + + false false #{$enableGroundTruth} + true + true + 1 diff --git a/submitted_models/x4_sensor_config_6/launch/vehicle_topics.launch b/submitted_models/x4_sensor_config_6/launch/vehicle_topics.launch index 69dd96fc..e8fda481 100644 --- a/submitted_models/x4_sensor_config_6/launch/vehicle_topics.launch +++ b/submitted_models/x4_sensor_config_6/launch/vehicle_topics.launch @@ -24,9 +24,16 @@ pkg="ros_ign_bridge" type="parameter_bridge" name="ros_ign_bridge_pose" - args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose"> + args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V"> + + + false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -794,6 +803,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ign/launch/cloudsim_sim.ign b/subt_ign/launch/cloudsim_sim.ign index 0d84ce32..95d0a6a5 100644 --- a/subt_ign/launch/cloudsim_sim.ign +++ b/subt_ign/launch/cloudsim_sim.ign @@ -518,6 +518,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -800,6 +809,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ign/launch/competition.ign b/subt_ign/launch/competition.ign index 90fd87dc..be275080 100644 --- a/subt_ign/launch/competition.ign +++ b/subt_ign/launch/competition.ign @@ -432,6 +432,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -744,6 +753,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ign/launch/competition_no_ros.ign b/subt_ign/launch/competition_no_ros.ign index f829fe12..b07a1e1d 100644 --- a/subt_ign/launch/competition_no_ros.ign +++ b/subt_ign/launch/competition_no_ros.ign @@ -431,6 +431,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -725,6 +734,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ign/launch/tunnel_circuit_practice.ign b/subt_ign/launch/tunnel_circuit_practice.ign index e1779cce..1979f024 100644 --- a/subt_ign/launch/tunnel_circuit_practice.ign +++ b/subt_ign/launch/tunnel_circuit_practice.ign @@ -424,6 +424,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -736,6 +745,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ign/launch/urban_circuit.ign b/subt_ign/launch/urban_circuit.ign index e9afb7d4..e277a704 100644 --- a/subt_ign/launch/urban_circuit.ign +++ b/subt_ign/launch/urban_circuit.ign @@ -448,6 +448,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -790,6 +799,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ign/launch/validate_connections.ign b/subt_ign/launch/validate_connections.ign index 4befbf4a..ac907fe7 100644 --- a/subt_ign/launch/validate_connections.ign +++ b/subt_ign/launch/validate_connections.ign @@ -385,6 +385,9 @@ ign service -s /connection/prev \ false false true + true + true + 1 diff --git a/subt_ign/launch/virtual_stix.ign b/subt_ign/launch/virtual_stix.ign index 9e692d1b..926f4782 100644 --- a/subt_ign/launch/virtual_stix.ign +++ b/subt_ign/launch/virtual_stix.ign @@ -394,6 +394,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -706,6 +715,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ign/launch/virtual_stix_headless.ign b/subt_ign/launch/virtual_stix_headless.ign index 21ef5ac9..614c1441 100644 --- a/subt_ign/launch/virtual_stix_headless.ign +++ b/subt_ign/launch/virtual_stix_headless.ign @@ -306,6 +306,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -618,6 +627,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ign/test/test.ign b/subt_ign/test/test.ign index fcbdd917..cd35e0f6 100644 --- a/subt_ign/test/test.ign +++ b/subt_ign/test/test.ign @@ -325,6 +325,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -565,6 +574,9 @@ " false\n"\ " false\n"\ " #{$enableGroundTruth}\n"\ + " true\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ros/launch/vehicle_topics.launch b/subt_ros/launch/vehicle_topics.launch index 81c59326..8e76f2f8 100644 --- a/subt_ros/launch/vehicle_topics.launch +++ b/subt_ros/launch/vehicle_topics.launch @@ -23,9 +23,16 @@ pkg="ros_ign_bridge" type="parameter_bridge" name="ros_ign_bridge_pose" - args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose"> + args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V"> + + + callback; diff --git a/subt_ros/src/pose_tf_broadcaster.cpp b/subt_ros/src/pose_tf_broadcaster.cpp index 6833bd04..c5242309 100644 --- a/subt_ros/src/pose_tf_broadcaster.cpp +++ b/subt_ros/src/pose_tf_broadcaster.cpp @@ -13,8 +13,6 @@ // limitations under the License. #include -#include -#include // include ROS 1 #ifdef __clang__ @@ -27,21 +25,34 @@ #endif #include -#include +#include +#include +#include -void poseCallback(const geometry_msgs::TransformStamped& msg) +void poseCallback(const tf2_msgs::TFMessage &_msg) { static tf2_ros::TransformBroadcaster br; - br.sendTransform(msg); + br.sendTransform(_msg.transforms); +} + +void poseStaticCallback(const tf2_msgs::TFMessage &_msg) +{ + static tf2_ros::StaticTransformBroadcaster brStatic; + brStatic.sendTransform(_msg.transforms); } ////////////////////////////////////////////////// int main(int argc, char * argv[]) { ros::init(argc, argv, "pose_tf_broadcaster"); + ros::NodeHandle node; ros::Subscriber sub = node.subscribe("pose", 10, &poseCallback); + ros::Subscriber subStatic = node.subscribe( + "pose_static", 10, &poseStaticCallback); + ros::spin(); + return 0; } From 7c764dd0c535e724ef4629bdb8b7b25da12ba042 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 16 Apr 2020 21:02:10 -0700 Subject: [PATCH 3/5] cleanup --- subt_ros/src/BridgeLogger.cc | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/subt_ros/src/BridgeLogger.cc b/subt_ros/src/BridgeLogger.cc index 7186b460..dff50805 100644 --- a/subt_ros/src/BridgeLogger.cc +++ b/subt_ros/src/BridgeLogger.cc @@ -19,7 +19,6 @@ #include #include #include -#include #include #include #include @@ -212,11 +211,6 @@ void BridgeLogger::OnSensorMsg(const topic_tools::ShapeShifter::ConstPtr &_msg, const auto msg = _msg->instantiate(); seq = msg->header.seq; } - else if (_msg->getDataType() == "geometry_msgs/TransformStamped") - { - const auto msg = _msg->instantiate(); - seq = msg->header.seq; - } else { // Debug output. @@ -239,12 +233,13 @@ void BridgeLogger::OnSensorMsg(const topic_tools::ShapeShifter::ConstPtr &_msg, if (it != this->topicTimestamp.end()) { auto d = systemTime - it->second; - seconds = std::chrono::duration_cast(d).count() / - 1000.0; + seconds = + std::chrono::duration_cast(d).count() / + 1000.0; if (seconds > 5.0) { - this->streams[topic] << "***Error: No messages received for more than " - << "5 seconds ***\n"; + this->streams[topic] << "***Error: No messages received for more than" + << " 5 seconds ***\n"; } } } @@ -265,8 +260,6 @@ void BridgeLogger::OnSensorMsg(const topic_tools::ShapeShifter::ConstPtr &_msg, << diff.count() << std::endl; this->prevSeq[_topic] = seq; - - } ///////////////////////////////////////////////// From 8db2b42e33e56e14679193b327b381e38e68acbd Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 17 Apr 2020 06:25:12 -0700 Subject: [PATCH 4/5] Rename parameters --- ign_migration_scripts/launch/preview.ign | 8 ++++---- .../scripts/spawn_preview_model.py | 4 ++-- .../launch/spawner.rb | 4 ++-- .../launch/spawner.rb | 4 ++-- .../launch/spawner.rb | 4 ++-- .../launch/spawner.rb | 4 ++-- .../launch/spawner.rb | 4 ++-- .../ssci_x2_sensor_config_1/launch/spawner.rb | 4 ++-- .../ssci_x4_sensor_config_1/launch/spawner.rb | 4 ++-- .../ssci_x4_sensor_config_2/launch/spawner.rb | 4 ++-- .../x1_sensor_config_6/launch/spawner.rb | 4 ++-- .../x2_sensor_config_7/launch/spawner.rb | 4 ++-- .../x3_sensor_config_5/launch/spawner.rb | 4 ++-- .../x4_sensor_config_6/launch/spawner.rb | 4 ++-- subt_ign/launch/cave_circuit.ign | 16 ++++++++-------- subt_ign/launch/cloudsim_sim.ign | 16 ++++++++-------- subt_ign/launch/competition.ign | 16 ++++++++-------- subt_ign/launch/competition_no_ros.ign | 16 ++++++++-------- subt_ign/launch/tunnel_circuit_practice.ign | 16 ++++++++-------- subt_ign/launch/urban_circuit.ign | 16 ++++++++-------- subt_ign/launch/validate_connections.ign | 4 ++-- subt_ign/launch/virtual_stix.ign | 16 ++++++++-------- subt_ign/launch/virtual_stix_headless.ign | 16 ++++++++-------- subt_ign/test/test.ign | 16 ++++++++-------- 24 files changed, 104 insertions(+), 104 deletions(-) diff --git a/ign_migration_scripts/launch/preview.ign b/ign_migration_scripts/launch/preview.ign index eef598ea..e0c13ca0 100644 --- a/ign_migration_scripts/launch/preview.ign +++ b/ign_migration_scripts/launch/preview.ign @@ -211,8 +211,8 @@ false #{$enableGroundTruth} true - true - 1 + true + 1 @@ -260,8 +260,8 @@ false #{$enableGroundTruth} true - true - 1 + true + 1 diff --git a/ign_migration_scripts/scripts/spawn_preview_model.py b/ign_migration_scripts/scripts/spawn_preview_model.py index 511b2d1e..5d64a928 100755 --- a/ign_migration_scripts/scripts/spawn_preview_model.py +++ b/ign_migration_scripts/scripts/spawn_preview_model.py @@ -75,8 +75,8 @@ def spawn_preview_model(args): etree.SubElement(plugin, 'publish_visual_pose').text = 'false' etree.SubElement(plugin, 'publish_nested_model_pose').text = 'true' etree.SubElement(plugin, 'use_pose_vector_msg').text = 'true' - etree.SubElement(plugin, 'static_pose_publisher').text = 'true' - etree.SubElement(plugin, 'static_pose_update_frequency').text = '1' + etree.SubElement(plugin, 'static_publisher').text = 'true' + etree.SubElement(plugin, 'static_update_frequency').text = '1' sdf_esc = etree.tostring(sdf, encoding="unicode", with_tail=False).replace('"', r'\"').replace('\n', '') diff --git a/submitted_models/costar_husky_sensor_config_1/launch/spawner.rb b/submitted_models/costar_husky_sensor_config_1/launch/spawner.rb index b8eba806..ca4c1fec 100644 --- a/submitted_models/costar_husky_sensor_config_1/launch/spawner.rb +++ b/submitted_models/costar_husky_sensor_config_1/launch/spawner.rb @@ -31,8 +31,8 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) false #{$enableGroundTruth} true - true - 1 + true + 1 false #{$enableGroundTruth} true - true - 1 + true + 1 false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false #{$enableGroundTruth} true - true - 1 + true + 1 false #{$enableGroundTruth} true - true - 1 + true + 1 diff --git a/submitted_models/ssci_x2_sensor_config_1/launch/spawner.rb b/submitted_models/ssci_x2_sensor_config_1/launch/spawner.rb index f7cd9e45..18e94253 100755 --- a/submitted_models/ssci_x2_sensor_config_1/launch/spawner.rb +++ b/submitted_models/ssci_x2_sensor_config_1/launch/spawner.rb @@ -45,8 +45,8 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) false #{$enableGroundTruth} true - true - 1 + true + 1 false #{$enableGroundTruth} true - true - 1 + true + 1 diff --git a/submitted_models/ssci_x4_sensor_config_2/launch/spawner.rb b/submitted_models/ssci_x4_sensor_config_2/launch/spawner.rb index 7635a912..b1155949 100755 --- a/submitted_models/ssci_x4_sensor_config_2/launch/spawner.rb +++ b/submitted_models/ssci_x4_sensor_config_2/launch/spawner.rb @@ -39,8 +39,8 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) false #{$enableGroundTruth} true - true - 1 + true + 1 diff --git a/submitted_models/x1_sensor_config_6/launch/spawner.rb b/submitted_models/x1_sensor_config_6/launch/spawner.rb index 668856f7..87ccf702 100644 --- a/submitted_models/x1_sensor_config_6/launch/spawner.rb +++ b/submitted_models/x1_sensor_config_6/launch/spawner.rb @@ -31,8 +31,8 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) false #{$enableGroundTruth} true - true - 1 + true + 1 false #{$enableGroundTruth} true - true - 1 + true + 1 false #{$enableGroundTruth} true - true - 1 + true + 1 diff --git a/submitted_models/x4_sensor_config_6/launch/spawner.rb b/submitted_models/x4_sensor_config_6/launch/spawner.rb index f1f1dbc9..7b201847 100644 --- a/submitted_models/x4_sensor_config_6/launch/spawner.rb +++ b/submitted_models/x4_sensor_config_6/launch/spawner.rb @@ -20,8 +20,8 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) false #{$enableGroundTruth} true - true - 1 + true + 1 diff --git a/subt_ign/launch/cave_circuit.ign b/subt_ign/launch/cave_circuit.ign index 400df881..bc6e61da 100644 --- a/subt_ign/launch/cave_circuit.ign +++ b/subt_ign/launch/cave_circuit.ign @@ -453,8 +453,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -804,8 +804,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ign/launch/cloudsim_sim.ign b/subt_ign/launch/cloudsim_sim.ign index 95d0a6a5..d7aa3d90 100644 --- a/subt_ign/launch/cloudsim_sim.ign +++ b/subt_ign/launch/cloudsim_sim.ign @@ -519,8 +519,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -810,8 +810,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ign/launch/competition.ign b/subt_ign/launch/competition.ign index be275080..c9ed0698 100644 --- a/subt_ign/launch/competition.ign +++ b/subt_ign/launch/competition.ign @@ -433,8 +433,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -754,8 +754,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ign/launch/competition_no_ros.ign b/subt_ign/launch/competition_no_ros.ign index b07a1e1d..66f02656 100644 --- a/subt_ign/launch/competition_no_ros.ign +++ b/subt_ign/launch/competition_no_ros.ign @@ -432,8 +432,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -735,8 +735,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ign/launch/tunnel_circuit_practice.ign b/subt_ign/launch/tunnel_circuit_practice.ign index 1979f024..ff687404 100644 --- a/subt_ign/launch/tunnel_circuit_practice.ign +++ b/subt_ign/launch/tunnel_circuit_practice.ign @@ -425,8 +425,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -746,8 +746,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ign/launch/urban_circuit.ign b/subt_ign/launch/urban_circuit.ign index e277a704..3b521d2d 100644 --- a/subt_ign/launch/urban_circuit.ign +++ b/subt_ign/launch/urban_circuit.ign @@ -449,8 +449,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -800,8 +800,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ign/launch/validate_connections.ign b/subt_ign/launch/validate_connections.ign index ac907fe7..d0ed7fbe 100644 --- a/subt_ign/launch/validate_connections.ign +++ b/subt_ign/launch/validate_connections.ign @@ -386,8 +386,8 @@ ign service -s /connection/prev \ false true true - true - 1 + true + 1 diff --git a/subt_ign/launch/virtual_stix.ign b/subt_ign/launch/virtual_stix.ign index 926f4782..ffb03255 100644 --- a/subt_ign/launch/virtual_stix.ign +++ b/subt_ign/launch/virtual_stix.ign @@ -395,8 +395,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -716,8 +716,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ign/launch/virtual_stix_headless.ign b/subt_ign/launch/virtual_stix_headless.ign index 614c1441..d831873c 100644 --- a/subt_ign/launch/virtual_stix_headless.ign +++ b/subt_ign/launch/virtual_stix_headless.ign @@ -307,8 +307,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -628,8 +628,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ diff --git a/subt_ign/test/test.ign b/subt_ign/test/test.ign index cd35e0f6..d814e78f 100644 --- a/subt_ign/test/test.ign +++ b/subt_ign/test/test.ign @@ -326,8 +326,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ @@ -575,8 +575,8 @@ " false\n"\ " #{$enableGroundTruth}\n"\ " true\n"\ - " true\n"\ - " 1\n"\ + " true\n"\ + " 1\n"\ " \n"\ " \n"\ From 296de9e804e58409787808649805d747e62110e2 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 17 Apr 2020 12:12:12 -0700 Subject: [PATCH 5/5] move heartbeat check into its own thread --- subt_ros/src/BridgeLogger.cc | 74 +++++++++++++++++++++--------------- 1 file changed, 44 insertions(+), 30 deletions(-) diff --git a/subt_ros/src/BridgeLogger.cc b/subt_ros/src/BridgeLogger.cc index dff50805..492a9ebe 100644 --- a/subt_ros/src/BridgeLogger.cc +++ b/subt_ros/src/BridgeLogger.cc @@ -16,6 +16,7 @@ */ #include #include +#include #include #include #include @@ -51,6 +52,9 @@ class BridgeLogger private: void OnSensorMsg(const topic_tools::ShapeShifter::ConstPtr &_msg, const std::string &_topic); + /// \brief Check heartbeat for all topics monitored + private: void CheckHeartbeat(); + /// \brief ROS node handler. private: ros::NodeHandle n; @@ -70,10 +74,13 @@ class BridgeLogger /// \brief Timer that triggers the update function. private: ros::Timer updateTimer; + /// \brief A map of topic names and time when their last heartbeat was + /// received. private: std::map> topicTimestamp; - private: std::chrono::time_point lastHeartbeatTime; + /// \brief Check heartbeat in separate thread + private: std::thread heartbeatThread; }; ///////////////////////////////////////////////// @@ -84,6 +91,8 @@ BridgeLogger::BridgeLogger() this->updateTimer = this->n.createTimer(ros::Duration(10.0), &BridgeLogger::Update, this); + + this->heartbeatThread = std::thread(&BridgeLogger::CheckHeartbeat, this); } ///////////////////////////////////////////////// @@ -102,7 +111,6 @@ void BridgeLogger::Update(const ros::TimerEvent &) // Skip if the topic has already been added to `this->streams` or if // the topic is one that we don't monitor, such as parameter updates. - if (this->streams.find(info.name) != this->streams.end() || info.name.find("parameter_descriptions") != std::string::npos || info.name.find("parameter_updates") != std::string::npos || @@ -156,7 +164,6 @@ void BridgeLogger::Update(const ros::TimerEvent &) this->subscribers.push_back( this->n.subscribe(info.name, 1000, callback)); } - } } @@ -218,33 +225,6 @@ void BridgeLogger::OnSensorMsg(const topic_tools::ShapeShifter::ConstPtr &_msg, return; } - // check heartbeat - auto tdiff = systemTime - this->lastHeartbeatTime; - auto seconds = - std::chrono::duration_cast(tdiff).count() / - 1000.0; - if (seconds > 5.0) - { - // check topic heartbeat - for (auto &tIt : this->streams) - { - std::string topic = tIt.first; - auto it = this->topicTimestamp.find(topic); - if (it != this->topicTimestamp.end()) - { - auto d = systemTime - it->second; - seconds = - std::chrono::duration_cast(d).count() / - 1000.0; - if (seconds > 5.0) - { - this->streams[topic] << "***Error: No messages received for more than" - << " 5 seconds ***\n"; - } - } - } - this->lastHeartbeatTime = systemTime; - } // log msg time received in wall clock time this->topicTimestamp[_topic] = systemTime; @@ -262,6 +242,40 @@ void BridgeLogger::OnSensorMsg(const topic_tools::ShapeShifter::ConstPtr &_msg, this->prevSeq[_topic] = seq; } +///////////////////////////////////////////////// +void BridgeLogger::CheckHeartbeat() +{ + // check heartbeat + using namespace std::chrono_literals; + double heartbeatCheckPeriod = 5.0; + while (ros::ok) + { + std::this_thread::sleep_for(5s); + auto systemTime = std::chrono::steady_clock::now(); + { + std::lock_guard lock(this->mutex); + // check topic heartbeat + for (auto &tIt : this->streams) + { + std::string topic = tIt.first; + auto it = this->topicTimestamp.find(topic); + if (it != this->topicTimestamp.end()) + { + auto d = systemTime - it->second; + auto seconds = + std::chrono::duration_cast(d).count() / + 1000.0; + if (seconds > heartbeatCheckPeriod) + { + this->streams[topic] << "***Error: No messages received for more " + << "than 5 seconds ***\n"; + } + } + } + } + } +} + ///////////////////////////////////////////////// int main(int argc, char** argv) {