remove file data_convert
This commit is contained in:
parent
5e9993268f
commit
bc3aa7060d
|
|
@ -44,6 +44,7 @@ target_link_libraries(laser_geometry PUBLIC
|
|||
geometry_msgs
|
||||
robot_time
|
||||
tf3
|
||||
data_convert
|
||||
)
|
||||
|
||||
if(TARGET Eigen3::Eigen)
|
||||
|
|
|
|||
|
|
@ -310,7 +310,7 @@ geometry_msgs_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/common_msgs/geometry
|
|||
laser_geometry_BINARY_DIR:STATIC=/home/duongtd/robotics_core/laser_geometry/build
|
||||
|
||||
//Dependencies for the target
|
||||
laser_geometry_LIB_DEPENDS:STATIC=general;robot_time;general;tf2;
|
||||
laser_geometry_LIB_DEPENDS:STATIC=general;robot_time;general;tf3;
|
||||
|
||||
//Value Computed by CMake
|
||||
laser_geometry_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/laser_geometry
|
||||
|
|
|
|||
|
|
@ -55,9 +55,9 @@ sys/time.h
|
|||
../include/laser_geometry/data_convert.h
|
||||
geometry_msgs/TransformStamped.h
|
||||
-
|
||||
tf2/utils.h
|
||||
tf3/utils.h
|
||||
-
|
||||
tf2/compat.h
|
||||
tf3/compat.h
|
||||
-
|
||||
robot/time.h
|
||||
-
|
||||
|
|
@ -73,8 +73,8 @@ string
|
|||
-
|
||||
Eigen/Core
|
||||
-
|
||||
tf2/buffer_core.h
|
||||
../include/laser_geometry/tf2/buffer_core.h
|
||||
tf3/buffer_core.h
|
||||
../include/laser_geometry/tf3/buffer_core.h
|
||||
sensor_msgs/LaserScan.h
|
||||
-
|
||||
sensor_msgs/PointCloud2.h
|
||||
|
|
@ -155,12 +155,12 @@ sensor_msgs/LaserScan.h
|
|||
-
|
||||
sensor_msgs/PointCloud2.h
|
||||
-
|
||||
tf2/buffer_core.h
|
||||
tf3/buffer_core.h
|
||||
-
|
||||
tf2/convert.h
|
||||
tf3/convert.h
|
||||
-
|
||||
tf2/LinearMath/Transform.h
|
||||
/home/duongtd/robotics_core/laser_geometry/src/tf2/LinearMath/Transform.h
|
||||
tf3/LinearMath/Transform.h
|
||||
/home/duongtd/robotics_core/laser_geometry/src/tf3/LinearMath/Transform.h
|
||||
|
||||
/usr/include/eigen3/Eigen/Core
|
||||
src/Core/util/DisableStupidWarnings.h
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
/usr/bin/c++ -fPIC -shared -Wl,-soname,liblaser_geometry.so -o liblaser_geometry.so CMakeFiles/laser_geometry.dir/src/laser_geometry.cpp.o robot_time_build/librobot_time.a -ltf2 robot_time_build/librobot_time.a
|
||||
/usr/bin/c++ -fPIC -shared -Wl,-soname,liblaser_geometry.so -o liblaser_geometry.so CMakeFiles/laser_geometry.dir/src/laser_geometry.cpp.o robot_time_build/librobot_time.a -ltf3 robot_time_build/librobot_time.a
|
||||
|
|
|
|||
|
|
@ -63,8 +63,8 @@ string
|
|||
-
|
||||
Eigen/Core
|
||||
-
|
||||
tf2/buffer_core.h
|
||||
../include/laser_geometry/tf2/buffer_core.h
|
||||
tf3/buffer_core.h
|
||||
../include/laser_geometry/tf3/buffer_core.h
|
||||
sensor_msgs/LaserScan.h
|
||||
-
|
||||
sensor_msgs/PointCloud2.h
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
/usr/bin/c++ CMakeFiles/projection_test.dir/test/projection_test.cpp.o -o projection_test -Wl,-rpath,/home/duongtd/robotics_core/laser_geometry/build liblaser_geometry.so -lpthread robot_time_build/librobot_time.a -ltf2 /usr/local/lib/libgtest_main.a /usr/local/lib/libgtest.a -lpthread
|
||||
/usr/bin/c++ CMakeFiles/projection_test.dir/test/projection_test.cpp.o -o projection_test -Wl,-rpath,/home/duongtd/robotics_core/laser_geometry/build liblaser_geometry.so -lpthread robot_time_build/librobot_time.a -ltf3 /usr/local/lib/libgtest_main.a /usr/local/lib/libgtest.a -lpthread
|
||||
|
|
|
|||
|
|
@ -1,100 +0,0 @@
|
|||
#ifndef DATA_CONVERT_H
|
||||
#define DATA_CONVERT_H
|
||||
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <tf3/utils.h>
|
||||
#include <tf3/compat.h>
|
||||
#include <robot/time.h>
|
||||
|
||||
namespace laser_geometry
|
||||
{
|
||||
robot::Time convertTime(tf3::Time time)
|
||||
{
|
||||
robot::Time time_tmp;
|
||||
time_tmp.sec = time.sec;
|
||||
time_tmp.nsec = time.nsec;
|
||||
return time_tmp;
|
||||
}
|
||||
|
||||
tf3::Time convertTime(robot::Time time)
|
||||
{
|
||||
tf3::Time time_tmp;
|
||||
time_tmp.sec = time.sec;
|
||||
time_tmp.nsec = time.nsec;
|
||||
return time_tmp;
|
||||
}
|
||||
|
||||
|
||||
tf3::Transform convertToTf2Transform(const geometry_msgs::Transform& msg)
|
||||
{
|
||||
tf3::Quaternion q(
|
||||
msg.rotation.x,
|
||||
msg.rotation.y,
|
||||
msg.rotation.z,
|
||||
msg.rotation.w
|
||||
);
|
||||
|
||||
tf3::Vector3 t(
|
||||
msg.translation.x,
|
||||
msg.translation.y,
|
||||
msg.translation.z
|
||||
);
|
||||
|
||||
tf3::Transform tf(q, t);
|
||||
return tf;
|
||||
}
|
||||
|
||||
tf3::Transform convertToTf2Transform(const tf3::TransformMsg& msg)
|
||||
{
|
||||
tf3::Quaternion q(
|
||||
msg.rotation.x,
|
||||
msg.rotation.y,
|
||||
msg.rotation.z,
|
||||
msg.rotation.w
|
||||
);
|
||||
|
||||
tf3::Vector3 t(
|
||||
msg.translation.x,
|
||||
msg.translation.y,
|
||||
msg.translation.z
|
||||
);
|
||||
|
||||
tf3::Transform tf(q, t);
|
||||
return tf;
|
||||
}
|
||||
|
||||
tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg)
|
||||
{
|
||||
tf3::TransformStampedMsg out;
|
||||
out.header.seq = msg.header.seq;
|
||||
out.header.stamp = convertTime(msg.header.stamp);
|
||||
out.header.frame_id = msg.header.frame_id;
|
||||
out.child_frame_id = msg.child_frame_id;
|
||||
out.transform.translation.x = msg.transform.translation.x;
|
||||
out.transform.translation.y = msg.transform.translation.y;
|
||||
out.transform.translation.z = msg.transform.translation.z;
|
||||
out.transform.rotation.x = msg.transform.rotation.x;
|
||||
out.transform.rotation.y = msg.transform.rotation.y;
|
||||
out.transform.rotation.z = msg.transform.rotation.z;
|
||||
out.transform.rotation.w = msg.transform.rotation.w;
|
||||
return out;
|
||||
}
|
||||
geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
|
||||
{
|
||||
geometry_msgs::TransformStamped out;
|
||||
out.header.seq = msg.header.seq;
|
||||
out.header.stamp = convertTime(msg.header.stamp);
|
||||
out.header.frame_id = msg.header.frame_id;
|
||||
out.child_frame_id = msg.child_frame_id;
|
||||
out.transform.translation.x = msg.transform.translation.x;
|
||||
out.transform.translation.y = msg.transform.translation.y;
|
||||
out.transform.translation.z = msg.transform.translation.z;
|
||||
out.transform.rotation.x = msg.transform.rotation.x;
|
||||
out.transform.rotation.y = msg.transform.rotation.y;
|
||||
out.transform.rotation.z = msg.transform.rotation.z;
|
||||
out.transform.rotation.w = msg.transform.rotation.w;
|
||||
return out;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // DATA_CONVERT_H
|
||||
|
|
@ -29,7 +29,7 @@
|
|||
// POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
#include "laser_geometry/laser_geometry.hpp"
|
||||
#include "laser_geometry/data_convert.h"
|
||||
#include "data_convert/data_convert.h"
|
||||
|
||||
#include <Eigen/Core>
|
||||
|
||||
|
|
@ -436,8 +436,8 @@ void LaserProjection::transformLaserScanToPointCloud_(
|
|||
end_time = start_time + duration;
|
||||
}
|
||||
|
||||
tf3::Time st = convertTime(start_time);
|
||||
tf3::Time e = convertTime(end_time);
|
||||
tf3::Time st = data_convert::convertTime(start_time);
|
||||
tf3::Time e = data_convert::convertTime(end_time);
|
||||
|
||||
tf3::TransformStampedMsg start_transform_msg = tf.lookupTransform(
|
||||
target_frame, scan_in.header.frame_id, st);
|
||||
|
|
@ -445,8 +445,8 @@ void LaserProjection::transformLaserScanToPointCloud_(
|
|||
tf3::TransformStampedMsg end_transform_msg = tf.lookupTransform(
|
||||
target_frame, scan_in.header.frame_id, e);
|
||||
|
||||
geometry_msgs::TransformStamped start_transform = convertToTransformStamped(start_transform_msg);
|
||||
geometry_msgs::TransformStamped end_transform = convertToTransformStamped(end_transform_msg);
|
||||
geometry_msgs::TransformStamped start_transform = data_convert::convertToTransformStamped(start_transform_msg);
|
||||
geometry_msgs::TransformStamped end_transform = data_convert::convertToTransformStamped(end_transform_msg);
|
||||
|
||||
tf3::Quaternion quat_start(start_transform.transform.rotation.x,
|
||||
start_transform.transform.rotation.y,
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user