Closed cckaixin closed 1 year ago
自问自答:目前已通过修改消息格式,将发布的点云更改成我需要的livox_ros_driver::CustomMsg格式
Hi @cckaixin. Can you share the modified version of the code please?
ok, i have revised the following function:
void LivoxPointsPlugin::OnNewLaserScans() {
if (rayShape)
{
std::vector<std::pair<int, AviaRotateInfo>> points_pair;
InitializeRays(points_pair, rayShape);
rayShape->Update();
msgs::Set(laserMsg.mutable_time(), world->SimTime());
msgs::LaserScan *scan = laserMsg.mutable_scan();
InitializeScan(scan);
SendRosTf(parentEntity->WorldPose(), world->Name(), raySensor->ParentName());
auto rayCount = RayCount();
auto verticalRayCount = VerticalRayCount();
auto angle_min = AngleMin().Radian();
auto angle_incre = AngleResolution();
auto verticle_min = VerticalAngleMin().Radian();
auto verticle_incre = VerticalAngleResolution();
////////////////////////////////////////////////
//revised by ckx frome cloud to cloud2 msg
//////////////////////////////////////////////// START
livox_ros_driver::CustomMsg pp_livox;
pp_livox.header.stamp = ros::Time::now();
pp_livox.header.frame_id = "livox";
int count = 0;
boost::chrono::high_resolution_clock::time_point start_time = boost::chrono::high_resolution_clock::now();
for (auto &pair : points_pair)
{
auto range = rayShape->GetRange(pair.first);
auto intensity = rayShape->GetRetro(pair.first);
if (range >= RangeMax()) {
range = 0;
} else if (range <= RangeMin()) {
range = 0;
}
auto rotate_info = pair.second;
ignition::math::Quaterniond ray;
ray.Euler(ignition::math::Vector3d(0.0, rotate_info.zenith, rotate_info.azimuth));
auto axis = ray * ignition::math::Vector3d(1.0, 0.0, 0.0);
auto point = range * axis;
livox_ros_driver::CustomPoint p;
p.x = point.X();
p.y = point.Y();
p.z = point.Z();
p.reflectivity = intensity;
boost::chrono::high_resolution_clock::time_point end_time = boost::chrono::high_resolution_clock::now();
boost::chrono::nanoseconds elapsed_time = boost::chrono::duration_cast<boost::chrono::nanoseconds>(end_time - start_time);
// std::cout << "Elapsed time: " << elapsed_time.count() << " microseconds" << std::endl;
p.offset_time = elapsed_time.count();
pp_livox.points.push_back(p);
count ++;
}
if (scanPub && scanPub->HasConnections()) scanPub->Publish(laserMsg);
pp_livox.point_num = count;
// ROS_INFO_STREAM("pp_livox.point_num: " << pp_livox.point_num << " !");
livox_pub.publish(pp_livox);
ros::spinOnce();
}
////////////////////////////////////////////////
// revised by ckx frome cloud to cloud2 msg
//////////////////////////////////////////////// END
}
and include this header:
#include <livox_ros_driver/CustomMsg.h>
also, you should modify CMakelists.txt by adding livox_ros_driver:
find_package(catkin REQUIRED COMPONENTS
roscpp
tf
livox_ros_driver
)
and include livox_ros_driver package into your worksapce accordingly.
hope you can fix it : ) ckx
@cckaixin Hi. Thank you for sharing the code! I coded myself right after the question but yours looks much better :) I will change mine referring yours. For future users, you guys can refer this repo: https://github.com/engcang/livox_laser_simulation