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>
)
# 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
geometry_msgs
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;
}
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.frame_id = "foobar";
@ -121,7 +121,7 @@ robot_geometry_msgs::TransformStamped& value_initialize(robot_geometry_msgs::Tra
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_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
sensor_msgs
robot_sensor_msgs
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
#include <tf3/convert.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <data_convert/data_convert.h>
@ -52,7 +52,7 @@ namespace tf3
*/
template <>
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.
* 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 <>
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
template <>
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.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.y, t_in.transform.rotation.z);
sensor_msgs::PointCloud2ConstIterator<float> x_in(p_in, "x");
sensor_msgs::PointCloud2ConstIterator<float> y_in(p_in, "y");
sensor_msgs::PointCloud2ConstIterator<float> z_in(p_in, "z");
robot_sensor_msgs::PointCloud2ConstIterator<float> x_in(p_in, "x");
robot_sensor_msgs::PointCloud2ConstIterator<float> y_in(p_in, "y");
robot_sensor_msgs::PointCloud2ConstIterator<float> z_in(p_in, "z");
sensor_msgs::PointCloud2Iterator<float> x_out(p_out, "x");
sensor_msgs::PointCloud2Iterator<float> y_out(p_out, "y");
sensor_msgs::PointCloud2Iterator<float> z_out(p_out, "z");
robot_sensor_msgs::PointCloud2Iterator<float> x_out(p_out, "x");
robot_sensor_msgs::PointCloud2Iterator<float> y_out(p_out, "y");
robot_sensor_msgs::PointCloud2Iterator<float> z_out(p_out, "z");
Eigen::Vector3f point;
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
sensor_msgs::PointCloud2 toMsg(const sensor_msgs::PointCloud2 &in)
robot_sensor_msgs::PointCloud2 toMsg(const robot_sensor_msgs::PointCloud2 &in)
{
return in;
}
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;
}

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)
{
sensor_msgs::PointCloud2 cloud;
sensor_msgs::PointCloud2Modifier modifier(cloud);
robot_sensor_msgs::PointCloud2 cloud;
robot_sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
modifier.resize(1);
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
*iter_x = 1;
*iter_y = 2;
@ -62,12 +62,12 @@ TEST(Tf2Sensor, PointCloud2)
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);
// sensor_msgs::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", ros::Duration(2.0));
sensor_msgs::PointCloud2Iterator<float> iter_x_after(cloud_simple, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y_after(cloud_simple, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z_after(cloud_simple, "z");
// robot_sensor_msgs::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", ros::Duration(2.0));
robot_sensor_msgs::PointCloud2Iterator<float> iter_x_after(cloud_simple, "x");
robot_sensor_msgs::PointCloud2Iterator<float> iter_y_after(cloud_simple, "y");
robot_sensor_msgs::PointCloud2Iterator<float> iter_z_after(cloud_simple, "z");
EXPECT_NEAR(*iter_x_after, -9, EPS);
EXPECT_NEAR(*iter_y_after, 18, EPS);
EXPECT_NEAR(*iter_z_after, 27, EPS);
@ -79,13 +79,13 @@ TEST(Tf2Sensor, PointCloud2)
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);
// 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));
sensor_msgs::PointCloud2Iterator<float> iter_x_advanced(cloud_advanced, "x");
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_x_advanced(cloud_advanced, "x");
robot_sensor_msgs::PointCloud2Iterator<float> iter_y_advanced(cloud_advanced, "y");
robot_sensor_msgs::PointCloud2Iterator<float> iter_z_advanced(cloud_advanced, "z");
EXPECT_NEAR(*iter_x_advanced, -9, EPS);
EXPECT_NEAR(*iter_y_advanced, 18, EPS);
EXPECT_NEAR(*iter_z_advanced, 27, EPS);