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

Adds pose and twist with covariance messages bridging #222

Merged
merged 18 commits into from
Apr 27, 2022
Merged

Adds pose and twist with covariance messages bridging #222

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
7ce0ae7
Changed templates
adityapande-1995 Mar 7, 2022
1403fc5
Removed debug statements
adityapande-1995 Mar 7, 2022
20b24e8
Added twist with covariance msg bridge
adityapande-1995 Mar 7, 2022
57681a3
Populate cov matrix
adityapande-1995 Mar 8, 2022
9d345b1
bridged odometry (ros) and odometry with covariance (ign)
adityapande-1995 Mar 8, 2022
a2aff09
Added tests for PoseWithCovariance
adityapande-1995 Mar 9, 2022
1bd8b1d
Added tests for TwistWithCovariance
adityapande-1995 Mar 9, 2022
e1f9cec
Added tests for OdometryWithCovariance
adityapande-1995 Mar 9, 2022
c7fc616
Updated README, check size of Float_V before conversion to ros msg
adityapande-1995 Mar 9, 2022
a1bec61
linter
adityapande-1995 Mar 9, 2022
4851f46
Populate covariance matrix with nontrivial values
adityapande-1995 Mar 10, 2022
c74af28
Tests check for nontrivial covariance matrix
adityapande-1995 Mar 10, 2022
bb52954
Changed nav_msgs.cpp to snake_case
adityapande-1995 Mar 11, 2022
8154303
Linter fixes
adityapande-1995 Apr 25, 2022
b6e3dab
Linter fixes
adityapande-1995 Apr 25, 2022
3e33a26
Liner fixes
adityapande-1995 Apr 25, 2022
80130d2
Fixed test failures
adityapande-1995 Apr 27, 2022
557a680
Flake8 fixes
adityapande-1995 Apr 27, 2022
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
93 changes: 48 additions & 45 deletions ros_ign_bridge/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.

Expand Down
28 changes: 28 additions & 0 deletions ros_ign_bridge/include/ros_ign_bridge/convert/geometry_msgs.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,17 +19,21 @@
#include <ignition/msgs/quaternion.pb.h>
#include <ignition/msgs/vector3d.pb.h>
#include <ignition/msgs/pose.pb.h>
#include <ignition/msgs/pose_with_covariance.pb.h>
#include <ignition/msgs/pose_v.pb.h>
#include <ignition/msgs/twist.pb.h>
#include <ignition/msgs/twist_with_covariance.pb.h>
#include <ignition/msgs/wrench.pb.h>

// ROS 2 messages
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_with_covariance.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/twist_with_covariance.hpp>
#include <geometry_msgs/msg/wrench.hpp>

#include <ros_ign_bridge/convert_decl.hpp>
Expand Down Expand Up @@ -86,6 +90,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(
Expand Down Expand Up @@ -134,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(
Expand Down
13 changes: 13 additions & 0 deletions ros_ign_bridge/include/ros_ign_bridge/convert/nav_msgs.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

// Ignition messages
#include <ignition/msgs/odometry.pb.h>
#include <ignition/msgs/odometry_with_covariance.pb.h>

// ROS 2 messages
#include <nav_msgs/msg/odometry.hpp>
Expand All @@ -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_
60 changes: 60 additions & 0 deletions ros_ign_bridge/src/convert/geometry_msgs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,6 +106,36 @@ 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)
{
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<>
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);
int data_size = ign_msg.covariance().data_size();
if (data_size == 36) {
Copy link
Contributor

Choose a reason for hiding this comment

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

How likely is this going to be false? Seems less than ideal.

Copy link
Contributor Author

@adityapande-1995 adityapande-1995 Mar 17, 2022 •
edited
Loading

Choose a reason for hiding this comment

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

I agree, this is not likely to be false, but I was still worried if this loop will segfault at runtime if the data array in ros_msg.covariance[ ] exceeds index. There is no explicit check on Float_V size.

Copy link
Contributor

Choose a reason for hiding this comment

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

Is there a way to define the type such that it always has a size of 36?

Copy link
Contributor Author

@adityapande-1995 adityapande-1995 Mar 17, 2022 •
edited
Loading

Choose a reason for hiding this comment

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

Maybe those are not supported by protobuf, used by ign-msgs : https://www.aapelivuorinen.com/blog/2019/07/12/protobuf-arrays/

Copy link
Member

Choose a reason for hiding this comment

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

yeah I don't think protobuf supports fixed size arrays

we are on the verge of releasing the ign-msgs file into ign-msgs8. Does this approach seem reasonable to you?

Copy link
Contributor

Choose a reason for hiding this comment

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

I think it's fine to keep this PR as-is. I was just curious if it was possible to avoid.

for (int i = 0; i < data_size; i++) {
auto data = ign_msg.covariance().data()[i];
ros_msg.covariance[i] = data;
}
}
}

template<>
void
convert_ros_to_ign(
Expand Down Expand Up @@ -197,6 +227,36 @@ 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()));
for (const auto & elem : ros_msg.covariance) {
ign_msg.mutable_covariance()->add_data(elem);
}
}

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);
int data_size = ign_msg.covariance().data_size();
if (data_size == 36) {
for (int i = 0; i < data_size; i++) {
auto data = ign_msg.covariance().data()[i];
ros_msg.covariance[i] = data;
}
}
}

template<>
void
convert_ros_to_ign(
Expand Down
40 changes: 37 additions & 3 deletions ros_ign_bridge/src/convert/nav_msgs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,9 +44,43 @@ 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;
}
}
}

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 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;
}
}
Expand Down
Loading