diff --git a/CMakeLists.txt b/CMakeLists.txt index 95eb379..a354254 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,7 +27,7 @@ target_include_directories(laser_geometry ) target_link_libraries(laser_geometry PUBLIC - sensor_msgs + robot_sensor_msgs geometry_msgs robot_time tf3 diff --git a/include/laser_geometry/laser_geometry.hpp b/include/laser_geometry/laser_geometry.hpp index b1a6124..1a8d8ce 100644 --- a/include/laser_geometry/laser_geometry.hpp +++ b/include/laser_geometry/laser_geometry.hpp @@ -40,8 +40,8 @@ #include // NOLINT (cpplint cannot handle include order here) #include "tf3/buffer_core.h" -#include -#include +#include +#include #include #include "laser_geometry/visibility_control.hpp" @@ -104,7 +104,7 @@ public: LaserProjection() : angle_min_(0), angle_max_(0) {} - // Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 + // Project a robot_sensor_msgs::LaserScan into a robot_sensor_msgs::PointCloud2 /*! * Project a single laser scan from a linear array into a 3D * point cloud. The generated cloud will be in the same frame @@ -122,15 +122,15 @@ public: */ LASER_GEOMETRY_PUBLIC void projectLaser( - const sensor_msgs::LaserScan & scan_in, - sensor_msgs::PointCloud2 & cloud_out, + const robot_sensor_msgs::LaserScan & scan_in, + robot_sensor_msgs::PointCloud2 & cloud_out, double range_cutoff = -1.0, int channel_options = channel_option::Default) { projectLaser_(scan_in, cloud_out, range_cutoff, channel_options); } - // Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame + // Transform a robot_sensor_msgs::LaserScan into a robot_sensor_msgs::PointCloud2 in target frame /*! * Transform a single laser scan from a linear array into a 3D * point cloud, accounting for movement of the laser over the @@ -155,8 +155,8 @@ public: LASER_GEOMETRY_PUBLIC void transformLaserScanToPointCloud( const std::string & target_frame, - const sensor_msgs::LaserScan & scan_in, - sensor_msgs::PointCloud2 & cloud_out, + const robot_sensor_msgs::LaserScan & scan_in, + robot_sensor_msgs::PointCloud2 & cloud_out, tf3::BufferCore & tf, double range_cutoff = -1.0, int channel_options = channel_option::Default) @@ -168,16 +168,16 @@ public: private: // Internal hidden representation of projectLaser void projectLaser_( - const sensor_msgs::LaserScan & scan_in, - sensor_msgs::PointCloud2 & cloud_out, + const robot_sensor_msgs::LaserScan & scan_in, + robot_sensor_msgs::PointCloud2 & cloud_out, double range_cutoff, int channel_options); // Internal hidden representation of transformLaserScanToPointCloud2 void transformLaserScanToPointCloud_( const std::string & target_frame, - const sensor_msgs::LaserScan & scan_in, - sensor_msgs::PointCloud2 & cloud_out, + const robot_sensor_msgs::LaserScan & scan_in, + robot_sensor_msgs::PointCloud2 & cloud_out, tf3::BufferCore & tf, double range_cutoff, int channel_options); @@ -185,8 +185,8 @@ private: // Function used by the several forms of transformLaserScanToPointCloud_ void transformLaserScanToPointCloud_( const std::string & target_frame, - const sensor_msgs::LaserScan & scan_in, - sensor_msgs::PointCloud2 & cloud_out, + const robot_sensor_msgs::LaserScan & scan_in, + robot_sensor_msgs::PointCloud2 & cloud_out, tf3::Quaternion quat_start, tf3::Vector3 origin_start, tf3::Quaternion quat_end, diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index cd5fb3e..7620516 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -38,8 +38,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -48,8 +48,8 @@ namespace laser_geometry { void LaserProjection::projectLaser_( - const sensor_msgs::LaserScan & scan_in, - sensor_msgs::PointCloud2 & cloud_out, + const robot_sensor_msgs::LaserScan & scan_in, + robot_sensor_msgs::PointCloud2 & cloud_out, double range_cutoff, int channel_options) { @@ -89,15 +89,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 = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[0].datatype = robot_sensor_msgs::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 = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[1].datatype = robot_sensor_msgs::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 = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[2].datatype = robot_sensor_msgs::PointField::FLOAT32; cloud_out.fields[2].count = 1; // Define 4 indices in the channel array for each possible value type @@ -110,7 +110,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 = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[field_size].datatype = robot_sensor_msgs::PointField::FLOAT32; cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].count = 1; offset += 4; @@ -121,7 +121,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 = sensor_msgs::PointField::INT32; + cloud_out.fields[field_size].datatype = robot_sensor_msgs::PointField::INT32; cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].count = 1; offset += 4; @@ -132,7 +132,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 = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[field_size].datatype = robot_sensor_msgs::PointField::FLOAT32; cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].count = 1; offset += 4; @@ -143,7 +143,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 = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[field_size].datatype = robot_sensor_msgs::PointField::FLOAT32; cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].count = 1; offset += 4; @@ -155,19 +155,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 = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[field_size].datatype = robot_sensor_msgs::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 = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[field_size + 1].datatype = robot_sensor_msgs::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 = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[field_size + 2].datatype = robot_sensor_msgs::PointField::FLOAT32; cloud_out.fields[field_size + 2].offset = offset; cloud_out.fields[field_size + 2].count = 1; offset += 4; @@ -269,8 +269,8 @@ void LaserProjection::projectLaser_( void LaserProjection::transformLaserScanToPointCloud_( const std::string & target_frame, - const sensor_msgs::LaserScan & scan_in, - sensor_msgs::PointCloud2 & cloud_out, + const robot_sensor_msgs::LaserScan & scan_in, + robot_sensor_msgs::PointCloud2 & cloud_out, tf3::Quaternion quat_start, tf3::Vector3 origin_start, tf3::Quaternion quat_end, @@ -368,7 +368,7 @@ void LaserProjection::transformLaserScanToPointCloud_( // if the user didn't request the index field, then we need to copy the PointCloud and drop it if (!requested_index) { - sensor_msgs::PointCloud2 cloud_without_index; + robot_sensor_msgs::PointCloud2 cloud_without_index; // copy basic meta data cloud_without_index.header = cloud_out.header; @@ -419,8 +419,8 @@ void LaserProjection::transformLaserScanToPointCloud_( void LaserProjection::transformLaserScanToPointCloud_( const std::string & target_frame, - const sensor_msgs::LaserScan & scan_in, - sensor_msgs::PointCloud2 & cloud_out, + const robot_sensor_msgs::LaserScan & scan_in, + robot_sensor_msgs::PointCloud2 & cloud_out, tf3::BufferCore & tf, double range_cutoff, int channel_options) diff --git a/src/laser_geometry/laser_geometry.py b/src/laser_geometry/laser_geometry.py index ef2dcde..0e3eb0b 100644 --- a/src/laser_geometry/laser_geometry.py +++ b/src/laser_geometry/laser_geometry.py @@ -30,8 +30,8 @@ import numpy as np import rclpy import rclpy.logging -from sensor_msgs.msg import PointCloud2 -import sensor_msgs_py.point_cloud2 as pc2 +from robot_sensor_msgs.msg import PointCloud2 +import robot_sensor_msgs_py.point_cloud2 as pc2 class LaserProjection: @@ -86,7 +86,7 @@ class LaserProjection: self, scan_in, range_cutoff=-1.0, channel_options=ChannelOption.DEFAULT): """ - Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2. + Project a robot_sensor_msgs::LaserScan into a robot_sensor_msgs::PointCloud2. Project a single laser scan from a linear array into a 3D point cloud. The generated cloud will be in the same frame diff --git a/test/projection_test.cpp b/test/projection_test.cpp index cba025a..ce54cac 100644 --- a/test/projection_test.cpp +++ b/test/projection_test.cpp @@ -37,8 +37,8 @@ #include #include "laser_geometry/laser_geometry.hpp" -#include -#include +#include +#include #define PROJECTION_TEST_RANGE_MIN (0.23f) #define PROJECTION_TEST_RANGE_MAX (40.0f) @@ -70,13 +70,13 @@ struct ScanOptions scan_time_(scan_time) {} }; -sensor_msgs::LaserScan build_constant_scan(const ScanOptions & options) +robot_sensor_msgs::LaserScan build_constant_scan(const ScanOptions & options) { if (((options.ang_max_ - options.ang_min_) / options.ang_increment_) < 0) { throw (BuildScanException()); } - sensor_msgs::LaserScan scan; + robot_sensor_msgs::LaserScan scan; robot::Time now = robot::Time::now(); scan.header.stamp.sec = now.sec; scan.header.stamp.nsec = now.nsec; @@ -100,7 +100,7 @@ sensor_msgs::LaserScan build_constant_scan(const ScanOptions & options) } template -T cloudData(sensor_msgs::PointCloud2 cloud_out, uint32_t index) +T cloudData(robot_sensor_msgs::PointCloud2 cloud_out, uint32_t index) { return *reinterpret_cast(&cloud_out.data[index]); } @@ -157,9 +157,9 @@ TEST(laser_geometry, projectLaser2) { try { // printf("%f %f %f %f %f %f\n", // range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); - sensor_msgs::LaserScan scan = build_constant_scan(option); + robot_sensor_msgs::LaserScan scan = build_constant_scan(option); - sensor_msgs::PointCloud2 cloud_out; + robot_sensor_msgs::PointCloud2 cloud_out; projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index); EXPECT_EQ(cloud_out.fields.size(), 4u); projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity); @@ -205,7 +205,7 @@ TEST(laser_geometry, projectLaser2) { uint32_t index_offset = 0; uint32_t distance_offset = 0; uint32_t stamps_offset = 0; - for (std::vector::iterator f = cloud_out.fields.begin(); + for (std::vector::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) { if (f->name == "x") {x_offset = f->offset;} @@ -319,11 +319,11 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) { try { // printf("%f %f %f %f %f %f\n", // range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); - sensor_msgs::LaserScan scan = build_constant_scan(option); + robot_sensor_msgs::LaserScan scan = build_constant_scan(option); scan.header.frame_id = "laser_frame"; - sensor_msgs::PointCloud2 cloud_out; + robot_sensor_msgs::PointCloud2 cloud_out; projector.transformLaserScanToPointCloud( scan.header.frame_id, scan, cloud_out, tf3, -1.0, laser_geometry::channel_option::None); @@ -377,7 +377,7 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) { uint32_t index_offset = 0; uint32_t distance_offset = 0; uint32_t stamps_offset = 0; - for (std::vector::iterator f = cloud_out.fields.begin(); + for (std::vector::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) { if (f->name == "x") {x_offset = f->offset;}