Skip to content

Commit 59e99ec

Browse files
SteveMacenskiSteveMacenski
authored
fixing a second merge conflict resolution error (#3621)
1 parent ef0fff4 commit 59e99ec

File tree

4 files changed

+117
-44
lines changed

4 files changed

+117
-44
lines changed

‎nav2_collision_monitor/include/nav2_collision_monitor/source.hpp‎

Lines changed: 0 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -91,21 +91,6 @@ class Source
9191
const rclcpp::Time & source_time,
9292
const rclcpp::Time & curr_time) const;
9393

94-
/**
95-
* @brief Obtains a transform from source_frame_id at source_time ->
96-
* to base_frame_id_ at curr_time time
97-
* @param source_frame_id Source frame ID to convert data from
98-
* @param source_time Source timestamp to convert data from
99-
* @param curr_time Current node time to interpolate data to
100-
* @param tf_transform Output source->base transform
101-
* @return True if got correct transform, otherwise false
102-
*/
103-
bool getTransform(
104-
const std::string & source_frame_id,
105-
const rclcpp::Time & source_time,
106-
const rclcpp::Time & curr_time,
107-
tf2::Transform & tf_transform) const;
108-
10994
// ----- Variables -----
11095

11196
/// @brief Collision Monitor node

‎nav2_collision_monitor/src/source.cpp‎

Lines changed: 0 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -78,33 +78,4 @@ bool Source::sourceValid(
7878
return true;
7979
}
8080

81-
bool Source::getTransform(
82-
const std::string & source_frame_id,
83-
const rclcpp::Time & source_time,
84-
const rclcpp::Time & curr_time,
85-
tf2::Transform & tf2_transform) const
86-
{
87-
geometry_msgs::msg::TransformStamped transform;
88-
tf2_transform.setIdentity(); // initialize by identical transform
89-
90-
try {
91-
// Obtaining the transform to get data from source to base frame.
92-
// This also considers the time shift between source and base.
93-
transform = tf_buffer_->lookupTransform(
94-
base_frame_id_, curr_time,
95-
source_frame_id, source_time,
96-
global_frame_id_, transform_tolerance_);
97-
} catch (tf2::TransformException & e) {
98-
RCLCPP_ERROR(
99-
logger_,
100-
"[%s]: Failed to get \"%s\"->\"%s\" frame transform: %s",
101-
source_name_.c_str(), source_frame_id.c_str(), base_frame_id_.c_str(), e.what());
102-
return false;
103-
}
104-
105-
// Convert TransformStamped to TF2 transform
106-
tf2::fromMsg(transform.transform, tf2_transform);
107-
return true;
108-
}
109-
11081
} // namespace nav2_collision_monitor

‎nav2_util/include/nav2_util/robot_utils.hpp‎

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,57 @@ bool transformPoseInTargetFrame(
5656
tf2_ros::Buffer & tf_buffer, const std::string target_frame,
5757
const double transform_timeout = 0.1);
5858

