From 7ce0ae7d1ed17c754c6f4f59ce257a6b210c1293 Mon Sep 17 00:00:00 2001 From: Aditya Date: Mon, 7 Mar 2022 10:08:48 -0800 Subject: [PATCH 01/18] Changed templates Signed-off-by: Aditya --- .../ros_ign_bridge/convert/geometry_msgs.hpp | 14 ++++++++ ros_ign_bridge/src/convert/geometry_msgs.cpp | 22 ++++++++++++ .../src/factories/geometry_msgs.cpp | 34 +++++++++++++++++++ ros_ign_bridge/src/parameter_bridge.cpp | 1 + 4 files changed, 71 insertions(+) diff --git a/ros_ign_bridge/include/ros_ign_bridge/convert/geometry_msgs.hpp b/ros_ign_bridge/include/ros_ign_bridge/convert/geometry_msgs.hpp index 89b1c9d6..06dc19de 100644 --- a/ros_ign_bridge/include/ros_ign_bridge/convert/geometry_msgs.hpp +++ b/ros_ign_bridge/include/ros_ign_bridge/convert/geometry_msgs.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -26,6 +27,7 @@ // ROS 2 messages #include #include +#include #include #include #include @@ -86,6 +88,18 @@ convert_ign_to_ros( const ignition::msgs::Pose & ign_msg, geometry_msgs::msg::Pose & ros_msg); +template<> +void +convert_ros_to_ign( + const geometry_msgs::msg::PoseWithCovariance & ros_msg, + ignition::msgs::PoseWithCovariance & ign_msg); + +template<> +void +convert_ign_to_ros( + const ignition::msgs::PoseWithCovariance & ign_msg, + geometry_msgs::msg::PoseWithCovariance & ros_msg); + template<> void convert_ros_to_ign( diff --git a/ros_ign_bridge/src/convert/geometry_msgs.cpp b/ros_ign_bridge/src/convert/geometry_msgs.cpp index 486e6ead..3a8dd136 100644 --- a/ros_ign_bridge/src/convert/geometry_msgs.cpp +++ b/ros_ign_bridge/src/convert/geometry_msgs.cpp @@ -106,6 +106,28 @@ convert_ign_to_ros( convert_ign_to_ros(ign_msg.orientation(), ros_msg.orientation); } +template<> +void +convert_ros_to_ign( + const geometry_msgs::msg::PoseWithCovariance & ros_msg, + ignition::msgs::PoseWithCovariance & ign_msg) +{ + std::cout << "dbg 1" << std::endl; + convert_ros_to_ign(ros_msg.pose.position, *ign_msg.mutable_pose()->mutable_position()); + convert_ros_to_ign(ros_msg.pose.orientation, *ign_msg.mutable_pose()->mutable_orientation()); + std::cout << "dbg 2" << std::endl; +} + +template<> +void +convert_ign_to_ros( + const ignition::msgs::PoseWithCovariance & ign_msg, + geometry_msgs::msg::PoseWithCovariance & ros_msg) +{ + convert_ign_to_ros(ign_msg.pose().position(), ros_msg.pose.position); + convert_ign_to_ros(ign_msg.pose().orientation(), ros_msg.pose.orientation); +} + template<> void convert_ros_to_ign( diff --git a/ros_ign_bridge/src/factories/geometry_msgs.cpp b/ros_ign_bridge/src/factories/geometry_msgs.cpp index 711cd5cb..b4915186 100644 --- a/ros_ign_bridge/src/factories/geometry_msgs.cpp +++ b/ros_ign_bridge/src/factories/geometry_msgs.cpp @@ -68,6 +68,16 @@ get_factory__geometry_msgs( > >("geometry_msgs/msg/Pose", ign_type_name); } + if ((ros_type_name == "geometry_msgs/msg/PoseWithCovariance" || ros_type_name.empty()) && + ign_type_name == "ignition.msgs.PoseWithCovariance") + { + return std::make_shared< + Factory< + geometry_msgs::msg::PoseWithCovariance, + ignition::msgs::PoseWithCovariance + > + >("geometry_msgs/msg/Pose", ign_type_name); + } if ((ros_type_name == "geometry_msgs/msg/PoseStamped" || ros_type_name.empty()) && ign_type_name == "ignition.msgs.Pose") { @@ -219,6 +229,30 @@ Factory< ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg); } +template<> +void +Factory< + geometry_msgs::msg::PoseWithCovariance, + ignition::msgs::PoseWithCovariance +>::convert_ros_to_ign( + const geometry_msgs::msg::PoseWithCovariance & ros_msg, + ignition::msgs::PoseWithCovariance & ign_msg) +{ + ros_ign_bridge::convert_ros_to_ign(ros_msg, ign_msg); +} + +template<> +void +Factory< + geometry_msgs::msg::PoseWithCovariance, + ignition::msgs::PoseWithCovariance +>::convert_ign_to_ros( + const ignition::msgs::PoseWithCovariance & ign_msg, + geometry_msgs::msg::PoseWithCovariance & ros_msg) +{ + ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg); +} + template<> void Factory< diff --git a/ros_ign_bridge/src/parameter_bridge.cpp b/ros_ign_bridge/src/parameter_bridge.cpp index 5680b145..81265c50 100644 --- a/ros_ign_bridge/src/parameter_bridge.cpp +++ b/ros_ign_bridge/src/parameter_bridge.cpp @@ -170,6 +170,7 @@ int main(int argc, char * argv[]) break; } } catch (std::runtime_error & _e) { + std::cout << _e.what() << std::endl; std::cerr << "Failed to create a bridge for topic [" << topic_name << "] " << "with ROS2 type [" << ros_type_name << "] and " << "Ignition Transport type [" << ign_type_name << "]" << std::endl; From 1403fc5bb6340ce279c87037b5db58c65f6ade5b Mon Sep 17 00:00:00 2001 From: Aditya Date: Mon, 7 Mar 2022 15:10:44 -0800 Subject: [PATCH 02/18] Removed debug statements Signed-off-by: Aditya --- ros_ign_bridge/src/convert/geometry_msgs.cpp | 2 -- ros_ign_bridge/src/factories/geometry_msgs.cpp | 2 +- ros_ign_bridge/src/parameter_bridge.cpp | 1 - 3 files changed, 1 insertion(+), 4 deletions(-) diff --git a/ros_ign_bridge/src/convert/geometry_msgs.cpp b/ros_ign_bridge/src/convert/geometry_msgs.cpp index 3a8dd136..cd245d21 100644 --- a/ros_ign_bridge/src/convert/geometry_msgs.cpp +++ b/ros_ign_bridge/src/convert/geometry_msgs.cpp @@ -112,10 +112,8 @@ convert_ros_to_ign( const geometry_msgs::msg::PoseWithCovariance & ros_msg, ignition::msgs::PoseWithCovariance & ign_msg) { - std::cout << "dbg 1" << std::endl; convert_ros_to_ign(ros_msg.pose.position, *ign_msg.mutable_pose()->mutable_position()); convert_ros_to_ign(ros_msg.pose.orientation, *ign_msg.mutable_pose()->mutable_orientation()); - std::cout << "dbg 2" << std::endl; } template<> diff --git a/ros_ign_bridge/src/factories/geometry_msgs.cpp b/ros_ign_bridge/src/factories/geometry_msgs.cpp index b4915186..a2683167 100644 --- a/ros_ign_bridge/src/factories/geometry_msgs.cpp +++ b/ros_ign_bridge/src/factories/geometry_msgs.cpp @@ -76,7 +76,7 @@ get_factory__geometry_msgs( geometry_msgs::msg::PoseWithCovariance, ignition::msgs::PoseWithCovariance > - >("geometry_msgs/msg/Pose", ign_type_name); + >("geometry_msgs/msg/PoseWithCovariance", ign_type_name); } if ((ros_type_name == "geometry_msgs/msg/PoseStamped" || ros_type_name.empty()) && ign_type_name == "ignition.msgs.Pose") diff --git a/ros_ign_bridge/src/parameter_bridge.cpp b/ros_ign_bridge/src/parameter_bridge.cpp index 81265c50..5680b145 100644 --- a/ros_ign_bridge/src/parameter_bridge.cpp +++ b/ros_ign_bridge/src/parameter_bridge.cpp @@ -170,7 +170,6 @@ int main(int argc, char * argv[]) break; } } catch (std::runtime_error & _e) { - std::cout << _e.what() << std::endl; std::cerr << "Failed to create a bridge for topic [" << topic_name << "] " << "with ROS2 type [" << ros_type_name << "] and " << "Ignition Transport type [" << ign_type_name << "]" << std::endl; From 20b24e85cd641123a651a802fe9d12635eda33ab Mon Sep 17 00:00:00 2001 From: Aditya Date: Mon, 7 Mar 2022 15:54:28 -0800 Subject: [PATCH 03/18] Added twist with covariance msg bridge Signed-off-by: Aditya --- .../ros_ign_bridge/convert/geometry_msgs.hpp | 14 ++++++++ ros_ign_bridge/src/convert/geometry_msgs.cpp | 20 +++++++++++ .../src/factories/geometry_msgs.cpp | 35 +++++++++++++++++++ 3 files changed, 69 insertions(+) diff --git a/ros_ign_bridge/include/ros_ign_bridge/convert/geometry_msgs.hpp b/ros_ign_bridge/include/ros_ign_bridge/convert/geometry_msgs.hpp index 06dc19de..8df05b14 100644 --- a/ros_ign_bridge/include/ros_ign_bridge/convert/geometry_msgs.hpp +++ b/ros_ign_bridge/include/ros_ign_bridge/convert/geometry_msgs.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include // ROS 2 messages @@ -32,6 +33,7 @@ #include #include #include +#include #include #include @@ -148,6 +150,18 @@ convert_ign_to_ros( const ignition::msgs::Twist & ign_msg, geometry_msgs::msg::Twist & ros_msg); +template<> +void +convert_ros_to_ign( + const geometry_msgs::msg::TwistWithCovariance & ros_msg, + ignition::msgs::TwistWithCovariance & ign_msg); + +template<> +void +convert_ign_to_ros( + const ignition::msgs::TwistWithCovariance & ign_msg, + geometry_msgs::msg::TwistWithCovariance & ros_msg); + template<> void convert_ros_to_ign( diff --git a/ros_ign_bridge/src/convert/geometry_msgs.cpp b/ros_ign_bridge/src/convert/geometry_msgs.cpp index cd245d21..d080de79 100644 --- a/ros_ign_bridge/src/convert/geometry_msgs.cpp +++ b/ros_ign_bridge/src/convert/geometry_msgs.cpp @@ -217,6 +217,26 @@ convert_ign_to_ros( convert_ign_to_ros(ign_msg.angular(), ros_msg.angular); } +template<> +void +convert_ros_to_ign( + const geometry_msgs::msg::TwistWithCovariance & ros_msg, + ignition::msgs::TwistWithCovariance & ign_msg) +{ + convert_ros_to_ign(ros_msg.twist.linear, (*ign_msg.mutable_twist()->mutable_linear())); + convert_ros_to_ign(ros_msg.twist.angular, (*ign_msg.mutable_twist()->mutable_angular())); +} + +template<> +void +convert_ign_to_ros( + const ignition::msgs::TwistWithCovariance & ign_msg, + geometry_msgs::msg::TwistWithCovariance & ros_msg) +{ + convert_ign_to_ros(ign_msg.twist().linear(), ros_msg.twist.linear); + convert_ign_to_ros(ign_msg.twist().angular(), ros_msg.twist.angular); +} + template<> void convert_ros_to_ign( diff --git a/ros_ign_bridge/src/factories/geometry_msgs.cpp b/ros_ign_bridge/src/factories/geometry_msgs.cpp index a2683167..48d7887a 100644 --- a/ros_ign_bridge/src/factories/geometry_msgs.cpp +++ b/ros_ign_bridge/src/factories/geometry_msgs.cpp @@ -119,6 +119,17 @@ get_factory__geometry_msgs( > >("geometry_msgs/msg/Twist", ign_type_name); } + if ( + (ros_type_name == "geometry_msgs/msg/TwistWithCovariance" || ros_type_name.empty()) && + ign_type_name == "ignition.msgs.TwistWithCovariance") + { + return std::make_shared< + Factory< + geometry_msgs::msg::TwistWithCovariance, + ignition::msgs::TwistWithCovariance + > + >("geometry_msgs/msg/TwistWithCovariance", ign_type_name); + } if ( (ros_type_name == "geometry_msgs/msg/Wrench" || ros_type_name.empty()) && ign_type_name == "ignition.msgs.Wrench") @@ -349,6 +360,30 @@ Factory< ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg); } +template<> +void +Factory< + geometry_msgs::msg::TwistWithCovariance, + ignition::msgs::TwistWithCovariance +>::convert_ros_to_ign( + const geometry_msgs::msg::TwistWithCovariance & ros_msg, + ignition::msgs::TwistWithCovariance & ign_msg) +{ + ros_ign_bridge::convert_ros_to_ign(ros_msg, ign_msg); +} + +template<> +void +Factory< + geometry_msgs::msg::TwistWithCovariance, + ignition::msgs::TwistWithCovariance +>::convert_ign_to_ros( + const ignition::msgs::TwistWithCovariance & ign_msg, + geometry_msgs::msg::TwistWithCovariance & ros_msg) +{ + ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg); +} + template<> void Factory< From 57681a3fe61d7d7582cdb84a648d9f50a90859bc Mon Sep 17 00:00:00 2001 From: Aditya Date: Mon, 7 Mar 2022 18:43:54 -0800 Subject: [PATCH 04/18] Populate cov matrix Signed-off-by: Aditya --- ros_ign_bridge/src/convert/geometry_msgs.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/ros_ign_bridge/src/convert/geometry_msgs.cpp b/ros_ign_bridge/src/convert/geometry_msgs.cpp index d080de79..142066be 100644 --- a/ros_ign_bridge/src/convert/geometry_msgs.cpp +++ b/ros_ign_bridge/src/convert/geometry_msgs.cpp @@ -114,6 +114,9 @@ convert_ros_to_ign( { convert_ros_to_ign(ros_msg.pose.position, *ign_msg.mutable_pose()->mutable_position()); convert_ros_to_ign(ros_msg.pose.orientation, *ign_msg.mutable_pose()->mutable_orientation()); + for (const auto & elem : ros_msg.covariance) { + ign_msg.mutable_covariance()->add_data(elem); + } } template<> @@ -124,6 +127,11 @@ convert_ign_to_ros( { convert_ign_to_ros(ign_msg.pose().position(), ros_msg.pose.position); convert_ign_to_ros(ign_msg.pose().orientation(), ros_msg.pose.orientation); + int data_size = ign_msg.covariance().data_size(); + for (int i = 0 ; i < data_size ; i++) { + auto data = ign_msg.covariance().data()[i]; + ros_msg.covariance[i] = data; + } } template<> @@ -225,6 +233,9 @@ convert_ros_to_ign( { convert_ros_to_ign(ros_msg.twist.linear, (*ign_msg.mutable_twist()->mutable_linear())); convert_ros_to_ign(ros_msg.twist.angular, (*ign_msg.mutable_twist()->mutable_angular())); + for (const auto & elem : ros_msg.covariance) { + ign_msg.mutable_covariance()->add_data(elem); + } } template<> @@ -235,6 +246,11 @@ convert_ign_to_ros( { convert_ign_to_ros(ign_msg.twist().linear(), ros_msg.twist.linear); convert_ign_to_ros(ign_msg.twist().angular(), ros_msg.twist.angular); + int data_size = ign_msg.covariance().data_size(); + for (int i = 0 ; i < data_size ; i++) { + auto data = ign_msg.covariance().data()[i]; + ros_msg.covariance[i] = data; + } } template<> From 9d345b1b4891822aad07e5a7bfd29c3f4b6273d8 Mon Sep 17 00:00:00 2001 From: Aditya Date: Mon, 7 Mar 2022 21:52:45 -0800 Subject: [PATCH 05/18] bridged odometry (ros) and odometry with covariance (ign) Signed-off-by: Aditya --- .../ros_ign_bridge/convert/nav_msgs.hpp | 13 +++++++ ros_ign_bridge/src/convert/nav_msgs.cpp | 34 +++++++++++++++++++ ros_ign_bridge/src/factories/nav_msgs.cpp | 34 +++++++++++++++++++ 3 files changed, 81 insertions(+) diff --git a/ros_ign_bridge/include/ros_ign_bridge/convert/nav_msgs.hpp b/ros_ign_bridge/include/ros_ign_bridge/convert/nav_msgs.hpp index 6f829eb1..882bc4ce 100644 --- a/ros_ign_bridge/include/ros_ign_bridge/convert/nav_msgs.hpp +++ b/ros_ign_bridge/include/ros_ign_bridge/convert/nav_msgs.hpp @@ -17,6 +17,7 @@ // Ignition messages #include +#include // ROS 2 messages #include @@ -38,6 +39,18 @@ convert_ign_to_ros( const ignition::msgs::Odometry & ign_msg, nav_msgs::msg::Odometry & ros_msg); +template<> +void +convert_ros_to_ign( + const nav_msgs::msg::Odometry & ros_msg, + ignition::msgs::OdometryWithCovariance & ign_msg); + +template<> +void +convert_ign_to_ros( + const ignition::msgs::OdometryWithCovariance & ign_msg, + nav_msgs::msg::Odometry & ros_msg); + } // namespace ros_ign_bridge #endif // ROS_IGN_BRIDGE__CONVERT__NAV_MSGS_HPP_ diff --git a/ros_ign_bridge/src/convert/nav_msgs.cpp b/ros_ign_bridge/src/convert/nav_msgs.cpp index 739b4bb3..d157815f 100644 --- a/ros_ign_bridge/src/convert/nav_msgs.cpp +++ b/ros_ign_bridge/src/convert/nav_msgs.cpp @@ -52,4 +52,38 @@ convert_ign_to_ros( } } +template<> +void +convert_ros_to_ign( + const nav_msgs::msg::Odometry & ros_msg, + ignition::msgs::OdometryWithCovariance & ign_msg) +{ + convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header())); + convert_ros_to_ign(ros_msg.pose, (*ign_msg.mutable_pose_with_covariance())); + convert_ros_to_ign(ros_msg.twist, (*ign_msg.mutable_twist_with_covariance())); + + auto childFrame = ign_msg.mutable_header()->add_data(); + childFrame->set_key("child_frame_id"); + childFrame->add_value(ros_msg.child_frame_id); +} + +template<> +void +convert_ign_to_ros( + const ignition::msgs::OdometryWithCovariance & ign_msg, + nav_msgs::msg::Odometry & ros_msg) +{ + convert_ign_to_ros(ign_msg.header(), ros_msg.header); + convert_ign_to_ros(ign_msg.pose_with_covariance(), ros_msg.pose); + convert_ign_to_ros(ign_msg.twist_with_covariance(), ros_msg.twist); + + for (auto i = 0; i < ign_msg.header().data_size(); ++i) { + auto aPair = ign_msg.header().data(i); + if (aPair.key() == "child_frame_id" && aPair.value_size() > 0) { + ros_msg.child_frame_id = frame_id_ign_to_ros(aPair.value(0)); + break; + } + } +} + } // namespace ros_ign_bridge diff --git a/ros_ign_bridge/src/factories/nav_msgs.cpp b/ros_ign_bridge/src/factories/nav_msgs.cpp index 3b8a2e8f..f6845c8d 100644 --- a/ros_ign_bridge/src/factories/nav_msgs.cpp +++ b/ros_ign_bridge/src/factories/nav_msgs.cpp @@ -38,6 +38,16 @@ get_factory__nav_msgs( > >("nav_msgs/msg/Odometry", ign_type_name); } + if ((ros_type_name == "nav_msgs/msg/Odometry" || ros_type_name.empty()) && + ign_type_name == "ignition.msgs.OdometryWithCovariance") + { + return std::make_shared< + Factory< + nav_msgs::msg::Odometry, + ignition::msgs::OdometryWithCovariance + > + >("nav_msgs/msg/Odometry", ign_type_name); + } return nullptr; } @@ -65,4 +75,28 @@ Factory< ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg); } +template<> +void +Factory< + nav_msgs::msg::Odometry, + ignition::msgs::OdometryWithCovariance +>::convert_ros_to_ign( + const nav_msgs::msg::Odometry & ros_msg, + ignition::msgs::OdometryWithCovariance & ign_msg) +{ + ros_ign_bridge::convert_ros_to_ign(ros_msg, ign_msg); +} + +template<> +void +Factory< + nav_msgs::msg::Odometry, + ignition::msgs::OdometryWithCovariance +>::convert_ign_to_ros( + const ignition::msgs::OdometryWithCovariance & ign_msg, + nav_msgs::msg::Odometry & ros_msg) +{ + ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg); +} + } // namespace ros_ign_bridge From a2aff09a49b94a86cd36eadbf130b02ebe830fd6 Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 8 Mar 2022 16:20:35 -0800 Subject: [PATCH 06/18] Added tests for PoseWithCovariance Signed-off-by: Aditya --- .../test/launch/test_ign_subscriber.launch.py | 1 + .../test/launch/test_ros_subscriber.launch.py | 1 + .../test/publishers/ign_publisher.cpp | 6 ++++++ .../test/publishers/ros_publisher.cpp | 8 ++++++++ .../test/subscribers/ign_subscriber.cpp | 12 ++++++++++++ .../geometry_msgs_subscriber.cpp | 12 ++++++++++++ ros_ign_bridge/test/utils/ign_test_msg.cpp | 12 ++++++++++++ ros_ign_bridge/test/utils/ign_test_msg.hpp | 11 +++++++++++ ros_ign_bridge/test/utils/ros_test_msg.cpp | 18 ++++++++++++++++++ ros_ign_bridge/test/utils/ros_test_msg.hpp | 14 ++++++++++++++ 10 files changed, 95 insertions(+) diff --git a/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py b/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py index 271c1461..fe53238c 100644 --- a/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py +++ b/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py @@ -53,6 +53,7 @@ def generate_test_description(): '/clock@rosgraph_msgs/msg/Clock@ignition.msgs.Clock', '/point@geometry_msgs/msg/Point@ignition.msgs.Vector3d', '/pose@geometry_msgs/msg/Pose@ignition.msgs.Pose', + '/pose_with_covariance@geometry_msgs/msg/PoseWithCovariance@ignition.msgs.PoseWithCovariance', '/pose_stamped@geometry_msgs/msg/PoseStamped@ignition.msgs.Pose', '/transform@geometry_msgs/msg/Transform@ignition.msgs.Pose', '/tf2_message@tf2_msgs/msg/TFMessage@ignition.msgs.Pose_V', diff --git a/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py b/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py index 52d5ad3c..429b6d9b 100644 --- a/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py +++ b/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py @@ -53,6 +53,7 @@ def generate_test_description(): '/clock@rosgraph_msgs/msg/Clock@ignition.msgs.Clock', '/point@geometry_msgs/msg/Point@ignition.msgs.Vector3d', '/pose@geometry_msgs/msg/Pose@ignition.msgs.Pose', + '/pose_with_covariance@geometry_msgs/msg/PoseWithCovariance@ignition.msgs.PoseWithCovariance', '/pose_stamped@geometry_msgs/msg/PoseStamped@ignition.msgs.Pose', '/transform@geometry_msgs/msg/Transform@ignition.msgs.Pose', '/tf2_message@tf2_msgs/msg/TFMessage@ignition.msgs.Pose_V', diff --git a/ros_ign_bridge/test/publishers/ign_publisher.cpp b/ros_ign_bridge/test/publishers/ign_publisher.cpp index dedccaf8..f9f2fb4f 100644 --- a/ros_ign_bridge/test/publishers/ign_publisher.cpp +++ b/ros_ign_bridge/test/publishers/ign_publisher.cpp @@ -119,6 +119,11 @@ int main(int /*argc*/, char **/*argv*/) ignition::msgs::Pose pose_msg; ros_ign_bridge::testing::createTestMsg(pose_msg); + // ignition::msgs::PoseWithCovariance. + auto pose_cov_pub = node.Advertise("pose_with_covariance"); + ignition::msgs::PoseWithCovariance pose_cov_msg; + ros_ign_bridge::testing::createTestMsg(pose_cov_msg); + // ignition::msgs::PoseStamped. auto pose_stamped_pub = node.Advertise("pose_stamped"); ignition::msgs::Pose pose_stamped_msg; @@ -274,6 +279,7 @@ int main(int /*argc*/, char **/*argv*/) clock_pub.Publish(clock_msg); point_pub.Publish(point_msg); pose_pub.Publish(pose_msg); + pose_cov_pub.Publish(pose_cov_msg); pose_stamped_pub.Publish(pose_stamped_msg); transform_pub.Publish(transform_msg); transform_stamped_pub.Publish(transform_stamped_msg); diff --git a/ros_ign_bridge/test/publishers/ros_publisher.cpp b/ros_ign_bridge/test/publishers/ros_publisher.cpp index dd54f364..76e8cdc7 100644 --- a/ros_ign_bridge/test/publishers/ros_publisher.cpp +++ b/ros_ign_bridge/test/publishers/ros_publisher.cpp @@ -97,6 +97,13 @@ int main(int argc, char ** argv) geometry_msgs::msg::Pose pose_msg; ros_ign_bridge::testing::createTestMsg(pose_msg); + // geometry_msgs::msg::PoseWithCovariance. + auto pose_cov_pub = + node->create_publisher( + "pose_with_covariance", 1000); + geometry_msgs::msg::PoseWithCovariance pose_cov_msg; + ros_ign_bridge::testing::createTestMsg(pose_cov_msg); + // geometry_msgs::msg::PoseStamped. auto pose_stamped_pub = node->create_publisher("pose_stamped", 1000); @@ -274,6 +281,7 @@ int main(int argc, char ** argv) clock_pub->publish(clock_msg); point_pub->publish(point_msg); pose_pub->publish(pose_msg); + pose_cov_pub->publish(pose_cov_msg); pose_stamped_pub->publish(pose_stamped_msg); transform_pub->publish(transform_msg); transform_stamped_pub->publish(transform_stamped_msg); diff --git a/ros_ign_bridge/test/subscribers/ign_subscriber.cpp b/ros_ign_bridge/test/subscribers/ign_subscriber.cpp index 0c53823f..d8798aec 100644 --- a/ros_ign_bridge/test/subscribers/ign_subscriber.cpp +++ b/ros_ign_bridge/test/subscribers/ign_subscriber.cpp @@ -201,6 +201,18 @@ TEST(IgnSubscriberTest, Pose) EXPECT_TRUE(client.callbackExecuted); } +///////////////////////////////////////////////// +TEST(IgnSubscriberTest, PoseWithCovariance) +{ + MyTestClass client("pose_with_covariance"); + + using namespace std::chrono_literals; + ros_ign_bridge::testing::waitUntilBoolVar( + client.callbackExecuted, 100ms, 200); + + EXPECT_TRUE(client.callbackExecuted); +} + ///////////////////////////////////////////////// TEST(IgnSubscriberTest, PoseStamped) { diff --git a/ros_ign_bridge/test/subscribers/ros_subscriber/geometry_msgs_subscriber.cpp b/ros_ign_bridge/test/subscribers/ros_subscriber/geometry_msgs_subscriber.cpp index ea235bda..a51a17a3 100644 --- a/ros_ign_bridge/test/subscribers/ros_subscriber/geometry_msgs_subscriber.cpp +++ b/ros_ign_bridge/test/subscribers/ros_subscriber/geometry_msgs_subscriber.cpp @@ -69,6 +69,18 @@ TEST(ROSSubscriberTest, Pose) EXPECT_TRUE(client.callbackExecuted); } +///////////////////////////////////////////////// +TEST(ROSSubscriberTest, PoseWithCovariance) +{ + MyTestClass client("pose_with_covariance"); + + using namespace std::chrono_literals; + ros_ign_bridge::testing::waitUntilBoolVarAndSpin( + ros_subscriber::TestNode(), client.callbackExecuted, 10ms, 200); + + EXPECT_TRUE(client.callbackExecuted); +} + ///////////////////////////////////////////////// TEST(ROSSubscriberTest, PoseStamped) { diff --git a/ros_ign_bridge/test/utils/ign_test_msg.cpp b/ros_ign_bridge/test/utils/ign_test_msg.cpp index 65918d60..33d919f4 100644 --- a/ros_ign_bridge/test/utils/ign_test_msg.cpp +++ b/ros_ign_bridge/test/utils/ign_test_msg.cpp @@ -235,6 +235,18 @@ void compareTestMsg(const std::shared_ptr & _msg) compareTestMsg(std::make_shared(_msg->orientation())); } +void createTestMsg(ignition::msgs::PoseWithCovariance & _msg) +{ + createTestMsg(*_msg.mutable_pose()->mutable_position()); + createTestMsg(*_msg.mutable_pose()->mutable_orientation()); +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + compareTestMsg(std::make_shared(_msg->pose().position())); + compareTestMsg(std::make_shared(_msg->pose().orientation())); +} + void createTestMsg(ignition::msgs::Pose_V & _msg) { createTestMsg(*(_msg.mutable_header())); diff --git a/ros_ign_bridge/test/utils/ign_test_msg.hpp b/ros_ign_bridge/test/utils/ign_test_msg.hpp index d8d6acdc..d4c2b9fc 100644 --- a/ros_ign_bridge/test/utils/ign_test_msg.hpp +++ b/ros_ign_bridge/test/utils/ign_test_msg.hpp @@ -40,14 +40,17 @@ #include #include #include +#include #include #include +#include #include #include #include #include #include #include +#include #include #include #include @@ -151,6 +154,14 @@ void createTestMsg(ignition::msgs::Pose & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(ignition::msgs::PoseWithCovariance & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(ignition::msgs::Pose_V & _msg); diff --git a/ros_ign_bridge/test/utils/ros_test_msg.cpp b/ros_ign_bridge/test/utils/ros_test_msg.cpp index c7def56f..c53c7791 100644 --- a/ros_ign_bridge/test/utils/ros_test_msg.cpp +++ b/ros_ign_bridge/test/utils/ros_test_msg.cpp @@ -264,6 +264,24 @@ void compareTestMsg(const std::shared_ptr & _msg) compareTestMsg(_msg->orientation); } +void compareTestMsg(const geometry_msgs::msg::PoseWithCovariance & _msg) +{ + compareTestMsg(_msg.pose.position); + compareTestMsg(_msg.pose.orientation); +} + +void createTestMsg(geometry_msgs::msg::PoseWithCovariance & _msg) +{ + createTestMsg(_msg.pose.position); + createTestMsg(_msg.pose.orientation); +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + compareTestMsg(_msg->pose.position); + compareTestMsg(_msg->pose.orientation); +} + void createTestMsg(geometry_msgs::msg::PoseStamped & _msg) { createTestMsg(_msg.header); diff --git a/ros_ign_bridge/test/utils/ros_test_msg.hpp b/ros_ign_bridge/test/utils/ros_test_msg.hpp index d6a8f5aa..9612b8b4 100644 --- a/ros_ign_bridge/test/utils/ros_test_msg.hpp +++ b/ros_ign_bridge/test/utils/ros_test_msg.hpp @@ -28,10 +28,12 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include @@ -200,6 +202,18 @@ void createTestMsg(geometry_msgs::msg::Pose & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const geometry_msgs::msg::PoseWithCovariance & _msg); + +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(geometry_msgs::msg::PoseWithCovariance & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(geometry_msgs::msg::PoseStamped & _msg); From 1bd8b1d486ceacc84536c6a6f45e45c591fc6db3 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 9 Mar 2022 10:53:30 -0800 Subject: [PATCH 07/18] Added tests for TwistWithCovariance Signed-off-by: Aditya --- .../test/launch/test_ign_subscriber.launch.py | 1 + .../test/launch/test_ros_subscriber.launch.py | 1 + .../test/publishers/ign_publisher.cpp | 7 +++++++ .../test/publishers/ros_publisher.cpp | 8 ++++++++ .../test/subscribers/ign_subscriber.cpp | 12 ++++++++++++ .../geometry_msgs_subscriber.cpp | 12 ++++++++++++ ros_ign_bridge/test/utils/ign_test_msg.cpp | 18 ++++++++++++++++++ ros_ign_bridge/test/utils/ign_test_msg.hpp | 8 ++++++++ ros_ign_bridge/test/utils/ros_test_msg.cpp | 12 ++++++++++++ ros_ign_bridge/test/utils/ros_test_msg.hpp | 8 ++++++++ 10 files changed, 87 insertions(+) diff --git a/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py b/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py index fe53238c..1afcbc4c 100644 --- a/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py +++ b/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py @@ -59,6 +59,7 @@ def generate_test_description(): '/tf2_message@tf2_msgs/msg/TFMessage@ignition.msgs.Pose_V', '/transform_stamped@geometry_msgs/msg/TransformStamped@ignition.msgs.Pose', '/twist@geometry_msgs/msg/Twist@ignition.msgs.Twist', + '/twist_with_covariance@geometry_msgs/msg/TwistWithCovariance@ignition.msgs.TwistWithCovariance', '/wrench@geometry_msgs/msg/Wrench@ignition.msgs.Wrench', '/joint_wrench@ros_ign_interfaces/msg/JointWrench@ignition.msgs.JointWrench', '/entity@ros_ign_interfaces/msg/Entity@ignition.msgs.Entity', diff --git a/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py b/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py index 429b6d9b..dfbae87e 100644 --- a/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py +++ b/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py @@ -59,6 +59,7 @@ def generate_test_description(): '/tf2_message@tf2_msgs/msg/TFMessage@ignition.msgs.Pose_V', '/transform_stamped@geometry_msgs/msg/TransformStamped@ignition.msgs.Pose', '/twist@geometry_msgs/msg/Twist@ignition.msgs.Twist', + '/twist_with_covariance@geometry_msgs/msg/TwistWithCovariance@ignition.msgs.TwistWithCovariance', '/wrench@geometry_msgs/msg/Wrench@ignition.msgs.Wrench', '/joint_wrench@ros_ign_interfaces/msg/JointWrench@ignition.msgs.JointWrench', '/entity@ros_ign_interfaces/msg/Entity@ignition.msgs.Entity', diff --git a/ros_ign_bridge/test/publishers/ign_publisher.cpp b/ros_ign_bridge/test/publishers/ign_publisher.cpp index f9f2fb4f..26559266 100644 --- a/ros_ign_bridge/test/publishers/ign_publisher.cpp +++ b/ros_ign_bridge/test/publishers/ign_publisher.cpp @@ -197,6 +197,12 @@ int main(int /*argc*/, char **/*argv*/) ignition::msgs::Twist twist_msg; ros_ign_bridge::testing::createTestMsg(twist_msg); + // ignition::msgs::TwistWithCovariance. + auto twist_cov_pub = node.Advertise( + "twist_with_covariance"); + ignition::msgs::TwistWithCovariance twist_cov_msg; + ros_ign_bridge::testing::createTestMsg(twist_cov_msg); + // ignition::msgs::Wrench. auto wrench_pub = node.Advertise("wrench"); ignition::msgs::Wrench wrench_msg; @@ -299,6 +305,7 @@ int main(int /*argc*/, char **/*argv*/) odometry_pub.Publish(odometry_msg); joint_states_pub.Publish(joint_states_msg); twist_pub.Publish(twist_msg); + twist_cov_pub.Publish(twist_cov_msg); pointcloudpacked_pub.Publish(pointcloudpacked_msg); battery_state_pub.Publish(battery_state_msg); joint_trajectory_pub.Publish(joint_trajectory_msg); diff --git a/ros_ign_bridge/test/publishers/ros_publisher.cpp b/ros_ign_bridge/test/publishers/ros_publisher.cpp index 76e8cdc7..3349a1bd 100644 --- a/ros_ign_bridge/test/publishers/ros_publisher.cpp +++ b/ros_ign_bridge/test/publishers/ros_publisher.cpp @@ -128,6 +128,13 @@ int main(int argc, char ** argv) geometry_msgs::msg::Twist twist_msg; ros_ign_bridge::testing::createTestMsg(twist_msg); + // geometry_msgs::msg::TwistWithCovariance. + auto twist_cov_pub = + node->create_publisher( + "twist_with_covariance", 1000); + geometry_msgs::msg::TwistWithCovariance twist_cov_msg; + ros_ign_bridge::testing::createTestMsg(twist_cov_msg); + auto tf2_message_pub = node->create_publisher("tf2_message", 1000); tf2_msgs::msg::TFMessage tf2_msg; @@ -287,6 +294,7 @@ int main(int argc, char ** argv) transform_stamped_pub->publish(transform_stamped_msg); tf2_message_pub->publish(tf2_msg); twist_pub->publish(twist_msg); + twist_cov_pub->publish(twist_cov_msg); wrench_pub->publish(wrench_msg); light_pub->publish(light_msg); joint_wrench_pub->publish(joint_wrench_msg); diff --git a/ros_ign_bridge/test/subscribers/ign_subscriber.cpp b/ros_ign_bridge/test/subscribers/ign_subscriber.cpp index d8798aec..9404a208 100644 --- a/ros_ign_bridge/test/subscribers/ign_subscriber.cpp +++ b/ros_ign_bridge/test/subscribers/ign_subscriber.cpp @@ -273,6 +273,18 @@ TEST(IgnSubscriberTest, Twist) EXPECT_TRUE(client.callbackExecuted); } +///////////////////////////////////////////////// +TEST(IgnSubscriberTest, TwistWithCovariance) +{ + MyTestClass client("twist_with_covariance"); + + using namespace std::chrono_literals; + ros_ign_bridge::testing::waitUntilBoolVar( + client.callbackExecuted, 100ms, 200); + + EXPECT_TRUE(client.callbackExecuted); +} + ///////////////////////////////////////////////// TEST(IgnSubscriberTest, Wrench) { diff --git a/ros_ign_bridge/test/subscribers/ros_subscriber/geometry_msgs_subscriber.cpp b/ros_ign_bridge/test/subscribers/ros_subscriber/geometry_msgs_subscriber.cpp index a51a17a3..3289891a 100644 --- a/ros_ign_bridge/test/subscribers/ros_subscriber/geometry_msgs_subscriber.cpp +++ b/ros_ign_bridge/test/subscribers/ros_subscriber/geometry_msgs_subscriber.cpp @@ -141,6 +141,18 @@ TEST(ROSSubscriberTest, Twist) EXPECT_TRUE(client.callbackExecuted); } +///////////////////////////////////////////////// +TEST(ROSSubscriberTest, TwistWithCovariance) +{ + MyTestClass client("twist_with_covariance"); + + using namespace std::chrono_literals; + ros_ign_bridge::testing::waitUntilBoolVarAndSpin( + ros_subscriber::TestNode(), client.callbackExecuted, 10ms, 200); + + EXPECT_TRUE(client.callbackExecuted); +} + ///////////////////////////////////////////////// TEST(ROSSubscriberTest, Wrench) { diff --git a/ros_ign_bridge/test/utils/ign_test_msg.cpp b/ros_ign_bridge/test/utils/ign_test_msg.cpp index 33d919f4..44324a3e 100644 --- a/ros_ign_bridge/test/utils/ign_test_msg.cpp +++ b/ros_ign_bridge/test/utils/ign_test_msg.cpp @@ -283,6 +283,24 @@ void compareTestMsg(const std::shared_ptr & _msg) compareTestMsg(std::make_shared(_msg->angular())); } +void createTestMsg(ignition::msgs::TwistWithCovariance & _msg) +{ + ignition::msgs::Vector3d linear_msg; + ignition::msgs::Vector3d angular_msg; + + createTestMsg(linear_msg); + createTestMsg(angular_msg); + + _msg.mutable_twist()->mutable_linear()->CopyFrom(linear_msg); + _msg.mutable_twist()->mutable_angular()->CopyFrom(angular_msg); +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + compareTestMsg(std::make_shared(_msg->twist().linear())); + compareTestMsg(std::make_shared(_msg->twist().angular())); +} + void createTestMsg(ignition::msgs::Wrench & _msg) { ignition::msgs::Header header_msg; diff --git a/ros_ign_bridge/test/utils/ign_test_msg.hpp b/ros_ign_bridge/test/utils/ign_test_msg.hpp index d4c2b9fc..0c9a24d8 100644 --- a/ros_ign_bridge/test/utils/ign_test_msg.hpp +++ b/ros_ign_bridge/test/utils/ign_test_msg.hpp @@ -178,6 +178,14 @@ void createTestMsg(ignition::msgs::Twist & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(ignition::msgs::TwistWithCovariance & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(ignition::msgs::Wrench & _msg); diff --git a/ros_ign_bridge/test/utils/ros_test_msg.cpp b/ros_ign_bridge/test/utils/ros_test_msg.cpp index c53c7791..48c8e10d 100644 --- a/ros_ign_bridge/test/utils/ros_test_msg.cpp +++ b/ros_ign_bridge/test/utils/ros_test_msg.cpp @@ -355,6 +355,18 @@ void compareTestMsg(const std::shared_ptr & _msg) compareTestMsg(std::make_shared(_msg->angular)); } +void createTestMsg(geometry_msgs::msg::TwistWithCovariance & _msg) +{ + createTestMsg(_msg.twist.linear); + createTestMsg(_msg.twist.angular); +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + compareTestMsg(std::make_shared(_msg->twist.linear)); + compareTestMsg(std::make_shared(_msg->twist.angular)); +} + void createTestMsg(geometry_msgs::msg::Wrench & _msg) { createTestMsg(_msg.force); diff --git a/ros_ign_bridge/test/utils/ros_test_msg.hpp b/ros_ign_bridge/test/utils/ros_test_msg.hpp index 9612b8b4..84e7316c 100644 --- a/ros_ign_bridge/test/utils/ros_test_msg.hpp +++ b/ros_ign_bridge/test/utils/ros_test_msg.hpp @@ -250,6 +250,14 @@ void createTestMsg(geometry_msgs::msg::Twist & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(geometry_msgs::msg::TwistWithCovariance & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(geometry_msgs::msg::Wrench & _msg); From e1f9ceca308307e4e350f2bf0c4fe2a2d1387779 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 9 Mar 2022 11:41:20 -0800 Subject: [PATCH 08/18] Added tests for OdometryWithCovariance Signed-off-by: Aditya --- .../test/launch/test_ign_subscriber.launch.py | 1 + .../test/launch/test_ros_subscriber.launch.py | 1 + .../test/publishers/ign_publisher.cpp | 7 ++++++ .../test/publishers/ros_publisher.cpp | 7 ++++++ .../test/subscribers/ign_subscriber.cpp | 12 ++++++++++ .../ros_subscriber/nav_msgs_subscriber.cpp | 12 ++++++++++ ros_ign_bridge/test/utils/ign_test_msg.cpp | 22 +++++++++++++++++++ ros_ign_bridge/test/utils/ign_test_msg.hpp | 8 +++++++ 8 files changed, 70 insertions(+) diff --git a/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py b/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py index 1afcbc4c..e5de981d 100644 --- a/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py +++ b/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py @@ -78,6 +78,7 @@ def generate_test_description(): '/magnetic@sensor_msgs/msg/MagneticField@ignition.msgs.Magnetometer', # '/actuators@mav_msgs/msg/Actuators@ignition.msgs.Actuators', '/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry', + '/odometry_with_covariance@nav_msgs/msg/Odometry@ignition.msgs.OdometryWithCovariance', '/pointcloud2@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked', '/joint_states@sensor_msgs/msg/JointState@ignition.msgs.Model', '/battery_state@sensor_msgs/msg/BatteryState@ignition.msgs.BatteryState', diff --git a/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py b/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py index dfbae87e..23098ec1 100644 --- a/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py +++ b/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py @@ -78,6 +78,7 @@ def generate_test_description(): '/magnetic@sensor_msgs/msg/MagneticField@ignition.msgs.Magnetometer', # '/actuators@mav_msgs/msg/Actuators@ignition.msgs.Actuators', '/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry', + '/odometry_with_covariance@nav_msgs/msg/Odometry@ignition.msgs.OdometryWithCovariance', '/pointcloud2@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked', '/joint_states@sensor_msgs/msg/JointState@ignition.msgs.Model', '/battery_state@sensor_msgs/msg/BatteryState@ignition.msgs.BatteryState', diff --git a/ros_ign_bridge/test/publishers/ign_publisher.cpp b/ros_ign_bridge/test/publishers/ign_publisher.cpp index 26559266..8f2dc829 100644 --- a/ros_ign_bridge/test/publishers/ign_publisher.cpp +++ b/ros_ign_bridge/test/publishers/ign_publisher.cpp @@ -187,6 +187,12 @@ int main(int /*argc*/, char **/*argv*/) ignition::msgs::Odometry odometry_msg; ros_ign_bridge::testing::createTestMsg(odometry_msg); + // ignition::msgs::OdometryWithCovariance. + auto odometry_cov_pub = node.Advertise( + "odometry_with_covariance"); + ignition::msgs::OdometryWithCovariance odometry_cov_msg; + ros_ign_bridge::testing::createTestMsg(odometry_cov_msg); + // ignition::msgs::Model. auto joint_states_pub = node.Advertise("joint_states"); ignition::msgs::Model joint_states_msg; @@ -303,6 +309,7 @@ int main(int /*argc*/, char **/*argv*/) magnetic_pub.Publish(magnetometer_msg); actuators_pub.Publish(actuators_msg); odometry_pub.Publish(odometry_msg); + odometry_cov_pub.Publish(odometry_cov_msg); joint_states_pub.Publish(joint_states_msg); twist_pub.Publish(twist_msg); twist_cov_pub.Publish(twist_cov_msg); diff --git a/ros_ign_bridge/test/publishers/ros_publisher.cpp b/ros_ign_bridge/test/publishers/ros_publisher.cpp index 3349a1bd..60fbe817 100644 --- a/ros_ign_bridge/test/publishers/ros_publisher.cpp +++ b/ros_ign_bridge/test/publishers/ros_publisher.cpp @@ -212,6 +212,12 @@ int main(int argc, char ** argv) nav_msgs::msg::Odometry odometry_msg; ros_ign_bridge::testing::createTestMsg(odometry_msg); + // nav_msgs::msg::Odometry. + auto odometry_cov_pub = + node->create_publisher("odometry_with_covariance", 1000); + nav_msgs::msg::Odometry odometry_cov_msg; + ros_ign_bridge::testing::createTestMsg(odometry_cov_msg); + // sensor_msgs::msg::Image. auto image_pub = node->create_publisher("image", 1000); @@ -303,6 +309,7 @@ int main(int argc, char ** argv) contacts_pub->publish(contacts_msg); // actuators_pub->publish(actuators_msg); odometry_pub->publish(odometry_msg); + odometry_cov_pub->publish(odometry_cov_msg); image_pub->publish(image_msg); camera_info_pub->publish(camera_info_msg); fluid_pressure_pub->publish(fluid_pressure_msg); diff --git a/ros_ign_bridge/test/subscribers/ign_subscriber.cpp b/ros_ign_bridge/test/subscribers/ign_subscriber.cpp index 9404a208..594ee31b 100644 --- a/ros_ign_bridge/test/subscribers/ign_subscriber.cpp +++ b/ros_ign_bridge/test/subscribers/ign_subscriber.cpp @@ -441,6 +441,18 @@ TEST(IgnSubscriberTest, Odometry) EXPECT_TRUE(client.callbackExecuted); } +///////////////////////////////////////////////// +TEST(IgnSubscriberTest, OdometryWithCovariance) +{ + MyTestClass client("odometry_with_covariance"); + + using namespace std::chrono_literals; + ros_ign_bridge::testing::waitUntilBoolVar( + client.callbackExecuted, 100ms, 200); + + EXPECT_TRUE(client.callbackExecuted); +} + ///////////////////////////////////////////////// TEST(IgnSubscriberTest, JointStates) { diff --git a/ros_ign_bridge/test/subscribers/ros_subscriber/nav_msgs_subscriber.cpp b/ros_ign_bridge/test/subscribers/ros_subscriber/nav_msgs_subscriber.cpp index 92c5adf1..7627378c 100644 --- a/ros_ign_bridge/test/subscribers/ros_subscriber/nav_msgs_subscriber.cpp +++ b/ros_ign_bridge/test/subscribers/ros_subscriber/nav_msgs_subscriber.cpp @@ -32,3 +32,15 @@ TEST(ROSSubscriberTest, Odometry) EXPECT_TRUE(client.callbackExecuted); } + +///////////////////////////////////////////////// +TEST(ROSSubscriberTest, OdometryWithCovariance) +{ + MyTestClass client("odometry_with_covariance"); + + using namespace std::chrono_literals; + ros_ign_bridge::testing::waitUntilBoolVarAndSpin( + ros_subscriber::TestNode(), client.callbackExecuted, 10ms, 200); + + EXPECT_TRUE(client.callbackExecuted); +} diff --git a/ros_ign_bridge/test/utils/ign_test_msg.cpp b/ros_ign_bridge/test/utils/ign_test_msg.cpp index 44324a3e..67fed01f 100644 --- a/ros_ign_bridge/test/utils/ign_test_msg.cpp +++ b/ros_ign_bridge/test/utils/ign_test_msg.cpp @@ -804,6 +804,28 @@ void compareTestMsg(const std::shared_ptr & _msg) compareTestMsg(std::make_shared(_msg->twist())); } +void createTestMsg(ignition::msgs::OdometryWithCovariance & _msg) +{ + ignition::msgs::Header header_msg; + ignition::msgs::PoseWithCovariance pose_cov_msg; + ignition::msgs::TwistWithCovariance twist_cov_msg; + + createTestMsg(header_msg); + createTestMsg(pose_cov_msg); + createTestMsg(twist_cov_msg); + + _msg.mutable_header()->CopyFrom(header_msg); + _msg.mutable_pose_with_covariance()->CopyFrom(pose_cov_msg); + _msg.mutable_twist_with_covariance()->CopyFrom(twist_cov_msg); +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + compareTestMsg(std::make_shared(_msg->header())); + compareTestMsg(std::make_shared(_msg->pose_with_covariance())); + compareTestMsg(std::make_shared(_msg->twist_with_covariance())); +} + void createTestMsg(ignition::msgs::PointCloudPacked & _msg) { ignition::msgs::Header header_msg; diff --git a/ros_ign_bridge/test/utils/ign_test_msg.hpp b/ros_ign_bridge/test/utils/ign_test_msg.hpp index 0c9a24d8..5ea1b386 100644 --- a/ros_ign_bridge/test/utils/ign_test_msg.hpp +++ b/ros_ign_bridge/test/utils/ign_test_msg.hpp @@ -306,6 +306,14 @@ void createTestMsg(ignition::msgs::Odometry & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(ignition::msgs::OdometryWithCovariance & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(ignition::msgs::PointCloudPacked & _msg); From c7fc616a7eba510ba3c39ca5e89f7f75bebe39d3 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 9 Mar 2022 12:17:27 -0800 Subject: [PATCH 09/18] Updated README, check size of Float_V before conversion to ros msg Signed-off-by: Aditya --- ros_ign_bridge/README.md | 93 ++++++++++---------- ros_ign_bridge/src/convert/geometry_msgs.cpp | 16 ++-- 2 files changed, 58 insertions(+), 51 deletions(-) diff --git a/ros_ign_bridge/README.md b/ros_ign_bridge/README.md index 6fc0ce57..66bbc1f8 100644 --- a/ros_ign_bridge/README.md +++ b/ros_ign_bridge/README.md @@ -6,51 +6,54 @@ between ROS and Ignition Transport. The bridge is currently implemented in C++. At this point there's no support for service calls. Its support is limited to only the following message types: -| ROS type | Ignition Transport type | -|--------------------------------------|:------------------------------------:| -| builtin_interfaces/msg/Time | ignition::msgs::Time | -| std_msgs/msg/Bool | ignition::msgs::Boolean | -| std_msgs/msg/ColorRGBA | ignition::msgs::Color | -| std_msgs/msg/Empty | ignition::msgs::Empty | -| std_msgs/msg/Float32 | ignition::msgs::Float | -| std_msgs/msg/Float64 | ignition::msgs::Double | -| std_msgs/msg/Header | ignition::msgs::Header | -| std_msgs/msg/Int32 | ignition::msgs::Int32 | -| std_msgs/msg/UInt32 | ignition::msgs::UInt32 | -| std_msgs/msg/String | ignition::msgs::StringMsg | -| geometry_msgs/msg/Wrench | ignition::msgs::Wrench | -| geometry_msgs/msg/Quaternion | ignition::msgs::Quaternion | -| geometry_msgs/msg/Vector3 | ignition::msgs::Vector3d | -| geometry_msgs/msg/Point | ignition::msgs::Vector3d | -| geometry_msgs/msg/Pose | ignition::msgs::Pose | -| geometry_msgs/msg/PoseStamped | ignition::msgs::Pose | -| geometry_msgs/msg/Transform | ignition::msgs::Pose | -| geometry_msgs/msg/TransformStamped | ignition::msgs::Pose | -| geometry_msgs/msg/Twist | ignition::msgs::Twist | -| mav_msgs/msg/Actuators (TODO) | ignition::msgs::Actuators (TODO) | -| nav_msgs/msg/Odometry | ignition::msgs::Odometry | -| ros_ign_interfaces/msg/Contact | ignition::msgs::Contact | -| ros_ign_interfaces/msg/Contacts | ignition::msgs::Contacts | -| ros_ign_interfaces/msg/Entity | ignition::msgs::Entity | -| ros_ign_interfaces/msg/GuiCamera | ignition::msgs::GUICamera | -| ros_ign_interfaces/msg/JointWrench | ignition::msgs::JointWrench | -| ros_ign_interfaces/msg/Light | ignition::msgs::Light | -| ros_ign_interfaces/msg/StringVec | ignition::msgs::StringMsg_V | -| ros_ign_interfaces/msg/TrackVisual | ignition::msgs::TrackVisual | -| ros_ign_interfaces/msg/VideoRecord | ignition::msgs::VideoRecord | -| ros_ign_interfaces/msg/WorldControl | ignition::msgs::WorldControl | -| rosgraph_msgs/msg/Clock | ignition::msgs::Clock | -| sensor_msgs/msg/BatteryState | ignition::msgs::BatteryState | -| sensor_msgs/msg/CameraInfo | ignition::msgs::CameraInfo | -| sensor_msgs/msg/FluidPressure | ignition::msgs::FluidPressure | -| sensor_msgs/msg/Imu | ignition::msgs::IMU | -| sensor_msgs/msg/Image | ignition::msgs::Image | -| sensor_msgs/msg/JointState | ignition::msgs::Model | -| sensor_msgs/msg/LaserScan | ignition::msgs::LaserScan | -| sensor_msgs/msg/MagneticField | ignition::msgs::Magnetometer | -| sensor_msgs/msg/PointCloud2 | ignition::msgs::PointCloudPacked | -| tf2_msgs/msg/TFMessage | ignition::msgs::Pose_V | -| trajectory_msgs/msg/JointTrajectory | ignition::msgs::JointTrajectory | +| ROS type | Ignition Transport type | +|--------------------------------------|:--------------------------------------:| +| builtin_interfaces/msg/Time | ignition::msgs::Time | +| std_msgs/msg/Bool | ignition::msgs::Boolean | +| std_msgs/msg/ColorRGBA | ignition::msgs::Color | +| std_msgs/msg/Empty | ignition::msgs::Empty | +| std_msgs/msg/Float32 | ignition::msgs::Float | +| std_msgs/msg/Float64 | ignition::msgs::Double | +| std_msgs/msg/Header | ignition::msgs::Header | +| std_msgs/msg/Int32 | ignition::msgs::Int32 | +| std_msgs/msg/UInt32 | ignition::msgs::UInt32 | +| std_msgs/msg/String | ignition::msgs::StringMsg | +| geometry_msgs/msg/Wrench | ignition::msgs::Wrench | +| geometry_msgs/msg/Quaternion | ignition::msgs::Quaternion | +| geometry_msgs/msg/Vector3 | ignition::msgs::Vector3d | +| geometry_msgs/msg/Point | ignition::msgs::Vector3d | +| geometry_msgs/msg/Pose | ignition::msgs::Pose | +| geometry_msgs/msg/PoseWithCovariance | ignition::msgs::PoseWithCovariance | +| geometry_msgs/msg/PoseStamped | ignition::msgs::Pose | +| geometry_msgs/msg/Transform | ignition::msgs::Pose | +| geometry_msgs/msg/TransformStamped | ignition::msgs::Pose | +| geometry_msgs/msg/Twist | ignition::msgs::Twist | +| geometry_msgs/msg/TwistWithCovariance| ignition::msgs::TwistWithCovariance | +| mav_msgs/msg/Actuators (TODO) | ignition::msgs::Actuators (TODO) | +| nav_msgs/msg/Odometry | ignition::msgs::Odometry | +| nav_msgs/msg/Odometry | ignition::msgs::OdometryWithCovariance | +| ros_ign_interfaces/msg/Contact | ignition::msgs::Contact | +| ros_ign_interfaces/msg/Contacts | ignition::msgs::Contacts | +| ros_ign_interfaces/msg/Entity | ignition::msgs::Entity | +| ros_ign_interfaces/msg/GuiCamera | ignition::msgs::GUICamera | +| ros_ign_interfaces/msg/JointWrench | ignition::msgs::JointWrench | +| ros_ign_interfaces/msg/Light | ignition::msgs::Light | +| ros_ign_interfaces/msg/StringVec | ignition::msgs::StringMsg_V | +| ros_ign_interfaces/msg/TrackVisual | ignition::msgs::TrackVisual | +| ros_ign_interfaces/msg/VideoRecord | ignition::msgs::VideoRecord | +| ros_ign_interfaces/msg/WorldControl | ignition::msgs::WorldControl | +| rosgraph_msgs/msg/Clock | ignition::msgs::Clock | +| sensor_msgs/msg/BatteryState | ignition::msgs::BatteryState | +| sensor_msgs/msg/CameraInfo | ignition::msgs::CameraInfo | +| sensor_msgs/msg/FluidPressure | ignition::msgs::FluidPressure | +| sensor_msgs/msg/Imu | ignition::msgs::IMU | +| sensor_msgs/msg/Image | ignition::msgs::Image | +| sensor_msgs/msg/JointState | ignition::msgs::Model | +| sensor_msgs/msg/LaserScan | ignition::msgs::LaserScan | +| sensor_msgs/msg/MagneticField | ignition::msgs::Magnetometer | +| sensor_msgs/msg/PointCloud2 | ignition::msgs::PointCloudPacked | +| tf2_msgs/msg/TFMessage | ignition::msgs::Pose_V | +| trajectory_msgs/msg/JointTrajectory | ignition::msgs::JointTrajectory | Run `ros2 run ros_ign_bridge parameter_bridge -h` for instructions. diff --git a/ros_ign_bridge/src/convert/geometry_msgs.cpp b/ros_ign_bridge/src/convert/geometry_msgs.cpp index 142066be..8bb1c0b2 100644 --- a/ros_ign_bridge/src/convert/geometry_msgs.cpp +++ b/ros_ign_bridge/src/convert/geometry_msgs.cpp @@ -128,9 +128,11 @@ convert_ign_to_ros( convert_ign_to_ros(ign_msg.pose().position(), ros_msg.pose.position); convert_ign_to_ros(ign_msg.pose().orientation(), ros_msg.pose.orientation); int data_size = ign_msg.covariance().data_size(); - for (int i = 0 ; i < data_size ; i++) { - auto data = ign_msg.covariance().data()[i]; - ros_msg.covariance[i] = data; + if (data_size == 36) { + for (int i = 0 ; i < data_size ; i++) { + auto data = ign_msg.covariance().data()[i]; + ros_msg.covariance[i] = data; + } } } @@ -247,9 +249,11 @@ convert_ign_to_ros( convert_ign_to_ros(ign_msg.twist().linear(), ros_msg.twist.linear); convert_ign_to_ros(ign_msg.twist().angular(), ros_msg.twist.angular); int data_size = ign_msg.covariance().data_size(); - for (int i = 0 ; i < data_size ; i++) { - auto data = ign_msg.covariance().data()[i]; - ros_msg.covariance[i] = data; + if (data_size == 36){ + for (int i = 0 ; i < data_size ; i++) { + auto data = ign_msg.covariance().data()[i]; + ros_msg.covariance[i] = data; + } } } From a1bec615a650b19c62a363a7803a4f6be9693dd5 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 9 Mar 2022 12:19:04 -0800 Subject: [PATCH 10/18] linter Signed-off-by: Aditya --- ros_ign_bridge/src/convert/geometry_msgs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_ign_bridge/src/convert/geometry_msgs.cpp b/ros_ign_bridge/src/convert/geometry_msgs.cpp index 8bb1c0b2..7299e4e7 100644 --- a/ros_ign_bridge/src/convert/geometry_msgs.cpp +++ b/ros_ign_bridge/src/convert/geometry_msgs.cpp @@ -249,7 +249,7 @@ convert_ign_to_ros( convert_ign_to_ros(ign_msg.twist().linear(), ros_msg.twist.linear); convert_ign_to_ros(ign_msg.twist().angular(), ros_msg.twist.angular); int data_size = ign_msg.covariance().data_size(); - if (data_size == 36){ + if (data_size == 36) { for (int i = 0 ; i < data_size ; i++) { auto data = ign_msg.covariance().data()[i]; ros_msg.covariance[i] = data; From 4851f465f64c4bb35e11fa47890877132291d797 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 9 Mar 2022 16:56:54 -0800 Subject: [PATCH 11/18] Populate covariance matrix with nontrivial values Signed-off-by: Aditya --- ros_ign_bridge/test/utils/ign_test_msg.cpp | 6 ++++++ ros_ign_bridge/test/utils/ros_test_msg.cpp | 10 ++++++++++ 2 files changed, 16 insertions(+) diff --git a/ros_ign_bridge/test/utils/ign_test_msg.cpp b/ros_ign_bridge/test/utils/ign_test_msg.cpp index 67fed01f..5b5f0faf 100644 --- a/ros_ign_bridge/test/utils/ign_test_msg.cpp +++ b/ros_ign_bridge/test/utils/ign_test_msg.cpp @@ -239,6 +239,9 @@ void createTestMsg(ignition::msgs::PoseWithCovariance & _msg) { createTestMsg(*_msg.mutable_pose()->mutable_position()); createTestMsg(*_msg.mutable_pose()->mutable_orientation()); + for (int i = 0; i < 36; i++) { + _msg.mutable_covariance()->add_data(i); + } } void compareTestMsg(const std::shared_ptr & _msg) @@ -293,6 +296,9 @@ void createTestMsg(ignition::msgs::TwistWithCovariance & _msg) _msg.mutable_twist()->mutable_linear()->CopyFrom(linear_msg); _msg.mutable_twist()->mutable_angular()->CopyFrom(angular_msg); + for (int i = 0 ; i < 36 ; i++) { + _msg.mutable_covariance()->add_data(i); + } } void compareTestMsg(const std::shared_ptr & _msg) diff --git a/ros_ign_bridge/test/utils/ros_test_msg.cpp b/ros_ign_bridge/test/utils/ros_test_msg.cpp index 48c8e10d..4dbeab82 100644 --- a/ros_ign_bridge/test/utils/ros_test_msg.cpp +++ b/ros_ign_bridge/test/utils/ros_test_msg.cpp @@ -274,6 +274,9 @@ void createTestMsg(geometry_msgs::msg::PoseWithCovariance & _msg) { createTestMsg(_msg.pose.position); createTestMsg(_msg.pose.orientation); + for (int i = 0 ; i < 36; i++){ + _msg.covariance[i] = i; + } } void compareTestMsg(const std::shared_ptr & _msg) @@ -359,6 +362,9 @@ void createTestMsg(geometry_msgs::msg::TwistWithCovariance & _msg) { createTestMsg(_msg.twist.linear); createTestMsg(_msg.twist.angular); + for (int i = 0 ; i < 36; i++){ + _msg.covariance[i] = i; + } } void compareTestMsg(const std::shared_ptr & _msg) @@ -643,6 +649,10 @@ void createTestMsg(nav_msgs::msg::Odometry & _msg) createTestMsg(_msg.header); createTestMsg(_msg.pose.pose); createTestMsg(_msg.twist.twist); + for (int i = 0; i < 36; i++) { + _msg.pose.covariance[i] = i; + _msg.twist.covariance[i] = i; + } } void compareTestMsg(const std::shared_ptr & _msg) From c74af2839603fec9d1bf01bbc489b9d7bb27f4e3 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 9 Mar 2022 22:03:12 -0800 Subject: [PATCH 12/18] Tests check for nontrivial covariance matrix Signed-off-by: Aditya --- ros_ign_bridge/test/utils/ign_test_msg.cpp | 6 ++++++ ros_ign_bridge/test/utils/ros_test_msg.cpp | 9 +++++++++ 2 files changed, 15 insertions(+) diff --git a/ros_ign_bridge/test/utils/ign_test_msg.cpp b/ros_ign_bridge/test/utils/ign_test_msg.cpp index 5b5f0faf..3e00c09b 100644 --- a/ros_ign_bridge/test/utils/ign_test_msg.cpp +++ b/ros_ign_bridge/test/utils/ign_test_msg.cpp @@ -248,6 +248,9 @@ void compareTestMsg(const std::shared_ptr & { compareTestMsg(std::make_shared(_msg->pose().position())); compareTestMsg(std::make_shared(_msg->pose().orientation())); + for (int i = 0; i < 36; i++) { + EXPECT_EQ(_msg->covariance().data(i), i); + } } void createTestMsg(ignition::msgs::Pose_V & _msg) @@ -305,6 +308,9 @@ void compareTestMsg(const std::shared_ptr & { compareTestMsg(std::make_shared(_msg->twist().linear())); compareTestMsg(std::make_shared(_msg->twist().angular())); + for (int i = 0; i < 36; i++) { + EXPECT_EQ(_msg->covariance().data()[i], i); + } } void createTestMsg(ignition::msgs::Wrench & _msg) diff --git a/ros_ign_bridge/test/utils/ros_test_msg.cpp b/ros_ign_bridge/test/utils/ros_test_msg.cpp index 4dbeab82..27931a5b 100644 --- a/ros_ign_bridge/test/utils/ros_test_msg.cpp +++ b/ros_ign_bridge/test/utils/ros_test_msg.cpp @@ -268,6 +268,9 @@ void compareTestMsg(const geometry_msgs::msg::PoseWithCovariance & _msg) { compareTestMsg(_msg.pose.position); compareTestMsg(_msg.pose.orientation); + for (int i = 0; i < 36; i++) { + EXPECT_EQ(_msg.covariance[i], i); + } } void createTestMsg(geometry_msgs::msg::PoseWithCovariance & _msg) @@ -283,6 +286,9 @@ void compareTestMsg(const std::shared_ptrpose.position); compareTestMsg(_msg->pose.orientation); + for (int i = 0; i < 36; i++) { + EXPECT_EQ(_msg->covariance[i], i); + } } void createTestMsg(geometry_msgs::msg::PoseStamped & _msg) @@ -371,6 +377,9 @@ void compareTestMsg(const std::shared_ptr(_msg->twist.linear)); compareTestMsg(std::make_shared(_msg->twist.angular)); + for (int i = 0; i < 36; i++) { + EXPECT_EQ(_msg->covariance[i], i); + } } void createTestMsg(geometry_msgs::msg::Wrench & _msg) From bb529548ff34e125969b02094384383acfa98135 Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 11 Mar 2022 13:00:30 -0800 Subject: [PATCH 13/18] Changed nav_msgs.cpp to snake_case Signed-off-by: Aditya --- ros_ign_bridge/src/convert/nav_msgs.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ros_ign_bridge/src/convert/nav_msgs.cpp b/ros_ign_bridge/src/convert/nav_msgs.cpp index d157815f..297debe7 100644 --- a/ros_ign_bridge/src/convert/nav_msgs.cpp +++ b/ros_ign_bridge/src/convert/nav_msgs.cpp @@ -44,9 +44,9 @@ convert_ign_to_ros( convert_ign_to_ros(ign_msg.twist(), ros_msg.twist.twist); for (auto i = 0; i < ign_msg.header().data_size(); ++i) { - auto aPair = ign_msg.header().data(i); - if (aPair.key() == "child_frame_id" && aPair.value_size() > 0) { - ros_msg.child_frame_id = frame_id_ign_to_ros(aPair.value(0)); + auto a_pair = ign_msg.header().data(i); + if (a_pair.key() == "child_frame_id" && a_pair.value_size() > 0) { + ros_msg.child_frame_id = frame_id_ign_to_ros(a_pair.value(0)); break; } } @@ -78,9 +78,9 @@ convert_ign_to_ros( convert_ign_to_ros(ign_msg.twist_with_covariance(), ros_msg.twist); for (auto i = 0; i < ign_msg.header().data_size(); ++i) { - auto aPair = ign_msg.header().data(i); - if (aPair.key() == "child_frame_id" && aPair.value_size() > 0) { - ros_msg.child_frame_id = frame_id_ign_to_ros(aPair.value(0)); + auto a_pair = ign_msg.header().data(i); + if (a_pair.key() == "child_frame_id" && a_pair.value_size() > 0) { + ros_msg.child_frame_id = frame_id_ign_to_ros(a_pair.value(0)); break; } } From 81543037fc230ca0de7eb985e97c2e7f7555548d Mon Sep 17 00:00:00 2001 From: Aditya Date: Mon, 25 Apr 2022 11:30:43 -0700 Subject: [PATCH 14/18] Linter fixes Signed-off-by: Aditya --- ros_ign_bridge/src/convert/geometry_msgs.cpp | 2 +- ros_ign_bridge/test/launch/test_ros_subscriber.launch.py | 9 ++++++--- ros_ign_bridge/test/utils/ign_test_msg.cpp | 6 ++++-- 3 files changed, 11 insertions(+), 6 deletions(-) diff --git a/ros_ign_bridge/src/convert/geometry_msgs.cpp b/ros_ign_bridge/src/convert/geometry_msgs.cpp index 7299e4e7..dc0960a7 100644 --- a/ros_ign_bridge/src/convert/geometry_msgs.cpp +++ b/ros_ign_bridge/src/convert/geometry_msgs.cpp @@ -112,7 +112,7 @@ convert_ros_to_ign( const geometry_msgs::msg::PoseWithCovariance & ros_msg, ignition::msgs::PoseWithCovariance & ign_msg) { - convert_ros_to_ign(ros_msg.pose.position, *ign_msg.mutable_pose()->mutable_position()); + convert_ros_to_ign(ros_msg.pose.position, *ign_msg.mutable_pose()->mutable_position()); convert_ros_to_ign(ros_msg.pose.orientation, *ign_msg.mutable_pose()->mutable_orientation()); for (const auto & elem : ros_msg.covariance) { ign_msg.mutable_covariance()->add_data(elem); diff --git a/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py b/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py index 23098ec1..2bc5685a 100644 --- a/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py +++ b/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py @@ -53,13 +53,15 @@ def generate_test_description(): '/clock@rosgraph_msgs/msg/Clock@ignition.msgs.Clock', '/point@geometry_msgs/msg/Point@ignition.msgs.Vector3d', '/pose@geometry_msgs/msg/Pose@ignition.msgs.Pose', - '/pose_with_covariance@geometry_msgs/msg/PoseWithCovariance@ignition.msgs.PoseWithCovariance', + '/pose_with_covariance@geometry_msgs/msg/PoseWithCovariance@\ + ignition.msgs.PoseWithCovariance', '/pose_stamped@geometry_msgs/msg/PoseStamped@ignition.msgs.Pose', '/transform@geometry_msgs/msg/Transform@ignition.msgs.Pose', '/tf2_message@tf2_msgs/msg/TFMessage@ignition.msgs.Pose_V', '/transform_stamped@geometry_msgs/msg/TransformStamped@ignition.msgs.Pose', '/twist@geometry_msgs/msg/Twist@ignition.msgs.Twist', - '/twist_with_covariance@geometry_msgs/msg/TwistWithCovariance@ignition.msgs.TwistWithCovariance', + '/twist_with_covariance@geometry_msgs/msg/TwistWithCovariance@\ + ignition.msgs.TwistWithCovariance', '/wrench@geometry_msgs/msg/Wrench@ignition.msgs.Wrench', '/joint_wrench@ros_ign_interfaces/msg/JointWrench@ignition.msgs.JointWrench', '/entity@ros_ign_interfaces/msg/Entity@ignition.msgs.Entity', @@ -78,7 +80,8 @@ def generate_test_description(): '/magnetic@sensor_msgs/msg/MagneticField@ignition.msgs.Magnetometer', # '/actuators@mav_msgs/msg/Actuators@ignition.msgs.Actuators', '/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry', - '/odometry_with_covariance@nav_msgs/msg/Odometry@ignition.msgs.OdometryWithCovariance', + '/odometry_with_covariance@nav_msgs/msg/Odometry@\ + ignition.msgs.OdometryWithCovariance', '/pointcloud2@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked', '/joint_states@sensor_msgs/msg/JointState@ignition.msgs.Model', '/battery_state@sensor_msgs/msg/BatteryState@ignition.msgs.BatteryState', diff --git a/ros_ign_bridge/test/utils/ign_test_msg.cpp b/ros_ign_bridge/test/utils/ign_test_msg.cpp index 3e00c09b..7ce682e0 100644 --- a/ros_ign_bridge/test/utils/ign_test_msg.cpp +++ b/ros_ign_bridge/test/utils/ign_test_msg.cpp @@ -834,8 +834,10 @@ void createTestMsg(ignition::msgs::OdometryWithCovariance & _msg) void compareTestMsg(const std::shared_ptr & _msg) { compareTestMsg(std::make_shared(_msg->header())); - compareTestMsg(std::make_shared(_msg->pose_with_covariance())); - compareTestMsg(std::make_shared(_msg->twist_with_covariance())); + compareTestMsg(std::make_shared(_msg-> + pose_with_covariance())); + compareTestMsg(std::make_shared(_msg-> + twist_with_covariance())); } void createTestMsg(ignition::msgs::PointCloudPacked & _msg) From b6e3dab28918bd75e89719d0fe54509b97b72714 Mon Sep 17 00:00:00 2001 From: Aditya Date: Mon, 25 Apr 2022 11:50:37 -0700 Subject: [PATCH 15/18] Linter fixes Signed-off-by: Aditya --- ros_ign_bridge/test/launch/test_ign_subscriber.launch.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py b/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py index e5de981d..93f42397 100644 --- a/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py +++ b/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py @@ -53,13 +53,15 @@ def generate_test_description(): '/clock@rosgraph_msgs/msg/Clock@ignition.msgs.Clock', '/point@geometry_msgs/msg/Point@ignition.msgs.Vector3d', '/pose@geometry_msgs/msg/Pose@ignition.msgs.Pose', - '/pose_with_covariance@geometry_msgs/msg/PoseWithCovariance@ignition.msgs.PoseWithCovariance', + '/pose_with_covariance@geometry_msgs/msg/PoseWithCovariance@\ + ignition.msgs.PoseWithCovariance', '/pose_stamped@geometry_msgs/msg/PoseStamped@ignition.msgs.Pose', '/transform@geometry_msgs/msg/Transform@ignition.msgs.Pose', '/tf2_message@tf2_msgs/msg/TFMessage@ignition.msgs.Pose_V', '/transform_stamped@geometry_msgs/msg/TransformStamped@ignition.msgs.Pose', '/twist@geometry_msgs/msg/Twist@ignition.msgs.Twist', - '/twist_with_covariance@geometry_msgs/msg/TwistWithCovariance@ignition.msgs.TwistWithCovariance', + '/twist_with_covariance@geometry_msgs/msg/TwistWithCovariance@\ + ignition.msgs.TwistWithCovariance', '/wrench@geometry_msgs/msg/Wrench@ignition.msgs.Wrench', '/joint_wrench@ros_ign_interfaces/msg/JointWrench@ignition.msgs.JointWrench', '/entity@ros_ign_interfaces/msg/Entity@ignition.msgs.Entity', @@ -78,7 +80,8 @@ def generate_test_description(): '/magnetic@sensor_msgs/msg/MagneticField@ignition.msgs.Magnetometer', # '/actuators@mav_msgs/msg/Actuators@ignition.msgs.Actuators', '/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry', - '/odometry_with_covariance@nav_msgs/msg/Odometry@ignition.msgs.OdometryWithCovariance', + '/odometry_with_covariance@nav_msgs/msg/Odometry@\ + ignition.msgs.OdometryWithCovariance', '/pointcloud2@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked', '/joint_states@sensor_msgs/msg/JointState@ignition.msgs.Model', '/battery_state@sensor_msgs/msg/BatteryState@ignition.msgs.BatteryState', From 3e33a26ec07fd389f5367ab82817f66f102d8516 Mon Sep 17 00:00:00 2001 From: Aditya Date: Mon, 25 Apr 2022 14:17:21 -0700 Subject: [PATCH 16/18] Liner fixes Signed-off-by: Aditya --- ros_ign_bridge/src/convert/geometry_msgs.cpp | 4 ++-- ros_ign_bridge/test/publishers/ros_publisher.cpp | 4 ++-- ros_ign_bridge/test/utils/ign_test_msg.cpp | 14 +++++++++----- ros_ign_bridge/test/utils/ros_test_msg.cpp | 4 ++-- 4 files changed, 15 insertions(+), 11 deletions(-) diff --git a/ros_ign_bridge/src/convert/geometry_msgs.cpp b/ros_ign_bridge/src/convert/geometry_msgs.cpp index dc0960a7..15bd0261 100644 --- a/ros_ign_bridge/src/convert/geometry_msgs.cpp +++ b/ros_ign_bridge/src/convert/geometry_msgs.cpp @@ -129,7 +129,7 @@ convert_ign_to_ros( convert_ign_to_ros(ign_msg.pose().orientation(), ros_msg.pose.orientation); int data_size = ign_msg.covariance().data_size(); if (data_size == 36) { - for (int i = 0 ; i < data_size ; i++) { + for (int i = 0; i < data_size; i++) { auto data = ign_msg.covariance().data()[i]; ros_msg.covariance[i] = data; } @@ -250,7 +250,7 @@ convert_ign_to_ros( convert_ign_to_ros(ign_msg.twist().angular(), ros_msg.twist.angular); int data_size = ign_msg.covariance().data_size(); if (data_size == 36) { - for (int i = 0 ; i < data_size ; i++) { + for (int i = 0; i < data_size; i++) { auto data = ign_msg.covariance().data()[i]; ros_msg.covariance[i] = data; } diff --git a/ros_ign_bridge/test/publishers/ros_publisher.cpp b/ros_ign_bridge/test/publishers/ros_publisher.cpp index 60fbe817..0bef8a57 100644 --- a/ros_ign_bridge/test/publishers/ros_publisher.cpp +++ b/ros_ign_bridge/test/publishers/ros_publisher.cpp @@ -100,7 +100,7 @@ int main(int argc, char ** argv) // geometry_msgs::msg::PoseWithCovariance. auto pose_cov_pub = node->create_publisher( - "pose_with_covariance", 1000); + "pose_with_covariance", 1000); geometry_msgs::msg::PoseWithCovariance pose_cov_msg; ros_ign_bridge::testing::createTestMsg(pose_cov_msg); @@ -131,7 +131,7 @@ int main(int argc, char ** argv) // geometry_msgs::msg::TwistWithCovariance. auto twist_cov_pub = node->create_publisher( - "twist_with_covariance", 1000); + "twist_with_covariance", 1000); geometry_msgs::msg::TwistWithCovariance twist_cov_msg; ros_ign_bridge::testing::createTestMsg(twist_cov_msg); diff --git a/ros_ign_bridge/test/utils/ign_test_msg.cpp b/ros_ign_bridge/test/utils/ign_test_msg.cpp index 7ce682e0..42c3409a 100644 --- a/ros_ign_bridge/test/utils/ign_test_msg.cpp +++ b/ros_ign_bridge/test/utils/ign_test_msg.cpp @@ -299,7 +299,7 @@ void createTestMsg(ignition::msgs::TwistWithCovariance & _msg) _msg.mutable_twist()->mutable_linear()->CopyFrom(linear_msg); _msg.mutable_twist()->mutable_angular()->CopyFrom(angular_msg); - for (int i = 0 ; i < 36 ; i++) { + for (int i = 0; i < 36; i++) { _msg.mutable_covariance()->add_data(i); } } @@ -834,10 +834,14 @@ void createTestMsg(ignition::msgs::OdometryWithCovariance & _msg) void compareTestMsg(const std::shared_ptr & _msg) { compareTestMsg(std::make_shared(_msg->header())); - compareTestMsg(std::make_shared(_msg-> - pose_with_covariance())); - compareTestMsg(std::make_shared(_msg-> - twist_with_covariance())); + compareTestMsg( + std::make_shared( + _msg-> + pose_with_covariance())); + compareTestMsg( + std::make_shared( + _msg-> + twist_with_covariance())); } void createTestMsg(ignition::msgs::PointCloudPacked & _msg) diff --git a/ros_ign_bridge/test/utils/ros_test_msg.cpp b/ros_ign_bridge/test/utils/ros_test_msg.cpp index 27931a5b..79da0add 100644 --- a/ros_ign_bridge/test/utils/ros_test_msg.cpp +++ b/ros_ign_bridge/test/utils/ros_test_msg.cpp @@ -277,7 +277,7 @@ void createTestMsg(geometry_msgs::msg::PoseWithCovariance & _msg) { createTestMsg(_msg.pose.position); createTestMsg(_msg.pose.orientation); - for (int i = 0 ; i < 36; i++){ + for (int i = 0; i < 36; i++) { _msg.covariance[i] = i; } } @@ -368,7 +368,7 @@ void createTestMsg(geometry_msgs::msg::TwistWithCovariance & _msg) { createTestMsg(_msg.twist.linear); createTestMsg(_msg.twist.angular); - for (int i = 0 ; i < 36; i++){ + for (int i = 0; i < 36; i++) { _msg.covariance[i] = i; } } From 80130d2b87955bbce198c5e9ad03956e06da5179 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 27 Apr 2022 15:03:15 -0700 Subject: [PATCH 17/18] Fixed test failures Signed-off-by: Aditya --- .../test/launch/test_ign_subscriber.launch.py | 12 ++++++------ .../test/launch/test_ros_subscriber.launch.py | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py b/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py index 93f42397..2a4a6439 100644 --- a/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py +++ b/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py @@ -53,15 +53,15 @@ def generate_test_description(): '/clock@rosgraph_msgs/msg/Clock@ignition.msgs.Clock', '/point@geometry_msgs/msg/Point@ignition.msgs.Vector3d', '/pose@geometry_msgs/msg/Pose@ignition.msgs.Pose', - '/pose_with_covariance@geometry_msgs/msg/PoseWithCovariance@\ - ignition.msgs.PoseWithCovariance', + '/pose_with_covariance@geometry_msgs/msg/PoseWithCovariance@' + 'ignition.msgs.PoseWithCovariance', '/pose_stamped@geometry_msgs/msg/PoseStamped@ignition.msgs.Pose', '/transform@geometry_msgs/msg/Transform@ignition.msgs.Pose', '/tf2_message@tf2_msgs/msg/TFMessage@ignition.msgs.Pose_V', '/transform_stamped@geometry_msgs/msg/TransformStamped@ignition.msgs.Pose', '/twist@geometry_msgs/msg/Twist@ignition.msgs.Twist', - '/twist_with_covariance@geometry_msgs/msg/TwistWithCovariance@\ - ignition.msgs.TwistWithCovariance', + '/twist_with_covariance@geometry_msgs/msg/TwistWithCovariance@' + 'ignition.msgs.TwistWithCovariance', '/wrench@geometry_msgs/msg/Wrench@ignition.msgs.Wrench', '/joint_wrench@ros_ign_interfaces/msg/JointWrench@ignition.msgs.JointWrench', '/entity@ros_ign_interfaces/msg/Entity@ignition.msgs.Entity', @@ -80,8 +80,8 @@ def generate_test_description(): '/magnetic@sensor_msgs/msg/MagneticField@ignition.msgs.Magnetometer', # '/actuators@mav_msgs/msg/Actuators@ignition.msgs.Actuators', '/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry', - '/odometry_with_covariance@nav_msgs/msg/Odometry@\ - ignition.msgs.OdometryWithCovariance', + '/odometry_with_covariance@nav_msgs/msg/Odometry@' + 'ignition.msgs.OdometryWithCovariance', '/pointcloud2@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked', '/joint_states@sensor_msgs/msg/JointState@ignition.msgs.Model', '/battery_state@sensor_msgs/msg/BatteryState@ignition.msgs.BatteryState', diff --git a/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py b/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py index 2bc5685a..3d2e78f6 100644 --- a/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py +++ b/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py @@ -53,15 +53,15 @@ def generate_test_description(): '/clock@rosgraph_msgs/msg/Clock@ignition.msgs.Clock', '/point@geometry_msgs/msg/Point@ignition.msgs.Vector3d', '/pose@geometry_msgs/msg/Pose@ignition.msgs.Pose', - '/pose_with_covariance@geometry_msgs/msg/PoseWithCovariance@\ - ignition.msgs.PoseWithCovariance', + '/pose_with_covariance@geometry_msgs/msg/PoseWithCovariance@' + 'ignition.msgs.PoseWithCovariance', '/pose_stamped@geometry_msgs/msg/PoseStamped@ignition.msgs.Pose', '/transform@geometry_msgs/msg/Transform@ignition.msgs.Pose', '/tf2_message@tf2_msgs/msg/TFMessage@ignition.msgs.Pose_V', '/transform_stamped@geometry_msgs/msg/TransformStamped@ignition.msgs.Pose', '/twist@geometry_msgs/msg/Twist@ignition.msgs.Twist', - '/twist_with_covariance@geometry_msgs/msg/TwistWithCovariance@\ - ignition.msgs.TwistWithCovariance', + '/twist_with_covariance@geometry_msgs/msg/TwistWithCovariance@' + 'ignition.msgs.TwistWithCovariance', '/wrench@geometry_msgs/msg/Wrench@ignition.msgs.Wrench', '/joint_wrench@ros_ign_interfaces/msg/JointWrench@ignition.msgs.JointWrench', '/entity@ros_ign_interfaces/msg/Entity@ignition.msgs.Entity', @@ -80,8 +80,8 @@ def generate_test_description(): '/magnetic@sensor_msgs/msg/MagneticField@ignition.msgs.Magnetometer', # '/actuators@mav_msgs/msg/Actuators@ignition.msgs.Actuators', '/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry', - '/odometry_with_covariance@nav_msgs/msg/Odometry@\ - ignition.msgs.OdometryWithCovariance', + '/odometry_with_covariance@nav_msgs/msg/Odometry@' + 'ignition.msgs.OdometryWithCovariance', '/pointcloud2@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked', '/joint_states@sensor_msgs/msg/JointState@ignition.msgs.Model', '/battery_state@sensor_msgs/msg/BatteryState@ignition.msgs.BatteryState', From 557a6806fd99df5fda14d1156a4f508e2791c80a Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 27 Apr 2022 15:36:42 -0700 Subject: [PATCH 18/18] Flake8 fixes Signed-off-by: Aditya --- ros_ign_bridge/test/launch/test_ign_subscriber.launch.py | 6 +++--- ros_ign_bridge/test/launch/test_ros_subscriber.launch.py | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py b/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py index 2a4a6439..2df7cb00 100644 --- a/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py +++ b/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py @@ -54,14 +54,14 @@ def generate_test_description(): '/point@geometry_msgs/msg/Point@ignition.msgs.Vector3d', '/pose@geometry_msgs/msg/Pose@ignition.msgs.Pose', '/pose_with_covariance@geometry_msgs/msg/PoseWithCovariance@' - 'ignition.msgs.PoseWithCovariance', + 'ignition.msgs.PoseWithCovariance', '/pose_stamped@geometry_msgs/msg/PoseStamped@ignition.msgs.Pose', '/transform@geometry_msgs/msg/Transform@ignition.msgs.Pose', '/tf2_message@tf2_msgs/msg/TFMessage@ignition.msgs.Pose_V', '/transform_stamped@geometry_msgs/msg/TransformStamped@ignition.msgs.Pose', '/twist@geometry_msgs/msg/Twist@ignition.msgs.Twist', '/twist_with_covariance@geometry_msgs/msg/TwistWithCovariance@' - 'ignition.msgs.TwistWithCovariance', + 'ignition.msgs.TwistWithCovariance', '/wrench@geometry_msgs/msg/Wrench@ignition.msgs.Wrench', '/joint_wrench@ros_ign_interfaces/msg/JointWrench@ignition.msgs.JointWrench', '/entity@ros_ign_interfaces/msg/Entity@ignition.msgs.Entity', @@ -81,7 +81,7 @@ def generate_test_description(): # '/actuators@mav_msgs/msg/Actuators@ignition.msgs.Actuators', '/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry', '/odometry_with_covariance@nav_msgs/msg/Odometry@' - 'ignition.msgs.OdometryWithCovariance', + 'ignition.msgs.OdometryWithCovariance', '/pointcloud2@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked', '/joint_states@sensor_msgs/msg/JointState@ignition.msgs.Model', '/battery_state@sensor_msgs/msg/BatteryState@ignition.msgs.BatteryState', diff --git a/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py b/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py index 3d2e78f6..93415b7a 100644 --- a/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py +++ b/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py @@ -54,14 +54,14 @@ def generate_test_description(): '/point@geometry_msgs/msg/Point@ignition.msgs.Vector3d', '/pose@geometry_msgs/msg/Pose@ignition.msgs.Pose', '/pose_with_covariance@geometry_msgs/msg/PoseWithCovariance@' - 'ignition.msgs.PoseWithCovariance', + 'ignition.msgs.PoseWithCovariance', '/pose_stamped@geometry_msgs/msg/PoseStamped@ignition.msgs.Pose', '/transform@geometry_msgs/msg/Transform@ignition.msgs.Pose', '/tf2_message@tf2_msgs/msg/TFMessage@ignition.msgs.Pose_V', '/transform_stamped@geometry_msgs/msg/TransformStamped@ignition.msgs.Pose', '/twist@geometry_msgs/msg/Twist@ignition.msgs.Twist', '/twist_with_covariance@geometry_msgs/msg/TwistWithCovariance@' - 'ignition.msgs.TwistWithCovariance', + 'ignition.msgs.TwistWithCovariance', '/wrench@geometry_msgs/msg/Wrench@ignition.msgs.Wrench', '/joint_wrench@ros_ign_interfaces/msg/JointWrench@ignition.msgs.JointWrench', '/entity@ros_ign_interfaces/msg/Entity@ignition.msgs.Entity', @@ -81,7 +81,7 @@ def generate_test_description(): # '/actuators@mav_msgs/msg/Actuators@ignition.msgs.Actuators', '/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry', '/odometry_with_covariance@nav_msgs/msg/Odometry@' - 'ignition.msgs.OdometryWithCovariance', + 'ignition.msgs.OdometryWithCovariance', '/pointcloud2@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked', '/joint_states@sensor_msgs/msg/JointState@ignition.msgs.Model', '/battery_state@sensor_msgs/msg/BatteryState@ignition.msgs.BatteryState',