diff --git a/ros1_ign/CHANGELOG.rst b/ros1_ign/CHANGELOG.rst index 30861158..c62b4786 100644 --- a/ros1_ign/CHANGELOG.rst +++ b/ros1_ign/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros1_ign ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2019-07-16) +------------------ + 0.3.1 (2019-07-01) ------------------ diff --git a/ros1_ign/package.xml b/ros1_ign/package.xml index d6a83b0c..65e31a81 100644 --- a/ros1_ign/package.xml +++ b/ros1_ign/package.xml @@ -1,7 +1,7 @@ ros1_ign - 0.3.1 + 0.4.0 Meta-package containing interfaces for using ROS with Ignition simulation. Apache 2.0 Louise Poubel diff --git a/ros1_ign_bridge/CHANGELOG.rst b/ros1_ign_bridge/CHANGELOG.rst index b1b211cb..308a0ea6 100644 --- a/ros1_ign_bridge/CHANGELOG.rst +++ b/ros1_ign_bridge/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros1_ign_bridge ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2019-07-16) +------------------ +* tests and reverse bridge for pointcloud +* Bridge point cloud packed +* Contributors: Nate Koenig + 0.3.1 (2019-07-01) ------------------ diff --git a/ros1_ign_bridge/include/ros1_ign_bridge/builtin_interfaces_factories.hpp b/ros1_ign_bridge/include/ros1_ign_bridge/builtin_interfaces_factories.hpp index 7e0b6246..372efd3f 100644 --- a/ros1_ign_bridge/include/ros1_ign_bridge/builtin_interfaces_factories.hpp +++ b/ros1_ign_bridge/include/ros1_ign_bridge/builtin_interfaces_factories.hpp @@ -449,18 +449,18 @@ template<> void Factory< sensor_msgs::PointCloud2, - ignition::msgs::PointCloud + ignition::msgs::PointCloudPacked >::convert_1_to_ign( const sensor_msgs::PointCloud2 & ros1_msg, - ignition::msgs::PointCloud & ign_msg); + ignition::msgs::PointCloudPacked & ign_msg); template<> void Factory< sensor_msgs::PointCloud2, - ignition::msgs::PointCloud + ignition::msgs::PointCloudPacked >::convert_ign_to_1( - const ignition::msgs::PointCloud & ign_msg, + const ignition::msgs::PointCloudPacked & ign_msg, sensor_msgs::PointCloud2 & ros1_msg); } // namespace ros1_ign_bridge diff --git a/ros1_ign_bridge/include/ros1_ign_bridge/convert_builtin_interfaces.hpp b/ros1_ign_bridge/include/ros1_ign_bridge/convert_builtin_interfaces.hpp index c52b45cf..3f6ab5e1 100644 --- a/ros1_ign_bridge/include/ros1_ign_bridge/convert_builtin_interfaces.hpp +++ b/ros1_ign_bridge/include/ros1_ign_bridge/convert_builtin_interfaces.hpp @@ -309,12 +309,12 @@ template<> void convert_1_to_ign( const sensor_msgs::PointCloud2 & ros1_msg, - ignition::msgs::PointCloud & ign_msg); + ignition::msgs::PointCloudPacked & ign_msg); template<> void convert_ign_to_1( - const ignition::msgs::PointCloud & ign_msg, + const ignition::msgs::PointCloudPacked & ign_msg, sensor_msgs::PointCloud2 & ros1_msg); } // namespace ros1_ign_bridge diff --git a/ros1_ign_bridge/package.xml b/ros1_ign_bridge/package.xml index 86164620..fbfceda4 100644 --- a/ros1_ign_bridge/package.xml +++ b/ros1_ign_bridge/package.xml @@ -1,6 +1,6 @@ ros1_ign_bridge - 0.3.1 + 0.4.0 Bridge communication between ROS 1 and Ignition Transport Apache 2.0 Carlos Agüero diff --git a/ros1_ign_bridge/src/builtin_interfaces_factories.cpp b/ros1_ign_bridge/src/builtin_interfaces_factories.cpp index e5b14a41..5a7fe798 100644 --- a/ros1_ign_bridge/src/builtin_interfaces_factories.cpp +++ b/ros1_ign_bridge/src/builtin_interfaces_factories.cpp @@ -261,12 +261,12 @@ get_factory_builtin_interfaces( } if ( (ros1_type_name == "sensor_msgs/PointCloud2" || ros1_type_name == "") && - ign_type_name == "ignition.msgs.PointCloud") + ign_type_name == "ignition.msgs.PointCloudPacked") { return std::make_shared< Factory< sensor_msgs::PointCloud2, - ignition::msgs::PointCloud + ignition::msgs::PointCloudPacked > >("sensor_msgs/PointCloud2", ign_type_name); } @@ -801,10 +801,10 @@ template<> void Factory< sensor_msgs::PointCloud2, - ignition::msgs::PointCloud + ignition::msgs::PointCloudPacked >::convert_1_to_ign( const sensor_msgs::PointCloud2 & ros1_msg, - ignition::msgs::PointCloud & ign_msg) + ignition::msgs::PointCloudPacked & ign_msg) { ros1_ign_bridge::convert_1_to_ign(ros1_msg, ign_msg); } @@ -813,9 +813,9 @@ template<> void Factory< sensor_msgs::PointCloud2, - ignition::msgs::PointCloud + ignition::msgs::PointCloudPacked >::convert_ign_to_1( - const ignition::msgs::PointCloud & ign_msg, + const ignition::msgs::PointCloudPacked & ign_msg, sensor_msgs::PointCloud2 & ros1_msg) { ros1_ign_bridge::convert_ign_to_1(ign_msg, ros1_msg); diff --git a/ros1_ign_bridge/src/convert_builtin_interfaces.cpp b/ros1_ign_bridge/src/convert_builtin_interfaces.cpp index d4c7bde5..a58b2ea6 100644 --- a/ros1_ign_bridge/src/convert_builtin_interfaces.cpp +++ b/ros1_ign_bridge/src/convert_builtin_interfaces.cpp @@ -904,24 +904,110 @@ template<> void convert_1_to_ign( const sensor_msgs::PointCloud2 & ros1_msg, - ignition::msgs::PointCloud & ign_msg) + ignition::msgs::PointCloudPacked &ign_msg) { convert_1_to_ign(ros1_msg.header, (*ign_msg.mutable_header())); - std::cerr << "Unsupported conversion from [sensor_msgs::PointCloud2] to " - << "[ignition::msgs::PointCloud]" << std::endl; + ign_msg.set_height(ros1_msg.height); + ign_msg.set_width(ros1_msg.width); + ign_msg.set_is_bigendian(ros1_msg.is_bigendian); + ign_msg.set_point_step(ros1_msg.point_step); + ign_msg.set_row_step(ros1_msg.row_step); + ign_msg.set_is_dense(ros1_msg.is_dense); + ign_msg.mutable_data()->resize(ros1_msg.data.size()); + memcpy(ign_msg.mutable_data()->data(), ros1_msg.data.data(), + ros1_msg.data.size()); + + for (unsigned int i = 0; i < ros1_msg.fields.size(); ++i) + { + ignition::msgs::PointCloudPacked::Field *pf = ign_msg.add_field(); + pf->set_name(ros1_msg.fields[i].name); + pf->set_count(ros1_msg.fields[i].count); + pf->set_offset(ros1_msg.fields[i].offset); + switch (ros1_msg.fields[i].datatype) + { + default: + case sensor_msgs::PointField::INT8: + pf->set_datatype(ignition::msgs::PointCloudPacked::Field::INT8); + break; + case sensor_msgs::PointField::UINT8: + pf->set_datatype(ignition::msgs::PointCloudPacked::Field::UINT8); + break; + case sensor_msgs::PointField::INT16: + pf->set_datatype(ignition::msgs::PointCloudPacked::Field::INT16); + break; + case sensor_msgs::PointField::UINT16: + pf->set_datatype(ignition::msgs::PointCloudPacked::Field::UINT16); + break; + case sensor_msgs::PointField::INT32: + pf->set_datatype(ignition::msgs::PointCloudPacked::Field::INT32); + break; + case sensor_msgs::PointField::UINT32: + pf->set_datatype(ignition::msgs::PointCloudPacked::Field::UINT32); + break; + case sensor_msgs::PointField::FLOAT32: + pf->set_datatype(ignition::msgs::PointCloudPacked::Field::FLOAT32); + break; + case sensor_msgs::PointField::FLOAT64: + pf->set_datatype(ignition::msgs::PointCloudPacked::Field::FLOAT64); + break; + }; + } } template<> void convert_ign_to_1( - const ignition::msgs::PointCloud & ign_msg, + const ignition::msgs::PointCloudPacked & ign_msg, sensor_msgs::PointCloud2 & ros1_msg) { convert_ign_to_1(ign_msg.header(), ros1_msg.header); - std::cerr << "Unsupported conversion from [ignition::msgs::PointCloud] to " - << "[sensor_msgs::PointCloud2]" << std::endl; + ros1_msg.height = ign_msg.height(); + ros1_msg.width = ign_msg.width(); + ros1_msg.is_bigendian = ign_msg.is_bigendian(); + ros1_msg.point_step = ign_msg.point_step(); + ros1_msg.row_step = ign_msg.row_step(); + ros1_msg.is_dense = ign_msg.is_dense(); + ros1_msg.data.resize(ign_msg.data().size()); + memcpy(ros1_msg.data.data(), ign_msg.data().c_str(), ign_msg.data().size()); + + for (int i = 0; i < ign_msg.field_size(); ++i) + { + sensor_msgs::PointField pf; + pf.name = ign_msg.field(i).name(); + pf.count = ign_msg.field(i).count(); + pf.offset = ign_msg.field(i).offset(); + switch (ign_msg.field(i).datatype()) + { + default: + case ignition::msgs::PointCloudPacked::Field::INT8: + pf.datatype = sensor_msgs::PointField::INT8; + break; + case ignition::msgs::PointCloudPacked::Field::UINT8: + pf.datatype = sensor_msgs::PointField::UINT8; + break; + case ignition::msgs::PointCloudPacked::Field::INT16: + pf.datatype = sensor_msgs::PointField::INT16; + break; + case ignition::msgs::PointCloudPacked::Field::UINT16: + pf.datatype = sensor_msgs::PointField::UINT16; + break; + case ignition::msgs::PointCloudPacked::Field::INT32: + pf.datatype = sensor_msgs::PointField::INT32; + break; + case ignition::msgs::PointCloudPacked::Field::UINT32: + pf.datatype = sensor_msgs::PointField::UINT32; + break; + case ignition::msgs::PointCloudPacked::Field::FLOAT32: + pf.datatype = sensor_msgs::PointField::FLOAT32; + break; + case ignition::msgs::PointCloudPacked::Field::FLOAT64: + pf.datatype = sensor_msgs::PointField::FLOAT64; + break; + }; + ros1_msg.fields.push_back(pf); + } } } // namespace ros1_ign_bridge diff --git a/ros1_ign_bridge/test/launch/test_ign_subscriber.launch b/ros1_ign_bridge/test/launch/test_ign_subscriber.launch index b358bb16..935742d0 100644 --- a/ros1_ign_bridge/test/launch/test_ign_subscriber.launch +++ b/ros1_ign_bridge/test/launch/test_ign_subscriber.launch @@ -23,6 +23,7 @@ /magnetic@sensor_msgs/MagneticField@ignition.msgs.Magnetometer /actuators@mav_msgs/Actuators@ignition.msgs.Actuators /odometry@nav_msgs/Odometry@ignition.msgs.Odometry + /pointcloud2@sensor_msgs/PointCloud2@ignition.msgs.PointCloudPacked /joint_states@sensor_msgs/JointState@ignition.msgs.Model" /> diff --git a/ros1_ign_bridge/test/launch/test_ros1_subscriber.launch b/ros1_ign_bridge/test/launch/test_ros1_subscriber.launch index 27794447..3b007f42 100644 --- a/ros1_ign_bridge/test/launch/test_ros1_subscriber.launch +++ b/ros1_ign_bridge/test/launch/test_ros1_subscriber.launch @@ -23,6 +23,7 @@ /magnetic@sensor_msgs/MagneticField@ignition.msgs.Magnetometer /actuators@mav_msgs/Actuators@ignition.msgs.Actuators /odometry@nav_msgs/Odometry@ignition.msgs.Odometry + /pointcloud2@sensor_msgs/PointCloud2@ignition.msgs.PointCloudPacked /joint_states@sensor_msgs/JointState@ignition.msgs.Model" /> diff --git a/ros1_ign_bridge/test/publishers/ign_publisher.cpp b/ros1_ign_bridge/test/publishers/ign_publisher.cpp index 7fbfef6d..571a315e 100644 --- a/ros1_ign_bridge/test/publishers/ign_publisher.cpp +++ b/ros1_ign_bridge/test/publishers/ign_publisher.cpp @@ -156,6 +156,13 @@ int main(int /*argc*/, char **/*argv*/) ignition::msgs::Twist twist_msg; ros1_ign_bridge::testing::createTestMsg(twist_msg); + // ignition::msgs::PointCloudPacked. + auto pointcloudpacked_pub = node.Advertise( + "pointcloud2"); + ignition::msgs::PointCloudPacked pointcloudpacked_msg; + ros1_ign_bridge::testing::createTestMsg(pointcloudpacked_msg); + + // Publish messages at 1Hz. while (!g_terminatePub) { @@ -180,6 +187,7 @@ int main(int /*argc*/, char **/*argv*/) odometry_pub.Publish(odometry_msg); joint_states_pub.Publish(joint_states_msg); twist_pub.Publish(twist_msg); + pointcloudpacked_pub.Publish(pointcloudpacked_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); } diff --git a/ros1_ign_bridge/test/publishers/ros1_publisher.cpp b/ros1_ign_bridge/test/publishers/ros1_publisher.cpp index e62fdf88..beb2c8f0 100644 --- a/ros1_ign_bridge/test/publishers/ros1_publisher.cpp +++ b/ros1_ign_bridge/test/publishers/ros1_publisher.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include "../test_utils.h" ////////////////////////////////////////////////// @@ -167,6 +168,12 @@ int main(int argc, char ** argv) sensor_msgs::MagneticField magnetic_msg; ros1_ign_bridge::testing::createTestMsg(magnetic_msg); + // sensor_msgs::PointCloud2. + ros::Publisher pointcloud2_pub = + n.advertise("pointcloud2", 1000); + sensor_msgs::PointCloud2 pointcloud2_msg; + ros1_ign_bridge::testing::createTestMsg(pointcloud2_msg); + while (ros::ok()) { // Publish all messages. @@ -191,6 +198,7 @@ int main(int argc, char ** argv) laserscan_pub.publish(laserscan_msg); magnetic_pub.publish(magnetic_msg); joint_states_pub.publish(joint_states_msg); + pointcloud2_pub.publish(pointcloud2_msg); ros::spinOnce(); loop_rate.sleep(); diff --git a/ros1_ign_bridge/test/subscribers/ign_subscriber.cpp b/ros1_ign_bridge/test/subscribers/ign_subscriber.cpp index c54a1054..ae162fa6 100644 --- a/ros1_ign_bridge/test/subscribers/ign_subscriber.cpp +++ b/ros1_ign_bridge/test/subscribers/ign_subscriber.cpp @@ -299,6 +299,18 @@ TEST(IgnSubscriberTest, JointStates) EXPECT_TRUE(client.callbackExecuted); } +///////////////////////////////////////////////// +TEST(IgnSubscriberTest, PointCloudPacked) +{ + MyTestClass client("pointcloud2"); + + using namespace std::chrono_literals; + ros1_ign_bridge::testing::waitUntilBoolVar( + client.callbackExecuted, 10ms, 200); + + EXPECT_TRUE(client.callbackExecuted); +} + ///////////////////////////////////////////////// int main(int argc, char **argv) { diff --git a/ros1_ign_bridge/test/subscribers/ros1_subscriber.cpp b/ros1_ign_bridge/test/subscribers/ros1_subscriber.cpp index 4c3ab43f..5de5aa52 100644 --- a/ros1_ign_bridge/test/subscribers/ros1_subscriber.cpp +++ b/ros1_ign_bridge/test/subscribers/ros1_subscriber.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include "../test_utils.h" @@ -320,6 +321,18 @@ TEST(ROS1SubscriberTest, Odometry) EXPECT_TRUE(client.callbackExecuted); } +///////////////////////////////////////////////// +TEST(ROS1SubscriberTest, PointCloud2) +{ + MyTestClass client("pointcloud2"); + + using namespace std::chrono_literals; + ros1_ign_bridge::testing::waitUntilBoolVarAndSpin( + client.callbackExecuted, 10ms, 200); + + EXPECT_TRUE(client.callbackExecuted); +} + ///////////////////////////////////////////////// int main(int argc, char **argv) { diff --git a/ros1_ign_bridge/test/test_utils.h b/ros1_ign_bridge/test/test_utils.h index 655f18c6..859fad30 100644 --- a/ros1_ign_bridge/test/test_utils.h +++ b/ros1_ign_bridge/test/test_utils.h @@ -41,6 +41,8 @@ #include #include #include +#include +#include #include #include #include @@ -664,6 +666,75 @@ namespace testing EXPECT_FLOAT_EQ(0, _msg.magnetic_field_covariance[i]); } + /// \brief Create a message used for testing. + /// \param[out] _msg The message populated. + void createTestMsg(sensor_msgs::PointCloud2 &_msg) + { + createTestMsg(_msg.header); + + sensor_msgs::PointField field; + field.name = "x"; + field.offset = 0; + field.datatype = sensor_msgs::PointField::FLOAT32; + field.count = 1; + _msg.fields.push_back(field); + + uint32_t height = 4; + uint32_t width = 2; + + _msg.height = height; + _msg.width = width; + _msg.is_bigendian = false; + _msg.point_step = 4; + _msg.row_step = 4 * width; + _msg.is_dense = true; + + _msg.data.resize(_msg.row_step * _msg.height); + uint8_t *msgBufferIndex = _msg.data.data(); + + for (uint32_t j = 0; j < height; ++j) + { + for (uint32_t i = 0; i < width; ++i) + { + *reinterpret_cast(msgBufferIndex + _msg.fields[0].offset) = + j * width + i; + msgBufferIndex += _msg.point_step; + } + } + } + + /// \brief Compare a message with the populated for testing. + /// \param[in] _msg The message to compare. + void compareTestMsg(const sensor_msgs::PointCloud2 &_msg) + { + compareTestMsg(_msg.header); + + uint32_t height = 4; + uint32_t width = 2; + + EXPECT_EQ(height, _msg.height); + EXPECT_EQ(width, _msg.width); + EXPECT_FALSE(_msg.is_bigendian); + EXPECT_EQ(4u, _msg.point_step); + EXPECT_EQ(4U * width, _msg.row_step); + EXPECT_TRUE(_msg.is_dense); + + unsigned char *msgBufferIndex = + const_cast(_msg.data.data()); + + for (uint32_t j = 0; j < height; ++j) + { + for (uint32_t i = 0; i < width; ++i) + { + float *value = + reinterpret_cast(msgBufferIndex + _msg.fields[0].offset); + + EXPECT_FLOAT_EQ(static_cast(j * width + i), *value); + msgBufferIndex += _msg.point_step; + } + } + } + ////////////////////////////////////////////////// /// Ignition::msgs test utils ////////////////////////////////////////////////// @@ -1253,6 +1324,77 @@ namespace testing compareTestMsg(_msg.pose()); compareTestMsg(_msg.twist()); } + + /// \brief Create a message used for testing. + /// \param[out] _msg The message populated. + void createTestMsg(ignition::msgs::PointCloudPacked &_msg) + { + ignition::msgs::Header header_msg; + + createTestMsg(header_msg); + + _msg.mutable_header()->CopyFrom(header_msg); + ignition::msgs::PointCloudPacked::Field *field = _msg.add_field(); + field->set_name("x"); + field->set_offset(0); + field->set_datatype(ignition::msgs::PointCloudPacked::Field::FLOAT32); + field->set_count(1); + + uint32_t height = 4; + uint32_t width = 2; + + _msg.set_height(height); + _msg.set_width(width); + _msg.set_is_bigendian(false); + _msg.set_point_step(4); + _msg.set_row_step(4 * width); + _msg.set_is_dense(true); + + std::string *msgBuffer = _msg.mutable_data(); + msgBuffer->resize(_msg.row_step() * _msg.height()); + char *msgBufferIndex = msgBuffer->data(); + + for (uint32_t j = 0; j < height; ++j) + { + for (uint32_t i = 0; i < width; ++i) + { + *reinterpret_cast(msgBufferIndex + _msg.field(0).offset()) = + j * width + i; + msgBufferIndex += _msg.point_step(); + } + } + } + + /// \brief Compare a message with the populated for testing. + /// \param[in] _msg The message to compare. + void compareTestMsg(const ignition::msgs::PointCloudPacked &_msg) + { + compareTestMsg(_msg.header()); + + uint32_t height = 4; + uint32_t width = 2; + + EXPECT_EQ(height, _msg.height()); + EXPECT_EQ(width, _msg.width()); + EXPECT_FALSE(_msg.is_bigendian()); + EXPECT_EQ(4u, _msg.point_step()); + EXPECT_EQ(4U * width, _msg.row_step()); + EXPECT_TRUE(_msg.is_dense()); + + char *msgBufferIndex = const_cast(_msg.data().data()); + + for (uint32_t j = 0; j < height; ++j) + { + for (uint32_t i = 0; i < width; ++i) + { + float *value = + reinterpret_cast(msgBufferIndex + _msg.field(0).offset()); + + EXPECT_FLOAT_EQ(static_cast(j * width + i), *value); + msgBufferIndex += _msg.point_step(); + } + } + } } } diff --git a/ros1_ign_gazebo_demos/CHANGELOG.rst b/ros1_ign_gazebo_demos/CHANGELOG.rst index 8e9762e2..0cb6d33b 100644 --- a/ros1_ign_gazebo_demos/CHANGELOG.rst +++ b/ros1_ign_gazebo_demos/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros1_ign_gazebo_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2019-07-16) +------------------ + 0.3.1 (2019-07-01) ------------------ * Merge pull request `#24 `_ from osrf/fix_dep diff --git a/ros1_ign_gazebo_demos/package.xml b/ros1_ign_gazebo_demos/package.xml index 4cc10c55..0c91f64a 100644 --- a/ros1_ign_gazebo_demos/package.xml +++ b/ros1_ign_gazebo_demos/package.xml @@ -1,6 +1,6 @@ ros1_ign_gazebo_demos - 0.3.1 + 0.4.0 Demos using Ignition Gazebo simulation with ROS 1. Apache 2.0 Louise Poubel diff --git a/ros1_ign_point_cloud/CHANGELOG.rst b/ros1_ign_point_cloud/CHANGELOG.rst index 19e32e9b..012c538e 100644 --- a/ros1_ign_point_cloud/CHANGELOG.rst +++ b/ros1_ign_point_cloud/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros1_ign_point_cloud ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2019-07-16) +------------------ + 0.3.1 (2019-07-01) ------------------ diff --git a/ros1_ign_point_cloud/package.xml b/ros1_ign_point_cloud/package.xml index 5b62c7f1..102656e1 100644 --- a/ros1_ign_point_cloud/package.xml +++ b/ros1_ign_point_cloud/package.xml @@ -1,6 +1,6 @@ ros1_ign_point_cloud - 0.3.1 + 0.4.0 Point cloud utilities for Ignition simulation with ROS 1. Apache 2.0 Louise Poubel