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