HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

uodate

Browse Source
This commit is contained in:
HiepLM
2026-01-14 17:53:27 +07:00
parent 6549425279
commit 0f58db3481
11 changed files with 125 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/Libraries/robot_nav_2d_utils/CMakeLists.txt
View File

@@ -37,7 +37,7 @@ if (NOT BUILDING_WITH_CATKIN)
tf3
robot_tf3_geometry_msgs
robot_cpp
console_bridge
data_convert
)
else()

12
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h
View File

@@ -42,6 +42,18 @@
namespace robot_nav_2d_utils
{
/**
* @brief Transform a PoseStamped from one frame to another
*
* @param tf Smart pointer to TFListener
* @param in_pose Pose to transform
* @param out_pose Place to store the resulting transformed pose
* @param frame Frame to transform the pose into
* @return True if successful transform
*/
bool transform(const TFListenerPtr tf, const ::robot_geometry_msgs::PoseStamped &in_pose,::robot_geometry_msgs::PoseStamped &out_pose, const ::std::string &frame);
/**
* @brief Transform a PoseStamped from one frame to another while catching exceptions
*

4
src/Libraries/robot_nav_2d_utils/package.xml
View File

@@ -25,11 +25,13 @@
<build_depend>nav_grid</build_depend>
<build_depend>robot_nav_core2</build_depend>
<build_depend>robot_cpp</build_depend>
<build_depend>data_convert</build_depend>
<run_depend>robot_nav_2d_msgs</run_depend>
<run_depend>robot_geometry_msgs</run_depend>
<run_depend>robot_nav_msgs</run_depend>
<run_depend>nav_grid</run_depend>
<run_depend>robot_nav_core2</run_depend>
<run_depend>robot_cpp</run_depend>
<run_depend>data_convert</run_depend>
</package>

16
src/Libraries/robot_nav_2d_utils/src/conversions.cpp
View File

@@ -31,6 +31,8 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <robot/robot.h>
#include <data_convert/data_convert.h>
#include <robot_nav_2d_utils/conversions.h>
#include <vector>
#include <string>
@@ -105,7 +107,7 @@ namespace robot_nav_2d_utils
pose2d.header = pose.header;
pose2d.pose.x = pose.pose.position.x;
pose2d.pose.y = pose.pose.position.y;
// pose2d.pose.theta = tf::getYaw(pose.pose.orientation);
pose2d.pose.theta = data_convert::getYaw(pose.pose.orientation);
return pose2d;
}
@@ -114,7 +116,7 @@ namespace robot_nav_2d_utils
robot_geometry_msgs::Pose pose;
pose.position.x = pose2d.x;
pose.position.y = pose2d.y;
// pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta);
pose.orientation = data_convert::createQuaternionMsgFromYaw(pose2d.theta);
return pose;
}
@@ -134,7 +136,7 @@ namespace robot_nav_2d_utils
pose.header.stamp = stamp;
pose.pose.position.x = pose2d.x;
pose.pose.position.y = pose2d.y;
// pose.pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta);
pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(pose2d.theta);
return pose;
}
@@ -284,10 +286,10 @@ namespace robot_nav_2d_utils
info.height = metadata.height;
info.origin_x = metadata.origin.position.x;
info.origin_y = metadata.origin.position.y;
// if (std::abs(tf::getYaw(metadata.origin.orientation)) > 1e-5)
// {
// std::cout << "Conversion from MapMetaData to NavGridInfo encountered a non-zero rotation. Ignoring" << std::endl;
// }
if (std::abs(data_convert::getYaw(metadata.origin.orientation)) > 1e-5)
{
robot::log_warning("[conversions] conversion from MapMetaData to NavGridInfo encountered a non-zero rotation. Ignoring");
}
return info;
}

68
src/Libraries/robot_nav_2d_utils/src/tf_help.cpp
View File

@@ -41,36 +41,54 @@
namespace robot_nav_2d_utils
{
bool transform(const TFListenerPtr tf,
const ::robot_geometry_msgs::PoseStamped &in_pose,
::robot_geometry_msgs::PoseStamped &out_pose, const ::std::string &frame)
{
const std::string target_frame = frame;
const std::string source_frame = in_pose.header.frame_id;
std::string *error_msg;
if(tf->canTransform(target_frame, source_frame, tf3::Time(), error_msg))
{
tf3::TransformStampedMsg transform =tf->lookupTransform(target_frame, source_frame, tf3::Time());
tf3::doTransform(in_pose, out_pose, transform);
return true;
}
return false;
}
bool transformPose(const TFListenerPtr tf, const ::std::string frame,
const ::robot_geometry_msgs::PoseStamped &in_pose, ::robot_geometry_msgs::PoseStamped &out_pose,
const bool extrapolation_fallback)
{
// if (in_pose.header.frame_id == frame)
// {
// out_pose = in_pose;
// return true;
// }
if (in_pose.header.frame_id == frame)
{
out_pose = in_pose;
return true;
}
// try
// {
// tf->transform(in_pose, out_pose, frame);
// return true;
// }
// catch (tf3::ExtrapolationException &ex)
// {
// if (!extrapolation_fallback)
// throw;
// robot_geometry_msgs::PoseStamped latest_in_pose;
// latest_in_pose.header.frame_id = in_pose.header.frame_id;
// latest_in_pose.pose = in_pose.pose;
// tf->transform(latest_in_pose, out_pose, frame);
// return true;
// }
// catch (tf3::TransformException &ex)
// {
// robot::log_error("Exception in transformPose: %s", ex.what());
// return false;
// }
try
{
transform(tf, in_pose, out_pose, frame);
return true;
}
catch (tf3::ExtrapolationException &ex)
{
if (!extrapolation_fallback)
throw;
robot_geometry_msgs::PoseStamped latest_in_pose;
latest_in_pose.header.frame_id = in_pose.header.frame_id;
latest_in_pose.pose = in_pose.pose;
transform(tf, latest_in_pose, out_pose, frame);
return true;
}
catch (tf3::TransformException &ex)
{
robot::log_error("Exception in transformPose: %s", ex.what());
return false;
}
return false;
}