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

HiepLM update

Browse Source
This commit is contained in:
HiepLM
2025-12-30 09:06:48 +07:00
parent 3c51b228ec
commit 2987c1a481
8 changed files with 158 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
tf3_geometry_msgs/test/test_tf2_geometry_msgs.cpp
View File

@@ -43,7 +43,7 @@ static const double EPS = 1e-3;
TEST(TfGeometry, Frame)
{
geometry_msgs::PoseStamped v1;
robot_geometry_msgs::PoseStamped v1;
v1.pose.position.x = 1;
v1.pose.position.y = 2;
v1.pose.position.z = 3;
@@ -55,9 +55,9 @@ TEST(TfGeometry, Frame)
v1.header.frame_id = "A";
// simple api
// geometry_msgs::PoseStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
// robot_geometry_msgs::PoseStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
// std::cout << "STEP 2" << std::endl;
geometry_msgs::PoseStamped v_simple = v1;
robot_geometry_msgs::PoseStamped v_simple = v1;
// std::cout << "lookupTransform call:" << std::endl;
tf3::TransformStampedMsg tfm = tf_buffer->lookupTransform(
@@ -86,7 +86,7 @@ TEST(TfGeometry, Frame)
EXPECT_NEAR(v_simple.pose.orientation.w, 1.0, EPS);
// advanced api
// geometry_msgs::PoseStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
// robot_geometry_msgs::PoseStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
// "A", ros::Duration(3.0));
tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform(
@@ -95,7 +95,7 @@ TEST(TfGeometry, Frame)
data_convert::convertTime(robot::Time(2.0))
);
geometry_msgs::PoseStamped v_advanced = v1; // hoặc v1
robot_geometry_msgs::PoseStamped v_advanced = v1; // hoặc v1
tf3::doTransform(v1, v_advanced, tfm1);
// std::cout << v_advanced.pose.position.x << " , "
// << v_advanced.pose.position.y << " , "
@@ -116,7 +116,7 @@ TEST(TfGeometry, Frame)
TEST(TfGeometry, PoseWithCovarianceStamped)
{
geometry_msgs::PoseWithCovarianceStamped v1;
robot_geometry_msgs::PoseWithCovarianceStamped v1;
v1.pose.pose.position.x = 1;
v1.pose.pose.position.y = 2;
v1.pose.pose.position.z = 3;
@@ -138,7 +138,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
// simple api
geometry_msgs::PoseWithCovarianceStamped v_simple;
robot_geometry_msgs::PoseWithCovarianceStamped v_simple;
tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform(
"B", // frame đích
"A", // frame nguồn
@@ -170,7 +170,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
EXPECT_NEAR(v_simple.pose.covariance[35], 1.0, EPS);
// advanced api
geometry_msgs::PoseWithCovarianceStamped v_advanced;
robot_geometry_msgs::PoseWithCovarianceStamped v_advanced;
tf3::TransformStampedMsg tfm2 = tf_buffer->lookupTransform(
"B", // frame đích
"A", // frame nguồn
@@ -178,7 +178,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
);
tf3::doTransform(v1, v_advanced, tfm2);
// const geometry_msgs::PoseWithCovarianceStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
// const robot_geometry_msgs::PoseWithCovarianceStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
// "A", ros::Duration(3.0));
std::cout << v_advanced.pose.pose.position.x << " , "
<< v_advanced.pose.pose.position.y << " , "
@@ -206,7 +206,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
/** now add rotation to transform to test the effect on covariance **/
// rotate pi/2 radians about x-axis
geometry_msgs::TransformStamped t_rot;
robot_geometry_msgs::TransformStamped t_rot;
tf3::Quaternion q_rot = tf3::Quaternion(tf3::Vector3(1,0,0), M_PI/2);
t_rot.transform.rotation.x = q_rot.x();
t_rot.transform.rotation.y = q_rot.y();
@@ -224,7 +224,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
v1.pose.covariance[12] = 1;
// perform rotation
geometry_msgs::PoseWithCovarianceStamped v_rotated;
robot_geometry_msgs::PoseWithCovarianceStamped v_rotated;
tf3::TransformStampedMsg tfm3 = tf_buffer->lookupTransform(
"rotated", // frame đích
"A", // frame nguồn

90
tf3_geometry_msgs/test/test_tomsg_frommsg.cpp
View File

@@ -40,7 +40,7 @@ tf3::Vector3 get_tf3_vector()
return tf3::Vector3(1.0, 2.0, 3.0);
}
geometry_msgs::Vector3& value_initialize(geometry_msgs::Vector3 &m1)
robot_geometry_msgs::Vector3& value_initialize(robot_geometry_msgs::Vector3 &m1)
{
m1.x = 1;
m1.y = 2;
@@ -55,14 +55,14 @@ std_msgs::Header& value_initialize(std_msgs::Header & h)
return h;
}
geometry_msgs::Vector3Stamped& value_initialize(geometry_msgs::Vector3Stamped &m1)
robot_geometry_msgs::Vector3Stamped& value_initialize(robot_geometry_msgs::Vector3Stamped &m1)
{
value_initialize(m1.header);
value_initialize(m1.vector);
return m1;
}
geometry_msgs::Point& value_initialize(geometry_msgs::Point &m1)
robot_geometry_msgs::Point& value_initialize(robot_geometry_msgs::Point &m1)
{
m1.x = 1;
m1.y = 2;
@@ -70,14 +70,14 @@ geometry_msgs::Point& value_initialize(geometry_msgs::Point &m1)
return m1;
}
geometry_msgs::PointStamped& value_initialize(geometry_msgs::PointStamped &m1)
robot_geometry_msgs::PointStamped& value_initialize(robot_geometry_msgs::PointStamped &m1)
{
value_initialize(m1.header);
value_initialize(m1.point);
return m1;
}
geometry_msgs::Quaternion & value_initialize(geometry_msgs::Quaternion &m1)
robot_geometry_msgs::Quaternion & value_initialize(robot_geometry_msgs::Quaternion &m1)
{
m1.x = 0;
m1.y = 0;
@@ -86,35 +86,35 @@ geometry_msgs::Quaternion & value_initialize(geometry_msgs::Quaternion &m1)
return m1;
}
geometry_msgs::QuaternionStamped& value_initialize(geometry_msgs::QuaternionStamped &m1)
robot_geometry_msgs::QuaternionStamped& value_initialize(robot_geometry_msgs::QuaternionStamped &m1)
{
value_initialize(m1.header);
value_initialize(m1.quaternion);
return m1;
}
geometry_msgs::Pose & value_initialize(geometry_msgs::Pose & m1)
robot_geometry_msgs::Pose & value_initialize(robot_geometry_msgs::Pose & m1)
{
value_initialize(m1.position);
value_initialize(m1.orientation);
return m1;
}
geometry_msgs::PoseStamped& value_initialize(geometry_msgs::PoseStamped &m1)
robot_geometry_msgs::PoseStamped& value_initialize(robot_geometry_msgs::PoseStamped &m1)
{
value_initialize(m1.header);
value_initialize(m1.pose);
return m1;
}
geometry_msgs::Transform & value_initialize(geometry_msgs::Transform & m1)
robot_geometry_msgs::Transform & value_initialize(robot_geometry_msgs::Transform & m1)
{
value_initialize(m1.translation);
value_initialize(m1.rotation);
return m1;
}
geometry_msgs::TransformStamped& value_initialize(geometry_msgs::TransformStamped &m1)
robot_geometry_msgs::TransformStamped& value_initialize(robot_geometry_msgs::TransformStamped &m1)
{
value_initialize(m1.header);
value_initialize(m1.transform);
@@ -130,14 +130,14 @@ void expect_near(const std_msgs::Header & h1, const std_msgs::Header & h2)
/*
* Vector3
*/
void expect_near(const geometry_msgs::Vector3 & v1, const tf3::Vector3 & v2)
void expect_near(const robot_geometry_msgs::Vector3 & v1, const tf3::Vector3 & v2)
{
EXPECT_NEAR(v1.x, v2.x(), EPS);
EXPECT_NEAR(v1.y, v2.y(), EPS);
EXPECT_NEAR(v1.z, v2.z(), EPS);
}
void expect_near(const geometry_msgs::Vector3 & v1, const geometry_msgs::Vector3 & v2)
void expect_near(const robot_geometry_msgs::Vector3 & v1, const robot_geometry_msgs::Vector3 & v2)
{
EXPECT_NEAR(v1.x, v2.x, EPS);
EXPECT_NEAR(v1.y, v2.y, EPS);
@@ -151,7 +151,7 @@ void expect_near(const tf3::Vector3 & v1, const tf3::Vector3 & v2)
EXPECT_NEAR(v1.z(), v2.z(), EPS);
}
void expect_near(const geometry_msgs::Vector3Stamped & p1, const geometry_msgs::Vector3Stamped & p2)
void expect_near(const robot_geometry_msgs::Vector3Stamped & p1, const robot_geometry_msgs::Vector3Stamped & p2)
{
expect_near(p1.header, p2.header);
expect_near(p1.vector, p2.vector);
@@ -160,21 +160,21 @@ void expect_near(const geometry_msgs::Vector3Stamped & p1, const geometry_msgs::
/*
* Point
*/
void expect_near(const geometry_msgs::Point & p1, const tf3::Vector3 & v2)
void expect_near(const robot_geometry_msgs::Point & p1, const tf3::Vector3 & v2)
{
EXPECT_NEAR(p1.x, v2.x(), EPS);
EXPECT_NEAR(p1.y, v2.y(), EPS);
EXPECT_NEAR(p1.z, v2.z(), EPS);
}
void expect_near(const geometry_msgs::Point & p1, const geometry_msgs::Point & v2)
void expect_near(const robot_geometry_msgs::Point & p1, const robot_geometry_msgs::Point & v2)
{
EXPECT_NEAR(p1.x, v2.x, EPS);
EXPECT_NEAR(p1.y, v2.y, EPS);
EXPECT_NEAR(p1.z, v2.z, EPS);
}
void expect_near(const geometry_msgs::PointStamped & p1, const geometry_msgs::PointStamped & p2)
void expect_near(const robot_geometry_msgs::PointStamped & p1, const robot_geometry_msgs::PointStamped & p2)
{
expect_near(p1.header, p2.header);
expect_near(p1.point, p2.point);
@@ -184,21 +184,21 @@ void expect_near(const geometry_msgs::PointStamped & p1, const geometry_msgs::Po
/*
* Quaternion
*/
void expect_near(const geometry_msgs::Quaternion & q1, const tf3::Quaternion & v2)
void expect_near(const robot_geometry_msgs::Quaternion & q1, const tf3::Quaternion & v2)
{
EXPECT_NEAR(q1.x, v2.x(), EPS);
EXPECT_NEAR(q1.y, v2.y(), EPS);
EXPECT_NEAR(q1.z, v2.z(), EPS);
}
void expect_near(const geometry_msgs::Quaternion & q1, const geometry_msgs::Quaternion & v2)
void expect_near(const robot_geometry_msgs::Quaternion & q1, const robot_geometry_msgs::Quaternion & v2)
{
EXPECT_NEAR(q1.x, v2.x, EPS);
EXPECT_NEAR(q1.y, v2.y, EPS);
EXPECT_NEAR(q1.z, v2.z, EPS);
}
void expect_near(const geometry_msgs::QuaternionStamped & p1, const geometry_msgs::QuaternionStamped & p2)
void expect_near(const robot_geometry_msgs::QuaternionStamped & p1, const robot_geometry_msgs::QuaternionStamped & p2)
{
expect_near(p1.header, p2.header);
expect_near(p1.quaternion, p2.quaternion);
@@ -207,19 +207,19 @@ void expect_near(const geometry_msgs::QuaternionStamped & p1, const geometry_msg
/*
* Pose
*/
void expect_near(const geometry_msgs::Pose & p, const tf3::Transform & t)
void expect_near(const robot_geometry_msgs::Pose & p, const tf3::Transform & t)
{
expect_near(p.position, t.getOrigin());
expect_near(p.orientation, t.getRotation());
}
void expect_near(const geometry_msgs::Pose & p1, const geometry_msgs::Pose & p2)
void expect_near(const robot_geometry_msgs::Pose & p1, const robot_geometry_msgs::Pose & p2)
{
expect_near(p1.position, p2.position);
expect_near(p1.orientation, p2.orientation);
}
void expect_near(const geometry_msgs::PoseStamped & p1, const geometry_msgs::PoseStamped & p2)
void expect_near(const robot_geometry_msgs::PoseStamped & p1, const robot_geometry_msgs::PoseStamped & p2)
{
expect_near(p1.header, p2.header);
expect_near(p1.pose, p2.pose);
@@ -228,19 +228,19 @@ void expect_near(const geometry_msgs::PoseStamped & p1, const geometry_msgs::Pos
/*
* Transform
*/
void expect_near(const geometry_msgs::Transform & p, const tf3::Transform & t)
void expect_near(const robot_geometry_msgs::Transform & p, const tf3::Transform & t)
{
expect_near(p.translation, t.getOrigin());
expect_near(p.rotation, t.getRotation());
}
void expect_near(const geometry_msgs::Transform & p1, const geometry_msgs::Transform & p2)
void expect_near(const robot_geometry_msgs::Transform & p1, const robot_geometry_msgs::Transform & p2)
{
expect_near(p1.translation, p2.translation);
expect_near(p1.rotation, p2.rotation);
}
void expect_near(const geometry_msgs::TransformStamped & p1, const geometry_msgs::TransformStamped & p2)
void expect_near(const robot_geometry_msgs::TransformStamped & p1, const robot_geometry_msgs::TransformStamped & p2)
{
expect_near(p1.header, p2.header);
expect_near(p1.transform, p2.transform);
@@ -262,78 +262,78 @@ void expect_near(const tf3::Stamped<T>& s1, const tf3::Stamped<T>& s2)
TEST(tf3_geometry_msgs, Vector3)
{
geometry_msgs::Vector3 m1;
robot_geometry_msgs::Vector3 m1;
value_initialize(m1);
tf3::Vector3 v1;
fromMsg(m1, v1);
SCOPED_TRACE("m1 v1");
expect_near(m1, v1);
geometry_msgs::Vector3 m2 = toMsg(v1);
robot_geometry_msgs::Vector3 m2 = toMsg(v1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(tf3_geometry_msgs, Point)
{
geometry_msgs::Point m1;
robot_geometry_msgs::Point m1;
value_initialize(m1);
tf3::Vector3 v1;
SCOPED_TRACE("m1 v1");
fromMsg(m1, v1);
expect_near(m1, v1);
geometry_msgs::Point m2 = toMsg(v1, m2);
robot_geometry_msgs::Point m2 = toMsg(v1, m2);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(tf3_geometry_msgs, Quaternion)
{
geometry_msgs::Quaternion m1;
robot_geometry_msgs::Quaternion m1;
value_initialize(m1);
tf3::Quaternion q1;
SCOPED_TRACE("m1 q1");
fromMsg(m1, q1);
expect_near(m1, q1);
geometry_msgs::Quaternion m2 = toMsg(q1);
robot_geometry_msgs::Quaternion m2 = toMsg(q1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(tf3_geometry_msgs, Pose)
{
geometry_msgs::Pose m1;
robot_geometry_msgs::Pose m1;
value_initialize(m1);
tf3::Transform t1;
fromMsg(m1, t1);
SCOPED_TRACE("m1 t1");
expect_near(m1, t1);
geometry_msgs::Pose m2 = toMsg(t1, m2);
robot_geometry_msgs::Pose m2 = toMsg(t1, m2);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(tf3_geometry_msgs, Transform)
{
geometry_msgs::Transform m1;
robot_geometry_msgs::Transform m1;
value_initialize(m1);
tf3::Transform t1;
fromMsg(m1, t1);
SCOPED_TRACE("m1 t1");
expect_near(m1, t1);
geometry_msgs::Transform m2 = toMsg(t1);
robot_geometry_msgs::Transform m2 = toMsg(t1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(tf3_geometry_msgs, Vector3Stamped)
{
geometry_msgs::Vector3Stamped m1;
robot_geometry_msgs::Vector3Stamped m1;
value_initialize(m1);
tf3::Stamped<tf3::Vector3> v1;
fromMsg(m1, v1);
SCOPED_TRACE("m1 v1");
// expect_near(m1, v1);
geometry_msgs::Vector3Stamped m2;
robot_geometry_msgs::Vector3Stamped m2;
m2 = toMsg(v1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
@@ -341,13 +341,13 @@ TEST(tf3_geometry_msgs, Vector3Stamped)
TEST(tf3_geometry_msgs, PointStamped)
{
geometry_msgs::PointStamped m1;
robot_geometry_msgs::PointStamped m1;
value_initialize(m1);
tf3::Stamped<tf3::Vector3> v1;
fromMsg(m1, v1);
SCOPED_TRACE("m1 v1");
// expect_near(m1, v1); //TODO implement cross verification explicityly
geometry_msgs::PointStamped m2;
robot_geometry_msgs::PointStamped m2;
m2 = toMsg(v1, m2);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
@@ -355,13 +355,13 @@ TEST(tf3_geometry_msgs, PointStamped)
TEST(tf3_geometry_msgs, QuaternionStamped)
{
geometry_msgs::QuaternionStamped m1;
robot_geometry_msgs::QuaternionStamped m1;
value_initialize(m1);
tf3::Stamped<tf3::Quaternion> v1;
fromMsg(m1, v1);
SCOPED_TRACE("m1 v1");
// expect_near(m1, v1); //TODO implement cross verification explicityly
geometry_msgs::QuaternionStamped m2;
robot_geometry_msgs::QuaternionStamped m2;
m2 = tf3::toMsg(v1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
@@ -369,13 +369,13 @@ TEST(tf3_geometry_msgs, QuaternionStamped)
TEST(tf3_geometry_msgs, PoseStamped)
{
geometry_msgs::PoseStamped m1;
robot_geometry_msgs::PoseStamped m1;
value_initialize(m1);
tf3::Stamped<tf3::Transform> v1;
SCOPED_TRACE("m1 v1");
fromMsg(m1, v1);
// expect_near(m1, v1); //TODO implement cross verification explicityly
geometry_msgs::PoseStamped m2;
robot_geometry_msgs::PoseStamped m2;
m2 = tf3::toMsg(v1, m2);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
@@ -383,13 +383,13 @@ TEST(tf3_geometry_msgs, PoseStamped)
TEST(tf3_geometry_msgs, TransformStamped)
{
geometry_msgs::TransformStamped m1;
robot_geometry_msgs::TransformStamped m1;
value_initialize(m1);
tf3::Stamped<tf3::Transform> v1;
fromMsg(m1, v1);
SCOPED_TRACE("m1 v1");
// expect_near(m1, v1);
geometry_msgs::TransformStamped m2;
robot_geometry_msgs::TransformStamped m2;
m2 = tf3::toMsg(v1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);