Skip to content

Commit 766be2c

Browse files
artzhaartzha
committed
Updated ros dependent packages for cross ros1/ros2 compatibility
1 parent 5f1eeee commit 766be2c

File tree

2 files changed

+82
-49
lines changed

2 files changed

+82
-49
lines changed

‎ros/ros_helpers.h

+62-49
Original file line numberDiff line numberDiff line change
@@ -17,44 +17,65 @@
1717
// If not, see <http://www.gnu.org/licenses/>.
1818
// ========================================================================
1919

20-
// C++ headers.
21-
#include <string>
20+
#ifndef ROS_HELPERS_H
21+
#define ROS_HELPERS_H
2222

23-
// C++ Library headers.
23+
#include <string>
2424
#include "eigen3/Eigen/Core"
25-
#include "geometry_msgs/Point.h"
26-
#include "geometry_msgs/Vector3.h"
27-
#include "ros/ros.h"
28-
#include "std_msgs/ColorRGBA.h"
29-
#include "std_msgs/Header.h"
30-
#include "visualization_msgs/Marker.h"
3125

32-
// Custom headers.
33-
#include "math/math_util.h"
34-
35-
#ifndef ROS_HELPERS_H
36-
#define ROS_HELPERS_H
26+
#ifdef ROS2
27+
// Use ROS2 headers
28+
#include "rclcpp/rclcpp.hpp"
29+
#include "std_msgs/msg/color_rgba.hpp"
30+
#include "std_msgs/msg/header.hpp"
31+
#include "geometry_msgs/msg/point.hpp"
32+
#include "visualization_msgs/msg/marker.hpp"
33+
#else
34+
// Use ROS1 headers
35+
#include "ros/ros.h"
36+
#include "std_msgs/ColorRGBA.h"
37+
#include "std_msgs/Header.h"
38+
#include "geometry_msgs/Point.h"
39+
#include "visualization_msgs/Marker.h"
40+
#endif
3741

