How to create a sensor plugin
I am trying to implement my own solid state lidar plugin to simulate a Livox. And was using the velodyne simulator as reference, but they use this method void OnScan(const ConstLaserScanStampedPtr &_msg);
as a callback from a topic subscription, instead of the void OnNewLaserScans()
provided by the base class. Which does not explain how the collision between the sensor ray and the object works. Later I tried using something that has already been "done", but that only works when the world physics is set for bullet
, otherwise there is a collision between the ray and the object, causing the object to be launched far away.
I tried also using the method public: virtual void SetPose(const ignition::math::Pose3d &_pose);
from Sensor.hh, but it didn't change a thing. That was my attempt to make a moving sensor perform whatever pattern I would require for it to do.
void GazeboRosLivoxSensor::PublishPointCloud2(ConstLaserScanStampedPtr &_msg)
{
// Create message to send
sensor_msgs::PointCloud2 pc;
ignition::math::v4::Pose3d new_pose(4,4,0,0,0,0);
ignition::math::v4::Quaterniond rot;
rot.Euler(0.001,0,0);
// new_pose.CoordRotationAdd(rot);
// new_pose.CoordPositionAdd();
// --------------------- HEEEEEREEE --------------------------------------
parent_ray_sensor_->SetPose(new_pose);
ROS_INFO_STREAM(this->parent_ray_sensor_->Pose());
ROS_INFO_STREAM(parent_ray_sensor_->ParentName());
// Fill header
pc.header.frame_id = frame_name_;
pc.header.stamp.sec = _msg->time().sec();
pc.header.stamp.nsec = _msg->time().nsec();
// Create fields in pointcloud
sensor_msgs::PointCloud2Modifier pcd_modifier(pc);
pcd_modifier.setPointCloud2Fields(4,
"x", 1, sensor_msgs::PointField::FLOAT32,
"y", 1, sensor_msgs::PointField::FLOAT32,
"z", 1, sensor_msgs::PointField::FLOAT32,
"intensity", 1, sensor_msgs::PointField::FLOAT32);
pcd_modifier.resize(verticalRangeCount * rangeCount);
pc.is_dense = true;
sensor_msgs::PointCloud2Iterator<float> iter_x(pc, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(pc, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(pc, "z");
sensor_msgs::PointCloud2Iterator<float> iter_intensity(pc, "intensity");
// Fill pointcloud with laser scan
for (int i = 0; i < rangeCount; i++)
{
// Calculate azimuth (horizontal angle)
double azimuth;
if (rangeCount > 1)
{
azimuth = i * yDiff / (rangeCount - 1) + minAngle;
}
else
{
azimuth = minAngle;
}
double c_azimuth = cos(azimuth);
double s_azimuth = sin(azimuth);
for (int j = 0; j < verticalRangeCount; ++j, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity)
{
double inclination;
// Calculate inclination (vertical angle)
if (verticalRayCount > 1)
{
inclination = j * pDiff / (verticalRangeCount - 1) + verticalMinAngle;
}
else
{
inclination = verticalMinAngle;
}
double c_inclination = cos(inclination);
double s_inclination = sin(inclination);
// Get range and intensity at this scan
size_t index = i + j * rangeCount;
double r = _msg->scan().ranges(index);
double intensity = _msg->scan().intensities(index);
if (intensity < this->min_intensity_)
intensity = this->min_intensity_;
// Convert spherical coordinates to cartesian for pointcloud
// See https://en.wikipedia.org/wiki/Spherical_coordinate_system
*iter_x = r * c_inclination * c_azimuth;
*iter_y = r * c_inclination * s_azimuth;
*iter_z = r * s_inclination;
*iter_intensity = intensity;
}
}
// Publish output
pub_.publish(pc);
}
I would like to have a RaySensor
working with GPU or CPU processing, where I could perform maybe a mathematical equation or a pattern for it to follow.
Any tips on how to do that?
Asked by igricart on 2021-04-06 09:39:27 UTC
Comments