Co-authored-by: Mike Purvis <mike@uwmike.com>
This commit is contained in:
parent
1273c7581d
commit
36eb607be7
@ -79,19 +79,19 @@ namespace pcl_conversions {
|
||||
/** PCLHeader <=> Header **/
|
||||
|
||||
inline
|
||||
void fromPCL(const pcl::uint64_t &pcl_stamp, rclcpp::Time &stamp)
|
||||
void fromPCL(const std::uint64_t &pcl_stamp, rclcpp::Time &stamp)
|
||||
{
|
||||
stamp = rclcpp::Time(pcl_stamp * 1000ull); // Convert from us to ns
|
||||
}
|
||||
|
||||
inline
|
||||
void toPCL(const rclcpp::Time &stamp, pcl::uint64_t &pcl_stamp)
|
||||
void toPCL(const rclcpp::Time &stamp, std::uint64_t &pcl_stamp)
|
||||
{
|
||||
pcl_stamp = stamp.nanoseconds() / 1000ull; // Convert from ns to us
|
||||
}
|
||||
|
||||
inline
|
||||
rclcpp::Time fromPCL(const pcl::uint64_t &pcl_stamp)
|
||||
rclcpp::Time fromPCL(const std::uint64_t &pcl_stamp)
|
||||
{
|
||||
rclcpp::Time stamp;
|
||||
fromPCL(pcl_stamp, stamp);
|
||||
@ -99,9 +99,9 @@ namespace pcl_conversions {
|
||||
}
|
||||
|
||||
inline
|
||||
pcl::uint64_t toPCL(const rclcpp::Time &stamp)
|
||||
std::uint64_t toPCL(const rclcpp::Time &stamp)
|
||||
{
|
||||
pcl::uint64_t pcl_stamp;
|
||||
std::uint64_t pcl_stamp;
|
||||
toPCL(stamp, pcl_stamp);
|
||||
return pcl_stamp;
|
||||
}
|
||||
@ -526,14 +526,14 @@ namespace pcl {
|
||||
|
||||
// sensor_msgs::image_encodings::BGR8;
|
||||
msg.encoding = "bgr8";
|
||||
msg.step = msg.width * sizeof (uint8_t) * 3;
|
||||
msg.step = msg.width * sizeof (std::uint8_t) * 3;
|
||||
msg.data.resize (msg.step * msg.height);
|
||||
for (size_t y = 0; y < cloud.height; y++)
|
||||
{
|
||||
for (size_t x = 0; x < cloud.width; x++)
|
||||
{
|
||||
uint8_t * pixel = &(msg.data[y * msg.step + x * 3]);
|
||||
memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(uint8_t));
|
||||
std::uint8_t * pixel = &(msg.data[y * msg.step + x * 3]);
|
||||
memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(std::uint8_t));
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -832,7 +832,7 @@ namespace ros
|
||||
length += 4; // point_step
|
||||
length += 4; // row_step
|
||||
length += 4; // data's size
|
||||
length += m.data.size() * sizeof(pcl::uint8_t);
|
||||
length += m.data.size() * sizeof(std::uint8_t);
|
||||
length += 1; // is_dense
|
||||
|
||||
return length;
|
||||
|
||||
@ -109,7 +109,7 @@ struct StampTestData
|
||||
explicit StampTestData(const rclcpp::Time &stamp)
|
||||
: stamp_(stamp)
|
||||
{
|
||||
pcl::uint64_t pcl_stamp;
|
||||
std::uint64_t pcl_stamp;
|
||||
pcl_conversions::toPCL(stamp_, pcl_stamp);
|
||||
pcl_conversions::fromPCL(pcl_stamp, stamp2_);
|
||||
}
|
||||
|
||||
@ -23,18 +23,18 @@ namespace pcl
|
||||
template<typename U> void operator() ()
|
||||
{
|
||||
const char* name = traits::name<PointT, U>::value;
|
||||
uint32_t name_length = strlen(name);
|
||||
std::uint32_t name_length = strlen(name);
|
||||
stream_.next(name_length);
|
||||
if (name_length > 0)
|
||||
memcpy(stream_.advance(name_length), name, name_length);
|
||||
|
||||
uint32_t offset = traits::offset<PointT, U>::value;
|
||||
std::uint32_t offset = traits::offset<PointT, U>::value;
|
||||
stream_.next(offset);
|
||||
|
||||
uint8_t datatype = traits::datatype<PointT, U>::value;
|
||||
std::uint8_t datatype = traits::datatype<PointT, U>::value;
|
||||
stream_.next(datatype);
|
||||
|
||||
uint32_t count = traits::datatype<PointT, U>::size;
|
||||
std::uint32_t count = traits::datatype<PointT, U>::size;
|
||||
stream_.next(count);
|
||||
}
|
||||
|
||||
@ -48,11 +48,11 @@ namespace pcl
|
||||
|
||||
template<typename U> void operator() ()
|
||||
{
|
||||
uint32_t name_length = strlen(traits::name<PointT, U>::value);
|
||||
std::uint32_t name_length = strlen(traits::name<PointT, U>::value);
|
||||
length += name_length + 13;
|
||||
}
|
||||
|
||||
uint32_t length;
|
||||
std::uint32_t length;
|
||||
};
|
||||
} // namespace pcl::detail
|
||||
} // namespace pcl
|
||||
|
||||
@ -58,7 +58,7 @@ typedef pcl::PointCloud<pcl::PointXYZRGBNormal> PCDType;
|
||||
/// Sets pcl_stamp from stamp, BUT alters stamp
|
||||
/// a little to round it to millisecond. This is because converting back
|
||||
/// and forth from pcd to ros time induces some rounding errors.
|
||||
void setStamp(ros::Time &stamp, pcl::uint64_t &pcl_stamp)
|
||||
void setStamp(ros::Time &stamp, std::uint64_t &pcl_stamp)
|
||||
{
|
||||
// round to millisecond
|
||||
static const uint32_t mult = 1e6;
|
||||
@ -187,7 +187,7 @@ TEST(MessageFilter, queueSize)
|
||||
filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
|
||||
|
||||
ros::Time stamp = ros::Time::now();
|
||||
pcl::uint64_t pcl_stamp;
|
||||
std::uint64_t pcl_stamp;
|
||||
setStamp(stamp, pcl_stamp);
|
||||
|
||||
for (int i = 0; i < 20; ++i)
|
||||
@ -281,7 +281,7 @@ TEST(MessageFilter, tolerance)
|
||||
filter.setTolerance(offset);
|
||||
|
||||
ros::Time stamp = ros::Time::now();
|
||||
pcl::uint64_t pcl_stamp;
|
||||
std::uint64_t pcl_stamp;
|
||||
setStamp(stamp, pcl_stamp);
|
||||
tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
|
||||
tf_client.setTransform(transform);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user