3842
namespace ros_helpers {
3943

40-
inline void InitRosHeader(const std::string& frame_id,
41-
std_msgs::Header* h) {
44+
#ifdef ROS2
45+
// Aliases for ROS2 types.
46+
using StdColorRGBA = std_msgs::msg::ColorRGBA;
47+
using StdHeader = std_msgs::msg::Header;
48+
using GeoPoint = geometry_msgs::msg::Point;
49+
using VisMarker = visualization_msgs::msg::Marker;
50+
#else
51+
// Aliases for ROS1 types.
52+
using StdColorRGBA = std_msgs::ColorRGBA;
53+
using StdHeader = std_msgs::Header;
54+
using GeoPoint = geometry_msgs::Point;
55+
using VisMarker = visualization_msgs::Marker;
56+
#endif
57+
58+
// Initializes a header with the specified frame_id and current time.
59+
inline void InitRosHeader(const std::string& frame_id, StdHeader* h) {
60+
#ifdef ROS2
61+
// Use a temporary clock since we don't have a node pointer here.
62+
rclcpp::Clock clock;
63+
h->stamp = clock.now();
64+
#else
65+
h->stamp = ros::Time::now();
4266
h->seq = 0;
67+
#endif
4368
h->frame_id = frame_id;
44-
h->stamp = ros::Time::now();
4569
}
4670

47-
inline void ClearMarker(visualization_msgs::Marker* m) {
71+
inline void ClearMarker(VisMarker* m) {
4872
m->points.clear();
4973
m->colors.clear();
5074
}
5175

5276
template<typename Tr, typename Tg, typename Tb, typename Ta>
53-
std_msgs::ColorRGBA RosColor(const Tr& r,
54-
const Tg& g,
55-
const Tb& b,
56-
const Ta& a) {
57-
std_msgs::ColorRGBA c;
77+
StdColorRGBA RosColor(const Tr& r, const Tg& g, const Tb& b, const Ta& a) {
78+
StdColorRGBA c;
5879
c.r = r;
5980
c.g = g;
6081
c.b = b;
@@ -63,29 +84,25 @@ std_msgs::ColorRGBA RosColor(const Tr& r,
6384
}
6485

6586
template<typename Tr, typename Tg, typename Tb, typename Ta, typename RosColor>
66-
void SetRosColor(const Tr& r,
67-
const Tg& g,
68-
const Tb& b,
69-
const Ta& a,
70-
RosColor* c) {
87+
void SetRosColor(const Tr& r, const Tg& g, const Tb& b, const Ta& a, RosColor* c) {
7188
c->r = r;
7289
c->g = g;
7390
c->b = b;
7491
c->a = a;
7592
}
7693

7794
template<typename Tx, typename Ty, typename Tz>
78-
geometry_msgs::Point RosPoint(const Tx& x, const Ty& y, const Tz& z) {
79-
geometry_msgs::Point p;
95+
GeoPoint RosPoint(const Tx& x, const Ty& y, const Tz& z) {
96+
GeoPoint p;
8097
p.x = x;
8198
p.y = y;
8299
p.z = z;
83100
return p;
84101
}
85102

86103
template<typename Tx, typename Ty>
87-
geometry_msgs::Point RosPoint(const Tx& x, const Ty& y) {
88-
geometry_msgs::Point p;
104+
GeoPoint RosPoint(const Tx& x, const Ty& y) {
105+
GeoPoint p;
89106
p.x = x;
90107
p.y = y;
91108
p.z = 0;
@@ -100,11 +117,7 @@ void SetRosVector(const Tx& x, const Ty& y, const Tz& z, RosVector* v) {
100117
}
101118

102119
template<typename RosVector, typename T>
103-
void SetRosQuaternion(const T& w,
104-
const T& x,
105-
const T& y,
106-
const T& z,
107-
RosVector* q) {
120+
void SetRosQuaternion(const T& w, const T& x, const T& y, const T& z, RosVector* q) {
108121
q->w = w;
109122
q->x = x;
110123
q->y = y;
@@ -120,17 +133,17 @@ void SetIdentityRosQuaternion(RosVector* q) {
120133
}
121134

122135
template <typename Derived>
123-
geometry_msgs::Point Eigen3DToRosPoint(const Eigen::DenseBase<Derived>& v) {
124-
geometry_msgs::Point p;
136+
GeoPoint Eigen3DToRosPoint(const Eigen::DenseBase<Derived>& v) {
137+
GeoPoint p;
125138
p.x = v[0];
126139
p.y = v[1];
127140
p.z = v[2];
128141
return p;
129142
}
130143

131144
template <typename Derived>
132-
geometry_msgs::Point Eigen2DToRosPoint(const Eigen::DenseBase<Derived>& v) {
133-
geometry_msgs::Point p;
145+
GeoPoint Eigen2DToRosPoint(const Eigen::DenseBase<Derived>& v) {
146+
GeoPoint p;
134147
p.x = v[0];
135148
p.y = v[1];
136149
p.z = 0;
@@ -140,16 +153,16 @@ geometry_msgs::Point Eigen2DToRosPoint(const Eigen::DenseBase<Derived>& v) {
140153
template <typename Derived>
141154
void DrawEigen2DLine(const Eigen::DenseBase<Derived>& v1,
142155
const Eigen::DenseBase<Derived>& v2,
143-
visualization_msgs::Marker* msg) {
156+
VisMarker* msg) {
144157
msg->points.push_back(Eigen2DToRosPoint<Derived>(v1));
145158
msg->points.push_back(Eigen2DToRosPoint<Derived>(v2));
146159
}
147160

148161
template <typename Derived>
149162
void DrawEigen2DLine(const Eigen::DenseBase<Derived>& v1,
150163
const Eigen::DenseBase<Derived>& v2,
151-
const std_msgs::ColorRGBA& c,
152-
visualization_msgs::Marker* msg) {
164+
const StdColorRGBA& c,
165+
VisMarker* msg) {
153166
msg->points.push_back(Eigen2DToRosPoint<Derived>(v1));
154167
msg->points.push_back(Eigen2DToRosPoint<Derived>(v2));
155168
msg->colors.push_back(c);
@@ -159,9 +172,9 @@ void DrawEigen2DLine(const Eigen::DenseBase<Derived>& v1,
159172
template <typename Derived>
160173
void DrawEigen2DLine(const Eigen::DenseBase<Derived>& v1,
161174
const Eigen::DenseBase<Derived>& v2,
162-
const std_msgs::ColorRGBA& c1,
163-
const std_msgs::ColorRGBA& c2,
164-
visualization_msgs::Marker* msg) {
175+
const StdColorRGBA& c1,
176+
const StdColorRGBA& c2,
177+
VisMarker* msg) {
165178
msg->points.push_back(Eigen2DToRosPoint<Derived>(v1));
166179
msg->points.push_back(Eigen2DToRosPoint<Derived>(v2));
167180
msg->colors.push_back(c1);
@@ -171,8 +184,8 @@ void DrawEigen2DLine(const Eigen::DenseBase<Derived>& v1,
171184
template <typename Vector2>
172185
void DrawCross(const Vector2& v,
173186
const float size,
174-
const std_msgs::ColorRGBA& color,
175-
visualization_msgs::Marker* msg) {
187+
const StdColorRGBA& color,
188+
VisMarker* msg) {
176189
msg->points.push_back(Eigen2DToRosPoint(v - Vector2(size, size)));
177190
msg->points.push_back(Eigen2DToRosPoint(v + Vector2(size, size)));
178191
msg->points.push_back(Eigen2DToRosPoint(v - Vector2(size, -size)));

‎ros/ros_macros.h

+20
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
#pragma once
2+
3+
#ifdef ROS1
4+
#include <ros/ros.h>
5+
#define LOG_ERROR(...) ROS_ERROR(__VA_ARGS__)
6+
#define LOG_WARN(...) ROS_WARN(__VA_ARGS__)
7+
#define LOG_INFO(...) ROS_INFO(__VA_ARGS__)
8+
#define GET_TIME() ros::Time::now()
9+
#define DURATION(secs) ros::Duration(secs)
10+
#else
11+
#include <rclcpp/rclcpp.hpp>
12+
// Define a default logger. You can either hard-code a logger name...
13+
#define DEFAULT_LOGGER rclcpp::get_logger("default_logger")
14+
15+
#define LOG_ERROR(...) RCLCPP_ERROR(DEFAULT_LOGGER, __VA_ARGS__)
16+
#define LOG_WARN(...) RCLCPP_WARN(DEFAULT_LOGGER, __VA_ARGS__)
17+
#define LOG_INFO(...) RCLCPP_INFO(DEFAULT_LOGGER, __VA_ARGS__)
18+
#define GET_TIME() rclcpp::Clock().now()
19+
#define DURATION(secs) rclcpp::Duration::from_seconds(secs)
20+
#endif

0 commit comments

Comments
 (0)