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

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
navigations/geometry2/tf2_sensor_msgs/test/test.launch Executable file
View File

@@ -0,0 +1,3 @@
<launch>
<test test-name="tf2_sensor_msgs" pkg="tf2_sensor_msgs" type="test_tf2_sensor_msgs_cpp" time-limit="120" />
</launch>

104
navigations/geometry2/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp Executable file
View File

@@ -0,0 +1,104 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_ros/transform_listener.h>
#include <ros/ros.h>
#include <gtest/gtest.h>
#include <tf2_ros/buffer.h>
tf2_ros::Buffer* tf_buffer;
static const double EPS = 1e-3;
TEST(Tf2Sensor, PointCloud2)
{
sensor_msgs::PointCloud2 cloud;
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");
*iter_x = 1;
*iter_y = 2;
*iter_z = 3;
cloud.header.stamp = ros::Time(2);
cloud.header.frame_id = "A";
// simple api
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");
EXPECT_NEAR(*iter_x_after, -9, EPS);
EXPECT_NEAR(*iter_y_after, 18, EPS);
EXPECT_NEAR(*iter_z_after, 27, EPS);
// advanced api
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");
EXPECT_NEAR(*iter_x_advanced, -9, EPS);
EXPECT_NEAR(*iter_y_advanced, 18, EPS);
EXPECT_NEAR(*iter_z_advanced, 27, EPS);
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test");
ros::NodeHandle n;
tf_buffer = new tf2_ros::Buffer();
// populate buffer
geometry_msgs::TransformStamped t;
t.transform.translation.x = 10;
t.transform.translation.y = 20;
t.transform.translation.z = 30;
t.transform.rotation.x = 1;
t.transform.rotation.y = 0;
t.transform.rotation.z = 0;
t.transform.rotation.w = 0;
t.header.stamp = ros::Time(2.0);
t.header.frame_id = "A";
t.child_frame_id = "B";
tf_buffer->setTransform(t, "test");
int ret = RUN_ALL_TESTS();
delete tf_buffer;
return ret;
}

103
navigations/geometry2/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py Executable file
View File

@@ -0,0 +1,103 @@
#!/usr/bin/env python
from __future__ import print_function
import unittest
import struct
import tf2_sensor_msgs
from sensor_msgs import point_cloud2
from sensor_msgs.msg import PointField
from tf2_ros import TransformStamped
import copy
## A sample python unit test
class PointCloudConversions(unittest.TestCase):
def setUp(self):
self.point_cloud_in = point_cloud2.PointCloud2()
self.point_cloud_in.fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)]
self.point_cloud_in.point_step = 4 * 3
self.point_cloud_in.height = 1
# we add two points (with x, y, z to the cloud)
self.point_cloud_in.width = 2
self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width
points = [1, 2, 0, 10, 20, 30]
self.point_cloud_in.data = struct.pack('%sf' % len(points), *points)
self.transform_translate_xyz_300 = TransformStamped()
self.transform_translate_xyz_300.transform.translation.x = 300
self.transform_translate_xyz_300.transform.translation.y = 300
self.transform_translate_xyz_300.transform.translation.z = 300
self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w
assert(list(point_cloud2.read_points(self.point_cloud_in)) == [(1.0, 2.0, 0.0), (10.0, 20.0, 30.0)])
def test_simple_transform(self):
old_data = copy.deepcopy(self.point_cloud_in.data) # deepcopy is not required here because we have a str for now
point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300)
k = 300
expected_coordinates = [(1+k, 2+k, 0+k), (10+k, 20+k, 30+k)]
new_points = list(point_cloud2.read_points(point_cloud_transformed))
print("new_points are %s" % new_points)
assert(expected_coordinates == new_points)
assert(old_data == self.point_cloud_in.data) # checking no modification in input cloud
## A simple unit test for tf2_sensor_msgs.do_transform_cloud (multi channel version)
class PointCloudConversionsMultichannel(unittest.TestCase):
TRANSFORM_OFFSET_DISTANCE = 300
def setUp(self):
self.point_cloud_in = point_cloud2.PointCloud2()
self.point_cloud_in.fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('index', 12, PointField.INT32, 1)]
self.point_cloud_in.point_step = 4 * 4
self.point_cloud_in.height = 1
# we add two points (with x, y, z to the cloud)
self.point_cloud_in.width = 2
self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width
self.points = [(1.0, 2.0, 0.0, 123), (10.0, 20.0, 30.0, 456)]
for point in self.points:
self.point_cloud_in.data += struct.pack('3fi', *point)
self.transform_translate_xyz_300 = TransformStamped()
self.transform_translate_xyz_300.transform.translation.x = self.TRANSFORM_OFFSET_DISTANCE
self.transform_translate_xyz_300.transform.translation.y = self.TRANSFORM_OFFSET_DISTANCE
self.transform_translate_xyz_300.transform.translation.z = self.TRANSFORM_OFFSET_DISTANCE
self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w
assert(list(point_cloud2.read_points(self.point_cloud_in)) == self.points)
def test_simple_transform_multichannel(self):
old_data = copy.deepcopy(self.point_cloud_in.data) # deepcopy is not required here because we have a str for now
point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300)
expected_coordinates = []
for point in self.points:
expected_coordinates += [(
point[0] + self.TRANSFORM_OFFSET_DISTANCE,
point[1] + self.TRANSFORM_OFFSET_DISTANCE,
point[2] + self.TRANSFORM_OFFSET_DISTANCE,
point[3] # index channel must be kept same
)]
new_points = list(point_cloud2.read_points(point_cloud_transformed))
print("new_points are %s" % new_points)
assert(expected_coordinates == new_points)
assert(old_data == self.point_cloud_in.data) # checking no modification in input cloud
if __name__ == '__main__':
import rosunit
rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversions)
rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversionsMultichannel)