Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Bridge point cloud packed #28

Merged
merged 4 commits into from
Jul 16, 2019
Merged

Bridge point cloud packed #28

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
32e8656
Bridge point cloud packed
nkoenig Jul 13, 2019
b13f677
tests and reverse bridge for pointcloud
Jul 16, 2019
3f487d4
Changelog
Jul 16, 2019
d855168
0.4.0
Jul 16, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions ros1_ign/CHANGELOG.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,9 @@
Changelog for package ros1_ign
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

0.4.0 (2019-07-16)
------------------

0.3.1 (2019-07-01)
------------------

Expand Down
2 changes: 1 addition & 1 deletion ros1_ign/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>ros1_ign</name>
<version>0.3.1</version>
<version>0.4.0</version>
<description>Meta-package containing interfaces for using ROS with <a href="https://ignitionrobotics.org">Ignition</a> simulation.</description>
<license>Apache 2.0</license>
<maintainer email="louise@openrobotics.org">Louise Poubel</maintainer>
Expand Down
6 changes: 6 additions & 0 deletions ros1_ign_bridge/CHANGELOG.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
------------------

Expand Down
8 changes: 4 additions & 4 deletions ros1_ign_bridge/include/ros1_ign_bridge/builtin_interfaces_factories.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
4 changes: 2 additions & 2 deletions ros1_ign_bridge/include/ros1_ign_bridge/convert_builtin_interfaces.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion ros1_ign_bridge/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<package format="2">
<name>ros1_ign_bridge</name>
<version>0.3.1</version>
<version>0.4.0</version>
<description>Bridge communication between ROS 1 and Ignition Transport</description>
<license>Apache 2.0</license>
<maintainer email="caguero@openrobotics.org">Carlos Agüero</maintainer>
Expand Down
12 changes: 6 additions & 6 deletions ros1_ign_bridge/src/builtin_interfaces_factories.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down Expand Up @@ -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);
}
Expand All @@ -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);
Expand Down
98 changes: 92 additions & 6 deletions ros1_ign_bridge/src/convert_builtin_interfaces.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Since the messages are so similar, it shouldn't be a lot of trouble to add the reverse conversion, and some tests with some dummy data.

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
1 change: 1 addition & 0 deletions ros1_ign_bridge/test/launch/test_ign_subscriber.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
/>

Expand Down
1 change: 1 addition & 0 deletions ros1_ign_bridge/test/launch/test_ros1_subscriber.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
/>

Expand Down
8 changes: 8 additions & 0 deletions ros1_ign_bridge/test/publishers/ign_publisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<ignition::msgs::PointCloudPacked>(
"pointcloud2");
ignition::msgs::PointCloudPacked pointcloudpacked_msg;
ros1_ign_bridge::testing::createTestMsg(pointcloudpacked_msg);


// Publish messages at 1Hz.
while (!g_terminatePub)
{
Expand All @@ -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));
}
Expand Down
8 changes: 8 additions & 0 deletions ros1_ign_bridge/test/publishers/ros1_publisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/MagneticField.h>
#include <sensor_msgs/PointCloud2.h>
#include "../test_utils.h"

//////////////////////////////////////////////////
Expand Down Expand Up @@ -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<sensor_msgs::PointCloud2>("pointcloud2", 1000);
sensor_msgs::PointCloud2 pointcloud2_msg;
ros1_ign_bridge::testing::createTestMsg(pointcloud2_msg);

while (ros::ok())
{
// Publish all messages.
Expand All @@ -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();
Expand Down
12 changes: 12 additions & 0 deletions ros1_ign_bridge/test/subscribers/ign_subscriber.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -299,6 +299,18 @@ TEST(IgnSubscriberTest, JointStates)
EXPECT_TRUE(client.callbackExecuted);
}

/////////////////////////////////////////////////
TEST(IgnSubscriberTest, PointCloudPacked)
{
MyTestClass<ignition::msgs::PointCloudPacked> 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)
{
Expand Down
13 changes: 13 additions & 0 deletions ros1_ign_bridge/test/subscribers/ros1_subscriber.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/MagneticField.h>
#include <sensor_msgs/PointCloud2.h>
#include <chrono>
#include "../test_utils.h"

Expand Down Expand Up @@ -320,6 +321,18 @@ TEST(ROS1SubscriberTest, Odometry)
EXPECT_TRUE(client.callbackExecuted);
}

/////////////////////////////////////////////////
TEST(ROS1SubscriberTest, PointCloud2)
{
MyTestClass<sensor_msgs::PointCloud2> 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)
{
Expand Down
Loading