Skip to content

Commit 54c28b1

Browse files
adityapande-1995adityapande-1995
authored
Adds pose and twist with covariance messages bridging (#222)
* Added pose, twist and odometry with covariance messages bridging Signed-off-by: Aditya <aditya050995@gmail.com>
1 parent 8efbf57 commit 54c28b1

File tree

18 files changed

+584
-48
lines changed

18 files changed

+584
-48
lines changed

‎ros_ign_bridge/README.md‎

Lines changed: 48 additions & 45 deletions
Original file line numberDiff line numberDiff line change
@@ -6,51 +6,54 @@ between ROS and Ignition Transport.
66
The bridge is currently implemented in C++. At this point there's no support for
77
service calls. Its support is limited to only the following message types:
88

9-
| ROS type | Ignition Transport type |
10-
|--------------------------------------|:------------------------------------:|
11-
| builtin_interfaces/msg/Time | ignition::msgs::Time |
12-
| std_msgs/msg/Bool | ignition::msgs::Boolean |
13-
| std_msgs/msg/ColorRGBA | ignition::msgs::Color |
14-
| std_msgs/msg/Empty | ignition::msgs::Empty |
15-
| std_msgs/msg/Float32 | ignition::msgs::Float |
16-
| std_msgs/msg/Float64 | ignition::msgs::Double |
17-
| std_msgs/msg/Header | ignition::msgs::Header |
18-
| std_msgs/msg/Int32 | ignition::msgs::Int32 |
19-
| std_msgs/msg/UInt32 | ignition::msgs::UInt32 |
20-
| std_msgs/msg/String | ignition::msgs::StringMsg |
21-
| geometry_msgs/msg/Wrench | ignition::msgs::Wrench |
22-
| geometry_msgs/msg/Quaternion | ignition::msgs::Quaternion |
23-
| geometry_msgs/msg/Vector3 | ignition::msgs::Vector3d |
24-
| geometry_msgs/msg/Point | ignition::msgs::Vector3d |
25-
| geometry_msgs/msg/Pose | ignition::msgs::Pose |
26-
| geometry_msgs/msg/PoseStamped | ignition::msgs::Pose |
27-
| geometry_msgs/msg/Transform | ignition::msgs::Pose |
28-
| geometry_msgs/msg/TransformStamped | ignition::msgs::Pose |
29-
| geometry_msgs/msg/Twist | ignition::msgs::Twist |
30-
| mav_msgs/msg/Actuators (TODO) | ignition::msgs::Actuators (TODO) |
31-
| nav_msgs/msg/Odometry | ignition::msgs::Odometry |
32-
| ros_ign_interfaces/msg/Contact | ignition::msgs::Contact |
33-
| ros_ign_interfaces/msg/Contacts | ignition::msgs::Contacts |
34-
| ros_ign_interfaces/msg/Entity | ignition::msgs::Entity |
35-
| ros_ign_interfaces/msg/GuiCamera | ignition::msgs::GUICamera |
36-
| ros_ign_interfaces/msg/JointWrench | ignition::msgs::JointWrench |
37-
| ros_ign_interfaces/msg/Light | ignition::msgs::Light |
38-
| ros_ign_interfaces/msg/StringVec | ignition::msgs::StringMsg_V |
39-
| ros_ign_interfaces/msg/TrackVisual | ignition::msgs::TrackVisual |
40-
| ros_ign_interfaces/msg/VideoRecord | ignition::msgs::VideoRecord |
41-
| ros_ign_interfaces/msg/WorldControl | ignition::msgs::WorldControl |
42-
| rosgraph_msgs/msg/Clock | ignition::msgs::Clock |
43-
| sensor_msgs/msg/BatteryState | ignition::msgs::BatteryState |
44-
| sensor_msgs/msg/CameraInfo | ignition::msgs::CameraInfo |
45-
| sensor_msgs/msg/FluidPressure | ignition::msgs::FluidPressure |
46-
| sensor_msgs/msg/Imu | ignition::msgs::IMU |
47-
| sensor_msgs/msg/Image | ignition::msgs::Image |
48-
| sensor_msgs/msg/JointState | ignition::msgs::Model |
49-
| sensor_msgs/msg/LaserScan | ignition::msgs::LaserScan |
50-
| sensor_msgs/msg/MagneticField | ignition::msgs::Magnetometer |
51-
| sensor_msgs/msg/PointCloud2 | ignition::msgs::PointCloudPacked |
52-
| tf2_msgs/msg/TFMessage | ignition::msgs::Pose_V |
53-
| trajectory_msgs/msg/JointTrajectory | ignition::msgs::JointTrajectory |
9+
| ROS type | Ignition Transport type |
10+
|--------------------------------------|:--------------------------------------:|
11+
| builtin_interfaces/msg/Time | ignition::msgs::Time |
12+
| std_msgs/msg/Bool | ignition::msgs::Boolean |
13+
| std_msgs/msg/ColorRGBA | ignition::msgs::Color |
14+
| std_msgs/msg/Empty | ignition::msgs::Empty |
15+
| std_msgs/msg/Float32 | ignition::msgs::Float |
16+
| std_msgs/msg/Float64 | ignition::msgs::Double |
17+
| std_msgs/msg/Header | ignition::msgs::Header |
18+
| std_msgs/msg/Int32 | ignition::msgs::Int32 |
19+
| std_msgs/msg/UInt32 | ignition::msgs::UInt32 |
20+
| std_msgs/msg/String | ignition::msgs::StringMsg |
21+
| geometry_msgs/msg/Wrench | ignition::msgs::Wrench |
22+
| geometry_msgs/msg/Quaternion | ignition::msgs::Quaternion |
23+
| geometry_msgs/msg/Vector3 | ignition::msgs::Vector3d |
24+
| geometry_msgs/msg/Point | ignition::msgs::Vector3d |
25+
| geometry_msgs/msg/Pose | ignition::msgs::Pose |
26+
| geometry_msgs/msg/PoseWithCovariance | ignition::msgs::PoseWithCovariance |
27+
| geometry_msgs/msg/PoseStamped | ignition::msgs::Pose |
28+
| geometry_msgs/msg/Transform | ignition::msgs::Pose |
29+
| geometry_msgs/msg/TransformStamped | ignition::msgs::Pose |
30+
| geometry_msgs/msg/Twist | ignition::msgs::Twist |
31+
| geometry_msgs/msg/TwistWithCovariance| ignition::msgs::TwistWithCovariance |
32+
| mav_msgs/msg/Actuators (TODO) | ignition::msgs::Actuators (TODO) |
33+
| nav_msgs/msg/Odometry | ignition::msgs::Odometry |
34+
| nav_msgs/msg/Odometry | ignition::msgs::OdometryWithCovariance |
35+
| ros_ign_interfaces/msg/Contact | ignition::msgs::Contact |
36+
| ros_ign_interfaces/msg/Contacts | ignition::msgs::Contacts |
37+
| ros_ign_interfaces/msg/Entity | ignition::msgs::Entity |
38+
| ros_ign_interfaces/msg/GuiCamera | ignition::msgs::GUICamera |
39+
| ros_ign_interfaces/msg/JointWrench | ignition::msgs::JointWrench |
40+
| ros_ign_interfaces/msg/Light | ignition::msgs::Light |
41+
| ros_ign_interfaces/msg/StringVec | ignition::msgs::StringMsg_V |
42+
| ros_ign_interfaces/msg/TrackVisual | ignition::msgs::TrackVisual |
43+
| ros_ign_interfaces/msg/VideoRecord | ignition::msgs::VideoRecord |
44+
| ros_ign_interfaces/msg/WorldControl | ignition::msgs::WorldControl |
45+
| rosgraph_msgs/msg/Clock | ignition::msgs::Clock |
46+
| sensor_msgs/msg/BatteryState | ignition::msgs::BatteryState |
47+
| sensor_msgs/msg/CameraInfo | ignition::msgs::CameraInfo |
48+
| sensor_msgs/msg/FluidPressure | ignition::msgs::FluidPressure |
49+
| sensor_msgs/msg/Imu | ignition::msgs::IMU |
50+
| sensor_msgs/msg/Image | ignition::msgs::Image |
51+
| sensor_msgs/msg/JointState | ignition::msgs::Model |
52+
| sensor_msgs/msg/LaserScan | ignition::msgs::LaserScan |
53+
| sensor_msgs/msg/MagneticField | ignition::msgs::Magnetometer |
54+
| sensor_msgs/msg/PointCloud2 | ignition::msgs::PointCloudPacked |
55+
| tf2_msgs/msg/TFMessage | ignition::msgs::Pose_V |
56+
| trajectory_msgs/msg/JointTrajectory | ignition::msgs::JointTrajectory |
5457

5558
Run `ros2 run ros_ign_bridge parameter_bridge -h` for instructions.
5659

‎ros_ign_bridge/include/ros_ign_bridge/convert/geometry_msgs.hpp‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,17 +19,21 @@
1919
#include <ignition/msgs/quaternion.pb.h>
2020
#include <ignition/msgs/vector3d.pb.h>
2121
#include <ignition/msgs/pose.pb.h>
22+
#include <ignition/msgs/pose_with_covariance.pb.h>
2223
#include <ignition/msgs/pose_v.pb.h>
2324
#include <ignition/msgs/twist.pb.h>
25+
#include <ignition/msgs/twist_with_covariance.pb.h>
2426
#include <ignition/msgs/wrench.pb.h>
2527

2628
// ROS 2 messages
2729
#include <geometry_msgs/msg/point.hpp>
2830
#include <geometry_msgs/msg/pose.hpp>
31+
#include <geometry_msgs/msg/pose_with_covariance.hpp>
2932
#include <geometry_msgs/msg/pose_stamped.hpp>
3033
#include <geometry_msgs/msg/quaternion.hpp>
3134
#include <geometry_msgs/msg/transform_stamped.hpp>
3235
#include <geometry_msgs/msg/twist.hpp>
36+
#include <geometry_msgs/msg/twist_with_covariance.hpp>
3337
#include <geometry_msgs/msg/wrench.hpp>
3438

3539
#include <ros_ign_bridge/convert_decl.hpp>
@@ -86,6 +90,18 @@ convert_ign_to_ros(
8690
const ignition::msgs::Pose & ign_msg,
8791
geometry_msgs::msg::Pose & ros_msg);
8892

93+
template<>
94+
void
95+
convert_ros_to_ign(
96+
const geometry_msgs::msg::PoseWithCovariance & ros_msg,
97+
ignition::msgs::PoseWithCovariance & ign_msg);
98+
99+
template<>
100+
void
101+
convert_ign_to_ros(
102+
const ignition::msgs::PoseWithCovariance & ign_msg,
103+
geometry_msgs::msg::PoseWithCovariance & ros_msg);
104+
89105
template<>
90106
void
91107
convert_ros_to_ign(
@@ -134,6 +150,18 @@ convert_ign_to_ros(
134150
const ignition::msgs::Twist & ign_msg,
135151
geometry_msgs::msg::Twist & ros_msg);
136152

153+
template<>
154+
void
155+
convert_ros_to_ign(
156+
const geometry_msgs::msg::TwistWithCovariance & ros_msg,
157+
ignition::msgs::TwistWithCovariance & ign_msg);
158+
159+
template<>
160+
void
161+
convert_ign_to_ros(
162+
const ignition::msgs::TwistWithCovariance & ign_msg,
163+
geometry_msgs::msg::TwistWithCovariance & ros_msg);
164+
137165
template<>
138166
void
139167
convert_ros_to_ign(

‎ros_ign_bridge/include/ros_ign_bridge/convert/nav_msgs.hpp‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
// Ignition messages
1919
#include <ignition/msgs/odometry.pb.h>
20+
#include <ignition/msgs/odometry_with_covariance.pb.h>
2021

2122
// ROS 2 messages
2223
#include <nav_msgs/msg/odometry.hpp>
@@ -38,6 +39,18 @@ convert_ign_to_ros(
3839
const ignition::msgs::Odometry & ign_msg,
3940
nav_msgs::msg::Odometry & ros_msg);
4041

42+
template<>
43+
void
44+
convert_ros_to_ign(
45+
const nav_msgs::msg::Odometry & ros_msg,
46+
ignition::msgs::OdometryWithCovariance & ign_msg);
47+
48+
template<>
49+
void
50+
convert_ign_to_ros(
51+
const ignition::msgs::OdometryWithCovariance & ign_msg,
52+
nav_msgs::msg::Odometry & ros_msg);
53+
4154
} // namespace ros_ign_bridge
4255

4356
#endif // ROS_IGN_BRIDGE__CONVERT__NAV_MSGS_HPP_

‎ros_ign_bridge/src/convert/geometry_msgs.cpp‎

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -106,6 +106,36 @@ convert_ign_to_ros(
106106
convert_ign_to_ros(ign_msg.orientation(), ros_msg.orientation);
107107
}
108108

109+
template<>
110+
void
111+
convert_ros_to_ign(
112+
const geometry_msgs::msg::PoseWithCovariance & ros_msg,
113+
ignition::msgs::PoseWithCovariance & ign_msg)
114+
{
115+
convert_ros_to_ign(ros_msg.pose.position, *ign_msg.mutable_pose()->mutable_position());
116+
convert_ros_to_ign(ros_msg.pose.orientation, *ign_msg.mutable_pose()->mutable_orientation());
117+
for (const auto & elem : ros_msg.covariance) {
118+
ign_msg.mutable_covariance()->add_data(elem);
119+
}
120+
}
121+
122+
template<>
123+
void
124+
convert_ign_to_ros(
125+
const ignition::msgs::PoseWithCovariance & ign_msg,
126+
geometry_msgs::msg::PoseWithCovariance & ros_msg)
127+
{
128+
convert_ign_to_ros(ign_msg.pose().position(), ros_msg.pose.position);
129+
convert_ign_to_ros(ign_msg.pose().orientation(), ros_msg.pose.orientation);
130+
int data_size = ign_msg.covariance().data_size();
131+
if (data_size == 36) {
132+
for (int i = 0; i < data_size; i++) {
133+
auto data = ign_msg.covariance().data()[i];
134+
ros_msg.covariance[i] = data;
135+
}
136+
}
137+
}
138+
109139
template<>
110140
void
111141
convert_ros_to_ign(
@@ -197,6 +227,36 @@ convert_ign_to_ros(
197227
convert_ign_to_ros(ign_msg.angular(), ros_msg.angular);
198228
}
199229

230+
template<>
231+
void
232+
convert_ros_to_ign(
233+
const geometry_msgs::msg::TwistWithCovariance & ros_msg,
234+
ignition::msgs::TwistWithCovariance & ign_msg)
235+
{
236+
convert_ros_to_ign(ros_msg.twist.linear, (*ign_msg.mutable_twist()->mutable_linear()));
237+
convert_ros_to_ign(ros_msg.twist.angular, (*ign_msg.mutable_twist()->mutable_angular()));
238+
for (const auto & elem : ros_msg.covariance) {
239+
ign_msg.mutable_covariance()->add_data(elem);
240+
}
241+
}
242+
243+
template<>
244+
void
245+
convert_ign_to_ros(
246+
const ignition::msgs::TwistWithCovariance & ign_msg,
247+
geometry_msgs::msg::TwistWithCovariance & ros_msg)
248+
{
249+
convert_ign_to_ros(ign_msg.twist().linear(), ros_msg.twist.linear);
250+
convert_ign_to_ros(ign_msg.twist().angular(), ros_msg.twist.angular);
251+
int data_size = ign_msg.covariance().data_size();
252+
if (data_size == 36) {
253+
for (int i = 0; i < data_size; i++) {
254+
auto data = ign_msg.covariance().data()[i];
255+
ros_msg.covariance[i] = data;
256+
}
257+
}
258+
}
259+
200260
template<>
201261
void
202262
convert_ros_to_ign(

‎ros_ign_bridge/src/convert/nav_msgs.cpp‎

Lines changed: 37 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -44,9 +44,43 @@ convert_ign_to_ros(
4444
convert_ign_to_ros(ign_msg.twist(), ros_msg.twist.twist);
4545

4646
for (auto i = 0; i < ign_msg.header().data_size(); ++i) {
47-
auto aPair = ign_msg.header().data(i);
48-
if (aPair.key() == "child_frame_id" && aPair.value_size() > 0) {
49-
ros_msg.child_frame_id = frame_id_ign_to_ros(aPair.value(0));
47+
auto a_pair = ign_msg.header().data(i);
48+
if (a_pair.key() == "child_frame_id" && a_pair.value_size() > 0) {
49+
ros_msg.child_frame_id = frame_id_ign_to_ros(a_pair.value(0));
50+
break;
51+
}
52+
}
53+
}
54+
55+
template<>
56+
void
57+
convert_ros_to_ign(
58+
const nav_msgs::msg::Odometry & ros_msg,
59+
ignition::msgs::OdometryWithCovariance & ign_msg)
60+
{
61+
convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
62+
convert_ros_to_ign(ros_msg.pose, (*ign_msg.mutable_pose_with_covariance()));
63+
convert_ros_to_ign(ros_msg.twist, (*ign_msg.mutable_twist_with_covariance()));
64+
65+
auto childFrame = ign_msg.mutable_header()->add_data();
66+
childFrame->set_key("child_frame_id");
67+
childFrame->add_value(ros_msg.child_frame_id);
68+
}
69+
70+
template<>
71+
void
72+
convert_ign_to_ros(
73+
const ignition::msgs::OdometryWithCovariance & ign_msg,
74+
nav_msgs::msg::Odometry & ros_msg)
75+
{
76+
convert_ign_to_ros(ign_msg.header(), ros_msg.header);
77+
convert_ign_to_ros(ign_msg.pose_with_covariance(), ros_msg.pose);
78+
convert_ign_to_ros(ign_msg.twist_with_covariance(), ros_msg.twist);
79+
80+
for (auto i = 0; i < ign_msg.header().data_size(); ++i) {
81+
auto a_pair = ign_msg.header().data(i);
82+
if (a_pair.key() == "child_frame_id" && a_pair.value_size() > 0) {
83+
ros_msg.child_frame_id = frame_id_ign_to_ros(a_pair.value(0));
5084
break;
5185
}
5286
}

0 commit comments

Comments
 (0)