Skip to content

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'rolling' into explicit_time
Browse files Browse the repository at this point in the history
quarkytale authored Dec 12, 2023

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
2 parents df10ffb + 3d913f6 commit 3d0f0ae
Showing 2 changed files with 30 additions and 26 deletions.
20 changes: 13 additions & 7 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -24,10 +24,18 @@ target_include_directories(laser_geometry
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>
${Eigen3_INCLUDE_DIRS}
)
ament_target_dependencies(laser_geometry
"rclcpp"
"sensor_msgs"
"tf2"
target_link_libraries(laser_geometry PUBLIC
${sensor_msgs_TARGETS}
tf2::tf2
)
if(TARGET Eigen3::Eigen)
target_link_libraries(laser_geometry PUBLIC Eigen3::Eigen)
else()
target_include_directories(laser_geometry PUBLIC ${Eigen3_INCLUDE_DIRS})
endif()

target_link_libraries(laser_geometry PRIVATE
rclcpp::rclcpp
)

# Causes the visibility macros to use dllexport rather than dllimport,
@@ -42,9 +50,7 @@ ament_export_libraries(laser_geometry)
ament_export_targets(laser_geometry)

ament_export_dependencies(
eigen3_cmake_module
Eigen3
rclcpp
sensor_msgs
tf2
)
@@ -80,7 +86,7 @@ if(BUILD_TESTING)
test/projection_test.cpp
TIMEOUT 240)
if(TARGET projection_test)
target_link_libraries(projection_test laser_geometry)
target_link_libraries(projection_test laser_geometry rclcpp::rclcpp)
endif()

# Python test
36 changes: 17 additions & 19 deletions src/laser_geometry.cpp
Original file line number Diff line number Diff line change
@@ -30,16 +30,14 @@

#include "laser_geometry/laser_geometry.hpp"

#include <Eigen/Core>

#include <algorithm>
#include <string>

#include "rclcpp/time.hpp"

#define TIME rclcpp::Time

#define POINT_FIELD sensor_msgs::msg::PointField

typedef double tfScalar;
#include "sensor_msgs/msg/laser_scan.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"

#include "tf2/LinearMath/Transform.h"

@@ -87,15 +85,15 @@ void LaserProjection::projectLaser_(
cloud_out.fields.resize(3);
cloud_out.fields[0].name = "x";
cloud_out.fields[0].offset = 0;
cloud_out.fields[0].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32;
cloud_out.fields[0].count = 1;
cloud_out.fields[1].name = "y";
cloud_out.fields[1].offset = 4;
cloud_out.fields[1].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32;
cloud_out.fields[1].count = 1;
cloud_out.fields[2].name = "z";
cloud_out.fields[2].offset = 8;
cloud_out.fields[2].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32;
cloud_out.fields[2].count = 1;

// Define 4 indices in the channel array for each possible value type
@@ -108,7 +106,7 @@ void LaserProjection::projectLaser_(
size_t field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "intensity";
cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].datatype = sensor_msgs::msg::PointField::FLOAT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
@@ -119,7 +117,7 @@ void LaserProjection::projectLaser_(
size_t field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "index";
cloud_out.fields[field_size].datatype = POINT_FIELD::INT32;
cloud_out.fields[field_size].datatype = sensor_msgs::msg::PointField::INT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
@@ -130,7 +128,7 @@ void LaserProjection::projectLaser_(
size_t field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "distances";
cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].datatype = sensor_msgs::msg::PointField::FLOAT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
@@ -141,7 +139,7 @@ void LaserProjection::projectLaser_(
size_t field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "stamps";
cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].datatype = sensor_msgs::msg::PointField::FLOAT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
@@ -153,19 +151,19 @@ void LaserProjection::projectLaser_(
cloud_out.fields.resize(field_size + 3);

cloud_out.fields[field_size].name = "vp_x";
cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].datatype = sensor_msgs::msg::PointField::FLOAT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;

cloud_out.fields[field_size + 1].name = "vp_y";
cloud_out.fields[field_size + 1].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size + 1].datatype = sensor_msgs::msg::PointField::FLOAT32;
cloud_out.fields[field_size + 1].offset = offset;
cloud_out.fields[field_size + 1].count = 1;
offset += 4;

cloud_out.fields[field_size + 2].name = "vp_z";
cloud_out.fields[field_size + 2].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size + 2].datatype = sensor_msgs::msg::PointField::FLOAT32;
cloud_out.fields[field_size + 2].offset = offset;
cloud_out.fields[field_size + 2].count = 1;
offset += 4;
@@ -330,7 +328,7 @@ void LaserProjection::transformLaserScanToPointCloud_(
memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t));

// Assume constant motion during the laser-scan and use slerp to compute intermediate transforms
tfScalar ratio = pt_index * ranges_norm;
double ratio = pt_index * ranges_norm;

// TODO(anon): Make a function that performs both the slerp and linear interpolation needed to
// interpolate a Full Transform (Quaternion + Vector)
@@ -423,8 +421,8 @@ void LaserProjection::transformLaserScanToPointCloud_(
double range_cutoff,
int channel_options)
{
TIME start_time(scan_in.header.stamp, RCL_ROS_TIME);
TIME end_time(scan_in.header.stamp, RCL_ROS_TIME);
rclcpp::TIME start_time(scan_in.header.stamp, RCL_ROS_TIME);
rclcpp::TIME end_time(scan_in.header.stamp, RCL_ROS_TIME);
// TODO(anonymous): reconcile all the different time constructs
if (!scan_in.ranges.empty()) {
end_time = start_time + rclcpp::Duration::from_seconds(

0 comments on commit 3d0f0ae

Please sign in to comment.