How to create a sensor plugin

asked 2021-04-06 09:39:27 -0600

igricart gravatar image

updated 2021-04-07 08:05:40 -0600

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?

edit retag flag offensive close merge delete