59+
/**
60+
* @brief Obtains a transform from source_frame_id at source_time ->
61+
* to target_frame_id at target_time time
62+
* @param source_frame_id Source frame ID to convert from
63+
* @param source_time Source timestamp to convert from
64+
* @param target_frame_id Target frame ID to convert to
65+
* @param target_time Target time to interpolate to
66+
* @param transform_tolerance Transform tolerance
67+
* @param tf_transform Output source->target transform
68+
* @return True if got correct transform, otherwise false
69+
*/
70+
71+
/**
72+
* @brief Obtains a transform from source_frame_id -> to target_frame_id
73+
* @param source_frame_id Source frame ID to convert from
74+
* @param target_frame_id Target frame ID to convert to
75+
* @param transform_tolerance Transform tolerance
76+
* @param tf_buffer TF buffer to use for the transformation
77+
* @param tf_transform Output source->target transform
78+
* @return True if got correct transform, otherwise false
79+
*/
80+
bool getTransform(
81+
const std::string & source_frame_id,
82+
const std::string & target_frame_id,
83+
const tf2::Duration & transform_tolerance,
84+
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
85+
tf2::Transform & tf2_transform);
86+
87+
/**
88+
* @brief Obtains a transform from source_frame_id at source_time ->
89+
* to target_frame_id at target_time time
90+
* @param source_frame_id Source frame ID to convert from
91+
* @param source_time Source timestamp to convert from
92+
* @param target_frame_id Target frame ID to convert to
93+
* @param target_time Current node time to interpolate to
94+
* @param fixed_frame_id The frame in which to assume the transform is constant in time
95+
* @param transform_tolerance Transform tolerance
96+
* @param tf_buffer TF buffer to use for the transformation
97+
* @param tf_transform Output source->target transform
98+
* @return True if got correct transform, otherwise false
99+
*/
100+
bool getTransform(
101+
const std::string & source_frame_id,
102+
const rclcpp::Time & source_time,
103+
const std::string & target_frame_id,
104+
const rclcpp::Time & target_time,
105+
const std::string & fixed_frame_id,
106+
const tf2::Duration & transform_tolerance,
107+
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
108+
tf2::Transform & tf2_transform);
109+
59110
} // end namespace nav2_util
60111

61112
#endif // NAV2_UTIL__ROBOT_UTILS_HPP_

‎nav2_util/src/robot_utils.cpp‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -75,4 +75,70 @@ bool transformPoseInTargetFrame(
7575
return false;
7676
}
7777

78+
bool getTransform(
79+
const std::string & source_frame_id,
80+
const std::string & target_frame_id,
81+
const tf2::Duration & transform_tolerance,
82+
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
83+
tf2::Transform & tf2_transform)
84+
{
85+
geometry_msgs::msg::TransformStamped transform;
86+
tf2_transform.setIdentity(); // initialize by identical transform
87+
88+
if (source_frame_id == target_frame_id) {
89+
// We are already in required frame
90+
return true;
91+
}
92+
93+
try {
94+
// Obtaining the transform to get data from source to target frame
95+
transform = tf_buffer->lookupTransform(
96+
target_frame_id, source_frame_id,
97+
tf2::TimePointZero, transform_tolerance);
98+
} catch (tf2::TransformException & e) {
99+
RCLCPP_ERROR(
100+
rclcpp::get_logger("getTransform"),
101+
"Failed to get \"%s\"->\"%s\" frame transform: %s",
102+
source_frame_id.c_str(), target_frame_id.c_str(), e.what());
103+
return false;
104+
}
105+
106+
// Convert TransformStamped to TF2 transform
107+
tf2::fromMsg(transform.transform, tf2_transform);
108+
return true;
109+
}
110+
111+
bool getTransform(
112+
const std::string & source_frame_id,
113+
const rclcpp::Time & source_time,
114+
const std::string & target_frame_id,
115+
const rclcpp::Time & target_time,
116+
const std::string & fixed_frame_id,
117+
const tf2::Duration & transform_tolerance,
118+
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
119+
tf2::Transform & tf2_transform)
120+
{
121+
geometry_msgs::msg::TransformStamped transform;
122+
tf2_transform.setIdentity(); // initialize by identical transform
123+
124+
try {
125+
// Obtaining the transform to get data from source to target frame.
126+
// This also considers the time shift between source and target.
127+
transform = tf_buffer->lookupTransform(
128+
target_frame_id, target_time,
129+
source_frame_id, source_time,
130+
fixed_frame_id, transform_tolerance);
131+
} catch (tf2::TransformException & ex) {
132+
RCLCPP_ERROR(
133+
rclcpp::get_logger("getTransform"),
134+
"Failed to get \"%s\"->\"%s\" frame transform: %s",
135+
source_frame_id.c_str(), target_frame_id.c_str(), ex.what());
136+
return false;
137+
}
138+
139+
// Convert TransformStamped to TF2 transform
140+
tf2::fromMsg(transform.transform, tf2_transform);
141+
return true;
142+
}
143+
78144
} // end namespace nav2_util

0 commit comments

Comments
 (0)