DuongTD/geometry2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

hiep sua ten file

Browse Source
This commit is contained in:
HiepLM 2025-12-30 09:56:49 +07:00
parent 2987c1a481
commit e4db1da907
5 changed files with 32 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
tf3_geometry_msgs/CMakeLists.txt
View File

@ -33,7 +33,7 @@ target_include_directories(tf3_geometry_msgs
$<INSTALL_INTERFACE:include> $<INSTALL_INTERFACE:include>
) )
# Liên kết với std_msgs nếu bạn có file Header.h trong include/std_msgs/ # Liên kết với robot_std_msgs nếu bạn có file Header.h trong include/robot_std_msgs/
target_link_libraries(tf3_geometry_msgs INTERFACE target_link_libraries(tf3_geometry_msgs INTERFACE
geometry_msgs geometry_msgs
data_convert data_convert

4
tf3_geometry_msgs/test/test_tomsg_frommsg.cpp
View File

@ -48,7 +48,7 @@ robot_geometry_msgs::Vector3& value_initialize(robot_geometry_msgs::Vector3 &m1)
return m1; return m1;
} }
std_msgs::Header& value_initialize(std_msgs::Header & h) robot_std_msgs::Header& value_initialize(robot_std_msgs::Header & h)
{ {
h.stamp = robot::Time(10); h.stamp = robot::Time(10);
h.frame_id = "foobar"; h.frame_id = "foobar";
@ -121,7 +121,7 @@ robot_geometry_msgs::TransformStamped& value_initialize(robot_geometry_msgs::Tra
return m1; return m1;
} }
void expect_near(const std_msgs::Header & h1, const std_msgs::Header & h2) void expect_near(const robot_std_msgs::Header & h1, const robot_std_msgs::Header & h2)
{ {
EXPECT_NEAR(h1.stamp.toSec(), h2.stamp.toSec(), EPS); EXPECT_NEAR(h1.stamp.toSec(), h2.stamp.toSec(), EPS);
EXPECT_STREQ(h1.frame_id.c_str(), h2.frame_id.c_str()); EXPECT_STREQ(h1.frame_id.c_str(), h2.frame_id.c_str());

2
tf3_sensor_msgs/CMakeLists.txt
View File

@ -31,7 +31,7 @@ target_include_directories(tf3_sensor_msgs
) )
target_link_libraries(tf3_sensor_msgs INTERFACE target_link_libraries(tf3_sensor_msgs INTERFACE
sensor_msgs robot_sensor_msgs
data_convert data_convert
) )

26
tf3_sensor_msgs/include/tf3_sensor_msgs/tf3_sensor_msgs.h
View File

@ -31,8 +31,8 @@
#define TF3_SENSOR_MSGS_H #define TF3_SENSOR_MSGS_H
#include <tf3/convert.h> #include <tf3/convert.h>
#include <sensor_msgs/PointCloud2.h> #include <robot_sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h> #include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <Eigen/Eigen> #include <Eigen/Eigen>
#include <Eigen/Geometry> #include <Eigen/Geometry>
#include <data_convert/data_convert.h> #include <data_convert/data_convert.h>
@ -52,7 +52,7 @@ namespace tf3
*/ */
template <> template <>
inline inline
const tf3::Time& getTimestamp(const sensor_msgs::PointCloud2& p) {return data_convert::convertTime(p.header.stamp);} const tf3::Time& getTimestamp(const robot_sensor_msgs::PointCloud2& p) {return data_convert::convertTime(p.header.stamp);}
/** \brief Extract a frame ID from the header of a PointCloud2 message. /** \brief Extract a frame ID from the header of a PointCloud2 message.
* This function is a specialization of the getFrameId template defined in tf3/convert.h. * This function is a specialization of the getFrameId template defined in tf3/convert.h.
@ -62,12 +62,12 @@ const tf3::Time& getTimestamp(const sensor_msgs::PointCloud2& p) {return data_co
*/ */
template <> template <>
inline inline
const std::string& getFrameId(const sensor_msgs::PointCloud2 &p) {return p.header.frame_id;} const std::string& getFrameId(const robot_sensor_msgs::PointCloud2 &p) {return p.header.frame_id;}
// this method needs to be implemented by client library developers // this method needs to be implemented by client library developers
template <> template <>
inline inline
void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 &p_out, const tf3::TransformStampedMsg& t_in) void doTransform(const robot_sensor_msgs::PointCloud2 &p_in, robot_sensor_msgs::PointCloud2 &p_out, const tf3::TransformStampedMsg& t_in)
{ {
p_out = p_in; p_out = p_in;
p_out.header.seq = p_in.header.seq; p_out.header.seq = p_in.header.seq;
@ -78,13 +78,13 @@ void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2
t_in.transform.rotation.w, t_in.transform.rotation.x, t_in.transform.rotation.w, t_in.transform.rotation.x,
t_in.transform.rotation.y, t_in.transform.rotation.z); t_in.transform.rotation.y, t_in.transform.rotation.z);
sensor_msgs::PointCloud2ConstIterator<float> x_in(p_in, "x"); robot_sensor_msgs::PointCloud2ConstIterator<float> x_in(p_in, "x");
sensor_msgs::PointCloud2ConstIterator<float> y_in(p_in, "y"); robot_sensor_msgs::PointCloud2ConstIterator<float> y_in(p_in, "y");
sensor_msgs::PointCloud2ConstIterator<float> z_in(p_in, "z"); robot_sensor_msgs::PointCloud2ConstIterator<float> z_in(p_in, "z");
sensor_msgs::PointCloud2Iterator<float> x_out(p_out, "x"); robot_sensor_msgs::PointCloud2Iterator<float> x_out(p_out, "x");
sensor_msgs::PointCloud2Iterator<float> y_out(p_out, "y"); robot_sensor_msgs::PointCloud2Iterator<float> y_out(p_out, "y");
sensor_msgs::PointCloud2Iterator<float> z_out(p_out, "z"); robot_sensor_msgs::PointCloud2Iterator<float> z_out(p_out, "z");
Eigen::Vector3f point; Eigen::Vector3f point;
for(; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) { for(; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) {
@ -96,12 +96,12 @@ void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2
} }
inline inline
sensor_msgs::PointCloud2 toMsg(const sensor_msgs::PointCloud2 &in) robot_sensor_msgs::PointCloud2 toMsg(const robot_sensor_msgs::PointCloud2 &in)
{ {
return in; return in;
} }
inline inline
void fromMsg(const sensor_msgs::PointCloud2 &msg, sensor_msgs::PointCloud2 &out) void fromMsg(const robot_sensor_msgs::PointCloud2 &msg, robot_sensor_msgs::PointCloud2 &out)
{ {
out = msg; out = msg;
} }

30
tf3_sensor_msgs/test/test_tf2_sensor_msgs.cpp
View File

@ -39,14 +39,14 @@ static const double EPS = 1e-3;
TEST(Tf2Sensor, PointCloud2) TEST(Tf2Sensor, PointCloud2)
{ {
sensor_msgs::PointCloud2 cloud; robot_sensor_msgs::PointCloud2 cloud;
sensor_msgs::PointCloud2Modifier modifier(cloud); robot_sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
modifier.resize(1); modifier.resize(1);
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x"); robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y"); robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z"); robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
*iter_x = 1; *iter_x = 1;
*iter_y = 2; *iter_y = 2;
@ -62,12 +62,12 @@ TEST(Tf2Sensor, PointCloud2)
data_convert::convertTime(robot::Time(2.0)) data_convert::convertTime(robot::Time(2.0))
); );
sensor_msgs::PointCloud2 cloud_simple; // hoặc v1 robot_sensor_msgs::PointCloud2 cloud_simple; // hoặc v1
tf3::doTransform(cloud, cloud_simple, tfm1); tf3::doTransform(cloud, cloud_simple, tfm1);
// sensor_msgs::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", ros::Duration(2.0)); // robot_sensor_msgs::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", ros::Duration(2.0));
sensor_msgs::PointCloud2Iterator<float> iter_x_after(cloud_simple, "x"); robot_sensor_msgs::PointCloud2Iterator<float> iter_x_after(cloud_simple, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y_after(cloud_simple, "y"); robot_sensor_msgs::PointCloud2Iterator<float> iter_y_after(cloud_simple, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z_after(cloud_simple, "z"); robot_sensor_msgs::PointCloud2Iterator<float> iter_z_after(cloud_simple, "z");
EXPECT_NEAR(*iter_x_after, -9, EPS); EXPECT_NEAR(*iter_x_after, -9, EPS);
EXPECT_NEAR(*iter_y_after, 18, EPS); EXPECT_NEAR(*iter_y_after, 18, EPS);
EXPECT_NEAR(*iter_z_after, 27, EPS); EXPECT_NEAR(*iter_z_after, 27, EPS);
@ -79,13 +79,13 @@ TEST(Tf2Sensor, PointCloud2)
data_convert::convertTime(robot::Time(2.0)) data_convert::convertTime(robot::Time(2.0))
); );
sensor_msgs::PointCloud2 cloud_advanced; // hoặc v1 robot_sensor_msgs::PointCloud2 cloud_advanced; // hoặc v1
tf3::doTransform(cloud, cloud_advanced, tfm2); tf3::doTransform(cloud, cloud_advanced, tfm2);
// sensor_msgs::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", ros::Time(2.0), // robot_sensor_msgs::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", ros::Time(2.0),
// "A", ros::Duration(3.0)); // "A", ros::Duration(3.0));
sensor_msgs::PointCloud2Iterator<float> iter_x_advanced(cloud_advanced, "x"); robot_sensor_msgs::PointCloud2Iterator<float> iter_x_advanced(cloud_advanced, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y_advanced(cloud_advanced, "y"); robot_sensor_msgs::PointCloud2Iterator<float> iter_y_advanced(cloud_advanced, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z_advanced(cloud_advanced, "z"); robot_sensor_msgs::PointCloud2Iterator<float> iter_z_advanced(cloud_advanced, "z");
EXPECT_NEAR(*iter_x_advanced, -9, EPS); EXPECT_NEAR(*iter_x_advanced, -9, EPS);
EXPECT_NEAR(*iter_y_advanced, 18, EPS); EXPECT_NEAR(*iter_y_advanced, 18, EPS);
EXPECT_NEAR(*iter_z_advanced, 27, EPS); EXPECT_NEAR(*iter_z_advanced, 27, EPS);